Enable serial trainer for foreign protocols (Ibus/SumD)

radio/src/CMakeLists.txt

@@ -58,6 +58,7 @@ option(IMRC_RELEASE "Used to build IMRC released firmware" OFF)
 option(HARDWARE_TRAINER_MULTI "Allow multi trainer" OFF)
 option(BOOTLOADER "Include Bootloader" ON)
 option(YAML_STORAGE "Enable YAML storage" ON)
+option(EXTENDED_TRAINER "Enable 32 trainer channels for full SumDV3 support" OFF)
 
 # since we reset all default CMAKE compiler flags for firmware builds, provide an alternate way for user to specify additional flags.
 set(FIRMWARE_C_FLAGS "" CACHE STRING "Additional flags for firmware target c compiler (note: all CMAKE_C_FLAGS[_*] are ignored for firmware/bootloader).")
@@ -453,6 +454,10 @@ if(HARDWARE_TRAINER_MULTI)
   add_definitions(-DHARDWARE_TRAINER_MULTI)
 endif()
 
+if(EXTENDED_TRAINER)
+  add_definitions(-DEXTENDED_TRAINER)
+endif()
+
 add_definitions(-DPOPUP_LEVEL=${POPUP_LEVEL})
 
 if(INTERNAL_MODULE_MULTI)
@@ -476,6 +481,9 @@ set(SRC
   model_init.cpp
   serial.cpp
   sbus.cpp
+  ibus.cpp
+  crsf.cpp
+  sumd.cpp
   )
 
 if(GUI)

radio/src/cli.cpp

@@ -31,6 +31,11 @@
 #include <stdarg.h>
 #include "cli.h"
 
+#include "sbus.h"
+#include "ibus.h"
+#include "crsf.h"
+#include "sumd.h"
+
 #if defined(INTMODULE_USART)
 #include "intmodule_serial_driver.h"
 #endif
@@ -1559,6 +1564,15 @@ int cliCrypt(const char ** argv)
 }
 #endif
 
+#if defined(EXTENDED_TRAINER)
+static int trainer_stats(const char** const argv) {
+    cliSerialPrint("packages: sbus: %d, ibus: %d, crsf: %d, sumd: %d", 
+                   SBus::Servo<0>::packages(), IBus::Servo<0>::packages, 
+                   CRSF::Servo<0>::packages(), SumDV3::Servo<0>::packages());        
+    return 0;
+}
+#endif
+
 const CliCommand cliCommands[] = {
   { "beep", cliBeep, "[<frequency>] [<duration>]" },
   { "ls", cliLs, "<directory>" },
@@ -1594,6 +1608,9 @@ const CliCommand cliCommands[] = {
 #if defined(ACCESS_DENIED) && defined(DEBUG_CRYPT)
   { "crypt", cliCrypt, "<string to be encrypted>" },
 #endif
+#if defined(EXTENDED_TRAINER)
+  { "tr_stats", trainer_stats, nullptr},
+#endif    
   { nullptr, nullptr, nullptr }  /* sentinel */
 };
 

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,

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

radio/src/crsf.cpp

@@ -0,0 +1,25 @@
+#include "crsf.h"
+
+void crsfTrainerPauseCheck() {
+#if !defined(SIMU)
+# if defined(AUX_SERIAL) || defined(AUX2_SERIAL)
+    if (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) {
+        CRSF::Servo<0>::tick1ms();
+        processCrsfInput();    
+    }
+# endif
+#endif    
+}
+
+void processCrsfInput() {
+#if !defined(SIMU)
+  uint8_t rxchar;
+
+  while (sbusAuxGetByte(&rxchar)) {
+      CRSF::Servo<0>::process(rxchar, [&](){
+          CRSF::Servo<0>::convert(ppmInput);
+          ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;        
+      });
+  }
+#endif
+}

radio/src/crsf.h

@@ -0,0 +1,163 @@
+#pragma once
+
+#include "opentx.h"
+#include "trainer.h"
+#include "crc.h"
+
+#include <algorithm>
+#include <limits>
+
+void processCrsfInput();
+void crsfTrainerPauseCheck();
+
+#ifdef __GNUC__
+# pragma GCC diagnostic push
+# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch
+#endif
+
+namespace CRSF {
+    static constexpr uint32_t baudrate{420000};
+
+    template<uint8_t Instance>
+    struct Servo {
+        using Crsf = Trainer::Protocol::Crsf;
+        using MesgType = Crsf::MesgType;
+
+        static constexpr uint8_t mPauseCount{2}; // 2ms
+
+        static constexpr uint8_t CRSFChannels{16};
+
+        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;
+            ++mBytesCounter;
+            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;
+                mData[mIndex] = b;
+                if (++mIndex >= mLength) {
+                    mState = State::AwaitCRCAndDecode;
+                }
+                break;
+            case State::AwaitCRC:
+                if (csum == b) {
+                    // only channel data is decoded, nothing todo here
+                } 
+                mState = State::Undefined;
+                break;
+            case State::AwaitCRCAndDecode:
+                if (csum == b) {
+                    ++mPackagesCounter;
+                    f();
+                } 
+                mState = State::Undefined;
+                break;
+            }            
+        }        
+        template<uint8_t N>
+        static inline void convert(int16_t (&pulses)[N]) {
+            static_assert(N >= 16, "array too small");
+            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 < N; ++i) {
+                pulses[i] = convertCrsfToPuls(pulses[i]);
+            }
+        }
+        static inline uint16_t packages() {
+            return mPackagesCounter;
+        }
+        static inline uint16_t getBytes() {
+            return mBytesCounter;
+        }
+    private:
+        static CRC8 csum;
+        static State mState;
+        static MesgType mData; 
+        static uint8_t mIndex;
+        static uint8_t mLength;
+        static uint16_t mPackagesCounter;
+        static uint16_t mBytesCounter;
+        static uint8_t mPauseCounter;
+    };
+    // inline static member definitions not until c++17
+    template<uint8_t Instance>
+    CRC8 Servo<Instance>::csum;
+    template<uint8_t Instance>
+    typename Servo<Instance>::State Servo<Instance>::mState{Servo::State::Undefined};
+    template<uint8_t Instance>
+    typename Servo<Instance>::MesgType Servo<Instance>::mData; 
+    template<uint8_t Instance>
+    uint8_t Servo<Instance>::mIndex{0};
+    template<uint8_t Instance>
+    uint8_t Servo<Instance>::mLength{0};
+    template<uint8_t Instance>
+    uint16_t Servo<Instance>::mPackagesCounter{0};
+    template<uint8_t Instance>
+    uint16_t Servo<Instance>::mBytesCounter{0};
+    template<uint8_t Instance>
+    uint8_t Servo<Instance>::mPauseCounter{Servo::mPauseCount}; // 2 ms
+}
+#ifdef __GNUC__
+# pragma GCC diagnostic pop
+#endif

radio/src/dataconstants.h

@@ -49,7 +49,11 @@
   #define MAX_INPUTS                   32
   #define MIN_TRAINER_CHANNELS         4
   #define DEF_TRAINER_CHANNELS         8
+#if defined(EXTENDED_TRAINER)
+  #define MAX_TRAINER_CHANNELS         32
+#else
   #define MAX_TRAINER_CHANNELS         16
+#endif
   #define MAX_TELEMETRY_SENSORS        60
   #define MAX_CUSTOM_SCREENS           5
 #elif defined(PCBX9D) || defined(PCBX9DP) || defined(PCBX9E)
@@ -64,7 +68,11 @@
   #define MAX_INPUTS                   32
   #define MIN_TRAINER_CHANNELS         4
   #define DEF_TRAINER_CHANNELS         8
+#if defined(EXTENDED_TRAINER)
+  #define MAX_TRAINER_CHANNELS         32
+#else
   #define MAX_TRAINER_CHANNELS         16
+#endif
   #define MAX_TELEMETRY_SENSORS        60
 #elif defined(PCBTARANIS)
   #define MAX_MODELS                   60
@@ -230,6 +238,9 @@ enum UartModes {
   UART_MODE_TELEMETRY_MIRROR,
   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,

radio/src/gui/gui_common.cpp

@@ -380,7 +380,23 @@ int hasSerialMode(int mode)
   return -1;
 }
 
-bool isSerialModeAvailable(uint8_t port_nr, int mode)
+bool isTrainerInputMode(const int mode) 
+{
+    return (mode == UART_MODE_IBUS_TRAINER) || (mode == UART_MODE_SBUS_TRAINER) || (mode == UART_MODE_CRSF_TRAINER) || (mode == UART_MODE_SUMD_TRAINER);
+}
+
+int trainerInputActive() 
+{
+    for(int p = 0; p < MAX_SERIAL_PORTS; ++p) {
+        const int mode = serialGetMode(p);
+        if (isTrainerInputMode(mode)) {
+            return p;
+        }   
+    }
+    return -1;
+}
+
+bool isSerialModeAvailable(const uint8_t port_nr, const int mode)
 {
   if (mode == UART_MODE_NONE)
     return true;
@@ -412,12 +428,23 @@ bool isSerialModeAvailable(uint8_t port_nr, 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_TELEMETRY || mode == UART_MODE_SBUS_TRAINER || mode == UART_MODE_IBUS_TRAINER || mode == UART_MODE_CRSF_TRAINER || mode == UART_MODE_SUMD_TRAINER))
     return false;
 #endif
 
-  auto p = hasSerialMode(mode);
-  if (p >= 0 && p != port_nr) return false;
+  {
+      const int p = hasSerialMode(mode);
+      if ((p >= 0) && (p != port_nr)) {
+          return false;
+      }
+  }
+  {
+      const int p = trainerInputActive(); 
+      if (isTrainerInputMode(mode) && (p >= 0) && (p != port_nr)) {
+          return false;
+      }
+  }
+
   return true;
 }
 
@@ -830,7 +857,8 @@ bool isTrainerModeAvailable(int mode)
 
   if (mode == TRAINER_MODE_MASTER_SERIAL) {
 #if defined(SBUS_TRAINER)
-    return hasSerialMode(UART_MODE_SBUS_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

radio/src/hal/serial_driver.h

@@ -73,7 +73,7 @@ typedef struct {
   void (*waitForTxCompleted)(void* ctx);
 
   // Fetch byte from internal buffer
-  int (*getByte)(void* ctx, uint8_t* data);
+  bool (*getByte)(void* ctx, uint8_t* data);
 
   // Get current baudrate
   uint32_t (*getBaudrate)(void*);