HAL_Linux: initial support for parallel SBUS and PPM-SUM decoding

libraries/AP_HAL_Linux/RCInput.cpp

@@ -15,6 +15,7 @@
 #include <stdint.h>
 
 #include "RCInput.h"
+#include "sbus.h"
 
 using namespace Linux;
 
@@ -95,7 +96,7 @@ void LinuxRCInput::clear_overrides()
 
 
 /*
-  process a pulse of the given width
+  process a PPM-sum pulse of the given width
  */
 void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec)
 {
@@ -114,11 +115,18 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec)
         return;
     }
 
+    /*
+      we limit inputs to between 700usec and 2300usec. This allows us
+      to decode SBUS on the same pin, as SBUS will have a maximum
+      pulse width of 100usec
+     */
+    if (width_usec > 700 && width_usec < 2300) {
         // take a reading for the current channel
         _pulse_capt[_channel_counter] = width_usec;
 
         // move to next channel
         _channel_counter++;
+    }
 
     // if we have reached the maximum supported channels then
     // mark as unsynchronised, so we wait for a wide pulse
@@ -129,4 +137,98 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec)
     }
 }
 
+/*
+  process a SBUS input pulse of the given width
+ */
+void LinuxRCInput::_process_sbus_pulse(uint16_t width_s0, uint16_t width_s1)
+{
+    // convert to bit widths, allowing for up to 1usec error, assuming 100000 bps
+    uint16_t bits_s0 = (width_s0+1) / 10;
+    uint16_t bits_s1 = (width_s1+1) / 10;
+    uint16_t nlow;
+
+    uint8_t byte_ofs = sbus_state.bit_ofs/12;
+    uint8_t bit_ofs = sbus_state.bit_ofs%12;
+
+    if (bits_s0 == 0 || bits_s1 == 0) {
+        // invalid data
+        goto reset;
+    }
+	
+    if (bits_s0+bit_ofs > 10) {
+        // invalid data as last two bits must be stop bits
+        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[LINUX_RC_INPUT_NUM_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, 
+                        LINUX_RC_INPUT_NUM_CHANNELS)) {
+            for (i=0; i<num_values; i++) {
+                _pulse_capt[i] = values[i];
+            }
+            _num_channels = num_values;
+        }
+        goto reset;
+    } else if (bits_s1 > 12) {
+        // break
+        goto reset;
+    }
+    return;
+reset:
+    memset(&sbus_state, 0, sizeof(sbus_state));        
+}
+
+/*
+  process a RC input pulse of the given width
+ */
+void LinuxRCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1)
+{
+    // treat as PPM-sum
+    _process_ppmsum_pulse(width_s0 + width_s1);
+
+    // treat as SBUS
+    _process_sbus_pulse(width_s0, width_s1);
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_Linux/RCInput.h

@@ -24,7 +24,7 @@ public:
     virtual void _timer_tick() {}
 
  protected:
-    void _process_ppmsum_pulse(uint16_t width_usec);
+    void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1);
 
  private:
     volatile bool new_rc_input;
@@ -35,8 +35,17 @@ public:
     // the channel we will receive input from next, or -1 when not synchronised
     int8_t _channel_counter;
 
+    void _process_ppmsum_pulse(uint16_t width);
+    void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1);
+
     /* override state */
     uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS];
+
+    // state of SBUS bit decoder
+    struct {
+	uint16_t bytes[25]; // including start bit, parity and stop bits
+	uint16_t bit_ofs;
+    } sbus_state;
 };
 
 #include "RCInput_PRU.h"

libraries/AP_HAL_Linux/RCInput_PRU.cpp

@@ -49,7 +49,7 @@ void LinuxRCInput_PRU::_timer_tick()
         } else {
             // the pulse value is the sum of the time spent in the low
             // and high states
-            _process_ppmsum_pulse(ring_buffer->buffer[ring_buffer->ring_head].delta_t + _s0_time);
+            _process_rc_pulse(_s0_time, ring_buffer->buffer[ring_buffer->ring_head].delta_t);
         }
         // move to the next ring buffer entry
         ring_buffer->ring_head = (ring_buffer->ring_head + 1) % NUM_RING_ENTRIES;