Subversion Repositories NaviCtrl

Rev

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