fix(bridge): fix gyro bias calibration corruption and zero-accel startup

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.

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:
+                mag = math.sqrt(
-        bx, by, bz = 0.0, 0.0, 0.0
+                    ctx.gyro_bias_x**2 + ctx.gyro_bias_y**2 + ctx.gyro_bias_z**2
-    else:
+                )
-        bx, by, bz = ctx.gyro_bias_x, ctx.gyro_bias_y, ctx.gyro_bias_z
+                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)