Enable serial trainer for foreign protocols (SBUS improvements / CRSF / 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(FWDRIVE "Attach also firmware drive with USB" OFF)
+option(EXTENDED_TRAINER "Enable 32 trainer channels for full SumDV3 support" OFF)
 
 if(PCB STREQUAL X9D+ AND PCBREV STREQUAL 2019)
   option(USBJ_EX "Enable USB Joystick Extension" OFF)
@@ -368,6 +369,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)
@@ -388,6 +393,7 @@ set(SRC
   strhelpers.cpp
   switches.cpp
   analogs.cpp
+  mixes.cpp
   mixer.cpp
   mixer_scheduler.cpp
   stamp.cpp
@@ -396,6 +402,9 @@ set(SRC
   model_init.cpp
   serial.cpp
   sbus.cpp
+  ibus.cpp
+  crsf.cpp
+  sumd.cpp
   input_mapping.cpp
   inactivity_timer.cpp
   tasks/mixer_task.cpp

radio/src/boards/generic_stm32/module_ports.cpp

@@ -29,7 +29,7 @@
 
 #if defined(INTMODULE_USART)
 
-#define INTMODULE_USART_IRQ_PRIORITY 6
+#define INTMODULE_USART_IRQ_PRIORITY 5
 
 static const stm32_usart_t intmoduleUSART = {
   .USARTx = INTMODULE_USART,
@@ -222,7 +222,7 @@ extern "C" void EXTMODULE_TIMER_IRQHandler()
 DEFINE_STM32_SOFTSERIAL_PORT(ExternalModule, extmoduleTimer);
 #endif
 
-#define TELEMETRY_USART_IRQ_PRIORITY 5
+#define TELEMETRY_USART_IRQ_PRIORITY 0
 #define TELEMETRY_DMA_IRQ_PRIORITY   0
 
 static void _set_sport_input(uint8_t enable)

radio/src/cli.cpp

@@ -45,6 +45,11 @@
 #include <new>
 #include <stdarg.h>
 
+#include "sbus.h"
+#include "ibus.h"
+#include "crsf.h"
+#include "sumd.h"
+
 
 #define CLI_COMMAND_MAX_ARGS           8
 #define CLI_COMMAND_MAX_LEN            256
@@ -1617,6 +1622,15 @@ int cliResetGT911(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>" },
@@ -1658,6 +1672,9 @@ const CliCommand cliCommands[] = {
 #if defined(HARDWARE_TOUCH) && !defined(PCBNV14)
   { "reset_gt911", cliResetGT911, ""},
 #endif
+#if defined(EXTENDED_TRAINER)
+  { "tr_stats", trainer_stats, nullptr},
+#endif    
   { nullptr, nullptr, nullptr }  /* sentinel */
 };
 

radio/src/crc.cpp

@@ -152,6 +152,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

@@ -35,4 +35,12 @@ uint8_t crc8(const uint8_t * ptr, uint32_t len);
 uint8_t crc8_BA(const uint8_t * ptr, uint32_t len);
 uint16_t crc16(uint8_t index, const uint8_t * buf, uint32_t len, uint16_t start = 0);
 
+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,27 @@
+#include "crsf.h"
+
+static CRSF::Servo crsfTrainer;
+
+void crsfTrainerPauseCheck() {
+#if !defined(SIMU)
+# if defined(AUX_SERIAL) || defined(AUX2_SERIAL)
+    if (hasSerialMode(UART_MODE_CRSF_TRAINER) >= 0) {
+        crsfTrainer.tick1ms();
+        processCrsfInput();    
+    }
+# endif
+#endif    
+}
+
+void processCrsfInput() {
+#if !defined(SIMU)
+  uint8_t rxchar;
+
+  while (sbusAuxGetByte(&rxchar)) {
+      crsfTrainer.process(rxchar, [&](){
+          crsfTrainer.convert(trainerInput);
+          trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT;
+      });
+  }
+#endif
+}

radio/src/crsf.h

@@ -0,0 +1,143 @@
+#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};
+    void crsfTrainerOnIdle();
+    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);
+        }
+
+        inline void tick1ms() {
+            if (mPauseCounter > 0) {
+                --mPauseCounter;
+            }
+            else {
+                mState = State::Undefined;
+            }
+        }
+
+        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;
+            }            
+        }        
+        inline void convert(int16_t pulses[]) {
+            pulses[0]  = (uint16_t) ((mData[0]     | (uint16_t)mData[1] <<8)                           & Crsf::ValueMask);
+            pulses[1]  = (uint16_t) ((mData[1] >>3 | (uint16_t)mData[2] <<5)                           & Crsf::ValueMask);
+            pulses[2]  = (uint16_t) ((mData[2] >>6 | (uint16_t)mData[3] <<2 | (uint16_t)mData[4]<<10)  & Crsf::ValueMask);
+            pulses[3]  = (uint16_t) ((mData[4] >>1 | (uint16_t)mData[5] <<7)                           & Crsf::ValueMask);
+            pulses[4]  = (uint16_t) ((mData[5] >>4 | (uint16_t)mData[6] <<4)                           & Crsf::ValueMask);
+            pulses[5]  = (uint16_t) ((mData[6] >>7 | (uint16_t)mData[7] <<1 | (uint16_t)mData[8]<<9)   & Crsf::ValueMask);
+            pulses[6]  = (uint16_t) ((mData[8] >>2 | (uint16_t)mData[9] <<6)                           & Crsf::ValueMask);
+            pulses[7]  = (uint16_t) ((mData[9] >>5 | (uint16_t)mData[10]<<3)                           & Crsf::ValueMask);
+            pulses[8]  = (uint16_t) ((mData[11]    | (uint16_t)mData[12]<<8)                           & Crsf::ValueMask);
+            pulses[9]  = (uint16_t) ((mData[12]>>3 | (uint16_t)mData[13]<<5)                           & Crsf::ValueMask);
+            pulses[10] = (uint16_t) ((mData[13]>>6 | (uint16_t)mData[14]<<2 | (uint16_t)mData[15]<<10) & Crsf::ValueMask);
+            pulses[11] = (uint16_t) ((mData[15]>>1 | (uint16_t)mData[16]<<7)                           & Crsf::ValueMask);
+            pulses[12] = (uint16_t) ((mData[16]>>4 | (uint16_t)mData[17]<<4)                           & Crsf::ValueMask);
+            pulses[13] = (uint16_t) ((mData[17]>>7 | (uint16_t)mData[18]<<1 | (uint16_t)mData[19]<<9)  & Crsf::ValueMask);
+            pulses[14] = (uint16_t) ((mData[19]>>2 | (uint16_t)mData[20]<<6)                           & Crsf::ValueMask);
+            pulses[15] = (uint16_t) ((mData[20]>>5 | (uint16_t)mData[21]<<3)                           & Crsf::ValueMask);
+
+            for(size_t i = 0; i < MAX_TRAINER_CHANNELS; ++i) {
+                pulses[i] = convertCrsfToPuls(pulses[i]);
+            }
+        }
+        inline uint16_t packages() {
+            return mPackagesCounter;
+        }
+        inline uint16_t getBytes() {
+            return mBytesCounter;
+        }
+    private:
+        CRC8 csum;
+        State mState = State::Undefined;
+        MesgType mData;
+        uint8_t mIndex = 0;
+        uint8_t mLength = 0;
+        uint16_t mPackagesCounter = 0;
+        uint16_t mBytesCounter = 0;
+        uint8_t mPauseCounter = mPauseCount;
+    };
+}
+#ifdef __GNUC__
+# pragma GCC diagnostic pop
+#endif

radio/src/dataconstants.h

@@ -51,7 +51,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           10
 #elif defined(PCBX9D) || defined(PCBX9DP) || defined(PCBX9E)
@@ -66,7 +70,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
@@ -237,6 +245,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/128x64/gui.h

@@ -158,9 +158,6 @@ extern uint8_t editNameCursorPos;
 uint8_t getExposCount();
 void insertExpo(uint8_t idx);
 void deleteExpo(uint8_t idx);
-uint8_t getMixesCount();
-void insertMix(uint8_t idx);
-void deleteMix(uint8_t idx);
 
 void onSwitchLongEnterPress(const char *result);
 void onSourceLongEnterPress(const char *result);

radio/src/gui/128x64/model_mix_edit.cpp

@@ -20,6 +20,7 @@
  */
 
 #include "opentx.h"
+#include "mixes.h"
 
 enum MixFields {
   MIX_FIELD_NAME,

radio/src/gui/212x64/gui.h

@@ -157,10 +157,6 @@ uint8_t getExposCount();
 void deleteExpo(uint8_t idx);
 void insertExpo(uint8_t idx);
 
-uint8_t getMixesCount();
-void deleteMix(uint8_t idx);
-void insertMix(uint8_t idx);
-
 #define STATUS_LINE_LENGTH             32
 extern char statusLineMsg[STATUS_LINE_LENGTH];
 void showStatusLine();

radio/src/gui/212x64/model_mix_edit.cpp

@@ -20,6 +20,7 @@
  */
 
 #include "opentx.h"
+#include "mixes.h"
 
 enum MixFields {
   MIX_FIELD_NAME,

radio/src/gui/colorlcd/menus.h

@@ -101,11 +101,6 @@ uint8_t getExposCount();
 void deleteExpo(uint8_t idx);
 void insertExpo(uint8_t idx, uint8_t input);
 
-uint8_t getMixesCount();
-void deleteMix(uint8_t idx);
-void insertMix(uint8_t idx);
-void copyMix(uint8_t source, uint8_t dest, int8_t ch);
-
 typedef int (*FnFuncP) (int x);
 void drawFunction(FnFuncP fn, int x, int y, int width);
 

radio/src/gui/colorlcd/mixer_edit.cpp

@@ -24,6 +24,7 @@
 #include "channel_bar.h"
 #include "gvar_numberedit.h"
 #include "curve_param.h"
+#include "mixes.h"
 
 #include "opentx.h"
 

radio/src/gui/colorlcd/mixer_edit_adv.cpp

@@ -22,6 +22,7 @@
 #include "mixer_edit_adv.h"
 #include "numberedit.h"
 #include "fm_matrix.h"
+#include "mixes.h"
 
 #include "opentx.h"