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