bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ahrs_fc.ltp_to_imu_quat, lp_accel, lp_mag); ahrs_fc.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ ahrs_float_get_quat_from_accel(&ahrs_fc.ltp_to_imu_quat, lp_accel); ahrs_fc.heading_aligned = FALSE; #endif /* Convert initial orientation from quat to rotation matrix representations. */ float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, *lp_gyro); RATES_FLOAT_OF_BFP(ahrs_fc.gyro_bias, bias0); ahrs_fc.status = AHRS_FC_RUNNING; ahrs_fc.is_aligned = TRUE; return TRUE; }
void ahrs_align(void) { #if USE_MAGNETOMETER /* 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); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); ahrs_impl.heading_aligned = FALSE; #endif /* Convert initial orientation from quat to rotation matrix representations. */ FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); /* Compute initial body orientation */ compute_body_orientation_and_rates(); /* 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; }