Merge pull request #35 from DzikuVx/tx-sbus

SBUS support on TX module

.vscode/c_cpp_properties.json

@@ -3,19 +3,12 @@
         {
             "name": "Win32",
             "includePath": [
-                "${workspaceRoot}",
+                "${workspaceRoot}"
-                "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\cores\\arduino",
-                "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries",
-                "C:\\Users\\pspyc\\Documents\\Arduino\\libraries",
-                "C:\\Users\\pspyc\\Documents\\Arduino\\libraries\\PPMReader"
             ],
             "browse": {
                 "limitSymbolsToIncludedHeaders": false,
                 "path": [
-                    "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\cores\\arduino",
+                    "${workspaceRoot}"
-                    "C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries",
-                    "${workspaceRoot}",
-                    "C:\\Users\\pspyc\\Documents\\Arduino\\libraries"
                 ]
             },
             "intelliSenseMode": "msvc-x64"

crossbow.ino

@@ -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;
@@ -170,14 +172,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
 
@@ -210,6 +207,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;
@@ -311,6 +313,10 @@ void loop(void)
 
 #ifdef DEVICE_MODE_TX
 
+    if (Serial1.available()) {
+        sbusRead(Serial1, &sbusInput);
+    }
+
     if (
         radioState.deviceState == RADIO_STATE_RX &&
         qsp.protocolState == QSP_STATE_IDLE &&
@@ -320,9 +326,12 @@ void loop(void)
         int8_t frameToSend = getFrameToTransmit(&qsp);
 
     #ifndef FORCE_TX_WITHOUT_INPUT
-        if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) {
+        /*
+         * If module is not receiving data from radio, do not send RC DATA
+         * This is the only way to trigger failsafe in that case
+         */
+        if (frameToSend == QSP_FRAME_RC_DATA && !isReceivingSbus(&sbusInput)) {
             frameToSend = -1;
-            //FIXME uncomment to enable full Failsafe
         }
     #endif
 
@@ -337,7 +346,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, volatile 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

@@ -12,6 +12,8 @@
 #define SBUS_STATE_FAILSAFE 0x08
 #define SBUS_STATE_SIGNALLOSS 0x04
 
+#define SBUS_IS_RECEIVING_THRESHOLD 250 //If there is no SBUS input for 250ms, assume connection is broken
+
 /*
 Precomputed mapping from 990-2010 to 173:1811
 equivalent to 
@@ -21,6 +23,11 @@ int mapChannelToSbus(int in) {
     return (((long) in * 1605l) / 1000l) - 1417;
 }
 
+int mapSbusToChannel(int in) {
+    //TODO, speed up this processing
+    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};
@@ -67,4 +74,62 @@ 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]);
+            }
+
+			sbusInput->lastChannelReceivedAt = millis();
+        }
+        
+	}
+}
+
+bool isReceivingSbus(SbusInput_t *sbusInput) {
+    return  !(millis() - sbusInput->lastChannelReceivedAt > SBUS_IS_RECEIVING_THRESHOLD);
 }

sbus.h

@@ -1,3 +1,5 @@
 #include "Arduino.h"
 
-void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe);
+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

@@ -124,4 +124,9 @@ struct QspConfiguration_t {
     uint32_t frameDecodingStartedAt = 0;
     uint32_t lastTxSlotTimestamp = 0;
     bool transmitWindowOpen = false;
+};
+
+struct SbusInput_t {
+    int16_t channels[16] = {};
+    uint32_t lastChannelReceivedAt = 0;
 };