AP_RCProtocol: make DSM parsing more robust
this makes DSM parsing much more robust. It fixes an issue with DSM input for the Solo, where it occasionally saw glitches
This commit is contained in:
Andrew Tridgell
parent
50df8c7e28
commit
24b23d6784
@ -21,9 +21,9 @@
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// #define DEBUG
|
||||
#ifdef DEBUG
|
||||
# define debug(fmt, args...) hal.console->printf(fmt "\n", ##args)
|
||||
// #define DSM_DEBUG
|
||||
#ifdef DSM_DEBUG
|
||||
# define debug(fmt, args...) printf(fmt "\n", ##args)
|
||||
#else
|
||||
# define debug(fmt, args...) do {} while(0)
|
||||
#endif
|
||||
@ -36,7 +36,7 @@ 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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -94,7 +94,7 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
|
||||
cs10 = 0;
|
||||
cs11 = 0;
|
||||
samples = 0;
|
||||
dsm_channel_shift = 0;
|
||||
channel_shift = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
@ -157,13 +157,13 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
|
||||
}
|
||||
|
||||
if ((votes11 == 1) && (votes10 == 0)) {
|
||||
dsm_channel_shift = 11;
|
||||
channel_shift = 11;
|
||||
debug("DSM: 11-bit format");
|
||||
return;
|
||||
}
|
||||
|
||||
if ((votes10 == 1) && (votes11 == 0)) {
|
||||
dsm_channel_shift = 10;
|
||||
channel_shift = 10;
|
||||
debug("DSM: 10-bit format");
|
||||
return;
|
||||
}
|
||||
@ -177,27 +177,22 @@ void AP_RCProtocol_DSM::dsm_guess_format(bool reset, const uint8_t dsm_frame[16]
|
||||
* Decode the entire dsm frame (all contained channels)
|
||||
*
|
||||
*/
|
||||
bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, 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 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 200ms, reset the
|
||||
* format guessing heuristic.
|
||||
*/
|
||||
if (((frame_time - dsm_last_frame_time) > 200000U) && (dsm_channel_shift != 0)) {
|
||||
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 */
|
||||
dsm_last_frame_time = frame_time;
|
||||
last_frame_time_ms = frame_time_ms;
|
||||
|
||||
/* if we don't know the dsm_frame format, update the guessing state machine */
|
||||
if (dsm_channel_shift == 0) {
|
||||
if (channel_shift == 0) {
|
||||
dsm_guess_format(false, dsm_frame);
|
||||
return false;
|
||||
}
|
||||
@ -219,7 +214,7 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[
|
||||
uint16_t raw = (dp[0] << 8) | dp[1];
|
||||
unsigned channel, value;
|
||||
|
||||
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) {
|
||||
if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@ -234,7 +229,7 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[
|
||||
}
|
||||
|
||||
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
|
||||
if (dsm_channel_shift == 10) {
|
||||
if (channel_shift == 10) {
|
||||
value *= 2;
|
||||
}
|
||||
|
||||
@ -291,7 +286,7 @@ bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time, const uint8_t dsm_frame[
|
||||
}
|
||||
|
||||
#if 0
|
||||
if (dsm_channel_shift == 11) {
|
||||
if (channel_shift == 11) {
|
||||
/* Set the 11-bit data indicator */
|
||||
*num_values |= 0x8000;
|
||||
}
|
||||
@ -371,22 +366,106 @@ void AP_RCProtocol_DSM::update(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
// support byte input
|
||||
void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_us, uint8_t b)
|
||||
/*
|
||||
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,
|
||||
uint16_t *num_values, uint16_t max_channels)
|
||||
{
|
||||
if (timestamp_us - byte_input.last_byte_us > 3000U ||
|
||||
byte_input.ofs == sizeof(byte_input.buf)) {
|
||||
/* this is set by the decoding state machine and will default to false
|
||||
* once everything that was decodable has been decoded.
|
||||
*/
|
||||
bool decode_ret = false;
|
||||
|
||||
/* 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");
|
||||
}
|
||||
byte_input.last_byte_us = timestamp_us;
|
||||
byte_input.buf[byte_input.ofs++] = b;
|
||||
if (byte_input.ofs == 16) {
|
||||
if (dsm_decode(timestamp_us, byte_input.buf, &last_values[0], &num_channels, AP_DSM_MAX_CHANNELS) &&
|
||||
num_channels >= MIN_RCIN_CHANNELS) {
|
||||
add_input(num_channels, last_values, false);
|
||||
|
||||
if (byte_input.ofs == DSM_FRAME_SIZE) {
|
||||
byte_input.ofs = 0;
|
||||
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
|
||||
debug("DSM: RESET (PACKET LIM)\n");
|
||||
}
|
||||
|
||||
#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;
|
||||
chan_count = 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;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
debug("UNKNOWN PROTO STATE");
|
||||
decode_ret = false;
|
||||
}
|
||||
|
||||
|
||||
if (decode_ret) {
|
||||
*num_values = chan_count;
|
||||
}
|
||||
|
||||
last_rx_time_ms = frame_time_ms;
|
||||
|
||||
/* return false as default */
|
||||
return decode_ret;
|
||||
}
|
||||
|
||||
// support byte input
|
||||
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_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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -396,5 +475,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);
|
||||
}
|
||||
|
||||
@ -31,25 +31,22 @@ public:
|
||||
void update(void) override;
|
||||
|
||||
private:
|
||||
void _process_byte(uint32_t timestamp_us, uint8_t byte);
|
||||
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_decode(uint32_t frame_time, 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_ms, const uint8_t dsm_frame[16],
|
||||
uint16_t *values, uint16_t *num_values, uint16_t max_values);
|
||||
|
||||
uint32_t dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
|
||||
unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
|
||||
// state of DSM decoder
|
||||
struct {
|
||||
uint16_t bytes[16]; // including start bit and stop bit
|
||||
uint16_t bit_ofs;
|
||||
} dsm_state;
|
||||
/**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
|
||||
uint8_t channel_shift;
|
||||
|
||||
// format guessing state
|
||||
uint32_t cs10;
|
||||
uint32_t cs11;
|
||||
unsigned samples;
|
||||
uint32_t samples;
|
||||
|
||||
// bind state machine
|
||||
enum {
|
||||
@ -62,13 +59,20 @@ private:
|
||||
uint32_t bind_last_ms;
|
||||
|
||||
uint16_t last_values[AP_DSM_MAX_CHANNELS];
|
||||
uint16_t num_channels;
|
||||
|
||||
struct {
|
||||
uint8_t buf[16];
|
||||
uint8_t ofs;
|
||||
uint32_t last_byte_us;
|
||||
} 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;
|
||||
uint16_t chan_count;
|
||||
|
||||
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user