void estimator_update_state_gps( void ) {
float gps_east = gps_utm_east / 100.;
float gps_north = gps_utm_north / 100.;
/* Relative position to reference */
gps_east -= nav_utm_east0;
gps_north -= nav_utm_north0;
EstimatorSetPosXY(gps_east, gps_north);
#ifndef USE_BARO_ETS
float falt = gps_alt / 100.;
EstimatorSetAlt(falt);
#endif
float fspeed = gps_gspeed / 100.;
float fclimb = gps_climb / 100.;
float fcourse = RadOfDeg(gps_course / 10.);
EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
// Heading estimator from wind-information, usually computed with -DWIND_INFO
// wind_north and wind_east initialized to 0, so still correct if not updated
float w_vn = cosf(estimator_hspeed_dir) * estimator_hspeed_mod - wind_north;
float w_ve = sinf(estimator_hspeed_dir) * estimator_hspeed_mod - wind_east;
estimator_psi = atan2f(w_ve, w_vn);
if (estimator_psi < 0.)
estimator_psi += 2 * M_PI;
#ifdef EXTRA_DOWNLINK_DEVICE
DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
#endif
}
void handle_ins_msg( void) {
// Send to Estimator (Control)
#ifdef XSENS_BACKWARDS
EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral));
EstimatorSetRate(-ins_p,-ins_q);
#else
EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
EstimatorSetRate(ins_p,ins_q);
#endif
// Position
float gps_east = gps.utm_pos.east / 100.;
float gps_north = gps.utm_pos.north / 100.;
gps_east -= nav_utm_east0;
gps_north -= nav_utm_north0;
EstimatorSetPosXY(gps_east, gps_north);
// Altitude and vertical speed
float hmsl = gps.hmsl;
hmsl /= 1000.0f;
EstimatorSetAlt(hmsl);
#ifndef ALT_KALMAN
#warning NO_VZ
#endif
// Horizontal speed
float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
if (gps.fix != GPS_FIX_3D)
{
fspeed = 0;
}
float fclimb = -ins_vz;
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
// Now also finish filling the gps struct for telemetry purposes
gps.gspeed = fspeed * 100.;
gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
gps.course = fcourse * 1e7;
}
void estimator_init( void ) {
EstimatorSetPosXY(0., 0.);
EstimatorSetAlt(0.);
EstimatorSetAtt (0., 0., 0);
EstimatorSetSpeedPol ( 0., 0., 0.);
EstimatorSetRate(0., 0.);
#ifdef USE_AIRSPEED
EstimatorSetAirspeed( 0. );
#endif
#ifdef USE_AOA
EstimatorSetAOA( 0. );
#endif
estimator_flight_time = 0;
estimator_airspeed = NOMINAL_AIRSPEED;
}
