diff --git a/sw/airborne/subsystems/radio_control/sbus.c b/sw/airborne/subsystems/radio_control/sbus.c index 726e0781c5..e5e9f704db 100644 --- a/sw/airborne/subsystems/radio_control/sbus.c +++ b/sw/airborne/subsystems/radio_control/sbus.c @@ -59,6 +59,23 @@ /** SBUS struct */ struct _sbus sbus; +// Telemetry function +#if DOWNLINK +#ifdef FBW +#define DOWNLINK_TELEMETRY &telemetry_Fbw +#else +#define DOWNLINK_TELEMETRY DefaultPeriodic +#endif + +#include "subsystems/datalink/telemetry.h" + +static void send_sbus(void) { + // Using PPM message for simplicity + DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, + &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus.pulses); +} +#endif + // Init function void radio_control_impl_init(void) { sbus.frame_available = FALSE; @@ -73,6 +90,11 @@ void radio_control_impl_init(void) { gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); #endif + + // Register telemetry message +#if DOWNLINK + register_periodic_telemetry(DOWNLINK_TELEMETRY, "PPM", send_sbus); +#endif }