WIP: checkpoint Wii native IMU and upstream IR bridge
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:
parent
3040c9d294
commit
9f6dddb790
41 changed files with 7990 additions and 150 deletions
|
|
@ -40,6 +40,10 @@ struct WiiAccelFixture {
|
|||
};
|
||||
WiiAccelFixture remote_accel_fixture;
|
||||
WiiAccelFixture nunchuk_accel_fixture;
|
||||
#ifdef SWITCH2_BRIDGE_WII_INPUT
|
||||
WiiAccelFixture gyro_fixture;
|
||||
bool wii_rumble_ready = true;
|
||||
#endif
|
||||
|
||||
bool wii_accel_fixture_snapshot(const WiiAccelFixture& fixture,
|
||||
uni_hid_device_t* controller,
|
||||
|
|
@ -749,6 +753,17 @@ extern "C" bool uni_hid_parser_wii_nunchuk_accel_snapshot(
|
|||
return wii_accel_fixture_snapshot(nunchuk_accel_fixture, controller, acceleration, sequence);
|
||||
}
|
||||
|
||||
#ifdef SWITCH2_BRIDGE_WII_INPUT
|
||||
extern "C" bool uni_hid_parser_wii_gyro_snapshot(
|
||||
uni_hid_device_t* controller, int32_t gyro[3], uint32_t* sequence) {
|
||||
return wii_accel_fixture_snapshot(gyro_fixture, controller, gyro, sequence);
|
||||
}
|
||||
extern "C" bool uni_hid_parser_wii_rumble_ready(uni_hid_device_t*) {
|
||||
require(state_lock_depth == 0, "Wii dispatch readiness must be checked on Core 1 outside the state lock");
|
||||
return wii_rumble_ready;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SWITCH_PICO_HAPTICS_EXPERIMENT
|
||||
namespace {
|
||||
std::vector<btstack_timer_source_t*> native_timers;
|
||||
|
|
@ -5605,6 +5620,186 @@ void dispatch_wii_requests() {
|
|||
wii_dispatch_active = false;
|
||||
}
|
||||
|
||||
#ifdef SWITCH2_BRIDGE_WII_INPUT
|
||||
void test_wii_bridge_sensors() {
|
||||
auto selected = wii_device(1);
|
||||
auto other = wii_device(0);
|
||||
bluepad32_input_backend_init();
|
||||
bluepad32_input_backend_select_wii_source(selected.conn.btaddr);
|
||||
start_pairing_backend();
|
||||
require(platform_on_device_ready(&other) == UNI_ERROR_SUCCESS, "unselected Wii must remain available normally");
|
||||
uni_controller_t input{};
|
||||
input.klass = UNI_CONTROLLER_CLASS_GAMEPAD;
|
||||
input.gamepad.buttons = BUTTON_A;
|
||||
remote_accel_fixture = {&other, {8193, -42, 91}, 7, true};
|
||||
gyro_fixture = {&other, {123456, -45678, 91011}, 11, true};
|
||||
platform_on_controller_data(&other, &input);
|
||||
Bluepad32WiiBridgeSnapshot snapshot{};
|
||||
bluepad32_input_backend_wii_snapshot(&snapshot);
|
||||
require(!snapshot.controller.active && snapshot.slot == 0xff,
|
||||
"an unrelated Wii cannot claim the selected native source");
|
||||
selected.controller_type = CONTROLLER_TYPE_UnknownController;
|
||||
require(platform_on_device_ready(&selected) == UNI_ERROR_SUCCESS, "non-Wii address match must connect normally");
|
||||
platform_on_controller_data(&selected, &input);
|
||||
bluepad32_input_backend_wii_snapshot(&snapshot);
|
||||
require(!snapshot.controller.active, "matching address alone must not identify a Wii");
|
||||
platform_on_device_disconnected(&selected);
|
||||
auto replacement = wii_device(1);
|
||||
require(platform_on_device_ready(&replacement) == UNI_ERROR_SUCCESS, "selected physical Wii must become ready");
|
||||
const uint32_t generation = slot_snapshot(1).connection_generation;
|
||||
if (kDefaultMotionEnabled) {
|
||||
require(bluepad32_input_backend_toggle_motion(1, generation), "legacy motion toggle must be independent");
|
||||
}
|
||||
remote_accel_fixture = {&replacement, {8193, -42, 91}, 7, true};
|
||||
gyro_fixture = {&replacement, {123456, -45678, 91011}, 11, true};
|
||||
now_ms = 100;
|
||||
platform_on_controller_data(&replacement, &input);
|
||||
bluepad32_input_backend_wii_snapshot(&snapshot);
|
||||
require(snapshot.controller.active && snapshot.slot == 1 &&
|
||||
snapshot.controller.state.button_south &&
|
||||
snapshot.controller.state.motion_sample_count == 0 &&
|
||||
snapshot.accel_valid && snapshot.gyro_valid &&
|
||||
snapshot.accel_q13[0] == 8193 && snapshot.gyro_q10[0] == 123456,
|
||||
"native raw precision and ordinary buttons must survive legacy motion disable");
|
||||
require(snapshot.accel_received_us == 100000 && snapshot.gyro_received_us == 100000,
|
||||
"both first genuine sensor samples need ingress timestamps");
|
||||
now_ms = 250;
|
||||
++remote_accel_fixture.sequence;
|
||||
platform_on_controller_data(&replacement, &input);
|
||||
now_ms = 300; // Status/ACK callback has no new sensor sequence.
|
||||
platform_on_controller_data(&replacement, &input);
|
||||
bluepad32_input_backend_report_sent(1);
|
||||
bluepad32_input_backend_wii_snapshot(&snapshot);
|
||||
require(snapshot.received_us == 300000 && snapshot.accel_received_us == 250000 &&
|
||||
snapshot.gyro_received_us == 100000 && snapshot.gyro_sequence == 11,
|
||||
"accel interleaves, status and USB consumption cannot rejuvenate gyro");
|
||||
require(bluepad32_input_backend_set_wii_orientation(
|
||||
snapshot.controller.identity, generation, true), "orientation change must queue");
|
||||
dispatch_wii_requests();
|
||||
platform_on_controller_data(&replacement, &input);
|
||||
bluepad32_input_backend_wii_snapshot(&snapshot);
|
||||
require(snapshot.controller.connection_generation != generation &&
|
||||
!snapshot.accel_valid && !snapshot.gyro_valid &&
|
||||
!snapshot.controller.accelerometer.valid,
|
||||
"a logical epoch must not republish cached parser samples as fresh");
|
||||
++remote_accel_fixture.sequence;
|
||||
++gyro_fixture.sequence;
|
||||
now_ms = 320;
|
||||
platform_on_controller_data(&replacement, &input);
|
||||
bluepad32_input_backend_wii_snapshot(&snapshot);
|
||||
require(snapshot.accel_valid && snapshot.gyro_valid && snapshot.gyro_received_us == 320000,
|
||||
"distinct samples must recover the new logical epoch");
|
||||
platform_on_device_disconnected(&replacement);
|
||||
bluepad32_input_backend_wii_snapshot(&snapshot);
|
||||
require(!snapshot.controller.active && !snapshot.accel_valid && !snapshot.gyro_valid &&
|
||||
snapshot.slot == 0xff, "disconnect must retire source identity and samples together");
|
||||
}
|
||||
|
||||
void (*during_wii_rumble)() = nullptr;
|
||||
void observe_wii_rumble(uni_hid_device_t* target, uint16_t delay,
|
||||
uint16_t duration, uint8_t weak, uint8_t strong) {
|
||||
require(state_lock_depth == 0, "Wii transport must run without the backend lock");
|
||||
play_rumble(target, delay, duration, weak, strong);
|
||||
if (during_wii_rumble != nullptr) during_wii_rumble();
|
||||
}
|
||||
|
||||
void test_wii_bridge_cues() {
|
||||
auto selected = wii_device(1);
|
||||
selected.report_parser.play_dual_rumble = observe_wii_rumble;
|
||||
auto other = wii_device(0);
|
||||
bluepad32_input_backend_init();
|
||||
bluepad32_input_backend_select_wii_source(selected.conn.btaddr);
|
||||
start_pairing_backend();
|
||||
require(platform_on_device_ready(&other) == UNI_ERROR_SUCCESS &&
|
||||
platform_on_device_ready(&selected) == UNI_ERROR_SUCCESS, "cue fixtures must connect");
|
||||
uint64_t token = 0;
|
||||
require(bluepad32_input_backend_wii_sample_request(3, &token) &&
|
||||
bluepad32_input_backend_wii_sample_result(token) == 0 &&
|
||||
selected.rumble_calls == 0, "Core 0 queue acceptance is not cue completion");
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(bluepad32_input_backend_wii_sample_result(token) == 1 &&
|
||||
bluepad32_input_backend_wii_sample_result(token) == -1 &&
|
||||
selected.rumble_calls == 1 && selected.last_rumble_duration_ms == 25,
|
||||
"double-click completion must be consumable once after its first driver dispatch");
|
||||
now_ms = 60;
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(selected.rumble_calls == 1, "double-click gap must not vibrate");
|
||||
now_ms = 115;
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(selected.rumble_calls == 2 && selected.last_rumble_duration_ms == 25 &&
|
||||
other.rumble_calls == 0, "double-click must dispatch exactly its second selected-Wii pulse");
|
||||
now_ms = 2000;
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
uint64_t newer;
|
||||
require(bluepad32_input_backend_wii_sample_request(1, &newer) && newer != token,
|
||||
"finite patterns must release the cue ledger without reusing tokens");
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(selected.last_rumble_duration_ms == 1000, "buzz must have a finite one-second driver timer");
|
||||
require(bluepad32_input_backend_wii_sample_request(0, &token), "stop must replace a running pattern");
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(selected.last_rumble_duration_ms == 0 &&
|
||||
bluepad32_input_backend_wii_sample_result(token) == 1 &&
|
||||
bluepad32_input_backend_wii_sample_result(newer) == -1,
|
||||
"stop must retire the old ledger and complete only after dispatch");
|
||||
const int stopped_calls = selected.rumble_calls;
|
||||
now_ms += 3000;
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(selected.rumble_calls == stopped_calls, "stopped patterns must never replay");
|
||||
}
|
||||
|
||||
void test_wii_bridge_cue_races() {
|
||||
auto selected = wii_device(0);
|
||||
selected.report_parser.play_dual_rumble = observe_wii_rumble;
|
||||
bluepad32_input_backend_init();
|
||||
bluepad32_input_backend_select_wii_source(selected.conn.btaddr);
|
||||
start_pairing_backend();
|
||||
require(platform_on_device_ready(&selected) == UNI_ERROR_SUCCESS, "selected Wii must connect");
|
||||
const auto connection = slot_snapshot(0);
|
||||
bluepad32_input_backend_queue_profile_feedback(
|
||||
0, connection.connection_generation, 1, ControllerProfileConfirmationPolicy::kRumble);
|
||||
uint64_t token;
|
||||
require(bluepad32_input_backend_wii_sample_request(3, &token), "cue must wait behind profile feedback");
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(selected.last_rumble_duration_ms == 75 &&
|
||||
bluepad32_input_backend_wii_sample_result(token) == 0,
|
||||
"profile confirmation has priority and cannot count as cue dispatch");
|
||||
now_ms = 150;
|
||||
during_wii_rumble = [] { bluepad32_input_backend_wii_sample_cancel(); };
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
during_wii_rumble = nullptr;
|
||||
require(bluepad32_input_backend_wii_sample_result(token) == -1,
|
||||
"cancellation during driver dispatch must defeat its late completion");
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(selected.last_rumble_duration_ms == 0, "canceled in-flight cue must receive a finite stop");
|
||||
wii_rumble_ready = false;
|
||||
require(bluepad32_input_backend_wii_sample_request(1, &token), "topology setup may defer cue dispatch");
|
||||
const int calls = selected.rumble_calls;
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(bluepad32_input_backend_wii_sample_result(token) == 0 && selected.rumble_calls == calls,
|
||||
"a Wii void-hook early return must not masquerade as dispatch");
|
||||
now_ms += 2000;
|
||||
require(bluepad32_input_backend_wii_sample_result(token) == -1, "undispatched cue deadline must expire");
|
||||
wii_rumble_ready = true;
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(selected.rumble_calls == calls, "expired cue must not dispatch when setup finally completes");
|
||||
uint64_t replacement_token;
|
||||
require(bluepad32_input_backend_wii_sample_request(2, &replacement_token) && replacement_token != token,
|
||||
"expiration must permit a uniquely identified new request");
|
||||
during_wii_rumble = [] { platform_on_device_disconnected(g_slots[0].device); };
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
during_wii_rumble = nullptr;
|
||||
auto replacement = wii_device(0);
|
||||
require(platform_on_device_ready(&replacement) == UNI_ERROR_SUCCESS, "replacement must connect");
|
||||
process_rumble_timer(&g_rumble_timer);
|
||||
require(bluepad32_input_backend_wii_sample_result(replacement_token) == -1 &&
|
||||
replacement.rumble_calls == 0,
|
||||
"late driver completion must never cross a physical generation");
|
||||
replacement.report_parser.play_dual_rumble = nullptr;
|
||||
require(!bluepad32_input_backend_wii_sample_request(1, &token),
|
||||
"a missing Wii rumble driver cannot accept a cue");
|
||||
}
|
||||
#endif
|
||||
|
||||
void test_wii_orientation() {
|
||||
start_pairing_backend();
|
||||
initialize_runtime_profile_storage();
|
||||
|
|
@ -6021,6 +6216,20 @@ void test_transport_background_scan() {
|
|||
int main(int argc, char** argv) {
|
||||
require(argc == 2, "scenario argument required");
|
||||
const std::string scenario = argv[1];
|
||||
#ifdef SWITCH2_BRIDGE_WII_INPUT
|
||||
if (scenario == "wii-bridge-sensors") {
|
||||
test_wii_bridge_sensors();
|
||||
return 0;
|
||||
}
|
||||
if (scenario == "wii-bridge-cues") {
|
||||
test_wii_bridge_cues();
|
||||
return 0;
|
||||
}
|
||||
if (scenario == "wii-bridge-cue-races") {
|
||||
test_wii_bridge_cue_races();
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
if (scenario == "transport-policy") {
|
||||
test_transport_mode_policy();
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -15,6 +15,11 @@ typedef enum wii_flags {
|
|||
void uni_hid_parser_wii_set_mode(uni_hid_device_t* device, wii_mode_t mode);
|
||||
bool uni_hid_parser_wii_accel_snapshot(
|
||||
uni_hid_device_t*, int32_t[3], uint32_t*);
|
||||
#ifdef SWITCH2_BRIDGE_WII_INPUT
|
||||
bool uni_hid_parser_wii_gyro_snapshot(
|
||||
uni_hid_device_t*, int32_t[3], uint32_t*);
|
||||
bool uni_hid_parser_wii_rumble_ready(uni_hid_device_t*);
|
||||
#endif
|
||||
bool uni_hid_parser_wii_nunchuk_accel_snapshot(
|
||||
uni_hid_device_t*, int32_t[3], uint32_t*);
|
||||
|
||||
|
|
|
|||
821
tests/switch2_native_imu_test.cpp
Normal file
821
tests/switch2_native_imu_test.cpp
Normal file
|
|
@ -0,0 +1,821 @@
|
|||
#include "native_imu.h"
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
namespace {
|
||||
int failures = 0;
|
||||
using Vector = std::array<double, 3>;
|
||||
using Quaternion = std::array<double, 4>;
|
||||
|
||||
void expect(bool okay, const char* message) {
|
||||
if (!okay) {
|
||||
std::cerr << message << '\n';
|
||||
++failures;
|
||||
}
|
||||
}
|
||||
|
||||
// Deliberately decode bit-by-bit rather than mirroring the encoder's word
|
||||
// boundaries. Double precision here is an independent host-side reference.
|
||||
uint32_t bits(const uint8_t* bytes, unsigned start, unsigned width) {
|
||||
uint32_t result = 0;
|
||||
for (unsigned i = 0; i < width; ++i) {
|
||||
result |= static_cast<uint32_t>((bytes[(start + i) / 8] >> ((start + i) % 8)) & 1) << i;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int64_t signed_bits(const uint8_t* bytes, unsigned start, unsigned width) {
|
||||
const uint32_t value = bits(bytes, start, width);
|
||||
return value & (UINT32_C(1) << (width - 1)) ? static_cast<int64_t>(value) - (INT64_C(1) << width) : value;
|
||||
}
|
||||
|
||||
struct Decoded {
|
||||
Quaternion q{};
|
||||
Vector acceleration{};
|
||||
std::array<int64_t, 3> acceleration_raw{};
|
||||
uint32_t counter = 0;
|
||||
uint32_t elapsed = 0;
|
||||
int64_t temperature = 0;
|
||||
};
|
||||
|
||||
Decoded decode(const uint8_t* bytes) {
|
||||
Decoded result;
|
||||
expect(bytes[3] == 0x0c, "wire block must use the recovered one-sample format");
|
||||
result.counter = bits(bytes, 0, 12);
|
||||
result.elapsed = bits(bytes, 12, 12);
|
||||
result.temperature = signed_bits(bytes, 224, 16);
|
||||
const unsigned largest = bits(bytes, 32, 3);
|
||||
expect(largest < 4, "wire quaternion tag must identify an actual component");
|
||||
if (largest >= 4) return result;
|
||||
Vector ratios{};
|
||||
double length_squared = 1.0;
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
ratios[i] = bits(bytes, 35 + 31 * i, 31) / 1073741824.0 - 1.0;
|
||||
length_squared += ratios[i] * ratios[i];
|
||||
result.acceleration_raw[i] = signed_bits(bytes, 128 + 32 * i, 32);
|
||||
result.acceleration[i] = result.acceleration_raw[i] / 268435456.0;
|
||||
}
|
||||
result.q[largest] = 1.0 / std::sqrt(length_squared);
|
||||
for (unsigned i = 0; i < 3; ++i) result.q[(largest + i + 1) & 3] = ratios[i] * result.q[largest];
|
||||
return result;
|
||||
}
|
||||
|
||||
Vector rotate(const Quaternion& q, const Vector& v) {
|
||||
const Vector cross{q[2] * v[2] - q[3] * v[1], q[3] * v[0] - q[1] * v[2], q[1] * v[1] - q[2] * v[0]};
|
||||
return {v[0] + 2.0 * (q[0] * cross[0] + q[2] * cross[2] - q[3] * cross[1]),
|
||||
v[1] + 2.0 * (q[0] * cross[1] + q[3] * cross[0] - q[1] * cross[2]),
|
||||
v[2] + 2.0 * (q[0] * cross[2] + q[1] * cross[1] - q[2] * cross[0])};
|
||||
}
|
||||
|
||||
bool close(const Vector& a, const Vector& b, double tolerance = 0.00002) {
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
if (!std::isfinite(a[i]) || std::abs(a[i] - b[i]) > tolerance) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
Quaternion normalized(const float input[4]) {
|
||||
double squared = 0.0;
|
||||
for (unsigned i = 0; i < 4; ++i) squared += static_cast<double>(input[i]) * input[i];
|
||||
Quaternion result{};
|
||||
for (unsigned i = 0; i < 4; ++i) result[i] = input[i] / std::sqrt(squared);
|
||||
return result;
|
||||
}
|
||||
|
||||
double camera_heading(const ProbeNativeMotion& motion) {
|
||||
const Vector forward = rotate(normalized(motion.quaternion()), {0.0, 1.0, 0.0});
|
||||
return std::atan2(forward[1], forward[0]);
|
||||
}
|
||||
|
||||
double heading_change(const ProbeNativeMotion& motion, double initial_heading) {
|
||||
return std::remainder(camera_heading(motion) - initial_heading, 2.0 * std::acos(-1.0));
|
||||
}
|
||||
|
||||
void check_encoded_orientation(const float q[4], const float accel[3]) {
|
||||
std::array<uint8_t, 32> guarded{};
|
||||
guarded.front() = 0x53;
|
||||
guarded.back() = 0x79;
|
||||
expect(probe_native_imu_pack(q, accel, 0xabc, 0xdef, -12345, guarded.data() + 1), "finite nonzero orientation must encode");
|
||||
expect(guarded.front() == 0x53 && guarded.back() == 0x79, "encoder must write exactly thirty bytes");
|
||||
const auto decoded = decode(guarded.data() + 1);
|
||||
expect(decoded.counter == 0xabc && decoded.elapsed == 0xdef && decoded.temperature == -12345,
|
||||
"wire counters and signed temperature must decode independently");
|
||||
for (unsigned axis = 0; axis < 3; ++axis) {
|
||||
Vector basis{};
|
||||
basis[axis] = 1.0;
|
||||
expect(close(rotate(decoded.q, basis), rotate(normalized(q), basis)),
|
||||
"wire orientation must preserve physical rotation for every largest-component choice");
|
||||
}
|
||||
expect(close(decoded.acceleration, {accel[0], accel[1], accel[2]}, 0.0000001),
|
||||
"wire acceleration must preserve signed Q28 g units");
|
||||
}
|
||||
|
||||
void codec_wire_edges() {
|
||||
const float accel[3]{1.25f, -2.5f, 0.0625f};
|
||||
const float orientations[][4]{{4.0f, -1.0f, 2.0f, -3.0f}, {-0.5f, -3.0f, 0.25f, 1.0f},
|
||||
{1.0f, -2.0f, 4.0f, -0.5f}, {-1.0f, 0.5f, -2.0f, -4.0f},
|
||||
{1.0f, -1.0f, 1.0f, 1.0f}};
|
||||
for (const auto& q : orientations) check_encoded_orientation(q, accel);
|
||||
const float large = std::numeric_limits<float>::max();
|
||||
const float tiny = std::numeric_limits<float>::denorm_min();
|
||||
const float large_q[4]{large, -large, large, -large};
|
||||
const float tiny_q[4]{tiny, -tiny, tiny, -tiny};
|
||||
check_encoded_orientation(large_q, accel);
|
||||
check_encoded_orientation(tiny_q, accel);
|
||||
|
||||
uint8_t bytes[30]{};
|
||||
const float identity[4]{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
const float saturated[3]{large, -large, -8.0f};
|
||||
expect(probe_native_imu_pack(identity, saturated, 0, 4095, -32768, bytes), "finite out-of-range acceleration must saturate");
|
||||
const auto limits = decode(bytes);
|
||||
expect(limits.acceleration_raw == std::array<int64_t, 3>{INT32_MAX, INT32_MIN, INT32_MIN},
|
||||
"positive saturation must never wrap to negative acceleration");
|
||||
expect(limits.counter == 0 && limits.elapsed == 4095 && limits.temperature == -32768,
|
||||
"wire signed and unsigned field boundaries must survive encoding");
|
||||
const float ties[3]{0.5f / 268435456.0f, 1.5f / 268435456.0f, -2.5f / 268435456.0f};
|
||||
const float fine_q[4]{1.0f, 5.5f / 1073741824.0f, -5.5f / 1073741824.0f, 0.0f};
|
||||
expect(probe_native_imu_pack(fine_q, ties, 0, 0, 0, bytes), "sub-count finite inputs must encode");
|
||||
expect(decode(bytes).acceleration_raw == std::array<int64_t, 3>{0, 2, -2}, "Q28 halfway values use nearest-even rounding");
|
||||
expect(bits(bytes, 35, 31) == UINT32_C(0x40000006) && bits(bytes, 66, 31) == UINT32_C(0x3ffffffa),
|
||||
"adding the ratio midpoint must not erase low quaternion bits");
|
||||
}
|
||||
|
||||
void codec_rejects_invalid_inputs() {
|
||||
float q[4]{1.0f, 0.0f, 0.0f, 0.0f};
|
||||
float accel[3]{0.0f, 0.0f, 1.0f};
|
||||
std::array<uint8_t, 30> output;
|
||||
output.fill(0xa5);
|
||||
const auto unchanged = output;
|
||||
const float invalid[]{std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::infinity(),
|
||||
-std::numeric_limits<float>::infinity()};
|
||||
for (float value : invalid) {
|
||||
q[2] = value;
|
||||
expect(!probe_native_imu_pack(q, accel, 1, 1, 0, output.data()), "nonfinite quaternion must be rejected");
|
||||
q[2] = 0.0f;
|
||||
accel[1] = value;
|
||||
expect(!probe_native_imu_pack(q, accel, 1, 1, 0, output.data()), "nonfinite acceleration must be rejected, not saturated");
|
||||
accel[1] = 0.0f;
|
||||
}
|
||||
q[0] = 0.0f;
|
||||
expect(!probe_native_imu_pack(q, accel, 1, 1, 0, output.data()), "zero quaternion cannot identify a real orientation");
|
||||
q[0] = 1.0f;
|
||||
expect(!probe_native_imu_pack(q, accel, 4096, 1, 0, output.data()), "counter overflow must fail rather than overwrite elapsed bits");
|
||||
expect(!probe_native_imu_pack(q, accel, 1, 4096, 0, output.data()), "elapsed overflow must fail rather than overwrite format bits");
|
||||
expect(output == unchanged, "failed encoding must leave the caller's pending packet unchanged");
|
||||
}
|
||||
|
||||
struct Rig {
|
||||
ProbeNativeMotion motion;
|
||||
ProbeNativeMotionSample sample{};
|
||||
uint32_t now;
|
||||
uint32_t generation = 1;
|
||||
static constexpr float residual[3]{0.4f, -0.3f, 0.2f};
|
||||
|
||||
explicit Rig(uint32_t start = 0) : now(start) {
|
||||
sample.accel_valid = sample.gyro_valid = true;
|
||||
sample.accel_g[2] = 1.0f;
|
||||
rest();
|
||||
}
|
||||
void rest() {
|
||||
for (unsigned i = 0; i < 3; ++i) sample.gyro_dps[i] = residual[i];
|
||||
}
|
||||
void fresh(uint32_t elapsed = 25000, bool accel = true, bool gyro = true) {
|
||||
now += elapsed;
|
||||
if (accel) {
|
||||
++sample.accel_sequence;
|
||||
sample.accel_us = now;
|
||||
}
|
||||
if (gyro) {
|
||||
++sample.gyro_sequence;
|
||||
sample.gyro_us = now;
|
||||
}
|
||||
motion.update(now, generation, sample);
|
||||
}
|
||||
void settle() {
|
||||
for (unsigned i = 0; i < 65; ++i) fresh();
|
||||
expect(motion.ready(), "fresh stationary sensors must finish residual bias calibration");
|
||||
}
|
||||
Decoded packet() {
|
||||
uint8_t bytes[30]{};
|
||||
expect(motion.ready(), "only calibrated fresh motion may be packed for the consumer");
|
||||
expect(probe_native_imu_pack(motion.quaternion(), motion.acceleration(), 0, 0, 0, bytes), "live motion must be encodable");
|
||||
return decode(bytes);
|
||||
}
|
||||
};
|
||||
|
||||
void startup_needs_count_and_stillness() {
|
||||
Rig fast;
|
||||
fast.fresh(0);
|
||||
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");
|
||||
for (unsigned i = 0; i < 56; ++i) fast.fresh();
|
||||
expect(!fast.motion.ready(), "calibration must not finish before the stillness duration");
|
||||
fast.fresh(37000);
|
||||
expect(fast.motion.ready(), "a stationary window may finish at exactly 1.5 seconds");
|
||||
|
||||
Rig slow;
|
||||
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");
|
||||
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");
|
||||
}
|
||||
|
||||
void quantized_wii_bias_calibrates_and_integrates() {
|
||||
Rig rig;
|
||||
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) {
|
||||
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;
|
||||
if (i % 2 == 0) {
|
||||
for (unsigned axis = 0; axis < 3; ++axis) {
|
||||
rig.sample.gyro_dps[axis] = device_bias[axis] + noise_q10[(i / 2 + axis * 3) % 8] / 1024.0f;
|
||||
}
|
||||
}
|
||||
rig.fresh(5000, true, i % 2 == 0);
|
||||
}
|
||||
expect(rig.motion.ready(), "quantized stationary Wii sensors with a large own-device bias must calibrate");
|
||||
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");
|
||||
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),
|
||||
"quantized gravity must establish the physical reference without rescaling acceleration");
|
||||
for (unsigned axis = 0; axis < 3; ++axis) {
|
||||
rig.sample.gyro_dps[axis] = device_bias[axis];
|
||||
rig.sample.accel_g[axis] = static_cast<float>(gravity[axis]);
|
||||
}
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 0; i < 100; ++i) rig.fresh(10000);
|
||||
expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), rotate(reference, {1.0, 0.0, 0.0}), 0.001),
|
||||
"learned large residual bias must not become stationary orientation drift");
|
||||
rig.sample.gyro_dps[2] += 90.0f;
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 1; i <= 100; ++i) {
|
||||
const double angle = i * std::acos(-1.0) / 200.0;
|
||||
rig.sample.accel_g[0] = static_cast<float>(std::cos(angle) * gravity[0] + std::sin(angle) * gravity[1]);
|
||||
rig.sample.accel_g[1] = static_cast<float>(-std::sin(angle) * gravity[0] + std::cos(angle) * gravity[1]);
|
||||
rig.fresh(10000);
|
||||
}
|
||||
expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), rotate(reference, {0.0, 1.0, 0.0}), 0.001),
|
||||
"one second of body rotation must subtract the measured Wii bias before integration");
|
||||
}
|
||||
|
||||
void measured_gravity_sets_reference() {
|
||||
const float poses[][3]{{1.0f, 0.0f, 0.0f}, {0.0f, 0.0f, -1.0f},
|
||||
{0.000001f, 0.000002f, -1.0f}, {0.3f, 0.4f, 0.8660254f}};
|
||||
for (const auto& pose : poses) {
|
||||
Rig rig;
|
||||
for (unsigned i = 0; i < 3; ++i) rig.sample.accel_g[i] = pose[i];
|
||||
rig.settle();
|
||||
const auto decoded = rig.packet();
|
||||
expect(close(rotate(decoded.q, decoded.acceleration), {0.0, 0.0, 1.0}),
|
||||
"measured gravity, including anti-parallel gravity, must align with reference +Z");
|
||||
expect(close(decoded.acceleration, {pose[0], pose[1], pose[2]}, 0.0000001),
|
||||
"reference initialization must not replace the real body acceleration");
|
||||
}
|
||||
}
|
||||
|
||||
void bias_corrected_body_rotation_reaches_wire() {
|
||||
Rig rig;
|
||||
rig.sample.accel_g[0] = 1.0f;
|
||||
rig.sample.accel_g[2] = 0.0f;
|
||||
rig.settle();
|
||||
for (unsigned i = 0; i < 100; ++i) rig.fresh(10000);
|
||||
expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {0.0, 0.0, 1.0}),
|
||||
"stationary residual bias must not rotate the initialized orientation");
|
||||
rig.sample.gyro_dps[2] += 90.0f;
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 1; i <= 100; ++i) {
|
||||
const double angle = i * std::acos(-1.0) / 200.0;
|
||||
rig.sample.accel_g[0] = static_cast<float>(std::cos(angle));
|
||||
rig.sample.accel_g[1] = static_cast<float>(-std::sin(angle));
|
||||
rig.fresh(10000);
|
||||
}
|
||||
const auto turned = rig.packet();
|
||||
expect(close(rotate(turned.q, {1.0, 0.0, 0.0}), {0.0, 1.0, 0.0}) &&
|
||||
close(rotate(turned.q, {0.0, 1.0, 0.0}), {0.0, 0.0, -1.0}) &&
|
||||
close(rotate(turned.q, {0.0, 0.0, 1.0}), {-1.0, 0.0, 0.0}),
|
||||
"one second of corrected body-Z rotation must compose after the nonidentity gravity alignment");
|
||||
rig.sample.accel_g[0] = 1.6f;
|
||||
rig.sample.accel_g[1] = -0.3f;
|
||||
rig.sample.accel_g[2] = 0.8f;
|
||||
rig.fresh(0);
|
||||
expect(close(rig.packet().acceleration, {1.6, -0.3, 0.8}, 0.0000001), "ready packets must retain real dynamic acceleration");
|
||||
expect(close({rig.motion.bias()[0], rig.motion.bias()[1], rig.motion.bias()[2]},
|
||||
{Rig::residual[0], Rig::residual[1], Rig::residual[2]}, 0.000001),
|
||||
"motion after calibration must not relearn or chase the gyro bias");
|
||||
}
|
||||
|
||||
void elapsed_time_not_packet_count_controls_rotation() {
|
||||
for (uint32_t interval : {5000u, 25000u}) {
|
||||
Rig rig;
|
||||
rig.settle();
|
||||
rig.sample.gyro_dps[2] += 120.0f;
|
||||
rig.fresh(0);
|
||||
for (uint32_t elapsed = interval; elapsed <= 1000000; elapsed += interval) {
|
||||
rig.fresh(interval, true, elapsed % 100000 == 0);
|
||||
}
|
||||
expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {-0.5, std::sqrt(0.75), 0.0}),
|
||||
"a fresh held gyro rate must integrate elapsed time regardless of polling cadence");
|
||||
}
|
||||
Rig split;
|
||||
split.settle();
|
||||
split.sample.gyro_dps[2] += 90.0f;
|
||||
++split.sample.gyro_sequence;
|
||||
split.sample.gyro_us = split.now + 10000;
|
||||
split.fresh(40000, true, false);
|
||||
const double angle = 2.7 * std::acos(-1.0) / 180.0;
|
||||
expect(close(rotate(split.packet().q, {1.0, 0.0, 0.0}), {std::cos(angle), std::sin(angle), 0.0}),
|
||||
"a rate arriving inside an update interval must only affect time after its receipt");
|
||||
}
|
||||
|
||||
void moving_startup_does_not_calibrate() {
|
||||
for (unsigned movement = 0; movement < 4; ++movement) {
|
||||
Rig rig;
|
||||
for (unsigned i = 0; i < 100; ++i) {
|
||||
rig.rest();
|
||||
rig.sample.accel_g[0] = 0.0f;
|
||||
rig.sample.accel_g[1] = 0.0f;
|
||||
rig.sample.accel_g[2] = 1.0f;
|
||||
if (movement == 0) {
|
||||
// A real slow body-X turn changes gravity even with a constant
|
||||
// gyro reading. Its per-packet tilt is smaller than Wii noise.
|
||||
const float angle = static_cast<float>(i) * 0.025f * 2.0f * static_cast<float>(std::acos(-1.0) / 180.0);
|
||||
rig.sample.gyro_dps[0] += 2.0f;
|
||||
rig.sample.accel_g[1] = std::sin(angle);
|
||||
rig.sample.accel_g[2] = std::cos(angle);
|
||||
}
|
||||
if (movement == 1) rig.sample.accel_g[2] = 0.5f;
|
||||
if (movement == 2) rig.sample.accel_g[0] = i & 1 ? 0.04f : -0.04f;
|
||||
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");
|
||||
rig.rest();
|
||||
rig.sample.accel_g[0] = 0.0f;
|
||||
rig.sample.accel_g[1] = 0.0f;
|
||||
rig.sample.accel_g[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");
|
||||
}
|
||||
Rig contaminated;
|
||||
for (unsigned i = 0; i < 40; ++i) contaminated.fresh();
|
||||
contaminated.sample.gyro_dps[0] = 4.0f;
|
||||
contaminated.fresh();
|
||||
contaminated.rest();
|
||||
for (unsigned i = 0; i < 40; ++i) contaminated.fresh();
|
||||
expect(!contaminated.motion.ready(), "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");
|
||||
|
||||
Rig interleaved;
|
||||
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");
|
||||
interleaved.settle();
|
||||
}
|
||||
|
||||
void duplicate_sequences_and_interleaved_recovery() {
|
||||
Rig rig;
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 0; i < 400; ++i) {
|
||||
// A caller cannot renew sensor lifetime by relabelling receipt time
|
||||
// while leaving the actual parser sequence unchanged.
|
||||
rig.sample.accel_us = rig.sample.gyro_us = rig.now + 5000;
|
||||
rig.fresh(5000, false, false);
|
||||
}
|
||||
expect(!rig.motion.ready(), "duplicate samples cannot establish calibrated sensor readiness");
|
||||
for (unsigned i = 0; i < 128; ++i) rig.fresh(25000, i % 2 == 0, i % 2 != 0);
|
||||
expect(rig.motion.ready(), "fresh independently interleaved sensors must recover after a stale reset");
|
||||
rig.sample.gyro_dps[2] += 100.0f;
|
||||
rig.fresh(10000, false, false);
|
||||
expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {1.0, 0.0, 0.0}),
|
||||
"mutating a duplicate sequence must not inject a new angular rate");
|
||||
}
|
||||
|
||||
void lifecycle_invalidates_bias_and_orientation() {
|
||||
for (unsigned fault = 0; fault < 7; ++fault) {
|
||||
Rig rig;
|
||||
rig.settle();
|
||||
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();
|
||||
if (fault == 3) rig.sample.gyro_dps[0] = std::numeric_limits<float>::infinity();
|
||||
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");
|
||||
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();
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
void stale_boundaries_and_hidden_gaps() {
|
||||
for (bool stale_gyro : {false, true}) {
|
||||
Rig rig;
|
||||
rig.settle();
|
||||
rig.fresh(50000, stale_gyro, !stale_gyro);
|
||||
rig.fresh(50000, stale_gyro, !stale_gyro);
|
||||
rig.fresh(49999, stale_gyro, !stale_gyro);
|
||||
expect(rig.motion.ready(), "a held sensor remains usable strictly below the 150ms age limit");
|
||||
rig.fresh(1, stale_gyro, !stale_gyro);
|
||||
expect(!rig.motion.ready(), "either sensor reaching the 150ms limit must invalidate motion");
|
||||
rig.settle();
|
||||
}
|
||||
Rig hidden;
|
||||
hidden.settle();
|
||||
hidden.sample.gyro_dps[2] += 90.0f;
|
||||
hidden.fresh(0);
|
||||
hidden.fresh(50000, false, false);
|
||||
hidden.fresh(50000, false, false);
|
||||
expect(hidden.motion.ready(), "a 50ms update gap is still within the integration limit");
|
||||
hidden.fresh(50000);
|
||||
expect(hidden.motion.ready(), "a replacement exactly at expiry must preserve the continuously covered interval");
|
||||
hidden.fresh(50000, false, false);
|
||||
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");
|
||||
hidden.rest();
|
||||
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");
|
||||
}
|
||||
|
||||
void clocks_sequences_and_connections_wrap() {
|
||||
Rig rig(UINT32_MAX - 2200000u);
|
||||
rig.sample.accel_sequence = rig.sample.gyro_sequence = UINT32_MAX - 30u;
|
||||
rig.settle();
|
||||
rig.sample.gyro_dps[2] += 90.0f;
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 0; i < 80; ++i) rig.fresh(12500);
|
||||
expect(close(rotate(rig.packet().q, {1.0, 0.0, 0.0}), {0.0, 1.0, 0.0}),
|
||||
"microsecond rollover must preserve one second of physical angular displacement");
|
||||
++rig.generation;
|
||||
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");
|
||||
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");
|
||||
|
||||
Rig calibration_wrap(UINT32_MAX - 750000u);
|
||||
calibration_wrap.settle();
|
||||
expect(calibration_wrap.motion.ready(), "the stationary window must span microsecond rollover safely");
|
||||
}
|
||||
|
||||
void fresh_gravity_bounds_warm_gyro_tilt_drift() {
|
||||
Rig normal, fast;
|
||||
for (Rig* rig : {&normal, &fast}) {
|
||||
rig->sample.accel_g[0] = 1.0f;
|
||||
rig->sample.accel_g[2] = 0.0f;
|
||||
rig->settle();
|
||||
// The console trace drifted roughly 0.6 degrees/s after startup.
|
||||
rig->sample.gyro_dps[1] += 0.6f;
|
||||
rig->fresh(0);
|
||||
}
|
||||
double maximum_tilt = 0.0;
|
||||
for (unsigned millisecond = 1; millisecond <= 60000; ++millisecond) {
|
||||
fast.fresh(1000, millisecond % 10 == 0, millisecond % 10 == 0);
|
||||
if (millisecond % 10 == 0) normal.fresh(10000);
|
||||
if (millisecond % 1000 == 0) {
|
||||
const Vector gravity = rotate(normal.packet().q, {1.0, 0.0, 0.0});
|
||||
const double tilt = std::atan2(std::hypot(gravity[0], gravity[1]), gravity[2]);
|
||||
if (tilt > maximum_tilt) maximum_tilt = tilt;
|
||||
}
|
||||
}
|
||||
expect(maximum_tilt < std::acos(-1.0) / 180.0,
|
||||
"fresh stationary gravity must keep warming gyro tilt drift below one degree");
|
||||
expect(close(rotate(normal.packet().q, {1.0, 0.0, 0.0}),
|
||||
rotate(fast.packet().q, {1.0, 0.0, 0.0}), 0.0002),
|
||||
"duplicate accelerometer polling must not strengthen gravity correction");
|
||||
}
|
||||
|
||||
void dynamic_acceleration_does_not_steer_orientation() {
|
||||
Rig rig;
|
||||
rig.settle();
|
||||
rig.sample.gyro_dps[2] += 60.0f;
|
||||
rig.sample.accel_g[0] = 2.0f;
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 0; i < 100; ++i) rig.fresh(10000);
|
||||
const auto packet = rig.packet();
|
||||
expect(close(rotate(packet.q, {1.0, 0.0, 0.0}), {0.5, std::sqrt(0.75), 0.0}),
|
||||
"non-gravity acceleration must not steer gyro orientation");
|
||||
expect(close(packet.acceleration, {2.0, 0.0, 1.0}),
|
||||
"gravity correction must not replace the consumer's dynamic acceleration");
|
||||
}
|
||||
|
||||
void optical_heading_bounds_drift_without_polling_gain() {
|
||||
Rig slow, frequent, gyro_only;
|
||||
for (Rig* rig : {&slow, &frequent, &gyro_only}) {
|
||||
rig->settle();
|
||||
rig->sample.gyro_dps[2] += 0.6f;
|
||||
rig->fresh(0);
|
||||
}
|
||||
const double initial = camera_heading(slow.motion);
|
||||
slow.motion.observe_optical_heading(slow.now, 1, 0, slow.now, 0.2f, true);
|
||||
frequent.motion.observe_optical_heading(frequent.now, 1, 0, frequent.now, 0.2f, true);
|
||||
double maximum_drift = 0.0;
|
||||
for (uint32_t step = 1; step <= 6000; ++step) {
|
||||
slow.fresh(10000);
|
||||
frequent.fresh(10000);
|
||||
gyro_only.fresh(10000);
|
||||
// The same physical reference at 10Hz and 50Hz, with extra polling of
|
||||
// duplicate 50Hz reports, must give essentially the same drift bound.
|
||||
if (step % 10 == 0) {
|
||||
slow.motion.observe_optical_heading(slow.now, 1, step / 10, slow.now, 0.2f, true);
|
||||
}
|
||||
frequent.motion.observe_optical_heading(frequent.now, 1, step / 2,
|
||||
frequent.now, 0.2f, true);
|
||||
if (step % 10 == 0) {
|
||||
const double drift = std::abs(heading_change(slow.motion, initial));
|
||||
if (drift > maximum_drift) maximum_drift = drift;
|
||||
}
|
||||
}
|
||||
expect(maximum_drift < 1.3 * std::acos(-1.0) / 180.0,
|
||||
"stable optical bearing must bound a minute of warming gyro yaw drift");
|
||||
expect(std::abs(heading_change(slow.motion, camera_heading(frequent.motion))) < 0.0002,
|
||||
"optical correction strength must follow sample time, not sample or polling count");
|
||||
expect(std::abs(heading_change(gyro_only.motion, initial) - 36.0 * std::acos(-1.0) / 180.0) < 0.0002,
|
||||
"without optical observations gravity must leave yaw gyro-derived");
|
||||
}
|
||||
|
||||
void optical_anchor_preserves_pose_and_corrects_only_world_yaw() {
|
||||
Rig rig;
|
||||
rig.sample.accel_g[0] = 0.3f;
|
||||
rig.sample.accel_g[1] = 0.4f;
|
||||
rig.sample.accel_g[2] = std::sqrt(0.75f);
|
||||
rig.settle();
|
||||
// Establish arbitrary nonzero world yaw while preserving this physical tilt.
|
||||
for (unsigned i = 0; i < 3; ++i) rig.sample.gyro_dps[i] += 60.0f * rig.sample.accel_g[i];
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 0; i < 20; ++i) rig.fresh(25000);
|
||||
rig.rest();
|
||||
rig.fresh(0);
|
||||
const Quaternion initial = normalized(rig.motion.quaternion());
|
||||
const double initial_heading = camera_heading(rig.motion);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, 1, rig.now, 0.45f, true);
|
||||
expect(normalized(rig.motion.quaternion()) == initial,
|
||||
"the first optical sample must preserve arbitrary existing yaw and tilt exactly");
|
||||
rig.sample.accel_g[0] = 1.8f;
|
||||
rig.sample.accel_g[1] = -0.2f;
|
||||
rig.sample.accel_g[2] = 0.6f;
|
||||
rig.fresh(0);
|
||||
for (uint32_t step = 1; step <= 400; ++step) {
|
||||
rig.fresh(25000);
|
||||
if (step % 4 == 0) {
|
||||
rig.motion.observe_optical_heading(rig.now, 1, step / 4 + 1, rig.now, 0.8f, true);
|
||||
}
|
||||
}
|
||||
expect(std::abs(heading_change(rig.motion, initial_heading) + 0.35) < 0.004,
|
||||
"increasing aim-right optical yaw must converge to negative world yaw, not reanchor away motion");
|
||||
const Quaternion corrected = normalized(rig.motion.quaternion());
|
||||
for (unsigned axis = 0; axis < 3; ++axis) {
|
||||
Vector basis{};
|
||||
basis[axis] = 1.0;
|
||||
expect(std::abs(rotate(initial, basis)[2] - rotate(corrected, basis)[2]) < 0.000002,
|
||||
"optical correction must preserve every body axis's gravity-aligned tilt");
|
||||
}
|
||||
expect(close(rig.packet().acceleration, {1.8, -0.2, 0.6}, 0.0000001),
|
||||
"optical correction must retain real dynamic acceleration in the native packet");
|
||||
expect(close({rig.motion.bias()[0], rig.motion.bias()[1], rig.motion.bias()[2]},
|
||||
{Rig::residual[0], Rig::residual[1], Rig::residual[2]}, 0.000001),
|
||||
"optical correction must not modify the calibrated raw gyro bias");
|
||||
}
|
||||
|
||||
void optical_duplicates_and_hidden_intervals_do_not_gain_weight() {
|
||||
Rig rig;
|
||||
rig.settle();
|
||||
const double initial_heading = camera_heading(rig.motion);
|
||||
uint32_t sequence = 10;
|
||||
rig.motion.observe_optical_heading(rig.now, 1, sequence, rig.now, 0.0f, true);
|
||||
rig.sample.gyro_dps[2] += 30.0f;
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 0; i < 40; ++i) {
|
||||
rig.fresh(25000);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, false);
|
||||
}
|
||||
rig.rest();
|
||||
rig.fresh(0);
|
||||
expect(std::abs(heading_change(rig.motion, initial_heading) - std::acos(-1.0) / 6.0) < 0.00002,
|
||||
"hidden or inferred optical reports must not invent a heading correction");
|
||||
const double hidden_heading = camera_heading(rig.motion);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, true);
|
||||
expect(std::abs(heading_change(rig.motion, hidden_heading)) < 0.000001,
|
||||
"reacquisition must not spend correction weight accumulated while hidden");
|
||||
for (unsigned i = 0; i < 4; ++i) rig.fresh(25000);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, true);
|
||||
expect(heading_change(rig.motion, hidden_heading) < -0.02,
|
||||
"continuous reacquisition must correct toward the retained original relative anchor");
|
||||
const double corrected_heading = camera_heading(rig.motion);
|
||||
for (unsigned i = 0; i < 200; ++i) {
|
||||
rig.fresh(1000);
|
||||
// A changed value and a newly labelled timestamp are still one packet.
|
||||
rig.motion.observe_optical_heading(rig.now, 1, sequence, rig.now, -1.0f, true);
|
||||
}
|
||||
expect(std::abs(heading_change(rig.motion, corrected_heading)) < 0.000001,
|
||||
"duplicate optical sequence identities cannot strengthen correction or inject changed bearings");
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, true);
|
||||
expect(std::abs(heading_change(rig.motion, corrected_heading)) < 0.000001,
|
||||
"duplicate timestamps cannot keep optical validity alive across a stale interval");
|
||||
}
|
||||
|
||||
void optical_rejects_bad_samples_and_clock_order() {
|
||||
Rig rig;
|
||||
rig.settle();
|
||||
rig.motion.observe_optical_heading(rig.now, 1, 1, rig.now, 0.0f, true);
|
||||
uint32_t sequence = 1;
|
||||
const double initial_heading = camera_heading(rig.motion);
|
||||
// Each rejected identity must remain rejected if its fields are rewritten.
|
||||
for (unsigned fault = 0; fault < 4; ++fault) {
|
||||
for (unsigned i = 0; i < 4; ++i) rig.fresh(25000);
|
||||
const float bearing = fault == 0 ? std::numeric_limits<float>::quiet_NaN() : 1.0f;
|
||||
const uint32_t timestamp = fault == 1 ? rig.now - 150000u :
|
||||
fault == 2 ? rig.now + 1u : rig.now;
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, timestamp, bearing, fault != 3);
|
||||
for (unsigned i = 0; i < 4; ++i) {
|
||||
rig.fresh(25000);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, sequence, rig.now, 1.0f, true);
|
||||
}
|
||||
expect(std::abs(heading_change(rig.motion, initial_heading)) < 0.000001,
|
||||
"nonfinite, stale, future or unavailable optical identities cannot later be relabelled valid");
|
||||
}
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, 0.0f, true);
|
||||
const uint32_t last_timestamp = rig.now;
|
||||
rig.fresh(25000);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, last_timestamp - 1u, 1.0f, true);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, sequence - 1u, rig.now, 1.0f, true);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, last_timestamp, 1.0f, true);
|
||||
expect(std::abs(heading_change(rig.motion, initial_heading)) < 0.000001,
|
||||
"backwards clocks, out-of-order sequences and zero-time samples must not correct heading");
|
||||
// A subsequent ordered sample still contributes its own sensor-time
|
||||
// interval after the rejected observations.
|
||||
rig.fresh(25000);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now - 1u, 1.0f, true);
|
||||
expect(heading_change(rig.motion, initial_heading) < -0.02,
|
||||
"a later fresh unique optical sample must still correct after rejected clock order");
|
||||
}
|
||||
|
||||
void optical_generation_and_motion_lifecycle_reanchor() {
|
||||
for (unsigned transition = 0; transition < 4; ++transition) {
|
||||
Rig rig;
|
||||
rig.settle();
|
||||
rig.motion.observe_optical_heading(rig.now, UINT32_MAX, 100, rig.now, 0.0f, true);
|
||||
for (unsigned i = 0; i < 4; ++i) rig.fresh(25000);
|
||||
rig.motion.observe_optical_heading(rig.now, UINT32_MAX, 101, rig.now, 1.0f, true);
|
||||
expect(heading_change(rig.motion, std::acos(-1.0) / 2.0) < -0.04,
|
||||
"the old optical reference must be active before lifecycle replacement");
|
||||
uint32_t optical_generation = UINT32_MAX;
|
||||
uint32_t sequence = 102;
|
||||
if (transition == 0) {
|
||||
optical_generation = 0;
|
||||
sequence = 0;
|
||||
} else {
|
||||
if (transition == 1) {
|
||||
rig.sample.gyro_valid = false;
|
||||
rig.fresh(0);
|
||||
rig.sample.gyro_valid = true;
|
||||
}
|
||||
if (transition == 2) rig.motion.reset();
|
||||
if (transition == 3) ++rig.generation;
|
||||
rig.settle();
|
||||
}
|
||||
const Quaternion before = normalized(rig.motion.quaternion());
|
||||
const double heading = camera_heading(rig.motion);
|
||||
rig.motion.observe_optical_heading(rig.now, optical_generation, sequence, rig.now, -0.8f, true);
|
||||
expect(normalized(rig.motion.quaternion()) == before,
|
||||
"optical generation, motion invalidation, reset and reconnect must anchor without an initial jump");
|
||||
for (unsigned i = 0; i < 40; ++i) {
|
||||
rig.fresh(25000);
|
||||
if (i % 4 == 3) {
|
||||
rig.motion.observe_optical_heading(rig.now, optical_generation, ++sequence, rig.now, -0.8f, true);
|
||||
}
|
||||
}
|
||||
expect(std::abs(heading_change(rig.motion, heading)) < 0.000002,
|
||||
"a replaced optical reference must not pull toward the preceding lifecycle's bearing");
|
||||
}
|
||||
}
|
||||
|
||||
void optical_angles_and_sample_clocks_wrap() {
|
||||
Rig rig(UINT32_MAX - 1750000u);
|
||||
rig.settle();
|
||||
const float pi = std::acos(-1.0f);
|
||||
const float initial_bearing = pi - 0.02f;
|
||||
const double initial_heading = camera_heading(rig.motion);
|
||||
uint32_t sequence = UINT32_MAX - 1u;
|
||||
rig.motion.observe_optical_heading(rig.now, 1, sequence, rig.now, initial_bearing, true);
|
||||
// Matching physical yaw and optical motion cross both wrap boundaries.
|
||||
rig.sample.gyro_dps[2] -= 30.0f;
|
||||
rig.fresh(0);
|
||||
for (unsigned step = 1; step <= 20; ++step) {
|
||||
rig.fresh(25000);
|
||||
if (step % 4 == 0) {
|
||||
const float bearing = std::remainder(initial_bearing + step * 0.025f * pi / 6.0f, 2.0f * pi);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, bearing, true);
|
||||
}
|
||||
}
|
||||
expect(std::abs(heading_change(rig.motion, initial_heading) + std::acos(-1.0) / 12.0) < 0.00002,
|
||||
"optical angle, sequence and microsecond wrap must preserve intentional yaw direction without a jump");
|
||||
rig.rest();
|
||||
rig.fresh(0);
|
||||
const float final_bearing = std::remainder(initial_bearing + pi / 12.0f + 0.1f, 2.0f * pi);
|
||||
for (unsigned step = 1; step <= 400; ++step) {
|
||||
rig.fresh(25000);
|
||||
if (step % 4 == 0) {
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, final_bearing, true);
|
||||
}
|
||||
}
|
||||
expect(std::abs(heading_change(rig.motion, initial_heading) + std::acos(-1.0) / 12.0 + 0.1) < 0.001,
|
||||
"wrapped optical observations must continue correcting toward the original relative anchor");
|
||||
}
|
||||
|
||||
void optical_vertical_forward_defers_the_anchor() {
|
||||
Rig rig;
|
||||
rig.sample.accel_g[1] = 1.0f;
|
||||
rig.sample.accel_g[2] = 0.0f;
|
||||
rig.settle();
|
||||
const Quaternion vertical = normalized(rig.motion.quaternion());
|
||||
uint32_t sequence = 0;
|
||||
for (unsigned i = 0; i < 20; ++i) {
|
||||
rig.fresh(25000);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, i * 0.1f, true);
|
||||
}
|
||||
expect(close(rotate(normalized(rig.motion.quaternion()), {1.0, 0.0, 0.0}),
|
||||
rotate(vertical, {1.0, 0.0, 0.0})),
|
||||
"near-vertical camera forward must reject ill-conditioned optical yaw");
|
||||
// Tilt smoothly away from vertical with matching physical gravity.
|
||||
rig.sample.gyro_dps[0] -= 90.0f;
|
||||
rig.fresh(0);
|
||||
for (unsigned i = 1; i <= 40; ++i) {
|
||||
const float angle = i * std::acos(-1.0f) / 80.0f;
|
||||
rig.sample.accel_g[1] = std::cos(angle);
|
||||
rig.sample.accel_g[2] = std::sin(angle);
|
||||
rig.fresh(25000);
|
||||
}
|
||||
rig.rest();
|
||||
rig.fresh(0);
|
||||
const Quaternion horizontal = normalized(rig.motion.quaternion());
|
||||
const double heading = camera_heading(rig.motion);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, -0.6f, true);
|
||||
expect(normalized(rig.motion.quaternion()) == horizontal,
|
||||
"the first well-conditioned optical observation must establish the anchor without jumping");
|
||||
for (unsigned i = 0; i < 4; ++i) rig.fresh(25000);
|
||||
rig.motion.observe_optical_heading(rig.now, 1, ++sequence, rig.now, -0.3f, true);
|
||||
expect(heading_change(rig.motion, heading) < -0.01,
|
||||
"heading correction must recover after the forward projection leaves vertical");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main() {
|
||||
codec_wire_edges();
|
||||
codec_rejects_invalid_inputs();
|
||||
startup_needs_count_and_stillness();
|
||||
quantized_wii_bias_calibrates_and_integrates();
|
||||
measured_gravity_sets_reference();
|
||||
bias_corrected_body_rotation_reaches_wire();
|
||||
elapsed_time_not_packet_count_controls_rotation();
|
||||
moving_startup_does_not_calibrate();
|
||||
duplicate_sequences_and_interleaved_recovery();
|
||||
lifecycle_invalidates_bias_and_orientation();
|
||||
stale_boundaries_and_hidden_gaps();
|
||||
clocks_sequences_and_connections_wrap();
|
||||
fresh_gravity_bounds_warm_gyro_tilt_drift();
|
||||
dynamic_acceleration_does_not_steer_orientation();
|
||||
optical_heading_bounds_drift_without_polling_gain();
|
||||
optical_anchor_preserves_pose_and_corrects_only_world_yaw();
|
||||
optical_duplicates_and_hidden_intervals_do_not_gain_weight();
|
||||
optical_rejects_bad_samples_and_clock_order();
|
||||
optical_generation_and_motion_lifecycle_reanchor();
|
||||
optical_angles_and_sample_clocks_wrap();
|
||||
optical_vertical_forward_defers_the_anchor();
|
||||
return failures ? 1 : 0;
|
||||
}
|
||||
263
tests/switch2_wii_bridge_test.cpp
Normal file
263
tests/switch2_wii_bridge_test.cpp
Normal file
|
|
@ -0,0 +1,263 @@
|
|||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "controller_input.h"
|
||||
#include "input/bluepad32_input_backend.h"
|
||||
#include "input/wii_ir_pointer.h"
|
||||
#include "platform/pico/bootsel_pairing_button.h"
|
||||
#include "platform/pico/system_clock.h"
|
||||
#include "profile/controller_profile_runtime.h"
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
static uint64_t now_us;
|
||||
static uint32_t stage;
|
||||
static Bluepad32WiiBridgeSnapshot source;
|
||||
static bool swap_faces = true;
|
||||
static bool publish_during_snapshot;
|
||||
static uint32_t ir_sequence;
|
||||
static uint32_t sensor_sequence;
|
||||
static uint32_t last_sensor_us;
|
||||
static uint16_t raw_ir_buttons;
|
||||
static uint16_t ir_x[4] = {420, 620, 0, 0};
|
||||
static uint16_t ir_y[4] = {384, 384, 0, 0};
|
||||
static uint8_t ir_mask = 3;
|
||||
static probe_controller_input controls;
|
||||
static uint8_t packet[63];
|
||||
static const int32_t bias_q10[3] = {1024, -2048, 512};
|
||||
|
||||
uint32_t time_us_32() { return static_cast<uint32_t>(now_us); }
|
||||
absolute_time_t get_absolute_time() { return now_us; }
|
||||
uint32_t to_ms_since_boot(absolute_time_t time) { return static_cast<uint32_t>(time / 1000); }
|
||||
absolute_time_t make_timeout_time_ms(uint32_t ms) { return now_us + 1000ull * ms; }
|
||||
bool time_reached(absolute_time_t deadline) { return now_us >= deadline; }
|
||||
void sleep_ms(uint32_t ms) { now_us += 1000ull * ms; }
|
||||
void system_clock_initialize() {}
|
||||
extern "C" int probe_debug_printf(const char*, ...) { return 0; }
|
||||
BootselPairingButtonEvent bootsel_pairing_button_task() { return BootselPairingButtonEvent::kNone; }
|
||||
|
||||
void bluepad32_input_backend_init() { stage = 1; wii_ir_pointer_init(); }
|
||||
void bluepad32_input_backend_start() { stage = 2; }
|
||||
void bluepad32_input_backend_diagnostics(Bluepad32BackendDiagnostics* out) { *out = {}; out->initialization_stage = stage; }
|
||||
void bluepad32_input_backend_open_pairing_window() {}
|
||||
void bluepad32_input_backend_select_wii_source(const uint8_t[6]) { wii_ir_pointer_reset(); }
|
||||
void bluepad32_input_backend_wii_snapshot(Bluepad32WiiBridgeSnapshot* out) {
|
||||
if (publish_during_snapshot) {
|
||||
// Model Core1 publishing after Core0 entered the snapshot call.
|
||||
now_us += 300;
|
||||
source.accel_sequence = source.gyro_sequence = ++sensor_sequence;
|
||||
source.received_us = source.accel_received_us = source.gyro_received_us = time_us_32();
|
||||
}
|
||||
*out = source;
|
||||
}
|
||||
bool bluepad32_input_backend_set_wii_orientation(const ControllerIdentity&, uint32_t, bool) { return false; }
|
||||
void bluepad32_input_backend_report_sent(uint8_t) {}
|
||||
bool bluepad32_input_backend_wii_sample_request(uint8_t, uint64_t*) { return false; }
|
||||
int bluepad32_input_backend_wii_sample_result(uint64_t) { return -1; }
|
||||
void bluepad32_input_backend_wii_sample_cancel() {}
|
||||
void bluepad32_input_backend_queue_profile_feedback(uint8_t, uint32_t, uint8_t, ControllerProfileConfirmationPolicy) {}
|
||||
void controller_profile_runtime_reset() {}
|
||||
bool controller_profile_runtime_take_initial_profile_indication(uint8_t, ControllerProfileRuntimeProfileChangeEvent*) { return false; }
|
||||
bool controller_profile_runtime_take_profile_change(uint8_t, ControllerProfileRuntimeProfileChangeEvent*) { return false; }
|
||||
ControllerProfileTransformResult controller_profile_runtime_transform(
|
||||
uint8_t, const Bluepad32SlotSnapshot& input, uint32_t, AdapterUsbMode) {
|
||||
ControllerProfileTransformResult result{};
|
||||
result.state = input.state;
|
||||
if (swap_faces) {
|
||||
result.state.button_south = input.state.button_east;
|
||||
result.state.button_east = input.state.button_south;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
static void put_pair(uint8_t* out, uint16_t x, uint16_t y) {
|
||||
out[0] = static_cast<uint8_t>(x);
|
||||
out[1] = static_cast<uint8_t>((x >> 8) | (y << 4));
|
||||
out[2] = static_cast<uint8_t>(y >> 4);
|
||||
}
|
||||
static uint16_t stick_x(const uint8_t* p) { return p[5] | ((p[6] & 15u) << 8); }
|
||||
static uint16_t stick_y(const uint8_t* p) { return (p[6] >> 4) | (p[7] << 4); }
|
||||
static int32_t signed32(const uint8_t* p) {
|
||||
const uint32_t raw = p[0] | (uint32_t(p[1]) << 8) | (uint32_t(p[2]) << 16) | (uint32_t(p[3]) << 24);
|
||||
return raw <= INT32_MAX ? static_cast<int32_t>(raw) : -1 - static_cast<int32_t>(UINT32_MAX - raw);
|
||||
}
|
||||
static uint32_t get_bits(const uint8_t* p, unsigned start, unsigned count) {
|
||||
uint32_t result = 0;
|
||||
for (unsigned i = 0; i < count; ++i) result |= uint32_t((p[(start+i)/8] >> ((start+i)%8)) & 1) << i;
|
||||
return result;
|
||||
}
|
||||
static void decode_quaternion(const uint8_t* p, double out[4]) {
|
||||
const uint8_t* imu = p + 16;
|
||||
const unsigned index = get_bits(imu, 32, 3);
|
||||
assert(index < 4);
|
||||
double ratios[3], norm = 1;
|
||||
for (unsigned i = 0; i < 3; ++i) {
|
||||
ratios[i] = get_bits(imu, 35 + 31*i, 31) / 1073741824.0 - 1;
|
||||
norm += ratios[i] * ratios[i];
|
||||
}
|
||||
out[index] = 1 / sqrt(norm);
|
||||
for (unsigned i = 0; i < 3; ++i) out[(index+i+1)&3] = ratios[i] * out[index];
|
||||
}
|
||||
static void publish(bool gyro_fresh = true) {
|
||||
source.received_us = time_us_32();
|
||||
++source.state_generation;
|
||||
if (time_us_32() - last_sensor_us >= 10000) {
|
||||
last_sensor_us = time_us_32();
|
||||
++sensor_sequence;
|
||||
source.accel_sequence = sensor_sequence;
|
||||
source.accel_received_us = time_us_32();
|
||||
if (gyro_fresh) {
|
||||
source.gyro_sequence = sensor_sequence;
|
||||
source.gyro_received_us = time_us_32();
|
||||
}
|
||||
}
|
||||
wii_ir_pointer_observe(source.slot, source.controller.connection_generation,
|
||||
++ir_sequence, raw_ir_buttons, ir_x, ir_y, ir_mask, false, 0.0f, true);
|
||||
}
|
||||
static uint32_t poll(bool commit = true, bool gyro_fresh = true) {
|
||||
now_us += 4000;
|
||||
publish(gyro_fresh);
|
||||
probe_controller_input_poll(to_ms_since_boot(now_us), &controls);
|
||||
const uint32_t token = probe_controller_input_peek_native_report(to_ms_since_boot(now_us), packet);
|
||||
if (commit && token) assert(probe_controller_input_commit_native_report(token));
|
||||
return token;
|
||||
}
|
||||
|
||||
int main() {
|
||||
probe_controller_input_init();
|
||||
assert(probe_controller_input_start());
|
||||
uint8_t calibration[9];
|
||||
put_pair(calibration, 2000, 2100);
|
||||
put_pair(calibration+3, 1500, 1400);
|
||||
put_pair(calibration+6, 1600, 1700);
|
||||
probe_controller_input_set_stick_calibration(calibration);
|
||||
probe_controller_input_set_native_features(0x37);
|
||||
probe_controller_input_set_native_stream(true);
|
||||
source.slot = 0;
|
||||
source.controller.active = true;
|
||||
source.controller.connection_generation = 7;
|
||||
source.layout = Bluepad32ControllerLayout::kWiiNunchuk;
|
||||
source.controller.state.button_south = true; // Profile maps this to native A.
|
||||
source.accel_valid = source.gyro_valid = true;
|
||||
source.accel_q13[1] = 8192; // SDL up -> virtual native rail-down +X.
|
||||
memcpy(source.gyro_q10, bias_q10, sizeof(bias_q10));
|
||||
for (unsigned i = 0; i < 450; ++i) assert(poll());
|
||||
assert(controls.active && packet[15] == 30 && packet[19] == 0x0c);
|
||||
assert(signed32(packet+32) == (1 << 28));
|
||||
assert(signed32(packet+36) == 0 && signed32(packet+40) == 0);
|
||||
assert(packet[2] == 2 && packet[3] == 0 && packet[13] == 20);
|
||||
assert(stick_x(packet) == 2000 && stick_y(packet) == 2100);
|
||||
double initial[4]; decode_quaternion(packet, initial);
|
||||
|
||||
raw_ir_buttons = 0x000c; // Physical A+B in pointer telemetry are not click overlays.
|
||||
for (unsigned i = 0; i < 8; ++i) assert(poll());
|
||||
assert(packet[2] == 2 && (packet[2] & 0x30) == 0);
|
||||
source.controller.state.left_stick_x = INT16_MAX;
|
||||
source.controller.state.left_stick_y = INT16_MIN;
|
||||
assert(poll());
|
||||
assert(stick_x(packet) == 3500 && stick_y(packet) == 3500);
|
||||
source.controller.state.left_stick_x = source.controller.state.left_stick_y = 0;
|
||||
|
||||
// Native IR displacement must survive failed submission, then be consumed once.
|
||||
raw_ir_buttons = 0x0002; // A mapped game button must not clutch native IR movement.
|
||||
bool horizontal_motion = false;
|
||||
int32_t horizontal_total = 0, horizontal_cross_axis = 0;
|
||||
for (unsigned i = 0; i < 24; ++i) {
|
||||
ir_x[0] += 2; ir_x[1] += 2;
|
||||
assert(poll());
|
||||
horizontal_motion = horizontal_motion || packet[9] != 0 || packet[10] != 0;
|
||||
const uint16_t x = packet[9] | (uint16_t(packet[10]) << 8);
|
||||
const uint16_t y = packet[11] | (uint16_t(packet[12]) << 8);
|
||||
horizontal_total += x <= INT16_MAX ? int32_t(x) : int32_t(x) - 65536;
|
||||
horizontal_cross_axis += y <= INT16_MAX ? int32_t(y) : int32_t(y) - 65536;
|
||||
}
|
||||
assert(horizontal_motion);
|
||||
assert(horizontal_total < 0);
|
||||
assert(horizontal_cross_axis * 4 > horizontal_total && horizontal_cross_axis * 4 < -horizontal_total);
|
||||
ir_x[0] += 8; ir_x[1] += 8;
|
||||
const uint32_t ticket = poll(false);
|
||||
uint8_t retry[63];
|
||||
assert(ticket && probe_controller_input_peek_native_report(to_ms_since_boot(now_us), retry) == ticket);
|
||||
assert(memcmp(packet, retry, sizeof(packet)) == 0);
|
||||
assert(probe_controller_input_commit_native_report(ticket));
|
||||
assert(!probe_controller_input_commit_native_report(ticket));
|
||||
|
||||
// Tilting the Wii up moves camera spots down. Native Joy-Con Y must
|
||||
// reverse the desktop-pointer convention without changing consumption.
|
||||
for (unsigned i = 0; i < 80; ++i) assert(poll());
|
||||
int32_t vertical_totals[2]{};
|
||||
for (unsigned direction = 0; direction < 2; ++direction) {
|
||||
for (unsigned i = 0; i < 104; ++i) {
|
||||
if (i < 24) {
|
||||
ir_y[0] += direction == 0 ? 2 : -2;
|
||||
ir_y[1] += direction == 0 ? 2 : -2;
|
||||
}
|
||||
assert(poll());
|
||||
const uint16_t y = packet[11] | (uint16_t(packet[12]) << 8);
|
||||
vertical_totals[direction] += y <= INT16_MAX ? int32_t(y) : int32_t(y) - 65536;
|
||||
WiiIrMouseReport pending{};
|
||||
(void)wii_ir_mouse_peek(&pending, INT16_MAX);
|
||||
assert(pending.dx == 0 && pending.dy == 0);
|
||||
}
|
||||
}
|
||||
assert(vertical_totals[0] > 0 && vertical_totals[1] < 0);
|
||||
// Upstream radial smoothing has a positional dead zone, so returning the
|
||||
// camera to its origin need not return the filtered pointer exactly there.
|
||||
// Per-report checks above instead defend once-only native consumption.
|
||||
|
||||
// A visible, stationary bar must anchor native heading against residual drift.
|
||||
for (unsigned i = 0; i < 3000; ++i) assert(poll());
|
||||
decode_quaternion(packet, initial);
|
||||
source.gyro_q10[1] = bias_q10[1] + 614; // ~0.6dps warming drift about vertical.
|
||||
for (unsigned i = 0; i < 5000; ++i) assert(poll());
|
||||
double optically_held[4]; decode_quaternion(packet, optically_held);
|
||||
double heading_dot = 0;
|
||||
for (unsigned i = 0; i < 4; ++i) heading_dot += initial[i] * optically_held[i];
|
||||
assert(fabs(heading_dot) > cos(2.0 * acos(-1.0) / 360.0));
|
||||
memcpy(source.gyro_q10, bias_q10, sizeof(bias_q10));
|
||||
|
||||
// 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());
|
||||
decode_quaternion(packet, initial);
|
||||
for (unsigned i = 0; i < 250; ++i) assert(poll());
|
||||
double after_bias[4]; decode_quaternion(packet, after_bias);
|
||||
double dot = 0; for (unsigned i=0;i<4;++i) dot += initial[i]*after_bias[i];
|
||||
assert(fabs(dot) > .99999);
|
||||
// SDL yaw -> native mouse-frame +X; one second at90dps rotates90 degrees.
|
||||
source.gyro_q10[1] = bias_q10[1] + 90 * 1024;
|
||||
for (unsigned i = 0; i < 250; ++i) assert(poll());
|
||||
double turned[4]; decode_quaternion(packet, turned);
|
||||
dot = 0; for (unsigned i=0;i<4;++i) dot += initial[i]*turned[i];
|
||||
assert(fabs(fabs(dot) - sqrt(.5)) < .015);
|
||||
memcpy(source.gyro_q10, bias_q10, sizeof(bias_q10));
|
||||
|
||||
ir_mask = 0;
|
||||
assert(poll());
|
||||
assert(packet[9] == 0 && packet[10] == 0 && packet[11] == 0 && packet[12] == 0 && packet[13] == 0xff);
|
||||
ir_x[0] = 500; ir_x[1] = 700; ir_mask = 3;
|
||||
assert(poll());
|
||||
assert(packet[9] == 0 && packet[10] == 0 && packet[11] == 0 && packet[12] == 0);
|
||||
|
||||
// Fresh controller/accelerometer traffic cannot rejuvenate stale MotionPlus.
|
||||
for (unsigned i = 0; i < 42; ++i) assert(poll(true, false));
|
||||
assert(controls.active && packet[15] == 0 && packet[13] == 0xff);
|
||||
const uint32_t obsolete = poll(false, false);
|
||||
++source.controller.connection_generation;
|
||||
assert(poll(false));
|
||||
assert(!probe_controller_input_commit_native_report(obsolete));
|
||||
probe_controller_input_set_native_stream(false);
|
||||
assert(probe_controller_input_peek_native_report(to_ms_since_boot(now_us), retry) == 0);
|
||||
probe_controller_input_set_native_stream(true);
|
||||
publish_during_snapshot = true;
|
||||
for (unsigned i = 0; i < 450; ++i) assert(poll());
|
||||
assert(packet[15] == 30);
|
||||
source.controller.active = false;
|
||||
now_us += 4000;
|
||||
probe_controller_input_poll(to_ms_since_boot(now_us), &controls);
|
||||
assert(!controls.active);
|
||||
assert(probe_controller_input_peek_native_report(to_ms_since_boot(now_us), retry) == 0);
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -10,14 +10,15 @@ def test_bluepad32_backend_lifecycle_native(tmp_path: Path) -> None:
|
|||
compiler = shutil.which("c++") or shutil.which("g++")
|
||||
assert compiler is not None, "a host C++ compiler is required"
|
||||
|
||||
for bluetooth_mode, native, short_packets in (
|
||||
("mixed", False, False),
|
||||
("mixed", True, False),
|
||||
("mixed", True, True),
|
||||
("ble", False, False),
|
||||
("classic", False, False),
|
||||
for bluetooth_mode, native, short_packets, wii_bridge in (
|
||||
("mixed", False, False, False),
|
||||
("mixed", True, False, False),
|
||||
("mixed", True, True, False),
|
||||
("ble", False, False, False),
|
||||
("classic", False, False, False),
|
||||
("mixed", False, False, True),
|
||||
):
|
||||
suffix = "_native32" if short_packets else "_native64" if native else ""
|
||||
suffix = "_wii_bridge" if wii_bridge else "_native32" if short_packets else "_native64" if native else ""
|
||||
executable = (
|
||||
tmp_path / f"bluepad32_backend_lifecycle_test_{bluetooth_mode}{suffix}"
|
||||
)
|
||||
|
|
@ -33,6 +34,8 @@ def test_bluepad32_backend_lifecycle_native(tmp_path: Path) -> None:
|
|||
f"-DSWITCH_PICO_ENABLE_BLE={int(bluetooth_mode != 'classic')}",
|
||||
f"-DSWITCH_PICO_ENABLE_CLASSIC={int(bluetooth_mode != 'ble')}",
|
||||
]
|
||||
if wii_bridge:
|
||||
command.append("-DSWITCH2_BRIDGE_WII_INPUT=1")
|
||||
if native:
|
||||
command.extend(
|
||||
[
|
||||
|
|
@ -112,6 +115,10 @@ def test_bluepad32_backend_lifecycle_native(tmp_path: Path) -> None:
|
|||
]
|
||||
)
|
||||
subprocess.run(command, check=True, cwd=root)
|
||||
if wii_bridge:
|
||||
for scenario in ("wii-bridge-sensors", "wii-bridge-cues", "wii-bridge-cue-races"):
|
||||
subprocess.run([str(executable), scenario], check=True, cwd=root)
|
||||
continue
|
||||
subprocess.run([str(executable), "transport-policy"], check=True, cwd=root)
|
||||
if bluetooth_mode != "classic":
|
||||
subprocess.run(
|
||||
|
|
|
|||
31
tests/test_switch2_native_imu.py
Normal file
31
tests/test_switch2_native_imu.py
Normal file
|
|
@ -0,0 +1,31 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import shutil
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
def test_switch2_native_imu(tmp_path: Path) -> None:
|
||||
root = Path(__file__).resolve().parents[1]
|
||||
compiler = shutil.which("c++") or shutil.which("g++")
|
||||
assert compiler is not None, "a host C++ compiler is required"
|
||||
|
||||
executable = tmp_path / "switch2_native_imu_test"
|
||||
subprocess.run(
|
||||
[
|
||||
compiler,
|
||||
"-std=c++17",
|
||||
"-Wall",
|
||||
"-Wextra",
|
||||
"-Werror",
|
||||
"-pedantic",
|
||||
f"-I{root / 'tools' / 'switch2_usb_probe'}",
|
||||
str(root / "tests" / "switch2_native_imu_test.cpp"),
|
||||
str(root / "tools" / "switch2_usb_probe" / "native_imu.cpp"),
|
||||
"-o",
|
||||
str(executable),
|
||||
],
|
||||
check=True,
|
||||
cwd=root,
|
||||
)
|
||||
subprocess.run([str(executable)], check=True, cwd=root)
|
||||
74
tests/test_switch2_wii_bridge_native.py
Normal file
74
tests/test_switch2_wii_bridge_native.py
Normal file
|
|
@ -0,0 +1,74 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import shutil
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1] / "tools"))
|
||||
|
||||
from prepare_libogc_ir import prepare
|
||||
|
||||
|
||||
def test_wii_native_bridge_sensor_pointer_and_profile_contract(tmp_path: Path) -> None:
|
||||
root = Path(__file__).resolve().parents[1]
|
||||
compiler = shutil.which("c++") or shutil.which("g++")
|
||||
assert compiler is not None, "a host C++ compiler is required"
|
||||
c_compiler = shutil.which("cc")
|
||||
assert c_compiler is not None, "a host C compiler is required"
|
||||
generated = prepare(tmp_path / "libogc_ir.c")
|
||||
ir_object = tmp_path / "libogc_ir.o"
|
||||
subprocess.run(
|
||||
[
|
||||
c_compiler,
|
||||
"-std=c11",
|
||||
"-Wall",
|
||||
"-Wextra",
|
||||
"-Werror",
|
||||
"-pedantic",
|
||||
f"-I{root / 'external'}",
|
||||
"-c",
|
||||
str(generated),
|
||||
"-o",
|
||||
str(ir_object),
|
||||
],
|
||||
check=True,
|
||||
cwd=root,
|
||||
)
|
||||
executable = tmp_path / "switch2_wii_bridge_test"
|
||||
subprocess.run(
|
||||
[
|
||||
compiler,
|
||||
"-std=c++17",
|
||||
"-Wall",
|
||||
"-Wextra",
|
||||
"-Werror",
|
||||
"-pedantic",
|
||||
"-pthread",
|
||||
"-DSWITCH_PICO_SWITCH2_USB_BRIDGE=1",
|
||||
"-DSWITCH2_BRIDGE_WII_INPUT=1",
|
||||
"-DSWITCH_PICO_BLUEPAD32=1",
|
||||
"-DSWITCH_PICO_ENABLE_BLE=1",
|
||||
"-DSWITCH_PICO_ENABLE_CLASSIC=1",
|
||||
"-DSWITCH_PICO_SWITCH2_MOUSE_CAPTURE=1",
|
||||
"-DSWITCH_PICO_SWITCH2_MOUSE_CAPTURE_NATIVE=1",
|
||||
"-DSWITCH_PICO_WII_IR=1",
|
||||
"-DSWITCH2_BRIDGE_SOURCE_ADDRESS_BYTES=0x02,0,0,0,0,1",
|
||||
f"-I{root / 'tests' / 'switch2_mouse_bridge_native_stubs'}",
|
||||
f"-I{root / 'tests' / 'wii_ir_aiming_native_stubs'}",
|
||||
f"-I{root / 'tools' / 'switch2_usb_probe'}",
|
||||
f"-I{root / 'src' / 'firmware'}",
|
||||
f"-I{root / 'bluepad32_config'}",
|
||||
f"-I{root / 'external'}",
|
||||
str(root / "tests" / "switch2_wii_bridge_test.cpp"),
|
||||
str(root / "tools" / "switch2_usb_probe" / "controller_input.cpp"),
|
||||
str(root / "tools" / "switch2_usb_probe" / "native_imu.cpp"),
|
||||
str(root / "src" / "firmware" / "input" / "wii_ir_pointer.cpp"),
|
||||
str(ir_object),
|
||||
"-o",
|
||||
str(executable),
|
||||
],
|
||||
check=True,
|
||||
cwd=root,
|
||||
)
|
||||
subprocess.run([str(executable)], check=True, cwd=root)
|
||||
75
tests/test_wii_ir_pointer_native.py
Normal file
75
tests/test_wii_ir_pointer_native.py
Normal file
|
|
@ -0,0 +1,75 @@
|
|||
from __future__ import annotations
|
||||
|
||||
import shutil
|
||||
import subprocess
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1] / "tools"))
|
||||
|
||||
from prepare_libogc_ir import prepare
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"native_bridge", [False, True], ids=["legacy-mouse", "native-wii"]
|
||||
)
|
||||
def test_wii_ir_pointer_output_contract(tmp_path: Path, native_bridge: bool) -> None:
|
||||
root = Path(__file__).resolve().parents[1]
|
||||
compiler = shutil.which("c++") or shutil.which("g++")
|
||||
assert compiler is not None, "a host C++ compiler is required"
|
||||
executable = tmp_path / "wii_ir_pointer_test"
|
||||
defines = ["-DSWITCH_PICO_WII_IR=1"]
|
||||
defines.append(
|
||||
"-DSWITCH2_BRIDGE_WII_INPUT=1"
|
||||
if native_bridge
|
||||
else "-DSWITCH_PICO_WII_IR_MOUSE=1"
|
||||
)
|
||||
if native_bridge:
|
||||
c_compiler = shutil.which("cc")
|
||||
assert c_compiler is not None, "a host C compiler is required"
|
||||
generated = prepare(tmp_path / "libogc_ir.c")
|
||||
ir_object = tmp_path / "libogc_ir.o"
|
||||
subprocess.run(
|
||||
[
|
||||
c_compiler,
|
||||
"-std=c11",
|
||||
"-Wall",
|
||||
"-Wextra",
|
||||
"-Werror",
|
||||
"-pedantic",
|
||||
f"-I{root / 'external'}",
|
||||
"-c",
|
||||
str(generated),
|
||||
"-o",
|
||||
str(ir_object),
|
||||
],
|
||||
check=True,
|
||||
cwd=root,
|
||||
)
|
||||
ir_sources = [str(ir_object)]
|
||||
else:
|
||||
ir_sources = [str(root / "src" / "firmware" / "input" / "wii_ir_tracker.cpp")]
|
||||
subprocess.run(
|
||||
[
|
||||
compiler,
|
||||
"-std=c++17",
|
||||
"-Wall",
|
||||
"-Wextra",
|
||||
"-Werror",
|
||||
"-pedantic",
|
||||
*defines,
|
||||
f"-I{root / 'tests' / 'wii_ir_aiming_native_stubs'}",
|
||||
f"-I{root / 'src' / 'firmware'}",
|
||||
f"-I{root / 'external'}",
|
||||
str(root / "tests" / "wii_ir_pointer_test.cpp"),
|
||||
str(root / "src" / "firmware" / "input" / "wii_ir_pointer.cpp"),
|
||||
*ir_sources,
|
||||
"-o",
|
||||
str(executable),
|
||||
],
|
||||
check=True,
|
||||
cwd=root,
|
||||
)
|
||||
subprocess.run([str(executable)], check=True, cwd=root)
|
||||
489
tests/wii_ir_pointer_test.cpp
Normal file
489
tests/wii_ir_pointer_test.cpp
Normal file
|
|
@ -0,0 +1,489 @@
|
|||
#include "input/wii_ir_pointer.h"
|
||||
|
||||
#include <cstdint>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <iostream>
|
||||
|
||||
static uint32_t clock_us;
|
||||
uint32_t time_us_32() { return clock_us; }
|
||||
|
||||
namespace {
|
||||
int failures;
|
||||
|
||||
void expect(bool okay, const char* message) {
|
||||
if (!okay) { std::cerr << message << '\n'; ++failures; }
|
||||
}
|
||||
|
||||
void observe(uint8_t slot, uint32_t connection_generation, uint32_t sequence,
|
||||
int dx, int dy, uint16_t buttons = 0, uint8_t mask = 3,
|
||||
bool nunchuk_c = false) {
|
||||
clock_us += 5000;
|
||||
const uint16_t x[4] = {static_cast<uint16_t>(400 + dx), static_cast<uint16_t>(624 + dx), 0, 0};
|
||||
const uint16_t y[4] = {static_cast<uint16_t>(384 + dy), static_cast<uint16_t>(384 + dy), 0, 0};
|
||||
wii_ir_pointer_observe(slot, connection_generation, sequence, buttons, x, y, mask, nunchuk_c,
|
||||
0.0f, false);
|
||||
}
|
||||
|
||||
struct Rig {
|
||||
uint8_t slot = 2;
|
||||
uint32_t connection_generation = 41;
|
||||
uint32_t sequence = 0;
|
||||
int horizontal = 0;
|
||||
int vertical = 0;
|
||||
|
||||
Rig() {
|
||||
wii_ir_pointer_init();
|
||||
wii_ir_pointer_reset();
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
expect(wii_ir_pointer_configure_screen(660, 370, 0, 0, 1320, 740),
|
||||
"explicit native viewport and count spans must be configurable");
|
||||
#endif
|
||||
wii_ir_mouse_set_output_enabled(true);
|
||||
clock_us = 100000;
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
// Prime the real upstream startup/glitch/smoothing history, then
|
||||
// consume its finite startup tail before comparing relative motion.
|
||||
settle(0, 0);
|
||||
#endif
|
||||
WiiIrMouseReport release{};
|
||||
if (wii_ir_mouse_peek(&release)) wii_ir_mouse_commit(release);
|
||||
input(0, 0);
|
||||
input(0, 0);
|
||||
}
|
||||
|
||||
void input(int dx, int dy, uint16_t buttons = 0, uint8_t mask = 3,
|
||||
bool nunchuk_c = false) {
|
||||
horizontal = dx;
|
||||
vertical = dy;
|
||||
observe(slot, connection_generation, ++sequence, dx, dy, buttons, mask, nunchuk_c);
|
||||
}
|
||||
|
||||
void baseline(int dx, int dy) {
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
for (unsigned i = 0; i < 16; ++i) {
|
||||
input(dx, dy);
|
||||
if (peek().tracking) return;
|
||||
}
|
||||
expect(false, "fresh stationary full-pair reports must establish a native baseline");
|
||||
#else
|
||||
input(dx, dy);
|
||||
input(dx, dy);
|
||||
#endif
|
||||
}
|
||||
|
||||
void settle(int dx, int dy, uint16_t buttons = 0) {
|
||||
for (int i = 0; i < 80; ++i) input(dx, dy, buttons);
|
||||
}
|
||||
|
||||
void move_to(int dx, int dy) {
|
||||
const int from_x = horizontal;
|
||||
const int from_y = vertical;
|
||||
for (int i = 1; i <= 20; ++i) {
|
||||
input(from_x + (dx - from_x) * i / 20, from_y + (dy - from_y) * i / 20);
|
||||
}
|
||||
settle(dx, dy);
|
||||
}
|
||||
|
||||
WiiIrMouseReport peek() {
|
||||
WiiIrMouseReport report{};
|
||||
wii_ir_mouse_peek(&report, 32767);
|
||||
return report;
|
||||
}
|
||||
};
|
||||
|
||||
void native_movement_and_legacy_limits_conserve_output() {
|
||||
Rig rig;
|
||||
rig.move_to(100, -100);
|
||||
const auto native = rig.peek();
|
||||
expect(native.dx < -127 && native.dy > 127,
|
||||
"large two-axis motion must remain signed and exceed the legacy 8-bit range");
|
||||
const auto retry = rig.peek();
|
||||
expect(retry.dx == native.dx && retry.dy == native.dy && retry.generation == native.generation,
|
||||
"peeking an unsubmitted native report must not consume movement");
|
||||
|
||||
WiiIrMouseReport legacy{};
|
||||
expect(wii_ir_mouse_peek(&legacy) && legacy.dx == -127 && legacy.dy == 127,
|
||||
"legacy callers must retain the signed 8-bit HID movement limits");
|
||||
WiiIrMouseReport bounded{};
|
||||
expect(wii_ir_mouse_peek(&bounded, 31) && bounded.dx == -31 && bounded.dy == 31,
|
||||
"both signed axes must obey the caller's movement bound");
|
||||
expect(!wii_ir_mouse_peek(&bounded, 0) && bounded.dx == 0 && bounded.dy == 0 && bounded.tracking,
|
||||
"a zero movement allowance must leave tracking live without consuming displacement");
|
||||
|
||||
wii_ir_mouse_commit(legacy);
|
||||
const auto remainder = rig.peek();
|
||||
expect(remainder.dx == native.dx - legacy.dx && remainder.dy == native.dy - legacy.dy,
|
||||
"committing a bounded legacy report must leave its full native remainder");
|
||||
wii_ir_mouse_commit(remainder);
|
||||
expect(!wii_ir_mouse_peek(&bounded, 32767) && bounded.dx == 0 && bounded.dy == 0 && bounded.tracking,
|
||||
"committed movement must not be sent again and idle tracking must remain visible");
|
||||
}
|
||||
|
||||
void prepared_report_preserves_newer_observations() {
|
||||
Rig rig;
|
||||
rig.move_to(80, -80);
|
||||
const auto prepared = rig.peek();
|
||||
rig.move_to(120, -120);
|
||||
const auto accumulated = rig.peek();
|
||||
expect(accumulated.dx < prepared.dx && accumulated.dy > prepared.dy,
|
||||
"fresh observations must continue accumulating while a USB report waits");
|
||||
wii_ir_mouse_commit(prepared);
|
||||
const auto remaining = rig.peek();
|
||||
expect(remaining.dx == accumulated.dx - prepared.dx && remaining.dy == accumulated.dy - prepared.dy,
|
||||
"a successful cached report must consume only its own displacement, not newer samples");
|
||||
}
|
||||
|
||||
void disabled_output_rebaselines_without_losing_owner() {
|
||||
Rig rig;
|
||||
rig.move_to(100, -100);
|
||||
const auto old = rig.peek();
|
||||
wii_ir_mouse_set_output_enabled(false);
|
||||
WiiIrMouseReport disabled{};
|
||||
expect(!wii_ir_mouse_peek(&disabled, 32767) && disabled.dx == 0 && disabled.dy == 0 && !disabled.tracking,
|
||||
"disabling output must immediately discard pending movement and tracking");
|
||||
expect(disabled.owner == rig.slot && disabled.connection_generation == rig.connection_generation &&
|
||||
disabled.generation != old.generation,
|
||||
"gating must invalidate prepared reports without relinquishing the selected owner");
|
||||
|
||||
rig.settle(-150, 100, 0x000c);
|
||||
expect(!wii_ir_mouse_peek(&disabled, 32767) && disabled.dx == 0 && disabled.dy == 0 && disabled.buttons == 3,
|
||||
"disabled camera observations must retain legacy button diagnostics but never queue output");
|
||||
uint8_t diagnostics[WII_IR_MOUSE_DIAGNOSTIC_SIZE]{};
|
||||
wii_ir_pointer_diagnostics(diagnostics, sizeof(diagnostics));
|
||||
expect(diagnostics[2] == 3 && diagnostics[32] == 0x0c && diagnostics[34] == 3,
|
||||
"disabled output must continue reporting observed spots and physical buttons in diagnostics");
|
||||
|
||||
wii_ir_mouse_set_output_enabled(true);
|
||||
const auto reopened = rig.peek();
|
||||
expect(reopened.dx == 0 && reopened.dy == 0 && !reopened.tracking &&
|
||||
reopened.owner == rig.slot && reopened.generation != disabled.generation,
|
||||
"reopening output must await a fresh camera baseline without replaying disabled movement");
|
||||
rig.baseline(-150, 100);
|
||||
const auto baseline = rig.peek();
|
||||
expect(baseline.tracking && baseline.dx == 0 && baseline.dy == 0,
|
||||
"fresh observations after reopening must establish a motionless baseline");
|
||||
|
||||
rig.move_to(-120, 70);
|
||||
const auto resumed = rig.peek();
|
||||
expect(resumed.dx < 0 && resumed.dx > -127 && resumed.dy > 0 && resumed.dy < 127,
|
||||
"reopened output must contain the small new motion, not the larger disabled repositioning");
|
||||
wii_ir_mouse_commit(old);
|
||||
const auto after_late_commit = rig.peek();
|
||||
expect(after_late_commit.dx == resumed.dx && after_late_commit.dy == resumed.dy,
|
||||
"a late commit from before disabling must not consume newly acquired movement");
|
||||
|
||||
wii_ir_mouse_set_output_enabled(false);
|
||||
wii_ir_pointer_reset();
|
||||
rig.slot = 4;
|
||||
rig.connection_generation = 99;
|
||||
rig.settle(0, 0);
|
||||
expect(!wii_ir_mouse_peek(&disabled, 32767) && !disabled.tracking && disabled.owner == 4 &&
|
||||
disabled.connection_generation == 99 && disabled.dx == 0 && disabled.dy == 0,
|
||||
"reset and source reselection must not silently reopen a disabled output gate");
|
||||
}
|
||||
|
||||
void repeated_enable_preserves_live_tracking() {
|
||||
Rig rig;
|
||||
rig.move_to(50, -50);
|
||||
const auto before = rig.peek();
|
||||
wii_ir_mouse_set_output_enabled(true);
|
||||
const auto unchanged = rig.peek();
|
||||
expect(unchanged.tracking && unchanged.generation == before.generation &&
|
||||
unchanged.dx == before.dx && unchanged.dy == before.dy,
|
||||
"repeated enable must neither reset live tracking nor discard pending displacement");
|
||||
for (int i = 1; i <= 20; ++i) {
|
||||
wii_ir_mouse_set_output_enabled(true);
|
||||
rig.input(50 + i, -50 - i);
|
||||
}
|
||||
const auto moved = rig.peek();
|
||||
expect(moved.tracking && moved.generation == before.generation && moved.dx < before.dx && moved.dy > before.dy,
|
||||
"a continuously asserted readiness gate must allow subsequent camera motion");
|
||||
}
|
||||
|
||||
void duplicate_samples_cannot_refresh_stale_tracking() {
|
||||
Rig rig;
|
||||
rig.move_to(80, -80);
|
||||
const auto prepared = rig.peek();
|
||||
for (int i = 0; i < 29; ++i) {
|
||||
observe(rig.slot, rig.connection_generation, rig.sequence, -100, 100, 0x000c);
|
||||
}
|
||||
const auto fresh = rig.peek();
|
||||
expect(fresh.tracking && fresh.dx == prepared.dx && fresh.dy == prepared.dy && fresh.buttons == 0,
|
||||
"duplicate sequences must not update camera movement, buttons, or the freshness deadline");
|
||||
observe(rig.slot, rig.connection_generation, rig.sequence, -100, 100, 0x000c);
|
||||
WiiIrMouseReport stale{};
|
||||
expect(!wii_ir_mouse_peek(&stale, 32767) && !stale.tracking && stale.dx == 0 && stale.dy == 0,
|
||||
"tracking and queued movement must expire at 150ms despite duplicate observations");
|
||||
expect(stale.owner == rig.slot && stale.connection_generation == rig.connection_generation &&
|
||||
stale.generation != prepared.generation,
|
||||
"stale tracking must invalidate prepared movement without forgetting the connected owner");
|
||||
rig.baseline(-100, 100);
|
||||
const auto reacquired = rig.peek();
|
||||
expect(reacquired.tracking && reacquired.dx == 0 && reacquired.dy == 0,
|
||||
"reacquisition after stale tracking must not include the stale interval");
|
||||
rig.move_to(-70, 70);
|
||||
const auto current = rig.peek();
|
||||
wii_ir_mouse_commit(prepared);
|
||||
const auto after = rig.peek();
|
||||
expect(after.dx == current.dx && after.dy == current.dy,
|
||||
"an expired report cannot consume movement from a fresh tracking generation");
|
||||
}
|
||||
|
||||
void owner_generation_and_tracking_loss_invalidate_reports() {
|
||||
Rig rig;
|
||||
WiiIrMouseReport idle{};
|
||||
expect(!wii_ir_mouse_peek(&idle, 32767) && idle.tracking && idle.owner == rig.slot &&
|
||||
idle.connection_generation == rig.connection_generation,
|
||||
"a stationary source must expose tracking and connection identity even without pending output");
|
||||
rig.move_to(80, -80);
|
||||
const auto old = rig.peek();
|
||||
observe(7, 900, 1, -100, 100, 0x000c);
|
||||
wii_ir_pointer_disconnect(7);
|
||||
const auto owned = rig.peek();
|
||||
expect(owned.owner == rig.slot && owned.connection_generation == rig.connection_generation &&
|
||||
owned.dx == old.dx && owned.dy == old.dy && owned.buttons == 0,
|
||||
"an unrelated source or disconnect must not steal ownership or alter movement");
|
||||
|
||||
++rig.connection_generation;
|
||||
rig.sequence = 0;
|
||||
rig.baseline(-100, 100);
|
||||
const auto reconnected = rig.peek();
|
||||
expect(reconnected.tracking && reconnected.dx == 0 && reconnected.dy == 0 &&
|
||||
reconnected.connection_generation == rig.connection_generation && reconnected.generation != old.generation,
|
||||
"reusing a slot with a new connection generation must discard the old association and backlog");
|
||||
rig.move_to(-70, 70);
|
||||
const auto current = rig.peek();
|
||||
wii_ir_mouse_commit(old);
|
||||
const auto after = rig.peek();
|
||||
expect(after.dx == current.dx && after.dy == current.dy,
|
||||
"a previous connection's report cannot consume current movement");
|
||||
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
// Upstream holds eight missing reports; only the ninth is true loss.
|
||||
for (unsigned i = 0; i < 8; ++i) rig.input(-70, 70, 0, 0);
|
||||
#endif
|
||||
rig.input(-70, 70, 0, 0);
|
||||
expect(!wii_ir_mouse_peek(&idle, 32767) && !idle.tracking && idle.dx == 0 && idle.dy == 0,
|
||||
"true camera tracking loss must drop unsent movement");
|
||||
wii_ir_pointer_disconnect(rig.slot);
|
||||
expect(!wii_ir_mouse_peek(&idle, 32767) && !idle.tracking && idle.owner == 0xff && idle.connection_generation == 0,
|
||||
"disconnect must clear tracking and connection identity even without pending movement");
|
||||
}
|
||||
|
||||
void legacy_buttons_and_clutch_remain_independent_of_movement() {
|
||||
Rig rig;
|
||||
rig.input(0, 0, 0x000c);
|
||||
WiiIrMouseReport buttons{};
|
||||
expect(wii_ir_mouse_peek(&buttons) && buttons.buttons == 3 && buttons.dx == 0 && buttons.dy == 0,
|
||||
"legacy mouse buttons must remain observable independently of camera movement");
|
||||
wii_ir_mouse_commit(buttons);
|
||||
expect(!wii_ir_mouse_peek(&buttons), "committing stationary legacy buttons must clear their pending change");
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
for (int i = 1; i <= 20; ++i) rig.input(i, -i, 0x0003, 3, true);
|
||||
const auto held = rig.peek();
|
||||
expect(held.tracking && held.dx < 0 && held.dy > 0 && held.buttons == 0,
|
||||
"native controller buttons and Nunchuk C must not activate either legacy clutch chord");
|
||||
#else
|
||||
rig.input(20, -20, 0x0002);
|
||||
expect(wii_ir_mouse_peek(&buttons) && buttons.buttons == 0 && !buttons.tracking && buttons.dx == 0 && buttons.dy == 0,
|
||||
"the legacy 1-button clutch must suppress movement while still releasing buttons");
|
||||
wii_ir_mouse_commit(buttons);
|
||||
rig.settle(100, -100, 0x0002);
|
||||
rig.input(100, -100);
|
||||
rig.input(100, -100);
|
||||
expect(!wii_ir_mouse_peek(&buttons) && buttons.tracking && buttons.dx == 0 && buttons.dy == 0,
|
||||
"releasing the legacy clutch must rebaseline rather than replay repositioning");
|
||||
rig.move_to(120, -120);
|
||||
const auto moved = rig.peek();
|
||||
expect(moved.dx < 0 && moved.dy > 0, "movement must resume normally after the legacy clutch is released");
|
||||
#endif
|
||||
}
|
||||
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
void native_cold_baseline_and_unique_camera_reports() {
|
||||
Rig rig;
|
||||
wii_ir_pointer_reset();
|
||||
const uint16_t x[4] = {300, 524, 0, 0};
|
||||
const uint16_t y[4] = {384, 384, 0, 0};
|
||||
clock_us += 5000;
|
||||
wii_ir_pointer_observe(rig.slot, rig.connection_generation, ++rig.sequence,
|
||||
0, x, y, 3, false, 0.0f, true);
|
||||
for (unsigned i = 0; i < 64; ++i) {
|
||||
wii_ir_pointer_observe(rig.slot, rig.connection_generation, rig.sequence,
|
||||
0, x, y, 3, false, 0.0f, true);
|
||||
const auto held = rig.peek();
|
||||
expect(!held.tracking && held.dx == 0 && held.dy == 0,
|
||||
"polls and duplicate sequences must not advance upstream's initial glitch hold");
|
||||
}
|
||||
rig.baseline(-100, 0);
|
||||
const auto first = rig.peek();
|
||||
expect(first.tracking && first.dx == 0 && first.dy == 0 && first.optical_valid,
|
||||
"first accepted upstream position must become a motionless relative baseline");
|
||||
expect(first.optical_sequence == rig.sequence && first.optical_received_us == clock_us &&
|
||||
std::fabs(first.optical_yaw_radians - std::atan(99.0f * 0.39f / 512.0f)) < 0.00001f,
|
||||
"optical heading must use mirrored raw X with upstream calc_yaw in radians");
|
||||
uint8_t diagnostics[WII_IR_MOUSE_DIAGNOSTIC_SIZE]{};
|
||||
expect(wii_ir_pointer_diagnostics(diagnostics, sizeof(diagnostics)) == 48 &&
|
||||
diagnostics[16] == 0x2c && diagnostics[17] == 0x01 &&
|
||||
diagnostics[18] == 0x80 && diagnostics[19] == 0x01 &&
|
||||
(diagnostics[3] & 0x19) == 0x19,
|
||||
"raw48 diagnostics must retain original unmirrored pixels and upstream tracking flags");
|
||||
}
|
||||
|
||||
void native_glitch_hold_excludes_optical_heading() {
|
||||
Rig rig;
|
||||
const auto before = rig.peek();
|
||||
for (unsigned frame = 0; frame < 6; ++frame) {
|
||||
rig.input(250, 0);
|
||||
const auto held = rig.peek();
|
||||
expect(held.tracking && !held.optical_valid && held.dx == 0 && held.dy == 0 &&
|
||||
held.generation == before.generation,
|
||||
"upstream glitch-held raw geometry must not move the pointer or feed optical heading");
|
||||
}
|
||||
rig.input(250, 0);
|
||||
const auto accepted = rig.peek();
|
||||
expect(accepted.tracking && accepted.optical_valid && accepted.dx < 0 &&
|
||||
accepted.generation == before.generation,
|
||||
"upstream accepting a persistent glitch must restore heading confidence without a custom rebase");
|
||||
}
|
||||
|
||||
void native_missing_report_grace_and_true_loss() {
|
||||
Rig rig;
|
||||
rig.input(20, -20); // Leave an upstream smoothing tail when the dots vanish.
|
||||
const auto settled = rig.peek();
|
||||
wii_ir_mouse_commit(settled);
|
||||
for (unsigned missing = 1; missing <= 8; ++missing) {
|
||||
rig.input(20, -20, 0, 0);
|
||||
const auto held = rig.peek();
|
||||
expect(held.tracking && !held.optical_valid && held.dx == 0 && held.dy == 0 &&
|
||||
held.generation == settled.generation,
|
||||
"upstream's eight missing-frame grace must hold position without movement or rebasing");
|
||||
// Core0 polling cannot age the frame-count error policy.
|
||||
for (unsigned poll = 0; poll < 16; ++poll) {
|
||||
const auto polled = rig.peek();
|
||||
expect(polled.tracking && polled.dx == 0 && polled.dy == 0,
|
||||
"USB peeks must neither age missing-frame grace nor synthesize movement");
|
||||
}
|
||||
}
|
||||
rig.input(20, -20, 0, 0);
|
||||
const auto lost = rig.peek();
|
||||
expect(!lost.tracking && !lost.optical_valid && lost.dx == 0 && lost.dy == 0 &&
|
||||
lost.generation != settled.generation,
|
||||
"ninth missing camera report must expose upstream's real loss and invalidate pending generations");
|
||||
rig.input(-100, 100);
|
||||
const auto acquired = rig.peek();
|
||||
expect(acquired.tracking && acquired.optical_valid && acquired.dx == 0 && acquired.dy == 0,
|
||||
"upstream loss recovery must rebaseline rather than bridge the absent camera interval");
|
||||
rig.move_to(-70, 70);
|
||||
expect(rig.peek().dx < 0 && rig.peek().dy > 0,
|
||||
"real movement must resume after loss recovery");
|
||||
}
|
||||
|
||||
void native_single_end_does_not_supply_full_optical_heading() {
|
||||
Rig rig;
|
||||
int vertical = 0;
|
||||
for (int i = 1; i <= 80; ++i) {
|
||||
rig.input(0, i, 0, 1);
|
||||
const auto report = rig.peek();
|
||||
expect(report.tracking && !report.optical_valid,
|
||||
"upstream single-end continuation must not claim a full-pair optical heading");
|
||||
vertical += report.dy;
|
||||
wii_ir_mouse_commit(report);
|
||||
}
|
||||
expect(vertical < -140,
|
||||
"continuous single-end observations must retain upstream's relative movement");
|
||||
rig.input(0, 80);
|
||||
expect(rig.peek().optical_valid,
|
||||
"a full pair returning must restore the native optical observer metadata");
|
||||
}
|
||||
|
||||
void native_roll_convention_and_invalid_gravity_retention() {
|
||||
Rig rig;
|
||||
wii_ir_pointer_reset();
|
||||
// A raw +90-degree bar becomes horizontal only after mirrored camera X
|
||||
// and upstream's +90-degree rotation. Raw X travel becomes corrected Y.
|
||||
int horizontal = 0, vertical = 0;
|
||||
for (unsigned i = 0; i < 100; ++i) {
|
||||
const uint16_t x[4] = {static_cast<uint16_t>(512 + (i > 30 ? 30 : 0)),
|
||||
static_cast<uint16_t>(512 + (i > 30 ? 30 : 0)), 0, 0};
|
||||
const uint16_t y[4] = {272, 496, 0, 0};
|
||||
clock_us += 5000;
|
||||
wii_ir_pointer_observe(rig.slot, rig.connection_generation, ++rig.sequence,
|
||||
0, x, y, 3, false,
|
||||
i == 0 ? 1.57079632679f : std::numeric_limits<float>::quiet_NaN(),
|
||||
(i % 2) == 0);
|
||||
const auto report = rig.peek();
|
||||
if (i > 30) {
|
||||
expect(report.tracking && report.optical_valid,
|
||||
"invalid new gravity must retain the last valid roll for upstream full-pair tracking");
|
||||
horizontal += report.dx;
|
||||
vertical += report.dy;
|
||||
}
|
||||
wii_ir_mouse_commit(report);
|
||||
}
|
||||
expect(vertical > 0 && vertical > 2 * std::abs(horizontal),
|
||||
"raw +90-degree roll must map travel predominantly to shared positive Y despite smoothing history");
|
||||
}
|
||||
void native_relative_motion_is_not_clipped_to_viewport() {
|
||||
Rig rig;
|
||||
wii_ir_pointer_reset();
|
||||
for (int i = 0; i < 80; ++i) {
|
||||
rig.input(0, 220);
|
||||
wii_ir_mouse_commit(rig.peek());
|
||||
}
|
||||
int vertical = 0;
|
||||
bool tracked = true;
|
||||
for (int i = 1; i <= 110; ++i) {
|
||||
rig.input(0, 220 + (i < 30 ? i : 30));
|
||||
const auto report = rig.peek();
|
||||
tracked = tracked && report.tracking;
|
||||
vertical += report.dy;
|
||||
wii_ir_mouse_commit(report);
|
||||
}
|
||||
expect(tracked && vertical < -40 && vertical > -80,
|
||||
"visible vertical motion outside the viewport must still reach a relative mouse");
|
||||
|
||||
expect(wii_ir_pointer_configure_screen(1, 1, 0, 0, 32767, 32767),
|
||||
"the narrowest supported viewport and largest count spans must be accepted");
|
||||
for (int i = 0; i < 80; ++i) {
|
||||
rig.input(300, 0);
|
||||
wii_ir_mouse_commit(rig.peek());
|
||||
}
|
||||
int horizontal = 0;
|
||||
bool bounded = true;
|
||||
for (int i = 0; i < 24; ++i) {
|
||||
rig.input(290, 0);
|
||||
const auto report = rig.peek();
|
||||
bounded = bounded && report.dx >= 0 && report.dx <= 4096 &&
|
||||
report.dy >= -4096 && report.dy <= 4096;
|
||||
horizontal += report.dx;
|
||||
wii_ir_mouse_commit(report);
|
||||
}
|
||||
expect(bounded && horizontal > 0,
|
||||
"wide off-viewport coordinates must not overflow or reverse bounded relative movement");
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
} // namespace
|
||||
|
||||
int main() {
|
||||
native_movement_and_legacy_limits_conserve_output();
|
||||
prepared_report_preserves_newer_observations();
|
||||
disabled_output_rebaselines_without_losing_owner();
|
||||
repeated_enable_preserves_live_tracking();
|
||||
duplicate_samples_cannot_refresh_stale_tracking();
|
||||
owner_generation_and_tracking_loss_invalidate_reports();
|
||||
legacy_buttons_and_clutch_remain_independent_of_movement();
|
||||
#if SWITCH2_BRIDGE_WII_INPUT
|
||||
native_cold_baseline_and_unique_camera_reports();
|
||||
native_glitch_hold_excludes_optical_heading();
|
||||
native_missing_report_grace_and_true_loss();
|
||||
native_single_end_does_not_supply_full_optical_heading();
|
||||
native_roll_convention_and_invalid_gravity_retention();
|
||||
native_relative_motion_is_not_clipped_to_viewport();
|
||||
#endif
|
||||
if (failures) return 1;
|
||||
std::cout << "IR pointer output contract passed\n";
|
||||
}
|
||||
|
|
@ -66,6 +66,13 @@ bool uni_hid_device_set_ready_complete(uni_hid_device_t* d) {
|
|||
void uni_log(const char* fmt, ...) { (void)fmt; }
|
||||
void printf_hexdump(const void* data, int len) { (void)data; (void)len; }
|
||||
|
||||
// This parser fixture does not schedule rumble. Teardown still asks BTstack
|
||||
// to remove both timers; neither is present in the fixture's empty timer list.
|
||||
bool btstack_run_loop_remove_timer(btstack_timer_source_t* timer) {
|
||||
assert(timer != NULL);
|
||||
return false;
|
||||
}
|
||||
|
||||
static void put_be16(uint8_t* output, uint16_t value) {
|
||||
output[0] = value >> 8;
|
||||
output[1] = value;
|
||||
|
|
@ -509,6 +516,88 @@ static void accelerometer_snapshot_requires_fresh_calibrated_reports(void) {
|
|||
assert(!uni_hid_parser_wii_accel_snapshot(&f.device, acceleration, &sequence));
|
||||
}
|
||||
|
||||
static void gyro_snapshot_advances_only_on_calibrated_motionplus_packets(void) {
|
||||
reset_fixture(0x0330, true, EXT_NUNCHUK);
|
||||
connect_device();
|
||||
int32_t gyro[3];
|
||||
uint32_t sequence;
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
send_nunchuk_controls(true);
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
send_motion(7760, 8200, 7560, false, true, false);
|
||||
assert(uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
expect_vector(gyro, 360 * 1024, -120 * 1024, 40 * 1024);
|
||||
const uint32_t first = sequence;
|
||||
send_nunchuk_controls(true);
|
||||
send_ack(0x16, 0);
|
||||
send_status(true);
|
||||
finish_setup();
|
||||
uint8_t short_report[11] = {0x35};
|
||||
feed(short_report, sizeof(short_report));
|
||||
assert(uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
assert(sequence == first);
|
||||
expect_vector(gyro, 360 * 1024, -120 * 1024, 40 * 1024);
|
||||
send_motion(7760, 8200, 7560, false, true, false);
|
||||
assert(uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
assert(sequence != first); // Equal values do not mean a duplicate packet.
|
||||
|
||||
reset_fixture(0x0330, true, EXT_NONE);
|
||||
f.mp_calibration[30] ^= 1;
|
||||
connect_device();
|
||||
send_motion(7760, 8200, 7560, false, true, false);
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
assert(uni_hid_parser_wii_accel_snapshot(&f.device, gyro, &sequence));
|
||||
}
|
||||
|
||||
static void gyro_snapshot_invalidates_on_topology_and_teardown(void) {
|
||||
reset_fixture(0x0306, true, EXT_NONE);
|
||||
connect_device();
|
||||
send_motion(7760, 8200, 7560, false, true, false);
|
||||
int32_t gyro[3];
|
||||
uint32_t sequence;
|
||||
assert(uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
const uint32_t first = sequence;
|
||||
assert(uni_hid_parser_wii_rumble_ready(&f.device));
|
||||
f.extension = EXT_NUNCHUK;
|
||||
send_motion(7760, 8200, 7560, false, true, false);
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
assert(!uni_hid_parser_wii_rumble_ready(&f.device));
|
||||
finish_setup();
|
||||
assert(uni_hid_parser_wii_rumble_ready(&f.device));
|
||||
send_nunchuk_controls(true);
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
send_motion(7760, 8200, 7560, false, true, false);
|
||||
assert(uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
assert(sequence > first); // Topology setup cannot reuse a pre-hotplug ID.
|
||||
|
||||
f.extension = EXT_NONE;
|
||||
f.motionplus = false;
|
||||
f.mp_active = false;
|
||||
send_status(false);
|
||||
finish_setup();
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
send_core_and_accel();
|
||||
assert(uni_hid_parser_wii_accel_snapshot(&f.device, gyro, &sequence));
|
||||
f.motionplus = true;
|
||||
send_status(true);
|
||||
finish_setup();
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
send_motion(7760, 8200, 7560, false, true, false);
|
||||
// The first gyro packet resolves the active MP's downstream topology,
|
||||
// which the attachment status bit alone cannot distinguish.
|
||||
finish_setup();
|
||||
send_motion(7760, 8200, 7560, false, true, false);
|
||||
assert(uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
assert(sequence > first);
|
||||
uni_hid_parser_wii_teardown(&f.device);
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
assert(!uni_hid_parser_wii_rumble_ready(&f.device));
|
||||
f.ready_count = 0; // A new parser connection gets its own ready notification.
|
||||
uni_hid_parser_wii_setup(&f.device);
|
||||
finish_setup();
|
||||
assert(!uni_hid_parser_wii_gyro_snapshot(&f.device, gyro, &sequence));
|
||||
}
|
||||
|
||||
static void setup_read_and_write_errors_leave_buttons_ready(void) {
|
||||
reset_fixture(0x0330, true, EXT_NONE);
|
||||
f.fail_read_address = 0xa60030;
|
||||
|
|
@ -1112,6 +1201,8 @@ static void run_case(const char* name, void (*test)(void)) {
|
|||
|
||||
int main(void) {
|
||||
run_case("fresh calibrated accelerometer snapshots without MotionPlus", accelerometer_snapshot_requires_fresh_calibrated_reports);
|
||||
run_case("independent fresh calibrated MotionPlus gyro snapshots", gyro_snapshot_advances_only_on_calibrated_motionplus_packets);
|
||||
run_case("gyro validity through hotplug, replacement and teardown", gyro_snapshot_invalidates_on_topology_and_teardown);
|
||||
run_case("plain Nunchuk independent packed acceleration", plain_nunchuk_accel_uses_own_packed_factory_points);
|
||||
run_case("MotionPlus Nunchuk acceleration bitpacking and freshness", passthrough_nunchuk_accel_bitpacking_and_freshness);
|
||||
run_case("Nunchuk accelerometer calibration independent of stick and Remote", nunchuk_accel_calibration_is_independent_of_stick_and_remote);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue