void estimator_update_state_gps( void ) { if (GPS_FIX_VALID(gps_mode)) { EstimatorSetPos(gps_east, gps_north, gps_falt); EstimatorSetSpeedPol(gps_fspeed, gps_fcourse, gps_fclimb); if (estimator_flight_time) estimator_update_ir_estim(); } }
void estimator_init( void ) { EstimatorSetPos (0., 0., 0.); EstimatorSetAtt (0., 0., 0); EstimatorSetSpeedPol ( 0., 0., 0.); EstimatorSetRotSpeed (0., 0., 0.); estimator_flight_time = 0; estimator_rad_of_ir = ir_rad_of_ir; }
void estimator_update_state_gps( void ) { float gps_east = gps_utm_east / 100. - nav_utm_east0; float gps_north = gps_utm_north / 100. - nav_utm_north0; float falt = gps_alt / 100.; EstimatorSetPos(gps_east, gps_north, falt); float fspeed = gps_gspeed / 100.; float fclimb = gps_climb / 100.; float fcourse = RadOfDeg(gps_course / 10.); EstimatorSetSpeedPol(fspeed, fcourse, fclimb); #ifdef INFRARED if (estimator_flight_time) estimator_update_ir_estim(); #endif }