diff --git a/firmware/switch-pico.elf b/firmware/switch-pico.elf index 114f962..75a02cb 100755 Binary files a/firmware/switch-pico.elf and b/firmware/switch-pico.elf differ diff --git a/firmware/switch-pico.uf2 b/firmware/switch-pico.uf2 index 40a226c..a9fce9d 100644 Binary files a/firmware/switch-pico.uf2 and b/firmware/switch-pico.uf2 differ diff --git a/src/firmware/main.cpp b/src/firmware/main.cpp index d8bd086..b28a0b3 100644 --- a/src/firmware/main.cpp +++ b/src/firmware/main.cpp @@ -137,6 +137,38 @@ static void handle_uart_command(const uint8_t* frame, uint8_t length) { } } +// Controls always reflect the newest UART packet. Motion samples accumulate +// across packets (newest CONTROLLER_MOTION_SAMPLE_CAPACITY kept) until a USB +// report consumes them, so the bridge can forward each sensor sample exactly +// once and the console never integrates a stale window twice. +static void merge_uart_state(ControllerState& slot_state, + const ControllerState& parsed) { + ControllerMotionSample pending[CONTROLLER_MOTION_SAMPLE_CAPACITY]; + const uint8_t pending_count = slot_state.motion_sample_count; + memcpy(pending, slot_state.motion_samples, sizeof(pending)); + + slot_state = parsed; + const uint8_t incoming = parsed.motion_sample_count; + const uint8_t total = static_cast(pending_count + incoming); + const uint8_t kept = total > CONTROLLER_MOTION_SAMPLE_CAPACITY + ? CONTROLLER_MOTION_SAMPLE_CAPACITY + : total; + const uint8_t kept_pending = + kept > incoming ? static_cast(kept - incoming) : 0; + const uint8_t kept_incoming = static_cast(kept - kept_pending); + + uint8_t out = 0; + for (uint8_t i = static_cast(pending_count - kept_pending); + i < pending_count; ++i) { + slot_state.motion_samples[out++] = pending[i]; + } + for (uint8_t i = static_cast(incoming - kept_incoming); + i < incoming; ++i) { + slot_state.motion_samples[out++] = parsed.motion_samples[i]; + } + slot_state.motion_sample_count = kept; +} + // Consume UART bytes and forward complete frames to the Switch Pro driver. static bool poll_uart_frames() { static uint8_t buffer[64]; @@ -190,7 +222,7 @@ static bool poll_uart_frames() { ControllerState parsed{}; uint8_t slot = 0; if (switch_pro_apply_uart_packet(buffer, expected_len, parsed, slot)) { - g_user_states[slot] = parsed; + merge_uart_state(g_user_states[slot], parsed); new_data = true; LOG_PRINTF("[UART] slot=%u buttons=0x%04x hat=%u lx=%u ly=%u rx=%u ry=%u\n", slot, @@ -383,7 +415,12 @@ int main() { usb_output_driver_set_input(instance, g_user_states[instance], SWITCH_PRO_DIGITAL_TRIGGER_THRESHOLD, SWITCH_PRO_DIGITAL_TRIGGER_THRESHOLD); - (void)usb_output_driver_task(instance); + if (usb_output_driver_task(instance)) { + // Mirror the AIO backend: motion samples are integrated by + // exactly one USB report, then retired. Buttons and sticks + // persist until the next UART packet. + g_user_states[instance].motion_sample_count = 0; + } } #ifdef SWITCH_PICO_UART_USB_MANAGEMENT uart_usb_management_task(); diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index 287b1d6..f961ab5 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -19,6 +19,7 @@ from __future__ import annotations import argparse import ctypes +import math import os import sys import time @@ -303,6 +304,7 @@ class ControllerContext: gyro_bias_samples: int = 0 gyro_bias_locked: bool = False last_debug_imu_print: float = 0.0 + imu_debug_samples: int = 0 last_debug_rumble: Optional[Tuple[float, float]] = None debug_rumble_frames: int = 0 @@ -377,6 +379,14 @@ def initialize_controller_sensors(ctx: ControllerContext, console: Console) -> N console.print( f"[yellow]Controller {ctx.controller_index} failed to enable sensors[/yellow]" ) + return + if hasattr(sdl3, "SDL_GetGamepadSensorDataRate"): + accel_rate = float(sdl3.SDL_GetGamepadSensorDataRate(ctx.controller, SENSOR_ACCEL)) + gyro_rate = float(sdl3.SDL_GetGamepadSensorDataRate(ctx.controller, SENSOR_GYRO)) + console.print( + f"[cyan]Controller {ctx.controller_index} sensors enabled " + f"(SDL reports accel {accel_rate:.0f} Hz, gyro {gyro_rate:.0f} Hz)[/cyan]" + ) class HotkeyMonitor: @@ -1598,15 +1608,21 @@ def handle_sensor_update( if len(ctx.imu_samples) > IMU_SAMPLES_PER_REPORT: del ctx.imu_samples[:-IMU_SAMPLES_PER_REPORT] + ctx.imu_debug_samples += 1 if config.debug_imu: now = time.monotonic() if now - ctx.last_debug_imu_print > 0.2: + elapsed = now - ctx.last_debug_imu_print + rate = ctx.imu_debug_samples / elapsed if ctx.last_debug_imu_print else 0.0 + ctx.imu_debug_samples = 0 ctx.last_debug_imu_print = now + magnitude = math.sqrt(ax * ax + ay * ay + az * az) / MS2_PER_G print( - f"[IMU idx={ctx.controller_index}] " - f"accel_m_s2=({ax:.3f},{ay:.3f},{az:.3f}) " + f"[IMU idx={ctx.controller_index}] rate={rate:.0f}Hz " + f"accel_m_s2=({ax:.3f},{ay:.3f},{az:.3f}) |a|={magnitude:.2f}g " f"gyro_rad_s=({gx:.3f},{gy:.3f},{gz:.3f}) " f"bias_rad_s=({bx:.4f},{by:.4f},{bz:.4f}) " + f"bias_locked={ctx.gyro_bias_locked} " f"raw=({sample.accel_x},{sample.accel_y},{sample.accel_z};" f"{sample.gyro_x},{sample.gyro_y},{sample.gyro_z})" ) @@ -1724,9 +1740,11 @@ def service_link( for ctx in members: if now - ctx.last_send >= config.interval: if ctx.sensors_enabled and not config.no_imu: - # Keep publishing the latest complete sensor window. Draining - # this at the faster UART rate leaves most USB reports empty. + # Each sample goes over the wire once. The firmware pools + # pending samples until its next USB report, so a stalled + # sensor stream never gets re-integrated as motion. ctx.report.imu_samples = ctx.imu_samples + ctx.imu_samples = [] else: ctx.report.imu_samples = [] uart.send_report(ctx.report, ctx.slot) diff --git a/tests/test_controller_uart_bridge.py b/tests/test_controller_uart_bridge.py index 2b9e822..92f0a92 100644 --- a/tests/test_controller_uart_bridge.py +++ b/tests/test_controller_uart_bridge.py @@ -98,7 +98,10 @@ def test_sensor_buffer_retains_latest_three_samples() -> None: ] -def test_service_republishes_latest_imu_window(monkeypatch: MonkeyPatch) -> None: +def test_service_forwards_each_imu_window_once(monkeypatch: MonkeyPatch) -> None: + """A stalled sensor stream must not keep re-sending its last window: the + firmware pools samples per USB report, so a repeat would be integrated as + continuous rotation.""" uart = RecordingUART() controller = cast(sdl3.SDL_Gamepad, object()) ctx = bridge.ControllerContext(controller, 7, 0, "dualsense", "/dev/null") @@ -124,6 +127,8 @@ def test_service_republishes_latest_imu_window(monkeypatch: MonkeyPatch) -> None console = Console(file=StringIO()) bridge.service_contexts(1.0, args, config, contexts, links, console) bridge.service_contexts(2.0, args, config, contexts, links, console) + ctx.imu_samples.append(IMUSample(19, 20, 21, 22, 23, 24)) + bridge.service_contexts(3.0, args, config, contexts, links, console) - assert uart.sent_imu == [tuple(samples), tuple(samples)] - assert ctx.imu_samples == samples + assert uart.sent_imu == [tuple(samples), (), (IMUSample(19, 20, 21, 22, 23, 24),)] + assert ctx.imu_samples == []