jyapayne/switch-pico
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Start Wii motion immediately and track residual bias in background

Browse source
This commit is contained in:
Joey Yakimowich-Payne 2026-09-13 00:00:53 -06:00
commit 26a25a66af
14 changed files with 210 additions and 101 deletions
Download patch file Download diff file Expand all files Collapse all files

58
README.md
View file

@ -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

2
src/firmware/input/bluepad32_input_backend.cpp
View file

@ -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;

2
src/firmware/input/bluepad32_input_backend.h
View file

@ -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;

16
tests/native_gamepad_backend_test.cpp
View file

@ -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");
}

12
tests/switch2_native_gamepad_bridge_test.cpp
View file

@ -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));

136
tests/switch2_native_imu_test.cpp
View file

@ -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<float>::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();

5
tests/switch2_wii_bridge_test.cpp
View file

@ -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);

4
tools/switch2_usb_probe/controller_input.cpp
View file

@ -290,7 +290,7 @@ void poll_wii_source(uint32_t now_ms) {
motion.gyro_dps[0] = static_cast<float>(g_wii.gyro_q10[1]) / 1024.0f;
motion.gyro_dps[1] = -static_cast<float>(g_wii.gyro_q10[2]) / 1024.0f;
motion.gyro_dps[2] = -static_cast<float>(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);

2
tools/switch2_usb_probe/controller_input.h
View file

@ -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

4
tools/switch2_usb_probe/main.c
View file

@ -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

9
tools/switch2_usb_probe/native_gamepad_input.cpp
View file

@ -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<float>(source.gyro_q10[2]) / 1024.0f;
sample.gyro_dps[2] = static_cast<float>(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;

47
tools/switch2_usb_probe/native_imu.cpp
View file

@ -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<float>(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();
}
}

10
tools/switch2_usb_probe/native_imu.h
View file

@ -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]{};

4
tools/switch2_usb_probe/probe_build.cmake
View file

@ -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")