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 |