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; }