diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 19e41e0ab2..8747a45f4e 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -50,28 +50,32 @@ #include "dsm.h" #include -#ifndef GPIO_SPEKTRUM_PWR_EN -#warning DSM input driver not supporting binding by this board config - #define POWER_SPEKTRUM(arg) -#endif - #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) - #define up_udelay(arg) + #define dsm_udelay(arg) usleep(arg) +#else + #include + #define dsm_udelay(arg) up_udelay(arg) #endif -#define DSM_FRAME_SIZE 16 /** 1000000) && (dsm_channel_shift != 0)) { + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { dsm_guess_format(true); } - /* we have received something we think is a dsm_frame */ - dsm_last_frame_time = frame_time; - /* if we don't know the dsm_frame format, update the guessing state machine */ if (dsm_channel_shift == 0) { - dsm_guess_format(false); - return false; + if (!dsm_guess_format(false)) { + return false; + } } /* @@ -377,6 +402,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig continue; } + /* reset bit guessing state machine if the channel index is out of bounds */ + if (channel > DSM_MAX_CHANNEL_COUNT) { + dsm_guess_format(true); + return false; + } + /* ignore channels out of range */ if (channel >= max_values) { continue; @@ -408,6 +439,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig /* scaled integer for decent accuracy while staying efficient */ value = ((((int)value - 1024) * 1000) / 1700) + 1500; + /* if the value is unrealistic, fail the parsing entirely */ + if (value < 600 || value > 2400) { +#ifdef DSM_DEBUG + printf("DSM: VALUE RANGE FAIL\n"); +#endif + dsm_chan_count = 0; + return false; + } + /* * Store the decoded channel into the R/C input buffer, taking into * account the different ideas about channel assignement that we have. @@ -444,14 +484,20 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig *num_values = 12; } - if (dsm_channel_shift == 11) { - /* Set the 11-bit data indicator */ - *num_values |= 0x8000; - } + /* Set the 11-bit data indicator */ + *dsm_11_bit = (dsm_channel_shift == 11); + + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ + +#ifdef DSM_DEBUG + printf("PARSED PACKET\n"); +#endif + return true; } @@ -477,55 +523,148 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig * @return true=decoded raw channel values updated, false=no update */ bool -dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values) +dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, + unsigned max_values) { - ssize_t ret; - hrt_abstime now; + int ret = 1; + hrt_abstime now; - /* - */ - now = hrt_absolute_time(); + /* + * The S.BUS protocol doesn't provide reliable framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 3ms passes between calls, + * the first byte we read will be the first byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ + now = hrt_absolute_time(); - if ((now - dsm_last_rx_time) > 5000) { - if (dsm_partial_frame_count > 0) { - dsm_frame_drops++; - dsm_partial_frame_count = 0; - } - } + /* + * Fetch bytes, but no more than we would need to complete + * a complete frame. + */ - /* - * Fetch bytes, but no more than we would need to complete - * the current dsm frame. - */ - ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); + ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0])); - /* if the read failed for any reason, just give up here */ - if (ret < 1) { - return false; + /* if the read failed for any reason, just give up here */ + if (ret < 1) { + return false; - } else { - *n_bytes = ret; - *bytes = &dsm_frame[dsm_partial_frame_count]; - } + } else { + *n_bytes = ret; + *bytes = &dsm_buf[0]; + } - dsm_last_rx_time = now; - - /* - * Add bytes to the current dsm frame - */ - dsm_partial_frame_count += ret; - - /* - * If we don't have a full dsm frame, return - */ - if (dsm_partial_frame_count < DSM_FRAME_SIZE) { - return false; - } - - /* - * Great, it looks like we might have a dsm frame. Go ahead and - * decode it. - */ - dsm_partial_frame_count = 0; - return dsm_decode(now, values, num_values, max_values); + /* + * Try to decode something with what we got + */ + return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values); +} + +bool +dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels) +{ + + /* this is set by the decoding state machine and will default to false + * once everything that was decodable has been decoded. + */ + bool decode_ret = false; + + /* keep decoding until we have consumed the buffer */ + for (unsigned d = 0; d < len; d++) { + + /* overflow check */ + if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) { + dsm_partial_frame_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; +#ifdef DSM_DEBUG + printf("DSM: RESET (BUF LIM)\n"); +#endif + } + + if (dsm_partial_frame_count == DSM_FRAME_SIZE) { + dsm_partial_frame_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; +#ifdef DSM_DEBUG + printf("DSM: RESET (PACKET LIM)\n"); +#endif + } + +#ifdef DSM_DEBUG +#if 1 + printf("dsm state: %s%s, count: %d, val: %02x\n", + (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "", + (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "", + dsm_partial_frame_count, + (unsigned)frame[d]); +#endif +#endif + + switch (dsm_decode_state) { + case DSM_DECODE_STATE_DESYNC: + + /* we are de-synced and only interested in the frame marker */ + if ((now - dsm_last_rx_time) > 5000) { + dsm_decode_state = DSM_DECODE_STATE_SYNC; + dsm_partial_frame_count = 0; + dsm_chan_count = 0; + dsm_frame[dsm_partial_frame_count++] = frame[d]; + } + + break; + + case DSM_DECODE_STATE_SYNC: { + dsm_frame[dsm_partial_frame_count++] = frame[d]; + + /* decode whatever we got and expect */ + if (dsm_partial_frame_count < DSM_FRAME_SIZE) { + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels); + + /* we consumed the partial frame, reset */ + dsm_partial_frame_count = 0; + + /* if decoding failed, set proto to desync */ + if (decode_ret == false) { + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + } + } + break; + + default: +#ifdef DSM_DEBUG + printf("UNKNOWN PROTO STATE"); +#endif + decode_ret = false; + } + + + } + + if (frame_drops) { + *frame_drops = dsm_frame_drops; + } + + if (decode_ret) { + *num_values = dsm_chan_count; + } + + dsm_last_rx_time = now; + + /* return false as default */ + return decode_ret; } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index acef60705d..f75fda9251 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -42,20 +42,34 @@ #pragma once #include +#include +#include +#include __BEGIN_DECLS +#define DSM_FRAME_SIZE 16 /**< DSM frame size in bytes */ +#define DSM_FRAME_CHANNELS 7 /**< Max supported DSM channels per frame */ +#define DSM_MAX_CHANNEL_COUNT 18 /**< Max channel count of any DSM RC */ +#define DSM_BUFFER_SIZE (DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2) + __EXPORT int dsm_init(const char *device); __EXPORT int dsm_config(int dsm_fd); -__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); -__EXPORT void dsm_bind(uint16_t cmd, int pulses); +__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); -enum { /* DSM bind states */ +__EXPORT bool dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels); + +#ifdef GPIO_SPEKTRUM_PWR_EN +__EXPORT void dsm_bind(uint16_t cmd, int pulses); +#endif + +enum DSM_CMD { /* DSM bind states */ DSM_CMD_BIND_POWER_DOWN = 0, DSM_CMD_BIND_POWER_UP, DSM_CMD_BIND_SET_RX_OUT, DSM_CMD_BIND_SEND_PULSES, DSM_CMD_BIND_REINIT_UART -} DSM_CMD; +}; __END_DECLS