diff --git a/conf/radios/T10CG_SBUS.xml b/conf/radios/T10CG_SBUS.xml index 137caa7b20..747b8329b3 100644 --- a/conf/radios/T10CG_SBUS.xml +++ b/conf/radios/T10CG_SBUS.xml @@ -45,11 +45,11 @@ - - - - - - - + + + + + + + diff --git a/sw/airborne/subsystems/radio_control/sbus.c b/sw/airborne/subsystems/radio_control/sbus.c index f704fee093..7dff0b4467 100644 --- a/sw/airborne/subsystems/radio_control/sbus.c +++ b/sw/airborne/subsystems/radio_control/sbus.c @@ -70,9 +70,9 @@ struct _sbus sbus; #include "subsystems/datalink/telemetry.h" static void send_sbus(void) { - // Using PPM message for simplicity + // Using PPM message DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice, - &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus.pulses); + &radio_control.frame_rate, SBUS_NB_CHANNEL, sbus.ppm); } #endif @@ -99,7 +99,7 @@ void radio_control_impl_init(void) { /** Decode the raw buffer */ -static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available) +static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available, uint16_t *dstppm) { // reset counters uint8_t byteInRawBuf = 0; @@ -124,6 +124,9 @@ static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *avail } if (bitInChannel == SBUS_BIT_PER_CHANNEL) { bitInChannel = 0; +#if PERIODIC_TELEMETRY + dstppm[channel] = USEC_OF_RC_PPM_TICKS(dst[channel]); +#endif channel++; } } @@ -153,7 +156,7 @@ void sbus_decode_event(void) { if (sbus.idx == SBUS_BUF_LENGTH) { // Decode if last byte is the correct end byte if (rbyte == SBUS_END_BYTE) { - decode_sbus_buffer(sbus.buffer, sbus.pulses, &sbus.frame_available); + decode_sbus_buffer(sbus.buffer, sbus.pulses, &sbus.frame_available, sbus.ppm); } sbus.status = SBUS_STATUS_UNINIT; } diff --git a/sw/airborne/subsystems/radio_control/sbus.h b/sw/airborne/subsystems/radio_control/sbus.h index 3f344194a7..4cfc202551 100644 --- a/sw/airborne/subsystems/radio_control/sbus.h +++ b/sw/airborne/subsystems/radio_control/sbus.h @@ -30,11 +30,14 @@ #include "std.h" /** - * Dummy macro to use radio.h file + * Macro to use radio.h file + * + * SBUS: 0..1024..2047 (sweep 2048) + * PPM: 880..1520..2160 (sweep 1280) */ -#define RC_PPM_TICKS_OF_USEC(_v) (_v) -#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (_v) -#define USEC_OF_RC_PPM_TICKS(_v) (_v) +#define RC_PPM_TICKS_OF_USEC(_v) ((((_v) - 880) * 8) / 5) +#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (((_v) * 8) / 5) +#define USEC_OF_RC_PPM_TICKS(_v) ((((_v) * 5) / 8) + 880) /** * Generated code holding the description of a given @@ -60,6 +63,7 @@ */ struct _sbus { uint16_t pulses[SBUS_NB_CHANNEL]; ///< decoded values + uint16_t ppm[SBUS_NB_CHANNEL]; ///< decoded and converted values bool_t frame_available; ///< new frame available uint8_t buffer[SBUS_BUF_LENGTH]; ///< input buffer uint8_t idx; ///< input index