From b134be093bb9203b0a0e5499fca3b17338d17e14 Mon Sep 17 00:00:00 2001 From: BelixRogner Date: Tue, 4 Aug 2026 22:57:14 +0200 Subject: [PATCH] feat(radio): support SBUS Trainer over USB-VCP Allow USB-VCP to be set to SBUS Trainer, so a PC can drive the trainer channels over the radio's USB-C port without a USB-serial dongle or AUX UART wiring. Useful for hardware-in-the-loop testing, simulators, head trackers and robotics, where trainer mode is the right injection point because the physical sticks stay live and a momentary switch gives an instant override. The AUX SBUS path finds frame boundaries with the USART idle-line interrupt, which USB CDC does not have: bytes arrive in arbitrarily chunked packets, so a frame may be split across packets and several frames may arrive in one. Add a byte-stream framer that syncs on 0x0F, accumulates 25 bytes and validates the end byte, dropping a single byte and re-syncing within the buffer on failure rather than discarding it. Both paths share the existing sbusProcessFrame() decoder, so trainer freshness and link-loss fallback behave identically to AUX. serialSetCallBacks() selects the framer only for ports that have no setIdleCb but do have setReceiveCb, leaving every hardware UART unchanged, and releases the CDC RX callback on de-init so the next user of the port gets a clean stream. USB CDC carries no line polarity, so the two SBUS trainer modes would behave identically on VCP. Only UART_MODE_SBUS_TRAINER_INV is offered there, as that is the one presented to the user as plain SBUS: normal SBUS is inverted serial, so the MCU-inverting mode is the one that reads "SBUS Trainer". Baud rate likewise does not apply, and usbSerialInit() already ignores the requested params. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_011yNWsmBtBtjmHtNSJjuUeA --- radio/src/gui/gui_common.cpp | 11 +- radio/src/sbus.cpp | 95 +++++++++- radio/src/sbus.h | 16 +- radio/src/serial.cpp | 8 + radio/src/tests/sbus_trainer.cpp | 302 +++++++++++++++++++++++++++++++ tools/sbus_vcp_test.py | 165 +++++++++++++++++ 6 files changed, 592 insertions(+), 5 deletions(-) create mode 100644 radio/src/tests/sbus_trainer.cpp create mode 100755 tools/sbus_vcp_test.py 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()