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

AP_RCProtocol: decode DSM frame markers to avoid timing related jitters especially in SITL 

complete re-write of DSM decoding
This commit is contained in:
Andy Piper 2020-04-23 19:17:18 +01:00 committed by Andrew Tridgell
parent 907ad5e25c
commit 80e1c0ebb0
3 changed files with 108 additions and 297 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

360
libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp
View File

@ -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 <stdio.h>
extern const AP_HAL::HAL& hal;
@ -31,194 +33,51 @@ 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 SPEKTRUM_VTX_CONTROL_FRAME_MASK 0xf000f000
#define SPEKTRUM_VTX_CONTROL_FRAME 0xe000e000
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()/1000U, b);
_process_byte(ss.get_byte_timestamp_us(), 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_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);
}

27
libraries/AP_RCProtocol/AP_RCProtocol_DSM.h
View File

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

18
libraries/AP_RCProtocol/examples/RCProtocolTest/RCProtocolTest.cpp
View File

@ -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();