diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index c7029b49dd..b0721a8ecf 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -21,9 +21,9 @@ extern const AP_HAL::HAL& hal; -// #define DEBUG -#ifdef DEBUG -# define debug(fmt, args...) hal.console->printf(fmt "\n", ##args) +// #define DSM_DEBUG +#ifdef DSM_DEBUG +# define debug(fmt, args...) printf(fmt "\n", ##args) #else # define debug(fmt, args...) do {} while(0) #endif @@ -36,7 +36,7 @@ void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1) { uint8_t b; if (ss.process_pulse(width_s0, width_s1, b)) { - _process_byte(ss.get_byte_timestamp_us(), b); + _process_byte(ss.get_byte_timestamp_us()/1000U, b); } } @@ -94,7 +94,7 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16] cs10 = 0; cs11 = 0; samples = 0; - dsm_channel_shift = 0; + channel_shift = 0; return; } @@ -157,13 +157,13 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16] } if ((votes11 == 1) && (votes10 == 0)) { - dsm_channel_shift = 11; + channel_shift = 11; debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { - dsm_channel_shift = 10; + channel_shift = 10; debug("DSM: 10-bit format"); return; } @@ -177,27 +177,22 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16] * Decode the entire dsm frame (all contained channels) * */ -bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[16], +bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values) { -#if 1 - debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", - dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], - dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]); -#endif /* * If we have lost signal for at least 200ms, reset the * format guessing heuristic. */ - if (((frame_time - dsm_last_frame_time) > 200000U) && (dsm_channel_shift != 0)) { + if (((frame_time_ms - last_frame_time_ms) > 200U) && (channel_shift != 0)) { dsm_guess_format(true, dsm_frame); } /* we have received something we think is a dsm_frame */ - dsm_last_frame_time = frame_time; + last_frame_time_ms = frame_time_ms; /* if we don't know the dsm_frame format, update the guessing state machine */ - if (dsm_channel_shift == 0) { + if (channel_shift == 0) { dsm_guess_format(false, dsm_frame); return false; } @@ -219,7 +214,7 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[ uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) { + if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) { continue; } @@ -234,7 +229,7 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[ } /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (dsm_channel_shift == 10) { + if (channel_shift == 10) { value *= 2; } @@ -291,7 +286,7 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[ } #if 0 - if (dsm_channel_shift == 11) { + if (channel_shift == 11) { /* Set the 11-bit data indicator */ *num_values |= 0x8000; } @@ -371,22 +366,106 @@ void AP_RCProtocol_DSM::update(void) #endif } -// support byte input -void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_us, uint8_t b) +/* + parse one DSM byte, maintaining decoder state + */ +bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values, + uint16_t *num_values, uint16_t max_channels) { - if (timestamp_us - byte_input.last_byte_us > 3000U || - byte_input.ofs == sizeof(byte_input.buf)) { + /* 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; + + /* overflow check */ + if (byte_input.ofs == sizeof(byte_input.buf) / sizeof(byte_input.buf[0])) { byte_input.ofs = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + debug("DSM: RESET (BUF LIM)\n"); } - byte_input.last_byte_us = timestamp_us; - byte_input.buf[byte_input.ofs++] = b; - if (byte_input.ofs == 16) { - if (dsm_decode(timestamp_us, byte_input.buf, &last_values[0], &num_channels, AP_DSM_MAX_CHANNELS) && - num_channels >= MIN_RCIN_CHANNELS) { - add_input(num_channels, last_values, false); + + if (byte_input.ofs == DSM_FRAME_SIZE) { + byte_input.ofs = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + debug("DSM: RESET (PACKET LIM)\n"); + } + +#ifdef DSM_DEBUG + debug("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" : "", + byte_input.ofs, + (unsigned)b); +#endif + + switch (dsm_decode_state) { + case DSM_DECODE_STATE_DESYNC: + + /* we are de-synced and only interested in the frame marker */ + if ((frame_time_ms - last_rx_time_ms) >= 5) { + dsm_decode_state = DSM_DECODE_STATE_SYNC; + byte_input.ofs = 0; + chan_count = 0; + byte_input.buf[byte_input.ofs++] = b; + } + break; + + case DSM_DECODE_STATE_SYNC: { + if ((frame_time_ms - last_rx_time_ms) >= 5 && byte_input.ofs > 0) { + byte_input.ofs = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + break; + } + byte_input.buf[byte_input.ofs++] = b; + + /* decode whatever we got and expect */ + if (byte_input.ofs < DSM_FRAME_SIZE) { + break; } + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = dsm_decode(frame_time_ms, byte_input.buf, values, &chan_count, max_channels); + + /* we consumed the partial frame, reset */ byte_input.ofs = 0; + + /* if decoding failed, set proto to desync */ + if (decode_ret == false) { + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + } + break; + } + + default: + debug("UNKNOWN PROTO STATE"); + decode_ret = false; + } + + + if (decode_ret) { + *num_values = chan_count; + } + + last_rx_time_ms = frame_time_ms; + + /* return false as default */ + return decode_ret; +} + +// support byte input +void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_ms, uint8_t b) +{ + uint16_t v[AP_DSM_MAX_CHANNELS]; + uint16_t nchan; + memcpy(v, last_values, sizeof(v)); + if (dsm_parse_byte(timestamp_ms, b, v, &nchan, AP_DSM_MAX_CHANNELS)) { + memcpy(last_values, v, sizeof(v)); + if (nchan >= MIN_RCIN_CHANNELS) { + add_input(nchan, last_values, false); + } } } @@ -396,5 +475,5 @@ void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) if (baudrate != 115200) { return; } - _process_byte(AP_HAL::micros(), b); + _process_byte(AP_HAL::millis(), b); } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index 61fd9dd5ce..74d44ed147 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -31,25 +31,22 @@ public: void update(void) override; private: - void _process_byte(uint32_t timestamp_us, uint8_t byte); + void _process_byte(uint32_t timestamp_ms, uint8_t byte); void dsm_decode(); bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); void dsm_guess_format(bool reset, const uint8_t dsm_frame[16]); - bool dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[16], + bool dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values, + uint16_t *num_values, uint16_t max_channels); + bool dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values); - uint32_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */ - unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */ - // state of DSM decoder - struct { - uint16_t bytes[16]; // including start bit and stop bit - uint16_t bit_ofs; - } dsm_state; + /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */ + uint8_t channel_shift; // format guessing state uint32_t cs10; uint32_t cs11; - unsigned samples; + uint32_t samples; // bind state machine enum { @@ -62,13 +59,20 @@ private: uint32_t bind_last_ms; uint16_t last_values[AP_DSM_MAX_CHANNELS]; - uint16_t num_channels; struct { uint8_t buf[16]; uint8_t ofs; - uint32_t last_byte_us; } byte_input; + enum DSM_DECODE_STATE { + DSM_DECODE_STATE_DESYNC = 0, + DSM_DECODE_STATE_SYNC + } dsm_decode_state; + + uint32_t last_frame_time_ms; + uint32_t last_rx_time_ms; + uint16_t chan_count; + SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; };