Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: reverted DSM protocol decoder 

the new decoder done by Andy is much nicer looking code, but fails to
correctly parse several valid DSM setups, and parses both SBUS and
FPort as DSM, breaking setups that were previously working
This commit is contained in:
Andrew Tridgell 2020-08-17 10:14:37 +10:00
parent 6e1aeb905f
commit 78c4e27259
2 changed files with 292 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

397
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
View File

@ -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 <AP_Vehicle/AP_Vehicle_Type.h>
#include "AP_RCProtocol_DSM.h"
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
#include "AP_RCProtocol_SRXL2.h"
#endif
#include <stdio.h>
extern const AP_HAL::HAL& hal;
@ -38,69 +31,194 @@ extern const AP_HAL::HAL& hal;
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
#define DSM2_1024_22MS 0x01
#define DSM2_2048_11MS 0x12
#define DSMX_2048_22MS 0xa2
#define DSMX_2048_11MS 0xb2
#define DSMX_2048_SAT 0x00
#define DSMX_2048_SAT2 0x5a
#define SPEKTRUM_VTX_CONTROL_FRAME_MASK 0xf000f000
#define SPEKTRUM_VTX_CONTROL_FRAME 0xe000e000
#define SPEKTRUM_VTX_BAND_MASK 0x00e00000
#define SPEKTRUM_VTX_CHANNEL_MASK 0x000f0000
#define SPEKTRUM_VTX_PIT_MODE_MASK 0x00000010
#define SPEKTRUM_VTX_POWER_MASK 0x00000007
#define SPEKTRUM_VTX_BAND_SHIFT 21
#define SPEKTRUM_VTX_CHANNEL_SHIFT 16
#define SPEKTRUM_VTX_PIT_MODE_SHIFT 4
#define SPEKTRUM_VTX_POWER_SHIFT 0
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);
}
}
/**
* Attempt to decode a single channel raw channel datum
*
* The DSM* protocol doesn't provide any explicit framing,
* so we detect dsm frame boundaries by the inter-dsm frame delay.
*
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
* dsm frame transmission time is ~1.4ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a dsm frame.
*
* In the case where byte(s) are dropped from a dsm frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*
* Upon receiving a full dsm frame we attempt to decode it
*
* @param[in] raw 16 bit raw channel value from dsm frame
* @param[in] shift position of channel number in raw data
* @param[out] channel pointer to returned channel number
* @param[out] value pointer to returned channel value
* @return true=raw value successfully decoded
*/
bool AP_RCProtocol_DSM::dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
{
if (raw == 0xffff) {
return false;
}
*channel = (raw >> 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)
{
/* 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;
/*
* 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);
}
// 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);
/* we have received something we think is a dsm_frame */
last_frame_time_ms = frame_time_ms;
uint32_t value = ((uint32_t)(dsm_frame[b - 1] & channel_mask) << 8) + dsm_frame[b];
/* 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;
}
/*
* 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++) {
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;
byte_input.ofs = 0;
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 == 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();
}
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;
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();
}
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();
}
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
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");
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
decode_ret = dsm_decode(frame_time_us, byte_input.buf, values, &chan_count, max_channels);
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;
}
/*
* 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);
}

27
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h
View File

@ -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};