From 0cfe5b6c761ab388c98a9f8bbca056a93671ec1e Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <rco@iptel.org>
Date: Tue, 28 Jul 2015 23:29:04 +0200
Subject: [PATCH 1/2] Added support for SBUS failsafe bit

---
 radio/src/sbus.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp
index ce16fe33a..9321d65d6 100644
--- a/radio/src/sbus.cpp
+++ b/radio/src/sbus.cpp
@@ -36,6 +36,11 @@
 
 #include "opentx.h"
 
+#define SBUS_FLAGS_IDX 23
+
+#define SBUS_FRAMELOST_BIT   2
+#define SBUS_FAILSAFE_BIT    3
+
 Fifo<32> sbusFifo;
 uint8_t SbusFrame[28] ;
 uint16_t SbusTimer ;
@@ -54,6 +59,11 @@ void processSbusFrame(uint8_t *sbus, int16_t *pulses, uint32_t size)
     return;
   }
 
+  if (size > SBUS_FLAGS_IDX &&
+      (sbus[SBUS_FLAGS_IDX-1] & (1<<SBUS_FAILSAFE_BIT))) {
+    return; // SBUS failsafe mode
+  }
+
   for (uint32_t i=0; i<NUM_TRAINER; i++) {
     while (inputbitsavailable < 11) {
       inputbits |= *sbus++ << inputbitsavailable;

From d2ed8d92870eaa069fd525ea56e5eda6d26e01a0 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <rco@iptel.org>
Date: Wed, 29 Jul 2015 08:47:58 +0200
Subject: [PATCH 2/2] Beautified SBUS code
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- use “defines” for each constant.
- do not change the frame pointer while checking frame validity (improves readability).
---
 radio/src/sbus.cpp | 40 ++++++++++++++++++++++++++--------------
 1 file changed, 26 insertions(+), 14 deletions(-)

diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp
index 9321d65d6..1cb813481 100644
--- a/radio/src/sbus.cpp
+++ b/radio/src/sbus.cpp
@@ -36,13 +36,23 @@
 
 #include "opentx.h"
 
-#define SBUS_FLAGS_IDX 23
+#define SBUS_MIN_FRAME_SIZE   23
+#define SBUS_MAX_FRAME_SIZE   28
 
-#define SBUS_FRAMELOST_BIT   2
-#define SBUS_FAILSAFE_BIT    3
+#define SBUS_FRAME_GAP_DELAY 1000 // 500uS
+
+#define SBUS_START_BYTE     0x0F
+#define SBUS_FLAGS_IDX        23
+#define SBUS_FRAMELOST_BIT     2
+#define SBUS_FAILSAFE_BIT      3
+
+#define SBUS_CH_BITS          11
+#define SBUS_CH_MASK ((1<<SBUS_CH_BITS)-1)
+
+#define SBUS_CH_CENTER     0x3E0
 
 Fifo<32> sbusFifo;
-uint8_t SbusFrame[28] ;
+uint8_t SbusFrame[SBUS_MAX_FRAME_SIZE];
 uint16_t SbusTimer ;
 uint8_t SbusIndex = 0 ;
 
@@ -51,11 +61,11 @@ void processSbusFrame(uint8_t *sbus, int16_t *pulses, uint32_t size)
   uint32_t inputbitsavailable = 0;
   uint32_t inputbits = 0;
 
-  if (*sbus++ != 0x0F) {
+  if (sbus[0] != SBUS_START_BYTE) {
     return; // not a valid SBUS frame
   }
 
-  if (size < 23) {
+  if (size < SBUS_MIN_FRAME_SIZE) {
     return;
   }
 
@@ -64,20 +74,22 @@ void processSbusFrame(uint8_t *sbus, int16_t *pulses, uint32_t size)
     return; // SBUS failsafe mode
   }
 
+  // Skip start byte
+  sbus++;
+
   for (uint32_t i=0; i<NUM_TRAINER; i++) {
-    while (inputbitsavailable < 11) {
+    while (inputbitsavailable < SBUS_CH_BITS) {
       inputbits |= *sbus++ << inputbitsavailable;
       inputbitsavailable += 8;
     }
-    *pulses++ = ((int32_t) (inputbits & 0x7FF) - 0x3E0) * 5 / 8;
-    inputbitsavailable -= 11;
-    inputbits >>= 11;
+    *pulses++ = ((int32_t) (inputbits & SBUS_CH_MASK) - SBUS_CH_CENTER) * 5 / 8;
+    inputbitsavailable -= SBUS_CH_BITS;
+    inputbits >>= SBUS_CH_BITS;
   }
 
   ppmInValid = PPM_IN_VALID_TIMEOUT;
 }
 
-#define SBUS_DELAY 1000 // 500uS
 void processSbusInput()
 {
   uint8_t rxchar;
@@ -85,8 +97,8 @@ void processSbusInput()
   while (sbusFifo.pop(rxchar)) {
     active = 1;
     SbusFrame[SbusIndex++] = rxchar;
-    if (SbusIndex > 27) {
-      SbusIndex = 27;
+    if (SbusIndex > SBUS_MAX_FRAME_SIZE-1) {
+      SbusIndex = SBUS_MAX_FRAME_SIZE-1;
     }
   }
   if (active) {
@@ -95,7 +107,7 @@ void processSbusInput()
   }
   else {
     if (SbusIndex) {
-      if ((uint16_t) (getTmr2MHz() - SbusTimer) > SBUS_DELAY) {
+      if ((uint16_t) (getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) {
         processSbusFrame(SbusFrame, g_ppmIns, SbusIndex);
         SbusIndex = 0;
       }