diff --git a/bluepad32_config/parser/uni_hid_parser_imu.h b/bluepad32_config/parser/uni_hid_parser_imu.h deleted file mode 100644 index 4e4cf73..0000000 --- a/bluepad32_config/parser/uni_hid_parser_imu.h +++ /dev/null @@ -1,242 +0,0 @@ -#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 bc5fda4..1e84c6a 100644 --- a/bluepad32_input_backend.cpp +++ b/bluepad32_input_backend.cpp @@ -85,7 +85,6 @@ enum class ConnectionStatus { enum class ConnectionPolicyState { Uninitialized, Open, - Passive, Paused, FailedClosed, }; @@ -625,40 +624,26 @@ void process_clear_pairings(uint32_t now_ms) { void apply_connection_policy() { const bool free_slot = has_free_slot(); - 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) { + if ((!free_slot && + g_connection_policy_state == ConnectionPolicyState::Paused) || + (free_slot && + g_connection_policy_state == ConnectionPolicyState::Open)) { return; } - // Classic inquiry and BLE scanning consume radio time and measurably delay - // active controller HID traffic. Stop them before every policy transition. + uni_bt_allow_incoming_connections(false); uni_bt_stop_scanning_unsafe(); if (!free_slot) { - uni_bt_allow_incoming_connections(false); g_connection_policy_state = ConnectionPolicyState::Paused; return; } - // 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. + // Bluepad32's normal scan/autoconnect path handles both remembered + // controllers powering on and controllers in explicit pairing mode. uni_bt_allow_incoming_connections(true); - if (active_scan) { - uni_bt_start_scanning_and_autoconnect_unsafe(); - g_connection_policy_state = ConnectionPolicyState::Open; - } else { - g_connection_policy_state = ConnectionPolicyState::Passive; - } + uni_bt_start_scanning_and_autoconnect_unsafe(); + g_connection_policy_state = ConnectionPolicyState::Open; } void update_status_led() { @@ -691,9 +676,7 @@ 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(); - if (update_pairing_window(now_ms)) { - apply_connection_policy(); - } + update_pairing_window(now_ms); for (uint8_t slot_index = 0; slot_index < kSlotCount; ++slot_index) { RumbleEnvelope envelope{}; @@ -798,8 +781,7 @@ void platform_on_device_connected(uni_hid_device_t* device) { if (device == nullptr) { return; } - if (g_connection_policy_state != ConnectionPolicyState::Open && - g_connection_policy_state != ConnectionPolicyState::Passive) { + if (g_connection_policy_state != ConnectionPolicyState::Open) { uni_hid_device_disconnect(device); return; } @@ -851,9 +833,9 @@ void platform_on_device_disconnected(uni_hid_device_t* device) { critical_section_exit(&g_state_lock); if (disconnected_tracked_device) { - // Re-evaluate from scratch: resume discovery only after the final - // active controller disconnects; otherwise keep passive incoming - // reconnect support without inquiry-induced latency. + // 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. g_connection_policy_state = ConnectionPolicyState::Uninitialized; recompute_connection_status(); } diff --git a/firmware/switch-pico-aio.elf b/firmware/switch-pico-aio.elf index c490d09..f0fa733 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 1fd9861..6f93f85 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 50ad07c..cca33a1 100644 --- a/tests/bluepad32_backend_lifecycle_test.cpp +++ b/tests/bluepad32_backend_lifecycle_test.cpp @@ -800,23 +800,13 @@ 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, - "explicit BOOTSEL window must resume active discovery"); + "a freed slot must resume autoconnect after pairing indication expires"); require(platform_on_device_discovered(address, "controller", 0, 0) == UNI_ERROR_SUCCESS, - "pairing window discovery must accept a controller"); + "resumed autoconnect must accept a discovered controller"); } diff --git a/tests/test_bluepad32_imu_normalization_native.py b/tests/test_bluepad32_imu_normalization_native.py index 54a465b..9995079 100644 --- a/tests/test_bluepad32_imu_normalization_native.py +++ b/tests/test_bluepad32_imu_normalization_native.py @@ -19,7 +19,6 @@ 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",