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;
}
