From 6884f2512176a2dd3cec44addc367a0faae1a0f7 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Mon, 16 Mar 2026 18:57:26 -0600 Subject: [PATCH] fix: eliminate IMU jumping from stale FIFO samples and zero-accel startup Two root causes of camera 'wild jumping': 1. FIFO latency (bridge): Popped from the FRONT of the 32-sample FIFO, sending 145ms-stale data while fresh samples sat at the back. Movement played back on delay, making the camera feel disconnected from input. Fix: pop from the END (newest samples) and clear the entire FIFO. 2. Zero-accel startup (firmware): At boot, imuData was all zeros until the first UART frame with IMU arrived (~3-4 seconds later). The Switch interpreted zero accel as free-fall, corrupting its sensor fusion state. Fix: default imuData to a 'rest' sample (1G on accel_z, zero gyro) so the Switch always sees a valid gravity reference. --- .../controller_uart_bridge.py | 15 +++++++-- switch_pro_driver.cpp | 32 ++++++++++++++++++- 2 files changed, 44 insertions(+), 3 deletions(-) diff --git a/src/switch_pico_bridge/controller_uart_bridge.py b/src/switch_pico_bridge/controller_uart_bridge.py index 4c05ffe..32203f8 100644 --- a/src/switch_pico_bridge/controller_uart_bridge.py +++ b/src/switch_pico_bridge/controller_uart_bridge.py @@ -1628,12 +1628,23 @@ 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: - ctx.report.imu_samples = ctx.imu_samples[:count] - ctx.imu_samples = ctx.imu_samples[count:] + # 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() 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 diff --git a/switch_pro_driver.cpp b/switch_pro_driver.cpp index d7d3ba3..7680e8f 100644 --- a/switch_pro_driver.cpp +++ b/switch_pro_driver.cpp @@ -192,9 +192,26 @@ static std::map 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) { - memset(switch_report.imuData, 0x00, sizeof(switch_report.imuData)); + // 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); + } return; } uint8_t sample_count = state.imu_sample_count > 3 ? 3 : state.imu_sample_count; @@ -634,6 +651,19 @@ 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; }