Compare commits
1 commit
feat/imu-g
...
master
| Author | SHA1 | Date | |
|---|---|---|---|
| 0fe53a53b1 |
3 changed files with 55 additions and 374 deletions
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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<uint32_t, const uint8_t*> 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 {
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
Loading…
Add table
Add a link
Reference in a new issue