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