void ahrs_propagate(float dt) { int32_t freq = (int32_t)(1./dt); /* unbias gyro */ struct Int32Rates omega; RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias); /* low pass rate */ #ifdef AHRS_PROPAGATE_LOW_PASS_RATES RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate,2); RATES_ADD(ahrs_impl.imu_rate, omega); RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, 3); #else RATES_COPY(ahrs_impl.imu_rate, omega); #endif /* add correction */ RATES_ADD(omega, ahrs_impl.rate_correction); /* and zeros it */ INT_RATES_ZERO(ahrs_impl.rate_correction); /* integrate quaternion */ int32_quat_integrate_fi(&ahrs_impl.ltp_to_imu_quat, &ahrs_impl.high_rez_quat, &omega, freq); int32_quat_normalize(&ahrs_impl.ltp_to_imu_quat); set_body_state_from_quat(); // increase accel and mag propagation counters ahrs_impl.accel_cnt++; ahrs_impl.mag_cnt++; }
void ahrs_propagate(void) { /* unbias gyro */ struct Int32Rates omega; RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias); /* low pass rate */ //#ifdef AHRS_PROPAGATE_LOW_PASS_RATES if (gyro_lowpass_filter > 1) { RATES_SMUL(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter-1); RATES_ADD(ahrs_impl.imu_rate, omega); RATES_SDIV(ahrs_impl.imu_rate, ahrs_impl.imu_rate, gyro_lowpass_filter); //#else } else { RATES_COPY(ahrs_impl.imu_rate, omega); //#endif } /* add correction */ RATES_ADD(omega, ahrs_impl.rate_correction); /* and zeros it */ INT_RATES_ZERO(ahrs_impl.rate_correction); /* integrate quaternion */ INT32_QUAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY); INT32_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat); set_body_state_from_quat(); }
void ahrs_align(void) { /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* set initial body orientation */ set_body_state_from_quat(); /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); ahrs.status = AHRS_RUNNING; }
void ahrs_align(void) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ ahrs_int_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); ahrs_impl.heading_aligned = FALSE; #endif set_body_state_from_quat(); /* 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_propagate(void) { propagate_ref(); propagate_state(); set_body_state_from_quat(); }