diff --git a/bluepad32_config/parser/uni_hid_parser_imu.h b/bluepad32_config/parser/uni_hid_parser_imu.h new file mode 100644 index 0000000..4e4cf73 --- /dev/null +++ b/bluepad32_config/parser/uni_hid_parser_imu.h @@ -0,0 +1,242 @@ +#pragma once + +#include +#include +#include +#include +#include + +#define UNI_IMU_ACCEL_RES_PER_G 8192 +#define UNI_IMU_GYRO_RES_PER_DEG_S 1024 +#define UNI_PSMOVE_CALIBRATION_REPORT_SIZE 49 +#define UNI_PSMOVE_ZCM1_CALIBRATION_SIZE 143 +#define UNI_PSMOVE_ZCM2_CALIBRATION_SIZE 96 + +typedef enum { + UNI_PSMOVE_IMU_MODEL_UNKNOWN = 0, + UNI_PSMOVE_IMU_MODEL_ZCM1, + UNI_PSMOVE_IMU_MODEL_ZCM2, +} uni_psmove_imu_model_t; + +typedef enum { + UNI_PSMOVE_CALIBRATION_INVALID = 0, + UNI_PSMOVE_CALIBRATION_INCOMPLETE, + UNI_PSMOVE_CALIBRATION_COMPLETE, +} uni_psmove_calibration_result_t; + +typedef struct { + int32_t accel[3]; + int32_t gyro[3]; +} uni_imu_fixed_sample_t; + +typedef struct { + uint8_t data[UNI_PSMOVE_ZCM1_CALIBRATION_SIZE]; + uni_psmove_imu_model_t model; + uint8_t received_blocks; + bool complete; +} uni_psmove_imu_calibration_t; + +static inline int32_t uni_imu_clamp_i64(int64_t value) { + if (value > INT32_MAX) { + return INT32_MAX; + } + if (value < INT32_MIN) { + return INT32_MIN; + } + return (int32_t)value; +} + +static inline int32_t uni_imu_scale(int32_t value, int32_t span, + int32_t full_scale) { + if (span <= 0) { + return 0; + } + return uni_imu_clamp_i64((int64_t)value * full_scale / span); +} + +static inline int32_t uni_psmove_scale_gyro(int32_t raw, int32_t bias, + int32_t span, + int32_t full_scale) { + if (span <= 0) { + return 0; + } + return uni_imu_clamp_i64( + ((int64_t)raw - bias) * full_scale / span); +} + +static inline int32_t uni_psmove_decode_value( + uni_psmove_imu_model_t model, uint16_t value) { + if (model == UNI_PSMOVE_IMU_MODEL_ZCM1) { + return (int32_t)value - 0x8000; + } + return (int16_t)value; +} + +static inline int32_t uni_psmove_read_calibration_value( + const uint8_t* data, uni_psmove_imu_model_t model, uint8_t offset) { + const uint16_t value = + (uint16_t)(data[offset] | ((uint16_t)data[offset + 1] << 8)); + return uni_psmove_decode_value(model, value); +} + +static inline uni_psmove_calibration_result_t +uni_psmove_add_calibration_report(uni_psmove_imu_calibration_t* calibration, + uni_psmove_imu_model_t model, + const uint8_t* report, uint16_t length) { + if (calibration == NULL || report == NULL || + length != UNI_PSMOVE_CALIBRATION_REPORT_SIZE || report[0] != 0x10 || + (model != UNI_PSMOVE_IMU_MODEL_ZCM1 && + model != UNI_PSMOVE_IMU_MODEL_ZCM2)) { + return UNI_PSMOVE_CALIBRATION_INVALID; + } + if (calibration->model != UNI_PSMOVE_IMU_MODEL_UNKNOWN && + calibration->model != model) { + return UNI_PSMOVE_CALIBRATION_INVALID; + } + calibration->model = model; + + size_t offset; + size_t source_offset; + uint8_t block_mask; + switch (report[1]) { + case 0x00: + offset = 0; + source_offset = 0; + block_mask = 0x01; + break; + case 0x01: + if (model != UNI_PSMOVE_IMU_MODEL_ZCM1) { + return UNI_PSMOVE_CALIBRATION_INVALID; + } + offset = UNI_PSMOVE_CALIBRATION_REPORT_SIZE; + source_offset = 2; + block_mask = 0x02; + break; + case 0x81: + if (model != UNI_PSMOVE_IMU_MODEL_ZCM2) { + return UNI_PSMOVE_CALIBRATION_INVALID; + } + offset = UNI_PSMOVE_CALIBRATION_REPORT_SIZE; + source_offset = 2; + block_mask = 0x02; + break; + case 0x82: + if (model != UNI_PSMOVE_IMU_MODEL_ZCM1) { + return UNI_PSMOVE_CALIBRATION_INVALID; + } + offset = 2 * UNI_PSMOVE_CALIBRATION_REPORT_SIZE - 2; + source_offset = 2; + block_mask = 0x04; + break; + default: + return UNI_PSMOVE_CALIBRATION_INVALID; + } + + const size_t copy_size = length - source_offset; + const size_t calibration_size = + model == UNI_PSMOVE_IMU_MODEL_ZCM1 + ? UNI_PSMOVE_ZCM1_CALIBRATION_SIZE + : UNI_PSMOVE_ZCM2_CALIBRATION_SIZE; + if (offset + copy_size > calibration_size) { + return UNI_PSMOVE_CALIBRATION_INVALID; + } + memcpy(&calibration->data[offset], &report[source_offset], copy_size); + calibration->received_blocks |= block_mask; + + const uint8_t required_blocks = + model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? 0x07 : 0x03; + calibration->complete = + (calibration->received_blocks & required_blocks) == required_blocks; + return calibration->complete ? UNI_PSMOVE_CALIBRATION_COMPLETE + : UNI_PSMOVE_CALIBRATION_INCOMPLETE; +} + +static inline bool uni_psmove_normalize_imu( + uni_psmove_imu_model_t model, + const uni_psmove_imu_calibration_t* calibration, + const uint16_t accel_first[3], const uint16_t accel_second[3], + const uint16_t gyro_first[3], const uint16_t gyro_second[3], + uni_imu_fixed_sample_t* output) { + if (output == NULL) { + return false; + } + memset(output, 0, sizeof(*output)); + if (calibration == NULL || !calibration->complete || + calibration->model != model || accel_first == NULL || + accel_second == NULL || gyro_first == NULL || gyro_second == NULL) { + return false; + } + + static const uint8_t zcm1_accel_low[] = {0x0a, 0x24, 0x14}; + static const uint8_t zcm1_accel_high[] = {0x16, 0x1e, 0x08}; + static const uint8_t zcm2_accel_low[] = {0x08, 0x16, 0x24}; + static const uint8_t zcm2_accel_high[] = {0x02, 0x10, 0x1e}; + static const uint8_t zcm1_gyro_bias[] = {0x2a, 0x2c, 0x2e}; + static const uint8_t zcm1_gyro_high[] = {0x46, 0x50, 0x5a}; + static const uint8_t zcm2_gyro_bias[] = {0x26, 0x28, 0x2a}; + static const uint8_t zcm2_gyro_low[] = {0x42, 0x4a, 0x52}; + static const uint8_t zcm2_gyro_high[] = {0x30, 0x38, 0x40}; + + const uint8_t* accel_low = + model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? zcm1_accel_low : zcm2_accel_low; + const uint8_t* accel_high = + model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? zcm1_accel_high : zcm2_accel_high; + const uint8_t* gyro_bias = + model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? zcm1_gyro_bias : zcm2_gyro_bias; + const uint8_t* gyro_high = + model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? zcm1_gyro_high : zcm2_gyro_high; + const int32_t gyro_full_scale = + (model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? 480 : 540) * + UNI_IMU_GYRO_RES_PER_DEG_S; + + for (uint8_t axis = 0; axis < 3; ++axis) { + const int32_t accel_low_value = uni_psmove_read_calibration_value( + calibration->data, model, accel_low[axis]); + const int32_t accel_high_value = uni_psmove_read_calibration_value( + calibration->data, model, accel_high[axis]); + const int32_t accel_center = + (accel_low_value + accel_high_value) / 2; + const int32_t accel_raw = + (uni_psmove_decode_value(model, accel_first[axis]) + + uni_psmove_decode_value(model, accel_second[axis])) / + 2; + const int32_t accel_delta = accel_raw - accel_center; + const int32_t accel_span = + accel_delta < 0 ? accel_center - accel_low_value + : accel_high_value - accel_center; + output->accel[axis] = + uni_imu_scale(accel_delta, accel_span, UNI_IMU_ACCEL_RES_PER_G); + + const int32_t gyro_bias_value = uni_psmove_read_calibration_value( + calibration->data, model, gyro_bias[axis]); + const int32_t gyro_raw = + (uni_psmove_decode_value(model, gyro_first[axis]) + + uni_psmove_decode_value(model, gyro_second[axis])) / + 2; + int32_t gyro_span; + if (model == UNI_PSMOVE_IMU_MODEL_ZCM1 || + gyro_raw >= gyro_bias_value) { + gyro_span = uni_psmove_read_calibration_value( + calibration->data, model, gyro_high[axis]) - + gyro_bias_value; + } else { + gyro_span = gyro_bias_value - uni_psmove_read_calibration_value( + calibration->data, model, + zcm2_gyro_low[axis]); + } + output->gyro[axis] = uni_psmove_scale_gyro( + gyro_raw, gyro_bias_value, gyro_span, gyro_full_scale); + } + return true; +} + +static inline void uni_imu_normalize_wii_accel(int32_t x, int32_t y, + int32_t z, + int32_t output[3]) { + if (output == NULL) { + return; + } + output[0] = uni_imu_scale(-x, 100, UNI_IMU_ACCEL_RES_PER_G); + output[1] = uni_imu_scale(z, 100, UNI_IMU_ACCEL_RES_PER_G); + output[2] = uni_imu_scale(y, 100, UNI_IMU_ACCEL_RES_PER_G); +} diff --git a/bluepad32_input_backend.cpp b/bluepad32_input_backend.cpp index 1e84c6a..bc5fda4 100644 --- a/bluepad32_input_backend.cpp +++ b/bluepad32_input_backend.cpp @@ -85,6 +85,7 @@ enum class ConnectionStatus { enum class ConnectionPolicyState { Uninitialized, Open, + Passive, Paused, FailedClosed, }; @@ -624,26 +625,40 @@ void process_clear_pairings(uint32_t now_ms) { void apply_connection_policy() { const bool free_slot = has_free_slot(); - if ((!free_slot && - g_connection_policy_state == ConnectionPolicyState::Paused) || - (free_slot && - g_connection_policy_state == ConnectionPolicyState::Open)) { + const bool active_controller = has_active_controller(); + const bool pairing_open = + pairing_window_active_at(btstack_run_loop_get_time_ms()); + const bool active_scan = + free_slot && (!active_controller || pairing_open); + const ConnectionPolicyState desired_state = + !free_slot + ? ConnectionPolicyState::Paused + : (active_scan ? ConnectionPolicyState::Open + : ConnectionPolicyState::Passive); + if (g_connection_policy_state == desired_state) { return; } - uni_bt_allow_incoming_connections(false); + // Classic inquiry and BLE scanning consume radio time and measurably delay + // active controller HID traffic. Stop them before every policy transition. uni_bt_stop_scanning_unsafe(); if (!free_slot) { + uni_bt_allow_incoming_connections(false); g_connection_policy_state = ConnectionPolicyState::Paused; return; } - // Bluepad32's normal scan/autoconnect path handles both remembered - // controllers powering on and controllers in explicit pairing mode. + // Passive mode still accepts controller-initiated reconnects without + // running inquiry. Active discovery is reserved for zero-controller idle + // state and the explicit BOOTSEL pairing window. uni_bt_allow_incoming_connections(true); - uni_bt_start_scanning_and_autoconnect_unsafe(); - g_connection_policy_state = ConnectionPolicyState::Open; + if (active_scan) { + uni_bt_start_scanning_and_autoconnect_unsafe(); + g_connection_policy_state = ConnectionPolicyState::Open; + } else { + g_connection_policy_state = ConnectionPolicyState::Passive; + } } void update_status_led() { @@ -676,7 +691,9 @@ void process_rumble_timer(btstack_timer_source_t* timer) { const uint32_t now_ms = btstack_run_loop_get_time_ms(); process_clear_pairings(now_ms); process_pairing_snapshot_request(); - update_pairing_window(now_ms); + if (update_pairing_window(now_ms)) { + apply_connection_policy(); + } for (uint8_t slot_index = 0; slot_index < kSlotCount; ++slot_index) { RumbleEnvelope envelope{}; @@ -781,7 +798,8 @@ void platform_on_device_connected(uni_hid_device_t* device) { if (device == nullptr) { return; } - if (g_connection_policy_state != ConnectionPolicyState::Open) { + if (g_connection_policy_state != ConnectionPolicyState::Open && + g_connection_policy_state != ConnectionPolicyState::Passive) { uni_hid_device_disconnect(device); return; } @@ -833,9 +851,9 @@ void platform_on_device_disconnected(uni_hid_device_t* device) { critical_section_exit(&g_state_lock); if (disconnected_tracked_device) { - // A controller can disconnect while the policy is already Open. - // Restart both scans so host-initiated reconnect controllers such as - // 8BitDo Ultimate become reachable without rebooting the Pico. + // Re-evaluate from scratch: resume discovery only after the final + // active controller disconnects; otherwise keep passive incoming + // reconnect support without inquiry-induced latency. g_connection_policy_state = ConnectionPolicyState::Uninitialized; recompute_connection_status(); } diff --git a/firmware/switch-pico-aio.elf b/firmware/switch-pico-aio.elf index f0fa733..c490d09 100755 Binary files a/firmware/switch-pico-aio.elf and b/firmware/switch-pico-aio.elf differ diff --git a/firmware/switch-pico-aio.uf2 b/firmware/switch-pico-aio.uf2 index 6f93f85..1fd9861 100644 Binary files a/firmware/switch-pico-aio.uf2 and b/firmware/switch-pico-aio.uf2 differ diff --git a/tests/bluepad32_backend_lifecycle_test.cpp b/tests/bluepad32_backend_lifecycle_test.cpp index cca33a1..50ad07c 100644 --- a/tests/bluepad32_backend_lifecycle_test.cpp +++ b/tests/bluepad32_backend_lifecycle_test.cpp @@ -800,13 +800,23 @@ void test_pairing_window_policy() { g_connection_policy_state == ConnectionPolicyState::Paused, "Classic and BLE pairing authentication must close at the deadline"); platform_on_device_disconnected(&devices[3]); + require(g_connection_policy_state == ConnectionPolicyState::Passive && + !classic_scanning_enabled && !scanning_enabled && + incoming_connections, + "a freed slot with active controllers must remain passive"); + require(platform_on_device_discovered(address, "controller", 0, 0) == + UNI_ERROR_IGNORE_DEVICE, + "passive policy must reject inquiry discoveries"); + + bluepad32_input_backend_open_pairing_window(); + process_rumble_timer(&g_rumble_timer); require(g_connection_policy_state == ConnectionPolicyState::Open && classic_scanning_enabled && scanning_enabled && incoming_connections, - "a freed slot must resume autoconnect after pairing indication expires"); + "explicit BOOTSEL window must resume active discovery"); require(platform_on_device_discovered(address, "controller", 0, 0) == UNI_ERROR_SUCCESS, - "resumed autoconnect must accept a discovered controller"); + "pairing window discovery must accept a controller"); } diff --git a/tests/test_bluepad32_imu_normalization_native.py b/tests/test_bluepad32_imu_normalization_native.py index 9995079..54a465b 100644 --- a/tests/test_bluepad32_imu_normalization_native.py +++ b/tests/test_bluepad32_imu_normalization_native.py @@ -19,6 +19,7 @@ def test_bluepad32_imu_normalization_native(tmp_path: Path) -> None: "-Wextra", "-Werror", "-pedantic", + f"-I{root / 'bluepad32_config'}", f"-I{root / 'external' / 'bluepad32' / 'src' / 'components' / 'bluepad32' / 'include'}", str(root / "tests" / "bluepad32_imu_normalization_test.cpp"), "-o",