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; }
void ahrs_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag) { /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ahrs_float_inv.state.quat, lp_accel, lp_mag); /* use average gyro as initial value for bias */ ahrs_float_inv.state.bias = *lp_gyro; // ins and ahrs are now running ahrs_float_inv.is_aligned = true; }
void ahrs_align(void) { /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* use average gyro as initial value for bias */ struct FloatRates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0); // ins and ahrs are now running ahrs.status = AHRS_RUNNING; ins.status = INS_RUNNING; }
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 ins_reset_altitude_ref(void) { #if INS_UPDATE_FW_ESTIMATOR struct UtmCoor_f utm = state.utm_origin_f; utm.alt = gps.hmsl / 1000.0f; stateSetLocalUtmOrigin_f(&utm); #else struct LlaCoor_i lla = { .lat = state.ned_origin_i.lla.lat, .lon = state.ned_origin_i.lla.lon, .alt = gps.lla_pos.alt }; struct LtpDef_i ltp_def; ltp_def_from_lla_i(<p_def, &lla); ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(<p_def); #endif } void ahrs_init(void) { ahrs.status = AHRS_UNINIT; } void ahrs_align(void) { /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* use average gyro as initial value for bias */ struct FloatRates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0); // ins and ahrs are now running ahrs.status = AHRS_RUNNING; ins.status = INS_RUNNING; }