Rev 48 | Rev 54 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 48 | Rev 49 | ||
---|---|---|---|
Line 99... | Line 99... | ||
99 | s32 FC_Kalman_K = 32; |
99 | s32 FC_Kalman_K = 32; |
100 | s32 Kalman_MaxDrift = 5 * 16; |
100 | s32 Kalman_MaxDrift = 5 * 16; |
101 | s32 Kalman_MaxFusion = 64; |
101 | s32 Kalman_MaxFusion = 64; |
102 | u32 CheckSPIOkay = 0; |
102 | u32 CheckSPIOkay = 0; |
Line 103... | Line 103... | ||
103 | 103 | ||
104 | u8 SPI_CommandSequence[] = { SPI_KALMAN }; |
104 | u8 SPI_CommandSequence[] = { SPI_KALMAN, SPI_CMD_OSD_DATA, SPI_CMD_GPS_POS, SPI_KALMAN, SPI_CMD_GPS_TARGET}; |
Line 105... | Line 105... | ||
105 | u8 SPI_CommandCounter = 0; |
105 | u8 SPI_CommandCounter = 0; |
Line 106... | Line 106... | ||
106 | 106 | ||
Line 296... | Line 296... | ||
296 | ToFlightCtrl.Param.Byte[0] = (char) FC_Kalman_K; |
296 | ToFlightCtrl.Param.Byte[0] = (char) FC_Kalman_K; |
297 | ToFlightCtrl.Param.Byte[1] = (char) Kalman_MaxFusion; |
297 | ToFlightCtrl.Param.Byte[1] = (char) Kalman_MaxFusion; |
298 | ToFlightCtrl.Param.Byte[2] = (char) Kalman_MaxDrift; |
298 | ToFlightCtrl.Param.Byte[2] = (char) Kalman_MaxDrift; |
299 | break; |
299 | break; |
Line -... | Line 300... | ||
- | 300 | ||
- | 301 | case SPI_CMD_GPS_POS: |
|
- | 302 | ToFlightCtrl.Param.Long[0] = GPSData.Position.Longitude; |
|
- | 303 | ToFlightCtrl.Param.Long[1] = GPSData.Position.Latitude; |
|
- | 304 | break; |
|
- | 305 | ||
- | 306 | case SPI_CMD_GPS_TARGET: |
|
- | 307 | if(GPS_pTargetPosition != NULL) |
|
- | 308 | { |
|
- | 309 | if(GPS_pTargetPosition->Status != INVALID) |
|
- | 310 | { |
|
- | 311 | ToFlightCtrl.Param.Long[0] = GPS_pTargetPosition->Longitude; |
|
- | 312 | ToFlightCtrl.Param.Long[1] = GPS_pTargetPosition->Latitude; |
|
- | 313 | } |
|
- | 314 | else |
|
- | 315 | { |
|
- | 316 | ToFlightCtrl.Param.Long[0] = 0; |
|
- | 317 | ToFlightCtrl.Param.Long[1] = 0; |
|
- | 318 | } |
|
- | 319 | } |
|
- | 320 | else |
|
- | 321 | { |
|
- | 322 | ToFlightCtrl.Param.Long[0] = 0; |
|
- | 323 | ToFlightCtrl.Param.Long[1] = 0; |
|
- | 324 | } |
|
- | 325 | break; |
|
300 | 326 | ||
301 | default: |
327 | default: |
302 | break; |
328 | break; |
303 | } |
329 | } |