Rev 49 | Rev 56 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 49 | Rev 54 | ||
---|---|---|---|
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, SPI_CMD_OSD_DATA, SPI_CMD_GPS_POS, SPI_KALMAN, SPI_CMD_GPS_TARGET}; |
104 | u8 SPI_CommandSequence[] = { SPI_KALMAN };//, SPI_CMD_OSD_DATA, SPI_CMD_GPS_POS, SPI_KALMAN, SPI_CMD_GPS_TARGET}; |
105 | u8 SPI_CommandCounter = 0; |
105 | u8 SPI_CommandCounter = 0; |
106 | 106 | ||
Line 107... | Line 107... | ||
107 | SPI_Version_t FC_Version; |
107 | SPI_Version_t FC_Version; |
108 | 108 | ||
Line 295... | Line 295... | ||
295 | case SPI_KALMAN: |
295 | case SPI_KALMAN: |
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; |
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; |
- | |
326 | - | ||
327 | default: |
301 | default: |
328 | break; |
302 | break; |
329 | } |
303 | } |
330 | VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
304 | VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |