void ahrs_propagate(void) { /* unbias gyro */ struct Int32Rates uf_rate; RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias); #if USE_NOISE_CUT static struct Int32Rates last_uf_rate = { 0, 0, 0 }; if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) { #endif /* low pass rate */ #if USE_NOISE_FILTER RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN); RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1); #else RATES_ADD(ahrs.imu_rate, uf_rate); RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2); #endif #if USE_NOISE_CUT } RATES_COPY(last_uf_rate, uf_rate); #endif /* integrate eulers */ struct Int32Eulers euler_dot; INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate); EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot); /* low pass measurement */ EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement); EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2); /* compute residual */ EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.hi_res_euler); INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi); struct Int32Eulers correction; /* compute a correction */ EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1); /* correct estimation */ EULERS_ADD(ahrs_impl.hi_res_euler, correction); INTEG_EULER_NORMALIZE(ahrs_impl.hi_res_euler.psi); /* Compute LTP to IMU eulers */ EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE); compute_imu_quat_and_rmat_from_euler(); compute_body_orientation(); }
void ahrs_ice_propagate(struct Int32Rates *gyro) { /* unbias gyro */ struct Int32Rates uf_rate; RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias); #if USE_NOISE_CUT static struct Int32Rates last_uf_rate = { 0, 0, 0 }; if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) { #endif /* low pass rate */ #if USE_NOISE_FILTER RATES_SUM_SCALED(ahrs_ice.imu_rate, ahrs_ice.imu_rate, uf_rate, NOISE_FILTER_GAIN); RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, NOISE_FILTER_GAIN + 1); #else RATES_ADD(ahrs_ice.imu_rate, uf_rate); RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, 2); #endif #if USE_NOISE_CUT } RATES_COPY(last_uf_rate, uf_rate); #endif /* integrate eulers */ struct Int32Eulers euler_dot; int32_eulers_dot_of_rates(&euler_dot, &ahrs_ice.ltp_to_imu_euler, &ahrs_ice.imu_rate); EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot); /* low pass measurement */ EULERS_ADD(ahrs_ice.measure, ahrs_ice.measurement); EULERS_SDIV(ahrs_ice.measure, ahrs_ice.measure, 2); /* compute residual */ EULERS_DIFF(ahrs_ice.residual, ahrs_ice.measure, ahrs_ice.hi_res_euler); INTEG_EULER_NORMALIZE(ahrs_ice.residual.psi); struct Int32Eulers correction; /* compute a correction */ EULERS_SDIV(correction, ahrs_ice.residual, ahrs_ice.reinj_1); /* correct estimation */ EULERS_ADD(ahrs_ice.hi_res_euler, correction); INTEG_EULER_NORMALIZE(ahrs_ice.hi_res_euler.psi); /* Compute LTP to IMU eulers */ EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE); }