diff --git a/sw/airborne/math/pprz_orientation_conversion.h b/sw/airborne/math/pprz_orientation_conversion.h index 941480d1d0..f2ce4b5b9d 100644 --- a/sw/airborne/math/pprz_orientation_conversion.h +++ b/sw/airborne/math/pprz_orientation_conversion.h @@ -137,6 +137,14 @@ static inline bool_t orienationCheckValid(struct OrientationReps *orientation) return (orientation->status); } +/// Set to identity orientation. +static inline void orientationSetIdentity(struct OrientationReps *orientation) +{ + int32_quat_identity(&orientation->quat_i); + /* clear bits for all attitude representations and only set the new one */ + orientation->status = (1 << ORREP_QUAT_I); +} + /// Set vehicle body attitude from quaternion (int). static inline void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat) { diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 6c96877098..e4fbe8d112 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -94,6 +94,9 @@ void ahrs_fc_init(void) /* init ltp_to_imu quaternion as zero/identity rotation */ float_quat_identity(&ahrs_fc.ltp_to_imu_quat); + orientationSetIdentity(&ahrs_fc.body_to_imu); + orientationSetIdentity(&ahrs_fc.ltp_to_body); + FLOAT_RATES_ZERO(ahrs_fc.imu_rate); /* set default filter cut-off frequency and damping */ @@ -434,16 +437,14 @@ void ahrs_fc_update_heading(float heading) FLOAT_ANGLE_NORMALIZE(heading); - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); - struct FloatQuat ltp_to_body_quat; - float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); - struct FloatRMat ltp_to_body_rmat; - float_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat); + ahrs_fc_recompute_ltp_to_body(); + struct FloatRMat *ltp_to_body_rmat = orientationGetRMat_f(&ahrs_fc.ltp_to_body); + // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y struct FloatVect2 expected_ltp = { - RMAT_ELMT(ltp_to_body_rmat, 0, 0), - RMAT_ELMT(ltp_to_body_rmat, 0, 1) + RMAT_ELMT(*ltp_to_body_rmat, 0, 0), + RMAT_ELMT(*ltp_to_body_rmat, 0, 1) }; // expected_heading cross measured_heading @@ -486,12 +487,11 @@ void ahrs_fc_realign_heading(float heading) q_h_new.qz = sinf(heading / 2.0); q_h_new.qi = cosf(heading / 2.0); - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); - struct FloatQuat ltp_to_body_quat; - float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); + ahrs_fc_recompute_ltp_to_body(); + struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body); + /* quaternion representing current heading only */ - struct FloatQuat q_h; - QUAT_COPY(q_h, ltp_to_body_quat); + struct FloatQuat q_h = *ltp_to_body_quat; q_h.qx = 0.; q_h.qy = 0.; float_quat_normalize(&q_h); @@ -502,9 +502,10 @@ void ahrs_fc_realign_heading(float heading) /* correct current heading in body frame */ struct FloatQuat q; - float_quat_comp_norm_shortest(&q, &q_c, <p_to_body_quat); + float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat); float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat); @@ -526,3 +527,11 @@ void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i) ahrs_fc.ltp_to_imu_rmat = *orientationGetRMat_f(&ahrs_fc.body_to_imu); } } + +void ahrs_fc_recompute_ltp_to_body(void) +{ + struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); + struct FloatQuat ltp_to_body_quat; + float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); + orientationSetQuat_f(&ahrs_fc.ltp_to_body, <p_to_body_quat); +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h index ba271bbf3b..e3e5009074 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.h @@ -70,6 +70,7 @@ struct AhrsFloatCmpl { uint16_t mag_cnt; ///< number of propagations since last mag update struct OrientationReps body_to_imu; + struct OrientationReps ltp_to_body; enum AhrsFCStatus status; bool_t is_aligned; @@ -80,6 +81,7 @@ extern struct AhrsFloatCmpl ahrs_fc; extern void ahrs_fc_init(void); extern void ahrs_fc_set_body_to_imu(struct OrientationReps *body_to_imu); extern void ahrs_fc_set_body_to_imu_quat(struct FloatQuat *q_b2i); +extern void ahrs_fc_recompute_ltp_to_body(void); extern bool_t ahrs_fc_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag); extern void ahrs_fc_propagate(struct Int32Rates *gyro, float dt); diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c index 9f1f4a1d7a..5a04e63790 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c @@ -73,22 +73,17 @@ static void send_euler_int(struct transport_tx *trans, struct link_device *dev) struct Int32Eulers eulers_imu; EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler); - /* compute Eulers in int (body frame) */ - struct FloatQuat ltp_to_body_quat; - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); - float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); - struct FloatEulers ltp_to_body_euler; - float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat); - struct Int32Eulers eulers_body; - EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler); + /* get Eulers in int (body frame) */ + ahrs_fc_recompute_ltp_to_body(); + struct Int32Eulers *eulers_body = orientationGetEulers_i(&ahrs_fc.ltp_to_body); pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, &eulers_imu.phi, &eulers_imu.theta, &eulers_imu.psi, - &eulers_body.phi, - &eulers_body.theta, - &eulers_body.psi, + &eulers_body->phi, + &eulers_body->theta, + &eulers_body->psi, &ahrs_fc_id); } @@ -247,12 +242,11 @@ static bool_t ahrs_fc_enable_output(bool_t enable) static void compute_body_orientation_and_rates(void) { if (ahrs_fc_output_enabled) { - /* Compute LTP to BODY quaternion */ - struct FloatQuat ltp_to_body_quat; - struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu); - float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat); + /* recompute LTP to BODY quaternion */ + ahrs_fc_recompute_ltp_to_body(); + struct FloatQuat *ltp_to_body_quat = orientationGetQuat_f(&ahrs_fc.ltp_to_body); /* Set state */ - stateSetNedToBodyQuat_f(<p_to_body_quat); + stateSetNedToBodyQuat_f(ltp_to_body_quat); /* compute body rates */ struct FloatRates body_rate;