diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 3271c148e5..22aeeddb63 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -51,17 +51,17 @@ #include #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) - #define dsm_udelay(arg) usleep(arg) +#define dsm_udelay(arg) usleep(arg) #else - #include - #define dsm_udelay(arg) up_udelay(arg) +#include +#define dsm_udelay(arg) up_udelay(arg) #endif //#define DSM_DEBUG static enum DSM_DECODE_STATE { - DSM_DECODE_STATE_DESYNC = 0, - DSM_DECODE_STATE_SYNC + DSM_DECODE_STATE_DESYNC = 0, + DSM_DECODE_STATE_SYNC } dsm_decode_state = DSM_DECODE_STATE_DESYNC; static int dsm_fd = -1; /**< File handle to the DSM UART */ @@ -128,9 +128,9 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va static bool dsm_guess_format(bool reset) { - static uint32_t cs10 = 0; - static uint32_t cs11 = 0; - static unsigned samples = 0; + static uint32_t cs10 = 0; + static uint32_t cs11 = 0; + static unsigned samples = 0; /* reset the 10/11 bit sniffed channel masks */ if (reset) { @@ -138,7 +138,7 @@ dsm_guess_format(bool reset) cs11 = 0; samples = 0; dsm_channel_shift = 0; - return false; + return false; } /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ @@ -160,16 +160,16 @@ dsm_guess_format(bool reset) /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } - samples++; + samples++; #ifdef DSM_DEBUG - printf("dsm guess format: samples: %d %s\n", samples, - (reset) ? "RESET" : ""); + printf("dsm guess format: samples: %d %s\n", samples, + (reset) ? "RESET" : ""); #endif /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples < 5) { - return false; + if (samples < 5) { + return false; } /* @@ -209,25 +209,25 @@ dsm_guess_format(bool reset) if ((votes11 == 1) && (votes10 == 0)) { dsm_channel_shift = 11; #ifdef DSM_DEBUG - printf("DSM: 11-bit format\n"); + printf("DSM: 11-bit format\n"); #endif - return true; + return true; } if ((votes10 == 1) && (votes11 == 0)) { dsm_channel_shift = 10; #ifdef DSM_DEBUG - printf("DSM: 10-bit format\n"); + printf("DSM: 10-bit format\n"); #endif - return true; + return true; } /* call ourselves to reset our state ... we have to try again */ #ifdef DSM_DEBUG - printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11); + printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11); #endif dsm_guess_format(true); - return false; + return false; } int @@ -278,10 +278,10 @@ dsm_init(const char *device) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); } - dsm_channel_shift = 0; - dsm_frame_drops = 0; - dsm_chan_count = 0; - dsm_decode_state = DSM_DECODE_STATE_DESYNC; + dsm_channel_shift = 0; + dsm_frame_drops = 0; + dsm_chan_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; int ret = dsm_config(dsm_fd); @@ -325,17 +325,17 @@ dsm_bind(uint16_t cmd, int pulses) case DSM_CMD_BIND_SET_RX_OUT: /*Set UART RX pin to active output mode*/ - SPEKTRUM_RX_AS_GPIO(); + SPEKTRUM_RX_AS_GPIO(); break; case DSM_CMD_BIND_SEND_PULSES: /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { - dsm_udelay(120); - SPEKTRUM_RX_HIGH(false); - dsm_udelay(120); - SPEKTRUM_RX_HIGH(true); + dsm_udelay(120); + SPEKTRUM_RX_HIGH(false); + dsm_udelay(120); + SPEKTRUM_RX_HIGH(true); } break; @@ -343,7 +343,7 @@ dsm_bind(uint16_t cmd, int pulses) case DSM_CMD_BIND_REINIT_UART: /*Restore USART RX pin to RS232 receive mode*/ - SPEKTRUM_RX_AS_UART(); + SPEKTRUM_RX_AS_UART(); break; } @@ -370,15 +370,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { dsm_guess_format(true); } /* if we don't know the dsm_frame format, update the guessing state machine */ if (dsm_channel_shift == 0) { - if (!dsm_guess_format(false)) { - return false; - } + if (!dsm_guess_format(false)) { + return false; + } } /* @@ -402,11 +402,11 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool 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; - } + /* 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) { @@ -475,31 +475,31 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *num_values = 12; } - /* Set the 11-bit data indicator */ - *dsm_11_bit = (dsm_channel_shift == 11); + /* 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; + /* 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"); + printf("PARSED PACKET\n"); #endif - /* check all values */ - for (unsigned i = 0; i < *num_values; i++) { - /* if the value is unrealistic, fail the parsing entirely */ - if (values[i] < 600 || values[i] > 2400) { + /* check all values */ + for (unsigned i = 0; i < *num_values; i++) { + /* if the value is unrealistic, fail the parsing entirely */ + if (values[i] < 600 || values[i] > 2400) { #ifdef DSM_DEBUG - printf("DSM: VALUE RANGE FAIL\n"); + printf("DSM: VALUE RANGE FAIL\n"); #endif - dsm_chan_count = 0; - return false; - } - } + dsm_chan_count = 0; + return false; + } + } return true; } @@ -527,148 +527,148 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool */ 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) + unsigned max_values) { - int ret = 1; - hrt_abstime now; + int ret = 1; + hrt_abstime now; - /* - * 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(); + /* + * 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(); - /* - * 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 + * a complete frame. + */ - ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0])); + 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_buf[0]; - } + } else { + *n_bytes = ret; + *bytes = &dsm_buf[0]; + } - /* - * 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); + /* + * 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) + 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; + /* 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++) { + /* 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; + /* 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"); + 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; + 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"); + 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]); + 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: + 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]; - } + /* 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; + break; - case DSM_DECODE_STATE_SYNC: { - dsm_frame[dsm_partial_frame_count++] = frame[d]; + 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; - } + /* 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); + /* + * 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; + /* 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; - dsm_frame_drops++; - } - } - break; + /* if decoding failed, set proto to desync */ + if (decode_ret == false) { + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + dsm_frame_drops++; + } + } + break; - default: + default: #ifdef DSM_DEBUG - printf("UNKNOWN PROTO STATE"); + printf("UNKNOWN PROTO STATE"); #endif - decode_ret = false; - } + decode_ret = false; + } - } + } - if (frame_drops) { - *frame_drops = dsm_frame_drops; - } + if (frame_drops) { + *frame_drops = dsm_frame_drops; + } - if (decode_ret) { - *num_values = dsm_chan_count; - } + if (decode_ret) { + *num_values = dsm_chan_count; + } - dsm_last_rx_time = now; + dsm_last_rx_time = now; - /* return false as default */ - return decode_ret; + /* return false as default */ + return decode_ret; } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index f75fda9251..669b319017 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -55,10 +55,11 @@ __BEGIN_DECLS __EXPORT int dsm_init(const char *device); __EXPORT int dsm_config(int dsm_fd); -__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); +__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); __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); + 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);