Rev 285 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 285 | Rev 286 | ||
|---|---|---|---|
| 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; |
106 | u32 ToFC_AltitudeRate = 0; |
| 107 | s32 ToFC_AltitudeSetpoint = 0; |
107 | s32 ToFC_AltitudeSetpoint = 0; |
| Line 108... | Line 108... | ||
| 108 | 108 | ||
| Line 109... | Line 109... | ||
| 109 | SPI_Version_t FC_Version; |
109 | SPI_Version_t FC_Version; |
| Line 280... | Line 280... | ||
| 280 | // 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 |
| 281 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
281 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
| 282 | ToFlightCtrl.CompassHeading = Compass_Heading; |
282 | ToFlightCtrl.CompassHeading = Compass_Heading; |
| 283 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
283 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
| 284 | 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; |
| 285 | // ToFlightCtrl.MagVecX = MagVector.X; |
285 | ToFlightCtrl.MagVecX = MagVector.X; |
| 286 | // ToFlightCtrl.MagVecY = MagVector.Y; |
286 | ToFlightCtrl.MagVecY = MagVector.Y; |
| 287 | // ToFlightCtrl.MagVecZ = MagVector.Z; |
287 | ToFlightCtrl.MagVecZ = MagVector.Z; |
| 288 | ToFlightCtrl.NCStatus = 0; |
288 | ToFlightCtrl.NCStatus = 0; |
| 289 | // cycle spi commands |
289 | // cycle spi commands |
| 290 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
290 | ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| 291 | // restart command cycle at the end |
291 | // restart command cycle at the end |
| 292 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
292 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
| Line 332... | Line 332... | ||
| 332 | ToFlightCtrl.Param.Byte[2] = GPSData.SatFix; |
332 | ToFlightCtrl.Param.Byte[2] = GPSData.SatFix; |
| 333 | ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s |
333 | ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s |
| 334 | ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5 |
334 | ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5 |
| 335 | ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7 |
335 | ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7 |
| 336 | 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; |
337 | ToFlightCtrl.Param.Byte[9] = (u8) ToFC_AltitudeRate; |
| 338 | ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint; |
338 | ToFlightCtrl.Param.sInt[5] = (s16) ToFC_AltitudeSetpoint; |
| 339 | break; |
339 | break; |
| 340 | default: |
340 | default: |
| 341 | break; |
341 | break; |
| 342 | // 0 = 0,1 |
342 | // 0 = 0,1 |