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.
This commit is contained in:
parent
12dc3d58ee
commit
9d33bc4be7
1 changed files with 83 additions and 19 deletions
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue