diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c index ce8d9c62ae..9f1f4a1d7a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c @@ -67,18 +67,28 @@ static void send_bias(struct transport_tx *trans, struct link_device *dev) static void send_euler_int(struct transport_tx *trans, struct link_device *dev) { + /* compute eulers in int (IMU frame) */ struct FloatEulers ltp_to_imu_euler; float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat); - struct Int32Eulers euler_i; - EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); - struct Int32Eulers *eulers_body = stateGetNedToBodyEulers_i(); + 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); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, - &euler_i.phi, - &euler_i.theta, - &euler_i.psi, - &(eulers_body->phi), - &(eulers_body->theta), - &(eulers_body->psi) + &eulers_imu.phi, + &eulers_imu.theta, + &eulers_imu.psi, + &eulers_body.phi, + &eulers_body.theta, + &eulers_body.psi, &ahrs_fc_id); } diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c index c11e1ab417..77e31dd0cb 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_invariant_wrapper.c @@ -50,18 +50,28 @@ static void compute_body_orientation_and_rates(void); static void send_att(struct transport_tx *trans, struct link_device *dev) { + /* compute eulers in int (IMU frame) */ struct FloatEulers ltp_to_imu_euler; float_eulers_of_quat(<p_to_imu_euler, &ahrs_float_inv.state.quat); - struct Int32Eulers euler_i; - EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); - struct Int32Eulers *eulers_body = stateGetNedToBodyEulers_i(); + 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_float_inv.body_to_imu); + float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.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); + pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID, - &euler_i.phi, - &euler_i.theta, - &euler_i.psi, - &(eulers_body->phi), - &(eulers_body->theta), - &(eulers_body->psi), + &eulers_imu.phi, + &eulers_imu.theta, + &eulers_imu.psi, + &eulers_body.phi, + &eulers_body.theta, + &eulers_body.psi, &ahrs_finv_id); }