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);
}
void ahrs_align(void) {
get_phi_theta_measurement_fom_accel(&ahrs_impl.hi_res_euler.phi, &ahrs_impl.hi_res_euler.theta, ahrs_aligner.lp_accel);
get_psi_measurement_from_mag(&ahrs_impl.hi_res_euler.psi,
ahrs_impl.hi_res_euler.phi/F_UPDATE, ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag);
EULERS_COPY(ahrs_impl.measure, ahrs_impl.hi_res_euler);
EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler);
/* 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();
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
ahrs.status = AHRS_RUNNING;
}
bool ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
struct Int32Vect3 *lp_mag)
{
get_phi_theta_measurement_fom_accel(&ahrs_ice.hi_res_euler.phi,
&ahrs_ice.hi_res_euler.theta, lp_accel);
get_psi_measurement_from_mag(&ahrs_ice.hi_res_euler.psi,
ahrs_ice.hi_res_euler.phi / F_UPDATE,
ahrs_ice.hi_res_euler.theta / F_UPDATE, lp_mag);
EULERS_COPY(ahrs_ice.measure, ahrs_ice.hi_res_euler);
EULERS_COPY(ahrs_ice.measurement, ahrs_ice.hi_res_euler);
/* Compute LTP to IMU eulers */
EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE);
RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro);
ahrs_ice.status = AHRS_ICE_RUNNING;
ahrs_ice.is_aligned = true;
return true;
}