Sbus decoding

.vscode/arduino.json

@@ -1,6 +1,6 @@
 {
     "board": "bsfrance:avr:lora32u4",
     "sketch": "crossbow.ino",
-    "port": "COM17",
+    "port": "COM4",
     "output": "./build"
 }

.vscode/c_cpp_properties.json

@@ -3,8 +3,7 @@
         {
             "name": "Win32",
             "includePath": [
-                "C:\\Users\\pspyc\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\avr\\1.6.19\\cores\\arduino",
+                "C:\\Users\\pspyc\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\avr\\1.6.19\\cores\\arduino"
-                "C:\\Users\\pspyc\\Documents\\Arduino\\libraries\\PPMReader"
             ],
             "browse": {
                 "limitSymbolsToIncludedHeaders": false,

crossbow.ino

@@ -1,10 +1,10 @@
 #define DEVICE_MODE_TX
 // #define DEVICE_MODE_RX
 
-#define FEATURE_TX_OLED
+// #define FEATURE_TX_OLED
-// #define FORCE_TX_WITHOUT_INPUT
+#define FORCE_TX_WITHOUT_INPUT
 
-// #define DEBUG_SERIAL
+#define DEBUG_SERIAL
 // #define DEBUG_PING_PONG
 // #define DEBUG_LED
 
@@ -12,6 +12,7 @@
 #include "variables.h"
 #include "main_variables.h"
 #include "qsp.h"
+#include "sbus.h"
 
 // LoRa32u4 ports
 #define LORA32U4_SS_PIN     8
@@ -22,8 +23,12 @@
  * Main defines for device working in TX mode
  */
 #ifdef DEVICE_MODE_TX
-#include <PPMReader.h>
+// #include <PPMReader.h>
-PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
+// PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
+
+SbusInput_t sbusInput = {};
+
+// FUTABA_SBUS sBus;
 
 #include "txbuzzer.h"
 
@@ -44,9 +49,6 @@ uint32_t lastOledTaskTime = 0;
  * Main defines for device working in RX mode
  */
 #ifdef DEVICE_MODE_RX
-
-    #include "sbus.h"
-
     uint32_t sbusTime = 0;
     uint8_t sbusPacket[SBUS_PACKET_LENGTH] = {0};
     uint32_t lastRxStateTaskTime = 0;
@@ -177,14 +179,9 @@ void setup(void)
     pinMode(RX_ADC_PIN_1, INPUT);
     pinMode(RX_ADC_PIN_2, INPUT);
     pinMode(RX_ADC_PIN_3, INPUT);
-
-    Serial1.begin(100000, SERIAL_8E2);
 #endif
 
 #ifdef DEVICE_MODE_TX
-    TCCR1A = 0;  //reset timer1
-    TCCR1B = 0;
-    TCCR1B |= (1 << CS11);  //set timer1 to increment every 0,5 us or 1us on 8MHz
 
 #ifdef FEATURE_TX_OLED
     display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
@@ -214,6 +211,11 @@ void setup(void)
     qsp.debugConfig |= DEBUG_FLAG_LED;
 #endif
 
+    /*
+     * Prepare Serial1 for S.Bus processing
+     */
+    Serial1.begin(100000, SERIAL_8N2);
+    // sBus.begin();
 }
 
 uint8_t currentSequenceIndex = 0;
@@ -292,6 +294,10 @@ void loop(void)
 
 #ifdef DEVICE_MODE_TX
 
+    if (Serial1.available()) {
+        sbusRead(Serial1, &sbusInput);
+    }
+
     if (
         qsp.protocolState == QSP_STATE_IDLE &&
         qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis
@@ -300,9 +306,9 @@ void loop(void)
         int8_t frameToSend = getFrameToTransmit(&qsp);
 
     #ifndef FORCE_TX_WITHOUT_INPUT
-        if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) {
+    //FIXME detect if taranis is really transmitting here
+        if (frameToSend == QSP_FRAME_RC_DATA && false) {
             frameToSend = -1;
-            //FIXME uncomment to enable full Failsafe
         }
     #endif
 
@@ -317,7 +323,7 @@ void loop(void)
                     break;
 
                 case QSP_FRAME_RC_DATA:
-                    encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT);
+                    encodeRcDataPayload(&qsp, sbusInput.channels, PPM_INPUT_CHANNEL_COUNT);
                     break;
             }
 

qsp.cpp

@@ -97,11 +97,11 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
 /**
  * Encode 10 RC channels 
  */
-void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t noOfChannels)
+void encodeRcDataPayload(QspConfiguration_t *qsp, int channels[], uint8_t noOfChannels)
 {
     for (uint8_t i = 0; i < noOfChannels; i++)
     {
-        int cV = constrain(ppmSource->get(i), 1000, 2000);
+        int cV = constrain(channels[i], 1000, 2000);
 
         uint16_t channelValue10 = map(cV, 1000, 2000, 0, 1000) & 0x03ff;
         uint8_t channelValue8 = map(cV, 1000, 2000, 0, 255) & 0xff;

qsp.h

@@ -8,7 +8,7 @@ uint8_t get10bitHighShift(uint8_t channel);
 uint8_t get10bitLowShift(uint8_t channel);
 void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte);
 void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, RadioState_t *radioState);
-void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t noOfChannels);
+void encodeRcDataPayload(QspConfiguration_t *qsp, int channels[], uint8_t noOfChannels);
 void qspDecodeIncomingFrame(
     QspConfiguration_t *qsp, 
     uint8_t incomingByte, 

sbus.cpp

@@ -21,6 +21,10 @@ int mapChannelToSbus(int in) {
     return (((long) in * 1605l) / 1000l) - 1417;
 }
 
+int mapSbusToChannel(int in) {
+    return map(in, 173, 1811, 990, 2010);
+}
+
 void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){
     
     static int output[SBUS_CHANNEL_NUMBER] = {0};
@@ -68,3 +72,61 @@ void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool
     packet[23] = stateByte; //Flags byte
     packet[24] = SBUS_FRAME_FOOTER; //Footer
 }
+
+void sbusRead(HardwareSerial &_serial, SbusInput_t *sbusInput) {
+    static byte buffer[25];
+	static byte buffer_index = 0;
+
+    static uint32_t _decoderErrorFrames;
+    static uint32_t _goodFrames;
+
+	while (_serial.available()) {
+		byte rx = _serial.read();
+		if (buffer_index == 0 && rx != SBUS_FRAME_HEADER) {
+			//incorrect start byte, out of sync
+			_decoderErrorFrames++;
+			continue;
+		}
+
+        buffer[buffer_index] = rx;
+        buffer_index++;
+
+		if (buffer_index == 25) {
+			buffer_index = 0;
+			if (buffer[24] != SBUS_FRAME_FOOTER) {
+				//incorrect end byte, out of sync
+				_decoderErrorFrames++;
+				continue;
+			}
+			_goodFrames++;
+
+			sbusInput->channels[0]  = ((buffer[1]    |buffer[2]<<8)                 & 0x07FF);
+			sbusInput->channels[1]  = ((buffer[2]>>3 |buffer[3]<<5)                 & 0x07FF);
+			sbusInput->channels[2]  = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10)  & 0x07FF);
+			sbusInput->channels[3]  = ((buffer[5]>>1 |buffer[6]<<7)                 & 0x07FF);
+			sbusInput->channels[4]  = ((buffer[6]>>4 |buffer[7]<<4)                 & 0x07FF);
+			sbusInput->channels[5]  = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9)   & 0x07FF);
+			sbusInput->channels[6]  = ((buffer[9]>>2 |buffer[10]<<6)                & 0x07FF);
+			sbusInput->channels[7]  = ((buffer[10]>>5|buffer[11]<<3)                & 0x07FF);
+			sbusInput->channels[8]  = ((buffer[12]   |buffer[13]<<8)                & 0x07FF);
+			sbusInput->channels[9]  = ((buffer[13]>>3|buffer[14]<<5)                & 0x07FF);
+			sbusInput->channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
+			sbusInput->channels[11] = ((buffer[16]>>1|buffer[17]<<7)                & 0x07FF);
+			sbusInput->channels[12] = ((buffer[17]>>4|buffer[18]<<4)                & 0x07FF);
+			sbusInput->channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9)  & 0x07FF);
+			sbusInput->channels[14] = ((buffer[20]>>2|buffer[21]<<6)                & 0x07FF);
+			sbusInput->channels[15] = ((buffer[21]>>5|buffer[22]<<3)                & 0x07FF);
+
+			for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
+                sbusInput->channels[channelIndex] = mapSbusToChannel(sbusInput->channels[channelIndex]);
+            }
+
+            //FIXME remove this debug
+            static uint32_t prev = 0;
+			sbusInput->lastChannelReceivedAt = millis();
+            Serial.println(sbusInput->lastChannelReceivedAt - prev);
+            prev = sbusInput->lastChannelReceivedAt;
+        }
+        
+	}
+}

sbus.h

@@ -1,3 +1,5 @@
 #include "Arduino.h"
 
 void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe);
+void sbusRead(HardwareSerial &_serial, SbusInput_t *sbusInput);
+bool isReceivingSbus(SbusInput_t *sbusInput);

variables.h

@@ -121,3 +121,8 @@ struct QspConfiguration_t {
     uint32_t lastTxSlotTimestamp = 0;
     bool transmitWindowOpen = false;
 };
+
+struct SbusInput_t {
+    int16_t channels[16] = {};
+    uint32_t lastChannelReceivedAt = 0;
+};