Merge branch 'feat/imu-gyro-support' of github.com:jyapayne/switch-pico into feat/imu-gyro-support

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 <clio-agent@sisyphuslabs.ai>

src/switch_pico_bridge/controller_uart_bridge.py

@@ -59,8 +59,12 @@ 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)
-GYRO_BIAS_SAMPLES = 200
+SDL_EVENT_GAMEPAD_SENSOR_UPDATE = getattr(
+    sdl3, "SDL_EVENT_GAMEPAD_SENSOR_UPDATE", 0x658
+)
+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
 
 
@@ -249,12 +253,16 @@ 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
     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
 
 
@@ -322,7 +330,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(
@@ -819,7 +829,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
 
 
@@ -853,7 +863,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 +956,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 +969,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 +1025,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 +1129,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 +1250,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,22 +1365,55 @@ def handle_sensor_update(
     gx, gy, gz = float(data[0]), float(data[1]), float(data[2])
 
     if not ctx.gyro_bias_locked:
+        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
+            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
+            )
+            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
+
+    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 +1500,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 +1519,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)
@@ -1546,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
 

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,
 
@@ -135,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,
@@ -184,9 +197,26 @@ static std::map<uint32_t, const uint8_t*> 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;
@@ -323,6 +353,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;
@@ -624,6 +656,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;
             }
@@ -792,7 +837,6 @@ void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_
     } else if (switchReportID == REPORT_CONFIGURATION) {
         queued_report_id = report_id;
         handle_config_report(switchReportID, switchReportSubID, buffer, bufsize);
-    } else {
     }
 }
 

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()