AP_RCProtocol: switched SBUS to SoftSerial decoder

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -19,7 +19,6 @@
 #include "AP_RCProtocol_PPMSum.h"
 #include "AP_RCProtocol_DSM.h"
 #include "AP_RCProtocol_SBUS.h"
-#include "AP_RCProtocol_SBUS_NI.h"
 #include "AP_RCProtocol_SUMD.h"
 #include "AP_RCProtocol_SRXL.h"
 #include "AP_RCProtocol_ST24.h"
@@ -30,8 +29,8 @@ AP_RCProtocol *AP_RCProtocol::instance;
 void AP_RCProtocol::init()
 {
     backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this);
-    backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this);
+    backend[AP_RCProtocol::SBUS] = new AP_RCProtocol_SBUS(*this, true);
-    backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS_NI(*this);
+    backend[AP_RCProtocol::SBUS_NI] = new AP_RCProtocol_SBUS(*this, false);
     backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this);
     backend[AP_RCProtocol::SUMD] = new AP_RCProtocol_SUMD(*this);
     backend[AP_RCProtocol::SRXL] = new AP_RCProtocol_SRXL(*this);

libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp

@@ -107,6 +107,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
 };
 
 
+// constructor
+AP_RCProtocol_SBUS::AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool _inverted) :
+    AP_RCProtocol_Backend(_frontend),
+    inverted(_inverted)
+{}
+
+// decode a full SBUS frame
 bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
                                      bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
 {
@@ -203,97 +210,29 @@ bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values,
  */
 void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1)
 {
-    // convert to bit widths, allowing for up to 4usec error, assuming 100000 bps
+    uint32_t w0 = width_s0;
-    uint16_t bits_s0 = (width_s0+4) / 10;
+    uint32_t w1 = width_s1;
-    uint16_t bits_s1 = (width_s1+4) / 10;
+    if (inverted) {
-    uint16_t nlow;
+        w0 = saved_width;
-
+        w1 = width_s0;
-    uint8_t byte_ofs = sbus_state.bit_ofs/12;
+        saved_width = width_s1;
-    uint8_t bit_ofs = sbus_state.bit_ofs%12;
-
-    if (bits_s0 == 0 || bits_s1 == 0) {
-        // invalid data
-        goto reset;
     }
-
+    uint8_t b;
-    if (bits_s0+bit_ofs > 10) {
+    if (ss.process_pulse(w0, w1, b)) {
-        // invalid data as last two bits must be stop bits
+        _process_byte(ss.get_byte_timestamp_us(), b);
-        goto reset;
     }
-
-    if (byte_ofs >= ARRAY_SIZE(sbus_state.bytes)) {
-        goto reset;
-    }
-    // pull in the high bits
-    sbus_state.bytes[byte_ofs] |= ((1U<<bits_s0)-1) << bit_ofs;
-    sbus_state.bit_ofs += bits_s0;
-    bit_ofs += bits_s0;
-
-    // pull in the low bits
-    nlow = bits_s1;
-    if (nlow + bit_ofs > 12) {
-        nlow = 12 - bit_ofs;
-    }
-    bits_s1 -= nlow;
-    sbus_state.bit_ofs += nlow;
-
-    if (sbus_state.bit_ofs == 25*12 && bits_s1 > 12) {
-        // we have a full frame
-        uint8_t bytes[25];
-        uint8_t i;
-        for (i=0; i<25; i++) {
-            // get inverted data
-            uint16_t v = ~sbus_state.bytes[i];
-            // check start bit
-            if ((v & 1) != 0) {
-                goto reset;
-            }
-            // check stop bits
-            if ((v & 0xC00) != 0xC00) {
-                goto reset;
-            }
-            // check parity
-            uint8_t parity = 0, j;
-            for (j=1; j<=8; j++) {
-                parity ^= (v & (1U<<j))?1:0;
-            }
-            if (parity != (v&0x200)>>9) {
-                goto reset;
-            }
-            bytes[i] = ((v>>1) & 0xFF);
-        }
-        uint16_t values[MAX_RCIN_CHANNELS];
-        uint16_t num_values=0;
-        bool sbus_failsafe=false, sbus_frame_drop=false;
-        if (sbus_decode(bytes, values, &num_values,
-                        &sbus_failsafe, &sbus_frame_drop,
-                        MAX_RCIN_CHANNELS) &&
-            num_values >= MIN_RCIN_CHANNELS) {
-            add_input(num_values, values, sbus_failsafe);
-        }
-        goto reset;
-    } else if (bits_s1 > 12) {
-        // break
-        goto reset;
-    }
-    return;
-reset:
-    memset(&sbus_state, 0, sizeof(sbus_state));
 }
 
 // support byte input
-void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate)
+void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b)
 {
-    if (baudrate != 100000) {
+    if (timestamp_us - byte_input.last_byte_us > 2000U ||
-        return;
-    }
-    uint32_t now = AP_HAL::millis();
-    if (now - byte_input.last_byte_ms > 2 ||
         byte_input.ofs == sizeof(byte_input.buf)) {
         byte_input.ofs = 0;
     }
-    byte_input.last_byte_ms = now;
+    byte_input.last_byte_us = timestamp_us;
     byte_input.buf[byte_input.ofs++] = b;
+
     if (byte_input.ofs == sizeof(byte_input.buf)) {
         uint16_t values[SBUS_INPUT_CHANNELS];
         uint16_t num_values=0;
@@ -307,3 +246,12 @@ void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate)
         byte_input.ofs = 0;
     }
 }
+
+// support byte input
+void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate)
+{
+    if (baudrate != 100000) {
+        return;
+    }
+    _process_byte(AP_HAL::micros(), b);
+}

libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h

@@ -18,23 +18,25 @@
 #pragma once
 
 #include "AP_RCProtocol.h"
+#include "SoftSerial.h"
 
 class AP_RCProtocol_SBUS : public AP_RCProtocol_Backend {
 public:
-    AP_RCProtocol_SBUS(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {}
+    AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool inverted);
     void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
     void process_byte(uint8_t byte, uint32_t baudrate) override;
 private:
+    void _process_byte(uint32_t timestamp_us, uint8_t byte);
     bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
                      bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values);
-    struct {
+
-        uint16_t bytes[25]; // including start bit, parity and stop bits
+    bool inverted;
-        uint16_t bit_ofs;
+    SoftSerial ss{100000, SoftSerial::SERIAL_CONFIG_8E2I};
-    } sbus_state;
+    uint32_t saved_width;
 
     struct {
         uint8_t buf[25];
         uint8_t ofs;
-        uint32_t last_byte_ms;
+        uint32_t last_byte_us;
     } byte_input;
 };

libraries/AP_RCProtocol/AP_RCProtocol_SBUS_NI.h

@@ -1,32 +0,0 @@
-/*
- * This file is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This file is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-#pragma once
-
-#include "AP_RCProtocol.h"
-#include "AP_RCProtocol_SBUS.h"
-
-class AP_RCProtocol_SBUS_NI : public AP_RCProtocol_SBUS {
-public:
-    AP_RCProtocol_SBUS_NI(AP_RCProtocol &_frontend) : AP_RCProtocol_SBUS(_frontend), saved_width(0) {}
-    void process_pulse(uint32_t width_s0, uint32_t width_s1) override
-    {
-        AP_RCProtocol_SBUS::process_pulse(saved_width, width_s0);
-        saved_width = width_s1;
-    }
-private:
-    uint32_t saved_width;
-};