From 80e1c0ebb0f97f9890682f673c4761a98b68dba9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 23 Apr 2020 19:17:18 +0100 Subject: [PATCH] AP_RCProtocol: decode DSM frame markers to avoid timing related jitters especially in SITL complete re-write of DSM decoding --- libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp | 360 +++++------------- libraries/AP_RCProtocol/AP_RCProtocol_DSM.h | 27 +- .../RCProtocolTest/RCProtocolTest.cpp | 18 +- 3 files changed, 108 insertions(+), 297 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index b2df3476e2..eab104ed6e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -15,9 +15,11 @@ * Code by Andrew Tridgell and Siddharth Bharat Purohit */ /* - with thanks to PX4 dsm.c for DSM decoding approach + * See https://www.spektrumrc.com/ProdInfo/Files/Remote%20Receiver%20Interfacing%20Rev%20A.pdf for official + * Spektrum documentation on the format. */ #include "AP_RCProtocol_DSM.h" +#include extern const AP_HAL::HAL& hal; @@ -31,194 +33,51 @@ extern const AP_HAL::HAL& hal; #define DSM_FRAME_SIZE 16 /**> shift) & 0xf; - - uint16_t data_mask = (1 << shift) - 1; - *value = raw & data_mask; - - //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); - - return true; -} - -/** - * Attempt to guess if receiving 10 or 11 bit channel values - * - * @param[in] reset true=reset the 10/11 bit state to unknown - */ -void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]) -{ - /* reset the 10/11 bit sniffed channel masks */ - if (reset) { - cs10 = 0; - cs11 = 0; - samples = 0; - channel_shift = 0; - return; - } - - /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - - const uint8_t *dp = &dsm_frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - unsigned channel, value; - - /* if the channel decodes, remember the assigned number */ - if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) { - cs10 |= (1 << channel); - } - - if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) { - cs11 |= (1 << channel); - } - - /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ - } - - /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples++ < 5) { - return; - } - - /* - * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognize. - * - * XXX Note that due to what seem to be bugs in the DSM2 high-resolution - * stream, we may want to sniff for longer in some cases when we think we - * are talking to a DSM2 receiver in high-resolution mode (so that we can - * reject it, ideally). - * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion - * of this issue. - */ - static const uint32_t masks[] = { - 0x3f, /* 6 channels (DX6) */ - 0x7f, /* 7 channels (DX7) */ - 0xff, /* 8 channels (DX8) */ - 0x1ff, /* 9 channels (DX9, etc.) */ - 0x3ff, /* 10 channels (DX10) */ - 0x7ff, /* 11 channels DX8 22ms */ - 0xfff, /* 12 channels DX8 22ms */ - 0x1fff, /* 13 channels (DX10t) */ - 0x3fff /* 18 channels (DX10) */ - }; - unsigned votes10 = 0; - unsigned votes11 = 0; - - for (unsigned i = 0; i < sizeof(masks)/sizeof(masks[0]); i++) { - - if (cs10 == masks[i]) { - votes10++; - } - - if (cs11 == masks[i]) { - votes11++; - } - } - - if ((votes11 == 1) && (votes10 == 0)) { - channel_shift = 11; - debug("DSM: 11-bit format"); - return; - } - - if ((votes10 == 1) && (votes11 == 0)) { - channel_shift = 10; - debug("DSM: 10-bit format"); - return; - } - - /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detect fail, 10: 0x%08x %u 11: 0x%08x %u", cs10, votes10, cs11, votes11); - dsm_guess_format(true, dsm_frame); -} - /** * Decode the entire dsm frame (all contained channels) * */ -bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_frame[16], +bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_us, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values) { - /* - * If we have lost signal for at least 200ms, reset the - * format guessing heuristic. - */ - 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 */ - last_frame_time_ms = frame_time_ms; + last_frame_time_us = frame_time_us; + // Get the VTX control bytes in a frame + uint32_t vtxControl = ((dsm_frame[AP_DSM_FRAME_SIZE-4] << 24) + | (dsm_frame[AP_DSM_FRAME_SIZE-3] << 16) + | (dsm_frame[AP_DSM_FRAME_SIZE-2] << 8) + | (dsm_frame[AP_DSM_FRAME_SIZE-1] << 0)); - /* if we don't know the dsm_frame format, update the guessing state machine */ - if (channel_shift == 0) { - dsm_guess_format(false, dsm_frame); - return false; + uint8_t dsm_frame_data_size; + // Handle VTX control frame. + if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME + && (dsm_frame[2] & 0x80) == 0) { + dsm_frame_data_size = AP_DSM_FRAME_SIZE - 4; + // someday do something with the data + } else { + dsm_frame_data_size = AP_DSM_FRAME_SIZE; } - /* - * The encoding of the first two bytes is uncertain, so we're - * going to ignore them for now. - * - * Each channel is a 16-bit unsigned value containing either a 10- - * or 11-bit channel value and a 4-bit channel number, shifted - * either 10 or 11 bits. The MSB may also be set to indicate the - * second dsm_frame in variants of the protocol where more than - * seven channels are being transmitted. - */ + // Get the RC control channel inputs + for (uint8_t b = 3; b < dsm_frame_data_size; b += 2) { + uint8_t channel = 0x0F & (dsm_frame[b - 1] >> channel_shift); - for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - - const uint8_t *dp = &dsm_frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; - unsigned channel, value; - - if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) { - continue; - } + uint32_t value = ((uint32_t)(dsm_frame[b - 1] & channel_mask) << 8) + dsm_frame[b]; /* ignore channels out of range */ if (channel >= max_values) { @@ -231,33 +90,23 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_fra } /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (channel_shift == 10) { + if (channel_shift == 2) { value *= 2; } - /* - * Spektrum scaling is special. There are these basic considerations - * - * * Midpoint is 1520 us - * * 100% travel channels are +- 400 us - * - * We obey the original Spektrum scaling (so a default setup will scale from - * 1100 - 1900 us), but we do not obey the weird 1520 us center point - * and instead (correctly) center the center around 1500 us. This is in order - * to get something useful without requiring the user to calibrate on a digital - * link for no reason. - */ - + /* Spektrum scaling is defined as (see reference): + 2048: PWM_OUT = (ServoPosition x 58.3μs) + 903 + 1024: PWM_OUT = (ServoPosition x 116.6μs) + 903 */ /* scaled integer for decent accuracy while staying efficient */ - value = ((((int)value - 1024) * 1000) / 1700) + 1500; + value = ((int32_t)value * 1194) / 2048 + 903; /* - * Store the decoded channel into the R/C input buffer, taking into - * account the different ideas about channel assignement that we have. - * - * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, - * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. - */ + * Store the decoded channel into the R/C input buffer, taking into + * account the different ideas about channel assignement that we have. + * + * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, + * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. + */ switch (channel) { case 0: channel = 2; @@ -278,23 +127,6 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_ms, const uint8_t dsm_fra values[channel] = value; } - /* - * Spektrum likes to send junk in higher channel numbers to fill - * their packets. We don't know about a 13 channel model in their TX - * lines, so if we get a channel count of 13, we'll return 12 (the last - * data index that is stable). - */ - if (*num_values == 13) { - *num_values = 12; - } - -#if 0 - if (channel_shift == 11) { - /* Set the 11-bit data indicator */ - *num_values |= 0x8000; - } -#endif - /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ @@ -372,7 +204,7 @@ void AP_RCProtocol_DSM::update(void) /* 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, +bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_us, uint8_t b, uint16_t *values, uint16_t *num_values, uint16_t max_channels) { /* this is set by the decoding state machine and will default to false @@ -380,92 +212,82 @@ bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16 */ bool decode_ret = false; + // we took too long decoding, start again + if (byte_input.ofs > 0 && (frame_time_us - start_frame_time_us) > 6000U) { + start_frame_time_us = frame_time_us; + byte_input.ofs = 0; + } + + // there will be at least a 5ms gap between successive DSM frames. if we see it + // assume we are starting a new frame + if ((frame_time_us - last_rx_time_us) > 5000U) { + start_frame_time_us = frame_time_us; + byte_input.ofs = 0; + } + /* overflow check */ - if (byte_input.ofs == sizeof(byte_input.buf) / sizeof(byte_input.buf[0])) { + if (byte_input.ofs >= AP_DSM_FRAME_SIZE) { + start_frame_time_us = frame_time_us; byte_input.ofs = 0; - dsm_decode_state = DSM_DECODE_STATE_DESYNC; - debug("DSM: RESET (BUF LIM)\n"); } - if (byte_input.ofs == DSM_FRAME_SIZE) { - byte_input.ofs = 0; - dsm_decode_state = DSM_DECODE_STATE_DESYNC; - debug("DSM: RESET (PACKET LIM)\n"); + if (byte_input.ofs == 1) { + // saw a beginning of frame marker + if (b == DSM2_1024_22MS || b == DSM2_2048_11MS || b == DSMX_2048_22MS || b == DSMX_2048_11MS) { + if (b == DSM2_1024_22MS) { + // 10 bit frames + channel_shift = 2; + channel_mask = 0x03; + } else { + // 11 bit frames + channel_shift = 3; + channel_mask = 0x07; + } + // bad frame marker so reset + } else { + start_frame_time_us = frame_time_us; + byte_input.ofs = 0; + } } + byte_input.buf[byte_input.ofs++] = b; + + /* decode whatever we got and expect */ + if (byte_input.ofs == AP_DSM_FRAME_SIZE) { + log_data(AP_RCProtocol::DSM, frame_time_us, byte_input.buf, byte_input.ofs); #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); + for (uint16_t i = 0; i < 16; i++) { + printf("%02x", byte_input.buf[i]); + } + printf("\n%02x%02x", byte_input.buf[0], byte_input.buf[1]); + for (uint16_t i = 2; i < 16; i+=2) { + printf(" %01x/%03x", (byte_input.buf[i] & 0x78) >> 4, (byte_input.buf[i] & 0x7) << 8 | byte_input.buf[i+1]); + } + printf("\n"); #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; - 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; - } - - log_data(AP_RCProtocol::DSM, frame_time_ms*1000U, byte_input.buf, byte_input.ofs); - - /* - * 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); + decode_ret = dsm_decode(frame_time_us, 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; + last_rx_time_us = frame_time_us; /* return false as default */ return decode_ret; } // support byte input -void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_ms, uint8_t b) +void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_us, 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)) { + if (dsm_parse_byte(timestamp_us, b, v, &nchan, AP_DSM_MAX_CHANNELS)) { memcpy(last_values, v, sizeof(v)); if (nchan >= MIN_RCIN_CHANNELS) { add_input(nchan, last_values, false); @@ -479,5 +301,5 @@ void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) if (baudrate != 115200) { return; } - _process_byte(AP_HAL::millis(), b); + _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 74d44ed147..83a4dc5371 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -21,6 +21,7 @@ #include "SoftSerial.h" #define AP_DSM_MAX_CHANNELS 12 +#define AP_DSM_FRAME_SIZE 16 class AP_RCProtocol_DSM : public AP_RCProtocol_Backend { public: @@ -32,21 +33,13 @@ public: private: 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_parse_byte(uint32_t frame_time_ms, uint8_t b, uint16_t *values, + bool dsm_parse_byte(uint32_t frame_time_us, 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], + bool dsm_decode(uint32_t frame_time_us, const uint8_t dsm_frame[16], uint16_t *values, uint16_t *num_values, uint16_t max_values); - /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */ uint8_t channel_shift; - - // format guessing state - uint32_t cs10; - uint32_t cs11; - uint32_t samples; + uint8_t channel_mask; // bind state machine enum { @@ -61,17 +54,13 @@ private: uint16_t last_values[AP_DSM_MAX_CHANNELS]; struct { - uint8_t buf[16]; + uint8_t buf[AP_DSM_FRAME_SIZE]; uint8_t ofs; } 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; + uint32_t last_frame_time_us; + uint32_t last_rx_time_us; + uint32_t start_frame_time_us; uint16_t chan_count; SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; diff --git a/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp b/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp index 3cc0ac280f..6fa01eafc6 100644 --- a/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp +++ b/libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp @@ -209,19 +209,19 @@ void loop() 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; const uint16_t sbus_output[] = {1562, 1496, 1000, 1531, 1806, 2006, 1495, 1495, 875, 875, 875, 875, 875, 875, 875, 875}; - - const uint8_t dsm_bytes[] = {0x00, 0xab, 0x00, 0xae, 0x08, 0xbf, 0x10, 0xd0, 0x18, - 0xe1, 0x20, 0xf2, 0x29, 0x03, 0x31, 0x14, 0x00, 0xab, + // DSM2_2048_11MS + const uint8_t dsm_bytes[] = {0x00, 0x12, 0x00, 0xae, 0x08, 0xbf, 0x10, 0xd0, 0x18, + 0xe1, 0x20, 0xf2, 0x29, 0x03, 0x31, 0x14, 0x00, 0x12, 0x39, 0x25, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; - const uint16_t dsm_output[] = {1010, 1020, 1000, 1030, 1040, 1050, 1060, 1070}; - + const uint16_t dsm_output[] = {1014, 1024, 1004, 1034, 1044, 1053, 1063, 1073}; + // DSMX_2048_11MS const uint8_t dsm_bytes2[] = {0x00, 0xb2, 0x80, 0x94, 0x3c, 0x02, 0x1b, 0xfe, 0x44, 0x00, 0x4c, 0x00, 0x5c, 0x00, 0xff, 0xff, 0x00, 0xb2, 0x0c, 0x03, 0x2e, 0xaa, 0x14, 0x00, - 0x21, 0x56, 0x34, 0x02, 0x54, 0x00, 0xff, 0xff}; + 0x21, 0x56, 0x34, 0x02, 0x54, 0x00, 0xff, 0xff }; - const uint16_t dsm_output2[] = {1501, 1500, 985, 1499, 1099, 1901, 1501, 1501, 1500, 1500, 1500, 1500}; + const uint16_t dsm_output2[] = {1501, 1500, 989, 1498, 1102, 1897, 1501, 1501, 1500, 1500, 1500, 1500}; const uint8_t sumd_bytes[] = {0xA8, 0x01, 0x08, 0x2F, 0x50, 0x31, 0xE8, 0x21, 0xA0, 0x2F, 0x50, 0x22, 0x60, 0x22, 0x60, 0x2E, 0xE0, 0x2E, @@ -252,8 +252,8 @@ void loop() test_protocol("SBUS", 100000, sbus_bytes, sizeof(sbus_bytes), sbus_output, ARRAY_SIZE(sbus_output), 3); // DSM needs 8 repeats, 5 to guess the format, then 3 to pass the RCProtocol 3 frames test - test_protocol("DSM", 115200, dsm_bytes, sizeof(dsm_bytes), dsm_output, ARRAY_SIZE(dsm_output), 9); - test_protocol("DSM2", 115200, dsm_bytes2, sizeof(dsm_bytes2), dsm_output2, ARRAY_SIZE(dsm_output2), 9, 16); + test_protocol("DSM2", 115200, dsm_bytes, sizeof(dsm_bytes), dsm_output, ARRAY_SIZE(dsm_output), 9); + test_protocol("DSMX", 115200, dsm_bytes2, sizeof(dsm_bytes2), dsm_output2, ARRAY_SIZE(dsm_output2), 9, 16); } AP_HAL_MAIN();