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)