diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index a5a22af..3721407 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/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 diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index ee68f68..7e835e3 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, @@ -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 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 { } } 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()