feat(radio): support CRSF Trainer over USB-VCP

radio/src/crsf_trainer.cpp

@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) EdgeTX
+ *
+ * Based on code named
+ *   opentx - https://github.com/opentx/opentx
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "crsf_trainer.h"
+
+#include <string.h>
+
+#include "crc.h"
+#include "edgetx.h"
+#include "telemetry/crossfire.h"
+#include "telemetry/telemetry.h"
+
+//
+// CRSF frame: [addr][len][type][payload...][crc8]
+//
+// len counts type + payload + crc, so the frame occupies len + 2 bytes.
+// The CRC covers type + payload, i.e. bytes [2 .. len].
+//
+// A frame may be split over several USB packets and several frames may arrive
+// in one, so the assembler keeps its partial-frame state across chunks.
+//
+// Invariant: _crsf_len == 0, or _crsf_buf[0] is a valid address byte.
+//
+#define CRSF_MIN_FRAME_LEN 3
+
+static uint8_t _crsf_buf[TELEMETRY_RX_PACKET_SIZE];
+static uint8_t _crsf_len = 0;
+
+static const etx_serial_driver_t* _crsf_drv = nullptr;
+static void* _crsf_ctx = nullptr;
+
+static inline bool _validAddr(uint8_t b)
+{
+  return b == RADIO_ADDRESS || b == UART_SYNC;
+}
+
+static inline bool _lenIsSane(uint32_t len)
+{
+  // at least type + payload + crc, and must fit the buffer
+  return len > 2 && len < TELEMETRY_RX_PACKET_SIZE - 1;
+}
+
+static bool _checkCRC(const uint8_t* frame)
+{
+  uint8_t len = frame[1];
+  return crc8(&frame[2], len - 1) == frame[len + 1];
+}
+
+// Drop the leading byte of a rejected frame and re-sync on the next valid
+// address byte held in what is left. Never discards the whole buffer: a valid
+// frame may well have started inside it.
+static void _resync()
+{
+  uint8_t i = 1;
+  while (i < _crsf_len && !_validAddr(_crsf_buf[i])) i++;
+
+  _crsf_len -= i;
+  if (_crsf_len > 0) {
+    memmove(_crsf_buf, _crsf_buf + i, _crsf_len);
+  }
+}
+
+void crsfTrainerReceiveData(uint8_t* data, uint32_t len)
+{
+  // The model is not asking for CRSF trainer input: stay out of the way.
+  if (g_model.trainerData.mode != TRAINER_MODE_CRSF) {
+    _crsf_len = 0;
+    return;
+  }
+
+  while (len > 0) {
+    // Hunt for an address byte while no frame is being assembled
+    if (_crsf_len == 0 && !_validAddr(*data)) {
+      data++; len--;
+      continue;
+    }
+
+    _crsf_buf[_crsf_len++] = *data++;
+    len--;
+
+    // Need the length byte before the frame size is known
+    if (_crsf_len < 2) continue;
+
+    uint32_t pkt_len = (uint32_t)_crsf_buf[1] + 2;
+    if (!_lenIsSane(pkt_len)) {
+      // Bogus length: this was not a frame start after all
+      _resync();
+      continue;
+    }
+
+    if (_crsf_len < pkt_len) continue;  // incomplete, wait for more bytes
+
+    if (_checkCRC(_crsf_buf) && _crsf_buf[2] == CHANNELS_ID) {
+      // Fills trainerInput[] and resets the trainer validity timer
+      crossfireProcessChannelsFrame(_crsf_buf);
+    }
+
+    // Consume the whole frame either way, as _processFrames() does for the
+    // module path. With a valid address and a sane length, a CRC failure is
+    // far more likely to be corruption inside a real frame than a false lock,
+    // so dropping the frame keeps the stream aligned; byte-wise re-sync would
+    // instead risk latching onto a payload byte that happens to look like an
+    // address. Frame types other than CHANNELS_ID are consumed and ignored.
+    _crsf_len = 0;
+  }
+}
+
+void crsfTrainerStart(void* ctx, const etx_serial_driver_t* drv)
+{
+  if (!drv || !drv->setReceiveCb) return;
+
+  _crsf_len = 0;
+  _crsf_drv = drv;
+  _crsf_ctx = ctx;
+
+  drv->setReceiveCb(ctx, crsfTrainerReceiveData);
+}
+
+void crsfTrainerStop()
+{
+  auto drv = _crsf_drv;
+  auto ctx = _crsf_ctx;
+
+  _crsf_drv = nullptr;
+  _crsf_ctx = nullptr;
+  _crsf_len = 0;
+
+  // Release the RX stream, so the next user of the port gets it
+  if (drv && drv->setReceiveCb) drv->setReceiveCb(ctx, nullptr);
+}

radio/src/crsf_trainer.h

@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) EdgeTX
+ *
+ * Based on code named
+ *   opentx - https://github.com/opentx/opentx
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#pragma once
+
+#include "hal/serial_driver.h"
+
+//
+// CRSF trainer input over a serial port.
+//
+// Unlike the SBUS trainer path, which relies on the USART idle line to mark
+// frame boundaries, CRSF is self-framing: every frame carries a length byte
+// and a CRC. That makes it usable on ports that deliver arbitrarily chunked
+// buffers with no idle-line detection at all, i.e. USB-VCP.
+//
+// Attach / detach the driver's receive callback:
+void crsfTrainerStart(void* ctx, const etx_serial_driver_t* drv);
+void crsfTrainerStop();
+
+// Receive callback: feeds a chunk of the byte stream into the frame assembler
+void crsfTrainerReceiveData(uint8_t* data, uint32_t len);

radio/src/dataconstants.h

@@ -258,6 +258,7 @@ enum UartModes {
   UART_MODE_DEBUG,
   UART_MODE_SPACEMOUSE,
   UART_MODE_EXT_MODULE,
+  UART_MODE_CRSF_TRAINER,
   UART_MODE_COUNT SKIP,
   UART_MODE_MAX SKIP = UART_MODE_COUNT-1
 };

radio/src/gui/gui_common.cpp

@@ -572,6 +572,16 @@ bool isSerialModeAvailable(uint8_t port_nr, int mode)
     return false;
 #endif
 
+  // CRSF trainer input is driven from a receive callback, which only the USB
+  // CDC driver provides today (the STM32 USART driver has no setReceiveCb).
+  if (mode == UART_MODE_CRSF_TRAINER) {
+#if defined(USB_SERIAL) && defined(CROSSFIRE)
+    if (port_nr != SP_VCP) return false;
+#else
+    return false;
+#endif
+  }
+
   auto p = serialGetModePort(mode);
   if (p >= 0 && p != port_nr) return false;
   return true;
@@ -1190,6 +1200,10 @@ bool isTrainerModeAvailable(int mode)
 #if !defined(CROSSFIRE)
     return false;
 #else
+    // CRSF trainer frames can also arrive on a serial port instead of a
+    // module's telemetry stream, in which case no module has to be enabled.
+    if (serialGetModePort(UART_MODE_CRSF_TRAINER) >= 0) return true;
+
     if ((!IS_INTERNAL_MODULE_ENABLED() && !IS_EXTERNAL_MODULE_ENABLED()) ||
          (!(isModuleELRS(INTERNAL_MODULE) && CRSF_ELRS_MIN_VER(INTERNAL_MODULE, 4, 0)) &&
           !(isModuleELRS(EXTERNAL_MODULE) && CRSF_ELRS_MIN_VER(EXTERNAL_MODULE, 4, 0))))

radio/src/serial.cpp

@@ -44,6 +44,9 @@
 
 #if defined(CROSSFIRE)
   #include "telemetry/crossfire.h"
+  #if !defined(BOOT)
+    #include "crsf_trainer.h"
+  #endif
 #endif
 
 #if defined(DEBUG_SEGGER_RTT)
@@ -225,6 +228,20 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por
     }
     break;
 
+#if defined(CROSSFIRE)
+  case UART_MODE_CRSF_TRAINER:
+    // CRSF is self-framing, so it needs no idle-line detection: a receive
+    // callback is enough. On de-init ctx (and hence drv) is null, and the
+    // callback has to be released so the next user of the port gets a clean
+    // stream.
+    if (drv && drv->setReceiveCb) {
+      crsfTrainerStart(ctx, drv);
+    } else {
+      crsfTrainerStop();
+    }
+    break;
+#endif
+
   case UART_MODE_TELEMETRY:
     // telemetrySetGetByte(ctx, getByte);
 
@@ -318,6 +335,16 @@ static void serialSetupPort(int mode, etx_serial_init& params)
     params.direction = ETX_Dir_RX;
     break;
 
+#if defined(CROSSFIRE)
+  case UART_MODE_CRSF_TRAINER:
+    // Only offered on USB-VCP, where the baud rate is not carried and
+    // usbSerialInit() ignores these params. It still has to be non-zero:
+    // serialInit() treats a zero baudrate as "nothing to set up".
+    params.baudrate = CROSSFIRE_BAUDRATES[1];
+    params.direction = ETX_Dir_RX;
+    break;
+#endif
+
   case UART_MODE_SBUS_TRAINER_INV:
     params.baudrate = SBUS_BAUDRATE;
     params.encoding = ETX_Encoding_8E2,

radio/src/storage/yaml/yaml_datastructs_funcs.cpp

@@ -2660,6 +2660,7 @@ static const struct YamlIdStr enum_UartModes[] = {
   {  UART_MODE_DEBUG, "DEBUG"  },
   {  UART_MODE_SPACEMOUSE, "SPACEMOUSE"  },
   {  UART_MODE_EXT_MODULE, "EXT_MODULE"  },
+  {  UART_MODE_CRSF_TRAINER, "CRSF_TRAINER"  },
   {  0, NULL  }
 };
 

radio/src/targets/common/arm/CMakeLists.txt

@@ -254,6 +254,7 @@ if(CROSSFIRE)
   set(SRC
     ${SRC}
     telemetry/crossfire.cpp
+    crsf_trainer.cpp
     )
 endif()
 

radio/src/telemetry/crossfire.cpp

@@ -147,6 +147,31 @@ bool getCrossfireTelemetryValue(uint8_t index, int32_t& value,
   return result;
 }
 
+// Decode an RC channels packed frame (CHANNELS_ID) into the trainer inputs.
+// rxBuffer points at the start of the frame: [addr][len][type][payload...]
+//
+// Shared by the module telemetry path and the USB-VCP trainer path, so both
+// produce identical channel values and identical freshness behaviour.
+void crossfireProcessChannelsFrame(const uint8_t* rxBuffer)
+{
+  uint8_t inputbitsavailable = 0;
+  uint32_t inputbits = 0;
+  uint8_t byteIdx = 3;
+  int16_t* pulses = trainerInput;
+
+  for (int i = 0; i < min(CROSSFIRE_CHANNELS_COUNT, MAX_TRAINER_CHANNELS); i++) {
+    while (inputbitsavailable < CROSSFIRE_CH_BITS) {
+      inputbits |= (uint32_t)(rxBuffer[byteIdx++]) << inputbitsavailable;
+      inputbitsavailable += 8;
+    }
+    *pulses++ = ((int32_t)(inputbits & CROSSFIRE_CH_MASK) - CROSSFIRE_CH_CENTER) * 5 / 8;
+    inputbitsavailable -= CROSSFIRE_CH_BITS;
+    inputbits >>= CROSSFIRE_CH_BITS;
+  }
+
+  trainerResetTimer();
+}
+
 void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
                                     uint8_t rxBufferCount)
 {
@@ -327,22 +352,7 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
 
     case CHANNELS_ID:
       if (g_model.trainerData.mode == TRAINER_MODE_CRSF) {
-        uint8_t inputbitsavailable = 0;
-        uint32_t inputbits = 0;
-        uint8_t  byteIdx = 3;
-        int16_t *pulses = trainerInput;
-
-        for (int i = 0; i < min(CROSSFIRE_CHANNELS_COUNT, MAX_TRAINER_CHANNELS); i++) {
-          while (inputbitsavailable < CROSSFIRE_CH_BITS) {
-            inputbits |= (uint32_t)(rxBuffer[byteIdx++]) << inputbitsavailable;
-            inputbitsavailable += 8;
-          }
-          *pulses++ = ((int32_t)(inputbits & CROSSFIRE_CH_MASK) - CROSSFIRE_CH_CENTER) * 5 / 8;
-          inputbitsavailable -= CROSSFIRE_CH_BITS;
-          inputbits >>= CROSSFIRE_CH_BITS;
-        }
-
-        trainerResetTimer();
+        crossfireProcessChannelsFrame(rxBuffer);
       }
       break;
 

radio/src/telemetry/crossfire.h

@@ -121,6 +121,10 @@ extern CrossfireModuleStatus crossfireModuleStatus[2];
 
 void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
                                     uint8_t rxBufferCount);
+
+// Decode a CHANNELS_ID frame into trainerInput[] and reset the trainer
+// validity timer. rxBuffer points at [addr][len][type][payload...].
+void crossfireProcessChannelsFrame(const uint8_t* rxBuffer);
 void crossfireSetDefault(int index, uint16_t id, uint8_t subId);
 
 const uint32_t CROSSFIRE_BAUDRATES[] = {