Consume UART motion samples once per USB report like the AIO

The bridge now forwards each SDL sensor sample once instead of re-sending
its last window at 500 Hz, and the firmware pools pending samples per slot
until a USB report retires them. A stalled or slow sensor stream can no
longer be re-integrated as continuous rotation in quaternion mode.
--debug-imu reports the sample rate, |a| in g and bias lock state.

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<uint8_t>(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<uint8_t>(kept - incoming) : 0;
+    const uint8_t kept_incoming = static_cast<uint8_t>(kept - kept_pending);
+
+    uint8_t out = 0;
+    for (uint8_t i = static_cast<uint8_t>(pending_count - kept_pending);
+         i < pending_count; ++i) {
+        slot_state.motion_samples[out++] = pending[i];
+    }
+    for (uint8_t i = static_cast<uint8_t>(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();

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)

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 == []