diff --git a/radio/src/crsf_trainer.cpp b/radio/src/crsf_trainer.cpp new file mode 100644 index 00000000000..c34b1810458 --- /dev/null +++ b/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 + +#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); +} diff --git a/radio/src/crsf_trainer.h b/radio/src/crsf_trainer.h new file mode 100644 index 00000000000..7c7fb6595c0 --- /dev/null +++ b/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); diff --git a/radio/src/dataconstants.h b/radio/src/dataconstants.h index 9b6681a9223..1297a958174 100644 --- a/radio/src/dataconstants.h +++ b/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 }; diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index 5f716d537f2..82519ae54a8 100644 --- a/radio/src/gui/gui_common.cpp +++ b/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)))) diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 1f09287cdbc..2aa2c019aed 100644 --- a/radio/src/serial.cpp +++ b/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, diff --git a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp index 4862d99cc39..5cf1b78cb34 100644 --- a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp +++ b/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 } }; diff --git a/radio/src/targets/common/arm/CMakeLists.txt b/radio/src/targets/common/arm/CMakeLists.txt index 2bb691fd4dd..f819edd7670 100644 --- a/radio/src/targets/common/arm/CMakeLists.txt +++ b/radio/src/targets/common/arm/CMakeLists.txt @@ -254,6 +254,7 @@ if(CROSSFIRE) set(SRC ${SRC} telemetry/crossfire.cpp + crsf_trainer.cpp ) endif() diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 5b7508a59a7..b20ae5399cd 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/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; diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index bdfc4956180..1a64a4b5522 100644 --- a/radio/src/telemetry/crossfire.h +++ b/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[] = { diff --git a/radio/src/tests/crsf_trainer.cpp b/radio/src/tests/crsf_trainer.cpp new file mode 100644 index 00000000000..47f6c4d9fa5 --- /dev/null +++ b/radio/src/tests/crsf_trainer.cpp @@ -0,0 +1,347 @@ +/* + * 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. + */ + +#if defined(CROSSFIRE) + +#include + +#include "crc.h" +#include "crsf_trainer.h" +#include "gtests.h" +#include "gui/gui_common.h" +#include "serial.h" +#include "telemetry/crossfire.h" +#include "trainer.h" + +// Tests for the CRSF trainer frame assembler used on USB-VCP. CDC delivers +// arbitrarily chunked buffers with no idle line, so frames have to be +// recovered from the byte stream itself. + +namespace { + +constexpr uint8_t CRSF_CH_BITS = 11; +constexpr uint16_t CRSF_CH_CENTER = 0x3E0; + +// Distinct value per channel, so channel ordering is checked too +uint16_t testChannelValue(int ch) { return 200 + ch * 100; } + +int16_t expectedPulse(uint16_t raw) +{ + return ((int32_t)raw - CRSF_CH_CENTER) * 5 / 8; +} + +// Build an RC channels packed frame: [addr][len][type][22 bytes][crc] +std::vector buildChannelsFrame(uint8_t addr = UART_SYNC) +{ + std::vector payload; + uint32_t bits = 0; + uint8_t bitsAvailable = 0; + for (int ch = 0; ch < 16; ch++) { + bits |= (uint32_t)(testChannelValue(ch) & 0x7FF) << bitsAvailable; + bitsAvailable += CRSF_CH_BITS; + while (bitsAvailable >= 8) { + payload.push_back(bits & 0xFF); + bits >>= 8; + bitsAvailable -= 8; + } + } + + std::vector frame; + frame.push_back(addr); + frame.push_back((uint8_t)(payload.size() + 2)); // type + payload + crc + frame.push_back(CHANNELS_ID); + frame.insert(frame.end(), payload.begin(), payload.end()); + frame.push_back(crc8(frame.data() + 2, frame.size() - 2)); + return frame; +} + +// A well-formed frame of some other type, which must be consumed but ignored +std::vector buildOtherFrame() +{ + std::vector frame = {UART_SYNC, 4, LINK_ID, 0x11, 0x22}; + frame.push_back(crc8(frame.data() + 2, frame.size() - 2)); + return frame; +} + +void append(std::vector& dst, const std::vector& src) +{ + dst.insert(dst.end(), src.begin(), src.end()); +} + +void feed(std::vector stream, size_t chunkSize = 0) +{ + if (chunkSize == 0) chunkSize = stream.size(); + size_t offset = 0; + while (offset < stream.size()) { + size_t n = std::min(chunkSize, stream.size() - offset); + crsfTrainerReceiveData(stream.data() + offset, n); + offset += n; + } +} + +void expectTestChannels() +{ + for (int ch = 0; ch < 16; ch++) { + EXPECT_EQ(trainerInput[ch], expectedPulse(testChannelValue(ch))) + << "channel " << ch; + } +} + +class CrsfTrainerTest : public ::testing::Test +{ + protected: + void SetUp() override + { + crsfTrainerStop(); // clears any partial frame from a previous test + memset(&g_model, 0, sizeof(g_model)); + g_model.trainerData.mode = TRAINER_MODE_CRSF; + memset(trainerInput, 0, sizeof(trainerInput)); + trainerSetTimer(0); + } + + void TearDown() override { g_model.trainerData.mode = TRAINER_MODE_OFF; } +}; + +} // namespace + +TEST_F(CrsfTrainerTest, singleFrame) +{ + feed(buildChannelsFrame()); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// Frames get split across USB packets at arbitrary points. +TEST_F(CrsfTrainerTest, splitAcrossChunkBoundaries) +{ + auto frame = buildChannelsFrame(); + for (size_t chunkSize = 1; chunkSize <= frame.size(); chunkSize++) { + crsfTrainerStop(); + memset(trainerInput, 0, sizeof(trainerInput)); + + feed(frame, chunkSize); + + for (int ch = 0; ch < 16; ch++) { + EXPECT_EQ(trainerInput[ch], expectedPulse(testChannelValue(ch))) + << "chunk size " << chunkSize << ", channel " << ch; + } + } +} + +TEST_F(CrsfTrainerTest, backToBackFramesInOneChunk) +{ + std::vector stream; + append(stream, buildChannelsFrame()); + append(stream, buildChannelsFrame()); + append(stream, buildChannelsFrame()); + + feed(stream); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +TEST_F(CrsfTrainerTest, garbagePrefixIsSkipped) +{ + std::vector stream = {0x11, 0x22, 0x33, 0x44}; + append(stream, buildChannelsFrame()); + + feed(stream); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// The whole point of CRSF over SBUS: a corrupted frame is detected, not +// silently applied to the trainer channels. +TEST_F(CrsfTrainerTest, badCrcIsRejected) +{ + auto frame = buildChannelsFrame(); + frame[frame.size() - 1] ^= 0xFF; // corrupt the CRC + + feed(frame); + + EXPECT_FALSE(isTrainerValid()) << "frame with a bad CRC was accepted"; + for (int ch = 0; ch < 16; ch++) EXPECT_EQ(trainerInput[ch], 0); +} + +// Corruption of the payload is caught by the CRC as well. +TEST_F(CrsfTrainerTest, corruptedPayloadIsRejected) +{ + auto frame = buildChannelsFrame(); + frame[5] ^= 0xFF; + + feed(frame); + + EXPECT_FALSE(isTrainerValid()); +} + +// A corrupted frame must be dropped whole, leaving the stream aligned so the +// very next frame is decoded. +// +// Note the test channel values put 0xC8 (UART_SYNC) inside the payload -- ch0 +// is 200 -- so a byte-wise re-sync here would latch onto that payload byte, +// read a bogus length from the next one, and swallow the following good frame +// while waiting for it to complete. +TEST_F(CrsfTrainerTest, corruptedFrameIsDroppedAndStreamStaysAligned) +{ + auto bad = buildChannelsFrame(); + bad[bad.size() - 1] ^= 0xFF; + + feed(bad); + ASSERT_FALSE(isTrainerValid()); + + feed(buildChannelsFrame()); + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// Truncated frame followed by good ones, with awkward chunking on top. +TEST_F(CrsfTrainerTest, resyncAfterTruncatedFrame) +{ + auto frame = buildChannelsFrame(); + + std::vector stream; + append(stream, frame); + stream.insert(stream.end(), frame.begin(), frame.begin() + 12); + for (uint8_t b : {0x55, 0xAA, 0x37}) stream.push_back(b); + append(stream, frame); + append(stream, frame); + + feed(stream, 7); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// A bogus length byte must not wedge the assembler. +TEST_F(CrsfTrainerTest, insaneLengthIsRejected) +{ + std::vector stream = {UART_SYNC, 0x00, 0x00}; + append(stream, buildChannelsFrame()); + + feed(stream); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// Non-channel frames share the stream; they must be consumed without +// disturbing alignment, and without touching the trainer channels. +TEST_F(CrsfTrainerTest, otherFrameTypesAreIgnoredButConsumed) +{ + feed(buildOtherFrame()); + EXPECT_FALSE(isTrainerValid()); + + std::vector stream; + append(stream, buildOtherFrame()); + append(stream, buildChannelsFrame()); + append(stream, buildOtherFrame()); + + feed(stream); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// Both address bytes are accepted as a frame start. +TEST_F(CrsfTrainerTest, acceptsBothAddressBytes) +{ + feed(buildChannelsFrame(RADIO_ADDRESS)); + EXPECT_TRUE(isTrainerValid()); + expectTestChannels(); +} + +// Nothing may reach the trainer channels unless the model asked for CRSF +// trainer input. +TEST_F(CrsfTrainerTest, ignoredWhenTrainerModeIsNotCrsf) +{ + g_model.trainerData.mode = TRAINER_MODE_OFF; + + feed(buildChannelsFrame()); + + EXPECT_FALSE(isTrainerValid()); + for (int ch = 0; ch < 16; ch++) EXPECT_EQ(trainerInput[ch], 0) << "channel " << ch; +} + +// Link loss: frames stop, trainer input goes stale, sticks take over. +TEST_F(CrsfTrainerTest, inputGoesStaleWhenFramesStop) +{ + feed(buildChannelsFrame()); + ASSERT_TRUE(isTrainerValid()); + + for (int i = 0; i < 100; i++) { + EXPECT_TRUE(isTrainerValid()) << "went stale early, at tick " << i; + trainerDecTimer(); + } + + EXPECT_FALSE(isTrainerValid()); +} + +// A partial frame left over when the port is released must not be completed +// by whatever the next user of the port sends. +TEST_F(CrsfTrainerTest, partialFrameIsDroppedOnStop) +{ + auto frame = buildChannelsFrame(); + std::vector partial(frame.begin(), frame.begin() + 20); + feed(partial); + + crsfTrainerStop(); + + std::vector rest(frame.begin() + 20, frame.end()); + feed(rest); + + EXPECT_FALSE(isTrainerValid()); +} + +#if defined(USB_SERIAL) +// The option has to be offered on USB-VCP, and only there: the mode is driven +// from a receive callback, which the STM32 USART driver does not provide. +TEST(CrsfTrainerMenu, crsfTrainerIsOfferedOnVcpOnly) +{ + EXPECT_TRUE(isSerialModeAvailable(SP_VCP, UART_MODE_CRSF_TRAINER)); + EXPECT_FALSE(isSerialModeAvailable(SP_AUX1, UART_MODE_CRSF_TRAINER)); +} + +// Selecting the serial mode has to make TRAINER_MODE_CRSF selectable too. +// Without this, the port can be configured but the trainer mode that consumes +// it stays hidden unless an ELRS module happens to be enabled -- which is not +// the case when the frames come from a PC over USB. +TEST(CrsfTrainerMenu, crsfTrainerModeAvailableWhenPortIsConfigured) +{ + auto restore = g_eeGeneral.serialPort; + + serialSetMode(SP_VCP, UART_MODE_NONE); + bool withoutPort = isTrainerModeAvailable(TRAINER_MODE_CRSF); + + serialSetMode(SP_VCP, UART_MODE_CRSF_TRAINER); + EXPECT_TRUE(isTrainerModeAvailable(TRAINER_MODE_CRSF)) + << "CRSF trainer mode hidden even though USB-VCP is set to CRSF Trainer"; + + // Sanity: the port is what made the difference here, not something else + EXPECT_FALSE(withoutPort); + + g_eeGeneral.serialPort = restore; +} +#endif + +#endif // CROSSFIRE diff --git a/radio/src/translations/i18n/cn.h b/radio/src/translations/i18n/cn.h index 483fa0ce4a3..70944a98ac9 100644 --- a/radio/src/translations/i18n/cn.h +++ b/radio/src/translations/i18n/cn.h @@ -144,6 +144,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "外置发射" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "禁用" #define TR_SWTYPES_2 "回弹" #define TR_SWTYPES_3 "2段" diff --git a/radio/src/translations/i18n/cz.h b/radio/src/translations/i18n/cz.h index 3a25c3fc068..c57b301ffe8 100644 --- a/radio/src/translations/i18n/cz.h +++ b/radio/src/translations/i18n/cz.h @@ -150,6 +150,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Externí modul" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Žádný" #define TR_SWTYPES_2 "Bez aretace" #define TR_SWTYPES_3 "2-polohový" diff --git a/radio/src/translations/i18n/da.h b/radio/src/translations/i18n/da.h index 0c54b1408bd..b5acd731b8d 100644 --- a/radio/src/translations/i18n/da.h +++ b/radio/src/translations/i18n/da.h @@ -144,6 +144,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Eksternt modul" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Ingen" #define TR_SWTYPES_2 "2 pos skift" diff --git a/radio/src/translations/i18n/de.h b/radio/src/translations/i18n/de.h index bca8b70f9a7..91ad4beb357 100644 --- a/radio/src/translations/i18n/de.h +++ b/radio/src/translations/i18n/de.h @@ -147,6 +147,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Externes Modul" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Kein" #define TR_SWTYPES_2 "Taster" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/en.h b/radio/src/translations/i18n/en.h index f7b47e1f439..62642b9c3ee 100644 --- a/radio/src/translations/i18n/en.h +++ b/radio/src/translations/i18n/en.h @@ -144,6 +144,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "External module" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "None" #define TR_SWTYPES_2 "Toggle" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/es.h b/radio/src/translations/i18n/es.h index 97354276c6b..c64b33f450a 100644 --- a/radio/src/translations/i18n/es.h +++ b/radio/src/translations/i18n/es.h @@ -145,6 +145,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Módulo externo" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Nada" #define TR_SWTYPES_2 "Palanca" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/fi.h b/radio/src/translations/i18n/fi.h index 0ac929af423..fdf7d2eb8fc 100644 --- a/radio/src/translations/i18n/fi.h +++ b/radio/src/translations/i18n/fi.h @@ -145,6 +145,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "External module" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "None" #define TR_SWTYPES_2 "Toggle" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/fr.h b/radio/src/translations/i18n/fr.h index 8438e5c047b..7a34dd5a394 100644 --- a/radio/src/translations/i18n/fr.h +++ b/radio/src/translations/i18n/fr.h @@ -149,6 +149,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Module externe" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Rien" #define TR_SWTYPES_2 "Monostable" diff --git a/radio/src/translations/i18n/he.h b/radio/src/translations/i18n/he.h index 1aa90014f3e..11f34c2605c 100644 --- a/radio/src/translations/i18n/he.h +++ b/radio/src/translations/i18n/he.h @@ -147,6 +147,7 @@ #define TR_AUX_SERIAL_MODES_9 "מיפוי שגיאות" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "מודול חיצוני" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "כלום" #define TR_SWTYPES_2 "להחליף" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/it.h b/radio/src/translations/i18n/it.h index ff20e47bef2..39a56d7bb29 100644 --- a/radio/src/translations/i18n/it.h +++ b/radio/src/translations/i18n/it.h @@ -144,6 +144,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Modulo esterno" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Disab." #define TR_SWTYPES_2 "Toggle" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/jp.h b/radio/src/translations/i18n/jp.h index ebf7c5cd1b0..488a61b3ddf 100644 --- a/radio/src/translations/i18n/jp.h +++ b/radio/src/translations/i18n/jp.h @@ -144,6 +144,7 @@ #define TR_AUX_SERIAL_MODES_9 "デバッグ" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "外部モジュール" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "なし" #define TR_SWTYPES_2 "トグル" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/ko.h b/radio/src/translations/i18n/ko.h index 8165f851b4e..f27eaa1df5a 100644 --- a/radio/src/translations/i18n/ko.h +++ b/radio/src/translations/i18n/ko.h @@ -144,6 +144,7 @@ #define TR_AUX_SERIAL_MODES_9 "디버그" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "외부 모듈" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "없음" #define TR_SWTYPES_2 "토글" #define TR_SWTYPES_3 "2단" diff --git a/radio/src/translations/i18n/nl.h b/radio/src/translations/i18n/nl.h index c710d2b26df..90f3b79c26e 100644 --- a/radio/src/translations/i18n/nl.h +++ b/radio/src/translations/i18n/nl.h @@ -146,6 +146,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "External module" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Geen" #define TR_SWTYPES_2 "Wissel" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/pl.h b/radio/src/translations/i18n/pl.h index c0ac974c9ef..83685d6d3c9 100644 --- a/radio/src/translations/i18n/pl.h +++ b/radio/src/translations/i18n/pl.h @@ -145,6 +145,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Moduł zewnętrzny" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Brak" #define TR_SWTYPES_2 "Chwil." #define TR_SWTYPES_3 "2POZ" diff --git a/radio/src/translations/i18n/pt.h b/radio/src/translations/i18n/pt.h index 8681b69781b..b69f7dcc8f9 100644 --- a/radio/src/translations/i18n/pt.h +++ b/radio/src/translations/i18n/pt.h @@ -145,6 +145,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "External module" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Nenhum" #define TR_SWTYPES_2 "Tátil" #define TR_SWTYPES_3 "2POS" diff --git a/radio/src/translations/i18n/ru.h b/radio/src/translations/i18n/ru.h index 6199d12f9e9..f1a7f07cfd0 100644 --- a/radio/src/translations/i18n/ru.h +++ b/radio/src/translations/i18n/ru.h @@ -147,6 +147,7 @@ #define TR_AUX_SERIAL_MODES_9 "Отлад" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Внеш модуль" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Нет" #define TR_SWTYPES_2 "тумбл" #define TR_SWTYPES_3 "2Х-поз пер" diff --git a/radio/src/translations/i18n/se.h b/radio/src/translations/i18n/se.h index 0fe1bb248be..55e4df5d11b 100644 --- a/radio/src/translations/i18n/se.h +++ b/radio/src/translations/i18n/se.h @@ -151,6 +151,7 @@ #define TR_AUX_SERIAL_MODES_9 "Debug" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Extern modul" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Ingen" #define TR_SWTYPES_2 "2 pos flipp" diff --git a/radio/src/translations/i18n/tw.h b/radio/src/translations/i18n/tw.h index ccb47c84036..f7f0f132240 100644 --- a/radio/src/translations/i18n/tw.h +++ b/radio/src/translations/i18n/tw.h @@ -143,6 +143,7 @@ #define TR_AUX_SERIAL_MODES_9 "調試" #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "外置發射" +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "無" #define TR_SWTYPES_2 "回彈" #define TR_SWTYPES_3 "2段" diff --git a/radio/src/translations/i18n/ua.h b/radio/src/translations/i18n/ua.h index 78e78f1e5c2..3d86687cbde 100644 --- a/radio/src/translations/i18n/ua.h +++ b/radio/src/translations/i18n/ua.h @@ -146,6 +146,7 @@ #define TR_AUX_SERIAL_MODES_9 "Відлаг." #define TR_AUX_SERIAL_MODES_10 "SpaceMouse" #define TR_AUX_SERIAL_MODES_11 "Зовн. модуль" /* for untranslated - use english */ +#define TR_AUX_SERIAL_MODES_12 "CRSF Trainer" #define TR_SWTYPES_1 "Немає" #define TR_SWTYPES_2 "Перемик." #define TR_SWTYPES_3 "2поз. пер." diff --git a/radio/src/translations/translation_def.h b/radio/src/translations/translation_def.h index 43572d82c13..078e17faf17 100644 --- a/radio/src/translations/translation_def.h +++ b/radio/src/translations/translation_def.h @@ -129,7 +129,7 @@ #define TR_VCELLINDEX SA11(TR_VCELLINDEX) -#define TR_AUX_SERIAL_MODES SA11(TR_AUX_SERIAL_MODES) +#define TR_AUX_SERIAL_MODES SA12(TR_AUX_SERIAL_MODES) #define TR_VFORMULAS SA9(TR_VFORMULAS) #define TR_FS_COLOR_LIST SA9(TR_FS_COLOR_LIST) diff --git a/tools/crsf_vcp_test.py b/tools/crsf_vcp_test.py new file mode 100755 index 00000000000..4c8b18363cb --- /dev/null +++ b/tools/crsf_vcp_test.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 +"""Drive an EdgeTX radio's trainer channels with CRSF frames over USB-VCP. + +Requires the radio to be set up as: + + SYS -> Radio Setup -> USB Mode = Serial + SYS -> Hardware -> Serial Port -> USB-VCP = CRSF Trainer + MDL -> Setup -> Trainer Mode = CRSF + +A Trainer special function also has to be active for the channels to reach the +mixer (Function: Trainer, Value: Axis). + +Watch the result in Quick Menu -> Tools -> Channel Monitor, or Model -> Outputs. + +Do this with props off and RF disabled. + +Examples: + ./crsf_vcp_test.py --port /dev/ttyACM0 --sweep + ./crsf_vcp_test.py --port /dev/ttyACM0 --constant --stop-after 200 + ./crsf_vcp_test.py --port /dev/ttyACM0 --sweep --garbage +""" + +import argparse +import glob +import random +import sys +import time + +try: + import serial +except ImportError: + sys.exit("pyserial is required: pip install pyserial") + +UART_SYNC = 0xC8 +CHANNELS_ID = 0x16 + +NUM_CHANNELS = 16 +CH_BITS = 11 + +# Raw 11-bit channel values. EdgeTX decodes these as (raw - 0x3E0) * 5 / 8, +# so these give the trainer input its full -512..+512 range. +CRSF_MIN = 172 +CRSF_CENTER = 992 +CRSF_MAX = 1811 + +# CRC8 with polynomial 0xD5, as used by CRSF +_CRC8_POLY = 0xD5 +_CRC8_TAB = [] +for _i in range(256): + _c = _i + for _ in range(8): + _c = ((_c << 1) ^ _CRC8_POLY) & 0xFF if _c & 0x80 else (_c << 1) & 0xFF + _CRC8_TAB.append(_c) + + +def crc8(data): + crc = 0 + for b in data: + crc = _CRC8_TAB[crc ^ b] + return crc + + +def encode_channels_frame(channels): + """Build a CRSF RC channels packed frame: [addr][len][type][22 bytes][crc].""" + if len(channels) != NUM_CHANNELS: + raise ValueError(f"expected {NUM_CHANNELS} channels, got {len(channels)}") + + payload = bytearray() + bits = 0 + bits_available = 0 + for value in channels: + bits |= (int(value) & 0x7FF) << bits_available + bits_available += CH_BITS + while bits_available >= 8: + payload.append(bits & 0xFF) + bits >>= 8 + bits_available -= 8 + + assert len(payload) == 22, len(payload) + + frame = bytearray([UART_SYNC, len(payload) + 2, CHANNELS_ID]) + frame += payload + frame.append(crc8(frame[2:])) + return bytes(frame) + + +def triangle(phase): + return 2 * phase if phase < 0.5 else 2 * (1.0 - phase) + + +def sweep_channels(elapsed, period): + """Slow triangle on ch1-4, staggered so channel order is visible.""" + channels = [CRSF_CENTER] * NUM_CHANNELS + for ch in range(4): + phase = ((elapsed / period) + ch * 0.25) % 1.0 + channels[ch] = int(CRSF_MIN + (CRSF_MAX - CRSF_MIN) * triangle(phase)) + return channels + + +def autodetect_port(): + candidates = sorted(glob.glob("/dev/ttyACM*") + glob.glob("/dev/ttyUSB*")) + if not candidates: + sys.exit("no /dev/ttyACM* or /dev/ttyUSB* found; pass --port explicitly") + if len(candidates) > 1: + print(f"multiple ports found {candidates}, using {candidates[0]}", file=sys.stderr) + return candidates[0] + + +def main(): + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("--port", help="CDC device (default: autodetect)") + parser.add_argument("--rate", type=float, default=100.0, + help="frames per second (default: 100)") + parser.add_argument("--period", type=float, default=4.0, + help="--sweep period in seconds (default: 4)") + parser.add_argument("--stop-after", type=int, metavar="N", + help="send N frames then stop, holding the port open. " + "The radio must fall back to the sticks within 1s.") + parser.add_argument("--garbage", action="store_true", + help="inject random bytes mid-stream to test re-sync") + parser.add_argument("--garbage-every", type=int, default=50, metavar="N", + help="inject garbage every N frames (default: 50)") + + mode = parser.add_mutually_exclusive_group() + mode.add_argument("--sweep", action="store_true", + help="triangle wave on ch1-4 (default)") + mode.add_argument("--constant", action="store_true", + help="hold all channels centered") + + args = parser.parse_args() + + port = args.port or autodetect_port() + interval = 1.0 / args.rate + + # Baud rate is not carried over USB CDC and is ignored by the radio. + with serial.Serial(port, baudrate=400000, timeout=0) as ser: + print(f"{port}: sending CRSF at {args.rate:g} Hz, ctrl-c to stop") + + start = time.monotonic() + next_send = start + count = 0 + + try: + while True: + now = time.monotonic() + + if args.stop_after is not None and count >= args.stop_after: + print(f"sent {count} frames, stopping. Port stays open; " + f"the radio should fall back to the sticks within 1s.") + while True: + time.sleep(1) + + if args.constant: + channels = [CRSF_CENTER] * NUM_CHANNELS + else: + channels = sweep_channels(now - start, args.period) + + if args.garbage and count and count % args.garbage_every == 0: + noise = bytes(random.randrange(256) for _ in range(random.randrange(1, 12))) + ser.write(noise) + print(f"frame {count}: injected {len(noise)} garbage bytes") + + ser.write(encode_channels_frame(channels)) + count += 1 + + next_send += interval + delay = next_send - time.monotonic() + if delay > 0: + time.sleep(delay) + else: + next_send = time.monotonic() + except KeyboardInterrupt: + print(f"\nstopped after {count} frames") + + +if __name__ == "__main__": + main()