Rev 338 | Rev 345 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 338 | Rev 342 | ||
---|---|---|---|
Line 99... | Line 99... | ||
99 | s32 Kalman_MaxDrift = 5 * 16; |
99 | s32 Kalman_MaxDrift = 5 * 16; |
100 | s32 Kalman_MaxFusion = 64; |
100 | s32 Kalman_MaxFusion = 64; |
101 | s32 Kalman_Kompass = 32; |
101 | s32 Kalman_Kompass = 32; |
102 | s32 ToFcGpsZ = 0; |
102 | s32 ToFcGpsZ = 0; |
Line 103... | Line 103... | ||
103 | 103 | ||
104 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN}; |
104 | u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN }; |
105 | u8 SPI_CommandCounter = 0; |
105 | u8 SPI_CommandCounter = 0; |
106 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
106 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
107 | s32 HeadFreeStartAngle = 0; |
107 | s32 HeadFreeStartAngle = 0; |
108 | s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde |
108 | s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde |
Line 290... | Line 290... | ||
290 | if (SPI_RxBuffer_Request) |
290 | if (SPI_RxBuffer_Request) |
291 | { |
291 | { |
292 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
292 | // avoid sending data via SPI during the update of the ToFlightCtrl structure |
293 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
293 | VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
294 | ToFlightCtrl.CompassHeading = Compass_Heading; |
294 | ToFlightCtrl.CompassHeading = Compass_Heading; |
- | 295 | //ToFlightCtrl.CompassHeading += 360 + ((s32) FC.Poti[7] - 100); |
|
- | 296 | //ToFlightCtrl.CompassHeading %= 360; |
|
295 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
297 | DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
296 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
298 | if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
297 | // ToFlightCtrl.MagVecX = MagVector.X; |
299 | // ToFlightCtrl.MagVecX = MagVector.X; |
298 | // ToFlightCtrl.MagVecY = MagVector.Y; |
300 | // ToFlightCtrl.MagVecY = MagVector.Y; |
299 | ToFlightCtrl.MagVecZ = MagVector.Z; |
301 | ToFlightCtrl.MagVecZ = MagVector.Z; |
Line 345... | Line 347... | ||
345 | ToFlightCtrl.Param.Byte[5] = DebugOut.StatusGreen; |
347 | ToFlightCtrl.Param.Byte[5] = DebugOut.StatusGreen; |
346 | ToFlightCtrl.Param.Byte[6] = DebugOut.StatusRed; |
348 | ToFlightCtrl.Param.Byte[6] = DebugOut.StatusRed; |
347 | ToFlightCtrl.Param.Byte[7] = ErrorCode; |
349 | ToFlightCtrl.Param.Byte[7] = ErrorCode; |
348 | ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter; |
350 | ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter; |
349 | ToFlightCtrl.Param.Byte[9] = SerialLinkOkay; |
351 | ToFlightCtrl.Param.Byte[9] = SerialLinkOkay; |
- | 352 | ToFlightCtrl.Param.Byte[10] = 0; |
|
- | 353 | ToFlightCtrl.Param.Byte[11] = 0; |
|
- | 354 | break; |
|
- | 355 | case SPI_MISC: |
|
- | 356 | ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5; |
|
- | 357 | ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination; |
|
- | 358 | ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic; |
|
- | 359 | ToFlightCtrl.Param.Byte[3] = 0; |
|
- | 360 | ToFlightCtrl.Param.Byte[4] = 0; |
|
- | 361 | ToFlightCtrl.Param.Byte[5] = 0; |
|
- | 362 | ToFlightCtrl.Param.Byte[6] = 0; |
|
- | 363 | ToFlightCtrl.Param.Byte[7] = 0; |
|
- | 364 | ToFlightCtrl.Param.Byte[8] = 0; |
|
- | 365 | ToFlightCtrl.Param.Byte[9] = 0; |
|
- | 366 | ToFlightCtrl.Param.Byte[10] = 0; |
|
- | 367 | ToFlightCtrl.Param.Byte[11] = 0; |
|
350 | break; |
368 | break; |
Line 351... | Line 369... | ||
351 | 369 | ||
352 | case SPI_NCCMD_GPSINFO: |
370 | case SPI_NCCMD_GPSINFO: |
353 | ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |
371 | ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |