added CRSF trainer input

SUMD is still only stub
EdgeTX · Mar 25, 2022 · 7adf049 · 7adf049
1 parent d9dc058
commit 7adf049
Show file tree

Hide file tree

Showing 27 changed files with 258 additions and 34 deletions.
diff --git a/radio/src/CMakeLists.txt b/radio/src/CMakeLists.txt
@@ -477,6 +477,8 @@ set(SRC
   serial.cpp
   sbus.cpp
   ibus.cpp
+  crsf.cpp
+  sumd.cpp
   )
 
 if(GUI)

diff --git a/radio/src/crc.cpp b/radio/src/crc.cpp
@@ -81,6 +81,17 @@ uint8_t crc8(const uint8_t * ptr, uint32_t len)
   return crc;
 }
 
+void CRC8::reset() {
+    mSum = 0;
+}
+CRC8& CRC8::operator+=(const uint8_t b) {
+    mSum = crc8tab[mSum ^ b];
+    return *this;
+}
+CRC8::operator uint8_t() const {
+    return mSum;
+}
+
 // CRC8 implementation with polynom = 0xBA
 const unsigned char crc8tab_BA[256] = {
   0x00, 0xBA, 0xCE, 0x74, 0x26, 0x9C, 0xE8, 0x52,

diff --git a/radio/src/crc.h b/radio/src/crc.h
@@ -104,4 +104,12 @@ static const unsigned short crc16tab_1189[256] = {
   0x7bc7,0x6a4e,0x58d5,0x495c,0x3de3,0x2c6a,0x1ef1,0x0f78
 };
 
+struct CRC8 final {
+    void reset();
+    CRC8& operator+=(const uint8_t b);
+    operator uint8_t() const;
+private:
+    uint8_t mSum = 0;
+};
+
 #endif
diff --git a/radio/src/crsf.cpp b/radio/src/crsf.cpp
@@ -0,0 +1,150 @@
+#include "crsf.h"
+#include "opentx.h"
+#include "trainer.h"
+
+#include <algorithm>
+#include <limits>
+
+#ifdef __GNUC__
+# pragma GCC diagnostic push
+# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch
+#endif
+
+namespace CRSF {
+    struct Servo {
+        using Crsf = Trainer::Protocol::Crsf;
+        using MesgType = Crsf::MesgType;
+
+        static constexpr uint8_t mPauseCount{2}; // 2ms
+
+        enum class State : uint8_t {Undefined, GotFCAddress, GotLength, Channels, Data, AwaitCRC, AwaitCRCAndDecode};
+
+        static inline int16_t convertCrsfToPuls(uint16_t const value) {
+            const int16_t centered = value - Crsf::CenterValue;
+            return Trainer::clamp((centered * 5) / 8);
+        }
+
+        static inline void tick1ms() {
+            if (mPauseCounter > 0) {
+                --mPauseCounter;
+            }
+            else {
+                mState = State::Undefined;
+            }
+        }
+
+        static inline void process(const uint8_t b, const std::function<void()> f) {
+            mPauseCounter = mPauseCount;
+            switch(mState) { // enum-switch -> no default (intentional)
+            case State::Undefined:
+                csum.reset();
+                if (b == Crsf::FcAddress) {
+                    mState = State::GotFCAddress;
+                }
+                break;
+            case State::GotFCAddress:
+                if ((b > 2) && (b <= mData.size())) {
+                    mLength = b - 2; // only payload (not including type and crc)
+                    mIndex = 0;
+                    mState = State::GotLength;
+                }
+                else {
+                    mState = State::Undefined;
+                }
+                break;
+            case State::GotLength:
+                csum += b;
+                if ((b == Crsf::FrametypeChannels) && (mLength == 22)) {
+                    mState = State::Channels;
+                }
+                else {
+                    mState = State::Data;
+                }
+                break;
+            case State::Data:
+                csum += b;
+                if (++mIndex == mLength) {
+                    mState = State::AwaitCRC;
+                }
+                break;
+            case State::Channels:
+                csum += b;
+                break;
+            case State::AwaitCRC:
+                if (csum == b) {
+                    mState = State::Undefined;
+                } 
+                else {
+                    mState = State::Undefined;
+                }
+                break;
+            case State::AwaitCRCAndDecode:
+                if (csum == b) {
+                    ++mPackages;
+                    f();
+                    mState = State::Undefined;
+                } 
+                else {
+                    mState = State::Undefined;
+                }
+                break;
+            }            
+        }        
+        static inline void convert(int16_t* const pulses) {
+            static_assert(MAX_TRAINER_CHANNELS == 16);
+            pulses[0]  = (uint16_t) (((mData[0]    | mData[1] << 8))                 & Crsf::ValueMask);
+            pulses[1]  = (uint16_t) ((mData[1]>>3  | mData[2] <<5)                   & Crsf::ValueMask);
+            pulses[2]  = (uint16_t) ((mData[2]>>6  | mData[3] <<2 | mData[4]<<10)  	 & Crsf::ValueMask);
+            pulses[3]  = (uint16_t) ((mData[4]>>1  | mData[5] <<7)                   & Crsf::ValueMask);
+            pulses[4]  = (uint16_t) ((mData[5]>>4  | mData[6] <<4)                   & Crsf::ValueMask);
+            pulses[5]  = (uint16_t) ((mData[6]>>7  | mData[7] <<1 | mData[8]<<9)   	 & Crsf::ValueMask);
+            pulses[6]  = (uint16_t) ((mData[8]>>2  | mData[9] <<6)                   & Crsf::ValueMask);
+            pulses[7]  = (uint16_t) ((mData[9]>>5  | mData[10]<<3)                   & Crsf::ValueMask);
+            pulses[8]  = (uint16_t) ((mData[11]    | mData[12]<<8)                   & Crsf::ValueMask);
+            pulses[9]  = (uint16_t) ((mData[12]>>3 | mData[13]<<5)                   & Crsf::ValueMask);
+            pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 | mData[15]<<10) 	 & Crsf::ValueMask);
+            pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7)                   & Crsf::ValueMask);
+            pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4)                   & Crsf::ValueMask);
+            pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 | mData[19]<<9)  	 & Crsf::ValueMask);
+            pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6)                   & Crsf::ValueMask);
+            pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3)                   & Crsf::ValueMask);
+
+            for(size_t i = 0; i < MAX_TRAINER_CHANNELS; ++i) {
+                pulses[i] = convertCrsfToPuls(pulses[i]);
+            }
+        }
+    private:
+        static CRC8 csum;
+        static State mState;
+        static MesgType mData; 
+        static uint8_t mIndex;
+        static uint8_t mLength;
+        static uint16_t mPackages;
+        static uint8_t mPauseCounter;
+    };
+
+    CRC8 Servo::csum;
+    Servo::State Servo::mState{Servo::State::Undefined};
+    Servo::MesgType Servo::mData; 
+    uint8_t Servo::mIndex{0};
+    uint8_t Servo::mLength{0};
+    uint16_t Servo::mPackages{0};
+    uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms
+}
+
+void processCrsfInput() {
+#if !defined(SIMU)
+  uint8_t rxchar;
+
+  while (sbusAuxGetByte(&rxchar)) {
+      CRSF::Servo::process(rxchar, [&](){
+          CRSF::Servo::convert(ppmInput);
+          ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;        
+      });
+  }
+#endif
+}
+
+#ifdef __GNUC__
+# pragma GCC diagnostic pop
+#endif
diff --git a/radio/src/crsf.h b/radio/src/crsf.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "dataconstants.h"
+#include "crc.h"
+#include "telemetry/crossfire.h"
+
+#define CRSF_BAUDRATE         400000
+
+void processCrsfInput();
+
diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h
@@ -231,6 +231,8 @@ enum UartModes {
   UART_MODE_TELEMETRY,
   UART_MODE_SBUS_TRAINER,
   UART_MODE_IBUS_TRAINER,
+  UART_MODE_CRSF_TRAINER,
+  UART_MODE_SUMD_TRAINER,
   UART_MODE_LUA,
   UART_MODE_CLI,
   UART_MODE_GPS,

diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp
@@ -382,7 +382,7 @@ int hasSerialMode(int mode)
 
 bool isTrainerInputMode(const int mode) 
 {
-    return (mode == UART_MODE_IBUS_TRAINER) || (mode == UART_MODE_SBUS_TRAINER);
+    return (mode == UART_MODE_IBUS_TRAINER) || (mode == UART_MODE_SBUS_TRAINER) || (mode == UART_MODE_CRSF_TRAINER) || (mode == UART_MODE_SUMD_TRAINER);
 }
 
 int trainerInputActive() 
@@ -428,7 +428,7 @@ bool isSerialModeAvailable(const uint8_t port_nr, const int mode)
 #if defined(USB_SERIAL)
   // Telemetry input & SBUS trainer on VCP is not yet supported
   if (port_nr == SP_VCP &&
-      (mode == UART_MODE_TELEMETRY || mode == UART_MODE_SBUS_TRAINER || mode == UART_MODE_IBUS_TRAINER))
+      (mode == UART_MODE_TELEMETRY || mode == UART_MODE_SBUS_TRAINER || mode == UART_MODE_IBUS_TRAINER || mode == UART_MODE_CRSF_TRAINER || mode == UART_MODE_SUMD_TRAINER))
     return false;
 #endif
 
@@ -857,7 +857,8 @@ bool isTrainerModeAvailable(int mode)
 
   if (mode == TRAINER_MODE_MASTER_SERIAL) {
 #if defined(SBUS_TRAINER)
-    return (hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0) || (hasSerialMode(UART_MODE_IBUS_TRAINER) >= 0);
+    return (hasSerialMode(UART_MODE_SBUS_TRAINER) >= 0) || (hasSerialMode(UART_MODE_IBUS_TRAINER) >= 0) 
+            || (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) || (hasSerialMode(UART_MODE_SUMD_TRAINER) >= 0);
 #else
     return false;
 #endif

diff --git a/radio/src/opentx.h b/radio/src/opentx.h
@@ -610,6 +610,8 @@ static inline void GET_ADC_IF_MIXER_NOT_RUNNING()
 
 #include "sbus.h"
 #include "ibus.h"
+#include "crsf.h"
+#include "sumd.h"
 
 void resetBacklightTimeout();
 void checkBacklight();

diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp
@@ -158,11 +158,12 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por
 
 #if defined(SBUS_TRAINER)
   case UART_MODE_SBUS_TRAINER:
+  case UART_MODE_IBUS_TRAINER:
+  case UART_MODE_CRSF_TRAINER:
+  case UART_MODE_SUMD_TRAINER:
     sbusSetAuxGetByte(ctx, getByte);
     // TODO: setRxCb (see MODE_LUA)
     break;
-  case UART_MODE_IBUS_TRAINER:
-    sbusSetAuxGetByte(ctx, getByte);
     break;
 #endif
 
@@ -241,7 +242,25 @@ static void serialSetupPort(int mode, etx_serial_init& params, bool& power_requi
     params.rx_enable = true;
     power_required = true;
     break;
+
+  case UART_MODE_CRSF_TRAINER:
+    params.baudrate = CRSF_BAUDRATE;
+    params.word_length = ETX_WordLength_8;
+    params.parity = ETX_Parity_None;
+    params.stop_bits = ETX_StopBits_One;
+    params.rx_enable = true;
+    power_required = true;
+    break;
 
+  case UART_MODE_SUMD_TRAINER:
+    params.baudrate = SUMD_BAUDRATE;
+    params.word_length = ETX_WordLength_8;
+    params.parity = ETX_Parity_None;
+    params.stop_bits = ETX_StopBits_One;
+    params.rx_enable = true;
+    power_required = true;
+    break;
+
 #if defined(LUA)
   case UART_MODE_LUA:
     params.baudrate = LUA_DEFAULT_BAUDRATE;

diff --git a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp
@@ -1774,6 +1774,8 @@ static const struct YamlIdStr enum_UartModes[] = {
   {  UART_MODE_TELEMETRY, "TELEMETRY_IN"  },
   {  UART_MODE_SBUS_TRAINER, "SBUS_TRAINER"  },
   {  UART_MODE_IBUS_TRAINER, "IBUS_TRAINER"  },
+  {  UART_MODE_CRSF_TRAINER, "CRSF_TRAINER"  },
+  {  UART_MODE_SUMD_TRAINER, "SUMD_TRAINER"  },
   {  UART_MODE_LUA, "LUA"  },
   {  UART_MODE_CLI, "CLI"  },
   {  UART_MODE_GPS, "GPS"  },

diff --git a/radio/src/sumd.cpp b/radio/src/sumd.cpp
@@ -0,0 +1 @@
+#include "sumd.h"
diff --git a/radio/src/sumd.h b/radio/src/sumd.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#define SUMD_BAUDRATE         115200
+
+void processSumdInput();
+
diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp
@@ -127,23 +127,13 @@ void execMixerFrequentActions()
     else if (hasSerialMode(UART_MODE_IBUS_TRAINER) >= 0) {
         processIbusInput();
     }
+    else if (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) {
+        processCrsfInput();
+    }
+    else if (hasSerialMode(UART_MODE_SUMD_TRAINER) >= 0) {
+//        processSumdInput();
+    }
 #endif
-//#if defined(SBUS_TRAINER) && (defined(AUX_SERIAL) || defined(AUX2_SERIAL))
-//    if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER) 
-//        #if defined(AUX2_SERIAL)
-//            || (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER)
-//        #endif
-//            ) {
-//        processSbusInput();
-//    }
-//    else if ((g_eeGeneral.auxSerialMode == UART_MODE_IBUS_TRAINER)
-//         #if defined(AUX2_SERIAL)
-//             || (g_eeGeneral.aux2SerialMode == UART_MODE_IBUS_TRAINER)
-//         #endif
-//             ) {
-//        processIbusInput();
-//    }
-//#endif
 
 #if defined(GYRO)
   gyro.wakeup();

diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h
@@ -27,6 +27,7 @@
 
 // Device address
 #define BROADCAST_ADDRESS              0x00
+#define FC_ADDRESS                     0xC8
 #define RADIO_ADDRESS                  0xEA
 #define MODULE_ADDRESS                 0xEE
 

diff --git a/radio/src/trainer.h b/radio/src/trainer.h
@@ -56,7 +56,26 @@ namespace Trainer {
             static constexpr uint16_t MaxValue = 988;            
             static constexpr uint16_t MinValue = 2011;            
             static constexpr uint16_t CenterValue = (MaxValue + MinValue) / 2;            
-        };        
+        };     
+        struct Crsf {
+//            Every frame has the structure:
+//            <Device address><Frame length><Type><Payload><CRC>            
+            using MesgType = std::array<uint8_t, 64>;
+
+            static constexpr uint8_t  ValueBits = 11;
+            static constexpr uint16_t ValueMask = ((1 << ValueBits) - 1);
+
+            static constexpr uint8_t FcAddress = FC_ADDRESS;
+            static constexpr uint8_t FrametypeChannels = CHANNELS_ID;
+            static constexpr uint8_t FrametypeLink = LINK_ID;
+
+            static constexpr uint16_t CenterValue = 0x3e0;            
+
+        };
+        struct SumDV3 {
+            using MesgType = std::array<uint8_t, 28>;
+
+        };
     }    
 }
 

diff --git a/radio/src/translations/cn.h.txt b/radio/src/translations/cn.h.txt
@@ -63,7 +63,7 @@
 #define TR_TRNCHN                      "CH1CH2CH3CH4"
 
 #define LEN_AUX_SERIAL_MODES           "\010"
-#define TR_AUX_SERIAL_MODES            "调试\0   " "回传镜像" "回传输入" "SBUS教练" "IBUS教练" "LUA脚本\0""CLI\0     ""GPS\0     ""Debug\0   "
+#define TR_AUX_SERIAL_MODES            "调试\0   " "回传镜像" "回传输入" "SBUS教练" "IBUS教练""CRSF教练""SUMD教练" "LUA脚本\0""CLI\0     ""GPS\0     ""Debug\0   "
 
 #define LEN_SWTYPES                    "\004"
 #define TR_SWTYPES                     "无\0 " "回弹" "2段\0""3段\0"

diff --git a/radio/src/translations/cz.h.txt b/radio/src/translations/cz.h.txt
@@ -61,7 +61,7 @@
 #define TR_TRNCHN                      "CH1CH2CH3CH4"
 
 #define LEN_AUX_SERIAL_MODES           "\015"
-#define TR_AUX_SERIAL_MODES            "VYP\0         ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""IBUS Trenér\0 ""LUA\0         ""CLI\0         ""GPS\0         ""Debug\0       "
+#define TR_AUX_SERIAL_MODES            "VYP\0         ""Telem Mirror\0""Telemetry In\0""SBUS Trenér\0 ""IBUS Trenér\0 ""CRSF Trenér\0 ""SUMD Trenér\0 ""LUA\0         ""CLI\0         ""GPS\0         ""Debug\0       "
 
 #define LEN_SWTYPES                    "\013"
 #define TR_SWTYPES                     "Žádný\0     ""Bez aretace""2-polohový\0""3-polohový\0"

diff --git a/radio/src/translations/de.h.txt b/radio/src/translations/de.h.txt
@@ -62,7 +62,7 @@
 #define TR_TRNCHN                      "CH1CH2CH3CH4"
 
 #define LEN_AUX_SERIAL_MODES           "\015"
-#define TR_AUX_SERIAL_MODES    			"AUS\0         ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""IBUS Eingang\0""LUA\0         ""CLI\0         ""GPS\0         ""Debug\0       "
+#define TR_AUX_SERIAL_MODES    			"AUS\0         ""Telem Mirror\0""Telemetry In\0""SBUS Eingang\0""IBUS Eingang\0""CRSF Eingang\0""SUMD Eingang\0""LUA\0         ""CLI\0         ""GPS\0         ""Debug\0       "
 
 #define LEN_SWTYPES            			"\006"
 #define TR_SWTYPES            			 "Kein\0 ""Taster""2POS\0 ""3POS\0"