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