Rev 284 | Rev 286 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 284 | Rev 285 | ||
---|---|---|---|
Line 101... | Line 101... | ||
101 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN}; |
101 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN}; |
102 | u8 SPI_CommandCounter = 0; |
102 | u8 SPI_CommandCounter = 0; |
103 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
103 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
104 | s32 HeadFreeStartAngle = 0; |
104 | s32 HeadFreeStartAngle = 0; |
105 | s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde |
105 | s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde |
- | 106 | u32 ToFC_AltitudeSpeed = 0; |
|
- | 107 | s32 ToFC_AltitudeSetpoint = 0; |
|
Line 106... | Line 108... | ||
106 | 108 | ||
Line 107... | Line 109... | ||
107 | SPI_Version_t FC_Version; |
109 | SPI_Version_t FC_Version; |
108 | 110 | ||
Line 278... | Line 280... | ||
278 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
280 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
279 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
281 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
280 | ToFlightCtrl.CompassHeading = Compass_Heading; |
282 | ToFlightCtrl.CompassHeading = Compass_Heading; |
281 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
283 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
282 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
284 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
283 | ToFlightCtrl.MagVecX = MagVector.X; |
285 | // ToFlightCtrl.MagVecX = MagVector.X; |
284 | ToFlightCtrl.MagVecY = MagVector.Y; |
286 | // ToFlightCtrl.MagVecY = MagVector.Y; |
285 | ToFlightCtrl.MagVecZ = MagVector.Z; |
287 | // ToFlightCtrl.MagVecZ = MagVector.Z; |
286 | ToFlightCtrl.NCStatus = 0; |
288 | ToFlightCtrl.NCStatus = 0; |
287 | // cycle spi commands |
289 | // cycle spi commands |
288 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
290 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
289 | // restart command cycle at the end |
291 | // restart command cycle at the end |
290 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
292 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
Line 298... | Line 300... | ||
298 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
300 | ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
299 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
301 | ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
300 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
302 | ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
301 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
303 | ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
302 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
304 | ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
- | 305 | //ToFlightCtrl.Param.Byte[7] = |
|
303 | if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH) |
306 | if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH) |
304 | { |
307 | { |
305 | ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth; |
308 | ToFlightCtrl.Param.sInt[4] = CAM_Orientation.Azimuth; |
306 | CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH; |
309 | CAM_Orientation.UpdateMask &= ~CAM_UPDATE_AZIMUTH; |
307 | } |
310 | } |
Line 329... | Line 332... | ||
329 | ToFlightCtrl.Param.Byte[2] = GPSData.SatFix; |
332 | ToFlightCtrl.Param.Byte[2] = GPSData.SatFix; |
330 | ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s |
333 | ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s |
331 | ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5 |
334 | ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5 |
332 | ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7 |
335 | ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7 |
333 | ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110); |
336 | ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 110); |
- | 337 | ToFlightCtrl.Param.Byte[9] = (u8) ToFC_AltitudeSpeed; |
|
- | 338 | ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint; |
|
334 | break; |
339 | break; |
335 | default: |
340 | default: |
336 | break; |
341 | break; |
- | 342 | // 0 = 0,1 |
|
- | 343 | // 1 = 2,3 |
|
- | 344 | // 2 = 4,5 |
|
- | 345 | // 3 = 6,7 |
|
- | 346 | // 4 = 8,9 |
|
- | 347 | // 5 = 10,11 |
|
337 | } |
348 | } |
338 | VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
349 | VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
Line 339... | Line 350... | ||
339 | 350 |