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_align(void) { /* Compute an initial orientation using euler angles */ ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* Convert initial orientation in quaternion and rotation matrice representations. */ compute_imu_quat_and_rmat_from_euler(); compute_body_orientation(); /* Use low passed gyro value as initial bias */ RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28); ahrs.status = AHRS_RUNNING; }
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; }