switch-pico/tools/debug_imu_raw.py

#!/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()