diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index fd59954d1d..312819b14f 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -70,6 +70,7 @@
+
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
index ce962c049a..383082ef4c 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_wrapper.c
@@ -46,7 +46,26 @@ static void compute_body_orientation_and_rates(void);
#include "mcu_periph/sys_time.h"
#include "state.h"
-static void send_att(struct transport_tx *trans, struct link_device *dev)
+static void send_euler(struct transport_tx *trans, struct link_device *dev)
+{
+ struct FloatEulers ltp_to_imu_euler;
+ float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat);
+ pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
+ <p_to_imu_euler.phi,
+ <p_to_imu_euler.theta,
+ <p_to_imu_euler.psi,
+ &ahrs_fc_id);
+}
+
+static void send_bias(struct transport_tx *trans, struct link_device *dev)
+{
+ struct Int32Rates gyro_bias;
+ RATES_BFP_OF_REAL(gyro_bias, ahrs_fc.gyro_bias);
+ pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
+ &gyro_bias.p, &gyro_bias.q, &gyro_bias.r, &ahrs_fc_id);
+}
+
+static void send_euler_int(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_quat);
@@ -251,7 +270,9 @@ void ahrs_fc_register(void)
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY
- register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_att);
+ register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER", send_euler);
+ register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias);
+ register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler_int);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
index b53983895a..eeb12f26f8 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_mlkf_wrapper.c
@@ -46,6 +46,25 @@ static void set_body_state_from_quat(void);
#include "subsystems/datalink/telemetry.h"
#include "mcu_periph/sys_time.h"
+static void send_euler(struct transport_tx *trans, struct link_device *dev)
+{
+ struct FloatEulers ltp_to_imu_euler;
+ float_eulers_of_quat(<p_to_imu_euler, &ahrs_mlkf.ltp_to_imu_quat);
+ pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
+ <p_to_imu_euler.phi,
+ <p_to_imu_euler.theta,
+ <p_to_imu_euler.psi,
+ &ahrs_mlkf_id);
+}
+
+static void send_bias(struct transport_tx *trans, struct link_device *dev)
+{
+ struct Int32Rates gyro_bias;
+ RATES_BFP_OF_REAL(gyro_bias, ahrs_mlkf.gyro_bias);
+ pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
+ &gyro_bias.p, &gyro_bias.q, &gyro_bias.r, &ahrs_mlkf_id);
+}
+
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
@@ -203,6 +222,8 @@ void ahrs_mlkf_register(void)
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER", send_euler);
+ register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias);
register_periodic_telemetry(DefaultPeriodic, "GEO_MAG", send_geo_mag);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_filter_status);
#endif