chore: add raw SDL3 IMU diagnostic tool
This commit is contained in:
parent
5c18c75d33
commit
12dc3d58ee
1 changed files with 205 additions and 0 deletions
205
tools/debug_imu_raw.py
Executable file
205
tools/debug_imu_raw.py
Executable file
|
|
@ -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()
|
||||
Loading…
Add table
Add a link
Reference in a new issue