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

WIP: checkpoint Wii native IMU and upstream IR bridge

Browse source 
Pause Wii pointing work with firmware 0.33-wii-trace installed. Preserve the pinned libogc IR pipeline, calibrated native IMU, software BOOTSEL, and standard camera sensitivity trial.

Tracking instability, tracking loss, and ineffective vertical movement remain unresolved. Level-2 camera filtering is not hardware-qualified. Nine targeted regression tests pass; firmware and persistent storage were verified after flashing.
This commit is contained in:
Joey Yakimowich-Payne 2026-09-11 17:05:54 -06:00
commit 9f6dddb790
41 changed files with 7990 additions and 150 deletions
Download patch file Download diff file Expand all files Collapse all files

35
tools/switch2_usb_probe/bootsel.cpp Normal file
View file

@ -0,0 +1,35 @@
#include "bootsel.h"
#include "adapter/adapter_mode_controller.h"
#include "usb/usb_configuration_management.h"
namespace {
bool bootsel_accepted;
}
bool probe_bootsel_vendor_control(uint8_t rhport, uint8_t stage,
const tusb_control_request_t* request) {
using namespace UsbConfigurationManagement;
if (request == nullptr || request->bmRequestType != 0x40 ||
request->bRequest != static_cast<uint8_t>(Operation::kBootselReboot) ||
request->wValue != kRequestValue || request->wIndex != kRequestIndex ||
request->wLength != kRequestHeaderSize) {
return false;
}
// The shared handler receives the envelope at SETUP and validates and
// dispatches it only at ACK, after the host's control transfer completes.
const bool accepted =
usb_configuration_management_vendor_control(rhport, stage, request);
if (accepted && stage == CONTROL_STAGE_ACK) {
bootsel_accepted = true;
}
return accepted;
}
void probe_bootsel_task(uint32_t now_ms) {
// The native bridge does not initialize ordinary adapter-mode selection.
// A successful BOOTSEL dispatch guarantees the task takes its reboot path.
if (bootsel_accepted) {
adapter_mode_controller_task(now_ms);
}
}

20
tools/switch2_usb_probe/bootsel.h Normal file
View file

@ -0,0 +1,20 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "tusb.h"
#ifdef __cplusplus
extern "C" {
#endif
// Core 0: expose only the existing private BOOTSEL command, not configuration.
bool probe_bootsel_vendor_control(uint8_t rhport, uint8_t stage,
const tusb_control_request_t* request);
// Core 0: service the existing reboot delay only after a validated status ACK.
void probe_bootsel_task(uint32_t now_ms);
#ifdef __cplusplus
}
#endif

406
tools/switch2_usb_probe/controller_input.cpp
View file

@ -8,6 +8,13 @@
#include "platform/pico/system_clock.h"
#include "profile/controller_profile_runtime.h"
#include "pico/stdlib.h"
#if SWITCH2_BRIDGE_WII_INPUT
#include <math.h>
#include "input/wii_ir_pointer.h"
#include "native_imu.h"
#include "pico/time.h"
extern "C" int probe_debug_printf(const char* format, ...);
#endif
#if !SWITCH_PICO_SWITCH2_USB_BRIDGE || !SWITCH_PICO_BLUEPAD32 || \
!SWITCH_PICO_ENABLE_BLE || !SWITCH_PICO_SWITCH2_MOUSE_CAPTURE || \
@ -27,7 +34,323 @@ bool g_initialized;
bool g_start_attempted;
bool g_flash_ready;
probe_controller_input g_input;
#if !SWITCH2_BRIDGE_WII_INPUT
uint32_t g_received_ms;
#endif
#if SWITCH2_BRIDGE_WII_INPUT
#ifndef SWITCH2_WII_IR_SCREEN_CONFIG
#define SWITCH2_WII_IR_SCREEN_CONFIG 660, 370, 0, -115, 1920, 1080
#endif
constexpr float kIrScreenConfig[] = {SWITCH2_WII_IR_SCREEN_CONFIG};
static_assert(sizeof(kIrScreenConfig) / sizeof(kIrScreenConfig[0]) == 6);
bool g_screen_configured;
constexpr uint32_t kSensorDeadlineUs = 150000;
constexpr uint32_t kOutputDeadlineUs = 100000;
Bluepad32WiiBridgeSnapshot g_wii;
ProbeNativeMotion g_motion;
bool g_wii_active;
bool g_native_stream;
uint8_t g_native_features;
uint32_t g_wii_generation;
uint32_t g_orientation_requested_generation;
int g_sensor_status = -1;
uint16_t g_stick_center[2]{2048, 2048};
uint16_t g_stick_positive[2]{2047, 2047};
uint16_t g_stick_negative[2]{2048, 2048};
uint8_t g_power_info = 0x24; // Nominal battery until source status; no USB-power flag.
uint8_t g_report_counter;
uint32_t g_report_serial;
uint32_t g_pending_serial;
uint32_t g_pending_us;
uint32_t g_pending_generation;
uint32_t g_pending_ticks;
uint8_t g_pending_report[63];
WiiIrMouseReport g_pending_pointer{};
bool g_pending_motion_ready;
bool g_clock_started;
uint32_t g_clock_us;
uint32_t g_clock_ticks;
uint32_t g_clock_fraction;
bool g_have_committed_ticks;
uint32_t g_committed_ticks;
bool g_have_submission;
uint32_t g_submitted_us;
uint32_t g_output_open_us;
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
uint32_t g_last_ir_trace_us;
#endif
void unpack_stick_pair(const uint8_t* data, uint16_t values[2]) {
values[0] = data[0] | (static_cast<uint16_t>(data[1] & 15) << 8);
values[1] = (data[1] >> 4) | (static_cast<uint16_t>(data[2]) << 4);
}
uint16_t calibrated_stick_axis(int16_t value, unsigned axis, bool invert) {
const int32_t input = value;
const bool input_positive = input >= 0;
const bool output_positive = input_positive != invert;
const int32_t magnitude = input_positive ? input : -input;
const int32_t denominator = input_positive ? INT16_MAX : 32768;
const int32_t travel = output_positive ? g_stick_positive[axis] : g_stick_negative[axis];
const int32_t displacement = (magnitude * travel + denominator / 2) / denominator;
return static_cast<uint16_t>(g_stick_center[axis] +
(output_positive ? displacement : -displacement));
}
void pack_wii_controls(const ControllerProfileTransformResult& mapped) {
const ControllerState& state = mapped.state;
g_input.buttons[0] = static_cast<uint8_t>(
(state.button_south ? 0x01 : 0) | (state.button_east ? 0x02 : 0) |
(state.button_west ? 0x04 : 0) | (state.button_north ? 0x08 : 0) |
(state.button_right_shoulder ? 0x10 : 0) |
(state.right_trigger != 0 &&
state.right_trigger >= mapped.right_trigger_digital_threshold ? 0x20 : 0) |
(state.button_start ? 0x40 : 0) | (state.button_right_stick ? 0x80 : 0));
g_input.buttons[1] = static_cast<uint8_t>(
(state.button_system ? 0x01 : 0) | ((state.extra_buttons & 1) ? 0x10 : 0) |
((state.extra_buttons & (1u << 5)) ? 0x80 : 0) |
((state.extra_buttons & (1u << 6)) ? 0x40 : 0));
// One virtual right stick: honor a mapped right stick first, otherwise the
// Nunchuk stick. Left-only buttons are not repurposed as mouse clicks.
int16_t x = state.right_stick_x;
int16_t y = state.right_stick_y;
if (x == 0 && y == 0 && g_wii.layout == Bluepad32ControllerLayout::kWiiNunchuk) {
x = state.left_stick_x;
y = state.left_stick_y;
}
const uint16_t sx = calibrated_stick_axis(x, 0, false);
const uint16_t sy = calibrated_stick_axis(y, 1, true);
g_input.stick[0] = static_cast<uint8_t>(sx);
g_input.stick[1] = static_cast<uint8_t>((sx >> 8) | (sy << 4));
g_input.stick[2] = static_cast<uint8_t>(sy >> 4);
}
void advance_wii_clock(uint32_t now_us) {
if (!g_clock_started) {
g_clock_started = true;
g_clock_us = now_us;
return;
}
const uint64_t scaled = static_cast<uint64_t>(now_us - g_clock_us) * 960u +
g_clock_fraction;
g_clock_us = now_us;
g_clock_ticks += static_cast<uint32_t>(scaled / 1000000u);
g_clock_fraction = static_cast<uint32_t>(scaled % 1000000u);
}
bool wii_sensors_fresh(uint32_t now_us) {
return g_wii.accel_valid && g_wii.gyro_valid &&
static_cast<int32_t>(now_us - g_wii.accel_received_us) <
static_cast<int32_t>(kSensorDeadlineUs) &&
static_cast<int32_t>(now_us - g_wii.gyro_received_us) <
static_cast<int32_t>(kSensorDeadlineUs);
}
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
void trace_wii_ir(uint32_t now_us, bool output_enabled) {
// Ten snapshots/second keep camera diagnostics well below UART capacity.
if (now_us - g_last_ir_trace_us < 100000) return;
g_last_ir_trace_us = now_us;
uint8_t data[WII_IR_MOUSE_DIAGNOSTIC_SIZE];
if (wii_ir_pointer_diagnostics(data, sizeof(data)) != sizeof(data)) return;
static constexpr char hex[] = "0123456789abcdef";
char encoded[sizeof(data) * 2 + 1];
for (size_t i = 0; i < sizeof(data); ++i) {
encoded[2 * i] = hex[data[i] >> 4];
encoded[2 * i + 1] = hex[data[i] & 15];
}
encoded[sizeof(encoded) - 1] = 0;
// State bits: calibrated IMU, native stream, effective IR output gate.
const unsigned state = static_cast<unsigned>(g_motion.ready()) |
(static_cast<unsigned>(g_native_stream) << 1) |
(static_cast<unsigned>(output_enabled) << 2);
probe_debug_printf("[PROBE %lu] WII_IR_DIAGNOSTIC state=%u len=%u: %s\n",
static_cast<unsigned long>(to_ms_since_boot(get_absolute_time())), state,
static_cast<unsigned>(sizeof(data)), encoded);
}
#endif
void update_wii_ir_gate(uint32_t now_us) {
const uint32_t last_progress = g_have_submission ? g_submitted_us : g_output_open_us;
const bool output_fresh = static_cast<int32_t>(now_us - last_progress) <
static_cast<int32_t>(kOutputDeadlineUs);
const bool enabled = g_native_stream && g_wii_active &&
g_motion.ready() && wii_sensors_fresh(now_us) && output_fresh &&
(g_native_features & 0x10);
wii_ir_mouse_set_output_enabled(enabled);
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
trace_wii_ir(now_us, enabled);
#endif
}
void discard_wii_output() {
g_pending_serial = 0;
g_have_committed_ticks = false;
g_have_submission = false;
g_output_open_us = time_us_32();
wii_ir_mouse_set_output_enabled(false);
}
void lose_wii_source() {
if (g_wii_active) {
g_motion.reset();
discard_wii_output();
g_sensor_status = -1;
}
g_wii_active = false;
g_input = {};
}
void poll_wii_source(uint32_t now_ms) {
bluepad32_input_backend_wii_snapshot(&g_wii);
// Read the clock after the coherent snapshot so Core1 receipt timestamps
// cannot appear to be in the future to the motion integrator.
const uint32_t now_us = time_us_32();
advance_wii_clock(now_us);
if (!g_wii.controller.active || g_wii.slot >= BLUEPAD32_INPUT_BACKEND_SLOT_COUNT ||
static_cast<int32_t>(now_us - g_wii.received_us) >=
static_cast<int32_t>(kInputDeadlineMs * 1000u)) {
lose_wii_source();
g_orientation_requested_generation = 0;
return;
}
if (g_wii.layout == Bluepad32ControllerLayout::kWiiHorizontal) {
if (g_orientation_requested_generation != g_wii.controller.connection_generation &&
bluepad32_input_backend_set_wii_orientation(g_wii.controller.identity,
g_wii.controller.connection_generation, true)) {
g_orientation_requested_generation = g_wii.controller.connection_generation;
}
lose_wii_source();
return; // Never emit a transient sideways mapping while Core1 switches.
}
if (!g_wii_active || g_wii_generation != g_wii.controller.connection_generation) {
discard_wii_output();
g_motion.reset();
g_wii_generation = g_wii.controller.connection_generation;
g_sensor_status = -1;
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
g_last_ir_trace_us = now_us;
#endif
g_power_info = 0x24;
probe_debug_printf("[PROBE] Wii source active in slot %u; keep still for native motion calibration\n",
g_wii.slot);
}
g_wii_active = true;
g_input.active = true;
g_input.serial = g_wii.state_generation;
g_input.native_status = static_cast<uint8_t>(0x30 | ((g_native_features & 0x20) ? 8 : 0));
const ControllerProfileTransformResult mapped = controller_profile_runtime_transform(
g_wii.slot, g_wii.controller, now_ms, AdapterUsbMode::kSwitch);
pack_wii_controls(mapped);
ControllerProfileRuntimeProfileChangeEvent feedback{};
if (controller_profile_runtime_take_initial_profile_indication(g_wii.slot, &feedback) ||
controller_profile_runtime_take_profile_change(g_wii.slot, &feedback)) {
bluepad32_input_backend_queue_profile_feedback(g_wii.slot,
feedback.connection_generation, feedback.active_profile_number, feedback.policy);
}
if (g_wii.battery != 0) {
const unsigned level = (static_cast<unsigned>(g_wii.battery) * 9u + 127u) / 255u;
g_power_info = static_cast<uint8_t>(level << 2);
}
ProbeNativeMotionSample motion{};
motion.accel_valid = g_wii.accel_valid;
motion.gyro_valid = g_wii.gyro_valid;
motion.accel_sequence = g_wii.accel_sequence;
motion.gyro_sequence = g_wii.gyro_sequence;
motion.accel_us = g_wii.accel_received_us;
motion.gyro_us = g_wii.gyro_received_us;
// SDL -> physical right frame [X,-Z,Y], then a +90-degree mouse mounting
// rotation about forward Y. A face-up Wii becomes rail-down native +X gravity.
motion.accel_g[0] = static_cast<float>(g_wii.accel_q13[1]) / 8192.0f;
motion.accel_g[1] = -static_cast<float>(g_wii.accel_q13[2]) / 8192.0f;
motion.accel_g[2] = -static_cast<float>(g_wii.accel_q13[0]) / 8192.0f;
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);
WiiIrMouseReport optical{};
(void)wii_ir_mouse_peek(&optical, 0);
// Core 1 may publish during the peek. Read the clock after the snapshot.
g_motion.observe_optical_heading(time_us_32(), optical.generation,
optical.optical_sequence, optical.optical_received_us, optical.optical_yaw_radians,
optical.tracking && optical.optical_valid && optical.owner == g_wii.slot &&
optical.connection_generation == g_wii_generation);
const int sensor_status = !wii_sensors_fresh(now_us) ? 0 : g_motion.ready() ? 2 : 1;
if (sensor_status != g_sensor_status) {
g_sensor_status = sensor_status;
if (sensor_status == 2) {
const float* bias = g_motion.bias();
probe_debug_printf("[PROBE] Wii native IMU ready; bias_mdeg_s=%ld,%ld,%ld\n",
lroundf(bias[0] * 1000), lroundf(bias[1] * 1000),
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");
}
}
update_wii_ir_gate(now_us);
}
uint32_t prepare_wii_report(uint8_t report[63]) {
if (!report || !g_native_stream || !g_wii_active) return 0;
const uint32_t now_us = time_us_32();
if (static_cast<int32_t>(now_us - g_wii.received_us) >=
static_cast<int32_t>(kInputDeadlineMs * 1000u)) return 0;
advance_wii_clock(now_us);
update_wii_ir_gate(now_us);
WiiIrMouseReport pointer{};
(void)wii_ir_mouse_peek(&pointer, INT16_MAX);
const bool motion_ready = g_motion.ready() && wii_sensors_fresh(now_us);
if (g_pending_serial &&
(g_pending_generation != g_wii_generation ||
g_pending_pointer.generation != pointer.generation ||
g_pending_motion_ready != motion_ready ||
static_cast<int32_t>(now_us - g_pending_us) >= static_cast<int32_t>(kOutputDeadlineUs))) {
g_pending_serial = 0;
}
if (g_pending_serial) {
memcpy(report, g_pending_report, sizeof(g_pending_report));
return g_pending_serial;
}
if (g_report_serial == UINT32_MAX) return 0; // Never reuse a submission token.
memset(g_pending_report, 0, sizeof(g_pending_report));
g_pending_report[0] = g_report_counter;
g_pending_report[1] = g_power_info;
memcpy(g_pending_report + 2, g_input.buttons, sizeof(g_input.buttons));
g_pending_report[4] = 7;
memcpy(g_pending_report + 5, g_input.stick, sizeof(g_input.stick));
g_pending_report[8] = g_input.native_status;
g_pending_report[13] = 0xff; // Observed no-surface value.
g_pending_ticks = g_clock_ticks;
const uint32_t elapsed = g_have_committed_ticks ? g_pending_ticks - g_committed_ticks : 1;
const uint16_t wire_elapsed = static_cast<uint16_t>(elapsed <= 0xfff ? elapsed : 1);
const bool have_motion = g_motion.ready() && wii_sensors_fresh(now_us) &&
probe_native_imu_pack(g_motion.quaternion(), g_motion.acceleration(),
static_cast<uint16_t>(g_pending_ticks & 0xfff), wire_elapsed, 0,
g_pending_report + 16);
if (have_motion) g_pending_report[15] = 30;
if (have_motion && pointer.tracking && pointer.owner == g_wii.slot &&
pointer.connection_generation == g_wii_generation) {
const uint16_t dx = static_cast<uint16_t>(pointer.dx);
// Native Joy-Con Y is opposite to the shared desktop-pointer convention.
// Keep the original pointer delta for commit/consumption below.
const uint16_t dy = static_cast<uint16_t>(-pointer.dy);
g_pending_report[9] = static_cast<uint8_t>(dx);
g_pending_report[10] = static_cast<uint8_t>(dx >> 8);
g_pending_report[11] = static_cast<uint8_t>(dy);
g_pending_report[12] = static_cast<uint8_t>(dy >> 8);
g_pending_report[13] = 20; // Observed contact-range value for virtual IR tracking.
}
// IR buttons are deliberately NOT mapped to desktop/native click buttons.
g_pending_pointer = pointer;
g_pending_motion_ready = have_motion;
g_pending_generation = g_wii_generation;
g_pending_us = now_us;
g_pending_serial = ++g_report_serial;
memcpy(report, g_pending_report, sizeof(g_pending_report));
return g_pending_serial;
}
#endif
} // namespace
extern "C" void probe_controller_input_clock_init(void) {
@ -36,11 +359,19 @@ extern "C" void probe_controller_input_clock_init(void) {
extern "C" void probe_controller_input_init(void) {
if (g_initialized) return;
#if SWITCH2_BRIDGE_WII_INPUT
bluepad32_input_backend_init();
bluepad32_input_backend_select_wii_source(kSourceAddress);
g_screen_configured = wii_ir_pointer_configure_screen(
kIrScreenConfig[0], kIrScreenConfig[1], kIrScreenConfig[2],
kIrScreenConfig[3], kIrScreenConfig[4], kIrScreenConfig[5]);
if (!g_screen_configured) probe_debug_printf("[PROBE] Invalid native IR viewport configuration\n");
wii_ir_mouse_set_output_enabled(false);
#else
switch2_mouse_capture_init();
switch2_mouse_capture_select_input(kSourceAddress);
// Prepare the existing storage services without initializing legacy USB.
// Core 1 loads their persisted state during the normal backend startup.
bluepad32_input_backend_init();
#endif
controller_profile_runtime_reset();
g_initialized = true;
}
@ -48,6 +379,9 @@ extern "C" void probe_controller_input_init(void) {
extern "C" bool probe_controller_input_start(void) {
if (!g_initialized) probe_controller_input_init();
if (g_start_attempted) return g_flash_ready;
#if SWITCH2_BRIDGE_WII_INPUT
if (!g_screen_configured) return false;
#endif
g_start_attempted = true;
bluepad32_input_backend_start();
const absolute_time_t deadline = make_timeout_time_ms(kFlashCoordinationTimeoutMs);
@ -75,19 +409,61 @@ extern "C" bool probe_controller_input_pairing_task(void) {
return true;
}
#if SWITCH2_BRIDGE_WII_INPUT
extern "C" void probe_controller_input_set_stick_calibration(const uint8_t calibration[9]) {
if (!calibration) return;
unpack_stick_pair(calibration, g_stick_center);
unpack_stick_pair(calibration + 3, g_stick_positive);
unpack_stick_pair(calibration + 6, g_stick_negative);
g_pending_serial = 0;
}
extern "C" void probe_controller_input_set_native_features(uint8_t features) {
if (g_native_features == features) return;
g_native_features = features;
g_pending_serial = 0;
update_wii_ir_gate(time_us_32());
}
#endif
extern "C" void probe_controller_input_set_native_stream(bool enabled) {
#if SWITCH2_BRIDGE_WII_INPUT
enabled = enabled && g_flash_ready;
if (g_native_stream != enabled || !enabled) discard_wii_output();
g_native_stream = enabled;
update_wii_ir_gate(time_us_32());
#else
switch2_mouse_capture_set_native_stream(g_flash_ready && enabled);
#endif
}
extern "C" uint32_t probe_controller_input_peek_native_report(
uint32_t now_ms, uint8_t report[63]) {
if (!g_flash_ready) return 0;
#if SWITCH2_BRIDGE_WII_INPUT
(void)now_ms;
return prepare_wii_report(report);
#else
return switch2_mouse_capture_peek_native_report(now_ms, report);
#endif
}
extern "C" bool probe_controller_input_commit_native_report(uint32_t serial) {
if (!g_flash_ready) return false;
#if SWITCH2_BRIDGE_WII_INPUT
if (!g_native_stream || !serial || serial != g_pending_serial ||
g_pending_generation != g_wii_generation || !g_wii_active) return false;
wii_ir_mouse_commit(g_pending_pointer);
g_pending_serial = 0;
g_committed_ticks = g_pending_ticks;
g_have_committed_ticks = true;
g_have_submission = true;
g_submitted_us = time_us_32();
++g_report_counter;
return true;
#else
return switch2_mouse_capture_commit_native_report(serial);
#endif
}
extern "C" bool probe_controller_input_play_sample(uint8_t sample_id, uint64_t* token) {
@ -95,17 +471,30 @@ extern "C" bool probe_controller_input_play_sample(uint8_t sample_id, uint64_t*
if (token != nullptr) *token = 0;
return false;
}
#if SWITCH2_BRIDGE_WII_INPUT
return bluepad32_input_backend_wii_sample_request(sample_id, token);
#else
return switch2_mouse_capture_request_sample(
sample_id, to_ms_since_boot(get_absolute_time()), token);
#endif
}
extern "C" int probe_controller_input_sample_result(uint64_t token, uint32_t now_ms) {
if (!g_flash_ready) return -1;
#if SWITCH2_BRIDGE_WII_INPUT
(void)now_ms;
return bluepad32_input_backend_wii_sample_result(token);
#else
return switch2_mouse_capture_sample_result(token, now_ms);
#endif
}
extern "C" void probe_controller_input_cancel_sample(void) {
#if SWITCH2_BRIDGE_WII_INPUT
bluepad32_input_backend_wii_sample_cancel();
#else
switch2_mouse_capture_cancel_sample();
#endif
}
extern "C" void probe_controller_input_poll(uint32_t now_ms,
@ -115,13 +504,14 @@ extern "C" void probe_controller_input_poll(uint32_t now_ms,
*out = {};
return;
}
#if SWITCH2_BRIDGE_WII_INPUT
poll_wii_source(now_ms);
#else
Switch2MouseCaptureInput sample;
if (switch2_mouse_capture_latest_input(g_input.serial, &sample)) {
g_input.serial = sample.serial;
g_input.active = sample.active;
g_received_ms = sample.received_ms;
// Preserve physical byte meaning: never route through the generic
// solo Joy-Con rotation/mapping. Teardown fields are already zeroed.
memcpy(g_input.buttons, sample.buttons, sizeof(g_input.buttons));
memcpy(g_input.stick, sample.stick, sizeof(g_input.stick));
g_input.native_status = sample.native_status;
@ -130,11 +520,8 @@ extern "C" void probe_controller_input_poll(uint32_t now_ms,
g_input.mouse_total_y = sample.mouse_total_y;
g_input.mouse_surface = sample.mouse_surface;
}
// The producer can be a millisecond ahead of the caller's pre-poll clock.
// Signed elapsed time tolerates that race and ordinary uint32_t rollover.
// Expiration latches inactive until a newer capture serial arrives.
if (g_input.active &&
static_cast<int32_t>(now_ms - g_received_ms) >=
// A producer timestamp can be slightly ahead of this pre-poll clock.
if (g_input.active && static_cast<int32_t>(now_ms - g_received_ms) >=
static_cast<int32_t>(kInputDeadlineMs)) {
g_input.active = false;
memset(g_input.buttons, 0, sizeof(g_input.buttons));
@ -145,5 +532,6 @@ extern "C" void probe_controller_input_poll(uint32_t now_ms,
g_input.mouse_total_y = 0;
g_input.mouse_surface = 0;
}
#endif
*out = g_input;
}

34
tools/switch2_usb_probe/controller_input.h
View file

@ -36,30 +36,34 @@ bool probe_controller_input_start(void);
// True means a Bluetooth pairing-window request was queued. Long holds NEVER
// clear pairings in this bridge, and this does not inject USB controller input.
bool probe_controller_input_pairing_task(void);
// Core 0 native 08 relay: disabled until explicitly enabled after flash-ready
// startup. Disable clears queued data, repeated enable preserves it. Resets
// must disable the stream; this never changes Bluetooth bonds or pairing.
// Only subsequent selected-source packets enter the separate 32-entry FIFO.
// Overflow drops queued history and retains only the arriving packet.
#if SWITCH2_BRIDGE_WII_INPUT
// Main supplies the same validated calibration record advertised to the host.
void probe_controller_input_set_stick_calibration(const uint8_t calibration[9]);
// Native feature changes are output barriers, not Bluetooth/IMU resets.
void probe_controller_input_set_native_features(uint8_t features);
#endif
// Core0 native08 output. Disable discards queued/prepared data; repeated enable
// preserves it. Joy-Con mode relays its bounded FIFO; Wii mode synthesizes from
// fresh calibrated sensors and the selected IR pointer. No pairing changes.
void probe_controller_input_set_native_stream(bool enabled);
// Copy a full opaque 63-byte payload (without report ID), oldest first. Returns
// its never-reused boot-lifetime serial; 0 leaves report untouched. Latest source
// or head >=500 ms old discards the FIFO; now_ms is the Pico boot-ms clock.
// Nondestructive until successful HID submission followed by commit.
// Copy one63-byte payload without report ID. Returns a boot-unique token, or0
// without changing output. Nondestructive until successful HID submission and
// commit. now_ms uses the Pico boot-ms clock; unavailable/stale input is rejected.
uint32_t probe_controller_input_peek_native_report(uint32_t now_ms, uint8_t report[63]);
// Remove only the exact current head once. A stale/replaced token cannot pop a
// new stream's packet. Before flash-ready startup peek/commit return 0/false.
bool probe_controller_input_commit_native_report(uint32_t serial);
// Built-in vibration samples only; raw HD-rumble output is not forwarded.
// A nonzero token means queued, not acknowledged. Result: 0 pending, 1 real
// source ACK, -1 failed/stale. Reset cancels the request, never stored pairing.
// A nonzero token means queued, not completed. Result:0 pending,1 completion,
// -1 failed/stale. Joy-Con completion is its application ACK; Wii completion is
// actual bounded rumble-driver dispatch (not an HD-waveform fidelity claim).
// Reset cancels the request, never stored pairing.
bool probe_controller_input_play_sample(uint8_t sample_id, uint64_t* token);
int probe_controller_input_sample_result(uint64_t token, uint32_t now_ms);
void probe_controller_input_cancel_sample(void);
// Core 0 at 250 Hz; now_ms uses the Pico boot-millisecond clock. Only fresh
// native 08 buttons/stick and cumulative mouse totals are exposed. Inactive
// fields are zero except serial; the USB protocol must supply its calibrated
// stick center rather than forwarding inactive stick bytes.
// Core0 at250Hz; now_ms uses Pico boot milliseconds. Supplies current mapped
// controls for diagnostic reports; the native sender owns motion consumption.
// Inactive controls are zero except serial; USB supplies its calibrated center.
void probe_controller_input_poll(uint32_t now_ms, probe_controller_input* out);
#ifdef __cplusplus

67
tools/switch2_usb_probe/main.c
View file

@ -9,6 +9,7 @@
#include <string.h>
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
#include "bootsel.h"
#include "controller_input.h"
#else
#include "platform/pico/bootsel_button_sample.h"
@ -63,6 +64,9 @@ static uint32_t last_hid_complete_ms;
static bool hid_completion_seen;
static uint32_t mouse_delivered_reports, mouse_logged_reports;
static int64_t mouse_delivered_x, mouse_delivered_y;
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
static uint32_t last_native_trace_ms;
#endif
#endif
#ifndef SWITCH_PICO_SWITCH2_USB_BRIDGE
static probe_button_state button_test;
@ -214,8 +218,13 @@ static void reset_protocol(void) {
protocol.play_sample = probe_controller_input_play_sample;
#endif
#ifdef SWITCH2_PROBE_MEMORY
if (!probe_memory_right_stick_center(protocol.right_stick_center))
uint8_t stick_calibration[9];
if (!probe_memory_right_stick_calibration(stick_calibration))
panic("Invalid captured Joy-Con stick calibration");
memcpy(protocol.right_stick_center, stick_calibration, sizeof(protocol.right_stick_center));
#if SWITCH2_BRIDGE_WII_INPUT
probe_controller_input_set_stick_calibration(stick_calibration);
#endif
protocol.read_memory = probe_memory_read;
#endif
uint8_t pairing[PROBE_PAIRING_BLOB_SIZE];
@ -230,12 +239,18 @@ static void reset_protocol(void) {
command_expected = 8;
last_input_ms = 0;
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
#if SWITCH2_BRIDGE_WII_INPUT
probe_controller_input_set_native_features(0);
#endif
last_controller_poll_ms = 0;
last_delivered_buttons = 0;
probe_controller_input_set_native_stream(false);
native_stream_ready = false;
last_hid_complete_ms = 0;
hid_completion_seen = false;
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
last_native_trace_ms = 0;
#endif
#endif
#ifndef SWITCH_PICO_SWITCH2_USB_BRIDGE
(void)probe_button_update(&button_test, -1, false);
@ -267,12 +282,15 @@ static void complete_command(void) {
previous_features != protocol.enabled_features) {
probe_controller_input_set_native_stream(false);
native_stream_ready = false;
#if SWITCH2_BRIDGE_WII_INPUT
probe_controller_input_set_native_features(protocol.enabled_features);
#endif
}
#endif
reply->length = (uint8_t)length;
++reply_count;
if (reply->deferred_token) {
probe_debug_printf("[PROBE] Sample %u awaiting source ACK token=%" PRIu64 "\n",
probe_debug_printf("[PROBE] Sample %u awaiting source completion token=%" PRIu64 "\n",
command_frame[8], reply->deferred_token);
} else {
log_packet("BULK_REPLY_QUEUED", 0, 0, reply->data, reply->length);
@ -364,11 +382,23 @@ static void controller_input_task(uint32_t now) {
}
static void gate_native_report(uint8_t input[PROBE_INPUT_SIZE]) {
#if SWITCH2_BRIDGE_WII_INPUT
// Generated status follows virtual feature state, not a donor snapshot.
input[8] = (uint8_t)(0x30 | ((protocol.enabled_features & 0x20) ? 8 : 0));
#endif
if (!(protocol.enabled_features & 1)) memset(input + 2, 0, 2);
if (!(protocol.enabled_features & 2))
memcpy(input + 5, protocol.right_stick_center, sizeof(protocol.right_stick_center));
if (!(protocol.enabled_features & 0x10)) memset(input + 9, 0, 5);
#ifdef SWITCH2_PROBE_OMIT_NATIVE_IMU
// Deliberate A/B fault injection: leave every other field and feature bit intact.
memset(input + 15, 0, 41);
#elif defined(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
if (!(protocol.enabled_features & 4)) input[15] = 0;
memset(input + 16, 0, 40); // Otherwise preserve the genuine length byte.
#else
if (!(protocol.enabled_features & 4)) memset(input + 15, 0, 41);
#endif
}
#endif
@ -381,8 +411,13 @@ static void protocol_task(uint32_t now) {
if (!ready) {
const int result = probe_controller_input_sample_result(reply->deferred_token, now);
if (result > 0) {
#if SWITCH2_BRIDGE_WII_INPUT
probe_debug_printf("[PROBE] Wii cue dispatched token=%" PRIu64 "\n",
reply->deferred_token);
#else
probe_debug_printf("[PROBE] Source sample ACK token=%" PRIu64 "\n",
reply->deferred_token);
#endif
reply->deferred_token = 0;
ready = true;
log_packet("BULK_REPLY_QUEUED", 0, 0, reply->data, reply->length);
@ -468,6 +503,13 @@ void tud_hid_report_complete_cb(uint8_t instance, const uint8_t* report, uint16_
if (report[0] == 0x08) {
const int32_t dx = signed_mouse_delta(report + 10);
const int32_t dy = signed_mouse_delta(report + 12);
#ifdef SWITCH2_PROBE_TRACE_NATIVE_INPUT
if ((uint32_t)(last_hid_complete_ms - last_native_trace_ms) >= 1000) {
last_native_trace_ms = last_hid_complete_ms;
// Observe the completed transfer, not a proposed or ungated report.
log_packet("NATIVE_INPUT_DELIVERED", instance, report[0], report, length);
}
#endif
if (dx || dy) {
++mouse_delivered_reports;
mouse_delivered_x += dx;
@ -529,6 +571,10 @@ bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage,
const tusb_control_request_t* request) {
if (stage == CONTROL_STAGE_SETUP)
log_packet("VENDOR_CONTROL", rhport, 0, (const uint8_t*)request, sizeof(*request));
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
if (probe_bootsel_vendor_control(rhport, stage, request))
return true;
#endif
#ifdef SWITCH2_PROBE_IDENTITY_REPLY
if (request->bmRequestType == 0xc0 && request->bRequest == 0x03 &&
request->wValue == 0 && request->wIndex == 0) {
@ -611,9 +657,18 @@ int main(void) {
#else
probe_debug_printf("\n[PROBE] Joy-Con 2 (R) USB enumeration recorder\n");
#endif
#ifdef SWITCH2_PROBE_OMIT_NATIVE_IMU
probe_debug_printf("[PROBE] ACTIVATION_TEST=no-imu: native08 bytes 15..55 omitted; features, power, status, counters and cadence unchanged\n");
#elif defined(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
probe_debug_printf("[PROBE] ACTIVATION_TEST=zero-imu-payload: native08 bytes 16..55 zeroed; length, features and all other fields unchanged\n");
#endif
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
probe_controller_input_init();
#if SWITCH2_BRIDGE_WII_INPUT
probe_debug_printf("[PROBE] UART0 GP0=TX, 115200 8N1; selected Wii IR/MotionPlus source enabled\n");
#else
probe_debug_printf("[PROBE] UART0 GP0=TX, 115200 8N1; selected right Joy-Con Bluetooth source enabled\n");
#endif
#else
probe_debug_printf("[PROBE] UART0 GP0=TX, 115200 8N1; Bluetooth disabled\n");
#endif
@ -633,7 +688,12 @@ int main(void) {
probe_debug_printf("[PROBE] Own virtual pairing storage offset=%08" PRIx32 "\n",
probe_storage_offset());
#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] Hold BOOTSEL2s for pairing; Wii cue feedback uses bounded ERM patterns, not HD audio waveforms\n");
#else
probe_debug_printf("[PROBE] Live right Joy-Con buttons/stick/native mouse; hold BOOTSEL 2s for Bluetooth pairing (never clears pairings)\n");
#endif
#else
probe_debug_printf("[PROBE] Manual input test: hold BOOTSEL for SL+SR, release for neutral; no controller forwarding\n");
#endif
@ -650,6 +710,9 @@ int main(void) {
tud_task();
drain_log();
const uint32_t now = to_ms_since_boot(get_absolute_time());
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
probe_bootsel_task(now);
#endif
#ifdef SWITCH2_PROBE_USB_INIT
#ifdef SWITCH_PICO_SWITCH2_USB_BRIDGE
if (probe_controller_input_pairing_task())

4
tools/switch2_usb_probe/memory.c
View file

@ -43,7 +43,7 @@ static bool valid_calibration(const uint8_t* data) {
return true;
}
bool probe_memory_right_stick_center(uint8_t output[3]) {
bool probe_memory_right_stick_calibration(uint8_t output[9]) {
if (!output) return false;
// A solo Joy-Con uses the primary calibration record, even for the right
// controller. User magic precedes its 9-byte record; factory has no magic.
@ -52,6 +52,6 @@ bool probe_memory_right_stick_center(uint8_t output[3]) {
if (user[0] == 0xb2 && user[1] == 0xa1 && valid_calibration(user + 2))
selected = user + 2;
if (!valid_calibration(selected)) return false;
memcpy(output, selected, 3);
memcpy(output, selected, 9);
return true;
}

3
tools/switch2_usb_probe/memory.h
View file

@ -4,4 +4,5 @@
#include <stdint.h>
bool probe_memory_read(uint32_t address, uint8_t* output, size_t length);
bool probe_memory_right_stick_center(uint8_t output[3]);
// Packed center, positive travel, negative travel (two12-bit axes each).
bool probe_memory_right_stick_calibration(uint8_t output[9]);

440
tools/switch2_usb_probe/native_imu.cpp Normal file
View file

@ -0,0 +1,440 @@
#include "native_imu.h"
#include <cmath>
#include <limits.h>
namespace {
constexpr uint32_t kFreshUs = 150000;
constexpr uint32_t kMaximumUpdateGapUs = 50000;
constexpr uint32_t kCalibrationUs = 1500000;
constexpr uint32_t kCalibrationSamples = 64;
constexpr uint32_t kVariationSamples = 16;
// The stationary Wii trace has 0.383 dps / 0.010 g vector RMS variation;
// quantized sample-to-sample steps reach 0.806 dps / 0.030 g. Judge variance
// over distinct samples, with roughly twice that measured noise allowance.
constexpr float kGyroRmsDps = 0.75f;
constexpr float kAccelRmsG = 0.025f;
// One degree between averaged gravity directions, independent of g scale.
constexpr float kGravityDirectionCosSquared = 0.9996954135f;
constexpr float kGravityTimeConstantUs = 1000000.0f;
constexpr float kRadiansPerDegree = 0.017453292519943295f;
constexpr float kTwoPi = 6.2831853071795864769f;
// A two-second complementary heading correction, driven by distinct optical
// sample times rather than polling frequency. Reuse the 150ms sensor lifetime.
constexpr float kOpticalTimeConstantUs = 2000000.0f;
// Reject camera-forward directions within about 14.5 degrees of world vertical.
constexpr float kMinimumOpticalHorizontalSquared = 0.25f * 0.25f;
bool finite_vector(const float* values, unsigned count) {
for (unsigned i = 0; i < count; ++i) {
if (!std::isfinite(values[i])) return false;
}
return true;
}
float squared_norm(const float values[3]) {
return values[0] * values[0] + values[1] * values[1] + values[2] * values[2];
}
bool accumulate_stationary(const float sample[3], uint32_t count, float mean[3],
float& variation, float rms_limit) {
if (count == 1) {
for (unsigned i = 0; i < 3; ++i) mean[i] = sample[i];
variation = 0.0f;
return true;
}
// Welford's sum of squared vector deviations avoids subtracting the large
// uncalibrated bias from a sum of squares. No samples or heap state retained.
const float weight = 1.0f / static_cast<float>(count);
float deviation_squared = 0.0f;
for (unsigned i = 0; i < 3; ++i) {
const float delta = sample[i] - mean[i];
deviation_squared += delta * delta;
mean[i] += delta * weight;
variation += delta * (sample[i] - mean[i]);
}
const float variance_limit = rms_limit * rms_limit;
// A four-RMS excursion discards impulses immediately instead of letting a
// long quiet prefix dilute them. Ordinary quantization is judged by RMS.
return std::isfinite(deviation_squared) && std::isfinite(variation) &&
deviation_squared < 16.0f * variance_limit &&
(count < kVariationSamples || variation < static_cast<float>(count - 1) * variance_limit);
}
// Match the reference codec's nearest-even rounding without double arithmetic
// or a dependency on the process floating-point rounding mode. Callers ensure
// the input and its rounded result fit int32_t.
int32_t round_even(float value) {
int32_t integral = static_cast<int32_t>(value);
const float remainder = value - static_cast<float>(integral);
if (remainder > 0.5f || (remainder == 0.5f && (integral & 1))) ++integral;
if (remainder < -0.5f || (remainder == -0.5f && (integral & 1))) --integral;
return integral;
}
uint32_t ratio_code(float ratio) {
// Add the midpoint as an integer: (ratio + 1.0f) would first discard low
// ratio bits. The +1 endpoint is 2^31, outside the unsigned 31-bit field.
const int32_t offset = round_even(ratio * 1073741824.0f);
const uint32_t code = static_cast<uint32_t>(offset) + UINT32_C(0x40000000);
return code > UINT32_C(0x7fffffff) ? UINT32_C(0x7fffffff) : code;
}
int32_t acceleration_code(float acceleration) {
if (acceleration >= 8.0f) return INT32_MAX;
if (acceleration <= -8.0f) return INT32_MIN;
return round_even(acceleration * 268435456.0f);
}
void put_u32(uint8_t* output, uint32_t value) {
output[0] = static_cast<uint8_t>(value);
output[1] = static_cast<uint8_t>(value >> 8);
output[2] = static_cast<uint8_t>(value >> 16);
output[3] = static_cast<uint8_t>(value >> 24);
}
} // namespace
extern "C" bool probe_native_imu_pack(const float quaternion_wxyz[4], const float accel_g[3],
uint16_t counter_ticks, uint16_t elapsed_ticks,
int16_t temperature_raw, uint8_t output[30]) {
if (!quaternion_wxyz || !accel_g || !output || counter_ticks > 0x0fff ||
elapsed_ticks > 0x0fff || !finite_vector(quaternion_wxyz, 4) || !finite_vector(accel_g, 3)) {
return false;
}
unsigned largest = 0;
for (unsigned i = 1; i < 4; ++i) {
if (std::fabs(quaternion_wxyz[i]) > std::fabs(quaternion_wxyz[largest])) largest = i;
}
if (quaternion_wxyz[largest] == 0.0f) return false;
uint32_t ratios[3];
for (unsigned i = 0; i < 3; ++i) {
ratios[i] = ratio_code(quaternion_wxyz[(largest + i + 1) & 3] / quaternion_wxyz[largest]);
}
output[0] = static_cast<uint8_t>(counter_ticks);
output[1] = static_cast<uint8_t>((counter_ticks >> 8) | (elapsed_ticks << 4));
output[2] = static_cast<uint8_t>(elapsed_ticks >> 4);
output[3] = 0x0c;
put_u32(output + 4, largest | (ratios[0] << 3));
put_u32(output + 8, (ratios[0] >> 29) | (ratios[1] << 2));
put_u32(output + 12, (ratios[1] >> 30) | (ratios[2] << 1));
for (unsigned i = 0; i < 3; ++i) {
put_u32(output + 16 + i * 4, static_cast<uint32_t>(acceleration_code(accel_g[i])));
}
const uint16_t temperature = static_cast<uint16_t>(temperature_raw);
output[28] = static_cast<uint8_t>(temperature);
output[29] = static_cast<uint8_t>(temperature >> 8);
return true;
}
void ProbeNativeMotion::reset() {
*this = ProbeNativeMotion{};
}
void ProbeNativeMotion::clear_candidate() {
candidate_ = false;
gyro_count_ = 0;
accel_count_ = 0;
}
void ProbeNativeMotion::invalidate() {
ready_ = false;
have_accel_ = false;
have_gyro_ = false;
have_optical_reference_ = false;
have_optical_sample_ = false;
clear_candidate();
quaternion_[0] = 1.0f;
for (unsigned i = 0; i < 3; ++i) {
quaternion_[i + 1] = 0.0f;
acceleration_[i] = 0.0f;
gyro_dps_[i] = 0.0f;
bias_[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.
}
bool ProbeNativeMotion::initialize_orientation() {
const float norm = std::sqrt(squared_norm(mean_accel_));
if (!std::isfinite(norm) || norm <= 0.0f) return false;
const float x = mean_accel_[0] / norm;
const float y = mean_accel_[1] / norm;
const float z = mean_accel_[2] / norm;
const float horizontal = std::sqrt(x * x + y * y);
quaternion_[3] = 0.0f;
if (horizontal > 0.0f) {
// atan2 remains well conditioned near -Z, unlike normalizing [1+z,y,-x,0].
const float half_angle = 0.5f * std::atan2(horizontal, z);
const float sine = std::sin(half_angle);
quaternion_[0] = std::cos(half_angle);
quaternion_[1] = (y / horizontal) * sine;
quaternion_[2] = (-x / horizontal) * sine;
} else {
quaternion_[0] = z >= 0.0f ? 1.0f : 0.0f;
quaternion_[1] = z >= 0.0f ? 0.0f : 1.0f;
quaternion_[2] = 0.0f;
}
return true;
}
bool ProbeNativeMotion::integrate(const float gyro_dps[3], uint32_t elapsed_us) {
if (elapsed_us == 0) return true;
const float x = gyro_dps[0] - bias_[0];
const float y = gyro_dps[1] - bias_[1];
const float z = gyro_dps[2] - bias_[2];
const float norm_squared = x * x + y * y + z * z;
if (!std::isfinite(norm_squared)) return false;
if (norm_squared == 0.0f) return true;
const float norm = std::sqrt(norm_squared);
const float half_angle = norm * (static_cast<float>(elapsed_us) * (0.5e-6f * kRadiansPerDegree));
const float sine_scale = std::sin(half_angle) / norm;
const float dw = std::cos(half_angle);
const float dx = x * sine_scale;
const float dy = y * sine_scale;
const float dz = z * sine_scale;
const float w = quaternion_[0];
const float qx = quaternion_[1];
const float qy = quaternion_[2];
const float qz = quaternion_[3];
// Body-local rates multiply on the right of the body-to-reference rotation.
quaternion_[0] = w * dw - qx * dx - qy * dy - qz * dz;
quaternion_[1] = w * dx + qx * dw + qy * dz - qz * dy;
quaternion_[2] = w * dy - qx * dz + qy * dw + qz * dx;
quaternion_[3] = w * dz + qx * dy - qy * dx + qz * dw;
return normalize_orientation();
}
bool ProbeNativeMotion::normalize_orientation() {
const float length_squared = quaternion_[0] * quaternion_[0] + quaternion_[1] * quaternion_[1] +
quaternion_[2] * quaternion_[2] + quaternion_[3] * quaternion_[3];
if (!std::isfinite(length_squared) || length_squared <= 0.0f) return false;
const float reciprocal_length = 1.0f / std::sqrt(length_squared);
for (float& component : quaternion_) component *= reciprocal_length;
return true;
}
bool ProbeNativeMotion::correct_gravity(uint32_t elapsed_us) {
if (elapsed_us == 0) return true;
const float norm_squared = squared_norm(acceleration_);
// Do not treat obvious dynamic acceleration as a gravity observation.
// The real acceleration remains unchanged in the outgoing report.
if (norm_squared < 0.9f * 0.9f || norm_squared > 1.1f * 1.1f) return true;
const float reciprocal_norm = 1.0f / std::sqrt(norm_squared);
const float ax = acceleration_[0] * reciprocal_norm;
const float ay = acceleration_[1] * reciprocal_norm;
const float az = acceleration_[2] * reciprocal_norm;
const float w = quaternion_[0], x = quaternion_[1], y = quaternion_[2], z = quaternion_[3];
const float tx = 2.0f * (y * az - z * ay);
const float ty = 2.0f * (z * ax - x * az);
const float tz = 2.0f * (x * ay - y * ax);
const float gx = ax + w * tx + y * tz - z * ty;
const float gy = ay + w * ty + z * tx - x * tz;
const float gz = az + w * tz + x * ty - y * tx;
const float horizontal = std::sqrt(gx * gx + gy * gy);
if (horizontal == 0.0f && gz >= 0.0f) return true;
// Complementary tilt correction in reference space. Its rotation axis has
// no reference-Z component: do not invent a yaw observation from gravity.
// Sensor elapsed time, not Core 0 polling, controls the filter strength.
const float alpha = static_cast<float>(elapsed_us) /
(kGravityTimeConstantUs + static_cast<float>(elapsed_us));
const float half_angle = 0.5f * alpha * std::atan2(horizontal, gz);
const float sine = std::sin(half_angle);
const float cw = std::cos(half_angle);
const float cx = horizontal > 0.0f ? (gy / horizontal) * sine : sine;
const float cy = horizontal > 0.0f ? (-gx / horizontal) * sine : 0.0f;
quaternion_[0] = cw * w - cx * x - cy * y;
quaternion_[1] = cw * x + cx * w + cy * z;
quaternion_[2] = cw * y - cx * z + cy * w;
quaternion_[3] = cw * z + cx * y - cy * x;
return normalize_orientation();
}
void ProbeNativeMotion::observe_optical_heading(uint32_t now_us, uint32_t reference_generation,
uint32_t sequence, uint32_t sample_us,
float yaw_radians, bool valid) {
if (!ready_) return;
if (!have_optical_generation_ || reference_generation != optical_generation_) {
have_optical_generation_ = true;
optical_generation_ = reference_generation;
seen_optical_sequence_ = false;
have_optical_reference_ = false;
have_optical_sample_ = false;
}
if (!valid || (have_optical_sample_ && now_us - optical_sample_us_ >= kFreshUs)) {
// Retain the relative anchor while hidden, but never accumulate filter
// weight for the interval without observations.
have_optical_sample_ = false;
}
const uint32_t sequence_delta = sequence - optical_sequence_;
if (seen_optical_sequence_ && (sequence_delta == 0 || sequence_delta >= 0x80000000u)) return;
seen_optical_sequence_ = true;
optical_sequence_ = sequence;
if (!valid || !std::isfinite(yaw_radians) || now_us - sample_us >= kFreshUs) {
have_optical_sample_ = false;
return;
}
// Reject backwards timestamps while both observations could still be
// fresh. Unsigned differences admit normal microsecond-clock rollover.
if (have_optical_reference_ && now_us - optical_sample_us_ < kFreshUs &&
sample_us - optical_sample_us_ >= 0x80000000u) return;
const float w = quaternion_[0], x = quaternion_[1], y = quaternion_[2], z = quaternion_[3];
// Native body Y is camera forward. Its world XY projection supplies heading;
// near vertical, yaw is ill-conditioned and must not become an observation.
const float forward_x = 2.0f * (x * y - w * z);
const float forward_y = 1.0f - 2.0f * (x * x + z * z);
if (forward_x * forward_x + forward_y * forward_y < kMinimumOpticalHorizontalSquared) {
have_optical_sample_ = false;
return;
}
const uint32_t elapsed_us = have_optical_sample_ ? sample_us - optical_sample_us_ : 0;
optical_sample_us_ = sample_us;
have_optical_sample_ = true;
const float heading = std::atan2(forward_y, forward_x);
const float bearing = std::remainder(yaw_radians, kTwoPi);
if (!have_optical_reference_) {
optical_reference_radians_ = std::remainder(heading + bearing, kTwoPi);
have_optical_reference_ = true;
return;
}
if (elapsed_us == 0 || elapsed_us >= kFreshUs) return;
// Optical direction complements gyro heading; concept reference:
// https://github.com/dolphin-emu/dolphin/blob/master/Source/Core/InputCommon/ControllerInterface/Wiimote/WiimoteController.cpp
// Independently use a relative yaw anchor and elapsed-time gain, not its
// per-frame full-vector correction. Translation also changes bar bearing;
// bar placement/pitch can bias it, so this is not absolute world heading.
// Positive optical aim-right is a negative world-Z heading change.
const float error = std::remainder(optical_reference_radians_ - bearing - heading, kTwoPi);
const float alpha = static_cast<float>(elapsed_us) /
(kOpticalTimeConstantUs + static_cast<float>(elapsed_us));
const float half_angle = 0.5f * alpha * error;
const float cw = std::cos(half_angle), cz = std::sin(half_angle);
// Left multiplication about reference-world Z preserves gravity-aligned
// tilt, and neither the measured acceleration nor learned gyro bias changes.
quaternion_[0] = cw * w - cz * z;
quaternion_[1] = cw * x - cz * y;
quaternion_[2] = cw * y + cz * x;
quaternion_[3] = cw * z + cz * w;
if (!normalize_orientation()) invalidate();
}
void ProbeNativeMotion::update(uint32_t now_us, uint32_t connection_generation,
const ProbeNativeMotionSample& sample) {
if (!have_generation_ || connection_generation != connection_generation_) {
reset();
have_generation_ = true;
connection_generation_ = connection_generation;
}
const uint32_t elapsed_us = have_update_ ? now_us - update_us_ : 0;
update_us_ = now_us;
have_update_ = true;
if (elapsed_us > kMaximumUpdateGapUs) invalidate();
if (!sample.accel_valid || !sample.gyro_valid ||
!finite_vector(sample.accel_g, 3) || !finite_vector(sample.gyro_dps, 3)) {
invalidate();
return;
}
const bool new_accel = !seen_accel_sequence_ || sample.accel_sequence != accel_sequence_;
const bool new_gyro = !seen_gyro_sequence_ || sample.gyro_sequence != gyro_sequence_;
// A fresh replacement cannot conceal a stale interval between observations.
// Unsigned differences also reject timestamps that move backwards.
if ((have_accel_ && new_accel && sample.accel_us - accel_us_ > kFreshUs) ||
(have_gyro_ && new_gyro && sample.gyro_us - gyro_us_ > kFreshUs)) {
invalidate();
}
const bool was_ready = ready_;
const uint32_t accel_elapsed_us = was_ready && new_accel ? sample.accel_us - accel_us_ : 0;
float previous_gyro[3];
if (was_ready && new_gyro) {
for (unsigned i = 0; i < 3; ++i) previous_gyro[i] = gyro_dps_[i];
}
if (new_accel) {
seen_accel_sequence_ = true;
accel_sequence_ = sample.accel_sequence;
accel_us_ = sample.accel_us;
have_accel_ = true;
for (unsigned i = 0; i < 3; ++i) acceleration_[i] = sample.accel_g[i];
}
if (new_gyro) {
seen_gyro_sequence_ = true;
gyro_sequence_ = sample.gyro_sequence;
gyro_us_ = sample.gyro_us;
have_gyro_ = true;
for (unsigned i = 0; i < 3; ++i) gyro_dps_[i] = sample.gyro_dps[i];
}
if ((have_accel_ && now_us - accel_us_ >= kFreshUs) || (have_gyro_ && now_us - gyro_us_ >= kFreshUs)) {
invalidate();
return;
}
// Wii acceleration and gyro arrive on independent packets. Keep a fresh
// first half while waiting for the other sensor after invalidation.
if (!have_accel_ || !have_gyro_) return;
if (was_ready) {
// Zero-order hold uses actual receipt times, independent of update/USB
// cadence. Split only when a new rate arrived inside this update span.
const uint32_t rate_age_us = now_us - gyro_us_;
if (new_gyro && rate_age_us < elapsed_us) {
if (!integrate(previous_gyro, elapsed_us - rate_age_us) || !integrate(gyro_dps_, rate_age_us)) {
invalidate();
}
} else if (!integrate(gyro_dps_, elapsed_us)) {
invalidate();
}
if (ready_ && new_accel && !correct_gravity(accel_elapsed_us)) invalidate();
return;
}
const float acceleration_norm_squared = squared_norm(acceleration_);
if (acceleration_norm_squared < 0.85f * 0.85f ||
acceleration_norm_squared > 1.15f * 1.15f) {
clear_candidate();
return;
}
if (!candidate_) {
if (!new_gyro) return;
candidate_ = true;
candidate_us_ = gyro_us_;
gyro_count_ = 0;
accel_count_ = 0;
}
if (new_accel || accel_count_ == 0) {
if (!accumulate_stationary(acceleration_, ++accel_count_, mean_accel_,
accel_variation_, kAccelRmsG)) {
clear_candidate();
return;
}
if (accel_count_ == kVariationSamples) {
for (unsigned i = 0; i < 3; ++i) gravity_reference_[i] = mean_accel_[i];
} else if (accel_count_ > kVariationSamples) {
// Anchor the averaged direction rather than the previous packet:
// slow tilt must not be accepted as a series of small noisy steps.
float dot = 0.0f;
for (unsigned i = 0; i < 3; ++i) dot += gravity_reference_[i] * mean_accel_[i];
if (dot <= 0.0f || dot * dot < squared_norm(gravity_reference_) *
squared_norm(mean_accel_) * kGravityDirectionCosSquared) {
clear_candidate();
return;
}
}
}
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.
if (!accumulate_stationary(gyro_dps_, ++gyro_count_, mean_gyro_,
gyro_variation_, kGyroRmsDps)) {
clear_candidate();
return;
}
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;
clear_candidate();
}
}

99
tools/switch2_usb_probe/native_imu.h Normal file
View file

@ -0,0 +1,99 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
// Encode one native mode-0 IMU block. Inputs use the native-right body frame;
// quaternion wxyz rotates body vectors into the reference frame. Quaternion
// scale/sign do not matter. Finite acceleration saturates at signed Q28 limits.
// Invalid inputs leave output untouched; counter and elapsed must fit 12 bits.
bool probe_native_imu_pack(const float quaternion_wxyz[4], const float accel_g[3],
uint16_t counter_ticks, uint16_t elapsed_ticks,
int16_t temperature_raw, uint8_t output[30]);
#ifdef __cplusplus
}
struct ProbeNativeMotionSample {
bool accel_valid = false;
bool gyro_valid = false;
uint32_t accel_sequence = 0;
uint32_t gyro_sequence = 0;
uint32_t accel_us = 0;
uint32_t gyro_us = 0;
float accel_g[3]{};
float gyro_dps[3]{};
};
// 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.
// Startup requires the user to rest the controller: constant rotation about
// gravity is indistinguishable from an unknown gyro bias without another sensor.
// Fresh near-1g acceleration corrects tilt drift after startup. Heading remains
// gyro-derived unless a reliable optical heading observation is supplied.
class ProbeNativeMotion {
public:
void reset();
void update(uint32_t now_us, uint32_t connection_generation,
const ProbeNativeMotionSample& sample);
// Call after update, using a full observed sensor-bar pair (never inferred).
// Positive optical yaw means aim-right, the negative reference-world turn.
// The first sample in each optical generation anchors the current heading;
// it supplies neither absolute world yaw nor an absolute console cursor.
void observe_optical_heading(uint32_t now_us, uint32_t reference_generation,
uint32_t sequence, uint32_t sample_us,
float yaw_radians, bool valid);
bool ready() const { return ready_; }
const float* quaternion() const { return quaternion_; }
const float* acceleration() const { return acceleration_; }
const float* bias() const { return bias_; }
private:
void invalidate();
void clear_candidate();
bool initialize_orientation();
bool integrate(const float gyro_dps[3], uint32_t elapsed_us);
bool correct_gravity(uint32_t elapsed_us);
bool normalize_orientation();
bool have_generation_ = false;
bool have_update_ = false;
bool seen_accel_sequence_ = false;
bool seen_gyro_sequence_ = false;
bool have_accel_ = false;
bool have_gyro_ = false;
bool candidate_ = false;
bool ready_ = false;
bool have_optical_generation_ = false;
bool seen_optical_sequence_ = false;
bool have_optical_reference_ = false;
bool have_optical_sample_ = false;
uint32_t connection_generation_ = 0;
uint32_t update_us_ = 0;
uint32_t accel_sequence_ = 0;
uint32_t gyro_sequence_ = 0;
uint32_t accel_us_ = 0;
uint32_t gyro_us_ = 0;
uint32_t candidate_us_ = 0;
uint32_t gyro_count_ = 0;
uint32_t accel_count_ = 0;
uint32_t optical_generation_ = 0;
uint32_t optical_sequence_ = 0;
uint32_t optical_sample_us_ = 0;
float optical_reference_radians_ = 0.0f;
float gyro_variation_ = 0.0f;
float accel_variation_ = 0.0f;
float quaternion_[4]{1.0f, 0.0f, 0.0f, 0.0f};
float acceleration_[3]{};
float gyro_dps_[3]{};
float bias_[3]{};
float mean_gyro_[3]{};
float mean_accel_[3]{};
float gravity_reference_[3]{};
};
#endif

86
tools/switch2_usb_probe/probe_build.cmake
View file

@ -18,6 +18,68 @@ function(switch2_usb_probe_configure target)
${PICO_SDK_PATH}/lib/btstack/platform/embedded)
target_compile_definitions(${target} PRIVATE
CFG_TUSB_CONFIG_FILE="${SWITCH2_USB_PROBE_DIR}/tusb_config.h")
option(SWITCH2_PROBE_TRACE_NATIVE_INPUT
"Trace one completed native USB input report per second without changing its contents" OFF)
if(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
if(NOT SWITCH_PICO_SWITCH2_USB_BRIDGE)
message(FATAL_ERROR "Native input tracing requires the Bluetooth USB bridge")
endif()
target_compile_definitions(${target} PRIVATE SWITCH2_PROBE_TRACE_NATIVE_INPUT=1)
endif()
if(SWITCH2_BRIDGE_WII_INPUT)
set(SWITCH2_WII_IR_SENSITIVITY 3 CACHE STRING "Standard Wii camera sensitivity preset: 2 or 3")
set_property(CACHE SWITCH2_WII_IR_SENSITIVITY PROPERTY STRINGS 2 3)
if(NOT "${SWITCH2_WII_IR_SENSITIVITY}" MATCHES "^[23]$")
message(FATAL_ERROR "SWITCH2_WII_IR_SENSITIVITY must be standard preset 2 or 3")
endif()
target_compile_definitions(bluepad32 PRIVATE
SWITCH_PICO_WII_IR_SENSITIVITY_LEVEL=${SWITCH2_WII_IR_SENSITIVITY})
set(SWITCH2_WII_IR_VIEW_WIDTH 660 CACHE STRING "Native Wii IR viewport width in camera pixels")
set(SWITCH2_WII_IR_VIEW_HEIGHT 370 CACHE STRING "Native Wii IR viewport height in camera pixels")
set(SWITCH2_WII_IR_OFFSET_X 0 CACHE STRING "Native Wii IR viewport horizontal camera offset")
set(SWITCH2_WII_IR_OFFSET_Y -115 CACHE STRING "Native Wii IR viewport vertical offset: -115 below, +115 above")
set(SWITCH2_WII_IR_SPAN_X 1920 CACHE STRING "Native mouse counts across the IR viewport width")
set(SWITCH2_WII_IR_SPAN_Y 1080 CACHE STRING "Native mouse counts across the IR viewport height")
foreach(field VIEW_WIDTH VIEW_HEIGHT OFFSET_X OFFSET_Y SPAN_X SPAN_Y)
if(NOT "${SWITCH2_WII_IR_${field}}" MATCHES "^-?[0-9]+$")
message(FATAL_ERROR "SWITCH2_WII_IR_${field} must be an integer")
endif()
endforeach()
math(EXPR offset_x_twice "2 * ${SWITCH2_WII_IR_OFFSET_X}")
math(EXPR offset_y_twice "2 * ${SWITCH2_WII_IR_OFFSET_Y}")
math(EXPR room_x "1024 - ${SWITCH2_WII_IR_VIEW_WIDTH}")
math(EXPR room_y "768 - ${SWITCH2_WII_IR_VIEW_HEIGHT}")
if(SWITCH2_WII_IR_VIEW_WIDTH LESS 1 OR SWITCH2_WII_IR_VIEW_WIDTH GREATER 1024 OR
SWITCH2_WII_IR_VIEW_HEIGHT LESS 1 OR SWITCH2_WII_IR_VIEW_HEIGHT GREATER 768 OR
offset_x_twice LESS -${room_x} OR offset_x_twice GREATER ${room_x} OR
offset_y_twice LESS -${room_y} OR offset_y_twice GREATER ${room_y} OR
SWITCH2_WII_IR_SPAN_X LESS 1 OR SWITCH2_WII_IR_SPAN_X GREATER 32767 OR
SWITCH2_WII_IR_SPAN_Y LESS 1 OR SWITCH2_WII_IR_SPAN_Y GREATER 32767)
message(FATAL_ERROR "Native IR viewport must lie inside the camera; mouse spans must be 1..32767")
endif()
target_compile_definitions(${target} PRIVATE
SWITCH2_WII_IR_SCREEN_CONFIG=${SWITCH2_WII_IR_VIEW_WIDTH},${SWITCH2_WII_IR_VIEW_HEIGHT},${SWITCH2_WII_IR_OFFSET_X},${SWITCH2_WII_IR_OFFSET_Y},${SWITCH2_WII_IR_SPAN_X},${SWITCH2_WII_IR_SPAN_Y})
endif()
option(SWITCH2_PROBE_OMIT_NATIVE_IMU
"Activation experiment: omit only native IMU bytes while retaining advertised features" OFF)
option(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD
"Activation experiment: zero native IMU payload while preserving its length" OFF)
if(SWITCH2_PROBE_OMIT_NATIVE_IMU AND SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
message(FATAL_ERROR "Select only one IMU activation experiment")
endif()
if(SWITCH2_PROBE_OMIT_NATIVE_IMU OR SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
if(NOT SWITCH_PICO_SWITCH2_USB_BRIDGE)
message(FATAL_ERROR "Native IMU activation experiments require the Bluetooth USB bridge")
endif()
if(SWITCH2_BRIDGE_WII_INPUT)
message(FATAL_ERROR "Disable IMU ablation experiments for the Wii source bridge")
endif()
endif()
if(SWITCH2_PROBE_OMIT_NATIVE_IMU)
target_compile_definitions(${target} PRIVATE SWITCH2_PROBE_OMIT_NATIVE_IMU=1)
elseif(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
target_compile_definitions(${target} PRIVATE SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD=1)
endif()
set(SWITCH2_PROBE_IDENTITY_FILE "" CACHE FILEPATH "64-byte Joy-Con 2 (R) factory-format identity block")
if(SWITCH2_PROBE_IDENTITY_FILE)
@ -130,10 +192,30 @@ function(switch2_usb_probe_configure target)
hardware_flash pico_flash pico_mbedtls_crypto pico_mbedtls_headers tinyusb_device)
pico_enable_stdio_usb(${target} 0)
pico_enable_stdio_uart(${target} 1)
if(SWITCH_PICO_SWITCH2_USB_BRIDGE)
if(SWITCH2_BRIDGE_WII_INPUT)
pico_set_program_name(${target} "Switch 2 Wii IR and native motion bridge")
elseif(SWITCH_PICO_SWITCH2_USB_BRIDGE)
pico_set_program_name(${target} "Switch 2 right Joy-Con Bluetooth bridge")
else()
pico_set_program_name(${target} "Switch 2 USB initialization capture")
endif()
pico_set_program_version(${target} "0.24")
if(SWITCH2_PROBE_OMIT_NATIVE_IMU)
pico_set_program_version(${target} "0.24-no-imu")
elseif(SWITCH2_PROBE_ZERO_NATIVE_IMU_PAYLOAD)
pico_set_program_version(${target} "0.24-zero-imu-payload")
elseif(SWITCH2_BRIDGE_WII_INPUT)
if(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
pico_set_program_version(${target} "0.33-wii-trace")
else()
pico_set_program_version(${target} "0.33-wii")
endif()
elseif(SWITCH_PICO_SWITCH2_USB_BRIDGE)
if(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
pico_set_program_version(${target} "0.25-trace")
else()
pico_set_program_version(${target} "0.25")
endif()
else()
pico_set_program_version(${target} "0.24")
endif()
endfunction()