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

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