static void traj_static_sine_update(void) { aos.imu_rates.p = RadOfDeg(200) * cos(aos.time); aos.imu_rates.q = RadOfDeg(200) * cos(0.7 * aos.time + 2); aos.imu_rates.r = RadOfDeg(200) * cos(0.8 * aos.time + 1); float_quat_integrate(&aos.ltp_to_imu_quat, &aos.imu_rates, aos.dt); float_eulers_of_quat(&aos.ltp_to_imu_euler, &aos.ltp_to_imu_quat); }
static void traj_step_phi_2nd_order_update(void) { if (aos.time > 15) { const float omega = RadOfDeg(100); const float xi = 0.9; struct FloatRates raccel; RATES_ASSIGN(raccel, -2.*xi * omega * aos.imu_rates.p - omega * omega * (aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., 0.); float_rates_integrate_fi(&aos.imu_rates, &raccel, aos.dt); float_quat_integrate(&aos.ltp_to_imu_quat, &aos.imu_rates, aos.dt); float_eulers_of_quat(&aos.ltp_to_imu_euler, &aos.ltp_to_imu_quat); } }
void ahrs_fc_propagate(struct Int32Rates *gyro, float dt) { /* converts gyro to floating point */ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, *gyro); /* unbias measurement */ RATES_SUB(gyro_float, ahrs_fc.gyro_bias); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES const float alpha = 0.1; FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), gyro_float, alpha); #else RATES_COPY(ahrs_fc.imu_rate, gyro_float); #endif /* add correction */ struct FloatRates omega; RATES_SUM(omega, gyro_float, ahrs_fc.rate_correction); /* and zeros it */ FLOAT_RATES_ZERO(ahrs_fc.rate_correction); #if AHRS_PROPAGATE_RMAT float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt); float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat); float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat); #endif #if AHRS_PROPAGATE_QUAT float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt); float_quat_normalize(&ahrs_fc.ltp_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); #endif // increase accel and mag propagation counters ahrs_fc.accel_cnt++; ahrs_fc.mag_cnt++; }