diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index 6649a74cc4..4cf3eebe8d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -15,16 +15,9 @@ * Code by Andrew Tridgell and Siddharth Bharat Purohit */ /* - * See https://www.spektrumrc.com/ProdInfo/Files/Remote%20Receiver%20Interfacing%20Rev%20A.pdf for official - * Spektrum documentation on the format. + with thanks to PX4 dsm.c for DSM decoding approach */ - -#include #include "AP_RCProtocol_DSM.h" -#if !APM_BUILD_TYPE(APM_BUILD_iofirmware) -#include "AP_RCProtocol_SRXL2.h" -#endif -#include extern const AP_HAL::HAL& hal; @@ -38,69 +31,194 @@ 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_us, 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 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_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)); - - 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; -#if !APM_BUILD_TYPE(APM_BUILD_iofirmware) - AP_RCProtocol_SRXL2::configure_vtx( - (vtxControl & SPEKTRUM_VTX_BAND_MASK) >> SPEKTRUM_VTX_BAND_SHIFT, - (vtxControl & SPEKTRUM_VTX_CHANNEL_MASK) >> SPEKTRUM_VTX_CHANNEL_SHIFT, - (vtxControl & SPEKTRUM_VTX_POWER_MASK) >> SPEKTRUM_VTX_POWER_SHIFT, - (vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT); -#endif - } else { - dsm_frame_data_size = AP_DSM_FRAME_SIZE; + last_frame_time_ms = frame_time_ms; + + /* 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; } - // 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); + /* + * 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. + */ + + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - uint32_t value = ((uint32_t)(dsm_frame[b - 1] & channel_mask) << 8) + dsm_frame[b]; + 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; + } /* ignore channels out of range */ if (channel >= max_values) { @@ -113,23 +231,33 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_us, const uint8_t dsm_fra } /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (channel_shift == 2) { + if (channel_shift == 10) { value *= 2; } - /* 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 */ + /* + * 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. + */ + /* scaled integer for decent accuracy while staying efficient */ - value = ((int32_t)value * 1194) / 2048 + 903; + value = ((((int)value - 1024) * 1000) / 1700) + 1500; /* - * 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; @@ -150,6 +278,23 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_us, 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. */ @@ -227,7 +372,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_us, uint8_t b, uint16_t *values, +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) { /* this is set by the decoding state machine and will default to false @@ -235,89 +380,93 @@ bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_us, 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; + /* 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"); reset_rc_frame_count(); } - // 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 >= AP_DSM_FRAME_SIZE) { - start_frame_time_us = frame_time_us; + if (byte_input.ofs == DSM_FRAME_SIZE) { byte_input.ofs = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + debug("DSM: RESET (PACKET LIM)\n"); reset_rc_frame_count(); } - 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 || - b == DSMX_2048_SAT || b == DSMX_2048_SAT2) { - 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; +#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; - // reset protocol detection. Any bad data during detection - // and we need to not detect as DSM. This is needed as DSM - // is such a weak protocol. - reset_rc_frame_count(); + byte_input.buf[byte_input.ofs++] = b; } - } - - byte_input.buf[byte_input.ofs++] = b; + break; - /* 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 - for (uint16_t i = 0; i < 16; i++) { - printf("%02x", byte_input.buf[i]); + 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; } - 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]); + byte_input.buf[byte_input.ofs++] = b; + + /* decode whatever we got and expect */ + if (byte_input.ofs < DSM_FRAME_SIZE) { + break; } - printf("\n"); -#endif - decode_ret = dsm_decode(frame_time_us, byte_input.buf, values, &chan_count, max_channels); + + /* + * 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; + reset_rc_frame_count(); + } + break; } + default: + debug("UNKNOWN PROTO STATE"); + decode_ret = false; + } + + if (decode_ret) { *num_values = chan_count; } - last_rx_time_us = frame_time_us; + last_rx_time_ms = frame_time_ms; - return decode_ret; + /* return false as default */ + return decode_ret; } // support byte input -void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_us, uint8_t b) +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_us, b, v, &nchan, AP_DSM_MAX_CHANNELS)) { + 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); @@ -331,5 +480,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 83a4dc5371..74d44ed147 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -21,7 +21,6 @@ #include "SoftSerial.h" #define AP_DSM_MAX_CHANNELS 12 -#define AP_DSM_FRAME_SIZE 16 class AP_RCProtocol_DSM : public AP_RCProtocol_Backend { public: @@ -33,13 +32,21 @@ public: private: void _process_byte(uint32_t timestamp_ms, uint8_t byte); - bool dsm_parse_byte(uint32_t frame_time_us, uint8_t b, uint16_t *values, + 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, uint16_t *num_values, uint16_t max_channels); - bool dsm_decode(uint32_t frame_time_us, const uint8_t dsm_frame[16], + 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); + /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */ uint8_t channel_shift; - uint8_t channel_mask; + + // format guessing state + uint32_t cs10; + uint32_t cs11; + uint32_t samples; // bind state machine enum { @@ -54,13 +61,17 @@ private: uint16_t last_values[AP_DSM_MAX_CHANNELS]; struct { - uint8_t buf[AP_DSM_FRAME_SIZE]; + uint8_t buf[16]; uint8_t ofs; } byte_input; - uint32_t last_frame_time_us; - uint32_t last_rx_time_us; - uint32_t start_frame_time_us; + 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};