From 12dc3d58ee172a76046f7592320311a083668bf6 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 17:05:56 -0600 Subject: [PATCH 01/10] chore: add raw SDL3 IMU diagnostic tool --- tools/debug_imu_raw.py | 205 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 205 insertions(+) create mode 100755 tools/debug_imu_raw.py diff --git a/tools/debug_imu_raw.py b/tools/debug_imu_raw.py new file mode 100755 index 0000000..ad9b4cd --- /dev/null +++ b/tools/debug_imu_raw.py @@ -0,0 +1,205 @@ +#!/usr/bin/env python3 +""" +Raw SDL3 IMU diagnostic tool. + +Prints every gyro/accel sensor event directly from SDL3, bypassing all +bridge logic. Use this to confirm SDL3 is delivering sensor events before +debugging conversion or axis mapping issues. + +Usage: + uv run python tools/debug_imu_raw.py + uv run python tools/debug_imu_raw.py --count 500 # stop after N gyro events + uv run python tools/debug_imu_raw.py --no-bias # skip bias calibration window +""" + +import argparse +import ctypes +import math +import sys +import time + +import sdl3 + +# --------------------------------------------------------------------------- +# Constants +# --------------------------------------------------------------------------- +SDL_SENSOR_ACCEL = 1 +SDL_SENSOR_GYRO = 2 +GRAVITY = 9.80665 # m/s² +LSB_PER_G = 4096.0 # Nintendo accel scale +LSB_PER_RAD_S = 818.5 # Nintendo gyro scale +BIAS_SAMPLES = 200 # ~1 second at 200 Hz + +def main(): + parser = argparse.ArgumentParser(description="Raw SDL3 IMU diagnostic") + parser.add_argument("--count", type=int, default=0, + help="Stop after this many gyro events (0 = run forever)") + parser.add_argument("--no-bias", action="store_true", + help="Skip bias calibration window, print raw values immediately") + parser.add_argument("--raw", action="store_true", + help="Also print converted Nintendo-native raw counts") + args = parser.parse_args() + + # Init SDL3 with gamepad + sensor support + if not sdl3.SDL_Init(sdl3.SDL_INIT_GAMEPAD | sdl3.SDL_INIT_EVENTS): + print(f"SDL_Init failed: {sdl3.SDL_GetError().decode()}", file=sys.stderr) + sys.exit(1) + + sdl3.SDL_SetGamepadEventsEnabled(True) + + # Find first gamepad + count = ctypes.c_int(0) + ids = sdl3.SDL_GetJoysticks(ctypes.byref(count)) + if not ids or count.value == 0: + print("No joysticks/gamepads found.", file=sys.stderr) + sdl3.SDL_Quit() + sys.exit(1) + + gamepad = None + instance_id = None + for i in range(count.value): + if sdl3.SDL_IsGamepad(ids[i]): + gamepad = sdl3.SDL_OpenGamepad(ids[i]) + instance_id = ids[i] + break + sdl3.SDL_free(ids) + + if not gamepad: + print("No gamepad found (only non-gamepad joysticks detected).", file=sys.stderr) + sdl3.SDL_Quit() + sys.exit(1) + + name = sdl3.SDL_GetGamepadName(gamepad) + print(f"Gamepad: {name.decode() if name else 'unknown'} (instance_id={instance_id})") + + # Check sensor support + has_accel = bool(sdl3.SDL_GamepadHasSensor(gamepad, SDL_SENSOR_ACCEL)) + has_gyro = bool(sdl3.SDL_GamepadHasSensor(gamepad, SDL_SENSOR_GYRO)) + print(f" Accelerometer supported: {has_accel}") + print(f" Gyroscope supported: {has_gyro}") + + if not (has_accel and has_gyro): + print("\nThis controller does not expose IMU sensors to SDL3.") + print("Possible reasons:") + print(" - Controller doesn't have IMU (Xbox, generic gamepads)") + print(" - Missing kernel driver (Linux: hid-nintendo not loaded)") + print(" - SDL3 HIDAPI disabled for this controller") + sdl3.SDL_CloseGamepad(gamepad) + sdl3.SDL_Quit() + sys.exit(1) + + # Enable sensors + ok_accel = bool(sdl3.SDL_SetGamepadSensorEnabled(gamepad, SDL_SENSOR_ACCEL, True)) + ok_gyro = bool(sdl3.SDL_SetGamepadSensorEnabled(gamepad, SDL_SENSOR_GYRO, True)) + print(f" Accelerometer enabled: {ok_accel}") + print(f" Gyroscope enabled: {ok_gyro}") + + if not (ok_accel and ok_gyro): + print(f"\nFailed to enable sensors: {sdl3.SDL_GetError().decode()}") + sdl3.SDL_CloseGamepad(gamepad) + sdl3.SDL_Quit() + sys.exit(1) + + print() + if args.no_bias: + print("Skipping bias calibration. Showing raw values immediately.") + else: + print(f"Hold controller STILL — collecting {BIAS_SAMPLES} gyro samples for bias calibration...") + print("Press Ctrl+C to stop.\n") + print(f"{'EVENT':<8} {'AX':>8} {'AY':>8} {'AZ':>8} {'GX':>8} {'GY':>8} {'GZ':>8} {'STATUS'}") + print("-" * 80) + + # State + last_accel = (0.0, 0.0, 0.0) + bias = [0.0, 0.0, 0.0] + bias_count = 0 + bias_locked = args.no_bias + gyro_events = 0 + last_print = time.monotonic() + event = sdl3.SDL_Event() + + try: + while True: + while sdl3.SDL_PollEvent(ctypes.byref(event)): + t = event.type + + if t == sdl3.SDL_EVENT_GAMEPAD_SENSOR_UPDATE: + gs = event.gsensor + # Only handle events from our gamepad + if gs.which != instance_id: + continue + + sensor_type = gs.sensor + d = gs.data # c_float_Array_3 + + if sensor_type == SDL_SENSOR_ACCEL: + last_accel = (float(d[0]), float(d[1]), float(d[2])) + continue + + if sensor_type != SDL_SENSOR_GYRO: + continue + + gx, gy, gz = float(d[0]), float(d[1]), float(d[2]) + + # Bias accumulation + if not bias_locked: + if bias_count < BIAS_SAMPLES: + bias[0] += gx + bias[1] += gy + bias[2] += gz + bias_count += 1 + if bias_count >= BIAS_SAMPLES: + bias = [b / BIAS_SAMPLES for b in bias] + bias_locked = True + print(f" [BIAS LOCKED] bias_rad_s=({bias[0]:.5f}, {bias[1]:.5f}, {bias[2]:.5f})\n") + continue # Don't print during calibration + + gyro_events += 1 + ax, ay, az = last_accel + ux, uy, uz = gx - bias[0], gy - bias[1], gz - bias[2] + + now = time.monotonic() + if now - last_print >= 0.1: # 10 Hz display update + last_print = now + # In m/s² and rad/s (SDL values) + status = f"events={gyro_events}" + if args.raw: + # Nintendo-native counts (reversed SDL axis mapping) + nx = int(-uz * LSB_PER_RAD_S) + ny = int(-ux * LSB_PER_RAD_S) + nz = int( uy * LSB_PER_RAD_S) + nax = int(-az / GRAVITY * LSB_PER_G) + nay = int(-ax / GRAVITY * LSB_PER_G) + naz = int( ay / GRAVITY * LSB_PER_G) + status += f" raw_g=({nax},{nay},{naz}) raw_gyro=({nx},{ny},{nz})" + print( + f"{'GYRO':<8} " + f"{ax:>8.3f} {ay:>8.3f} {az:>8.3f} " + f"{ux:>8.4f} {uy:>8.4f} {uz:>8.4f} " + f"{status}" + ) + + elif t == sdl3.SDL_EVENT_GAMEPAD_REMOVED: + print("\nGamepad disconnected.") + break + + if args.count and gyro_events >= args.count: + print(f"\nReached {args.count} gyro events. Done.") + break + + time.sleep(0.001) + + except KeyboardInterrupt: + print("\n\nStopped.") + + print(f"\nTotal gyro events received: {gyro_events}") + if bias_locked: + print(f"Final bias (rad/s): ({bias[0]:.5f}, {bias[1]:.5f}, {bias[2]:.5f})") + print(f"Bias magnitude: {math.sqrt(sum(b**2 for b in bias)):.5f} rad/s " + f"= {math.sqrt(sum(b**2 for b in bias)) * 180/math.pi:.3f} deg/s") + + sdl3.SDL_CloseGamepad(gamepad) + sdl3.SDL_Quit() + +if __name__ == "__main__": + main() From 9d33bc4be7b03971251dc10d3f6beba3b2504508 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 17:16:49 -0600 Subject: [PATCH 02/10] fix(bridge): fix gyro bias calibration corruption and zero-accel startup MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Two bugs causing constant camera drift and jarring first-frame behaviour: 1. Bias calibration was immediately collecting samples at launch, while the user is still typing / setting down the controller. This polluted the bias estimate (observed: by=-0.073 rad/s vs true ~-0.009 rad/s), causing a permanent ~4 deg/s camera drift even when the controller is held still. Fix: reject samples with gyro magnitude >= 0.5 rad/s (motion threshold) during the calibration window so only truly still samples count. Also add a return-early so no IMU is sent to the Pico until bias is locked. 2. last_accel initialised to (0,0,0), but the first gyro event fires before the first accel event. The Pico received accel=(0,0,0) on the first sample instead of the expected ~4096 counts on the gravity axis. Fix: default last_accel to (0.0, 9.80665, 0.0) — gravity on SDL Y axis, which is correct for a Pro Controller held in normal gaming position. --- .../controller_uart_bridge.py | 102 ++++++++++++++---- 1 file changed, 83 insertions(+), 19 deletions(-) diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index a5a22af..cbf4104 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -59,9 +59,15 @@ RUMBLE_MIN_ACTIVE = 0.40 # below this, rumble is treated as off/noise RUMBLE_SCALE = 1.0 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) +SDL_EVENT_GAMEPAD_SENSOR_UPDATE = getattr( + sdl3, "SDL_EVENT_GAMEPAD_SENSOR_UPDATE", 0x658 +) GYRO_BIAS_SAMPLES = 200 IMU_BUFFER_SIZE = 32 +# Gyro samples exceeding this magnitude (rad/s) during calibration are treated as +# motion and discarded. ~0.5 rad/s = ~28 deg/s covers all realistic hand tremor +# while excluding deliberate movement or controller-pickup events. +GYRO_MOTION_THRESHOLD = 0.5 def parse_mapping(value: str) -> Tuple[int, str]: @@ -249,7 +255,10 @@ class ControllerContext: sensors_supported: bool = False sensors_enabled: bool = False imu_samples: List[IMUSample] = field(default_factory=list) - last_accel: Tuple[float, float, float] = (0.0, 0.0, 0.0) + # Default: gravity on SDL Y axis (~+9.8 m/s²) — controller held horizontally. + # This prevents the first IMU sample from having zero accel before the first + # accel event arrives. + last_accel: Tuple[float, float, float] = (0.0, 9.80665, 0.0) gyro_bias_x: float = 0.0 gyro_bias_y: float = 0.0 gyro_bias_z: float = 0.0 @@ -322,7 +331,9 @@ def initialize_controller_sensors(ctx: ControllerContext, console: Console) -> N accel_enabled = sdl3.SDL_SetGamepadSensorEnabled( ctx.controller, SENSOR_ACCEL, SDL_TRUE ) - gyro_enabled = sdl3.SDL_SetGamepadSensorEnabled(ctx.controller, SENSOR_GYRO, SDL_TRUE) + gyro_enabled = sdl3.SDL_SetGamepadSensorEnabled( + ctx.controller, SENSOR_GYRO, SDL_TRUE + ) ctx.sensors_enabled = accel_enabled and gyro_enabled if not ctx.sensors_enabled: console.print( @@ -853,7 +864,9 @@ class PairingState: ignore_port_desc: List[str] = field(default_factory=list) include_port_desc: List[str] = field(default_factory=list) include_port_mfr: List[str] = field(default_factory=list) - display_index_alloc: DisplayIndexAllocator = field(default_factory=DisplayIndexAllocator) + display_index_alloc: DisplayIndexAllocator = field( + default_factory=DisplayIndexAllocator + ) def load_button_maps( @@ -944,7 +957,11 @@ def detect_controllers( if sdl3.SDL_IsGamepad(instance_id): name = sdl3.SDL_GetGamepadNameForID(instance_id) name_str = ( - name.decode() if isinstance(name, bytes) else str(name) if name else "Unknown" + name.decode() + if isinstance(name, bytes) + else str(name) + if name + else "Unknown" ) if include_controller_name and all( substr not in name_str.lower() for substr in include_controller_name @@ -953,14 +970,20 @@ def detect_controllers( f"[yellow]Skipping controller ({name_str}) due to name filter[/yellow]" ) continue - console.print(f"[cyan]Detected controller {display_counter}: ({name_str})[/cyan]") + console.print( + f"[cyan]Detected controller {display_counter}: ({name_str})[/cyan]" + ) display_counter += 1 controller_ids.append(instance_id) controller_names[instance_id] = name_str else: name = sdl3.SDL_GetJoystickNameForID(instance_id) name_str = ( - name.decode() if isinstance(name, bytes) else str(name) if name else "Unknown" + name.decode() + if isinstance(name, bytes) + else str(name) + if name + else "Unknown" ) if include_controller_name and all( substr not in name_str.lower() for substr in include_controller_name @@ -1003,10 +1026,19 @@ def list_controllers_with_guids( if is_gc else sdl3.SDL_GetJoystickNameForID(instance_id) ) - name_str = name.decode() if isinstance(name, bytes) else str(name) if name else "Unknown" + name_str = ( + name.decode() + if isinstance(name, bytes) + else str(name) + if name + else "Unknown" + ) guid_str = guid_string_for_instance_id(instance_id) table.add_row( - str(instance_id), "GameController" if is_gc else "Joystick", name_str, guid_str + str(instance_id), + "GameController" if is_gc else "Joystick", + name_str, + guid_str, ) sdl3.SDL_free(joystick_ids) console.print(table) @@ -1098,7 +1130,9 @@ def assign_port_for_index( return port_choice -def ports_in_use(pairing: PairingState, contexts: Dict[int, ControllerContext]) -> set[str]: +def ports_in_use( + pairing: PairingState, contexts: Dict[int, ControllerContext] +) -> set[str]: """Return a set of UART paths currently reserved or mapped.""" used = set(pairing.mapping_by_index.values()) used.update(ctx.port for ctx in contexts.values() if ctx.port) @@ -1217,7 +1251,13 @@ def open_initial_contexts( for instance_id in controller_indices: if not sdl3.SDL_IsGamepad(instance_id): name = sdl3.SDL_GetJoystickNameForID(instance_id) - name_str = name.decode() if isinstance(name, bytes) else str(name) if name else "Unknown" + name_str = ( + name.decode() + if isinstance(name, bytes) + else str(name) + if name + else "Unknown" + ) console.print( f"[yellow]ID {instance_id} is not a GameController ({name_str}). Trying raw open failed.[/yellow]" ) @@ -1326,7 +1366,11 @@ def handle_sensor_update( 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: + # Reject motion samples — only accumulate when controller is still. + # This prevents startup movement (typing, setting down controller) from + # corrupting the bias estimate, which would cause constant camera drift. + magnitude = (gx * gx + gy * gy + gz * gz) ** 0.5 + if magnitude < GYRO_MOTION_THRESHOLD: ctx.gyro_bias_x += gx ctx.gyro_bias_y += gy ctx.gyro_bias_z += gz @@ -1337,11 +1381,21 @@ def handle_sensor_update( ctx.gyro_bias_y /= n ctx.gyro_bias_z /= n ctx.gyro_bias_locked = True + if config.debug_imu: + import math - 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 + mag = math.sqrt( + ctx.gyro_bias_x**2 + ctx.gyro_bias_y**2 + ctx.gyro_bias_z**2 + ) + print( + f"[IMU idx={ctx.controller_index}] bias locked: " + f"({ctx.gyro_bias_x:.5f}, {ctx.gyro_bias_y:.5f}, {ctx.gyro_bias_z:.5f}) rad/s " + f"magnitude={mag:.5f} rad/s = {mag * 180 / math.pi:.2f} deg/s" + ) + # Don't send IMU until bias is locked — raw unbiased values cause drift. + return + + bx, by, bz = ctx.gyro_bias_x, ctx.gyro_bias_y, ctx.gyro_bias_z ux, uy, uz = gx, gy, gz ux -= bx @@ -1428,7 +1482,13 @@ def handle_device_added( return if not sdl3.SDL_IsGamepad(sdl_id): name = sdl3.SDL_GetJoystickNameForID(sdl_id) - name_str = name.decode() if isinstance(name, bytes) else str(name) if name else "Unknown" + name_str = ( + name.decode() + if isinstance(name, bytes) + else str(name) + if name + else "Unknown" + ) console.print( f"[yellow]Device {sdl_id} is not a GameController ({name_str}).[/yellow]" ) @@ -1441,11 +1501,15 @@ def handle_device_added( try: controller, instance_id, guid = open_controller(sdl_id) except Exception as exc: - console.print(f"[red]Hotplug open failed for controller {display_idx}: {exc}[/red]") + console.print( + f"[red]Hotplug open failed for controller {display_idx}: {exc}[/red]" + ) pairing.display_index_alloc.release(display_idx) return stable_id = guid - should_swap = display_idx in config.swap_abxy_indices or stable_id in config.swap_abxy_ids + should_swap = ( + display_idx in config.swap_abxy_indices or stable_id in config.swap_abxy_ids + ) uart = open_uart_or_warn(port, args.baud, console) if port else None if uart: uarts.append(uart) From 30e22c210c678ff2157cb2a43ca2e920a487e976 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 17:22:52 -0600 Subject: [PATCH 03/10] fix(firmware): zero SPI IMU calibration origins to prevent phantom rotation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The Switch applies its stored SPI calibration when interpreting IMU data: gyro_dps = (raw - spi_origin) * 936 / coeff The firmware had real hardware offsets as calibration origins: gyro_origin = (9, -22, -95) accel_origin = (-29, -199, 493) A real Pro Controller sensor reads those values at rest, so the Switch subtracts them to get zero. But our bridge already removes hardware bias via gyro bias calibration and sends near-zero counts when still. The Switch was then applying a second origin correction: gyro_z=2 → (2 - (-95)) * 0.070 = 6.79 dps constant yaw rotation This caused the character to spin horizontally even when holding the controller perfectly still. Fix: zero all calibration origins. The bridge handles bias correction; the Switch must not apply a second offset on top. --- switch_pro_driver.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index ee68f68..b84af59 100644 --- a/switch_pro_driver.cpp +++ b/switch_pro_driver.cpp @@ -97,10 +97,18 @@ static const uint8_t factory_config_data[0xEFF] = { 0xFF, 0xFF, 0xFF, 0xFF, - // config & calibration 1 - 0xE3, 0xFF, 0x39, 0xFF, 0xED, 0x01, 0x00, 0x40, - 0x00, 0x40, 0x00, 0x40, 0x09, 0x00, 0xEA, 0xFF, - 0xA1, 0xFF, 0x3B, 0x34, 0x3B, 0x34, 0x3B, 0x34, + // config & calibration 1 (6-axis IMU, SPI 0x6020-0x6037) + // Accel origin (0,0,0): bridge pre-corrects for bias, so Switch must not + // apply a second origin offset. Real controllers have hardware DC offsets + // here, but our emulated sensor sends bias-corrected values. + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // Accel sensitivity coeff: 0x4000 = 16384 → 4096 LSB/G (matches bridge) + 0x00, 0x40, 0x00, 0x40, 0x00, 0x40, + // Gyro origin (0,0,0): bridge removes hardware bias before sending. + // Original values (9, -22, -95) caused phantom 6.7 dps yaw when still. + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // Gyro sensitivity coeff: 0x343B = 13371 → 818.5 LSB/rad_s (matches bridge) + 0x3B, 0x34, 0x3B, 0x34, 0x3B, 0x34, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, From 3c60841d237b5c37a9e5701a9ac609add6505766 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 17:42:01 -0600 Subject: [PATCH 04/10] fix(bridge): replace flawed motion-threshold bias with warmup delay + timeout The motion threshold (0.5 rad/s) caused two bugs: 1. Normal hand tremor could exceed the threshold, so bias_samples never reached 200, bias never locked, and IMU never activated. 2. Violent shaking produced occasional near-zero samples at direction reversals that contaminated the accumulator with wrong values, locking a completely wrong bias and causing immediate fast spinning. New approach: - 1.5s warmup phase: discard all samples while the user is still interacting with the keyboard/terminal after launch. Print a message so the user knows to hold still. - Unconditional collection of 100 samples (~0.5s at 200Hz) after warmup. - 10s force-lock timeout: if 100 still samples haven't accumulated after 10s total, lock anyway with whatever we have (> 10 samples required). - Print bias quality report: magnitude > 0.05 rad/s warns the user that the controller was moving during calibration and they should restart. --- .../controller_uart_bridge.py | 62 ++++++++++++------- 1 file changed, 40 insertions(+), 22 deletions(-) diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index cbf4104..4c05ffe 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -62,12 +62,10 @@ SDL_TRUE = True SDL_EVENT_GAMEPAD_SENSOR_UPDATE = getattr( sdl3, "SDL_EVENT_GAMEPAD_SENSOR_UPDATE", 0x658 ) -GYRO_BIAS_SAMPLES = 200 +GYRO_BIAS_SAMPLES = 100 # samples to collect for bias (~0.5 s at 200 Hz) +GYRO_BIAS_WARMUP_S = 1.5 # seconds to wait before starting calibration +GYRO_BIAS_TIMEOUT_S = 10.0 # force-lock after this many seconds even if still moving IMU_BUFFER_SIZE = 32 -# Gyro samples exceeding this magnitude (rad/s) during calibration are treated as -# motion and discarded. ~0.5 rad/s = ~28 deg/s covers all realistic hand tremor -# while excluding deliberate movement or controller-pickup events. -GYRO_MOTION_THRESHOLD = 0.5 def parse_mapping(value: str) -> Tuple[int, str]: @@ -264,6 +262,7 @@ class ControllerContext: gyro_bias_z: float = 0.0 gyro_bias_samples: int = 0 gyro_bias_locked: bool = False + gyro_bias_start_time: float = 0.0 # monotonic time when calibration began last_debug_imu_print: float = 0.0 @@ -1366,32 +1365,51 @@ def handle_sensor_update( gx, gy, gz = float(data[0]), float(data[1]), float(data[2]) if not ctx.gyro_bias_locked: - # Reject motion samples — only accumulate when controller is still. - # This prevents startup movement (typing, setting down controller) from - # corrupting the bias estimate, which would cause constant camera drift. - magnitude = (gx * gx + gy * gy + gz * gz) ** 0.5 - if magnitude < GYRO_MOTION_THRESHOLD: + now = time.monotonic() + + # Track when the first gyro event arrived so we can enforce the warmup. + if ctx.gyro_bias_start_time == 0.0: + ctx.gyro_bias_start_time = now + print( + f"[IMU idx={ctx.controller_index}] Gyro bias calibration started — " + f"hold controller still for {GYRO_BIAS_WARMUP_S:.0f}s..." + ) + + elapsed = now - ctx.gyro_bias_start_time + + # Phase 1: warmup — discard all samples, just wait. + if elapsed < GYRO_BIAS_WARMUP_S: + return + + # Phase 2: collect samples unconditionally. + # Timeout: after GYRO_BIAS_TIMEOUT_S total, force-lock with whatever we have. + 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 + + force_lock = elapsed > GYRO_BIAS_TIMEOUT_S and ctx.gyro_bias_samples > 10 + + if ctx.gyro_bias_samples >= GYRO_BIAS_SAMPLES or force_lock: + n = max(ctx.gyro_bias_samples, 1) ctx.gyro_bias_x /= n ctx.gyro_bias_y /= n ctx.gyro_bias_z /= n ctx.gyro_bias_locked = True - if config.debug_imu: - import math + import math - mag = math.sqrt( - ctx.gyro_bias_x**2 + ctx.gyro_bias_y**2 + ctx.gyro_bias_z**2 - ) - print( - f"[IMU idx={ctx.controller_index}] bias locked: " - f"({ctx.gyro_bias_x:.5f}, {ctx.gyro_bias_y:.5f}, {ctx.gyro_bias_z:.5f}) rad/s " - f"magnitude={mag:.5f} rad/s = {mag * 180 / math.pi:.2f} deg/s" - ) + mag = math.sqrt( + ctx.gyro_bias_x**2 + ctx.gyro_bias_y**2 + ctx.gyro_bias_z**2 + ) + quality = ( + "OK" if mag < 0.05 else "WARN: controller was moving during calibration" + ) + print( + f"[IMU idx={ctx.controller_index}] Bias locked{' (timeout)' if force_lock else ''}: " + f"({ctx.gyro_bias_x:.5f}, {ctx.gyro_bias_y:.5f}, {ctx.gyro_bias_z:.5f}) rad/s " + f"magnitude={mag:.4f} rad/s = {mag * 180 / math.pi:.2f} deg/s [{quality}]" + ) # Don't send IMU until bias is locked — raw unbiased values cause drift. return From e7c01d111642a8f6d8375c572d7c26c7be3fa9a9 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 18:32:45 -0600 Subject: [PATCH 05/10] fix(firmware): route 0x10/0x21 output reports to subcommand handler MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The Switch sends subcommands (IMU enable, SPI reads, vibration enable, player lights, etc.) inside 0x10 and 0x21 output reports at byte 10. The firmware extracted rumble data from these reports but never routed the subcommand to handle_feature_report() — it fell through the if-else chain silently. This caused the handshake to stall: the Switch kept retrying early subcommands (0x00-0x0f cycling) because it never received ACK replies. It never progressed to sending 0x40 (Toggle IMU), 0x10 (SPI Read), 0x48 (Enable Vibration), or 0x30 (Set Player Lights). The IMU was technically sending data, but the Switch never enabled it via subcommand 0x40, so the Switch's IMU processing was undefined. Fix: after extracting rumble from 0x10/0x21 reports, also pass them to handle_feature_report() so the subcommand at buffer[10] gets processed and ACK'd. Same fix applied to both tud_hid_set_report_cb and tud_hid_report_received_cb. --- switch_pro_driver.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index b84af59..b026473 100644 --- a/switch_pro_driver.cpp +++ b/switch_pro_driver.cpp @@ -794,13 +794,18 @@ void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_ if (switchReportID == REPORT_OUTPUT_00) { // No-op, just acknowledge to clear any stalls. return; + } else if (switchReportID == REPORT_OUTPUT_10 || switchReportID == REPORT_OUTPUT_21) { + // 0x10/0x21 output reports carry rumble (bytes 2-9) AND a subcommand + // at byte 10. The Switch sends IMU enable (0x40), SPI reads (0x10), + // vibration enable (0x48), player lights (0x30), etc. via these reports. + queued_report_id = report_id; + handle_feature_report(switchReportID, switchReportSubID, buffer, bufsize); } else if (switchReportID == REPORT_FEATURE) { queued_report_id = report_id; handle_feature_report(switchReportID, switchReportSubID, buffer, bufsize); } else if (switchReportID == REPORT_CONFIGURATION) { queued_report_id = report_id; handle_config_report(switchReportID, switchReportSubID, buffer, bufsize); - } else { } } @@ -817,6 +822,9 @@ void tud_hid_report_received_cb(uint8_t instance, uint8_t report_id, uint8_t con } if (switchReportID == REPORT_OUTPUT_00) { return; + } else if (switchReportID == REPORT_OUTPUT_10 || switchReportID == REPORT_OUTPUT_21) { + queued_report_id = report_id; + handle_feature_report(switchReportID, switchReportSubID, buffer, bufsize); } else if (switchReportID == REPORT_FEATURE) { queued_report_id = report_id; handle_feature_report(switchReportID, switchReportSubID, buffer, bufsize); From 0fbb18706885b2cde745d78bfe9c7aea6370dbb3 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 18:40:58 -0600 Subject: [PATCH 06/10] fix(firmware): revert broken 0x10 subcommand routing, add diagnostic logging MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Reverts the 0x10/0x21 routing to handle_feature_report() — those reports during handshake are rumble-only keep-alives where buffer[10] is coincidental data (always 0x01), not a real subcommand. Routing them caused every report to trigger BLUETOOTH_PAIR_REQUEST. The real subcommands (TOGGLE_IMU, SPI_READ, SET_MODE, etc.) are sent via 0x80 config reports and 0x01 feature reports, which were already routed correctly. UART0 debug log now confirms is_imu_enabled=1 after handshake. Added LOG_PRINTF to handle_feature_report() showing the report ID, command ID, and is_imu_enabled state for each processed subcommand. --- switch_pro_driver.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index b026473..d7d3ba3 100644 --- a/switch_pro_driver.cpp +++ b/switch_pro_driver.cpp @@ -331,6 +331,8 @@ static void handle_feature_report(uint8_t switchReportID, uint8_t switchReportSu uint8_t spiReadSize = 0; bool canSend = false; last_host_activity_ms = to_ms_since_boot(get_absolute_time()); + LOG_PRINTF("[HID] handle_feature rid=0x%02x cmd=0x%02x imu_enabled=%d\n", + switchReportID, commandID, is_imu_enabled); report_buffer[0] = REPORT_OUTPUT_21; report_buffer[1] = last_report_counter; @@ -794,12 +796,6 @@ void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_ if (switchReportID == REPORT_OUTPUT_00) { // No-op, just acknowledge to clear any stalls. return; - } else if (switchReportID == REPORT_OUTPUT_10 || switchReportID == REPORT_OUTPUT_21) { - // 0x10/0x21 output reports carry rumble (bytes 2-9) AND a subcommand - // at byte 10. The Switch sends IMU enable (0x40), SPI reads (0x10), - // vibration enable (0x48), player lights (0x30), etc. via these reports. - queued_report_id = report_id; - handle_feature_report(switchReportID, switchReportSubID, buffer, bufsize); } else if (switchReportID == REPORT_FEATURE) { queued_report_id = report_id; handle_feature_report(switchReportID, switchReportSubID, buffer, bufsize); @@ -822,9 +818,6 @@ void tud_hid_report_received_cb(uint8_t instance, uint8_t report_id, uint8_t con } if (switchReportID == REPORT_OUTPUT_00) { return; - } else if (switchReportID == REPORT_OUTPUT_10 || switchReportID == REPORT_OUTPUT_21) { - queued_report_id = report_id; - handle_feature_report(switchReportID, switchReportSubID, buffer, bufsize); } else if (switchReportID == REPORT_FEATURE) { queued_report_id = report_id; handle_feature_report(switchReportID, switchReportSubID, buffer, bufsize); From 6884f2512176a2dd3cec44addc367a0faae1a0f7 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 18:57:26 -0600 Subject: [PATCH 07/10] fix: eliminate IMU jumping from stale FIFO samples and zero-accel startup Two root causes of camera 'wild jumping': 1. FIFO latency (bridge): Popped from the FRONT of the 32-sample FIFO, sending 145ms-stale data while fresh samples sat at the back. Movement played back on delay, making the camera feel disconnected from input. Fix: pop from the END (newest samples) and clear the entire FIFO. 2. Zero-accel startup (firmware): At boot, imuData was all zeros until the first UART frame with IMU arrived (~3-4 seconds later). The Switch interpreted zero accel as free-fall, corrupting its sensor fusion state. Fix: default imuData to a 'rest' sample (1G on accel_z, zero gyro) so the Switch always sees a valid gravity reference. --- .../controller_uart_bridge.py | 15 +++++++-- switch_pro_driver.cpp | 32 ++++++++++++++++++- 2 files changed, 44 insertions(+), 3 deletions(-) diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index 4c05ffe..32203f8 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -1628,12 +1628,23 @@ def service_contexts( if ctx.sensors_enabled and not config.no_imu: count = min(len(ctx.imu_samples), IMU_SAMPLES_PER_REPORT) if count > 0: - ctx.report.imu_samples = ctx.imu_samples[:count] - ctx.imu_samples = ctx.imu_samples[count:] + # Take the NEWEST samples, discard stale ones. + # Previously took from front (oldest) which caused + # 145ms latency when FIFO was full at 29-32 samples. + ctx.report.imu_samples = ctx.imu_samples[-count:] + ctx.imu_samples.clear() else: ctx.report.imu_samples = [] else: ctx.report.imu_samples = [] + # Debug: log actual IMU values being sent via UART + if config.debug_imu and ctx.report.imu_samples: + s = ctx.report.imu_samples[0] + if abs(s.gyro_x) > 50 or abs(s.gyro_y) > 50 or abs(s.gyro_z) > 50: + print( + f"[UART_SEND] LARGE GYRO a=({s.accel_x},{s.accel_y},{s.accel_z}) " + f"g=({s.gyro_x},{s.gyro_y},{s.gyro_z}) fifo_remaining={len(ctx.imu_samples)}" + ) ctx.uart.send_report(ctx.report) ctx.last_send = now diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index d7d3ba3..7680e8f 100644 --- a/switch_pro_driver.cpp +++ b/switch_pro_driver.cpp @@ -192,9 +192,26 @@ static std::map spi_flash_data = { static inline uint16_t scale16To12(uint16_t pos) { return pos >> 4; } +// Default "at rest" IMU sample: zero gyro, ~1G on accel Z (face-up). +// Written to imuData at boot and whenever no fresh UART data is available, +// so the Switch never sees all-zero IMU (which it interprets as free-fall). +static const uint8_t DEFAULT_IMU_SAMPLE[12] = { + 0x00, 0x00, // accel_x = 0 + 0x00, 0x00, // accel_y = 0 + 0x00, 0x10, // accel_z = 0x1000 = 4096 = 1G + 0x00, 0x00, // gyro_x = 0 + 0x00, 0x00, // gyro_y = 0 + 0x00, 0x00, // gyro_z = 0 +}; + static void fill_imu_report_data(const SwitchInputState& state) { if (state.imu_sample_count == 0) { - memset(switch_report.imuData, 0x00, sizeof(switch_report.imuData)); + // No new IMU data — fill with default "at rest" sample. + // This prevents the Switch from seeing all-zero accel (free-fall) + // during startup or when the bridge hasn't sent IMU yet. + for (int i = 0; i < 3; ++i) { + memcpy(switch_report.imuData + i * 12, DEFAULT_IMU_SAMPLE, 12); + } return; } uint8_t sample_count = state.imu_sample_count > 3 ? 3 : state.imu_sample_count; @@ -634,6 +651,19 @@ void switch_pro_task() { uint16_t report_size = sizeof(switch_report); if (tud_hid_ready() && send_report(0, inputReport, report_size) == true ) { memcpy(last_report, inputReport, report_size); + // Log IMU data being sent (throttled to ~4Hz to avoid flooding UART0) + static uint32_t last_imu_log = 0; + if (now - last_imu_log > 250) { + last_imu_log = now; + int16_t ax = (int16_t)(switch_report.imuData[0] | (switch_report.imuData[1] << 8)); + int16_t ay = (int16_t)(switch_report.imuData[2] | (switch_report.imuData[3] << 8)); + int16_t az = (int16_t)(switch_report.imuData[4] | (switch_report.imuData[5] << 8)); + int16_t gx = (int16_t)(switch_report.imuData[6] | (switch_report.imuData[7] << 8)); + int16_t gy = (int16_t)(switch_report.imuData[8] | (switch_report.imuData[9] << 8)); + int16_t gz = (int16_t)(switch_report.imuData[10] | (switch_report.imuData[11] << 8)); + LOG_PRINTF("[IMU_OUT] a=(%d,%d,%d) g=(%d,%d,%d) cnt=%d\n", + ax, ay, az, gx, gy, gz, g_input_state.imu_sample_count); + } g_input_state.imu_sample_count = 0; report_sent = true; } From 731e1d8d152307cdefdb30005a35395a109e53a7 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 19:18:37 -0600 Subject: [PATCH 08/10] fix(firmware): correct SPI 0x6080 horizontal offsets to match bridge output MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The SPI horizontal offsets at 0x6080 tell the Switch what accelerometer values to expect when the controller is held in normal gaming position. The Switch uses this as a gravity reference for its sensor fusion. Old values (-688, 0, 4038) were from a real Pro Controller's physical IMU chip. Our bridge sends ~(0, 0, 4096) through the axis reversal pipeline. The 388-count mismatch on X (0.095G = 5.4° tilt error) caused the Switch's sensor fusion to continuously fight the gyro data, trying to correct toward the wrong reference orientation → camera swinging. New values (0, 0, 4096) match the bridge's output for a still controller after SDL axis reversal, matching the zeroed calibration origins at 0x6020. --- switch_pro_driver.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index 7680e8f..7e835e3 100644 --- a/switch_pro_driver.cpp +++ b/switch_pro_driver.cpp @@ -143,7 +143,12 @@ static const uint8_t factory_config_data[0xEFF] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, - 0x50, 0xFD, 0x00, 0x00, 0xC6, 0x0F, + // Six-Axis horizontal offsets (SPI 0x6080): expected accel when held in + // gaming position. Must match the bridge's actual output for a still + // controller. Old values (-688,0,4038) were for a real Pro Controller's + // physical IMU; our bridge sends (~0,~0,~4096). The 388-count mismatch + // on X caused the Switch's sensor fusion to fight the gyro → camera swing. + 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, // (0, 0, 4096) = 1G on Z 0x0F, 0x30, 0x61, 0xAE, 0x90, 0xD9, 0xD4, 0x14, 0x54, 0x41, 0x15, 0x54, 0xC7, 0x79, 0x9C, 0x33, 0x36, 0x63, From 4f7e4a8de66dcfe9a0c0288b8533f35bf5443e3b Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 20:34:05 -0600 Subject: [PATCH 09/10] chore(bridge): disable IMU support by default IMU sensor pipeline is not yet stable; default no_imu to True so it must be explicitly opted into with --no-no-imu or a future --imu flag. Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-opencode) Co-authored-by: Sisyphus --- src/switch_pico_bridge/controller_uart_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index a5a22af..f35a386 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -819,7 +819,7 @@ class BridgeConfig: swap_abxy_ids: set[str] swap_abxy_global: bool debug_imu: bool = False - no_imu: bool = False + no_imu: bool = True gyro_scale: float = 1.0 From 0fe53a53b155ab0f62b3c702ff5d9772a1d4b346 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 20:43:32 -0600 Subject: [PATCH 10/10] feat(bridge): add controller-side ABXY swap combo (LB+RB+SELECT+START) Holding all four buttons simultaneously toggles the ABXY layout for that controller with a 200ms rumble confirmation pulse. Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-opencode) Co-authored-by: Sisyphus --- .../controller_uart_bridge.py | 25 ++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index a5a22af..c1db5b1 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -1385,10 +1385,31 @@ def handle_sensor_update( ) +ABXY_SWAP_COMBO = frozenset({ + sdl3.SDL_GAMEPAD_BUTTON_LEFT_SHOULDER, + sdl3.SDL_GAMEPAD_BUTTON_RIGHT_SHOULDER, + sdl3.SDL_GAMEPAD_BUTTON_BACK, + sdl3.SDL_GAMEPAD_BUTTON_START, +}) + + +def _check_abxy_swap_combo( + ctx: ControllerContext, + config: BridgeConfig, + console: Console, +) -> None: + """Toggle ABXY layout when LB+RB+SELECT+START are all held.""" + if not all(ctx.button_state.get(b) for b in ABXY_SWAP_COMBO): + return + toggle_abxy_for_context(ctx, config, console) + sdl3.SDL_RumbleGamepad(ctx.controller, 0xAAAA, 0xAAAA, 200) + + def handle_button_event( event: sdl3.SDL_Event, config: BridgeConfig, contexts: Dict[int, ControllerContext], + console: Console, ) -> None: """Process button events into report/dpad state.""" ctx = contexts.get(event.gbutton.which) @@ -1411,6 +1432,8 @@ def handle_button_event( elif button in DPAD_BUTTONS: ctx.dpad[DPAD_BUTTONS[button]] = pressed ctx.report.hat = str_to_dpad(ctx.dpad) + if pressed and button in ABXY_SWAP_COMBO: + _check_abxy_swap_combo(ctx, config, console) def handle_device_added( @@ -1622,7 +1645,7 @@ def run_bridge_loop( sdl3.SDL_EVENT_GAMEPAD_BUTTON_DOWN, sdl3.SDL_EVENT_GAMEPAD_BUTTON_UP, ): - handle_button_event(event, config, contexts) + handle_button_event(event, config, contexts, console) elif event.type == SDL_EVENT_GAMEPAD_SENSOR_UPDATE: handle_sensor_update(event, contexts, config) elif event.type == sdl3.SDL_EVENT_GAMEPAD_ADDED: