diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index bfec926bce..ba1ff6078c 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -57,7 +57,16 @@ #define SBUS_FLAGS_BYTE 23 #define SBUS_FAILSAFE_BIT 3 #define SBUS_FRAMELOST_BIT 2 -#define SBUS1_FRAME_DELAY 14000 + +// testing with a SBUS->PWM adapter shows that +// above 300Hz SBUS becomes unreliable. 333 would +// be the theoretical achievable, but at 333Hz some +// frames are lost +#define SBUS1_MAX_RATE_HZ 300 +#define SBUS1_MIN_RATE_HZ 50 + +// this is the rate of the old code +#define SBUS1_DEFAULT_RATE_HZ 72 #define SBUS_SINGLE_CHAR_LEN_US (1/((100000/10)) * 1000 * 1000) @@ -108,6 +117,7 @@ static enum SBUS2_DECODE_STATE { static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)]; static unsigned partial_frame_count; +static unsigned sbus1_frame_delay = (1000U*1000U)/SBUS1_DEFAULT_RATE_HZ; static unsigned sbus_frame_drops; @@ -179,7 +189,7 @@ sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values) now = hrt_absolute_time(); - if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) { + if ((now - last_txframe_time) > sbus1_frame_delay) { last_txframe_time = now; uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f }; @@ -627,3 +637,17 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num return true; } + +/* + set output rate of SBUS in Hz + */ +void sbus1_set_output_rate_hz(uint16_t rate_hz) +{ + if (rate_hz > SBUS1_MAX_RATE_HZ) { + rate_hz = SBUS1_MAX_RATE_HZ; + } + if (rate_hz < SBUS1_MIN_RATE_HZ) { + rate_hz = SBUS1_MIN_RATE_HZ; + } + sbus1_frame_delay = (1000U*1000U) / rate_hz; +} diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 2c8f389c86..418657d5a0 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -76,6 +76,7 @@ __EXPORT bool sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *v uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, unsigned *frame_drops, uint16_t max_channels); __EXPORT void sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values); __EXPORT void sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values); +__EXPORT void sbus1_set_output_rate_hz(uint16_t rate_hz); /** * The number of incomplete frames we encountered