diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index 3721407..c1db5b1 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -59,12 +59,8 @@ 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 = 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 +SDL_EVENT_GAMEPAD_SENSOR_UPDATE = getattr(sdl3, "SDL_EVENT_GAMEPAD_SENSOR_UPDATE", 0x658) +GYRO_BIAS_SAMPLES = 200 IMU_BUFFER_SIZE = 32 @@ -253,16 +249,12 @@ class ControllerContext: sensors_supported: bool = False sensors_enabled: bool = False imu_samples: List[IMUSample] = field(default_factory=list) - # 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) + last_accel: Tuple[float, float, float] = (0.0, 0.0, 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 @@ -330,9 +322,7 @@ 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( @@ -829,7 +819,7 @@ class BridgeConfig: swap_abxy_ids: set[str] swap_abxy_global: bool debug_imu: bool = False - no_imu: bool = True + no_imu: bool = False gyro_scale: float = 1.0 @@ -863,9 +853,7 @@ 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( @@ -956,11 +944,7 @@ 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 @@ -969,20 +953,14 @@ 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 @@ -1025,19 +1003,10 @@ 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) @@ -1129,9 +1098,7 @@ 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) @@ -1250,13 +1217,7 @@ 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]" ) @@ -1365,55 +1326,22 @@ 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 - - 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) + if ctx.gyro_bias_samples >= GYRO_BIAS_SAMPLES: + n = ctx.gyro_bias_samples ctx.gyro_bias_x /= n ctx.gyro_bias_y /= n ctx.gyro_bias_z /= n ctx.gyro_bias_locked = True - import math - 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 + 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 ux, uy, uz = gx, gy, gz ux -= bx @@ -1457,10 +1385,31 @@ def handle_sensor_update( ) +ABXY_SWAP_COMBO = frozenset({ + sdl3.SDL_GAMEPAD_BUTTON_LEFT_SHOULDER, + sdl3.SDL_GAMEPAD_BUTTON_RIGHT_SHOULDER, + sdl3.SDL_GAMEPAD_BUTTON_BACK, + sdl3.SDL_GAMEPAD_BUTTON_START, +}) + + +def _check_abxy_swap_combo( + ctx: ControllerContext, + config: BridgeConfig, + console: Console, +) -> None: + """Toggle ABXY layout when LB+RB+SELECT+START are all held.""" + if not all(ctx.button_state.get(b) for b in ABXY_SWAP_COMBO): + return + toggle_abxy_for_context(ctx, config, console) + sdl3.SDL_RumbleGamepad(ctx.controller, 0xAAAA, 0xAAAA, 200) + + def handle_button_event( event: sdl3.SDL_Event, config: BridgeConfig, contexts: Dict[int, ControllerContext], + console: Console, ) -> None: """Process button events into report/dpad state.""" ctx = contexts.get(event.gbutton.which) @@ -1483,6 +1432,8 @@ def handle_button_event( elif button in DPAD_BUTTONS: ctx.dpad[DPAD_BUTTONS[button]] = pressed ctx.report.hat = str_to_dpad(ctx.dpad) + if pressed and button in ABXY_SWAP_COMBO: + _check_abxy_swap_combo(ctx, config, console) def handle_device_added( @@ -1500,13 +1451,7 @@ 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]" ) @@ -1519,15 +1464,11 @@ 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) @@ -1628,23 +1569,12 @@ 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: - # 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() + ctx.report.imu_samples = ctx.imu_samples[:count] + ctx.imu_samples = ctx.imu_samples[count:] 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 @@ -1715,7 +1645,7 @@ def run_bridge_loop( sdl3.SDL_EVENT_GAMEPAD_BUTTON_DOWN, sdl3.SDL_EVENT_GAMEPAD_BUTTON_UP, ): - handle_button_event(event, config, contexts) + handle_button_event(event, config, contexts, console) elif event.type == SDL_EVENT_GAMEPAD_SENSOR_UPDATE: handle_sensor_update(event, contexts, config) elif event.type == sdl3.SDL_EVENT_GAMEPAD_ADDED: diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index 7e835e3..ee68f68 100644 --- a/switch_pro_driver.cpp +++ b/switch_pro_driver.cpp @@ -97,18 +97,10 @@ static const uint8_t factory_config_data[0xEFF] = { 0xFF, 0xFF, 0xFF, 0xFF, - // 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, + // 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, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, @@ -143,12 +135,7 @@ static const uint8_t factory_config_data[0xEFF] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, - // 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 + 0x50, 0xFD, 0x00, 0x00, 0xC6, 0x0F, 0x0F, 0x30, 0x61, 0xAE, 0x90, 0xD9, 0xD4, 0x14, 0x54, 0x41, 0x15, 0x54, 0xC7, 0x79, 0x9C, 0x33, 0x36, 0x63, @@ -197,26 +184,9 @@ 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) { - // 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); - } + memset(switch_report.imuData, 0x00, sizeof(switch_report.imuData)); return; } uint8_t sample_count = state.imu_sample_count > 3 ? 3 : state.imu_sample_count; @@ -353,8 +323,6 @@ 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; @@ -656,19 +624,6 @@ 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; } @@ -837,6 +792,7 @@ 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 deleted file mode 100755 index ad9b4cd..0000000 --- a/tools/debug_imu_raw.py +++ /dev/null @@ -1,205 +0,0 @@ -#!/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()