Improved sbus handling with a possibility to abandon stuck SBUS frames

crossbow/crossbow.ino

@@ -342,8 +342,13 @@ void loop(void)
      */
 #ifdef DEVICE_MODE_TX
 
+    txInput.recoverStuckFrames();
+
     static uint32_t serialRestartMillis = 0;
 
+    /*
+     * Final guard for SBUS input. If there is no input, try to restart serial port
+     */
     if (!txInput.isReceiving() && serialRestartMillis + 100 < currentMillis) {
         txInput.restart();
         serialRestartMillis = currentMillis;

crossbow/sbus.cpp

@@ -102,6 +102,43 @@ void SbusInput::restart(void)
     start();
 }
 
+/*
+ * We prevent from frame decoding being stuck somewhere
+ */
+void SbusInput::recoverStuckFrames(void)
+{
+    if (
+        _protocolState == SBUS_DECODING_STATE_IN_PROGRESS &&
+        millis() - _frameDecodingStartedAt > 6
+    ) {
+        _protocolState = SBUS_DECODING_STATE_IDLE;
+        _serial.flush();
+    }
+}
+
+void sbusToChannels(volatile int16_t channels[], byte buffer[]) {
+    channels[0]  = ((buffer[1]    |buffer[2]<<8)                 & 0x07FF);
+    channels[1]  = ((buffer[2]>>3 |buffer[3]<<5)                 & 0x07FF);
+    channels[2]  = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10)  & 0x07FF);
+    channels[3]  = ((buffer[5]>>1 |buffer[6]<<7)                 & 0x07FF);
+    channels[4]  = ((buffer[6]>>4 |buffer[7]<<4)                 & 0x07FF);
+    channels[5]  = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9)   & 0x07FF);
+    channels[6]  = ((buffer[9]>>2 |buffer[10]<<6)                & 0x07FF);
+    channels[7]  = ((buffer[10]>>5|buffer[11]<<3)                & 0x07FF);
+    channels[8]  = ((buffer[12]   |buffer[13]<<8)                & 0x07FF);
+    channels[9]  = ((buffer[13]>>3|buffer[14]<<5)                & 0x07FF);
+    channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
+    channels[11] = ((buffer[16]>>1|buffer[17]<<7)                & 0x07FF);
+    channels[12] = ((buffer[17]>>4|buffer[18]<<4)                & 0x07FF);
+    channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9)  & 0x07FF);
+    channels[14] = ((buffer[20]>>2|buffer[21]<<6)                & 0x07FF);
+    channels[15] = ((buffer[21]>>5|buffer[22]<<3)                & 0x07FF);
+
+    for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
+        channels[channelIndex] = mapSbusToChannel(channels[channelIndex]);
+    }
+}
+
 void SbusInput::sbusRead() {
     static byte buffer[25];
 	static byte buffer_index = 0;
@@ -111,50 +148,36 @@ void SbusInput::sbusRead() {
 
 	while (_serial.available()) {
 		byte rx = _serial.read();
-		if (buffer_index == 0 && rx != SBUS_FRAME_HEADER) {
+
-			//incorrect start byte, out of sync
+        if (_protocolState == SBUS_DECODING_STATE_IDLE) {
-			_decoderErrorFrames++;
+            //We allow next frame to start only when previous frame eneded some time ago
-			continue;
+            if (
-		}
+                rx == SBUS_FRAME_HEADER &&
+                millis() - _frameDecodingEndedAt > 5
+            ) {
+                //Header is correct
+                _frameDecodingStartedAt = millis();
+                buffer_index = 0;
+                _protocolState = SBUS_DECODING_STATE_IN_PROGRESS;
+            }
+        } 
 
         buffer[buffer_index] = rx;
         buffer_index++;
 
-		if (buffer_index == 25) {
+        if (
-			buffer_index = 0;
+            _protocolState == SBUS_DECODING_STATE_IN_PROGRESS && 
-			if (buffer[24] != SBUS_FRAME_FOOTER) {
+            buffer_index == 25 &&
-				//incorrect end byte, out of sync
+            buffer[24] == SBUS_FRAME_FOOTER
-				_decoderErrorFrames++;
+        ) {
-				continue;
+            //We have full frame now
-			}
+            _frameDecodingEndedAt = millis();
-			_goodFrames++;
+            sbusToChannels(channels, buffer);
-
+            _protocolState = SBUS_DECODING_STATE_IDLE;
-			channels[0]  = ((buffer[1]    |buffer[2]<<8)                 & 0x07FF);
-			channels[1]  = ((buffer[2]>>3 |buffer[3]<<5)                 & 0x07FF);
-			channels[2]  = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10)  & 0x07FF);
-			channels[3]  = ((buffer[5]>>1 |buffer[6]<<7)                 & 0x07FF);
-			channels[4]  = ((buffer[6]>>4 |buffer[7]<<4)                 & 0x07FF);
-			channels[5]  = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9)   & 0x07FF);
-			channels[6]  = ((buffer[9]>>2 |buffer[10]<<6)                & 0x07FF);
-			channels[7]  = ((buffer[10]>>5|buffer[11]<<3)                & 0x07FF);
-			channels[8]  = ((buffer[12]   |buffer[13]<<8)                & 0x07FF);
-			channels[9]  = ((buffer[13]>>3|buffer[14]<<5)                & 0x07FF);
-			channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
-			channels[11] = ((buffer[16]>>1|buffer[17]<<7)                & 0x07FF);
-			channels[12] = ((buffer[17]>>4|buffer[18]<<4)                & 0x07FF);
-			channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9)  & 0x07FF);
-			channels[14] = ((buffer[20]>>2|buffer[21]<<6)                & 0x07FF);
-			channels[15] = ((buffer[21]>>5|buffer[22]<<3)                & 0x07FF);
-
-			for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
-                channels[channelIndex] = mapSbusToChannel(channels[channelIndex]);
-            }
-
-			_lastChannelReceivedAt = millis();
         }
 	}
 }
 
 bool SbusInput::isReceiving() {
-    return  !(millis() - _lastChannelReceivedAt > SBUS_IS_RECEIVING_THRESHOLD);
+    return  !(millis() - _frameDecodingEndedAt > SBUS_IS_RECEIVING_THRESHOLD);
 }

crossbow/sbus.h

@@ -5,6 +5,10 @@
 #include "Arduino.h"
 #include "tx_input.h"
 
+enum sbusProtocolStates {
+    SBUS_DECODING_STATE_IDLE,
+    SBUS_DECODING_STATE_IN_PROGRESS
+};
 
 class SbusInput : public TxInput
 {
@@ -14,9 +18,12 @@ class SbusInput : public TxInput
     void restart(void);
     void loop(void);
     bool isReceiving(void);
+    void recoverStuckFrames(void);
   private:
   	HardwareSerial &_serial;
-    uint32_t _lastChannelReceivedAt = 0;
+    uint32_t _frameDecodingStartedAt = 0;
+    uint32_t _frameDecodingEndedAt = 0 ;
+    uint8_t _protocolState = SBUS_DECODING_STATE_IDLE;
 	void sbusRead(void);
 };