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

Preserve and reconnect 8BitDo controllers

Browse source
This commit is contained in:
Joey Yakimowich-Payne 2026-09-01 08:58:16 -06:00
commit fd23842327
7 changed files with 52 additions and 287 deletions
Download patch file Download diff file Expand all files Collapse all files

307
patches/bluepad32-sdl3-imu.patch
View file

@ -1,287 +1,26 @@
diff --git a/src/components/bluepad32/include/parser/uni_hid_parser_imu.h b/src/components/bluepad32/include/parser/uni_hid_parser_imu.h
new file mode 100644
index 0000000..dbd3024
--- /dev/null
+++ b/src/components/bluepad32/include/parser/uni_hid_parser_imu.h
@@ -0,0 +1,278 @@
+// SPDX-License-Identifier: Apache-2.0
+// Fixed-point IMU normalization helpers shared by controller parsers.
+
+#ifndef UNI_HID_PARSER_IMU_H
+#define UNI_HID_PARSER_IMU_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#define UNI_IMU_ACCEL_RES_PER_G 8192
+#define UNI_IMU_GYRO_RES_PER_DEG_S 1024
+
+typedef struct {
+ int32_t accel[3];
+ int32_t gyro[3];
+} uni_imu_fixed_sample_t;
+
+static inline void uni_imu_normalize_wii_accel(int16_t x,
+ int16_t y,
+ int16_t z,
+ int32_t out[3]) {
+ // SDL's Wii convention is (-X, Z, Y), with about 100 raw counts per g.
+ out[0] = -(int32_t)x * UNI_IMU_ACCEL_RES_PER_G / 100;
+ out[1] = (int32_t)z * UNI_IMU_ACCEL_RES_PER_G / 100;
+ out[2] = (int32_t)y * UNI_IMU_ACCEL_RES_PER_G / 100;
+}
+
+typedef enum {
+ UNI_PSMOVE_IMU_MODEL_ZCM1,
+ UNI_PSMOVE_IMU_MODEL_ZCM2,
+} uni_psmove_imu_model_t;
+
+typedef enum {
+ UNI_PSMOVE_CALIBRATION_IGNORED,
+ UNI_PSMOVE_CALIBRATION_INCOMPLETE,
+ UNI_PSMOVE_CALIBRATION_COMPLETE,
+ UNI_PSMOVE_CALIBRATION_INVALID,
+} uni_psmove_calibration_result_t;
+
+#define UNI_PSMOVE_CALIBRATION_REPORT_SIZE 49
+#define UNI_PSMOVE_ZCM1_CALIBRATION_SIZE 143
+#define UNI_PSMOVE_ZCM2_CALIBRATION_SIZE 96
+
+typedef struct {
+ uint8_t blob[UNI_PSMOVE_ZCM1_CALIBRATION_SIZE];
+ uint8_t received_blocks;
+ bool valid;
+} uni_psmove_imu_calibration_t;
+
+static inline int32_t uni_psmove_decode_calibration_value(
+ const uni_psmove_imu_calibration_t* calibration,
+ uni_psmove_imu_model_t model,
+ uint8_t offset) {
+ const uint16_t value =
+ (uint16_t)calibration->blob[offset] |
+ ((uint16_t)calibration->blob[offset + 1] << 8u);
+ return model == UNI_PSMOVE_IMU_MODEL_ZCM1
+ ? (int32_t)value - 0x8000
+ : (int32_t)(int16_t)value;
+}
+
+static inline int32_t uni_psmove_decode_input_value(
+ uni_psmove_imu_model_t model,
+ uint16_t value) {
+ return model == UNI_PSMOVE_IMU_MODEL_ZCM1
+ ? (int32_t)value - 0x8000
+ : (int32_t)(int16_t)value;
+}
+
+static inline void uni_psmove_get_accel_bounds(
+ const uni_psmove_imu_calibration_t* calibration,
+ uni_psmove_imu_model_t model,
+ uint8_t axis,
+ int32_t* low,
+ int32_t* high) {
+ static const uint8_t zcm1_low_offsets[3] = {0x0a, 0x24, 0x14};
+ static const uint8_t zcm1_high_offsets[3] = {0x16, 0x1e, 0x08};
+ static const uint8_t zcm2_low_offsets[3] = {0x08, 0x16, 0x24};
+ static const uint8_t zcm2_high_offsets[3] = {0x02, 0x10, 0x1e};
+ const uint8_t* low_offsets =
+ model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? zcm1_low_offsets
+ : zcm2_low_offsets;
+ const uint8_t* high_offsets =
+ model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? zcm1_high_offsets
+ : zcm2_high_offsets;
+ *low = uni_psmove_decode_calibration_value(calibration, model,
+ low_offsets[axis]);
+ *high = uni_psmove_decode_calibration_value(calibration, model,
+ high_offsets[axis]);
+}
+
+static inline void uni_psmove_get_gyro_calibration(
+ const uni_psmove_imu_calibration_t* calibration,
+ uni_psmove_imu_model_t model,
+ uint8_t axis,
+ int32_t* offset,
+ int32_t* divisor,
+ int32_t* full_scale) {
+ static const uint8_t zcm1_bias_offsets[3] = {0x2a, 0x2c, 0x2e};
+ static const uint8_t zcm1_high_offsets[3] = {0x46, 0x50, 0x5a};
+ static const uint8_t zcm2_bias_offsets[3] = {0x26, 0x28, 0x2a};
+ static const uint8_t zcm2_low_offsets[3] = {0x42, 0x4a, 0x52};
+ static const uint8_t zcm2_high_offsets[3] = {0x30, 0x38, 0x40};
+
+ if (model == UNI_PSMOVE_IMU_MODEL_ZCM1) {
+ *offset = uni_psmove_decode_calibration_value(
+ calibration, model, zcm1_bias_offsets[axis]);
+ const int32_t high = uni_psmove_decode_calibration_value(
+ calibration, model, zcm1_high_offsets[axis]);
+ *divisor = high - *offset;
+ // ZCM1 gyro points are measured at +80 RPM = +480 degrees/s.
+ *full_scale = 480 * UNI_IMU_GYRO_RES_PER_DEG_S;
+ return;
+ }
+
+ *offset = uni_psmove_decode_calibration_value(
+ calibration, model, zcm2_bias_offsets[axis]);
+ const int32_t low = uni_psmove_decode_calibration_value(
+ calibration, model, zcm2_low_offsets[axis]);
+ const int32_t high = uni_psmove_decode_calibration_value(
+ calibration, model, zcm2_high_offsets[axis]);
+ *divisor = high - low;
+ // ZCM2 points span -90 to +90 RPM = 1080 degrees/s total.
+ *full_scale = 1080 * UNI_IMU_GYRO_RES_PER_DEG_S;
+}
+
+static inline bool uni_psmove_validate_calibration(
+ const uni_psmove_imu_calibration_t* calibration,
+ uni_psmove_imu_model_t model) {
+ for (uint8_t axis = 0; axis < 3; ++axis) {
+ int32_t low;
+ int32_t high;
+ uni_psmove_get_accel_bounds(calibration, model, axis, &low, &high);
+ if (high <= low) {
+ return false;
+ }
+
+ int32_t offset;
+ int32_t divisor;
+ int32_t full_scale;
+ uni_psmove_get_gyro_calibration(calibration, model, axis, &offset,
+ &divisor, &full_scale);
+ (void)offset;
+ (void)full_scale;
+ if (divisor <= 0) {
+ return false;
+ }
+ }
+ return true;
+}
+
+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 len) {
+ if (calibration == NULL || report == NULL ||
+ len < UNI_PSMOVE_CALIBRATION_REPORT_SIZE || report[0] != 0x10) {
+ return UNI_PSMOVE_CALIBRATION_IGNORED;
+ }
+
+ size_t destination;
+ size_t source;
+ uint8_t block_mask;
+ if (report[1] == 0x00) {
+ destination = 0;
+ source = 0;
+ block_mask = 0x01;
+ } else if (model == UNI_PSMOVE_IMU_MODEL_ZCM1 && report[1] == 0x01) {
+ destination = UNI_PSMOVE_CALIBRATION_REPORT_SIZE;
+ source = 2;
+ block_mask = 0x02;
+ } else if (model == UNI_PSMOVE_IMU_MODEL_ZCM1 && report[1] == 0x82) {
+ destination = 2 * UNI_PSMOVE_CALIBRATION_REPORT_SIZE - 2;
+ source = 2;
+ block_mask = 0x04;
+ } else if (model == UNI_PSMOVE_IMU_MODEL_ZCM2 && report[1] == 0x81) {
+ destination = UNI_PSMOVE_CALIBRATION_REPORT_SIZE;
+ source = 2;
+ block_mask = 0x02;
+ } else {
+ calibration->valid = false;
+ return UNI_PSMOVE_CALIBRATION_INVALID;
+ }
+
+ memcpy(calibration->blob + destination, report + source,
+ UNI_PSMOVE_CALIBRATION_REPORT_SIZE - source);
+ calibration->received_blocks |= block_mask;
+
+ const uint8_t required_blocks =
+ model == UNI_PSMOVE_IMU_MODEL_ZCM1 ? 0x07 : 0x03;
+ if ((calibration->received_blocks & required_blocks) != required_blocks) {
+ return UNI_PSMOVE_CALIBRATION_INCOMPLETE;
+ }
+
+ calibration->valid =
+ uni_psmove_validate_calibration(calibration, model);
+ return calibration->valid ? UNI_PSMOVE_CALIBRATION_COMPLETE
+ : UNI_PSMOVE_CALIBRATION_INVALID;
+}
+
+static inline int32_t uni_psmove_scale_accel(int32_t raw,
+ int32_t low,
+ int32_t high) {
+ const int64_t numerator =
+ (int64_t)(raw - low) * (2 * UNI_IMU_ACCEL_RES_PER_G);
+ return (int32_t)(numerator / (high - low)) - UNI_IMU_ACCEL_RES_PER_G;
+}
+
+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_psmove_scale_gyro(int32_t raw,
+ int32_t offset,
+ int32_t divisor,
+ int32_t full_scale) {
+ const int64_t scaled =
+ ((int64_t)(raw - offset) * full_scale) / divisor;
+ return uni_imu_clamp_i64(scaled);
+}
+
+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* out) {
+ if (out == NULL) {
+ return false;
+ }
+ memset(out, 0, sizeof(*out));
+ if (calibration == NULL || !calibration->valid || accel_first == NULL ||
+ accel_second == NULL || gyro_first == NULL || gyro_second == NULL) {
+ return false;
+ }
+
+ for (uint8_t axis = 0; axis < 3; ++axis) {
+ int32_t accel = uni_psmove_decode_input_value(model,
+ accel_first[axis]);
+ int32_t gyro = uni_psmove_decode_input_value(model,
+ gyro_first[axis]);
+ if (model == UNI_PSMOVE_IMU_MODEL_ZCM1) {
+ accel = (accel + uni_psmove_decode_input_value(
+ model, accel_second[axis])) /
+ 2;
+ gyro = (gyro + uni_psmove_decode_input_value(
+ model, gyro_second[axis])) /
+ 2;
+ }
+
+ int32_t low;
+ int32_t high;
+ uni_psmove_get_accel_bounds(calibration, model, axis, &low, &high);
+ out->accel[axis] = uni_psmove_scale_accel(accel, low, high);
+
+ int32_t offset;
+ int32_t divisor;
+ int32_t full_scale;
+ uni_psmove_get_gyro_calibration(calibration, model, axis, &offset,
+ &divisor, &full_scale);
+ out->gyro[axis] = uni_psmove_scale_gyro(
+ gyro, offset, divisor, full_scale);
+ }
+ return true;
+}
+
+#endif // UNI_HID_PARSER_IMU_H
diff --git a/src/components/bluepad32/bt/uni_bt_bredr.c b/src/components/bluepad32/bt/uni_bt_bredr.c
index 955cc6f..4013cc1 100644
--- a/src/components/bluepad32/bt/uni_bt_bredr.c
+++ b/src/components/bluepad32/bt/uni_bt_bredr.c
@@ -423,13 +423,14 @@ void uni_bt_bredr_on_l2cap_channel_opened(uint16_t channel, const uint8_t* packe
status = l2cap_event_channel_opened_get_status(packet);
if (status) {
logi("L2CAP Connection failed: 0x%02x.\n", status);
- // Practice showed that if the connection fails, just disconnect/remove
- // so that the connection can start again.
+ // Channel-open failures also include transient page timeouts when a
+ // paired controller powers down or is temporarily unreachable. Keep
+ // the persistent key so the controller can reconnect later. Users can
+ // remove genuinely stale keys through the explicit pairing reset.
if (status == L2CAP_CONNECTION_RESPONSE_RESULT_REFUSED_SECURITY) {
logi("Probably GAP-security-related issues. Set GAP security to 2\n");
}
- logi("Removing key for device: %s.\n", bd_addr_to_str(address));
- gap_drop_link_key_for_bd_addr(device->conn.btaddr);
+ logi("Removing failed device instance for: %s; preserving link key.\n", bd_addr_to_str(address));
uni_hid_device_disconnect(device);
uni_hid_device_delete(device);
/* 'device' is destroyed, don't use */
diff --git a/src/components/bluepad32/include/parser/uni_hid_parser_psmove.h b/src/components/bluepad32/include/parser/uni_hid_parser_psmove.h
index 6af4969..0aebb0a 100644
--- a/src/components/bluepad32/include/parser/uni_hid_parser_psmove.h