diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index 223478443c..7359cf4765 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -34,79 +34,10 @@ extern const AP_HAL::HAL& hal; void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1) { - // convert to bit widths, allowing for up to about 4usec error, assuming 115200 bps - uint16_t bits_s0 = ((width_s0+4)*(uint32_t)115200) / 1000000; - uint16_t bits_s1 = ((width_s1+4)*(uint32_t)115200) / 1000000; - uint8_t bit_ofs, byte_ofs; - uint16_t nbits; - - // maintain time using pulse widths - clock_us += (width_s0 + width_s1); - - if (bits_s0 == 0 || bits_s1 == 0) { - // invalid data - goto reset; + uint8_t b; + if (ss.process_pulse(width_s0, width_s1, b)) { + _process_byte(ss.get_byte_timestamp_us(), b); } - - byte_ofs = dsm_state.bit_ofs/10; - bit_ofs = dsm_state.bit_ofs%10; - - if (byte_ofs > 15) { - // invalid data - goto reset; - } - - // pull in the high bits - nbits = bits_s0; - if (nbits+bit_ofs > 10) { - nbits = 10 - bit_ofs; - } - dsm_state.bytes[byte_ofs] |= ((1U< 10) { - if (dsm_state.bit_ofs == 16*10) { - // we have a full frame - uint8_t bytes[16]; - uint8_t i; - for (i=0; i<16; i++) { - // get raw data - uint16_t v = dsm_state.bytes[i]; - - // check start bit - if ((v & 1) != 0) { - goto reset; - } - // check stop bits - if ((v & 0x200) != 0x200) { - goto reset; - } - bytes[i] = ((v>>1) & 0xFF); - } - uint16_t values[8]; - uint16_t num_values=0; - if (dsm_decode(clock_us, bytes, values, &num_values, 8) && - num_values >= MIN_RCIN_CHANNELS) { - add_input(num_values, values, false); - } - } - memset(&dsm_state, 0, sizeof(dsm_state)); - } - - byte_ofs = dsm_state.bit_ofs/10; - bit_ofs = dsm_state.bit_ofs%10; - - if (bits_s1+bit_ofs > 10) { - // invalid data - goto reset; - } - - // pull in the low bits - dsm_state.bit_ofs += bits_s1; - return; -reset: - memset(&dsm_state, 0, sizeof(dsm_state)); } /** @@ -158,10 +89,6 @@ bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigne */ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]) { - static uint32_t cs10; - static uint32_t cs11; - static unsigned samples; - /* reset the 10/11 bit sniffed channel masks */ if (reset) { cs10 = 0; @@ -206,7 +133,7 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16] * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion * of this issue. */ - static uint32_t masks[] = { + static const uint32_t masks[] = { 0x3f, /* 6 channels (DX6) */ 0x7f, /* 7 channels (DX7) */ 0xff, /* 8 channels (DX8) */ @@ -253,16 +180,16 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16] bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values) { -#if 0 +#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 a second, reset the + * If we have lost signal for at least 200ms, reset the * format guessing heuristic. */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { + if (((frame_time - dsm_last_frame_time) > 200000U) && (dsm_channel_shift != 0)) { dsm_guess_format(true, dsm_frame); } @@ -445,22 +372,18 @@ void AP_RCProtocol_DSM::update(void) } // support byte input -void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) +void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_us, uint8_t b) { - if (baudrate != 115200) { - return; - } - uint32_t now = AP_HAL::millis(); - if (now - byte_input.last_byte_ms > 3 || + if (timestamp_us - byte_input.last_byte_us > 3000U || byte_input.ofs == sizeof(byte_input.buf)) { byte_input.ofs = 0; } - byte_input.last_byte_ms = now; + byte_input.last_byte_us = timestamp_us; byte_input.buf[byte_input.ofs++] = b; if (byte_input.ofs == 16) { uint16_t values[8]; uint16_t num_values=0; - if (dsm_decode(AP_HAL::micros(), byte_input.buf, values, &num_values, 8) && + if (dsm_decode(timestamp_us, byte_input.buf, values, &num_values, 8) && num_values >= MIN_RCIN_CHANNELS) { add_input(num_values, values, false); } @@ -468,3 +391,12 @@ void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) byte_input.ofs = 0; } } + +// support byte input +void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) +{ + if (baudrate != 115200) { + return; + } + _process_byte(AP_HAL::micros(), b); +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index a666654807..c4af6ce700 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -18,6 +18,7 @@ #pragma once #include "AP_RCProtocol.h" +#include "SoftSerial.h" class AP_RCProtocol_DSM : public AP_RCProtocol_Backend { public: @@ -28,6 +29,7 @@ public: void update(void) override; private: + void _process_byte(uint32_t timestamp_us, 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]); @@ -42,6 +44,11 @@ private: uint16_t bit_ofs; } dsm_state; + // format guessing state + uint32_t cs10; + uint32_t cs11; + unsigned samples; + // bind state machine enum { BIND_STATE_NONE, @@ -52,12 +59,11 @@ private: } bind_state; uint32_t bind_last_ms; - // sum of clock pulses - uint32_t clock_us; - struct { uint8_t buf[16]; uint8_t ofs; - uint32_t last_byte_ms; + uint32_t last_byte_us; } byte_input; + + SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; };