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

libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp

@@ -21,9 +21,9 @@
 
 extern const AP_HAL::HAL& hal;
 
-// #define DEBUG
+// #define DSM_DEBUG
-#ifdef DEBUG
+#ifdef DSM_DEBUG
-# define debug(fmt, args...)	hal.console->printf(fmt "\n", ##args)
+# 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 ||
+    /* this is set by the decoding state machine and will default to false
-        byte_input.ofs == sizeof(byte_input.buf)) {
+	 * 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 == DSM_FRAME_SIZE) {
-    if (byte_input.ofs == 16) {
+        byte_input.ofs = 0;
-        if (dsm_decode(timestamp_us, byte_input.buf, &last_values[0], &num_channels, AP_DSM_MAX_CHANNELS) &&
+        dsm_decode_state = DSM_DECODE_STATE_DESYNC;
-            num_channels >= MIN_RCIN_CHANNELS) {
+        debug("DSM: RESET (PACKET LIM)\n");
-            add_input(num_channels, last_values, false);
+    }
+
+#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);
 }

libraries/AP_RCProtocol/AP_RCProtocol_DSM.h

@@ -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 */
+    /**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
-    unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 10=10 bit, 11=11 bit */
+    uint8_t channel_shift;
-    // state of DSM decoder
-    struct {
-        uint16_t bytes[16]; // including start bit and stop bit
-        uint16_t bit_ofs;
-    } dsm_state;
 
     // 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};
 };