Rev 285 | Rev 287 | 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 |