void ahrs_propagate(void) { /* converts gyro to floating point */ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev); /* unbias measurement */ RATES_SUB(gyro_float, ahrs_impl.gyro_bias); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES const float alpha = 0.1; FLOAT_RATES_LIN_CMB(ahrs_float.imu_rate, ahrs_float.imu_rate, (1.-alpha), gyro_float, alpha); #else RATES_COPY(ahrs_float.imu_rate,gyro_float); #endif /* add correction */ struct FloatRates omega; RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction); /* and zeros it */ FLOAT_RATES_ZERO(ahrs_impl.rate_correction); const float dt = 1./AHRS_PROPAGATE_FREQUENCY; #ifdef AHRS_PROPAGATE_RMAT FLOAT_RMAT_INTEGRATE_FI(ahrs_float.ltp_to_imu_rmat, omega, dt ); float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat); compute_imu_quat_and_euler_from_rmat(); #endif #ifdef AHRS_PROPAGATE_QUAT FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt); FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat); compute_imu_rmat_and_euler_from_quat(); #endif compute_body_orientation_and_rates(); }
void ahrs_propagate(void) { /* converts gyro to floating point */ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev); /* unbias measurement */ RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias); const float dt = 1./512.; /* add correction */ struct FloatRates omega; RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction); // DISPLAY_FLOAT_RATES("omega ", omega); /* and zeros it */ FLOAT_RATES_ZERO(ahrs_impl.rate_correction); /* first order integration of rotation matrix */ struct FloatRMat exp_omega_dt = { { 1. , dt*omega.r, -dt*omega.q, -dt*omega.r, 1. , dt*omega.p, dt*omega.q, -dt*omega.p, 1. }}; struct FloatRMat R_tdt; FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt); memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt)); float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat); // struct FloatRMat test; // FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat); // DISPLAY_FLOAT_RMAT("foo", test); compute_imu_quat_and_euler_from_rmat(); compute_body_orientation_and_rates(); }