feat(bridge): add controller-side ABXY swap combo (LB+RB+SELECT+START)

Holding all four buttons simultaneously toggles the ABXY layout for that controller with a 200ms rumble confirmation pulse.

Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-opencode)

Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>

.gitignore

@@ -7,9 +7,4 @@ debug
 *.egg-info
 hid-nintendo.c
 __pycache__
-Nintendo_Switch_Reverse_Engineering
-nxbt
-hid-nintendo.c
-*.png
-switch
 !.vscode/*

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 Joey Yakimowich-Payne
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -15,10 +15,8 @@ Raspberry Pi Pico firmware that emulates a Switch Pro controller over USB and a
 5. Connect the Pico to the Switch (dock USB-A or USB-C OTG); the Switch should see it as a wired Pro Controller.
 
 ## Planned features
-- IMU support for motion controls (gyro/accelerometer passthrough for controllers that support it to the Switch).
 
 ## Limitations
-- No motion controls/IMU passthrough yet (planned).
 - No NFC/amiibo/IR support.
 - Rumble is best-effort: it depends on the Switch sending rumble and SDL2 being able to drive haptics on your specific controller.
 - Requires a host computer running the bridge; the Pico is not a Bluetooth/USB host for controllers.
@@ -188,6 +186,9 @@ Options:
 - `--swap-abxy-guid GUID` (repeatable) to flip AB/XY for a specific physical controller (GUID is stable across runs).
 - `--swap-hotkey x` to pick the runtime hotkey that prompts you to toggle ABXY layout for a specific connected controller (default `x`; empty string disables).
 - `--sdl-mapping path/to/gamecontrollerdb.txt` to load extra SDL mappings (defaults to `switch_pico_bridge/controller_db/gamecontrollerdb.txt`).
+- `--debug-imu` to print raw gyroscope and accelerometer readings every ~200ms (useful for verifying sensor data and troubleshooting).
+- `--no-imu` to disable sensor reading entirely (useful for controllers without gyro, or if motion causes issues).
+- `--gyro-scale FLOAT` to adjust gyroscope sensitivity (default 1.0; reduce below 1.0 if camera rotates too fast; increase above 1.0 for more sensitivity).
 
 ### Runtime hotkeys
 - By default, pressing `z` in the terminal re-samples every connected controller's sticks and re-applies neutral offsets. Change/disable with `--zero-hotkey`.
@@ -228,6 +229,34 @@ with SwitchUARTClient("/dev/cu.usbserial-0001") as client:
 ### Linux tips
 - You may need udev permissions for `/dev/ttyUSB*`/`/dev/ttyACM*` (add user to `dialout`/`uucp` or use `udev` rules).
 
+## IMU / Motion Controls
+
+The bridge supports gyroscope and accelerometer passthrough from controllers that have motion sensors (e.g. the Nintendo Switch Pro Controller, DualSense). Motion data is forwarded to the Pico which injects it into the emulated Switch Pro Controller's HID reports.
+
+### Requirements
+- A controller with gyro/accelerometer support (SDL2 must be able to enable sensors on it).
+- The Switch will automatically use motion data once the controller is recognised as a Pro Controller.
+
+### Gyro bias calibration
+On startup, the bridge collects the first 200 gyro readings while the controller is stationary and averages them to compute a per-axis bias (zero-rate offset). Gyro output is zeroed during this ~1 second calibration window, then bias is subtracted from all subsequent readings. Keep the controller still when starting the bridge for best results. Use `--no-gyro-bias` to skip calibration and use raw values directly.
+
+### CLI flags
+- `--debug-imu`: Print raw sensor values (m/s² and rad/s) and converted Switch integer counts every ~200ms. Useful for verifying the sensor is detected and producing sensible data.
+- `--no-imu`: Disable IMU entirely. The bridge sends zero motion data to the Pico, which sends zero-filled IMU bytes to the Switch. Buttons and sticks are unaffected.
+- `--gyro-scale FLOAT` (default 1.0): Multiply all gyro values by this factor before sending. Reduce below 1.0 if the camera moves too fast; increase above 1.0 for more sensitivity.
+
+### Troubleshooting
+- **Gyro not detected**: Run with `--debug-imu`. If no IMU readings appear, SDL2 cannot see sensors on your controller (may not be supported or driver issue). On Linux, the `hid-nintendo` kernel driver routes Pro Controller IMU to a separate evdev device that SDL2 cannot read; use Windows or macOS for gyro passthrough.
+- **Wild camera swinging**: Start with `--gyro-scale 0.3` and increase gradually. Ensure the controller is still during the first second of startup (bias calibration).
+- **Verifying Pico output**: Use `python tools/read_pro_imu.py --vid 0x057E --pid 0x2009` to read raw IMU bytes directly from the Pico's USB HID output and confirm non-zero values appear.
+- **SDL2 accuracy**: SDL2 (version < 2.32.7) has a known inaccuracy bug with Switch Pro Controller gyro data. Updating the SDL2 shared library to 2.32.7 or later improves accuracy.
+
+## References
+- GP2040-CE (controller firmware ecosystem): https://github.com/OpenStickCommunity/GP2040-CE
+- nxbt (Switch controller research/tools): https://github.com/Brikwerk/nxbt
+- Nintendo Switch Reverse Engineering notes: https://github.com/dekuNukem/Nintendo_Switch_Reverse_Engineering
+- `hid-nintendo` driver reference: https://github.com/DanielOgorchock/linux/blob/ogorchock/drivers/hid/hid-nintendo.c
+
 ## Troubleshooting
 - **No input on Switch**: verify UART wiring (Pico GPIO4/5), baud matches both sides, Pico flashed with current firmware, and `Pro Controller Wired Communication` is enabled on the Switch.
 - **Constant buzzing rumble**: the bridge filters small rumble payloads; ensure baud isn’t dropping bytes. Try lowering rumble scale in `switch_pico_bridge.controller_uart_bridge` if needed.

gyro_support_plan.md

@@ -1,49 +0,0 @@
-# Gyro Support Plan
-
-## Checklist
-- [x] Review current UART input/rumble plumbing (`switch-pico.cpp` and `controller_uart_bridge.py`) to confirm the existing 0xAA input frame (buttons/hat/sticks) and 0xBB rumble return path.
-- [x] Map the current firmware IMU hooks (`switch_pro_driver.cpp`/`switch_pro_descriptors.h` `imuData` field, `is_imu_enabled` flag, report construction cadence) and note where IMU payloads should be injected.
-- [x] Pull expected IMU packet format, sample cadence (3 samples/report), and unit scaling from references: `Nintendo_Switch_Reverse_Engineering/imu_sensor_notes.md`, `hid-nintendo.c`, and NXBT (`nxbt/controller/protocol.py`).
-- [x] Examine GP2040-CE’s motion implementation (e.g., `GP2040-CE/src/addons/imu` and Switch report handling) for framing, calibration defaults, and rate control patterns to reuse.
-- [x] Decide on UART motion framing: header/length/checksum scheme, sample packing (likely 3 samples of accel+gyro int16 LE), endian/order, and compatibility with existing 8-byte frames (avoid breaking current host builds).
-- [ ] Define Switch-facing IMU payload layout inside `SwitchProReport` (axis order, sign conventions, zero/neutral sample) and ensure it matches the reverse-engineered descriptors.
-- [ ] Add firmware-side data structures/buffers for incoming IMU samples (triple-buffer if mirroring Joy-Con 3-sample bursts) and default zeroing when IMU is disabled/missing.
-- [ ] Extend UART parser in `switch-pico.cpp` to accept the new motion frame(s), validate checksum, and stash samples atomically alongside button state.
-- [ ] Gate IMU injection on the host’s `TOGGLE_IMU` feature flag (`is_imu_enabled`) and ensure reports carry motion data only when enabled; default to zeros otherwise.
-- [ ] Apply calibration/scaling constants: choose defaults from references (e.g., 0.06103 dps/LSB gyro, accel per imu_sensor_notes) and document where to adjust for sensor-specific offsets.
-- [ ] Update host bridge to enable SDL sensor support (`SDL_GameControllerSetSensorEnabled`, `SDL_CONTROLLERAXIS` vs sensor events) and capture gyro (and accel if needed) at the required rate.
-- [ ] Buffer and pack host IMU readings into the agreed UART motion frame, including timestamping/rate smoothing so the firmware sees stable 200 Hz-ish samples (3 per 5 ms report).
-- [ ] Keep backward compatibility: allow running without IMU-capable controllers (send zero motion) and keep rumble unchanged.
-- [ ] Add logging/metrics: lightweight counters for dropped/late IMU frames and a debug toggle to inspect raw samples.
-- [ ] Test matrix: host-only sensor capture sanity check; loopback UART frame validator; firmware USB capture with Switch (or nxbt PC host) verifying IMU report contents and that `TOGGLE_IMU` on/off behaves; regression check that buttons/rumble remain stable.
-
-## Findings to date
-- Firmware: `SwitchProReport.imuData[36]` exists but is always zero; `is_imu_enabled` is set only via `TOGGLE_IMU` feature report; `switch_pro_task` always sends `switch_report`, so IMU injection should happen before the memcmp/send path.
-- IMU payload layout (standard 0x30/31/32/33): bytes 13-24 are accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z (all Int16 LE); bytes 25-48 repeat two more samples (3 samples total, ~5 ms apart). Matches `imuData[36]` size and `hid-nintendo.c` parsing (`imu_raw_bytes` split into 3 `joycon_imu_data` structs).
-- Scaling from references: accel ≈ 0.000244 G/LSB (±8000 mG), gyro ≈ 0.06103 dps/LSB (±2000 dps) or 0.070 dps/LSB with ST’s +15% headroom; Switch internally also uses rev/s conversions. Typical packet cadence ~15 ms with 3 samples (≈200 Hz sampling).
-- NXBT reference: only injects IMU when `imu_enabled` flag is true; drops a 36-byte sample block at offset 14 (0-based) in the report. Example data is static; good for offset confirmation.
-- GP2040-CE reference: Switch Pro driver mirrors this project—`imuData` zeroed, no motion handling yet. No reusable IMU framing, but report/keepalive cadence matches.
-
-## UART IMU framing decision (breaking change OK)
-- New host→Pico frame (versioned) replaces the old 8-byte 0xAA packet:
-  - Byte0: `0xAA` header
-  - Byte1: `0x02` version
-  - Byte2: `payload_len` (44 for the layout below)
-  - Byte3-4: buttons LE (same masks as before)
-  - Byte5: hat
-  - Byte6-9: sticks `lx, ly, rx, ry` (0-255 as before)
-  - Byte10: `imu_sample_count` (host should send 3; firmware may accept 0-3)
-  - Byte11-46: IMU samples, 3 blocks of 12 bytes each:
-    - For sample i: `accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z` (all int16 LE, Pro Controller axis/sign convention)
-  - Byte47: checksum = (sum of bytes 0 through the last payload byte) & 0xFF
-- Host behavior: always populate 3 samples per packet (~200 Hz, 5 ms spacing) with reference/default scaling; send zeros and `imu_sample_count=0` if IMU unavailable/disabled. Buttons/sticks unchanged.
-- Firmware behavior: parse the new frame, validate `payload_len` and checksum, then atomically store button/stick plus up to `imu_sample_count` samples. If `is_imu_enabled` is true, copy samples into `switch_report.imuData` in the 3× sample layout; otherwise zero the IMU block.
-- Axis orientation: match Pro Controller orientation (no additional flips beyond standard report ordering).
-
-## Open Questions
-- All answered:
-  - Include both accelerometer and gyro (full IMU).
-  - Reference/default scaling is acceptable (no per-device calibration).
-  - Mirror 3-sample bursts (~200 Hz, 5 ms spacing).
-  - Use Pro Controller axis orientation/sign.
-  - Breaking UART framing change is acceptable (use the versioned packet above).

pyproject.toml

@@ -11,7 +11,7 @@ requires-python = ">=3.9"
 authors = [{ name = "Switch Pico Maintainers" }]
 dependencies = [
     "pyserial",
-    "pysdl2",
+    "PySDL3",
     "rich",
 ]
 

src/switch_pico_bridge/switch_pico_uart.py

@@ -12,6 +12,7 @@ depending on SDL. It mirrors the framing in ``switch-pico.cpp``:
 
 from __future__ import annotations
 
+import math
 import struct
 import time
 import threading
@@ -23,9 +24,28 @@ import serial
 from serial.tools import list_ports, list_ports_common
 
 UART_HEADER = 0xAA
+UART_PROTOCOL_VERSION = 0x02
 RUMBLE_HEADER = 0xBB
 RUMBLE_TYPE_RUMBLE = 0x01
 UART_BAUD = 921600
+IMU_SAMPLES_PER_REPORT = 3
+
+MS2_PER_G = 9.80665
+RAD_TO_DEG = 180.0 / math.pi
+ACCEL_LSB_PER_G = 4096.0
+GYRO_LSB_PER_RAD_S = 818.5
+
+try:
+    import sdl3 as _sdl3  # type: ignore[import-not-found]
+
+    _sensor_accel = getattr(_sdl3, "SDL_SENSOR_ACCEL", 1)
+    _sensor_gyro = getattr(_sdl3, "SDL_SENSOR_GYRO", 2)
+except ImportError:
+    _sensor_accel = 1
+    _sensor_gyro = 2
+
+SENSOR_ACCEL: int = _sensor_accel
+SENSOR_GYRO: int = _sensor_gyro
 
 
 class SwitchButton(IntFlag):
@@ -62,9 +82,9 @@ def _is_usb_serial_path(path: str) -> bool:
     """Heuristic for USB serial path prefixes."""
     lower = path.lower()
     usb_prefixes = (
-        "/dev/ttyusb",   # Linux USB serial
+        "/dev/ttyusb",  # Linux USB serial
-        "/dev/ttyacm",   # Linux CDC ACM
+        "/dev/ttyacm",  # Linux CDC ACM
-        "/dev/cu.usb",   # macOS cu/tty USB adapters
+        "/dev/cu.usb",  # macOS cu/tty USB adapters
         "/dev/tty.usb",
     )
     if lower.startswith(usb_prefixes):
@@ -144,6 +164,7 @@ def first_serial_port(
         return None
     return ports[0]["device"]
 
+
 def clamp_byte(value: Union[int, float]) -> int:
     """Clamp a numeric value to the 0-255 byte range."""
     return max(0, min(255, int(value)))
@@ -202,6 +223,21 @@ def str_to_dpad(flags: Mapping[str, bool]) -> SwitchDpad:
     return SwitchDpad.CENTER
 
 
+def compute_checksum(data: bytes) -> int:
+    """Compute UART checksum as sum of bytes modulo 256."""
+    return sum(data) & 0xFF
+
+
+@dataclass
+class IMUSample:
+    accel_x: int = 0
+    accel_y: int = 0
+    accel_z: int = 0
+    gyro_x: int = 0
+    gyro_y: int = 0
+    gyro_z: int = 0
+
+
 @dataclass
 class SwitchReport:
     buttons: int = 0
@@ -210,13 +246,38 @@ class SwitchReport:
     ly: int = 128
     rx: int = 128
     ry: int = 128
+    imu_samples: List[IMUSample] = field(default_factory=list)
 
     def to_bytes(self) -> bytes:
-        """Serialize the report into the UART packet format."""
+        """Serialize the report into UART v2 framed packet format."""
-        return struct.pack(
+        count = min(len(self.imu_samples), IMU_SAMPLES_PER_REPORT)
-            "<BHBBBBB", UART_HEADER, self.buttons & 0xFFFF, self.hat & 0xFF, self.lx, self.ly, self.rx, self.ry
+        payload = struct.pack(
+            "<HBBBBBB",
+            self.buttons & 0xFFFF,
+            int(self.hat) & 0xFF,
+            clamp_byte(self.lx),
+            clamp_byte(self.ly),
+            clamp_byte(self.rx),
+            clamp_byte(self.ry),
+            count,
         )
 
+        for i in range(count):
+            sample = self.imu_samples[i]
+            payload += struct.pack(
+                "<hhhhhh",
+                max(-32768, min(32767, int(sample.accel_x))),
+                max(-32768, min(32767, int(sample.accel_y))),
+                max(-32768, min(32767, int(sample.accel_z))),
+                max(-32768, min(32767, int(sample.gyro_x))),
+                max(-32768, min(32767, int(sample.gyro_y))),
+                max(-32768, min(32767, int(sample.gyro_z))),
+            )
+
+        payload_len = len(payload)
+        frame = bytes([UART_HEADER, UART_PROTOCOL_VERSION, payload_len]) + payload
+        return frame + bytes([compute_checksum(frame)])
+
 
 class PicoUART:
     def __init__(self, port: str, baudrate: int = UART_BAUD) -> None:
@@ -267,15 +328,15 @@ class PicoUART:
                     del self._buffer[:start]
                 return None
 
-            frame = self._buffer[start:start + 11]
+            frame = self._buffer[start : start + 11]
-            checksum = sum(frame[:10]) & 0xFF
+            checksum = compute_checksum(bytes(frame[:10]))
 
             if frame[1] == RUMBLE_TYPE_RUMBLE and checksum == frame[10]:
                 payload = bytes(frame[2:10])
-                del self._buffer[:start + 11]
+                del self._buffer[: start + 11]
                 return payload
 
-            del self._buffer[:start + 1]
+            del self._buffer[: start + 1]
 
     def close(self) -> None:
         """Close the UART connection."""
@@ -434,14 +495,20 @@ class SwitchUARTClient:
         self.state.move_right_stick(x, y)
         self.send()
 
-    def press_for(self, duration: float, *buttons: SwitchButton | SwitchDpad | int) -> None:
+    def press_for(
+        self, duration: float, *buttons: SwitchButton | SwitchDpad | int
+    ) -> None:
         """Press buttons/hat for a duration, then release."""
         self.press(*buttons)
         time.sleep(max(0.0, duration))
         self.release(*buttons)
 
     def move_left_stick_for(
-        self, x: Union[int, float], y: Union[int, float], duration: float, neutral_after: bool = True
+        self,
+        x: Union[int, float],
+        y: Union[int, float],
+        duration: float,
+        neutral_after: bool = True,
     ) -> None:
         """Move left stick for a duration, optionally returning it to neutral afterward."""
         self.move_left_stick(x, y)
@@ -451,7 +518,11 @@ class SwitchUARTClient:
             self.send()
 
     def move_right_stick_for(
-        self, x: Union[int, float], y: Union[int, float], duration: float, neutral_after: bool = True
+        self,
+        x: Union[int, float],
+        y: Union[int, float],
+        duration: float,
+        neutral_after: bool = True,
     ) -> None:
         """Move right stick for a duration, optionally returning it to neutral afterward."""
         self.move_right_stick(x, y)

switch-pico.cpp

@@ -1,4 +1,4 @@
-ginclude <stdio.h>
+#include <stdio.h>
 #include <string.h>
 #include "bsp/board.h"
 #include "hardware/uart.h"
@@ -86,21 +86,26 @@ static bool poll_uart_frames() {
             }
         }
 
+        if (index >= sizeof(buffer)) {
+            index = 0;
+            expected_len = 0;
+        }
+
         buffer[index++] = byte;
         if (index == 3) {
-            // We just stored payload_len; compute expected frame length (payload + header/version/len/checksum).
+            expected_len = static_cast<uint8_t>(buffer[2] + 4u);
-            expected_len = static_cast<uint8_t>(buffer[2] + 4);
+            if (expected_len < 12 || expected_len > sizeof(buffer)) {
-            if (expected_len > sizeof(buffer) || expected_len < 8) {
                 index = 0;
                 expected_len = 0;
                 continue;
             }
         }
 
-        if (expected_len && index >= expected_len) {
+        if (expected_len > 0 && index >= expected_len) {
             SwitchInputState parsed{};
             if (switch_pro_apply_uart_packet(buffer, expected_len, &parsed)) {
                 g_user_state = parsed;
+                new_data = true;
                 LOG_PRINTF("[UART] packet buttons=0x%04x hat=%u lx=%u ly=%u rx=%u ry=%u\n",
                            (parsed.button_a   ? SWITCH_PRO_MASK_A   : 0) |
                            (parsed.button_b   ? SWITCH_PRO_MASK_B   : 0) |
@@ -117,16 +122,16 @@ static bool poll_uart_frames() {
                            (parsed.button_l3  ? SWITCH_PRO_MASK_L3  : 0) |
                            (parsed.button_r3  ? SWITCH_PRO_MASK_R3  : 0),
                            parsed.dpad_up ? SWITCH_PRO_HAT_UP :
-                           parsed.dpad_down ? SWITCH_PRO_HAT_DOWN :
+                            parsed.dpad_down ? SWITCH_PRO_HAT_DOWN :
-                           parsed.dpad_left ? SWITCH_PRO_HAT_LEFT :
+                            parsed.dpad_left ? SWITCH_PRO_HAT_LEFT :
-                           parsed.dpad_right ? SWITCH_PRO_HAT_RIGHT : SWITCH_PRO_HAT_NOTHING,
+                            parsed.dpad_right ? SWITCH_PRO_HAT_RIGHT : SWITCH_PRO_HAT_NOTHING,
-                           parsed.lx >> 8, parsed.ly >> 8, parsed.rx >> 8, parsed.ry >> 8);
+                            parsed.lx >> 8, parsed.ly >> 8, parsed.rx >> 8, parsed.ry >> 8);
             }
             index = 0;
             expected_len = 0;
-            new_data = true;
         }
     }
+
     return new_data;
 }
 
@@ -163,12 +168,9 @@ int main() {
     while (true) {
         tud_task();          // USB device tasks
         bool new_data = poll_uart_frames();  // Pull controller state from UART1
+        (void)new_data;
         SwitchInputState state = g_user_state;
         switch_pro_set_input(state);
-        bool should_update = new_data;
-        if (should_update) {
-            switch_pro_set_input(state);
-        }
         switch_pro_task();   // Push state to the Switch host
         log_usb_state();
     }

switch_pro_driver.cpp

@@ -14,8 +14,8 @@
 #define LOG_PRINTF(...) ((void)0)
 #endif
 
-// force a report to be sent every X ms (roughly matches Pro Controller cadence)
+// force a report to be sent every X ms
-#define SWITCH_PRO_KEEPALIVE_TIMER 15
+#define SWITCH_PRO_KEEPALIVE_TIMER 5
 
 static SwitchInputState g_input_state{
     false, false, false, false,
@@ -184,6 +184,32 @@ static std::map<uint32_t, const uint8_t*> spi_flash_data = {
 
 static inline uint16_t scale16To12(uint16_t pos) { return pos >> 4; }
 
+static void fill_imu_report_data(const SwitchInputState& state) {
+    if (state.imu_sample_count == 0) {
+        memset(switch_report.imuData, 0x00, sizeof(switch_report.imuData));
+        return;
+    }
+    uint8_t sample_count = state.imu_sample_count > 3 ? 3 : state.imu_sample_count;
+    // If fewer than 3 samples, duplicate the last one to fill all 3 slots
+    uint8_t* dst = switch_report.imuData;
+    for (uint8_t i = 0; i < 3; ++i) {
+        const SwitchImuSample& s = (i < sample_count) ? state.imu_samples[i] : state.imu_samples[sample_count - 1];
+        dst[0]  = static_cast<uint8_t>(s.accel_x & 0xFF);
+        dst[1]  = static_cast<uint8_t>((s.accel_x >> 8) & 0xFF);
+        dst[2]  = static_cast<uint8_t>(s.accel_y & 0xFF);
+        dst[3]  = static_cast<uint8_t>((s.accel_y >> 8) & 0xFF);
+        dst[4]  = static_cast<uint8_t>(s.accel_z & 0xFF);
+        dst[5]  = static_cast<uint8_t>((s.accel_z >> 8) & 0xFF);
+        dst[6]  = static_cast<uint8_t>(s.gyro_x & 0xFF);
+        dst[7]  = static_cast<uint8_t>((s.gyro_x >> 8) & 0xFF);
+        dst[8]  = static_cast<uint8_t>(s.gyro_y & 0xFF);
+        dst[9]  = static_cast<uint8_t>((s.gyro_y >> 8) & 0xFF);
+        dst[10] = static_cast<uint8_t>(s.gyro_z & 0xFF);
+        dst[11] = static_cast<uint8_t>((s.gyro_z >> 8) & 0xFF);
+        dst += 12;
+    }
+}
+
 static SwitchInputState make_neutral_state() {
     SwitchInputState s{};
     s.lx = SWITCH_PRO_JOYSTICK_MID;
@@ -194,38 +220,6 @@ static SwitchInputState make_neutral_state() {
     return s;
 }
 
-static void fill_imu_report_data(const SwitchInputState& state) {
-    // Include IMU data when the host provided samples; otherwise zero.
-    if (state.imu_sample_count == 0) {
-        memset(switch_report.imuData, 0x00, sizeof(switch_report.imuData));
-        return;
-    }
-
-    uint8_t sample_count = state.imu_sample_count > 3 ? 3 : state.imu_sample_count;
-    uint8_t* dst = switch_report.imuData;
-    // Map host IMU axes (host already sends Pro-oriented X,Z,Y) into report layout:
-    // Report order per sample: accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z.
-    for (uint8_t i = 0; i < 3; ++i) {
-        SwitchImuSample sample{};
-        if (i < sample_count) {
-            sample = state.imu_samples[i];
-        }
-        dst[0] = static_cast<uint8_t>(sample.accel_x & 0xFF);
-        dst[1] = static_cast<uint8_t>((sample.accel_x >> 8) & 0xFF);
-        dst[2] = static_cast<uint8_t>(sample.accel_y & 0xFF);
-        dst[3] = static_cast<uint8_t>((sample.accel_y >> 8) & 0xFF);
-        dst[4] = static_cast<uint8_t>(sample.accel_z & 0xFF);
-        dst[5] = static_cast<uint8_t>((sample.accel_z >> 8) & 0xFF);
-        dst[6] = static_cast<uint8_t>(sample.gyro_x & 0xFF);
-        dst[7] = static_cast<uint8_t>((sample.gyro_x >> 8) & 0xFF);
-        dst[8] = static_cast<uint8_t>(sample.gyro_y & 0xFF);
-        dst[9] = static_cast<uint8_t>((sample.gyro_y >> 8) & 0xFF);
-        dst[10] = static_cast<uint8_t>(sample.gyro_z & 0xFF);
-        dst[11] = static_cast<uint8_t>((sample.gyro_z >> 8) & 0xFF);
-        dst += 12;
-    }
-}
-
 static void send_identify() {
     memset(report_buffer, 0x00, sizeof(report_buffer));
     report_buffer[0] = REPORT_USB_INPUT_81;
@@ -527,7 +521,6 @@ void switch_pro_init() {
     last_report_counter = 0;
     handshake_counter = 0;
     is_ready = false;
-    is_imu_enabled = true; // default on to allow IMU during host bring-up/debug
     is_initialized = false;
     is_report_queued = false;
     report_sent = false;
@@ -631,10 +624,8 @@ 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);
-                report_sent = true;
-                // Clear IMU samples so they aren't repeated in the next report
-                // if no new data arrives.
                 g_input_state.imu_sample_count = 0;
+                report_sent = true;
             }
 
             last_report_timer = now;
@@ -653,41 +644,32 @@ void switch_pro_task() {
 }
 
 bool switch_pro_apply_uart_packet(const uint8_t* packet, uint8_t length, SwitchInputState* out_state) {
-    // Packet v2 format:
+    // v2 format: 0xAA + 0x02 + payload_len + payload... + checksum
-    // 0:0xAA header
+    if (length < 12) {
-    // 1:version (0x02)
+        return false;
-    // 2:payload_len (bytes 3..3+len-1)
+    }
-    // 3-4: buttons LE
+    if (packet[0] != 0xAA) {
-    // 5: hat
-    // 6-9: lx, ly, rx, ry (0-255)
-    // 10: imu_sample_count (0-3)
-    // 11-46: up to 3 samples of accel/gyro (int16 LE each axis)
-    // 47: checksum (sum of bytes 0..46) & 0xFF
-    if (length < 8 || packet[0] != 0xAA) {
         return false;
     }
-
     if (packet[1] != 0x02) {
         return false;
     }
 
     uint8_t payload_len = packet[2];
-    uint16_t expected_len = static_cast<uint16_t>(payload_len) + 4; // header+version+len+checksum
+    if ((uint16_t)payload_len + 4u != length) {
-    if (length < expected_len) {
-        return false;
-    }
-
-    uint16_t checksum_end = static_cast<uint16_t>(3 + payload_len - 1); // last payload byte
-    uint16_t checksum_index = static_cast<uint16_t>(3 + payload_len);
-    if (checksum_index >= length) {
         return false;
     }
 
     uint16_t sum = 0;
-    for (uint16_t i = 0; i <= checksum_end; ++i) {
+    for (uint16_t i = 0; i < (uint16_t)(3u + payload_len); ++i) {
-        sum = static_cast<uint16_t>(sum + packet[i]);
+        sum += packet[i];
     }
-    if ((sum & 0xFF) != packet[checksum_index]) {
+    if ((sum & 0xFF) != packet[length - 1]) {
+        return false;
+    }
+
+    // payload: buttons(2 LE), hat, lx, ly, rx, ry, imu_count, [imu_samples...]
+    if (payload_len < 8) {
         return false;
     }
 
@@ -698,28 +680,34 @@ bool switch_pro_apply_uart_packet(const uint8_t* packet, uint8_t length, SwitchI
     out.ly = packet[7];
     out.rx = packet[8];
     out.ry = packet[9];
+    uint8_t imu_count = packet[10];
+    if (imu_count > 3) {
+        imu_count = 3;
+    }
+
+    uint16_t required_payload_len = static_cast<uint16_t>(8u + static_cast<uint16_t>(imu_count) * 12u);
+    if (payload_len < required_payload_len) {
+        return false;
+    }
 
     auto expand_axis = [](uint8_t v) -> uint16_t {
         return static_cast<uint16_t>(v) << 8 | v;
     };
 
     SwitchInputState state = make_neutral_state();
-
+    state.imu_sample_count = imu_count;
-    state.imu_sample_count = std::min<uint8_t>(packet[10], 3);
 
     auto read_int16 = [](const uint8_t* src) -> int16_t {
         return static_cast<int16_t>(static_cast<uint16_t>(src[0]) | (static_cast<uint16_t>(src[1]) << 8));
     };
-
+    for (uint8_t i = 0; i < imu_count; ++i) {
-    const uint8_t* imu_base = &packet[11];
+        const uint8_t* base = &packet[11 + i * 12];
-    for (uint8_t i = 0; i < state.imu_sample_count; ++i) {
+        state.imu_samples[i].accel_x = read_int16(base + 0);
-        const uint8_t* sample_ptr = imu_base + (i * 12);
+        state.imu_samples[i].accel_y = read_int16(base + 2);
-        state.imu_samples[i].accel_x = read_int16(sample_ptr + 0);
+        state.imu_samples[i].accel_z = read_int16(base + 4);
-        state.imu_samples[i].accel_y = read_int16(sample_ptr + 2);
+        state.imu_samples[i].gyro_x = read_int16(base + 6);
-        state.imu_samples[i].accel_z = read_int16(sample_ptr + 4);
+        state.imu_samples[i].gyro_y = read_int16(base + 8);
-        state.imu_samples[i].gyro_x = read_int16(sample_ptr + 6);
+        state.imu_samples[i].gyro_z = read_int16(base + 10);
-        state.imu_samples[i].gyro_y = read_int16(sample_ptr + 8);
-        state.imu_samples[i].gyro_z = read_int16(sample_ptr + 10);
     }
 
     switch (out.hat) {
@@ -754,11 +742,10 @@ bool switch_pro_apply_uart_packet(const uint8_t* packet, uint8_t length, SwitchI
     state.rx = expand_axis(out.rx);
     state.ry = expand_axis(out.ry);
 
-    if (out_state) {
+    if (!out_state) {
-        *out_state = state;
+        return false;
-    } else {
-        switch_pro_set_input(state);
     }
+    *out_state = state;
     return true;
 }
 
@@ -855,9 +842,9 @@ bool tud_control_request_cb(uint8_t rhport, tusb_control_request_t const * reque
 void tud_mount_cb(void) {
     LOG_PRINTF("[USB] mount_cb\n");
     last_host_activity_ms = to_ms_since_boot(get_absolute_time());
-    forced_ready = true;
+    forced_ready = false;
-    is_ready = true;
+    is_ready = false;
-    is_initialized = true;
+    is_initialized = false;
 }
 
 void tud_umount_cb(void) {

switch_pro_driver.h

@@ -44,7 +44,8 @@ typedef struct {
     uint16_t ly;
     uint16_t rx;
     uint16_t ry;
-    uint8_t imu_sample_count;
+
+    uint8_t imu_sample_count;     // 0-3
     SwitchImuSample imu_samples[3];
 } SwitchInputState;
 

tests/test_uart_protocol.py

@@ -0,0 +1,120 @@
+"""Tests for UART v2 protocol serialization in switch_pico_uart."""
+
+import struct
+import pytest
+from switch_pico_bridge.switch_pico_uart import (
+    SwitchReport,
+    IMUSample,
+    SwitchDpad,
+    UART_HEADER,
+    UART_PROTOCOL_VERSION,
+    ACCEL_LSB_PER_G,
+    GYRO_LSB_PER_RAD_S,
+    MS2_PER_G,
+    compute_checksum,
+)
+
+
+def test_v2_frame_with_imu_samples():
+    """V2 frame with 3 IMU samples should be 48 bytes with correct layout."""
+    r = SwitchReport(
+        buttons=0,
+        imu_samples=[
+            IMUSample(100, -200, 4096, 50, -50, 0),
+            IMUSample(101, -201, 4097, 51, -51, 1),
+            IMUSample(102, -202, 4098, 52, -52, 2),
+        ],
+    )
+    data = r.to_bytes()
+    assert len(data) == 48, f"Expected 48 bytes, got {len(data)}"
+    assert data[0] == UART_HEADER  # 0xAA
+    assert data[1] == UART_PROTOCOL_VERSION  # 0x02
+    assert data[2] == 44  # payload_len
+    assert data[10] == 3  # imu_count
+    # Verify checksum
+    assert data[-1] == compute_checksum(data[:-1])
+    # Verify first sample accel_x (int16 LE at byte 11)
+    ax0 = struct.unpack_from("<h", data, 11)[0]
+    assert ax0 == 100, f"Expected accel_x=100, got {ax0}"
+    # Verify first sample gyro_z (int16 LE at bytes 21-22)
+    gz0 = struct.unpack_from("<h", data, 21)[0]
+    assert gz0 == 0, f"Expected gyro_z=0, got {gz0}"
+
+
+def test_v2_frame_no_imu():
+    """V2 frame with no IMU samples should be 12 bytes."""
+    r = SwitchReport(
+        buttons=0x0004, hat=SwitchDpad.CENTER, lx=128, ly=128, rx=128, ry=128
+    )
+    data = r.to_bytes()
+    assert len(data) == 12, f"Expected 12 bytes, got {len(data)}"
+    assert data[0] == UART_HEADER
+    assert data[1] == UART_PROTOCOL_VERSION
+    assert data[2] == 8  # payload_len
+    assert data[10] == 0  # imu_count
+    assert data[-1] == compute_checksum(data[:-1])
+
+
+def test_checksum_validation():
+    """Checksum should match sum of all preceding bytes & 0xFF."""
+    r = SwitchReport(buttons=0x0001)
+    data = r.to_bytes()
+    expected_checksum = sum(data[:-1]) & 0xFF
+    assert data[-1] == expected_checksum
+    # Corrupt a byte and verify mismatch
+    corrupted = bytearray(data)
+    corrupted[3] ^= 0xFF  # flip bits in first payload byte
+    recalculated = sum(corrupted[:-1]) & 0xFF
+    assert corrupted[-1] != recalculated, "Checksum should not match corrupted data"
+
+
+def test_accel_scale_gravity():
+    """1G (9.80665 m/s²) should convert to ~4096 raw counts."""
+    # convert_accel_to_raw(9.80665) ≈ 4096
+    raw = int(round((MS2_PER_G / MS2_PER_G) * ACCEL_LSB_PER_G))
+    assert abs(raw - 4096) <= 5, f"Expected ~4096 for 1G, got {raw}"
+
+
+def test_gyro_scale_one_rad():
+    """1.0 rad/s should convert to ~818 raw counts."""
+    raw = int(round(1.0 * GYRO_LSB_PER_RAD_S))
+    assert abs(raw - 818) <= 5, f"Expected ~818 for 1 rad/s, got {raw}"
+
+
+def test_imu_sample_dataclass():
+    """IMUSample fields accept int16 range values."""
+    s = IMUSample(
+        accel_x=32767, accel_y=-32768, accel_z=0, gyro_x=100, gyro_y=-100, gyro_z=1000
+    )
+    assert s.accel_x == 32767
+    assert s.accel_y == -32768
+    assert s.gyro_z == 1000
+    # Values outside int16 range are clamped in to_bytes()
+    s2 = IMUSample(accel_x=99999)
+    r = SwitchReport(imu_samples=[s2])
+    data = r.to_bytes()
+    ax = struct.unpack_from("<h", data, 11)[0]
+    assert ax == 32767, f"Expected clamped value 32767, got {ax}"
+
+
+def test_backward_compat_switch_report():
+    """SwitchReport with no imu_samples produces valid v2 frame (backward compat)."""
+    r = SwitchReport(buttons=0x000A, lx=200, ly=50, rx=128, ry=128)
+    data = r.to_bytes()
+    assert len(data) == 12
+    assert data[1] == 0x02  # still v2
+    # Buttons at bytes 3-4
+    buttons = struct.unpack_from("<H", data, 3)[0]
+    assert buttons == 0x000A
+    # lx at byte 6
+    assert data[6] == 200
+
+
+def test_max_imu_samples_capped():
+    """Providing >3 IMU samples should cap at 3."""
+    samples = [IMUSample(i, 0, 0, 0, 0, 0) for i in range(5)]
+    r = SwitchReport(imu_samples=samples)
+    data = r.to_bytes()
+    assert len(data) == 48  # 3 samples, not 5
+    assert data[10] == 3
+    assert data[2] == 44  # payload_len for 3 samples

tools/read_pro_imu.py

@@ -10,11 +10,14 @@ import struct
 import sys
 from typing import List, Tuple
 
-import hid  # from pyhidapi
-
 DEFAULT_VENDOR_ID = 0x057E
 DEFAULT_PRODUCT_ID = 0x2009  # Switch Pro Controller (USB)
 
+try:
+    import hid  # from pyhidapi
+except ImportError:
+    hid = None
+
 
 def list_devices(filter_vid=None, filter_pid=None):
     devices = hid.enumerate()
@@ -42,23 +45,62 @@ def find_device(vendor_id: int, product_id: int):
 
 
 def main():
-    parser = argparse.ArgumentParser(description="Read raw 0x30 reports (IMU) from a Switch Pro Controller / Pico.")
+    parser = argparse.ArgumentParser(
-    parser.add_argument("--vid", type=lambda x: int(x, 0), default=DEFAULT_VENDOR_ID, help="Vendor ID (default 0x057E)")
+        description="Read raw 0x30 reports (IMU) from a Switch Pro Controller / Pico."
-    parser.add_argument("--pid", type=lambda x: int(x, 0), default=DEFAULT_PRODUCT_ID, help="Product ID (default 0x2009)")
+    )
+    parser.add_argument(
+        "--vid",
+        type=lambda x: int(x, 0),
+        default=DEFAULT_VENDOR_ID,
+        help="Vendor ID (default 0x057E)",
+    )
+    parser.add_argument(
+        "--pid",
+        type=lambda x: int(x, 0),
+        default=DEFAULT_PRODUCT_ID,
+        help="Product ID (default 0x2009)",
+    )
     parser.add_argument("--path", help="Explicit HID path to open (overrides VID/PID).")
-    parser.add_argument("--count", type=int, default=0, help="Stop after this many 0x30 reports (0 = infinite).")
+    parser.add_argument(
-    parser.add_argument("--timeout", type=int, default=3000, help="Read timeout ms (default 3000).")
+        "--count",
-    parser.add_argument("--list", action="store_true", help="List detected HID devices and exit.")
+        type=int,
-    parser.add_argument("--plot", action="store_true", help="Plot accel/gyro traces after capture (requires matplotlib).")
+        default=0,
-    parser.add_argument("--save-prefix", help="If set, save accel/gyro plots as '<prefix>_accel.png' and '<prefix>_gyro.png'.")
+        help="Stop after this many 0x30 reports (0 = infinite).",
+    )
+    parser.add_argument(
+        "--timeout", type=int, default=3000, help="Read timeout ms (default 3000)."
+    )
+    parser.add_argument(
+        "--list", action="store_true", help="List detected HID devices and exit."
+    )
+    parser.add_argument(
+        "--plot",
+        action="store_true",
+        help="Plot accel/gyro traces after capture (requires matplotlib).",
+    )
+    parser.add_argument(
+        "--save-prefix",
+        help="If set, save accel/gyro plots as '<prefix>_accel.png' and '<prefix>_gyro.png'.",
+    )
     args = parser.parse_args()
 
+    if hid is None:
+        print(
+            "pyhidapi is required for this tool. Install it with: pip install pyhidapi",
+            file=sys.stderr,
+        )
+        sys.exit(1)
+
     if args.list:
         list_devices()
         return
 
     if args.path:
-        dev_info = {"path": bytes(args.path, encoding="utf-8"), "vendor_id": args.vid, "product_id": args.pid}
+        dev_info = {
+            "path": bytes(args.path, encoding="utf-8"),
+            "vendor_id": args.vid,
+            "product_id": args.pid,
+        }
     else:
         dev_info = find_device(args.vid, args.pid)
         if not dev_info:
@@ -91,7 +133,9 @@ def main():
             samples = []
             offset = 13  # accel_x starts at byte 13
             for _ in range(3):
-                ax, ay, az, gx, gy, gz = struct.unpack_from("<hhhhhh", bytes(data), offset)
+                ax, ay, az, gx, gy, gz = struct.unpack_from(
+                    "<hhhhhh", bytes(data), offset
+                )
                 samples.append((ax, ay, az, gx, gy, gz))
                 offset += 12
             print(samples)
@@ -138,9 +182,15 @@ def main():
             ax2.legend()
 
             if args.save_prefix:
-                fig1.savefig(f"{args.save_prefix}_accel.png", dpi=150, bbox_inches="tight")
+                fig1.savefig(
-                fig2.savefig(f"{args.save_prefix}_gyro.png", dpi=150, bbox_inches="tight")
+                    f"{args.save_prefix}_accel.png", dpi=150, bbox_inches="tight"
-                print(f"Saved plots to {args.save_prefix}_accel.png and {args.save_prefix}_gyro.png")
+                )
+                fig2.savefig(
+                    f"{args.save_prefix}_gyro.png", dpi=150, bbox_inches="tight"
+                )
+                print(
+                    f"Saved plots to {args.save_prefix}_accel.png and {args.save_prefix}_gyro.png"
+                )
 
             plt.show()