From 3dc91e5f8d1ded80618e8aa49af5860bd2970223 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 24 Nov 2025 18:05:51 -0700 Subject: [PATCH] game gyro output as pro controller. Still not working --- .gitignore | 5 + gyro_support_plan.md | 49 ++++++ .../controller_uart_bridge.py | 88 +++++++++-- switch_pro_driver.cpp | 29 ++-- tools/read_pro_imu.py | 149 ++++++++++++++++++ 5 files changed, 294 insertions(+), 26 deletions(-) create mode 100644 gyro_support_plan.md create mode 100644 tools/read_pro_imu.py diff --git a/.gitignore b/.gitignore index b95424e..9990210 100644 --- a/.gitignore +++ b/.gitignore @@ -7,4 +7,9 @@ debug *.egg-info hid-nintendo.c __pycache__ +Nintendo_Switch_Reverse_Engineering +nxbt +hid-nintendo.c +*.png +switch !.vscode/* diff --git a/gyro_support_plan.md b/gyro_support_plan.md new file mode 100644 index 0000000..8015b19 --- /dev/null +++ b/gyro_support_plan.md @@ -0,0 +1,49 @@ +# Gyro Support Plan + +## Checklist +- [x] Review current UART input/rumble plumbing (`switch-pico.cpp` and `controller_uart_bridge.py`) to confirm the existing 0xAA input frame (buttons/hat/sticks) and 0xBB rumble return path. +- [x] Map the current firmware IMU hooks (`switch_pro_driver.cpp`/`switch_pro_descriptors.h` `imuData` field, `is_imu_enabled` flag, report construction cadence) and note where IMU payloads should be injected. +- [x] Pull expected IMU packet format, sample cadence (3 samples/report), and unit scaling from references: `Nintendo_Switch_Reverse_Engineering/imu_sensor_notes.md`, `hid-nintendo.c`, and NXBT (`nxbt/controller/protocol.py`). +- [x] Examine GP2040-CE’s motion implementation (e.g., `GP2040-CE/src/addons/imu` and Switch report handling) for framing, calibration defaults, and rate control patterns to reuse. +- [x] Decide on UART motion framing: header/length/checksum scheme, sample packing (likely 3 samples of accel+gyro int16 LE), endian/order, and compatibility with existing 8-byte frames (avoid breaking current host builds). +- [ ] Define Switch-facing IMU payload layout inside `SwitchProReport` (axis order, sign conventions, zero/neutral sample) and ensure it matches the reverse-engineered descriptors. +- [ ] Add firmware-side data structures/buffers for incoming IMU samples (triple-buffer if mirroring Joy-Con 3-sample bursts) and default zeroing when IMU is disabled/missing. +- [ ] Extend UART parser in `switch-pico.cpp` to accept the new motion frame(s), validate checksum, and stash samples atomically alongside button state. +- [ ] Gate IMU injection on the host’s `TOGGLE_IMU` feature flag (`is_imu_enabled`) and ensure reports carry motion data only when enabled; default to zeros otherwise. +- [ ] Apply calibration/scaling constants: choose defaults from references (e.g., 0.06103 dps/LSB gyro, accel per imu_sensor_notes) and document where to adjust for sensor-specific offsets. +- [ ] Update host bridge to enable SDL sensor support (`SDL_GameControllerSetSensorEnabled`, `SDL_CONTROLLERAXIS` vs sensor events) and capture gyro (and accel if needed) at the required rate. +- [ ] Buffer and pack host IMU readings into the agreed UART motion frame, including timestamping/rate smoothing so the firmware sees stable 200 Hz-ish samples (3 per 5 ms report). +- [ ] Keep backward compatibility: allow running without IMU-capable controllers (send zero motion) and keep rumble unchanged. +- [ ] Add logging/metrics: lightweight counters for dropped/late IMU frames and a debug toggle to inspect raw samples. +- [ ] Test matrix: host-only sensor capture sanity check; loopback UART frame validator; firmware USB capture with Switch (or nxbt PC host) verifying IMU report contents and that `TOGGLE_IMU` on/off behaves; regression check that buttons/rumble remain stable. + +## Findings to date +- Firmware: `SwitchProReport.imuData[36]` exists but is always zero; `is_imu_enabled` is set only via `TOGGLE_IMU` feature report; `switch_pro_task` always sends `switch_report`, so IMU injection should happen before the memcmp/send path. +- IMU payload layout (standard 0x30/31/32/33): bytes 13-24 are accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (all Int16 LE); bytes 25-48 repeat two more samples (3 samples total, ~5 ms apart). Matches `imuData[36]` size and `hid-nintendo.c` parsing (`imu_raw_bytes` split into 3 `joycon_imu_data` structs). +- Scaling from references: accel ≈ 0.000244 G/LSB (±8000 mG), gyro ≈ 0.06103 dps/LSB (±2000 dps) or 0.070 dps/LSB with ST’s +15% headroom; Switch internally also uses rev/s conversions. Typical packet cadence ~15 ms with 3 samples (≈200 Hz sampling). +- NXBT reference: only injects IMU when `imu_enabled` flag is true; drops a 36-byte sample block at offset 14 (0-based) in the report. Example data is static; good for offset confirmation. +- GP2040-CE reference: Switch Pro driver mirrors this project—`imuData` zeroed, no motion handling yet. No reusable IMU framing, but report/keepalive cadence matches. + +## UART IMU framing decision (breaking change OK) +- New host→Pico frame (versioned) replaces the old 8-byte 0xAA packet: + - Byte0: `0xAA` header + - Byte1: `0x02` version + - Byte2: `payload_len` (44 for the layout below) + - Byte3-4: buttons LE (same masks as before) + - Byte5: hat + - Byte6-9: sticks `lx, ly, rx, ry` (0-255 as before) + - Byte10: `imu_sample_count` (host should send 3; firmware may accept 0-3) + - Byte11-46: IMU samples, 3 blocks of 12 bytes each: + - For sample i: `accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z` (all int16 LE, Pro Controller axis/sign convention) + - Byte47: checksum = (sum of bytes 0 through the last payload byte) & 0xFF +- Host behavior: always populate 3 samples per packet (~200 Hz, 5 ms spacing) with reference/default scaling; send zeros and `imu_sample_count=0` if IMU unavailable/disabled. Buttons/sticks unchanged. +- Firmware behavior: parse the new frame, validate `payload_len` and checksum, then atomically store button/stick plus up to `imu_sample_count` samples. If `is_imu_enabled` is true, copy samples into `switch_report.imuData` in the 3× sample layout; otherwise zero the IMU block. +- Axis orientation: match Pro Controller orientation (no additional flips beyond standard report ordering). + +## Open Questions +- All answered: + - Include both accelerometer and gyro (full IMU). + - Reference/default scaling is acceptable (no per-device calibration). + - Mirror 3-sample bursts (~200 Hz, 5 ms spacing). + - Use Pro Controller axis orientation/sign. + - Breaking UART framing change is acceptable (use the versioned packet above). diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index bcd4f67..b34c2f1 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -58,6 +58,8 @@ RUMBLE_SCALE = 1.0 CONTROLLER_DB_URL_DEFAULT = "https://raw.githubusercontent.com/mdqinc/SDL_GameControllerDB/refs/heads/master/gamecontrollerdb.txt" SDL_TRUE = getattr(sdl2, "SDL_TRUE", 1) +GYRO_BIAS_SAMPLES = 200 +GYRO_DEADZONE_COUNTS = 15 def parse_mapping(value: str) -> Tuple[int, str]: @@ -245,6 +247,12 @@ class ControllerContext: sensors_enabled: bool = False imu_samples: List[IMUSample] = field(default_factory=list) last_sensor_poll: float = 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 def capture_stick_offsets(controller: sdl2.SDL_GameController) -> Dict[int, int]: @@ -594,8 +602,8 @@ def build_arg_parser() -> argparse.ArgumentParser: parser.add_argument( "--frequency", type=float, - default=500.0, - help="Report send frequency per controller (Hz, default 500)", + default=1000.0, + help="Report send frequency per controller (Hz, default 1000)", ) parser.add_argument( "--deadzone", @@ -697,6 +705,11 @@ def build_arg_parser() -> argparse.ArgumentParser: default=[], help="Path to an SDL2 controller mapping database (e.g. controllerdb.txt). Repeatable.", ) + parser.add_argument( + "--debug-imu", + action="store_true", + help="Print raw IMU readings (float and converted int16) for debugging.", + ) return parser @@ -742,6 +755,7 @@ class BridgeConfig: swap_abxy_indices: set[int] swap_abxy_ids: set[str] swap_abxy_global: bool + debug_imu: bool @dataclass @@ -775,12 +789,16 @@ def read_sensor_triplet( def convert_accel_to_raw(accel_ms2: float) -> int: g_units = accel_ms2 / MS2_PER_G - return clamp_int16(g_units * 4096.0) + return clamp_int16(g_units * 4000.0) def convert_gyro_to_raw(gyro_rad: float) -> int: + # SDL reports gyroscope data in radians/second; convert to dps then to Switch counts. dps = gyro_rad * RAD_TO_DEG - return clamp_int16(dps / 0.070) + counts = clamp_int16(dps / 0.070) + if abs(counts) < GYRO_DEADZONE_COUNTS: + return 0 + return counts def initialize_controller_sensors(ctx: ControllerContext, console: Console) -> None: @@ -823,25 +841,70 @@ def initialize_controller_sensors(ctx: ControllerContext, console: Console) -> N ) -def collect_imu_sample(ctx: ControllerContext) -> None: +def collect_imu_sample(ctx: ControllerContext, config: BridgeConfig) -> None: if not ctx.sensors_enabled or SENSOR_ACCEL is None or SENSOR_GYRO is None: return accel = read_sensor_triplet(ctx.controller, SENSOR_ACCEL) gyro = read_sensor_triplet(ctx.controller, SENSOR_GYRO) if not accel or not gyro: return + if not ctx.gyro_bias_locked and ctx.gyro_bias_samples < GYRO_BIAS_SAMPLES: + ctx.gyro_bias_x += gyro[0] + ctx.gyro_bias_y += gyro[1] + ctx.gyro_bias_z += gyro[2] + ctx.gyro_bias_samples += 1 + if ctx.gyro_bias_samples >= GYRO_BIAS_SAMPLES: + ctx.gyro_bias_x /= ctx.gyro_bias_samples + ctx.gyro_bias_y /= ctx.gyro_bias_samples + ctx.gyro_bias_z /= ctx.gyro_bias_samples + ctx.gyro_bias_locked = True + + bias_x = ctx.gyro_bias_x if ctx.gyro_bias_locked else 0.0 + bias_y = ctx.gyro_bias_y if ctx.gyro_bias_locked else 0.0 + bias_z = ctx.gyro_bias_z if ctx.gyro_bias_locked else 0.0 + gyro_bias_corrected = ( + gyro[0] - bias_x, + gyro[1] - bias_y, + gyro[2] - bias_z, + ) + + # Map SDL sensor axes to Pro Controller axes: gravity should land on Z. + # print(accel[2] / MS2_PER_G) + accel_raw_x = convert_accel_to_raw(accel[0]) + accel_raw_y = convert_accel_to_raw(accel[1]) + accel_raw_z = convert_accel_to_raw(accel[2]) + gyro_raw_x = convert_gyro_to_raw(gyro[0]) + gyro_raw_y = convert_gyro_to_raw(gyro[1]) + gyro_raw_z = convert_gyro_to_raw(gyro[2]) + + # Map SDL axes to Pro axes to match the native Pro USB output: + # Pro accel: ax = SDL_, ay = SDL_Z, az = SDL_Y (gravity). + # Pro gyro: gx = SDL_X, gy = SDL_Z, gz = SDL_Y. sample = IMUSample( - accel_x=convert_accel_to_raw(accel[0]), - accel_y=convert_accel_to_raw(accel[1]), - accel_z=convert_accel_to_raw(accel[2]), - gyro_x=convert_gyro_to_raw(gyro[0]), - gyro_y=convert_gyro_to_raw(gyro[1]), - gyro_z=convert_gyro_to_raw(gyro[2]), + accel_x=-accel_raw_z, + accel_y=-accel_raw_x, + accel_z=accel_raw_y, + gyro_x=-gyro_raw_z, + gyro_y=-gyro_raw_x, + gyro_z=gyro_raw_y, ) ctx.imu_samples.append(sample) if len(ctx.imu_samples) > 6: ctx.imu_samples = ctx.imu_samples[-6:] + if config.debug_imu: + now = time.monotonic() + if now - ctx.last_debug_imu_print > 0.2: + ctx.last_debug_imu_print = now + print( + f"[IMU dbg idx={ctx.controller_index}] " + f"accel_ms2=({accel[0]:.3f},{accel[1]:.3f},{accel[2]:.3f}) " + f"gyro_dps=({gyro[0]:.3f},{gyro[1]:.3f},{gyro[2]:.3f}) " + f"bias_dps=({bias_x:.3f},{bias_y:.3f},{bias_z:.3f}) " + f"raw=({sample.accel_x},{sample.accel_y},{sample.accel_z};" + f"{sample.gyro_x},{sample.gyro_y},{sample.gyro_z})" + ) + def load_button_maps( console: Console, args: argparse.Namespace @@ -897,6 +960,7 @@ def build_bridge_config(console: Console, args: argparse.Namespace) -> BridgeCon swap_abxy_indices=swap_abxy_indices, swap_abxy_ids=set(swap_abxy_guids), # filled later once stable IDs are known swap_abxy_global=bool(args.swap_abxy), + debug_imu=bool(args.debug_imu), ) @@ -1402,7 +1466,7 @@ def service_contexts( else config.button_map_default ) poll_controller_buttons(ctx, current_button_map) - collect_imu_sample(ctx) + collect_imu_sample(ctx, config) # Reconnect UART if needed. if ctx.port and ctx.uart is None and (now - ctx.last_reopen_attempt) > 1.0: ctx.last_reopen_attempt = now diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index 031e720..ee2b899 100644 --- a/switch_pro_driver.cpp +++ b/switch_pro_driver.cpp @@ -14,8 +14,8 @@ #define LOG_PRINTF(...) ((void)0) #endif -// force a report to be sent every X ms -#define SWITCH_PRO_KEEPALIVE_TIMER 5 +// force a report to be sent every X ms (roughly matches Pro Controller cadence) +#define SWITCH_PRO_KEEPALIVE_TIMER 15 static SwitchInputState g_input_state{ false, false, false, false, @@ -195,14 +195,16 @@ static SwitchInputState make_neutral_state() { } static void fill_imu_report_data(const SwitchInputState& state) { - // Only include IMU data when the host explicitly enabled it. - if (!is_imu_enabled || state.imu_sample_count == 0) { + // Include IMU data when the host provided samples; otherwise zero. + if (state.imu_sample_count == 0) { memset(switch_report.imuData, 0x00, sizeof(switch_report.imuData)); return; } uint8_t sample_count = state.imu_sample_count > 3 ? 3 : state.imu_sample_count; uint8_t* dst = switch_report.imuData; + // Map host IMU axes (host already sends Pro-oriented X,Z,Y) into report layout: + // Report order per sample: accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z. for (uint8_t i = 0; i < 3; ++i) { SwitchImuSample sample{}; if (i < sample_count) { @@ -525,6 +527,7 @@ void switch_pro_init() { last_report_counter = 0; handshake_counter = 0; is_ready = false; + is_imu_enabled = true; // default on to allow IMU during host bring-up/debug is_initialized = false; is_report_queued = false; report_sent = false; @@ -626,14 +629,12 @@ void switch_pro_task() { switch_report.timestamp = last_report_counter; void * inputReport = &switch_report; uint16_t report_size = sizeof(switch_report); - if (memcmp(last_report, inputReport, report_size) != 0) { - if (tud_hid_ready() && send_report(0, inputReport, report_size) == true ) { - memcpy(last_report, inputReport, report_size); - report_sent = true; - } - - last_report_timer = now; + if (tud_hid_ready() && send_report(0, inputReport, report_size) == true ) { + memcpy(last_report, inputReport, report_size); + report_sent = true; } + + last_report_timer = now; } } else { if (!is_initialized) { @@ -851,9 +852,9 @@ bool tud_control_request_cb(uint8_t rhport, tusb_control_request_t const * reque void tud_mount_cb(void) { LOG_PRINTF("[USB] mount_cb\n"); last_host_activity_ms = to_ms_since_boot(get_absolute_time()); - forced_ready = false; - is_ready = false; - is_initialized = false; + forced_ready = true; + is_ready = true; + is_initialized = true; } void tud_umount_cb(void) { diff --git a/tools/read_pro_imu.py b/tools/read_pro_imu.py new file mode 100644 index 0000000..cc81ee9 --- /dev/null +++ b/tools/read_pro_imu.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +""" +Read raw IMU samples from a Nintendo Switch Pro Controller (or Pico spoof) over USB. + +Uses the `hidapi` (pyhidapi) package. Press Ctrl+C to exit. +""" + +import argparse +import struct +import sys +from typing import List, Tuple + +import hid # from pyhidapi + +DEFAULT_VENDOR_ID = 0x057E +DEFAULT_PRODUCT_ID = 0x2009 # Switch Pro Controller (USB) + + +def list_devices(filter_vid=None, filter_pid=None): + devices = hid.enumerate() + for d in devices: + if filter_vid and d["vendor_id"] != filter_vid: + continue + if filter_pid and d["product_id"] != filter_pid: + continue + print( + f"VID=0x{d['vendor_id']:04X} PID=0x{d['product_id']:04X} " + f"path={d.get('path')} " + f"serial={d.get('serial_number')} " + f"manufacturer={d.get('manufacturer_string')} " + f"product={d.get('product_string')} " + f"interface={d.get('interface_number')}" + ) + return devices + + +def find_device(vendor_id: int, product_id: int): + for dev in hid.enumerate(): + if dev["vendor_id"] == vendor_id and dev["product_id"] == product_id: + return dev + return None + + +def main(): + parser = argparse.ArgumentParser(description="Read raw 0x30 reports (IMU) from a Switch Pro Controller / Pico.") + parser.add_argument("--vid", type=lambda x: int(x, 0), default=DEFAULT_VENDOR_ID, help="Vendor ID (default 0x057E)") + parser.add_argument("--pid", type=lambda x: int(x, 0), default=DEFAULT_PRODUCT_ID, help="Product ID (default 0x2009)") + parser.add_argument("--path", help="Explicit HID path to open (overrides VID/PID).") + parser.add_argument("--count", type=int, default=0, help="Stop after this many 0x30 reports (0 = infinite).") + parser.add_argument("--timeout", type=int, default=3000, help="Read timeout ms (default 3000).") + parser.add_argument("--list", action="store_true", help="List detected HID devices and exit.") + parser.add_argument("--plot", action="store_true", help="Plot accel/gyro traces after capture (requires matplotlib).") + parser.add_argument("--save-prefix", help="If set, save accel/gyro plots as '_accel.png' and '_gyro.png'.") + args = parser.parse_args() + + if args.list: + list_devices() + return + + if args.path: + dev_info = {"path": bytes(args.path, encoding="utf-8"), "vendor_id": args.vid, "product_id": args.pid} + else: + dev_info = find_device(args.vid, args.pid) + if not dev_info: + print( + f"No HID device found for VID=0x{args.vid:04X} PID=0x{args.pid:04X}. " + "Use --list to inspect devices or --path to target a specific one.", + file=sys.stderr, + ) + sys.exit(1) + + device = hid.device() + device.open_path(dev_info["path"]) + device.set_nonblocking(False) + print( + f"Reading raw 0x30 reports from device (VID=0x{args.vid:04X} PID=0x{args.pid:04X})... " + "Ctrl+C to stop." + ) + accel_series: List[Tuple[int, int, int]] = [] + gyro_series: List[Tuple[int, int, int]] = [] + try: + read_count = 0 + while args.count == 0 or read_count < args.count: + data = device.read(64, timeout_ms=args.timeout) + if not data: + print(f"(timeout after {args.timeout} ms, no data)") + continue + if data[0] != 0x30: + print(f"(non-0x30 report id=0x{data[0]:02X}, len={len(data)})") + continue + samples = [] + offset = 13 # accel_x starts at byte 13 + for _ in range(3): + ax, ay, az, gx, gy, gz = struct.unpack_from("