static inline void update_fw_estimator(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, ins_r);
#else
EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
EstimatorSetRate(ins_p, ins_q, ins_r);
#endif
}
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
{
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
}
else if(CHIMU_DATA.m_MsgID==0x02)
{
RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
}
}
}
}
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 handle_ins_msg( void) {
EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
EstimatorSetRate(ins_p,ins_q);
if (gps_mode != 0x03)
gps_gspeed = 0;
//gps_course = ins_psi * 1800 / M_PI;
gps_course = (DegOfRad(atan2f((float)ins_vx, (float)ins_vy))*10.0f);
gps_climb = (int16_t)(-ins_vz * 100);
gps_gspeed = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy) * 100);
EstimatorSetAtt(ins_phi, RadOfDeg(((float)gps_course) / 10.0), ins_theta);
// EstimatorSetSpeedPol(gps_gspeed, gps_course, ins_vz);
// EstimatorSetAlt(ins_z);
estimator_update_state_gps();
reset_gps_watchdog();
}
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;
}
