Rev 388 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 388 | Rev 392 | ||
|---|---|---|---|
| Line 98... | Line 98... | ||
| 98 | s32 Kalman_K = 32; |
98 | s32 Kalman_K = 32; |
| 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; |
| - | 103 | u8 CompassCalState = 0; |
|
| Line 103... | Line 104... | ||
| 103 | 104 | ||
| 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_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; |
106 | u8 SPI_CommandCounter = 0; |
| 106 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
107 | s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
| Line 282... | Line 283... | ||
| 282 | //------------------------------------------------------ |
283 | //------------------------------------------------------ |
| 283 | void SPI0_UpdateBuffer(void) |
284 | void SPI0_UpdateBuffer(void) |
| 284 | { |
285 | { |
| 285 | static u32 timeout = 0; |
286 | static u32 timeout = 0; |
| 286 | static u8 counter = 50,hott_index = 0; |
287 | static u8 counter = 50,hott_index = 0; |
| 287 | static u8 CompassCalState = 0; |
- | |
| 288 | s16 tmp; |
288 | s16 tmp; |
| 289 | s32 i1,i2; |
289 | s32 i1,i2; |
| Line 290... | Line 290... | ||
| 290 | 290 | ||
| Line 594... | Line 594... | ||
| 594 | break; |
594 | break; |
| Line 595... | Line 595... | ||
| 595 | 595 | ||
| 596 | case SPI_FCCMD_MISC: |
596 | case SPI_FCCMD_MISC: |
| 597 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
597 | if(CompassCalState != FromFlightCtrl.Param.Byte[0]) |
| 598 | { // put only new CompassCalState into queue to send via I2C |
598 | { // put only new CompassCalState into queue to send via I2C |
| 599 | if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1) |
599 | // if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1 || FromFlightCtrl.Param.Byte[0] == 0) |
| 600 | { |
600 | { |
| 601 | CompassCalState = FromFlightCtrl.Param.Byte[0]; |
601 | CompassCalState = FromFlightCtrl.Param.Byte[0]; |
| 602 | Compass_SetCalState(CompassCalState); |
602 | Compass_SetCalState(CompassCalState); |
| 603 | } |
603 | } |
| 604 | else CompassCalState = 0; |
604 | // else CompassCalState = 0; |
| 605 | } |
605 | } |
| 606 | Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1]; |
606 | Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1]; |
| 607 | NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch |
607 | NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch |
| 608 | NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm |
608 | NaviData.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm |