gweaks, nothing works yet

src/switch_pico_bridge/controller_uart_bridge.py

@@ -58,8 +58,11 @@ RUMBLE_SCALE = 1.0
 CONTROLLER_DB_URL_DEFAULT = "https://raw.githubusercontent.com/mdqinc/SDL_GameControllerDB/refs/heads/master/gamecontrollerdb.txt"
 
 SDL_TRUE = getattr(sdl2, "SDL_TRUE", 1)
+SDL_CONTROLLERSENSORUPDATE = getattr(sdl2, "SDL_CONTROLLERSENSORUPDATE", 0x658)
 GYRO_BIAS_SAMPLES = 200
-GYRO_DEADZONE_COUNTS = 15
+GYRO_DEADZONE_COUNTS = 0
+# Keep a small window of IMU samples to pack into the next report
+IMU_BUFFER_SIZE = 32
 
 
 def parse_mapping(value: str) -> Tuple[int, str]:
@@ -246,7 +249,7 @@ class ControllerContext:
     sensors_supported: bool = False
     sensors_enabled: bool = False
     imu_samples: List[IMUSample] = field(default_factory=list)
-    last_sensor_poll: float = 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
@@ -602,8 +605,8 @@ def build_arg_parser() -> argparse.ArgumentParser:
     parser.add_argument(
         "--frequency",
         type=float,
-        default=1000.0,
+        default=66.7,
-        help="Report send frequency per controller (Hz, default 1000)",
+        help="Report send frequency per controller (Hz, default ~66.7 => ~15ms)",
     )
     parser.add_argument(
         "--deadzone",
@@ -710,6 +713,17 @@ def build_arg_parser() -> argparse.ArgumentParser:
         action="store_true",
         help="Print raw IMU readings (float and converted int16) for debugging.",
     )
+    parser.add_argument(
+        "--gyro-scale",
+        type=float,
+        default=1.0,
+        help="Scale factor for gyro sensitivity (default 1.0). Reduce to < 1.0 if camera moves too fast.",
+    )
+    parser.add_argument(
+        "--no-gyro-bias",
+        action="store_true",
+        help="Disable automatic gyro bias calibration at startup.",
+    )
     return parser
 
 
@@ -756,6 +770,8 @@ class BridgeConfig:
     swap_abxy_ids: set[str]
     swap_abxy_global: bool
     debug_imu: bool
+    gyro_scale: float
+    no_gyro_bias: bool
 
 
 @dataclass
@@ -792,10 +808,11 @@ def convert_accel_to_raw(accel_ms2: float) -> int:
     return clamp_int16(g_units * 4000.0)
 
 
-def convert_gyro_to_raw(gyro_rad: float) -> int:
+def convert_gyro_to_raw(gyro_rad: float, scale: float = 1.0) -> int:
     # SDL reports gyroscope data in radians/second; convert to dps then to Switch counts.
     dps = gyro_rad * RAD_TO_DEG
-    counts = clamp_int16(dps / 0.070)
+    # 0.070 dps/LSB is approx 14.28 LSB/dps.
+    counts = clamp_int16((dps / 0.061) * scale)
     if abs(counts) < GYRO_DEADZONE_COUNTS:
         return 0
     return counts
@@ -841,13 +858,24 @@ def initialize_controller_sensors(ctx: ControllerContext, console: Console) -> N
         )
 
 
-def collect_imu_sample(ctx: ControllerContext, config: BridgeConfig) -> None:
+def handle_sensor_update(event: sdl2.SDL_Event, contexts: Dict[int, ControllerContext], config: BridgeConfig) -> None:
-    if not ctx.sensors_enabled or SENSOR_ACCEL is None or SENSOR_GYRO is None:
+    ctx = contexts.get(event.csensor.which)
+    if not ctx:
         return
-    accel = read_sensor_triplet(ctx.controller, SENSOR_ACCEL)
+
-    gyro = read_sensor_triplet(ctx.controller, SENSOR_GYRO)
+    sensor_type = event.csensor.sensor
-    if not accel or not gyro:
+    data = event.csensor.data
+
+    if sensor_type == SENSOR_ACCEL:
+        ctx.last_accel = (data[0], data[1], data[2])
         return
+
+    if sensor_type != SENSOR_GYRO:
+        return
+
+    # Process Gyro update (and combine with last accel)
+    gyro = (data[0], data[1], data[2])
+
     if not ctx.gyro_bias_locked and ctx.gyro_bias_samples < GYRO_BIAS_SAMPLES:
         ctx.gyro_bias_x += gyro[0]
         ctx.gyro_bias_y += gyro[1]
@@ -862,20 +890,17 @@ def collect_imu_sample(ctx: ControllerContext, config: BridgeConfig) -> None:
     bias_x = ctx.gyro_bias_x if ctx.gyro_bias_locked else 0.0
     bias_y = ctx.gyro_bias_y if ctx.gyro_bias_locked else 0.0
     bias_z = ctx.gyro_bias_z if ctx.gyro_bias_locked else 0.0
-    gyro_bias_corrected = (
+
-        gyro[0] - bias_x,
+    # Use last known accel
-        gyro[1] - bias_y,
+    accel = ctx.last_accel
-        gyro[2] - bias_z,
-    )
 
     # Map SDL sensor axes to Pro Controller axes: gravity should land on Z.
-    # print(accel[2] / MS2_PER_G)
     accel_raw_x = convert_accel_to_raw(accel[0])
     accel_raw_y = convert_accel_to_raw(accel[1])
     accel_raw_z = convert_accel_to_raw(accel[2])
-    gyro_raw_x = convert_gyro_to_raw(gyro[0])
+    gyro_raw_x = convert_gyro_to_raw(gyro[0] - bias_x, config.gyro_scale)
-    gyro_raw_y = convert_gyro_to_raw(gyro[1])
+    gyro_raw_y = convert_gyro_to_raw(gyro[1] - bias_y, config.gyro_scale)
-    gyro_raw_z = convert_gyro_to_raw(gyro[2])
+    gyro_raw_z = convert_gyro_to_raw(gyro[2] - bias_z, config.gyro_scale)
 
     # Map SDL axes to Pro axes to match the native Pro USB output:
     # Pro accel: ax = SDL_, ay = SDL_Z, az = SDL_Y (gravity).
@@ -884,13 +909,13 @@ def collect_imu_sample(ctx: ControllerContext, config: BridgeConfig) -> None:
         accel_x=-accel_raw_z,
         accel_y=-accel_raw_x,
         accel_z=accel_raw_y,
-        gyro_x=-gyro_raw_z,
+        gyro_x=convert_gyro_to_raw(-(gyro[2] - bias_z), config.gyro_scale),
-        gyro_y=-gyro_raw_x,
+        gyro_y=convert_gyro_to_raw(-(gyro[0] - bias_x), config.gyro_scale),
-        gyro_z=gyro_raw_y,
+        gyro_z=convert_gyro_to_raw(gyro[1] - bias_y, config.gyro_scale),
     )
     ctx.imu_samples.append(sample)
-    if len(ctx.imu_samples) > 6:
+    if len(ctx.imu_samples) > IMU_BUFFER_SIZE:
-        ctx.imu_samples = ctx.imu_samples[-6:]
+        ctx.imu_samples = ctx.imu_samples[-IMU_BUFFER_SIZE:]
 
     if config.debug_imu:
         now = time.monotonic()
@@ -936,7 +961,7 @@ def load_button_maps(
             console.print(
                 f"[red]Failed to load SDL mapping {mapping_path}: {exc}[/red]"
             )
-    return button_map_default, button_map_swapped, swap_abxy_indices
+    geturn button_map_default, button_map_swapped, swap_abxy_indices
 
 
 def build_bridge_config(console: Console, args: argparse.Namespace) -> BridgeConfig:
@@ -961,6 +986,8 @@ def build_bridge_config(console: Console, args: argparse.Namespace) -> BridgeCon
         swap_abxy_ids=set(swap_abxy_guids),  # filled later once stable IDs are known
         swap_abxy_global=bool(args.swap_abxy),
         debug_imu=bool(args.debug_imu),
+        gyro_scale=args.gyro_scale,
+        no_gyro_bias=args.no_gyro_bias,
     )
 
 
@@ -1466,7 +1493,7 @@ def service_contexts(
             else config.button_map_default
         )
         poll_controller_buttons(ctx, current_button_map)
-        collect_imu_sample(ctx, config)
+        # collect_imu_sample(ctx, config)  <-- Removed, using event loop
         # Reconnect UART if needed.
         if ctx.port and ctx.uart is None and (now - ctx.last_reopen_attempt) > 1.0:
             ctx.last_reopen_attempt = now
@@ -1481,8 +1508,11 @@ def service_contexts(
             continue
         try:
             if now - ctx.last_send >= config.interval:
-                if ctx.imu_samples:
+                # Consume up to 3 samples from the head of the queue (FIFO)
-                    ctx.report.imu_samples = ctx.imu_samples[-IMU_SAMPLES_PER_REPORT:]
+                count = min(len(ctx.imu_samples), IMU_SAMPLES_PER_REPORT)
+                if count > 0:
+                    ctx.report.imu_samples = ctx.imu_samples[:count]
+                    ctx.imu_samples = ctx.imu_samples[count:]
                 else:
                     ctx.report.imu_samples = []
                 ctx.uart.send_report(ctx.report)
@@ -1556,12 +1586,16 @@ def run_bridge_loop(
                 sdl2.SDL_CONTROLLERBUTTONUP,
             ):
                 handle_button_event(event, config, contexts)
+            elif event.type in (sdl2.SDL_CONTROLLERBUTTONDOWN, sdl2.SDL_CONTROLLERBUTTONUP):
+                handle_button_event(event, args, config, contexts)
+            elif event.type == SDL_CONTROLLERSENSORUPDATE:
+                handle_sensor_update(event, contexts, config)
             elif event.type == sdl2.SDL_CONTROLLERDEVICEADDED:
                 handle_device_added(
                     event, args, pairing, contexts, uarts, console, config
                 )
             elif event.type == sdl2.SDL_CONTROLLERDEVICEREMOVED:
-                handle_device_removed(event, pairing, contexts, console)
+                gandle_device_removed(event, pairing, contexts, console)
 
         now = time.monotonic()
         if now - last_port_scan > port_scan_interval:

switch-pico.cpp

@@ -1,4 +1,4 @@
-#include <stdio.h>
+ginclude <stdio.h>
 #include <string.h>
 #include "bsp/board.h"
 #include "hardware/uart.h"
@@ -61,12 +61,13 @@ static void on_rumble_from_switch(const uint8_t rumble[8]) {
 }
 
 // Consume UART bytes and forward complete frames to the Switch Pro driver.
-static void poll_uart_frames() {
+static bool poll_uart_frames() {
     static uint8_t buffer[64];
     static uint8_t index = 0;
     static uint8_t expected_len = 0;
     static absolute_time_t last_byte_time = {0};
     static bool has_last_byte = false;
+    bool new_data = false;
 
     while (uart_is_readable(UART_ID)) {
         uint8_t byte = uart_getc(UART_ID);
@@ -123,8 +124,10 @@ static void poll_uart_frames() {
             }
             index = 0;
             expected_len = 0;
+            new_data = true;
         }
     }
+    return new_data;
 }
 
 static void log_usb_state() {
@@ -159,9 +162,13 @@ int main() {
 
     while (true) {
         tud_task();          // USB device tasks
-        poll_uart_frames();  // Pull controller state from UART1
+        bool new_data = poll_uart_frames();  // Pull controller state from UART1
         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

@@ -632,6 +632,9 @@ void switch_pro_task() {
             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;
             }
 
             last_report_timer = now;