int main(int argc, char** argv) { read_ascii_flight_log(IN_FILE); imu_init(); ahrs_init(); for (int i=0; i<nb_samples; i++) { feed_imu(i); if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); } else { ahrs_propagate((1./AHRS_PROPAGATE_FREQUENCY)); #ifdef ENABLE_MAG_UPDATE if (MAG_AVAILABLE(samples[i].flag)) ahrs_update_mag(); #endif #ifdef ENABLE_ACCEL_UPDATE if (IMU_AVAILABLE(samples[i].flag) && (!MAG_AVAILABLE(samples[i].flag))) ahrs_update_accel(); #endif } store_filter_output(i); } dump_output(OUT_FILE); }
gboolean timeout_callback(gpointer data) { for (int i=0; i<20; i++) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_PROPAGATE ahrs_propagate(); #endif #ifndef DISABLE_ACCEL_UPDATE ahrs_update_accel(); #endif #ifndef DISABLE_MAG_UPDATE if (!(i%5)) ahrs_update_mag(); #endif } #if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", ahrs_impl.gyro_bias.p, ahrs_impl.gyro_bias.q, ahrs_impl.gyro_bias.r); #endif #if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 struct Int32Rates bias_i; RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias); IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d", bias_i.p, bias_i.q, bias_i.r); #endif IvySendMsg("183 AHRS_EULER %f %f %f", ahrs_float.ltp_to_imu_euler.phi, ahrs_float.ltp_to_imu_euler.theta, ahrs_float.ltp_to_imu_euler.psi); IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", DegOfRad(aos.imu_rates.p), DegOfRad(aos.imu_rates.q), DegOfRad(aos.imu_rates.r), DegOfRad(aos.ltp_to_imu_euler.phi), DegOfRad(aos.ltp_to_imu_euler.theta), DegOfRad(aos.ltp_to_imu_euler.psi)); IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f", DegOfRad(aos.gyro_bias.p), DegOfRad(aos.gyro_bias.q), DegOfRad(aos.gyro_bias.r)); return TRUE; }
static inline void on_gyro_accel_event( void ) { ImuScaleGyro(imu); ImuScaleAccel(imu); if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); } else { ahrs_propagate(); ahrs_update_accel(); ahrs_update_fw_estimator(); } }
void aos_run(void) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_ALIGNEMENT if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { ahrs_align(); } } else { #endif /* DISABLE_ALIGNEMENT */ ahrs_propagate(aos.dt); ahrs_update_accel(); #ifndef DISABLE_MAG_UPDATE ahrs_update_mag(); #endif #if AHRS_USE_GPS_HEADING #if AHRS_TYPE == AHRS_TYPE_ICQ int32_t heading = ANGLE_BFP_OF_REAL(aos.heading_meas); #endif #if AHRS_TYPE == AHRS_TYPE_FCQ float heading = aos.heading_meas; #endif #if AHRS_TYPE == AHRS_TYPE_FCR ahrs_impl.gps_course = aos.heading_meas; ahrs_impl.gps_course_valid = true; #else if (aos.time > 10) { if (!ahrs_impl.heading_aligned) { ahrs_realign_heading(heading); } else { RunOnceEvery(100, ahrs_update_heading(heading)); } } #endif #endif // AHRS_USE_GPS_HEADING #ifndef DISABLE_ALIGNEMENT } #endif }
void aos_run(void) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_ALIGNEMENT if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); } else { #endif /* DISABLE_ALIGNEMENT */ ahrs_propagate(); ahrs_update_accel(); ahrs_update_mag(); #ifndef DISABLE_ALIGNEMENT } #endif }
static inline void on_gyro_accel_event( void ) { #ifdef AHRS_CPU_LED LED_ON(AHRS_CPU_LED); #endif // Run aligner on raw data as it also makes averages. if (ahrs.status == AHRS_UNINIT) { ImuScaleGyro(imu); ImuScaleAccel(imu); ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); return; } #if PERIODIC_FREQUENCY == 60 ImuScaleGyro(imu); ImuScaleAccel(imu); ahrs_propagate(); ahrs_update_accel(); ahrs_update_fw_estimator(); #else //PERIODIC_FREQUENCY static uint8_t _reduced_propagation_rate = 0; static uint8_t _reduced_correction_rate = 0; static struct Int32Vect3 acc_avg; static struct Int32Rates gyr_avg; RATES_ADD(gyr_avg, imu.gyro_unscaled); VECT3_ADD(acc_avg, imu.accel_unscaled); _reduced_propagation_rate++; if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY)) { } else { _reduced_propagation_rate = 0; RATES_SDIV(imu.gyro_unscaled, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) ); INT_RATES_ZERO(gyr_avg); ImuScaleGyro(imu); ahrs_propagate(); _reduced_correction_rate++; if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY)) { _reduced_correction_rate = 0; VECT3_SDIV(imu.accel_unscaled, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) ); INT_VECT3_ZERO(acc_avg); ImuScaleAccel(imu); ahrs_update_accel(); ahrs_update_fw_estimator(); } } #endif //PERIODIC_FREQUENCY #ifdef AHRS_CPU_LED LED_OFF(AHRS_CPU_LED); #endif #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP new_ins_attitude = 1; #endif }
void ahrs_update(void) { ahrs_update_accel(); ahrs_update_mag(); }