From 26a25a66affb5c6bd5c5b9c6e9509550a0a57802 Mon Sep 17 00:00:00 2001 From: Joey Yakimowich-Payne Date: Sun, 13 Sep 2026 00:00:53 -0600 Subject: [PATCH] Start Wii motion immediately and track residual bias in background --- README.md | 58 ++++++-- .../input/bluepad32_input_backend.cpp | 2 +- src/firmware/input/bluepad32_input_backend.h | 2 +- tests/native_gamepad_backend_test.cpp | 16 +-- tests/switch2_native_gamepad_bridge_test.cpp | 12 +- tests/switch2_native_imu_test.cpp | 136 +++++++++++++----- tests/switch2_wii_bridge_test.cpp | 5 +- tools/switch2_usb_probe/controller_input.cpp | 4 +- tools/switch2_usb_probe/controller_input.h | 2 +- tools/switch2_usb_probe/main.c | 4 +- .../native_gamepad_input.cpp | 9 +- tools/switch2_usb_probe/native_imu.cpp | 47 +++--- tools/switch2_usb_probe/native_imu.h | 10 +- tools/switch2_usb_probe/probe_build.cmake | 4 +- 14 files changed, 210 insertions(+), 101 deletions(-) diff --git a/README.md b/README.md index ef82373..db0d24a 100644 --- a/README.md +++ b/README.md @@ -606,8 +606,9 @@ byte-identical to 0.35. Firmware 0.37 was flashed with persistent storage unchanged and both pairing records restored; its console chord test is pending. - **Motion:** factory-calibrated Wii acceleration and MotionPlus gyro have - independent freshness counters. Keep the Remote still at startup for at least - 1.5 seconds and 64 fresh gyro samples to estimate residual bias. The encoder + independent freshness counters. Motion starts with the first usable fresh + sensor pair; residual bias is refined in the background during quiet periods. + There is no mandatory startup settling period. The encoder integrates real gyro into an orientation quaternion, using fresh near-1g acceleration to correct tilt drift, and emits the recovered 30-byte native IMU format. Fresh full-bar observations gently correct relative heading @@ -827,9 +828,9 @@ The existing profile transform runs once for the full pad. R gets face buttons, right stick/shoulder/trigger, plus and home; L gets the D-pad, left stick/shoulder/ trigger, minus and capture. Each uses its own advertised stick calibration. One shared motion integrator consumes only fresh, complete, CRC-checked and -factory-calibrated DS5 sensor data. From 0.69, already-calibrated native sources -initialize from their first usable fresh sensor pair; the extra stationary -bias-estimation period is explicitly Wii-only. Invalid/stale sensors still +factory-calibrated DS5 sensor data. DS5 initializes from its first usable fresh +sensor pair. From 0.71, Wii also starts immediately and refines residual bias in +the background, without blocking IMU or resetting orientation. Invalid/stale sensors still withhold IMU while controls remain available. Each USB half has independent peek/commit, reset and backpressure state. No mouse movement or rail presses are invented. @@ -863,9 +864,9 @@ decoded with changing counters and quaternions. The Wii-style stationary gate had delayed readiness until about 30 seconds after boot in that run. Firmware 0.69 removes that extra gate for factory-calibrated sources; the regression checks first-sample output even while rotating, fresh-data recovery, and the -unchanged Wii settling behavior. Wii factory calibration is already used, but -its residual gyro bias still needs estimation; cached per-device bias without -revalidation can drift and is not implemented here. +then-current Wii settling behavior. From 0.71, Wii no longer waits for that +estimate before emitting motion; it uses the nonblocking policy described below. +Factory calibration still applies, and bias is not cached across boots. Translated full-controller builds expose `SWITCH2_BRIDGE_IMU_TARGET`: `LEFT`, `RIGHT`, or `BOTH` (default). For example, configure the existing private @@ -900,8 +901,8 @@ profile behavior is retained; original Switch Joy-Con grouping is not added. arrive. A paired left Joy-Con cannot refresh the right-owned sensor stream. Wii acceleration cannot refresh a stalled MotionPlus gyro stream. - `SWITCH2_BRIDGE_IMU_TARGET=LEFT|RIGHT|BOTH` also applies to `GAMEPAD`; Wii alone - estimates residual bias while stationary. Factory calibration already runs; - caching residual Wii bias across boots without revalidation is not implemented. + refines residual bias in the background while apparently stationary, without + withholding valid IMU. Factory calibration still runs; no bias is saved across boots. - Native cue requests use each source driver's bounded compatibility vibration. Mono drivers combine the two logical contributions; paired Switch2 Joy-Cons target their actual halves. This does not promise stereo, HD-waveform fidelity @@ -913,10 +914,39 @@ profile behavior is retained; original Switch Joy-Con grouping is not added. The private `build-switch2-native-gamepad` image uses mixed Bluetooth and the unchanged stock USB socket. Software regressions cover real parser calibration, -report integrity/freshness, source selection, split/reset/backpressure, Wii-only -settling and cue lifetimes. DualSense, generic, existing Joy-Con/Wii, and ordinary -AIO mixed/BLE/Classic firmware builds pass. The new generic image has not been -flashed or physically qualified across these controller families. +report integrity/freshness, source selection, split/reset/backpressure, Wii +background correction and cue lifetimes. Version 0.70 was flashed with saved +storage verified unchanged, and the user confirmed DualSense operation. +Other controller-family motion orientation and motor response still need +physical qualification. + +**Nonblocking Wii motion (0.71):** both the `GAMEPAD` and dedicated `WII` paths +emit motion on the first usable fresh acceleration/gyro pair. Bias collection +requires 1.5 seconds, at least 64 distinct gyro samples, low sensor variation and +stable gravity direction, but runs alongside output rather than gating it. +Accepted targets are applied at no more than 5 dps of correction per second; +they never reset the quaternion or undo accumulated yaw. The absolute candidate +gyro-vector limit is 30 dps, retaining headroom for the recorded Wii residual of +roughly 13 dps per axis without permitting unbounded learning or ratcheting. +Large rates, shaking and changing tilt discard the candidate; invalid/stale +sensors retire both the learned correction and target. Fresh recovery starts +immediately. DualSense and other factory-only sources do not run this tracker. + +Quiet periods still improve drift; initial drift can be substantial with a large +offset. A sufficiently steady rotation about gravity below the candidate limit +cannot be distinguished from bias using these sensors alone. This is not a +guarantee of drift-free aiming while continuously moving. Built-in MotionPlus +needs no accessory handling or manual calibration command. + +All 31 focused regressions pass, including immediate Wii output, bounded +background convergence without a pose reset, motion rejection, duplicate-poll +invariance and lifecycle recovery. A throwaway production-estimator smoke run +kept output ready from its first sample while converging to the recorded-scale +offset by six seconds. Generic, dedicated Wii, DualSense and mixed AIO builds pass. +Version 0.71 was then flashed and verified, with the saved-storage region +byte-for-byte unchanged. The hub and both native children enumerated, and UART +confirmed the nonblocking policy. Physical Wii startup/drift qualification is +still pending. For sensorless hardware, the checker supports `--input-only`: press real buttons and keep changing controls on both halves during the run. Neutral fallback diff --git a/src/firmware/input/bluepad32_input_backend.cpp b/src/firmware/input/bluepad32_input_backend.cpp index 359e60b..f15f2cc 100644 --- a/src/firmware/input/bluepad32_input_backend.cpp +++ b/src/firmware/input/bluepad32_input_backend.cpp @@ -1479,7 +1479,7 @@ void publish_device_state(uint8_t slot, uni_hid_device_t* device, g_native_snapshot.state_generation = target.state_generation; g_native_snapshot.received_us = motion.received_us; g_native_snapshot.battery = device->controller.battery; - g_native_snapshot.requires_stationary_bias = + g_native_snapshot.track_stationary_bias = device->controller_type == CONTROLLER_TYPE_WiiController; g_native_snapshot.accel_valid = motion.accel_valid; g_native_snapshot.gyro_valid = motion.gyro_valid; diff --git a/src/firmware/input/bluepad32_input_backend.h b/src/firmware/input/bluepad32_input_backend.h index 7979951..079c076 100644 --- a/src/firmware/input/bluepad32_input_backend.h +++ b/src/firmware/input/bluepad32_input_backend.h @@ -119,7 +119,7 @@ struct Bluepad32NativeGamepadSnapshot { uint8_t battery = 0; bool accel_valid = false; bool gyro_valid = false; - bool requires_stationary_bias = false; + bool track_stationary_bias = false; uint32_t accel_sequence = 0; uint32_t gyro_sequence = 0; uint32_t accel_received_us = 0; diff --git a/tests/native_gamepad_backend_test.cpp b/tests/native_gamepad_backend_test.cpp index ad0e519..535a53f 100644 --- a/tests/native_gamepad_backend_test.cpp +++ b/tests/native_gamepad_backend_test.cpp @@ -84,7 +84,7 @@ void source_isolation() { const auto initial = bridge_snapshot(); require(initial.slot == 0 && initial.controller.active && initial.controller.state.button_south && initial.battery == 176 && initial.accel_valid && initial.gyro_valid && - !initial.requires_stationary_bias && initial.accel_q13[1] == 8193 && + !initial.track_stationary_bias && initial.accel_q13[1] == 8193 && initial.gyro_q10[2] == -123456 && initial.gyro_received_us == 100000, "native snapshot must preserve coherent physical controls and calibrated precision"); now_ms = 120; @@ -319,7 +319,7 @@ void sensorless_admission() { uint64_t cue = 99; require(initial.controller.active && initial.slot == 0 && initial.controller.state.button_south && !initial.accel_valid && - !initial.gyro_valid && !initial.requires_stationary_bias && + !initial.gyro_valid && !initial.track_stationary_bias && initial.accel_sequence == 0 && initial.gyro_sequence == 0 && !bluepad32_input_backend_native_sample_request(0, 1, &cue) && cue == 0, "sensorless controls remain live without invented IMU or rumble capability"); @@ -356,9 +356,9 @@ void independent_motion() { now_ms = 100; report_gamepad(ds4); const auto first = bridge_snapshot(); - require(first.accel_valid && first.gyro_valid && !first.requires_stationary_bias && + require(first.accel_valid && first.gyro_valid && !first.track_stationary_bias && first.accel_q13[1] == 8193 && first.gyro_q10[2] == -123456, - "calibrated DS4 motion keeps raw precision without Wii settling"); + "calibrated DS4 motion retains precision without Wii background correction"); now_ms = 110; ++ds.report_sequence; ++ds.accel_sequence; @@ -408,8 +408,8 @@ void independent_motion() { sw.accel_valid = sw.gyro_valid = true; report_gamepad(pro); require(bridge_snapshot().accel_valid && bridge_snapshot().gyro_valid && - !bridge_snapshot().requires_stationary_bias, - "validated Switch motion starts without Wii stationary settling"); + !bridge_snapshot().track_stationary_bias, + "validated Switch motion does not enable Wii background correction"); platform_on_device_disconnected(&pro); auto remote = wii_device(2); @@ -420,7 +420,7 @@ void independent_motion() { now_ms = 200; report_gamepad(remote); require(bridge_snapshot().controller.state.button_south && bridge_snapshot().accel_valid && - !bridge_snapshot().gyro_valid && bridge_snapshot().requires_stationary_bias, + !bridge_snapshot().gyro_valid && bridge_snapshot().track_stationary_bias, "Wii without MotionPlus remains acceleration-capable, not a fabricated full IMU"); now_ms = 210; ++wii.gyro_sequence; @@ -428,7 +428,7 @@ void independent_motion() { report_gamepad(remote); require(bridge_snapshot().gyro_valid && bridge_snapshot().gyro_received_us == 210000 && bridge_snapshot().accel_received_us == 200000 && - bridge_snapshot().requires_stationary_bias, + bridge_snapshot().track_stationary_bias, "Wii MotionPlus ingress must retain independent acceleration age and Wii bias policy"); } diff --git a/tests/switch2_native_gamepad_bridge_test.cpp b/tests/switch2_native_gamepad_bridge_test.cpp index 101e40b..786bd96 100644 --- a/tests/switch2_native_gamepad_bridge_test.cpp +++ b/tests/switch2_native_gamepad_bridge_test.cpp @@ -321,12 +321,13 @@ void selected_motion_target_keeps_both_control_halves() { void wii_bias_and_independent_sensor_freshness() { ++source.controller.connection_generation; - source.requires_stationary_bias = true; + source.track_stationary_bias = true; source.gyro_q10[1] = 2 * 1024; publish(); pair(); assert(controls[0].active && controls[1].active); assert(reports[0][2] == 0x01 && reports[1][2] == 0x08); - assert(imu_length(0) == 0 && imu_length(1) == 0); + for (uint8_t instance = 0; instance < 2; ++instance) + assert(imu_length(instance) == ((SWITCH2_BRIDGE_IMU_TARGET_MASK & (1u << instance)) ? 30 : 0)); for (unsigned i = 0; i < 400; ++i) { publish(); pair(); } for (uint8_t instance = 0; instance < 2; ++instance) { assert(imu_length(instance) == ((SWITCH2_BRIDGE_IMU_TARGET_MASK & (1u << instance)) ? 30 : 0)); @@ -344,12 +345,13 @@ void wii_bias_and_independent_sensor_freshness() { publish(); pair(); assert(reports[0][2] == 0x01 && reports[1][2] == 0x08); assert(imu_length(0) == 0 && imu_length(1) == 0); - // Returning real Wii sensors must settle again, not reuse the old bias. + // Fresh Wii sensors recover immediately, without borrowing the old bias. source.gyro_valid = true; publish(); pair(); - assert(imu_length(0) == 0 && imu_length(1) == 0); + for (uint8_t instance = 0; instance < 2; ++instance) + assert(imu_length(instance) == ((SWITCH2_BRIDGE_IMU_TARGET_MASK & (1u << instance)) ? 30 : 0)); // A factory-calibrated source switching policy must initialize immediately. - source.requires_stationary_bias = false; + source.track_stationary_bias = false; publish(); pair(); for (uint8_t instance = 0; instance < 2; ++instance) { assert(imu_length(instance) == ((SWITCH2_BRIDGE_IMU_TARGET_MASK & (1u << instance)) ? 30 : 0)); diff --git a/tests/switch2_native_imu_test.cpp b/tests/switch2_native_imu_test.cpp index 492bf52..4ba988a 100644 --- a/tests/switch2_native_imu_test.cpp +++ b/tests/switch2_native_imu_test.cpp @@ -173,9 +173,11 @@ struct Rig { ProbeNativeMotionSample sample{}; uint32_t now; uint32_t generation = 1; - static constexpr float residual[3]{0.4f, -0.3f, 0.2f}; + static constexpr float residual[3]{0.0f, 0.0f, 0.0f}; + ProbeNativeMotionBias policy = ProbeNativeMotionBias::kAlreadyCalibrated; - explicit Rig(uint32_t start = 0) : now(start) { + explicit Rig(uint32_t start = 0, ProbeNativeMotionBias mode = ProbeNativeMotionBias::kAlreadyCalibrated) + : now(start), policy(mode) { sample.accel_valid = sample.gyro_valid = true; sample.accel_g[2] = 1.0f; rest(); @@ -193,7 +195,7 @@ struct Rig { ++sample.gyro_sequence; sample.gyro_us = now; } - motion.update(now, generation, sample, ProbeNativeMotionBias::kEstimateStationary); + motion.update(now, generation, sample, policy); } void settle() { for (unsigned i = 0; i < 65; ++i) fresh(); @@ -207,35 +209,45 @@ struct Rig { } }; -void startup_needs_count_and_stillness() { - Rig fast; +void background_bias_requires_quiet_samples_not_startup_delay() { + Rig fast(0, ProbeNativeMotionBias::kTrackStationary); + fast.sample.gyro_dps[2] = 1.0f; fast.fresh(0); + expect(fast.motion.ready(), "Wii motion must be available on the first real sensor pair"); for (unsigned i = 0; i < 63; ++i) fast.fresh(1000); - expect(!fast.motion.ready(), "sixty-four packets alone cannot replace 1.5 seconds of stillness"); + expect(fast.motion.bias()[2] == 0.0f, "packet count alone cannot establish a bias target"); for (unsigned i = 0; i < 56; ++i) fast.fresh(); - expect(!fast.motion.ready(), "calibration must not finish before the stillness duration"); + expect(fast.motion.bias()[2] == 0.0f, "partial quiet windows cannot change gyro correction"); + const double before = camera_heading(fast.motion); fast.fresh(37000); - expect(fast.motion.ready(), "a stationary window may finish at exactly 1.5 seconds"); + expect(fast.motion.ready() && fast.motion.bias()[2] == 0.0f, "learning a target must not reset or snap motion"); + expect(std::abs(heading_change(fast.motion, before) - 0.037 * std::acos(-1.0) / 180.0) < 0.000001, + "calibration completion must preserve accumulated yaw"); + fast.fresh(); + expect(fast.motion.bias()[2] > 0.0f && fast.motion.bias()[2] <= 0.125001f, + "a confirmed target must be applied with a bounded slew, not a jump"); - Rig slow; + Rig slow(0, ProbeNativeMotionBias::kTrackStationary); + slow.sample.gyro_dps[2] = 1.0f; slow.fresh(0); for (unsigned i = 0; i < 62; ++i) slow.fresh(30000); - expect(!slow.motion.ready(), "elapsed stillness cannot replace sixty-four distinct gyro samples"); + expect(slow.motion.ready() && slow.motion.bias()[2] == 0.0f, + "elapsed time without enough distinct samples cannot learn bias"); slow.fresh(30000); - expect(slow.motion.ready(), "the sixty-fourth stationary sample can finish calibration"); - expect(close({slow.motion.bias()[0], slow.motion.bias()[1], slow.motion.bias()[2]}, - {Rig::residual[0], Rig::residual[1], Rig::residual[2]}, 0.000001), - "calibration must learn the selected sensor's residual bias"); + slow.fresh(0); + expect(slow.motion.bias()[2] == 0.0f, "zero elapsed time cannot increase correction strength"); + slow.fresh(25000); + expect(slow.motion.bias()[2] > 0.0f, "a complete quiet window must refine bias without gating output"); } void quantized_wii_bias_calibrates_and_integrates() { - Rig rig; + Rig rig(0, ProbeNativeMotionBias::kTrackStationary); constexpr float device_bias[3]{-13.0625f, 12.4375f, -13.125f}; constexpr int noise_q10[8]{-576, 192, -320, 576, -192, 320, -64, 64}; const Vector gravity{8352.0 / 8192.0, 290.0 / 8192.0, -298.0 / 8192.0}; // Deterministic Wii-scale Q13 steps and Q10 noise, with accel and gyro // arriving independently at 200 / 100 Hz. Neither noise nor bias is motion. - for (unsigned i = 0; i < 320 && !rig.motion.ready(); ++i) { + for (unsigned i = 0; i < 4000; ++i) { rig.sample.accel_g[0] = (8352 + (i & 1 ? 80 : -80)) / 8192.0f; rig.sample.accel_g[1] = (290 + (i & 2 ? 40 : -40)) / 8192.0f; rig.sample.accel_g[2] = (-298 + (i & 4 ? 43 : -43)) / 8192.0f; @@ -245,11 +257,12 @@ void quantized_wii_bias_calibrates_and_integrates() { } } rig.fresh(5000, true, i % 2 == 0); + expect(rig.motion.ready(), "quantized Wii motion must remain available throughout background learning"); } - expect(rig.motion.ready(), "quantized stationary Wii sensors with a large own-device bias must calibrate"); + expect(rig.motion.ready(), "a large measured Wii residual must remain correctable in the background"); expect(close({rig.motion.bias()[0], rig.motion.bias()[1], rig.motion.bias()[2]}, - {device_bias[0], device_bias[1], device_bias[2]}, 0.01), - "stationary variation must average into the measured bias, not a nominal or donor zero"); + {device_bias[0], device_bias[1], device_bias[2]}, 0.02), + "quiet sensor observations must converge to the measured device bias"); const Quaternion reference = rig.packet().q; const double gravity_length = std::sqrt(gravity[0] * gravity[0] + gravity[1] * gravity[1] + gravity[2] * gravity[2]); expect(close(rotate(reference, gravity), {0.0, 0.0, gravity_length}, 0.0002), @@ -345,7 +358,7 @@ void elapsed_time_not_packet_count_controls_rotation() { void moving_startup_does_not_calibrate() { for (unsigned movement = 0; movement < 4; ++movement) { - Rig rig; + Rig rig(0, ProbeNativeMotionBias::kTrackStationary); for (unsigned i = 0; i < 100; ++i) { rig.rest(); rig.sample.accel_g[0] = 0.0f; @@ -364,32 +377,41 @@ void moving_startup_does_not_calibrate() { if (movement == 3) rig.sample.gyro_dps[0] = i & 1 ? 1.0f : -1.0f; rig.fresh(); } - expect(!rig.motion.ready(), "moving startup cannot be mistaken for a stationary calibration window"); + expect(rig.motion.ready() && rig.motion.bias()[0] == 0.0f && rig.motion.bias()[1] == 0.0f && + rig.motion.bias()[2] == 0.0f, "moving startup must stay live without learning movement as bias"); rig.rest(); rig.sample.accel_g[0] = 0.0f; rig.sample.accel_g[1] = 0.0f; rig.sample.accel_g[2] = 1.0f; + rig.sample.gyro_dps[2] = 1.0f; for (unsigned i = 0; i < 128; ++i) rig.fresh(); - expect(rig.motion.ready(), "a new stationary window must recover after changing motion ends"); + expect(rig.motion.ready() && rig.motion.bias()[2] > 0.0f, "background correction must recover after movement"); } - Rig contaminated; + Rig contaminated(0, ProbeNativeMotionBias::kTrackStationary); + contaminated.sample.gyro_dps[2] = 1.0f; for (unsigned i = 0; i < 40; ++i) contaminated.fresh(); contaminated.sample.gyro_dps[0] = 4.0f; contaminated.fresh(); contaminated.rest(); + contaminated.sample.gyro_dps[2] = 1.0f; for (unsigned i = 0; i < 40; ++i) contaminated.fresh(); - expect(!contaminated.motion.ready(), "a gyro impulse must discard, not pool, the partial bias window"); + expect(contaminated.motion.ready() && contaminated.motion.bias()[2] == 0.0f, + "a gyro impulse must discard, not pool, the partial bias window"); for (unsigned i = 0; i < 24; ++i) contaminated.fresh(); - expect(contaminated.motion.ready(), "an uncontaminated replacement window must eventually calibrate"); + expect(contaminated.motion.bias()[2] == 0.0f, "a completed estimate must not snap the current correction"); + contaminated.fresh(); + expect(contaminated.motion.bias()[2] > 0.0f, "a replacement quiet window must eventually refine bias"); - Rig interleaved; + Rig interleaved(0, ProbeNativeMotionBias::kTrackStationary); + interleaved.sample.gyro_dps[2] = 1.0f; for (unsigned i = 0; i < 55; ++i) interleaved.fresh(); interleaved.sample.accel_g[0] = 0.1f; interleaved.fresh(5000, true, false); interleaved.sample.accel_g[0] = 0.0f; interleaved.fresh(5000, true, false); for (unsigned i = 0; i < 20; ++i) interleaved.fresh(); - expect(!interleaved.motion.ready(), "acceleration-only movement must reset the gyro calibration window too"); + expect(interleaved.motion.ready() && interleaved.motion.bias()[2] == 0.0f, + "acceleration-only movement must reset bias collection without blocking motion"); interleaved.settle(); } @@ -413,8 +435,10 @@ void duplicate_sequences_and_interleaved_recovery() { void lifecycle_invalidates_bias_and_orientation() { for (unsigned fault = 0; fault < 7; ++fault) { - Rig rig; - rig.settle(); + Rig rig(0, ProbeNativeMotionBias::kTrackStationary); + rig.sample.gyro_dps[2] = 1.0f; + for (unsigned i = 0; i < 400; ++i) rig.fresh(10000); + expect(rig.motion.bias()[2] == 1.0f, "quiet samples must establish a correction before a lifecycle fault"); if (fault == 0) rig.sample.accel_valid = false; if (fault == 1) rig.sample.gyro_valid = false; if (fault == 2) rig.sample.accel_g[0] = std::numeric_limits::quiet_NaN(); @@ -422,18 +446,21 @@ void lifecycle_invalidates_bias_and_orientation() { if (fault == 4) ++rig.generation; if (fault == 5) rig.motion.reset(); rig.fresh(fault == 6 ? 50001 : 1000); - expect(!rig.motion.ready(), "lifecycle or invalid sensor input must fail closed immediately"); + expect(rig.motion.ready() == (fault >= 4), "invalid sensors fail closed; fresh sensors after reset recover immediately"); + expect(rig.motion.bias()[2] == 0.0f, "faults must retire the old correction and its target"); + ++rig.generation; rig.sample.accel_valid = rig.sample.gyro_valid = true; rig.sample.accel_g[0] = rig.sample.accel_g[1] = 0.0f; rig.sample.accel_g[2] = -1.0f; rig.sample.gyro_dps[0] = -0.25f; rig.sample.gyro_dps[1] = 0.1f; rig.sample.gyro_dps[2] = -0.35f; - rig.settle(); + rig.fresh(); + expect(rig.motion.ready() && close(rotate(rig.packet().q, {0.0, 0.0, -1.0}), {0.0, 0.0, 1.0}), + "a fresh connection must initialize from its real pose without waiting"); + for (unsigned i = 0; i < 400; ++i) rig.fresh(10000); expect(close({rig.motion.bias()[0], rig.motion.bias()[1], rig.motion.bias()[2]}, {-0.25, 0.1, -0.35}, 0.000001), - "recovery must calibrate a new bias instead of borrowing the previous connection's bias"); - expect(close(rotate(rig.packet().q, {0.0, 0.0, -1.0}), {0.0, 0.0, 1.0}), - "recovery must align the new physical pose instead of replaying old orientation"); + "background recovery must learn the new source, not resume a retired target"); } } @@ -462,8 +489,10 @@ void stale_boundaries_and_hidden_gaps() { hidden.fresh(50000, false, false); hidden.fresh(49999, false, false); hidden.fresh(2); - expect(!hidden.motion.ready(), "fresh replacements cannot hide an intervening stale sample interval"); + expect(hidden.motion.ready() && close(rotate(hidden.packet().q, {1.0, 0.0, 0.0}), {1.0, 0.0, 0.0}), + "fresh replacements after a hidden gap must reinitialize, not integrate stale angular displacement"); hidden.rest(); + hidden.fresh(0); hidden.settle(); expect(close(rotate(hidden.packet().q, {1.0, 0.0, 0.0}), {1.0, 0.0, 0.0}), "stale angular displacement must not replay after recalibration"); @@ -482,7 +511,7 @@ void clocks_sequences_and_connections_wrap() { rig.sample.accel_sequence = rig.sample.gyro_sequence = 0; rig.rest(); rig.fresh(0); - expect(!rig.motion.ready(), "a new connection cannot inherit readiness even if it reuses sample identities"); + expect(rig.motion.ready(), "a new connection must start immediately from its own fresh samples"); rig.settle(); expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {1.0, 0.0, 0.0}), "a new connection must establish its own reference orientation"); @@ -793,12 +822,45 @@ void optical_vertical_forward_defers_the_anchor() { "heading correction must recover after the forward projection leaves vertical"); } +void background_tracking_preserves_motion_and_sensor_time() { + Rig turn(0, ProbeNativeMotionBias::kTrackStationary); + turn.sample.gyro_dps[2] = 90.0f; + turn.fresh(0); + for (unsigned i = 1; i <= 1000; ++i) { + turn.fresh(10000); + expect(turn.motion.ready(), "continuous movement must never block Wii motion"); + if (i == 100) + expect(close(rotate(turn.packet().q, {1.0, 0.0, 0.0}), {0.0, 1.0, 0.0}), + "startup rotation must reach native output immediately"); + } + expect(turn.motion.bias()[2] == 0.0f, "large steady yaw must not be learned as a stationary offset"); + + Rig sparse(0, ProbeNativeMotionBias::kTrackStationary); + Rig frequent(0, ProbeNativeMotionBias::kTrackStationary); + sparse.sample.gyro_dps[2] = frequent.sample.gyro_dps[2] = 1.0f; + sparse.fresh(0); + frequent.fresh(0); + for (unsigned ms = 1; ms <= 4000; ++ms) { + frequent.fresh(1000, ms % 10 == 0, ms % 10 == 0); + if (ms % 10 == 0) sparse.fresh(10000); + } + expect(sparse.motion.bias()[2] == 1.0f && frequent.motion.bias()[2] == 1.0f, + "stationary gyro offset must converge under either polling cadence"); + expect(std::abs(heading_change(sparse.motion, camera_heading(frequent.motion))) < 0.000001, + "duplicate polling must not increase background correction or change accumulated yaw"); + const double corrected_heading = camera_heading(sparse.motion); + for (unsigned i = 0; i < 100; ++i) sparse.fresh(10000); + expect(std::abs(heading_change(sparse.motion, corrected_heading)) < 0.000001, + "learned correction must stop further stationary yaw drift without recentering"); +} + } // namespace int main() { codec_wire_edges(); codec_rejects_invalid_inputs(); - startup_needs_count_and_stillness(); + background_bias_requires_quiet_samples_not_startup_delay(); + background_tracking_preserves_motion_and_sensor_time(); quantized_wii_bias_calibrates_and_integrates(); measured_gravity_sets_reference(); bias_corrected_body_rotation_reaches_wire(); diff --git a/tests/switch2_wii_bridge_test.cpp b/tests/switch2_wii_bridge_test.cpp index 49755b0..691e923 100644 --- a/tests/switch2_wii_bridge_test.cpp +++ b/tests/switch2_wii_bridge_test.cpp @@ -144,7 +144,7 @@ int main() { source.accel_q13[1] = 8192; // SDL up -> virtual native rail-down +X. memcpy(source.gyro_q10, bias_q10, sizeof(bias_q10)); assert(poll()); - assert(controls.active && packet[15] == 0); // Wii alone still estimates residual bias before IMU output. + assert(controls.active && packet[15] == 30); // Wii IMU starts before background bias learning. for (unsigned i = 1; i < 450; ++i) assert(poll()); assert(controls.active && packet[15] == 30 && packet[19] == 0x0c); assert(signed32(packet+32) == (1 << 28)); @@ -222,7 +222,8 @@ int main() { // Gyro-only motion remains available while the sensor bar is out of view. ir_mask = 0; - for (unsigned i = 0; i < 20; ++i) assert(poll()); + // Give background correction a fresh quiet window after the simulated cooling. + for (unsigned i = 0; i < 750; ++i) assert(poll()); decode_quaternion(packet, initial); for (unsigned i = 0; i < 250; ++i) assert(poll()); double after_bias[4]; decode_quaternion(packet, after_bias); diff --git a/tools/switch2_usb_probe/controller_input.cpp b/tools/switch2_usb_probe/controller_input.cpp index 1f6af17..b8145ca 100644 --- a/tools/switch2_usb_probe/controller_input.cpp +++ b/tools/switch2_usb_probe/controller_input.cpp @@ -290,7 +290,7 @@ void poll_wii_source(uint32_t now_ms) { motion.gyro_dps[0] = static_cast(g_wii.gyro_q10[1]) / 1024.0f; motion.gyro_dps[1] = -static_cast(g_wii.gyro_q10[2]) / 1024.0f; motion.gyro_dps[2] = -static_cast(g_wii.gyro_q10[0]) / 1024.0f; - g_motion.update(now_us, g_wii_generation, motion, ProbeNativeMotionBias::kEstimateStationary); + g_motion.update(now_us, g_wii_generation, motion, ProbeNativeMotionBias::kTrackStationary); WiiIrMouseReport optical{}; (void)wii_ir_mouse_peek(&optical, 0); // Core 1 may publish during the peek. Read the clock after the snapshot. @@ -308,7 +308,7 @@ void poll_wii_source(uint32_t now_ms) { lroundf(bias[2] * 1000)); } else { probe_debug_printf("[PROBE] Wii native IMU %s\n", sensor_status ? - "calibrating: keep still" : "waiting for fresh calibrated accelerometer/MotionPlus"); + "waiting for a usable acceleration sample" : "waiting for fresh calibrated accelerometer/MotionPlus"); } } update_wii_ir_gate(now_us); diff --git a/tools/switch2_usb_probe/controller_input.h b/tools/switch2_usb_probe/controller_input.h index ee93b7e..46e48b5 100644 --- a/tools/switch2_usb_probe/controller_input.h +++ b/tools/switch2_usb_probe/controller_input.h @@ -56,7 +56,7 @@ void probe_controller_input_set_full_stick_calibration(uint8_t instance, const u // enable preserves it. Joy-Con mode relays its bounded FIFO; right-only Wii // mode synthesizes fresh calibrated sensors and the selected IR pointer. // Full-controller mode splits one supported gamepad into independent R/L output streams; -// controls remain live while motion is unavailable. Only Wii estimates stationary bias. +// controls remain live while motion is unavailable. Wii refines bias without blocking IMU. // No pairing changes. void probe_controller_input_set_native_stream(uint8_t instance, bool enabled); // Copy one63-byte payload without report ID. Returns a boot-unique token, or0 diff --git a/tools/switch2_usb_probe/main.c b/tools/switch2_usb_probe/main.c index 98261f6..e116b77 100644 --- a/tools/switch2_usb_probe/main.c +++ b/tools/switch2_usb_probe/main.c @@ -819,10 +819,10 @@ int main(void) { instance, probe_storage_offset(instance)); #ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE #if SWITCH2_BRIDGE_WII_INPUT - probe_debug_printf("[PROBE] Wii IR drives native mouse movement; buttons retain profile mapping; keep Wii still for MotionPlus calibration\n"); + probe_debug_printf("[PROBE] Wii IR drives native mouse movement; buttons retain profile mapping; MotionPlus bias learns in background\n"); probe_debug_printf("[PROBE] Hold BOOTSEL2s for pairing; Wii cue feedback uses bounded ERM patterns, not HD audio waveforms\n"); #elif SWITCH2_BRIDGE_FULL_INPUT - probe_debug_printf("[PROBE] Full gamepad controls on R/L; IMU mask=%u; only Wii requires settling\n", + probe_debug_printf("[PROBE] Full gamepad controls on R/L; IMU mask=%u; Wii bias learns without startup settling\n", (unsigned)SWITCH2_BRIDGE_IMU_TARGET_MASK); probe_debug_printf("[PROBE] Hold BOOTSEL 2s for Bluetooth pairing (never clears pairings); cues use source capabilities\n"); #else diff --git a/tools/switch2_usb_probe/native_gamepad_input.cpp b/tools/switch2_usb_probe/native_gamepad_input.cpp index 87b2619..1f3d047 100644 --- a/tools/switch2_usb_probe/native_gamepad_input.cpp +++ b/tools/switch2_usb_probe/native_gamepad_input.cpp @@ -181,7 +181,7 @@ void refresh(uint32_t now_ms) { source.accel_sequence == g_source.accel_sequence && source.gyro_sequence == g_source.gyro_sequence && source.accel_received_us == g_source.accel_received_us && source.gyro_received_us == g_source.gyro_received_us && source.accel_valid == g_source.accel_valid && source.gyro_valid == g_source.gyro_valid && - source.requires_stationary_bias == g_source.requires_stationary_bias) return; + source.track_stationary_bias == g_source.track_stationary_bias) return; if (changed_connection) { lose_source(now_ms); g_motion.reset(); @@ -216,14 +216,13 @@ void refresh(uint32_t now_ms) { sample.gyro_dps[1] = -static_cast(source.gyro_q10[2]) / 1024.0f; sample.gyro_dps[2] = static_cast(source.gyro_q10[1]) / 1024.0f; g_motion.update(now_us, source.controller.connection_generation, sample, - source.requires_stationary_bias ? ProbeNativeMotionBias::kEstimateStationary : + source.track_stationary_bias ? ProbeNativeMotionBias::kTrackStationary : ProbeNativeMotionBias::kAlreadyCalibrated); const int status = !sensors_fresh(g_source, now_us) ? 0 : g_motion.ready() ? 2 : 1; if (status != g_sensor_status) { g_sensor_status = status; probe_debug_printf("[PROBE] Native gamepad IMU %s\n", status == 2 ? "ready" : - status == 1 ? (source.requires_stationary_bias ? "Wii calibrating: keep still" : - "waiting for a usable acceleration sample") : "waiting for supported fresh sensors"); + status == 1 ? "waiting for a usable acceleration sample" : "waiting for supported fresh sensors"); } for (uint8_t i = 0; i < PROBE_CONTROLLER_COUNT; ++i) { // Latest-only: a blocked endpoint never queues obsolete controls/IMU. @@ -329,7 +328,7 @@ bool probe_native_gamepad_input_commit_native_report(uint8_t instance, uint32_t source.accel_sequence != g_source.accel_sequence || source.gyro_sequence != g_source.gyro_sequence || source.accel_received_us != g_source.accel_received_us || source.gyro_received_us != g_source.gyro_received_us || source.accel_valid != g_source.accel_valid || source.gyro_valid != g_source.gyro_valid || - source.requires_stationary_bias != g_source.requires_stationary_bias || + source.track_stationary_bias != g_source.track_stationary_bias || now_us - source.received_us >= kInputDeadlineUs || now_us - child.pending_us >= kOutputDeadlineUs || (child.pending_motion && !sensors_fresh(source, now_us))) return false; child.pending_token = 0; diff --git a/tools/switch2_usb_probe/native_imu.cpp b/tools/switch2_usb_probe/native_imu.cpp index 4b29c68..1db3552 100644 --- a/tools/switch2_usb_probe/native_imu.cpp +++ b/tools/switch2_usb_probe/native_imu.cpp @@ -14,6 +14,11 @@ constexpr uint32_t kVariationSamples = 16; // over distinct samples, with roughly twice that measured noise allowance. constexpr float kGyroRmsDps = 0.75f; constexpr float kAccelRmsG = 0.025f; +// The recorded Wii sample has roughly 13 dps residual on each axis. Keep that +// correctable, but never learn unrestricted gameplay rates or ratchet this +// limit relative to a previously learned bias. Low steady yaw remains ambiguous. +constexpr float kMaximumBiasDps = 30.0f; +constexpr float kBiasSlewDpsPerSecond = 5.0f; // One degree between averaged gravity directions, independent of g scale. constexpr float kGravityDirectionCosSquared = 0.9996954135f; constexpr float kGravityTimeConstantUs = 1000000.0f; @@ -150,6 +155,7 @@ void ProbeNativeMotion::invalidate() { acceleration_[i] = 0.0f; gyro_dps_[i] = 0.0f; bias_[i] = 0.0f; + bias_target_[i] = 0.0f; } // Keep the last observed identities until a connection change or explicit // reset. Restoring availability cannot turn the same packet into new data. @@ -346,6 +352,7 @@ void ProbeNativeMotion::update(uint32_t now_us, uint32_t connection_generation, } const bool was_ready = ready_; const uint32_t accel_elapsed_us = was_ready && new_accel ? sample.accel_us - accel_us_ : 0; + const uint32_t gyro_elapsed_us = was_ready && new_gyro ? sample.gyro_us - gyro_us_ : 0; float previous_gyro[3]; if (was_ready && new_gyro) { for (unsigned i = 0; i < 3; ++i) previous_gyro[i] = gyro_dps_[i]; @@ -383,20 +390,31 @@ void ProbeNativeMotion::update(uint32_t now_us, uint32_t connection_generation, invalidate(); } if (ready_ && new_accel && !correct_gravity(accel_elapsed_us)) invalidate(); - return; - } - - if (bias_mode_ == ProbeNativeMotionBias::kAlreadyCalibrated) { - // Trust only the caller's validated calibrated samples, not an estimated - // stationary bias. The first acceleration fixes a relative gravity frame. + } else { + // Admission depends on real fresh sensors, never on a quiet interval. for (unsigned i = 0; i < 3; ++i) mean_accel_[i] = acceleration_[i]; ready_ = initialize_orientation(); - return; + } + if (ready_ && bias_mode_ == ProbeNativeMotionBias::kTrackStationary) + track_bias(new_accel, new_gyro, gyro_elapsed_us); +} + +void ProbeNativeMotion::track_bias(bool new_accel, bool new_gyro, uint32_t gyro_elapsed_us) { + if (new_gyro && gyro_elapsed_us) { + float delta[3]; + for (unsigned i = 0; i < 3; ++i) delta[i] = bias_target_[i] - bias_[i]; + const float distance_squared = squared_norm(delta); + const float step = kBiasSlewDpsPerSecond * (static_cast(gyro_elapsed_us) * 1e-6f); + if (distance_squared > 0.0f) { + const float weight = distance_squared > step * step ? step / std::sqrt(distance_squared) : 1.0f; + for (unsigned i = 0; i < 3; ++i) bias_[i] += delta[i] * weight; + } } const float acceleration_norm_squared = squared_norm(acceleration_); if (acceleration_norm_squared < 0.85f * 0.85f || - acceleration_norm_squared > 1.15f * 1.15f) { + acceleration_norm_squared > 1.15f * 1.15f || + squared_norm(gyro_dps_) > kMaximumBiasDps * kMaximumBiasDps) { clear_candidate(); return; } @@ -428,9 +446,9 @@ void ProbeNativeMotion::update(uint32_t now_us, uint32_t connection_generation, } } if (!new_gyro) return; - // An absolute angular-rate limit cannot distinguish motion from the bias - // being estimated. Changing rates, acceleration and gravity direction can; - // perfectly steady rotation about gravity still requires the user to rest. + // Variation and changing gravity reject movement. A bounded, perfectly + // steady rotation about gravity can still look like bias; never claim an + // absolute heading or suppress real input while collecting this estimate. if (!accumulate_stationary(gyro_dps_, ++gyro_count_, mean_gyro_, gyro_variation_, kGyroRmsDps)) { clear_candidate(); @@ -438,12 +456,7 @@ void ProbeNativeMotion::update(uint32_t now_us, uint32_t connection_generation, } if (gyro_count_ >= kCalibrationSamples && accel_count_ >= kVariationSamples && gyro_us_ - candidate_us_ >= kCalibrationUs) { - if (!initialize_orientation()) { - clear_candidate(); - return; - } - for (unsigned i = 0; i < 3; ++i) bias_[i] = mean_gyro_[i]; - ready_ = true; + for (unsigned i = 0; i < 3; ++i) bias_target_[i] = mean_gyro_[i]; clear_candidate(); } } diff --git a/tools/switch2_usb_probe/native_imu.h b/tools/switch2_usb_probe/native_imu.h index b07db75..52bf0a7 100644 --- a/tools/switch2_usb_probe/native_imu.h +++ b/tools/switch2_usb_probe/native_imu.h @@ -31,15 +31,15 @@ struct ProbeNativeMotionSample { enum class ProbeNativeMotionBias : uint8_t { kAlreadyCalibrated, - kEstimateStationary, + kTrackStationary, }; // Core 0 only. Call update even when sensors are unavailable, and consult ready // before using the orientation. Sequence identities, not polling, admit samples; // repeated sequences cannot refresh timestamps or contribute to calibration. -// Already-calibrated sources initialize from the first usable fresh sensor pair. -// Wii explicitly requests stationary residual-bias estimation: constant rotation -// about gravity is indistinguishable from an unknown bias without another sensor. +// All sources initialize from the first usable fresh sensor pair. Wii optionally +// refines residual bias in the background without resetting orientation. Steady +// rotation about gravity remains indistinguishable from bias without a reference. // Fresh near-1g acceleration corrects tilt drift. Heading remains gyro-derived // unless a reliable optical heading observation is supplied. class ProbeNativeMotion { @@ -67,6 +67,7 @@ private: bool integrate(const float gyro_dps[3], uint32_t elapsed_us); bool correct_gravity(uint32_t elapsed_us); bool normalize_orientation(); + void track_bias(bool new_accel, bool new_gyro, uint32_t gyro_elapsed_us); bool have_generation_ = false; ProbeNativeMotionBias bias_mode_ = ProbeNativeMotionBias::kAlreadyCalibrated; @@ -100,6 +101,7 @@ private: float acceleration_[3]{}; float gyro_dps_[3]{}; float bias_[3]{}; + float bias_target_[3]{}; float mean_gyro_[3]{}; float mean_accel_[3]{}; float gravity_reference_[3]{}; diff --git a/tools/switch2_usb_probe/probe_build.cmake b/tools/switch2_usb_probe/probe_build.cmake index 66aae78..bfa7c4f 100644 --- a/tools/switch2_usb_probe/probe_build.cmake +++ b/tools/switch2_usb_probe/probe_build.cmake @@ -369,9 +369,9 @@ function(switch2_usb_probe_configure target) endif() if(SWITCH2_PROBE_HUB AND SWITCH2_BRIDGE_FULL_INPUT) if(SWITCH2_PROBE_TRACE_NATIVE_INPUT) - pico_set_program_version(${target} "0.70-native-gamepad-trace") + pico_set_program_version(${target} "0.71-native-gamepad-trace") else() - pico_set_program_version(${target} "0.70-native-gamepad") + pico_set_program_version(${target} "0.71-native-gamepad") endif() elseif(SWITCH2_PROBE_HUB) pico_set_program_version(${target} "0.67-native-hub-latency")