diff --git a/companion/src/firmwares/edgetx/yaml_generalsettings.cpp b/companion/src/firmwares/edgetx/yaml_generalsettings.cpp index b7b24d23b8b..5110760ecba 100644 --- a/companion/src/firmwares/edgetx/yaml_generalsettings.cpp +++ b/companion/src/firmwares/edgetx/yaml_generalsettings.cpp @@ -94,12 +94,14 @@ const YamlLookupTable uartModeLut = { { GeneralSettings::AUX_SERIAL_TELE_MIRROR, "TELEMETRY_MIRROR" }, { GeneralSettings::AUX_SERIAL_TELE_IN, "TELEMETRY_IN" }, { GeneralSettings::AUX_SERIAL_SBUS_TRAINER, "SBUS_TRAINER" }, + { GeneralSettings::AUX_SERIAL_SBUS_TRAINER_INV, "SBUS_TRAINER_INV" }, { GeneralSettings::AUX_SERIAL_LUA, "LUA" }, { GeneralSettings::AUX_SERIAL_CLI, "CLI" }, { GeneralSettings::AUX_SERIAL_GPS, "GPS" }, { GeneralSettings::AUX_SERIAL_DEBUG, "DEBUG" }, { GeneralSettings::AUX_SERIAL_SPACEMOUSE, "SPACEMOUSE" }, { GeneralSettings::AUX_SERIAL_EXT_MODULE, "EXT_MODULE" }, + { GeneralSettings::AUX_SERIAL_CRSF_TRAINER, "CRSF_TRAINER" }, }; const YamlLookupTable antennaModeLut = { diff --git a/companion/src/firmwares/generalsettings.cpp b/companion/src/firmwares/generalsettings.cpp index 1861118131a..fdbe796a8e0 100644 --- a/companion/src/firmwares/generalsettings.cpp +++ b/companion/src/firmwares/generalsettings.cpp @@ -535,6 +535,10 @@ QString GeneralSettings::serialModeToString(int value) return tr("SpaceMouse"); case AUX_SERIAL_EXT_MODULE: return tr("External module"); + case AUX_SERIAL_SBUS_TRAINER_INV: + return tr("SBUS Trainer Inv."); + case AUX_SERIAL_CRSF_TRAINER: + return tr("CRSF Trainer"); default: return CPN_STR_UNKNOWN_ITEM; } @@ -613,8 +617,18 @@ AbstractStaticItemModel * GeneralSettings::serialModeItemModel() if (i == AUX_SERIAL_EXT_MODULE) { contexts &= ~(AUX2Context | VCPContext); } + else if (i == AUX_SERIAL_SBUS_TRAINER_INV && + Boards::getCapability(getCurrentBoard(), Board::IsF4)) { + // needs the USART to invert RX, which F4 cannot do + contexts = 0; + } + else if (i == AUX_SERIAL_CRSF_TRAINER) { + // driven from a receive callback, which only USB-VCP provides + contexts &= VCPContext; + } else if (i == AUX_SERIAL_TELE_IN || i == AUX_SERIAL_SBUS_TRAINER || + i == AUX_SERIAL_SBUS_TRAINER_INV || i == AUX_SERIAL_GPS || i == AUX_SERIAL_SPACEMOUSE || i == AUX_SERIAL_EXT_MODULE) { diff --git a/companion/src/firmwares/generalsettings.h b/companion/src/firmwares/generalsettings.h index 14dad7e1277..b59c1600e53 100644 --- a/companion/src/firmwares/generalsettings.h +++ b/companion/src/firmwares/generalsettings.h @@ -167,6 +167,8 @@ class GeneralSettings { AUX_SERIAL_DEBUG, AUX_SERIAL_SPACEMOUSE, AUX_SERIAL_EXT_MODULE, + AUX_SERIAL_SBUS_TRAINER_INV, + AUX_SERIAL_CRSF_TRAINER, AUX_SERIAL_COUNT }; diff --git a/companion/src/firmwares/modeldata.cpp b/companion/src/firmwares/modeldata.cpp index 1c76eea97ef..15a068989aa 100644 --- a/companion/src/firmwares/modeldata.cpp +++ b/companion/src/firmwares/modeldata.cpp @@ -1853,10 +1853,17 @@ bool ModelData::isTrainerModeAvailable(const GeneralSettings & generalSettings, moduleData[1].protocol != PULSES_OFF) return false; - if (value == TRAINER_MODE_MASTER_SERIAL && - (generalSettings.serialPort[GeneralSettings::SP_AUX1] != GeneralSettings::AUX_SERIAL_SBUS_TRAINER && - generalSettings.serialPort[GeneralSettings::SP_AUX2] != GeneralSettings::AUX_SERIAL_SBUS_TRAINER)) - return false; + if (value == TRAINER_MODE_MASTER_SERIAL) { + // inverted SBUS relies on the USART inverting RX, which F4 cannot do + auto isSbusTrainer = [&](unsigned int mode) { + return mode == GeneralSettings::AUX_SERIAL_SBUS_TRAINER || + (mode == GeneralSettings::AUX_SERIAL_SBUS_TRAINER_INV && + !Boards::getCapability(board, Board::IsF4)); + }; + if (!isSbusTrainer(generalSettings.serialPort[GeneralSettings::SP_AUX1]) && + !isSbusTrainer(generalSettings.serialPort[GeneralSettings::SP_AUX2])) + return false; + } if ((value == TRAINER_MODE_MASTER_BLUETOOTH || value == TRAINER_MODE_SLAVE_BLUETOOTH) && !IS_TARANIS_X9E(board) && diff --git a/companion/src/tests/serial_mode_yaml_test.cpp b/companion/src/tests/serial_mode_yaml_test.cpp new file mode 100644 index 00000000000..e81ac3f7c77 --- /dev/null +++ b/companion/src/tests/serial_mode_yaml_test.cpp @@ -0,0 +1,82 @@ +/* + * 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. + */ + +// Serial port modes are stored by name. A name Companion does not know +// decodes to OFF, and is then silently lost when the settings are saved, so +// every mode the firmware can write must be known here. + +#include "gtests.h" + +#include + +#include "firmwares/eeprominterface.h" +#include "firmwares/edgetx/edgetxinterface.h" +#include "firmwares/generalsettings.h" +#include "firmwares/modeldata.h" + +namespace { + +void setFirmware(const char* flavour) +{ + Firmware::setCurrentVariant(Firmware::getFirmwareForFlavour(flavour)); + ASSERT_NE(getCurrentFirmware(), nullptr) << flavour << " firmware must be registered"; +} + +} // namespace + +TEST(SerialModeYaml, trainerModesRoundTrip) +{ + setFirmware("tx15"); + + GeneralSettings in; + in.serialPort[GeneralSettings::SP_AUX1] = GeneralSettings::AUX_SERIAL_SBUS_TRAINER_INV; + in.serialPort[GeneralSettings::SP_VCP] = GeneralSettings::AUX_SERIAL_CRSF_TRAINER; + + QByteArray yaml; + ASSERT_TRUE(writeRadioSettingsToYaml(in, yaml)); + EXPECT_TRUE(yaml.contains("SBUS_TRAINER_INV")) << yaml.constData(); + EXPECT_TRUE(yaml.contains("CRSF_TRAINER")) << yaml.constData(); + + GeneralSettings out; + ASSERT_TRUE(loadRadioSettingsFromYaml(out, yaml)); + EXPECT_EQ(out.serialPort[GeneralSettings::SP_AUX1], + (unsigned)GeneralSettings::AUX_SERIAL_SBUS_TRAINER_INV); + EXPECT_EQ(out.serialPort[GeneralSettings::SP_VCP], + (unsigned)GeneralSettings::AUX_SERIAL_CRSF_TRAINER); +} + +// Master/Serial needs an SBUS trainer port. The inverted variant only counts +// where the firmware offers it, i.e. not on F4 (see serialGetSbusTrainerPort()). +TEST(SerialModeYaml, masterSerialAcceptsInvertedSbusOffF4Only) +{ + GeneralSettings gs; + gs.serialPort[GeneralSettings::SP_AUX1] = GeneralSettings::AUX_SERIAL_SBUS_TRAINER_INV; + ModelData model; + + setFirmware("tx15"); // STM32H7 + EXPECT_TRUE(model.isTrainerModeAvailable(gs, getCurrentFirmware(), TRAINER_MODE_MASTER_SERIAL)); + + setFirmware("tx16s"); // STM32F4 + EXPECT_FALSE(model.isTrainerModeAvailable(gs, getCurrentFirmware(), TRAINER_MODE_MASTER_SERIAL)); + + gs.serialPort[GeneralSettings::SP_AUX1] = GeneralSettings::AUX_SERIAL_SBUS_TRAINER; + EXPECT_TRUE(model.isTrainerModeAvailable(gs, getCurrentFirmware(), TRAINER_MODE_MASTER_SERIAL)); +} diff --git a/radio/src/crsf_trainer.cpp b/radio/src/crsf_trainer.cpp new file mode 100644 index 00000000000..8d1a34994df --- /dev/null +++ b/radio/src/crsf_trainer.cpp @@ -0,0 +1,159 @@ +/* + * 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 + +// len byte of a CHANNELS_ID frame: type + 16 x 11-bit channels (22) + crc +#define CRSF_CHANNELS_FRAME_LEN 24 + +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 + + // A truncated CHANNELS_ID frame would be decoded from stale buffer bytes + if (_checkCRC(_crsf_buf) && _crsf_buf[2] == CHANNELS_ID && + _crsf_buf[1] >= CRSF_CHANNELS_FRAME_LEN) { + // 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; + + // Disarm first: no further callback can start once this returns, and the + // next user of the port gets a clean stream + if (drv && drv->setReceiveCb) drv->setReceiveCb(ctx, nullptr); + + _crsf_drv = nullptr; + _crsf_ctx = nullptr; + _crsf_len = 0; +} + +void crsfTrainerReleaseCtx(void* ctx) +{ + if (ctx && ctx == _crsf_ctx) crsfTrainerStop(); +} diff --git a/radio/src/crsf_trainer.h b/radio/src/crsf_trainer.h new file mode 100644 index 00000000000..34c60e5964b --- /dev/null +++ b/radio/src/crsf_trainer.h @@ -0,0 +1,42 @@ +/* + * 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(); + +// Stop if attached to this driver context, before the context goes away +void crsfTrainerReleaseCtx(void* ctx); + +// 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 b88d73c4b8d..a09a9176930 100644 --- a/radio/src/dataconstants.h +++ b/radio/src/dataconstants.h @@ -259,6 +259,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 99c52ce3038..47a37dccfd9 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -606,6 +606,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; @@ -1272,6 +1282,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 e6d77b740f6..c8c11d1aac4 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(GHOST) @@ -227,6 +230,20 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por // trainer mode selects it as input source. See sbusTrainerAcquire(). 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. The callback is normally released before the port + // is de-initialised (see crsfTrainerReleaseCtx()); on de-init ctx (and + // hence drv) is null, so make sure it is gone. + if (drv && drv->setReceiveCb) { + crsfTrainerStart(ctx, drv); + } else { + crsfTrainerStop(); + } + break; +#endif + case UART_MODE_TELEMETRY: // telemetrySetGetByte(ctx, getByte); @@ -326,6 +343,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, @@ -439,6 +466,9 @@ void serialInit(uint8_t port_nr, int mode) #if !defined(BOOT) // Drop the trainer input before the driver context goes away sbusTrainerReleaseCtx(state->usart_ctx); +#if defined(CROSSFIRE) + crsfTrainerReleaseCtx(state->usart_ctx); +#endif #endif auto drv = state->port->uart; if (drv && drv->deinit && state->usart_ctx) { @@ -578,6 +608,9 @@ void serialStop(uint8_t port_nr) #if !defined(BOOT) // Drop the trainer input before the driver context goes away sbusTrainerReleaseCtx(state->usart_ctx); +#if defined(CROSSFIRE) + crsfTrainerReleaseCtx(state->usart_ctx); +#endif #endif auto port = state->port; auto drv = port->uart; diff --git a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp index 9e709a7b543..c3a6223959f 100644 --- a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp @@ -2679,6 +2679,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 564ace9c2c0..fc92feb0cd1 100644 --- a/radio/src/targets/common/arm/CMakeLists.txt +++ b/radio/src/targets/common/arm/CMakeLists.txt @@ -255,6 +255,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 f431a8db87e..3066fc7a542 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -143,6 +143,31 @@ int32_t getCrossfireTelemetryValue(uint8_t index, uint8_t* rxBuffer, uint8_t siz return value; } +// 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) { @@ -319,22 +344,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..b53f2f1567c --- /dev/null +++ b/radio/src/tests/crsf_trainer.cpp @@ -0,0 +1,393 @@ +/* + * 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()); +} + +// A CHANNELS_ID frame too short to hold 16 channels has a valid CRC, but must +// not be decoded: the channel bits would come from stale buffer bytes. +TEST_F(CrsfTrainerTest, truncatedChannelsFrameIsIgnored) +{ + // Leave a good frame behind in the assembler buffer + feed(buildChannelsFrame()); + ASSERT_TRUE(isTrainerValid()); + memset(trainerInput, 0, sizeof(trainerInput)); + trainerSetTimer(0); + + std::vector frame = {UART_SYNC, 4, CHANNELS_ID, 0x11, 0x22}; + frame.push_back(crc8(frame.data() + 2, frame.size() - 2)); + feed(frame); + + EXPECT_FALSE(isTrainerValid()) << "truncated channels frame was accepted"; + for (int ch = 0; ch < 16; ch++) EXPECT_EQ(trainerInput[ch], 0) << "channel " << ch; + + // and the stream stays aligned + feed(buildChannelsFrame()); + 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()); +} + +namespace { +void (*stubRxCb)(uint8_t*, uint32_t) = nullptr; +void stubSetReceiveCb(void*, void (*cb)(uint8_t*, uint32_t)) { stubRxCb = cb; } +} // namespace + +// The receive callback is released before the port's driver context goes +// away, and only by the port it is attached to. +TEST_F(CrsfTrainerTest, releasedOnlyByOwningCtx) +{ + etx_serial_driver_t drv = {}; + drv.setReceiveCb = stubSetReceiveCb; + int ctx, otherCtx; + + crsfTrainerStart(&ctx, &drv); + ASSERT_EQ(stubRxCb, crsfTrainerReceiveData); + + crsfTrainerReleaseCtx(&otherCtx); + EXPECT_EQ(stubRxCb, crsfTrainerReceiveData) << "released by another port"; + + crsfTrainerReleaseCtx(&ctx); + EXPECT_EQ(stubRxCb, nullptr); +} + +#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 c3155682bf5..3db5e62f546 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 4e121ed8846..20dcc188910 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 edd7c0de5b0..c32cb697bc8 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 bab2d98d056..69fad25af0e 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 b63eeb8f961..46c9da19a69 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 606161c7bcc..d88c3cadba3 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 09435639432..4aee73a993a 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 7affb6028bf..fd01542c115 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 25606372dca..70124fbff03 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 8e35a7240e1..00e279c4d62 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 b3b672669d7..c4eb7dcc153 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 eeb06853bbb..8f8c7880c4f 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 1dde7b43364..04eebb7a201 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 6e20bac5192..2927dc6205c 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 3418aabfe20..91128a8f816 100644 --- a/radio/src/translations/i18n/pt.h +++ b/radio/src/translations/i18n/pt.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 "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 c803ea7840c..cf6502eb174 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 40faf87278b..528306b681b 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 b1c51779c23..cd31fe7dc7b 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 3eb15cb1b21..00fecbb1b11 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()