jyapayne/switch-pico
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add UART link statistics and drop host-side gyro zeroing

Browse source 
STATS command (0xAA 0xFE 0x08 0x02 'STATS') returns frames accepted/rejected,
bytes discarded, RX FIFO overruns and motion frame/sample counters;
controller-uart-bridge --debug-uart prints them beside host send counts.
The bridge no longer zeroes the gyro from its first 200 samples: like the
AIO's Bluepad32 path, controller-calibrated values are forwarded unchanged.
This commit is contained in:
Joey Yakimowich-Payne 2026-09-22 17:25:52 -06:00
commit e2f9e1f211
8 changed files with 229 additions and 48 deletions
Download patch file Download diff file Expand all files Collapse all files

16
README.md
View file

@ -2478,6 +2478,22 @@ the older 5-byte `0xBB, 0x02` frame (as slot 0). Firmware and bridge from before
this change are not compatible with each other beyond that: an old bridge sees
no rumble from new firmware, and an old firmware ignores v3 input.
Motion samples follow the AIO model: the bridge forwards each SDL sensor sample
once, the firmware pools the newest three per slot, and the next 15 ms USB
report consumes them (raw frames or quaternion integration), so a stalled or
bursty controller stream is never re-integrated as motion. Gyro values are
forwarded as the controller reports them, without host-side zeroing, like the
AIO's Bluepad32 path.
Link health is queryable: the command `0xAA 0xFE 0x08 0x02 "STATS" 0 0 ck` is
answered with `0xBB 0x05` followed by six little-endian `u32` counters (frames
accepted, frames rejected, bytes discarded while resynchronising, RX FIFO
overruns, motion-carrying frames, motion samples) and a checksum.
`controller-uart-bridge --debug-uart` polls it once a second and prints the
firmware counters beside the bridge's own send counts; matching numbers with
zero rejects/overruns mean the serial link is clean and any motion problem is
upstream in SDL or the controller.
Bridge usage with several controllers on one Pico:
```sh

BIN
firmware/switch-pico.elf
View file

Binary file not shown.

BIN
firmware/switch-pico.uf2
View file

Binary file not shown.

81
src/firmware/main.cpp
View file

@ -39,9 +39,11 @@
#define UART_RX_PIN 5
#define UART_RUMBLE_HEADER 0xBB
#define UART_RUMBLE_TYPE_SLOT 0x03
#define UART_STATS_TYPE 0x05
// Host -> Pico command frame: 0xAA 0xFE payload_len payload... checksum.
#define UART_COMMAND_VERSION 0xFE
#define UART_COMMAND_REBOOT_BOOTSEL 0x01
#define UART_COMMAND_STATS 0x02
#endif
#ifdef SWITCH_PICO_BLUEPAD32
@ -114,26 +116,75 @@ static void on_rumble_from_usb(uint8_t instance,
}
#ifndef SWITCH_PICO_BLUEPAD32
// Command frames share the report framing. The only command reboots into the
// ROM BOOTSEL loader; it must carry the "BOOTSEL" magic so line noise or a
// mis-framed report can never trigger it.
// Link health counters, reported on request so the host can tell frame loss
// from a quiet controller. Overruns count RX FIFO overflows flagged by the UART.
struct UartLinkStats {
uint32_t frames_ok;
uint32_t frames_rejected;
uint32_t bytes_discarded;
uint32_t overruns;
uint32_t motion_frames;
uint32_t motion_samples;
};
static UartLinkStats g_uart_stats{};
static void write_u32_le(uint8_t* dst, uint32_t value) {
dst[0] = static_cast<uint8_t>(value);
dst[1] = static_cast<uint8_t>(value >> 8);
dst[2] = static_cast<uint8_t>(value >> 16);
dst[3] = static_cast<uint8_t>(value >> 24);
}
// Pico -> host: 0xBB 0x05 then six little-endian u32 counters and checksum.
static void send_stats_uart_frame() {
uint8_t frame[2 + 6 * 4 + 1] = {UART_RUMBLE_HEADER, UART_STATS_TYPE};
const uint32_t values[6] = {
g_uart_stats.frames_ok, g_uart_stats.frames_rejected,
g_uart_stats.bytes_discarded, g_uart_stats.overruns,
g_uart_stats.motion_frames, g_uart_stats.motion_samples,
};
for (uint8_t i = 0; i < 6; ++i) {
write_u32_le(&frame[2 + i * 4], values[i]);
}
uint8_t sum = 0;
for (uint8_t i = 0; i + 1 < sizeof(frame); ++i) {
sum = static_cast<uint8_t>(sum + frame[i]);
}
frame[sizeof(frame) - 1] = sum;
uart_write_blocking(UART_ID, frame, sizeof(frame));
}
// Command frames share the report framing. BOOTSEL must carry the "BOOTSEL"
// magic so line noise or a mis-framed report can never trigger it; STATS
// carries "STATS" for the same reason.
static void handle_uart_command(const uint8_t* frame, uint8_t length) {
static const uint8_t kBootselMagic[7] = {'B', 'O', 'O', 'T', 'S', 'E', 'L'};
static const uint8_t kStatsMagic[7] = {'S', 'T', 'A', 'T', 'S', 0, 0};
uint8_t sum = 0;
for (uint8_t i = 0; i + 1 < length; ++i) {
sum = static_cast<uint8_t>(sum + frame[i]);
}
if (sum != frame[length - 1]) {
++g_uart_stats.frames_rejected;
return;
}
const uint8_t payload_len = frame[2];
const uint8_t* payload = frame + 3;
if (payload_len == 1 + sizeof(kBootselMagic) &&
payload[0] == UART_COMMAND_REBOOT_BOOTSEL &&
if (payload_len != 8) {
++g_uart_stats.frames_rejected;
return;
}
if (payload[0] == UART_COMMAND_REBOOT_BOOTSEL &&
memcmp(payload + 1, kBootselMagic, sizeof(kBootselMagic)) == 0) {
LOG_PRINTF("[UART] reboot to BOOTSEL requested\n");
uart_tx_wait_blocking(UART_ID);
reset_usb_boot(0, 0);
} else if (payload[0] == UART_COMMAND_STATS &&
memcmp(payload + 1, kStatsMagic, sizeof(kStatsMagic)) == 0) {
++g_uart_stats.frames_ok;
send_stats_uart_frame();
} else {
++g_uart_stats.frames_rejected;
}
}
@ -178,11 +229,20 @@ static bool poll_uart_frames() {
static bool has_last_byte = false;
bool new_data = false;
// The UART flags RX FIFO overflow in RSR; clear it once counted.
if (uart_get_hw(UART_ID)->rsr & UART_UARTRSR_OE_BITS) {
uart_get_hw(UART_ID)->rsr = UART_UARTRSR_BITS;
++g_uart_stats.overruns;
}
while (uart_is_readable(UART_ID)) {
uint8_t byte = uart_getc(UART_ID);
uint64_t now = to_ms_since_boot(get_absolute_time());
if (has_last_byte && (now - to_ms_since_boot(last_byte_time)) > 20) {
if (index != 0) {
g_uart_stats.bytes_discarded += index;
}
index = 0; // stale data, restart frame
expected_len = 0;
}
@ -191,11 +251,13 @@ static bool poll_uart_frames() {
if (index == 0) {
if (byte != 0xAA) {
++g_uart_stats.bytes_discarded;
continue; // wait for start-of-frame marker
}
}
if (index >= sizeof(buffer)) {
g_uart_stats.bytes_discarded += index;
index = 0;
expected_len = 0;
}
@ -206,6 +268,8 @@ static bool poll_uart_frames() {
const uint8_t overhead = buffer[1] == 0x03 ? 5u : 4u;
expected_len = static_cast<uint8_t>(buffer[2] + overhead);
if (expected_len < 12 || expected_len > sizeof(buffer)) {
g_uart_stats.bytes_discarded += index;
++g_uart_stats.frames_rejected;
index = 0;
expected_len = 0;
continue;
@ -224,6 +288,11 @@ static bool poll_uart_frames() {
if (switch_pro_apply_uart_packet(buffer, expected_len, parsed, slot)) {
merge_uart_state(g_user_states[slot], parsed);
new_data = true;
++g_uart_stats.frames_ok;
if (parsed.motion_sample_count != 0) {
++g_uart_stats.motion_frames;
g_uart_stats.motion_samples += parsed.motion_sample_count;
}
LOG_PRINTF("[UART] slot=%u buttons=0x%04x hat=%u lx=%u ly=%u rx=%u ry=%u\n",
slot,
(parsed.button_east ? SWITCH_PRO_MASK_A : 0) |
@ -248,6 +317,8 @@ static bool poll_uart_frames() {
controller_axis_to_unsigned(parsed.left_stick_y) >> 8,
controller_axis_to_unsigned(parsed.right_stick_x) >> 8,
controller_axis_to_unsigned(parsed.right_stick_y) >> 8);
} else {
++g_uart_stats.frames_rejected;
}
index = 0;
expected_len = 0;

93
src/switch_pico_bridge/controller_uart_bridge.py
View file

@ -39,6 +39,7 @@ from rich.text import Text
from .switch_pico_uart import (
UART_BAUD,
UART_SLOT_COUNT,
UartLinkStats,
MS2_PER_G,
RAD_TO_DEG,
ACCEL_LSB_PER_G,
@ -62,7 +63,6 @@ RUMBLE_DURATION_MS = 50
CONTROLLER_DB_URL_DEFAULT = "https://raw.githubusercontent.com/mdqinc/SDL_GameControllerDB/refs/heads/master/gamecontrollerdb.txt"
SDL_TRUE = True
SDL_EVENT_GAMEPAD_SENSOR_UPDATE = getattr(sdl3, "SDL_EVENT_GAMEPAD_SENSOR_UPDATE", 0x658)
GYRO_BIAS_SAMPLES = 200
def parse_mapping(value: str) -> Tuple[int, str, Optional[int]]:
@ -266,6 +266,15 @@ class UartLink:
port: str
uart: Optional[PicoUART] = None
last_reopen_attempt: float = 0.0
# Host-side send counters and the last firmware stats reply for --debug-uart.
frames_sent: int = 0
motion_frames_sent: int = 0
motion_samples_sent: int = 0
last_stats_request: float = 0.0
last_stats: Optional[UartLinkStats] = None
last_stats_frames_sent: int = 0
last_stats_motion_frames_sent: int = 0
last_stats_motion_samples_sent: int = 0
@dataclass
@ -298,11 +307,6 @@ class ControllerContext:
sensors_enabled: bool = False
imu_samples: List[IMUSample] = field(default_factory=list)
last_accel: Tuple[float, float, float] = (0.0, 0.0, 0.0)
gyro_bias_x: float = 0.0
gyro_bias_y: float = 0.0
gyro_bias_z: float = 0.0
gyro_bias_samples: int = 0
gyro_bias_locked: bool = False
last_debug_imu_print: float = 0.0
imu_debug_samples: int = 0
last_debug_rumble: Optional[Tuple[float, float]] = None
@ -856,6 +860,14 @@ def build_arg_parser() -> argparse.ArgumentParser:
action="store_true",
help="Print every decoded rumble frame received from the Pico and whether SDL accepted it.",
)
parser.add_argument(
"--debug-uart",
action="store_true",
help=(
"Once a second, ask the Pico for its UART link counters and print them next "
"to what the bridge sent, to spot frame loss, checksum failures or FIFO overruns."
),
)
parser.add_argument(
"--rumble-gain",
type=float,
@ -928,6 +940,7 @@ class BridgeConfig:
swap_abxy_global: bool
debug_imu: bool = False
debug_rumble: bool = False
debug_uart: bool = False
no_imu: bool = False
gyro_scale: float = 1.0
rumble_gain: float = 1.0
@ -1037,6 +1050,7 @@ def build_bridge_config(console: Console, args: argparse.Namespace) -> BridgeCon
swap_abxy_global=bool(args.swap_abxy),
debug_imu=bool(args.debug_imu),
debug_rumble=bool(args.debug_rumble),
debug_uart=bool(args.debug_uart),
no_imu=bool(args.no_imu),
gyro_scale=float(args.gyro_scale),
rumble_gain=float(args.rumble_gain),
@ -1561,31 +1575,9 @@ def handle_sensor_update(
return
gx, gy, gz = float(data[0]), float(data[1]), float(data[2])
if not ctx.gyro_bias_locked:
if ctx.gyro_bias_samples < GYRO_BIAS_SAMPLES:
ctx.gyro_bias_x += gx
ctx.gyro_bias_y += gy
ctx.gyro_bias_z += gz
ctx.gyro_bias_samples += 1
if ctx.gyro_bias_samples >= GYRO_BIAS_SAMPLES:
n = ctx.gyro_bias_samples
ctx.gyro_bias_x /= n
ctx.gyro_bias_y /= n
ctx.gyro_bias_z /= n
ctx.gyro_bias_locked = True
if not ctx.gyro_bias_locked:
bx, by, bz = 0.0, 0.0, 0.0
else:
bx, by, bz = ctx.gyro_bias_x, ctx.gyro_bias_y, ctx.gyro_bias_z
ux, uy, uz = gx, gy, gz
ux -= bx
uy -= by
uz -= bz
ax, ay, az = ctx.last_accel
# No host-side gyro zeroing: like the AIO backend, forward the controller's
# own calibrated values and leave bias handling to the console.
# SDL (hidapi_switch.c SendSensorUpdate) remaps Nintendo's native axes to match
# PlayStation convention before emitting sensor events:
# SDL_out[0] (X) = -(Nintendo_Y * scale)
@ -1599,9 +1591,9 @@ def handle_sensor_update(
accel_x=convert_accel_to_raw(-az),
accel_y=convert_accel_to_raw(-ax),
accel_z=convert_accel_to_raw(ay),
gyro_x=convert_gyro_to_raw(-uz, config.gyro_scale),
gyro_y=convert_gyro_to_raw(-ux, config.gyro_scale),
gyro_z=convert_gyro_to_raw(uy, config.gyro_scale),
gyro_x=convert_gyro_to_raw(-gz, config.gyro_scale),
gyro_y=convert_gyro_to_raw(-gx, config.gyro_scale),
gyro_z=convert_gyro_to_raw(gy, config.gyro_scale),
)
ctx.imu_samples.append(sample)
@ -1621,8 +1613,6 @@ def handle_sensor_update(
f"[IMU idx={ctx.controller_index}] rate={rate:.0f}Hz "
f"accel_m_s2=({ax:.3f},{ay:.3f},{az:.3f}) |a|={magnitude:.2f}g "
f"gyro_rad_s=({gx:.3f},{gy:.3f},{gz:.3f}) "
f"bias_rad_s=({bx:.4f},{by:.4f},{bz:.4f}) "
f"bias_locked={ctx.gyro_bias_locked} "
f"raw=({sample.accel_x},{sample.accel_y},{sample.accel_z};"
f"{sample.gyro_x},{sample.gyro_y},{sample.gyro_z})"
)
@ -1725,6 +1715,27 @@ def handle_device_removed(
sdl3.SDL_CloseGamepad(ctx.controller)
def report_link_stats(link: UartLink, stats: UartLinkStats) -> None:
"""Print firmware-side counters since the previous reply next to host sends."""
if link.last_stats is not None:
delta = stats.delta(link.last_stats)
sent = link.frames_sent - link.last_stats_frames_sent
motion_sent = link.motion_frames_sent - link.last_stats_motion_frames_sent
samples_sent = link.motion_samples_sent - link.last_stats_motion_samples_sent
# The stats request itself is one accepted frame on the firmware side.
print(
f"[UART {link.port}] host sent frames={sent} motion_frames={motion_sent} "
f"samples={samples_sent} | pico ok={delta.frames_ok - 1} "
f"motion_frames={delta.motion_frames} samples={delta.motion_samples} "
f"rejected={delta.frames_rejected} discarded_bytes={delta.bytes_discarded} "
f"fifo_overruns={delta.overruns}"
)
link.last_stats = stats
link.last_stats_frames_sent = link.frames_sent
link.last_stats_motion_frames_sent = link.motion_frames_sent
link.last_stats_motion_samples_sent = link.motion_samples_sent
def service_link(
now: float,
config: BridgeConfig,
@ -1749,6 +1760,14 @@ def service_link(
ctx.report.imu_samples = []
uart.send_report(ctx.report, ctx.slot)
ctx.last_send = now
link.frames_sent += 1
if ctx.report.imu_samples:
link.motion_frames_sent += 1
link.motion_samples_sent += len(ctx.report.imu_samples)
if config.debug_uart and now - link.last_stats_request >= 1.0:
link.last_stats_request = now
uart.request_stats()
# Keep only the freshest rumble command per slot seen during this tick.
latest_by_slot: Dict[int, Tuple[float, float]] = {}
@ -1761,6 +1780,10 @@ def service_link(
latest_by_slot[slot] = (low, high)
frames_by_slot[slot] = frames_by_slot.get(slot, 0) + 1
if config.debug_uart and uart.last_stats is not None:
report_link_stats(link, uart.last_stats)
uart.last_stats = None
for ctx in members:
ctx.debug_rumble_frames += frames_by_slot.get(ctx.slot, 0)
latest_rumble = latest_by_slot.get(ctx.slot)

60
src/switch_pico_bridge/switch_pico_uart.py
View file

@ -19,7 +19,7 @@ import math
import struct
import time
import threading
from dataclasses import dataclass, field
from dataclasses import astuple, dataclass, field
from enum import IntEnum, IntFlag
from typing import Iterable, Mapping, Optional, Tuple, Union, List, Dict
@ -28,14 +28,21 @@ from serial.tools import list_ports, list_ports_common
UART_HEADER = 0xAA
UART_PROTOCOL_VERSION = 0x03
# Command frames reuse the report framing with this version byte.
# Command frames reuse the report framing with this version byte and an
# 8-byte payload: command id, then a magic tag guarding against line noise.
UART_COMMAND_VERSION = 0xFE
UART_COMMAND_PAYLOAD_SIZE = 8
UART_COMMAND_REBOOT_BOOTSEL = 0x01
UART_BOOTSEL_MAGIC = b"BOOTSEL"
UART_COMMAND_STATS = 0x02
UART_STATS_MAGIC = b"STATS"
RUMBLE_HEADER = 0xBB
# Legacy 5-byte frame (no slot) from firmware before multi-controller support.
RUMBLE_TYPE_DECODED = 0x02
RUMBLE_TYPE_SLOT = 0x03
# Reply to the stats command: 0xBB 0x05 then six little-endian u32 counters.
STATS_TYPE = 0x05
STATS_FRAME_SIZE = 2 + 6 * 4 + 1
UART_BAUD = 921600
UART_SLOT_COUNT = 4
IMU_SAMPLES_PER_REPORT = 3
@ -284,6 +291,24 @@ class SwitchReport:
return frame + bytes([compute_checksum(frame)])
@dataclass(frozen=True)
class UartLinkStats:
"""Firmware-side UART link counters since boot (STATS command reply)."""
frames_ok: int
frames_rejected: int
bytes_discarded: int
overruns: int
motion_frames: int
motion_samples: int
def delta(self, previous: "UartLinkStats") -> "UartLinkStats":
"""Counter change since ``previous``, tolerant of u32 wrap."""
return UartLinkStats(
*((a - b) & 0xFFFFFFFF for a, b in zip(astuple(self), astuple(previous)))
)
class PicoUART:
def __init__(self, port: str, baudrate: int = UART_BAUD) -> None:
"""Open a UART connection to the Pico with non-blocking IO."""
@ -300,18 +325,28 @@ class PicoUART:
dsrdtr=False,
)
self._buffer = bytearray()
self.last_stats: Optional[UartLinkStats] = None
def send_report(self, report: SwitchReport, slot: int = 0) -> None:
"""Send a controller report to one of the Pico's controller slots."""
self.serial.write(report.to_bytes(slot))
@staticmethod
def reboot_bootsel_frame() -> bytes:
"""Command frame that makes the UART firmware reboot into ROM BOOTSEL."""
payload = bytes([UART_COMMAND_REBOOT_BOOTSEL]) + UART_BOOTSEL_MAGIC
def _command_frame(command: int, magic: bytes) -> bytes:
payload = (bytes([command]) + magic).ljust(UART_COMMAND_PAYLOAD_SIZE, b"\0")
frame = bytes([UART_HEADER, UART_COMMAND_VERSION, len(payload)]) + payload
return frame + bytes([compute_checksum(frame)])
@classmethod
def reboot_bootsel_frame(cls) -> bytes:
"""Command frame that makes the UART firmware reboot into ROM BOOTSEL."""
return cls._command_frame(UART_COMMAND_REBOOT_BOOTSEL, UART_BOOTSEL_MAGIC)
@classmethod
def stats_request_frame(cls) -> bytes:
"""Command frame asking the firmware for its link counters."""
return cls._command_frame(UART_COMMAND_STATS, UART_STATS_MAGIC)
def reboot_bootsel(self) -> None:
"""Ask the Pico to reboot into BOOTSEL so picotool can flash it.
@ -321,6 +356,10 @@ class PicoUART:
self.serial.write(self.reboot_bootsel_frame())
self.serial.flush()
def request_stats(self) -> None:
"""Ask for link counters; the reply lands in ``last_stats`` during ``read_rumble``."""
self.serial.write(self.stats_request_frame())
def read_rumble(self) -> Optional[Tuple[int, float, float]]:
"""
Extract one decoded rumble frame as (slot, low, high) with magnitudes
@ -350,7 +389,12 @@ class PicoUART:
return None
frame_type = self._buffer[start + 1]
length = 6 if frame_type == RUMBLE_TYPE_SLOT else 5
if frame_type == RUMBLE_TYPE_SLOT:
length = 6
elif frame_type == STATS_TYPE:
length = STATS_FRAME_SIZE
else:
length = 5
if len(self._buffer) - start < length:
if start > 0:
del self._buffer[:start]
@ -364,6 +408,10 @@ class PicoUART:
if frame_type == RUMBLE_TYPE_DECODED:
del self._buffer[: start + length]
return 0, frame[2] / 255.0, frame[3] / 255.0
if frame_type == STATS_TYPE:
del self._buffer[: start + length]
self.last_stats = UartLinkStats(*struct.unpack_from("<6I", frame, 2))
continue
del self._buffer[: start + 1]

3
tests/test_controller_uart_bridge.py
View file

@ -53,9 +53,8 @@ def make_config() -> bridge.BridgeConfig:
def test_sensor_buffer_retains_latest_three_samples() -> None:
controller = cast(sdl3.SDL_Gamepad, object())
ctx = bridge.ControllerContext(controller, 7, 0, "dualsense", None, None)
ctx = bridge.ControllerContext(controller, 7, 0, "dualsense", None)
ctx.sensors_enabled = True
ctx.gyro_bias_locked = True
contexts = {ctx.instance_id: ctx}
config = make_config()

24
tests/test_uart_protocol.py
View file

@ -10,6 +10,7 @@ from switch_pico_bridge.switch_pico_uart import (
UART_HEADER,
UART_PROTOCOL_VERSION,
UART_SLOT_COUNT,
UartLinkStats,
RUMBLE_HEADER,
RUMBLE_TYPE_DECODED,
RUMBLE_TYPE_SLOT,
@ -52,6 +53,7 @@ def make_uart(data: bytes = b"") -> tuple[PicoUART, BufferedSerial]:
serial_port = BufferedSerial(data)
uart.serial = serial_port
uart._buffer = bytearray()
uart.last_stats = None
return uart, serial_port
@ -227,3 +229,25 @@ def test_reboot_bootsel_frame_matches_firmware_contract():
assert frame[4:11] == b"BOOTSEL"
assert len(frame) == 12
assert frame[-1] == compute_checksum(frame[:-1])
def test_stats_request_frame_matches_firmware_contract():
frame = PicoUART.stats_request_frame()
assert frame[:3] == bytes([UART_HEADER, 0xFE, 8])
assert frame[3] == 0x02
assert frame[4:11] == b"STATS\0\0"
assert frame[-1] == compute_checksum(frame[:-1])
def test_stats_reply_is_captured_without_disturbing_rumble_parsing():
counters = (1000, 2, 30, 0, 300, 900)
stats = bytes([RUMBLE_HEADER, 0x05]) + struct.pack("<6I", *counters)
stats += bytes([compute_checksum(stats)])
uart, _ = make_uart(make_rumble_frame(5, 6, slot=1) + stats + make_rumble_frame(7, 8))
assert uart.read_rumble() == pytest.approx((1, 5 / 255.0, 6 / 255.0))
assert uart.last_stats is None
assert uart.read_rumble() == pytest.approx((0, 7 / 255.0, 8 / 255.0))
assert uart.last_stats == UartLinkStats(*counters)
later = UartLinkStats(1010, 2, 30, 1, 303, 909)
assert later.delta(uart.last_stats) == UartLinkStats(10, 0, 0, 1, 3, 9)