diff --git a/radio/src/gui/gui_common.cpp b/radio/src/gui/gui_common.cpp index 5f716d537f2..a0a5172a6f5 100644 --- a/radio/src/gui/gui_common.cpp +++ b/radio/src/gui/gui_common.cpp @@ -566,9 +566,14 @@ bool isSerialModeAvailable(uint8_t port_nr, int mode) #endif #if defined(USB_SERIAL) - // Telemetry input & SBUS trainer on VCP is not yet supported - if (port_nr == SP_VCP && - (mode == UART_MODE_TELEMETRY || mode == UART_MODE_SBUS_TRAINER)) + // Telemetry input on VCP is not yet supported + if (port_nr == SP_VCP && mode == UART_MODE_TELEMETRY) + return false; + + // USB CDC carries no line polarity, so the two SBUS trainer modes behave + // identically on VCP. Offer only the one the user knows as plain SBUS: + // normal SBUS is inverted serial, so that is UART_MODE_SBUS_TRAINER_INV. + if (port_nr == SP_VCP && mode == UART_MODE_SBUS_TRAINER) return false; #endif diff --git a/radio/src/sbus.cpp b/radio/src/sbus.cpp index 5a86b235367..4cb25af485c 100644 --- a/radio/src/sbus.cpp +++ b/radio/src/sbus.cpp @@ -21,6 +21,8 @@ #include "sbus.h" +#include + #include "edgetx.h" #include "timers_driver.h" @@ -42,6 +44,13 @@ static bool _sbus_aux_enabled = false; static void sbusProcessFrame(int16_t* pulses, uint8_t* sbus, uint32_t size); +// The last byte of a frame is 0x00 for Futaba / FrSky, but some +// implementations use it to carry frame flags (0x04 / 0x14 / 0x24). +static inline bool sbusIsEndByte(uint8_t b) +{ + return b == SBUS_END_BYTE || b == 0x04 || b == 0x14 || b == 0x24; +} + void sbusSetReceiveCtx(void* ctx, const etx_serial_driver_t* drv) { _sbus_ctx = ctx; @@ -74,11 +83,95 @@ void sbusFrameReceived(void*) sbusProcessFrame(trainerInput, frame, received); } +// +// Byte-stream framer, for ports with no idle-line detection (USB-VCP). +// +// USB CDC gives no frame boundaries: a 25 byte frame may be split over several +// packets, and several frames may arrive in one packet. So frames have to be +// recovered from the stream itself. +// +// Invariant: _sbus_stream_len == 0, or _sbus_stream_buf[0] == SBUS_START_BYTE. +// +static uint8_t _sbus_stream_buf[SBUS_FRAME_SIZE]; +static uint8_t _sbus_stream_len = 0; + +static const etx_serial_driver_t* _sbus_stream_drv = nullptr; +static void* _sbus_stream_ctx = nullptr; + +// Drop the leading byte of a rejected frame and re-sync on the next start byte +// found in what is left. Never drops the whole buffer: a valid frame may well +// have started inside it. +static void sbusStreamResync() +{ + uint8_t i = 1; + while (i < _sbus_stream_len && _sbus_stream_buf[i] != SBUS_START_BYTE) i++; + + _sbus_stream_len -= i; + if (_sbus_stream_len > 0) { + memmove(_sbus_stream_buf, _sbus_stream_buf + i, _sbus_stream_len); + } +} + +void sbusStreamReceiveData(uint8_t* data, uint32_t len) +{ + // Trainer mode is not asking for serial input: stay out of the way. + if (!_sbus_aux_enabled) { + _sbus_stream_len = 0; + return; + } + + while (len > 0) { + // Hunt for a start byte while no frame is being assembled + if (_sbus_stream_len == 0 && *data != SBUS_START_BYTE) { + data++; len--; + continue; + } + + _sbus_stream_buf[_sbus_stream_len++] = *data++; + len--; + + if (_sbus_stream_len < SBUS_FRAME_SIZE) continue; + + if (sbusIsEndByte(_sbus_stream_buf[SBUS_FRAME_SIZE - 1])) { + // Complete frame. sbusProcessFrame() re-checks it, and resets the + // trainer validity timer if it is accepted. + sbusProcessFrame(trainerInput, _sbus_stream_buf, SBUS_FRAME_SIZE); + _sbus_stream_len = 0; + } else { + sbusStreamResync(); + } + } +} + +void sbusStreamStart(void* ctx, const etx_serial_driver_t* drv) +{ + if (!drv || !drv->setReceiveCb) return; + + _sbus_stream_len = 0; + _sbus_stream_drv = drv; + _sbus_stream_ctx = ctx; + + drv->setReceiveCb(ctx, sbusStreamReceiveData); +} + +void sbusStreamStop() +{ + auto drv = _sbus_stream_drv; + auto ctx = _sbus_stream_ctx; + + _sbus_stream_drv = nullptr; + _sbus_stream_ctx = nullptr; + _sbus_stream_len = 0; + + // Release the RX stream, so the next user of the port gets it + if (drv && drv->setReceiveCb) drv->setReceiveCb(ctx, nullptr); +} + // Range for pulses (ppm input) is [-512:+512] static void sbusProcessFrame(int16_t* pulses, uint8_t* sbus, uint32_t size) { if (size != SBUS_FRAME_SIZE || sbus[0] != SBUS_START_BYTE || - sbus[SBUS_FRAME_SIZE - 1] != SBUS_END_BYTE) { + !sbusIsEndByte(sbus[SBUS_FRAME_SIZE - 1])) { return; // not a valid SBUS frame } if ((sbus[SBUS_FLAGS_IDX] & (1 << SBUS_FAILSAFE_BIT)) || diff --git a/radio/src/sbus.h b/radio/src/sbus.h index e8089d0fcb6..8a27f9d7870 100644 --- a/radio/src/sbus.h +++ b/radio/src/sbus.h @@ -31,7 +31,21 @@ void sbusSetReceiveCtx(void* ctx, const etx_serial_driver_t* drv); // SBUS AUX idle callback void sbusAuxFrameReceived(void* param); -// Enable / disable SBUS AUX +// Enable / disable serial trainer input (both the AUX UART and the USB-VCP +// paths). Driven by TRAINER_MODE_MASTER_SERIAL. void sbusAuxSetEnabled(bool enabled); void sbusFrameReceived(void* param); + +// +// SBUS byte-stream framer. +// +// For ports that deliver arbitrarily chunked buffers and have no idle-line +// detection to mark frame boundaries (USB-VCP). Attach / detach the driver's +// receive callback, and keep the partial-frame state across chunks. +// +void sbusStreamStart(void* ctx, const etx_serial_driver_t* drv); +void sbusStreamStop(); + +// Receive callback: feeds a chunk of the byte stream into the framer +void sbusStreamReceiveData(uint8_t* data, uint32_t len); diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index 1f09287cdbc..b2e969e7cf8 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -221,7 +221,15 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por case UART_MODE_SBUS_TRAINER_INV: sbusSetReceiveCtx(ctx, drv); if (drv && drv->setIdleCb) { + // Hardware UART: the idle line marks the frame boundaries drv->setIdleCb(ctx, sbusAuxFrameReceived, nullptr); + } else if (drv && drv->setReceiveCb) { + // No idle-line detection (USB-VCP): recover frames from the byte stream + sbusStreamStart(ctx, drv); + } else { + // De-init (ctx == nullptr, hence drv == nullptr), or a port that can do + // neither. No-op unless the framer is currently attached. + sbusStreamStop(); } break; diff --git a/radio/src/tests/sbus_trainer.cpp b/radio/src/tests/sbus_trainer.cpp new file mode 100644 index 00000000000..59ba6524ec5 --- /dev/null +++ b/radio/src/tests/sbus_trainer.cpp @@ -0,0 +1,302 @@ +/* + * 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 + +#include "gtests.h" +#include "gui/gui_common.h" +#include "sbus.h" +#include "serial.h" +#include "trainer.h" + +// Tests for the SBUS byte-stream framer used by ports without idle-line +// detection (USB-VCP). The framer has to recover 25 byte frames from a stream +// chunked at arbitrary boundaries. + +namespace { + +constexpr uint8_t SBUS_FRAME_SIZE = 25; +constexpr uint8_t SBUS_START = 0x0F; +constexpr uint16_t SBUS_CENTER = 992; + +// Distinct value per channel, so channel ordering is checked and not just +// channel content. Kept inside the 11 bit range. +uint16_t testChannelValue(int ch) { return 200 + ch * 100; } + +// Expected trainer value for a raw SBUS channel value, per sbusProcessFrame() +int16_t expectedPulse(uint16_t raw) { return ((int32_t)raw - SBUS_CENTER) * 5 / 8; } + +std::vector buildFrame(uint8_t flags = 0x00, uint8_t endByte = 0x00) +{ + std::vector frame; + frame.push_back(SBUS_START); + + uint32_t bits = 0; + uint8_t bitsAvailable = 0; + for (int ch = 0; ch < 16; ch++) { + bits |= (uint32_t)(testChannelValue(ch) & 0x7FF) << bitsAvailable; + bitsAvailable += 11; + while (bitsAvailable >= 8) { + frame.push_back(bits & 0xFF); + bits >>= 8; + bitsAvailable -= 8; + } + } + + frame.push_back(flags); + frame.push_back(endByte); + return frame; +} + +void append(std::vector& dst, const std::vector& src) +{ + dst.insert(dst.end(), src.begin(), src.end()); +} + +// Feed a stream to the framer, split into fixed size chunks. chunkSize == 0 +// means "one single chunk", which is what a small USB packet looks like. +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); + sbusStreamReceiveData(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 SbusStreamTest : public ::testing::Test +{ + protected: + void SetUp() override + { + sbusStreamStop(); // clears any partial frame left by a previous test + sbusAuxSetEnabled(true); + memset(trainerInput, 0, sizeof(trainerInput)); + trainerSetTimer(0); + } + + void TearDown() override { sbusAuxSetEnabled(false); } +}; + +} // namespace + +TEST_F(SbusStreamTest, singleFrame) +{ + feed(buildFrame()); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// A 25 byte frame is routinely split across USB packets. Every possible split +// point has to work, including one byte at a time. +TEST_F(SbusStreamTest, splitAcrossChunkBoundaries) +{ + for (size_t chunkSize = 1; chunkSize <= SBUS_FRAME_SIZE; chunkSize++) { + sbusStreamStop(); + memset(trainerInput, 0, sizeof(trainerInput)); + + feed(buildFrame(), chunkSize); + + for (int ch = 0; ch < 16; ch++) { + EXPECT_EQ(trainerInput[ch], expectedPulse(testChannelValue(ch))) + << "chunk size " << chunkSize << ", channel " << ch; + } + } +} + +// Several frames may arrive in a single USB packet. +TEST_F(SbusStreamTest, backToBackFramesInOneChunk) +{ + std::vector stream; + append(stream, buildFrame()); + append(stream, buildFrame()); + append(stream, buildFrame()); + + feed(stream); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// Bytes already in flight when the framer attaches are not a frame start. +TEST_F(SbusStreamTest, garbagePrefixIsSkipped) +{ + std::vector stream = {0x11, 0x22, 0x33, 0xFF, 0xAB}; + append(stream, buildFrame()); + + feed(stream); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// A frame whose end byte is wrong must be rejected, and the framer must +// re-sync rather than discard everything it holds. +TEST_F(SbusStreamTest, invalidEndByteIsRejectedThenResyncs) +{ + auto bad = buildFrame(0x00, 0xFF); + + feed(bad); + EXPECT_FALSE(isTrainerValid()) << "frame with a bad end byte was accepted"; + + feed(buildFrame()); + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// Garbage injected mid-stream (the --garbage case of the PC-side test tool): +// a truncated frame followed by good ones must recover. +TEST_F(SbusStreamTest, resyncAfterTruncatedFrame) +{ + auto frame = buildFrame(); + + std::vector stream; + append(stream, frame); + // half a frame, then noise, then two clean frames + stream.insert(stream.end(), frame.begin(), frame.begin() + 12); + for (uint8_t b : {0x55, 0xAA, 0x37, 0x91}) stream.push_back(b); + append(stream, frame); + append(stream, frame); + + feed(stream, 7); // awkward chunking on top + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// The re-sync path has to find a start byte held *inside* the buffer, not just +// drop the buffer: here a stray 0x0F precedes the real frame closely enough +// that the real frame is already partly buffered when the false frame fails. +TEST_F(SbusStreamTest, resyncFindsStartByteInsideBuffer) +{ + std::vector stream = {SBUS_START, 0x01, 0x02}; + append(stream, buildFrame()); + + feed(stream); + + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// Some implementations carry frame flags in the last byte. +TEST_F(SbusStreamTest, acceptsEndByteVariants) +{ + for (uint8_t endByte : {0x00, 0x04, 0x14, 0x24}) { + sbusStreamStop(); + memset(trainerInput, 0, sizeof(trainerInput)); + trainerSetTimer(0); + + feed(buildFrame(0x00, endByte)); + + EXPECT_TRUE(isTrainerValid()) << "end byte 0x" << std::hex << (int)endByte; + EXPECT_EQ(trainerInput[0], expectedPulse(testChannelValue(0))); + } +} + +// Failsafe / frame-lost frames must not drive the trainer channels. +TEST_F(SbusStreamTest, failsafeAndFrameLostFramesAreIgnored) +{ + feed(buildFrame(1 << 3)); // failsafe + EXPECT_FALSE(isTrainerValid()); + EXPECT_EQ(trainerInput[0], 0); + + feed(buildFrame(1 << 2)); // frame lost + EXPECT_FALSE(isTrainerValid()); + EXPECT_EQ(trainerInput[0], 0); + + // ... but the stream is still in sync afterwards + feed(buildFrame()); + expectTestChannels(); + EXPECT_TRUE(isTrainerValid()); +} + +// Nothing may reach the trainer channels unless the model actually asked for +// serial trainer input. +TEST_F(SbusStreamTest, ignoredWhenSerialTrainerDisabled) +{ + sbusAuxSetEnabled(false); + + feed(buildFrame()); + + EXPECT_FALSE(isTrainerValid()); + for (int ch = 0; ch < 16; ch++) EXPECT_EQ(trainerInput[ch], 0) << "channel " << ch; +} + +// Link loss: when frames stop arriving the trainer input has to go stale, so +// the sticks take back control. The timer is decremented from per10ms(). +TEST_F(SbusStreamTest, inputGoesStaleWhenFramesStop) +{ + feed(buildFrame()); + ASSERT_TRUE(isTrainerValid()); + + // 1s worth of per10ms() ticks with no data + for (int i = 0; i < 100; i++) { + EXPECT_TRUE(isTrainerValid()) << "went stale early, at tick " << i; + trainerDecTimer(); + } + + EXPECT_FALSE(isTrainerValid()); +} + +#if defined(USB_SERIAL) +// The option has to be offered for USB-VCP in SYS -> Hardware -> Serial Port. +// USB CDC carries no line polarity, so both SBUS trainer modes would behave +// identically here; only the one presented to the user as plain SBUS is +// offered, which is UART_MODE_SBUS_TRAINER_INV (normal SBUS is inverted +// serial, so it is the MCU-inverting mode that reads "SBUS Trainer"). +TEST(SbusVcpMenu, sbusTrainerIsOfferedOnVcp) +{ + EXPECT_TRUE(isSerialModeAvailable(SP_VCP, UART_MODE_SBUS_TRAINER_INV)); + EXPECT_FALSE(isSerialModeAvailable(SP_VCP, UART_MODE_SBUS_TRAINER)); + + // Both must still be offered on the AUX UARTs + EXPECT_TRUE(isSerialModeAvailable(SP_AUX1, UART_MODE_SBUS_TRAINER)); +} +#endif + +// 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(SbusStreamTest, partialFrameIsDroppedOnStop) +{ + auto frame = buildFrame(); + std::vector partial(frame.begin(), frame.begin() + 20); + feed(partial); + + sbusStreamStop(); + + // the remaining 5 bytes must not complete a frame + std::vector rest(frame.begin() + 20, frame.end()); + feed(rest); + + EXPECT_FALSE(isTrainerValid()); +} diff --git a/tools/sbus_vcp_test.py b/tools/sbus_vcp_test.py new file mode 100755 index 00000000000..9356b0b5ed9 --- /dev/null +++ b/tools/sbus_vcp_test.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +"""Drive an EdgeTX radio's trainer channels with SBUS frames over USB-VCP. + +Requires the radio to be set up as: + + SYS -> Radio Setup -> USB Mode = Serial + SYS -> Hardware -> Serial Port -> USB-VCP = SBUS Trainer + MDL -> Setup -> Trainer Mode = Master/Serial + +Watch the result in MDL -> Channels. + +Do this with props off and RF disabled. + +Examples: + ./sbus_vcp_test.py --port /dev/ttyACM0 --sweep + ./sbus_vcp_test.py --port /dev/ttyACM0 --sweep --rate 150 + ./sbus_vcp_test.py --port /dev/ttyACM0 --constant --stop-after 200 + ./sbus_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") + +SBUS_FRAME_SIZE = 25 +SBUS_START_BYTE = 0x0F +SBUS_END_BYTE = 0x00 + +# Raw 11 bit channel values. EdgeTX decodes these as (raw - 992) * 5 / 8, so +# these are the values that give the trainer input full -512..+512 travel. +SBUS_MIN = 172 +SBUS_CENTER = 992 +SBUS_MAX = 1811 + +NUM_CHANNELS = 16 + + +def encode_frame(channels, flags=0x00, end_byte=SBUS_END_BYTE): + """Pack 16 channels x 11 bits into a standard 25 byte SBUS frame.""" + if len(channels) != NUM_CHANNELS: + raise ValueError(f"expected {NUM_CHANNELS} channels, got {len(channels)}") + + frame = bytearray([SBUS_START_BYTE]) + + bits = 0 + bits_available = 0 + for value in channels: + bits |= (int(value) & 0x7FF) << bits_available + bits_available += 11 + while bits_available >= 8: + frame.append(bits & 0xFF) + bits >>= 8 + bits_available -= 8 + + frame.append(flags) + frame.append(end_byte) + + assert len(frame) == SBUS_FRAME_SIZE, len(frame) + return bytes(frame) + + +def triangle(phase): + """Triangle wave in 0.0 .. 1.0, phase in 0.0 .. 1.0.""" + 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 = [SBUS_CENTER] * NUM_CHANNELS + for ch in range(4): + phase = ((elapsed / period) + ch * 0.25) % 1.0 + channels[ch] = int(SBUS_MIN + (SBUS_MAX - SBUS_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/parity are meaningless over USB CDC and are ignored by the radio. + # They are set to the nominal SBUS values only so the host driver is happy. + with serial.Serial(port, baudrate=100000, parity=serial.PARITY_EVEN, + stopbits=serial.STOPBITS_TWO, timeout=0) as ser: + print(f"{port}: sending SBUS 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 = [SBUS_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_frame(channels)) + count += 1 + + next_send += interval + delay = next_send - time.monotonic() + if delay > 0: + time.sleep(delay) + else: + # fell behind; resynchronise the schedule + next_send = time.monotonic() + except KeyboardInterrupt: + print(f"\nstopped after {count} frames") + + +if __name__ == "__main__": + main()