jyapayne/switch-pico
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
switch-pico/patches/bluepad32-sdl3-imu.patch

580 lines
28 KiB
Diff
Raw Permalink Blame History

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
+++ b/src/components/bluepad32/include/parser/uni_hid_parser_psmove.h
@@ -14,6 +14,8 @@
void uni_hid_parser_psmove_setup(struct uni_hid_device_s* d);
void uni_hid_parser_psmove_init_report(struct uni_hid_device_s* d);
void uni_hid_parser_psmove_parse_input_report(struct uni_hid_device_s* d, const uint8_t* report, uint16_t len);
+void uni_hid_parser_psmove_parse_feature_report(
+ struct uni_hid_device_s* d, const uint8_t* report, uint16_t len);
void uni_hid_parser_psmove_set_lightbar_color(struct uni_hid_device_s* d, uint8_t r, uint8_t g, uint8_t b);
void uni_hid_parser_psmove_play_dual_rumble(struct uni_hid_device_s* d,
uint16_t start_delay_ms,
diff --git a/src/components/bluepad32/parser/uni_hid_parser_ds4.c b/src/components/bluepad32/parser/uni_hid_parser_ds4.c
index ea063b8..7670caf 100644
--- a/src/components/bluepad32/parser/uni_hid_parser_ds4.c
+++ b/src/components/bluepad32/parser/uni_hid_parser_ds4.c
@@ -297,17 +297,17 @@ void uni_hid_parser_ds4_parse_feature_report(uni_hid_device_t* d, const uint8_t*
// Set gyroscope calibration and normalization parameters.
// Data values will be normalized to 1/DS_GYRO_RES_PER_DEG_S degree/s.
speed_2x = r->gyro_speed_plus + r->gyro_speed_minus;
- ins->gyro_calib_data[0].bias = 0;
+ ins->gyro_calib_data[0].bias = r->gyro_pitch_bias;
ins->gyro_calib_data[0].sens_numer = speed_2x * DS4_GYRO_RES_PER_DEG_S;
ins->gyro_calib_data[0].sens_denom =
abs(r->gyro_pitch_plus - r->gyro_pitch_bias) + abs(r->gyro_pitch_minus + r->gyro_pitch_bias);
- ins->gyro_calib_data[1].bias = 0;
+ ins->gyro_calib_data[1].bias = r->gyro_yaw_bias;
ins->gyro_calib_data[1].sens_numer = speed_2x * DS4_GYRO_RES_PER_DEG_S;
ins->gyro_calib_data[1].sens_denom =
abs(r->gyro_yaw_plus - r->gyro_yaw_bias) + abs(r->gyro_yaw_minus - r->gyro_yaw_bias);
- ins->gyro_calib_data[2].bias = 0;
+ ins->gyro_calib_data[2].bias = r->gyro_roll_bias;
ins->gyro_calib_data[2].sens_numer = speed_2x * DS4_GYRO_RES_PER_DEG_S;
ins->gyro_calib_data[2].sens_denom =
abs(r->gyro_roll_plus - r->gyro_roll_bias) + abs(r->gyro_roll_minus - r->gyro_roll_bias);
@@ -476,7 +476,7 @@ static void ds4_parse_input_report_11(uni_hid_device_t* d, const ds4_input_repor
// Gyro
for (size_t i = 0; i < ARRAY_SIZE(r->gyro); i++) {
- int32_t raw_data = (int16_t)r->gyro[i];
+ int32_t raw_data = (int16_t)r->gyro[i] - ins->gyro_calib_data[i].bias;
int32_t calib_data =
mult_frac(ins->gyro_calib_data[i].sens_numer, raw_data, ins->gyro_calib_data[i].sens_denom);
ctl->gamepad.gyro[i] = calib_data;
@@ -484,7 +484,7 @@ static void ds4_parse_input_report_11(uni_hid_device_t* d, const ds4_input_repor
// Accel
for (size_t i = 0; i < ARRAY_SIZE(r->accel); i++) {
- int32_t raw_data = (int16_t)r->accel[i];
+ int32_t raw_data = (int16_t)r->accel[i] - ins->accel_calib_data[i].bias;
int32_t calib_data =
mult_frac(ins->accel_calib_data[i].sens_numer, raw_data, ins->accel_calib_data[i].sens_denom);
ctl->gamepad.accel[i] = calib_data;
diff --git a/src/components/bluepad32/parser/uni_hid_parser_ds5.c b/src/components/bluepad32/parser/uni_hid_parser_ds5.c
index a22ef26..3d5ecef 100644
--- a/src/components/bluepad32/parser/uni_hid_parser_ds5.c
+++ b/src/components/bluepad32/parser/uni_hid_parser_ds5.c
@@ -487,17 +487,17 @@ void uni_hid_parser_ds5_parse_feature_report(uni_hid_device_t* d, const uint8_t*
// Set gyroscope calibration and normalization parameters.
// Data values will be normalized to 1/DS_GYRO_RES_PER_DEG_S degree/s.
speed_2x = r->gyro_speed_plus + r->gyro_speed_minus;
- ins->gyro_calib_data[0].bias = 0;
+ ins->gyro_calib_data[0].bias = r->gyro_pitch_bias;
ins->gyro_calib_data[0].sens_numer = speed_2x * DS5_GYRO_RES_PER_DEG_S;
ins->gyro_calib_data[0].sens_denom =
abs(r->gyro_pitch_plus - r->gyro_pitch_bias) + abs(r->gyro_pitch_minus + r->gyro_pitch_bias);
- ins->gyro_calib_data[1].bias = 0;
+ ins->gyro_calib_data[1].bias = r->gyro_yaw_bias;
ins->gyro_calib_data[1].sens_numer = speed_2x * DS5_GYRO_RES_PER_DEG_S;
ins->gyro_calib_data[1].sens_denom =
abs(r->gyro_yaw_plus - r->gyro_yaw_bias) + abs(r->gyro_yaw_minus - r->gyro_yaw_bias);
- ins->gyro_calib_data[2].bias = 0;
+ ins->gyro_calib_data[2].bias = r->gyro_roll_bias;
ins->gyro_calib_data[2].sens_numer = speed_2x * DS5_GYRO_RES_PER_DEG_S;
ins->gyro_calib_data[2].sens_denom =
abs(r->gyro_roll_plus - r->gyro_roll_bias) + abs(r->gyro_roll_minus - r->gyro_roll_bias);
@@ -622,7 +622,7 @@ void uni_hid_parser_ds5_parse_input_report(uni_hid_device_t* d, const uint8_t* r
// Gyro
for (size_t i = 0; i < ARRAY_SIZE(r->gyro); i++) {
- int32_t raw_data = (int16_t)r->gyro[i];
+ int32_t raw_data = (int16_t)r->gyro[i] - ins->gyro_calib_data[i].bias;
int32_t calib_data =
mult_frac(ins->gyro_calib_data[i].sens_numer, raw_data, ins->gyro_calib_data[i].sens_denom);
ctl->gamepad.gyro[i] = calib_data;
@@ -630,7 +630,7 @@ void uni_hid_parser_ds5_parse_input_report(uni_hid_device_t* d, const uint8_t* r
// Accel
for (size_t i = 0; i < ARRAY_SIZE(r->accel); i++) {
- int32_t raw_data = (int16_t)r->accel[i];
+ int32_t raw_data = (int16_t)r->accel[i] - ins->accel_calib_data[i].bias;
int32_t calib_data =
mult_frac(ins->accel_calib_data[i].sens_numer, raw_data, ins->accel_calib_data[i].sens_denom);
ctl->gamepad.accel[i] = calib_data;
diff --git a/src/components/bluepad32/parser/uni_hid_parser_psmove.c b/src/components/bluepad32/parser/uni_hid_parser_psmove.c
index 0265f93..5c0f2bb 100644
--- a/src/components/bluepad32/parser/uni_hid_parser_psmove.c
+++ b/src/components/bluepad32/parser/uni_hid_parser_psmove.c
@@ -8,6 +8,7 @@
*/
#include "parser/uni_hid_parser_psmove.h"
+#include "parser/uni_hid_parser_imu.h"
#include <string.h>
@@ -27,11 +28,6 @@ typedef enum psmove_fsm {
PSMOVE_FSM_LED_UPDATED, // LED updated
} psmove_fsm_t;
-typedef enum psmove_model {
- PSMOVE_MODEL_UNK,
- PSMOVE_MODEL_ZCM1,
- PSMOVE_MODEL_ZCM2,
-} psmove_model_t;
typedef enum {
PSMOVE_STATE_RUMBLE_DISABLED,
@@ -41,9 +37,10 @@ typedef enum {
// psmove_instance_t represents data used by the psmove driver instance.
typedef struct psmove_instance_s {
- psmove_model_t model;
+ uni_psmove_imu_model_t model;
psmove_fsm_t state;
uint8_t led_rgb[3];
+ uni_psmove_imu_calibration_t imu_calibration;
btstack_timer_source_t rumble_timer_duration;
btstack_timer_source_t rumble_timer_delayed_start;
@@ -127,6 +124,7 @@ static void psmove_send_output_report(uni_hid_device_t* d, psmove_output_report_
static void on_psmove_set_rumble_on(btstack_timer_source_t* ts);
static void on_psmove_set_rumble_off(btstack_timer_source_t* ts);
static void psmove_play_dual_rumble_now(uni_hid_device_t* d, uint16_t duration_ms, uint8_t magnitude);
+static void psmove_request_calibration_report(uni_hid_device_t* d);
void uni_hid_parser_psmove_init_report(uni_hid_device_t* d) {
uni_controller_t* ctl = &d->controller;
@@ -154,6 +152,7 @@ void uni_hid_parser_psmove_parse_input_report(uni_hid_device_t* d, const uint8_t
}
uni_controller_t* ctl = &d->controller;
+ psmove_instance_t* ins = get_psmove_instance(d);
// Buttons
if (r->buttons[0] & 0x01)
@@ -187,18 +186,39 @@ void uni_hid_parser_psmove_parse_input_report(uni_hid_device_t* d, const uint8_t
ctl->gamepad.throttle = r->trigger * 4;
- ctl->gamepad.accel[0] = r->accel_x;
- ctl->gamepad.accel[1] = r->accel_y;
- ctl->gamepad.accel[2] = r->accel_z;
-
- ctl->gamepad.gyro[0] = r->gyro_x;
- ctl->gamepad.gyro[1] = r->gyro_y;
- ctl->gamepad.gyro[2] = r->gyro_z;
+ const uint16_t accel_first[3] = {r->accel_x, r->accel_y, r->accel_z};
+ const uint16_t accel_second[3] = {
+ r->accel_x2, r->accel_y2, r->accel_z2};
+ const uint16_t gyro_first[3] = {r->gyro_x, r->gyro_y, r->gyro_z};
+ const uint16_t gyro_second[3] = {
+ r->gyro_x2, r->gyro_y2, r->gyro_z2};
+ uni_imu_fixed_sample_t motion;
+ if (uni_psmove_normalize_imu(
+ ins->model, &ins->imu_calibration, accel_first, accel_second,
+ gyro_first, gyro_second, &motion)) {
+ memcpy(ctl->gamepad.accel, motion.accel, sizeof(motion.accel));
+ memcpy(ctl->gamepad.gyro, motion.gyro, sizeof(motion.gyro));
+ }
if (r->battery <= 5)
ctl->battery = r->battery * 51;
}
+void uni_hid_parser_psmove_parse_feature_report(
+ uni_hid_device_t* d, const uint8_t* report, uint16_t len) {
+ psmove_instance_t* ins = get_psmove_instance(d);
+ const uni_psmove_calibration_result_t result =
+ uni_psmove_add_calibration_report(
+ &ins->imu_calibration, ins->model, report, len);
+ if (result == UNI_PSMOVE_CALIBRATION_INCOMPLETE) {
+ psmove_request_calibration_report(d);
+ } else if (result == UNI_PSMOVE_CALIBRATION_COMPLETE) {
+ logi("psmove: IMU calibration ready\n");
+ } else if (result == UNI_PSMOVE_CALIBRATION_INVALID) {
+ loge("psmove: invalid IMU calibration; motion disabled\n");
+ }
+}
+
void uni_hid_parser_psmove_play_dual_rumble(struct uni_hid_device_s* d,
uint16_t start_delay_ms,
uint16_t duration_ms,
@@ -261,25 +281,34 @@ void uni_hid_parser_psmove_setup(struct uni_hid_device_s* d) {
switch (d->product_id) {
case ZCM1_PID:
- ins->model = PSMOVE_MODEL_ZCM1;
+ ins->model = UNI_PSMOVE_IMU_MODEL_ZCM1;
logi("psmove: Detected ZCM1 model\n");
break;
case ZCM2_PID:
- ins->model = PSMOVE_MODEL_ZCM2;
+ ins->model = UNI_PSMOVE_IMU_MODEL_ZCM2;
logi("psmove: Detected ZCM2 model\n");
break;
default:
- loge("psmove: Unknown PSMove PID = %#x, assuming ZCM1\n", ins->model);
- ins->model = PSMOVE_MODEL_ZCM1;
+ loge("psmove: Unknown PSMove PID = %#x, assuming ZCM1\n", d->product_id);
+ ins->model = UNI_PSMOVE_IMU_MODEL_ZCM1;
break;
}
+ psmove_request_calibration_report(d);
uni_hid_device_set_ready_complete(d);
}
//
// Helpers
//
+static void psmove_request_calibration_report(uni_hid_device_t* d) {
+ static const uint8_t report[] = {
+ ((HID_MESSAGE_TYPE_GET_REPORT << 4) | HID_REPORT_TYPE_FEATURE),
+ 0x10,
+ };
+ uni_hid_device_send_ctrl_report(d, report, sizeof(report));
+}
+
static psmove_instance_t* get_psmove_instance(uni_hid_device_t* d) {
return (psmove_instance_t*)&d->parser_data[0];
}
diff --git a/src/components/bluepad32/parser/uni_hid_parser_switch.c b/src/components/bluepad32/parser/uni_hid_parser_switch.c
index 599fc35..0105c53 100644
--- a/src/components/bluepad32/parser/uni_hid_parser_switch.c
+++ b/src/components/bluepad32/parser/uni_hid_parser_switch.c
@@ -51,13 +51,22 @@ static const int16_t DEFAULT_ACCEL_OFFSET = 0;
static const int16_t DEFAULT_ACCEL_SCALE = 16384;
static const int16_t DEFAULT_GYRO_OFFSET = 0;
static const int16_t DEFAULT_GYRO_SCALE = 13371;
-#define SWITCH_IMU_PREC_RANGE_SCALE 1000
+#define SWITCH_IMU_GYRO_RES_PER_DEG_S 1024
+#define SWITCH_IMU_ACCEL_RES_PER_G 8192
#define SWITCH_FACTORY_IMU_CAL_DATA_SIZE 24
static const uint16_t SWITCH_FACTORY_IMU_CAL_DATA_ADDR = 0x6020;
#define SWITCH_DUMP_ROM_DATA_SIZE 24 // Max size is 24
#define SWITCH_SETUP_TIMEOUT_MS 800
+#define SWITCH_RUMBLE_REFRESH_MS 40
+#define SWITCH_RUMBLE_LOW_FREQUENCY_HZ 453
+#define SWITCH_RUMBLE_LEFT_HIGH_FREQUENCY_HZ 135
+#define SWITCH_RUMBLE_RIGHT_HIGH_FREQUENCY_HZ 99
+#define SWITCH_RUMBLE_AMPLITUDE_MAX 1003
+#define SWITCH_RUMBLE_MAGNITUDE_MAX UINT8_MAX
+#define SWITCH_RUMBLE_AMPLITUDE_ROUNDING_BIAS \
+ (SWITCH_RUMBLE_MAGNITUDE_MAX / 2)
#if ENABLE_SPI_FLASH_DUMP
static const uint32_t SWITCH_DUMP_ROM_DATA_ADDR_START = 0x20000;
static const uint32_t SWITCH_DUMP_ROM_DATA_ADDR_END = 0x30000;
@@ -72,6 +81,7 @@ enum switch_state {
STATE_READ_FACTORY_IMU_CALIBRATION, // Factory IMU calibration info
STATE_SET_FULL_REPORT, // Request report 0x30
STATE_ENABLE_IMU, // Enable/Disable gyro/accel
+ STATE_ENABLE_RUMBLE, // Enable controller vibration
STATE_DUMP_FLASH, // Dump SPI Flash memory
STATE_UPDATE_LED, // Update LEDs
STATE_READY, // Gamepad setup ready!
@@ -111,6 +121,7 @@ enum switch_subcmd {
SUBCMD_SPI_FLASH_READ = 0x10,
SUBCMD_SET_PLAYER_LEDS = 0x30,
SUBCMD_ENABLE_IMU = 0x40,
+ SUBCMD_ENABLE_RUMBLE = 0x48,
};
typedef enum {
@@ -137,6 +148,7 @@ typedef struct switch_instance_s {
// Although technically, we can use one timer for delay and duration, easier to debug/maintain if we have two.
btstack_timer_source_t rumble_timer_duration;
btstack_timer_source_t rumble_timer_delayed_start;
+ btstack_timer_source_t rumble_timer_refresh;
switch_state_rumble_t rumble_state;
btstack_timer_source_t setup_timer;
@@ -322,6 +334,7 @@ static void fsm_read_user_stick_calibration(struct uni_hid_device_s* d);
static void fsm_read_factory_imu_calibration(struct uni_hid_device_s* d);
static void fsm_set_full_report(struct uni_hid_device_s* d);
static void fsm_enable_imu(struct uni_hid_device_s* d);
+static void fsm_enable_rumble(struct uni_hid_device_s* d);
static void fsm_update_led(struct uni_hid_device_s* d);
static void fsm_ready(struct uni_hid_device_s* d);
static void process_reply_read_spi_dump(struct uni_hid_device_s* d, const uint8_t* data, int len);
@@ -333,11 +346,16 @@ static void process_reply_set_report_mode(struct uni_hid_device_s* d, const stru
static void process_reply_spi_flash_read(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len);
static void process_reply_set_player_leds(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len);
static void process_reply_enable_imu(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len);
+static void process_reply_enable_rumble(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len);
static int32_t calibrate_axis(int32_t v, switch_cal_stick_t cal);
static void set_led(uni_hid_device_t* d, uint8_t leds);
static void on_switch_set_rumble_on(btstack_timer_source_t* ts);
static void on_switch_set_rumble_off(btstack_timer_source_t* ts);
+static void on_switch_refresh_rumble(btstack_timer_source_t* ts);
static void switch_stop_rumble_now(uni_hid_device_t* d);
+static void switch_send_dual_rumble_now(uni_hid_device_t* d,
+ uint8_t weak_magnitude,
+ uint8_t strong_magnitude);
static void switch_play_dual_rumble_now(uni_hid_device_t* d,
uint16_t duration_ms,
uint8_t weak_magnitude,
@@ -451,6 +469,10 @@ static void process_fsm(struct uni_hid_device_s* d) {
break;
case STATE_ENABLE_IMU:
logd("STATE_ENABLE_IMU\n");
+ fsm_enable_rumble(d);
+ break;
+ case STATE_ENABLE_RUMBLE:
+ logd("STATE_ENABLE_RUMBLE\n");
fsm_dump_rom(d);
break;
case STATE_DUMP_FLASH:
@@ -725,6 +747,12 @@ static void process_reply_enable_imu(struct uni_hid_device_s* d, const struct sw
ARG_UNUSED(r);
ARG_UNUSED(len);
}
+static void process_reply_enable_rumble(struct uni_hid_device_s* d, const struct switch_report_21_s* r, int len) {
+ ARG_UNUSED(d);
+ ARG_UNUSED(r);
+ ARG_UNUSED(len);
+}
+
// Process 0x21 input report: SWITCH_INPUT_SUBCMD_REPLY
static void process_input_subcmd_reply(struct uni_hid_device_s* d, const uint8_t* report, int len) {
@@ -752,6 +780,9 @@ static void process_input_subcmd_reply(struct uni_hid_device_s* d, const uint8_t
case SUBCMD_ENABLE_IMU:
process_reply_enable_imu(d, r, len);
break;
+ case SUBCMD_ENABLE_RUMBLE:
+ process_reply_enable_rumble(d, r, len);
+ break;
default:
loge("Switch: Error, unexpected subcmd_id=0x%02x in report 0x21\n", r->subcmd_id);
break;
@@ -823,19 +854,26 @@ static void parse_imu(uni_hid_device_t* d, const struct switch_imu_data_s* r) {
switch_instance_t* ins = get_switch_instance(d);
uni_controller_t* ctl = &d->controller;
- int accel[3];
- int gyro[3];
+ int32_t accel[3];
+ int32_t gyro[3];
for (int i = 0; i < 3; i++) {
- if (ins->imu_cal_accel_divisor[i] == 0)
- accel[i] = r->accel[i];
- else
- accel[i] = (r->accel[i] * ins->cal_accel.scale[i]) / ins->imu_cal_accel_divisor[i];
- gyro[i] = mult_frac((SWITCH_IMU_PREC_RANGE_SCALE * (r->gyro[i] - ins->cal_gyro.offset[i])),
- ins->cal_gyro.scale[i], ins->imu_cal_gyro_divisor[i]);
+ if (ins->imu_cal_accel_divisor[i] == 0) {
+ accel[i] = r->accel[i] * 2;
+ } else {
+ accel[i] = mult_frac(r->accel[i], 4 * SWITCH_IMU_ACCEL_RES_PER_G, ins->imu_cal_accel_divisor[i]);
+ }
+
+ if (ins->imu_cal_gyro_divisor[i] == 0) {
+ gyro[i] = mult_frac(r->gyro[i], 936 * SWITCH_IMU_GYRO_RES_PER_DEG_S, DEFAULT_GYRO_SCALE);
+ } else {
+ gyro[i] = mult_frac(r->gyro[i] - ins->cal_gyro.offset[i],
+ 936 * SWITCH_IMU_GYRO_RES_PER_DEG_S,
+ ins->imu_cal_gyro_divisor[i]);
+ }
}
- // Right joycon has Y and Z axes negated.
+ // Right Joy-Con has native Y and Z axes negated.
if (ins->controller_type == SWITCH_CONTROLLER_TYPE_JCR) {
accel[1] = -accel[1];
accel[2] = -accel[2];
@@ -843,10 +881,13 @@ static void parse_imu(uni_hid_device_t* d, const struct switch_imu_data_s* r) {
gyro[2] = -gyro[2];
}
- for (int i = 0; i < 3; i++) {
- ctl->gamepad.accel[i] = accel[i];
- ctl->gamepad.gyro[i] = gyro[i];
- }
+ // Match SDL3's PlayStation-oriented sensor coordinate convention.
+ ctl->gamepad.accel[0] = -accel[1];
+ ctl->gamepad.accel[1] = accel[2];
+ ctl->gamepad.accel[2] = -accel[0];
+ ctl->gamepad.gyro[0] = -gyro[1];
+ ctl->gamepad.gyro[1] = gyro[2];
+ ctl->gamepad.gyro[2] = -gyro[0];
}
// Process 0x30 input report: SWITCH_INPUT_IMU_DATA
@@ -1172,6 +1213,18 @@ static void fsm_enable_imu(struct uni_hid_device_s* d) {
req->data[0] = (ins->mode == SWITCH_MODE_IMU);
send_subcmd(d, req, sizeof(out));
}
+static void fsm_enable_rumble(struct uni_hid_device_s* d) {
+ switch_instance_t* ins = get_switch_instance(d);
+ ins->state = STATE_ENABLE_RUMBLE;
+
+ uint8_t out[sizeof(struct switch_subcmd_request) + 1] = {0};
+ struct switch_subcmd_request* req = (struct switch_subcmd_request*)&out[0];
+ req->report_id = OUTPUT_RUMBLE_AND_SUBCMD;
+ req->subcmd_id = SUBCMD_ENABLE_RUMBLE;
+ req->data[0] = 0x01;
+ send_subcmd(d, req, sizeof(out));
+}
+
static void fsm_update_led(struct uni_hid_device_s* d) {
switch_instance_t* ins = get_switch_instance(d);
@@ -1203,6 +1256,13 @@ static struct switch_rumble_freq_data find_rumble_freq(uint16_t freq) {
return rumble_freqs[i];
}
+static uint16_t switch_magnitude_to_amp(uint8_t magnitude) {
+ return (uint16_t)(
+ ((uint32_t)magnitude * SWITCH_RUMBLE_AMPLITUDE_MAX +
+ SWITCH_RUMBLE_AMPLITUDE_ROUNDING_BIAS) /
+ SWITCH_RUMBLE_MAGNITUDE_MAX);
+}
+
static struct switch_rumble_amp_data find_rumble_amp(uint16_t amp) {
unsigned int i = 0;
if (amp > rumble_amps[0].amp) {
@@ -1259,6 +1319,7 @@ void uni_hid_parser_switch_play_dual_rumble(struct uni_hid_device_s* d,
break;
case SWITCH_STATE_RUMBLE_IN_PROGRESS:
btstack_run_loop_remove_timer(&ins->rumble_timer_duration);
+ btstack_run_loop_remove_timer(&ins->rumble_timer_refresh);
break;
default:
// Do nothing
@@ -1366,6 +1427,7 @@ static void switch_stop_rumble_now(uni_hid_device_t* d) {
// No need to protect it with a mutex since it runs in the same main thread
assert(ins->rumble_state == SWITCH_STATE_RUMBLE_IN_PROGRESS);
+ btstack_run_loop_remove_timer(&ins->rumble_timer_refresh);
ins->rumble_state = SWITCH_STATE_RUMBLE_DISABLED;
struct switch_subcmd_request req = {0};
@@ -1379,6 +1441,24 @@ static void switch_stop_rumble_now(uni_hid_device_t* d) {
send_subcmd(d, (struct switch_subcmd_request*)&req, sizeof(req) - 1);
}
+static void switch_send_dual_rumble_now(uni_hid_device_t* d,
+ uint8_t weak_magnitude,
+ uint8_t strong_magnitude) {
+ struct switch_subcmd_request req = {
+ .report_id = OUTPUT_RUMBLE_ONLY,
+ };
+ // Fixed frequencies match the standard Switch LRA envelope and the
+ // 8BitDo Switch-mode implementation. Magnitudes control amplitude only.
+ switch_encode_rumble(req.rumble_left, SWITCH_RUMBLE_LOW_FREQUENCY_HZ,
+ SWITCH_RUMBLE_LEFT_HIGH_FREQUENCY_HZ,
+ switch_magnitude_to_amp(weak_magnitude));
+ switch_encode_rumble(req.rumble_right, SWITCH_RUMBLE_LOW_FREQUENCY_HZ,
+ SWITCH_RUMBLE_RIGHT_HIGH_FREQUENCY_HZ,
+ switch_magnitude_to_amp(strong_magnitude));
+ // Rumble request don't include the last byte of "switch_subcmd_request": subcmd_id
+ send_subcmd(d, &req, sizeof(req) - 1);
+}
+
static void switch_play_dual_rumble_now(uni_hid_device_t* d,
uint16_t duration_ms,
uint8_t weak_magnitude,
@@ -1391,14 +1471,17 @@ static void switch_play_dual_rumble_now(uni_hid_device_t* d,
return;
}
- struct switch_subcmd_request req = {
- .report_id = OUTPUT_RUMBLE_ONLY,
- };
- switch_encode_rumble(req.rumble_left, weak_magnitude << 2, weak_magnitude, 500);
- switch_encode_rumble(req.rumble_right, strong_magnitude << 2, strong_magnitude, 500);
+ ins->rumble_weak_magnitude = weak_magnitude;
+ ins->rumble_strong_magnitude = strong_magnitude;
+ switch_send_dual_rumble_now(d, weak_magnitude, strong_magnitude);
- // Rumble request don't include the last byte of "switch_subcmd_request": subcmd_id
- send_subcmd(d, &req, sizeof(req) - 1);
+ // Refresh active rumble for Switch-compatible controllers that do not
+ // retain a single output packet, including 8BitDo Switch mode.
+ ins->rumble_timer_refresh.process = &on_switch_refresh_rumble;
+ ins->rumble_timer_refresh.context = d;
+ btstack_run_loop_set_timer(&ins->rumble_timer_refresh,
+ SWITCH_RUMBLE_REFRESH_MS);
+ btstack_run_loop_add_timer(&ins->rumble_timer_refresh);
// Set timer to turn off rumble
ins->rumble_timer_duration.process = &on_switch_set_rumble_off;
@@ -1414,6 +1497,20 @@ static void on_switch_set_rumble_on(btstack_timer_source_t* ts) {
switch_play_dual_rumble_now(d, ins->rumble_duration_ms, ins->rumble_weak_magnitude, ins->rumble_strong_magnitude);
}
+static void on_switch_refresh_rumble(btstack_timer_source_t* ts) {
+ uni_hid_device_t* d = btstack_run_loop_get_timer_context(ts);
+ switch_instance_t* ins = get_switch_instance(d);
+ if (ins->rumble_state != SWITCH_STATE_RUMBLE_IN_PROGRESS) {
+ return;
+ }
+ switch_send_dual_rumble_now(
+ d, (uint8_t)ins->rumble_weak_magnitude,
+ (uint8_t)ins->rumble_strong_magnitude);
+ btstack_run_loop_set_timer(&ins->rumble_timer_refresh,
+ SWITCH_RUMBLE_REFRESH_MS);
+ btstack_run_loop_add_timer(&ins->rumble_timer_refresh);
+}
+
static void on_switch_set_rumble_off(btstack_timer_source_t* ts) {
uni_hid_device_t* d = btstack_run_loop_get_timer_context(ts);
diff --git a/src/components/bluepad32/parser/uni_hid_parser_wii.c b/src/components/bluepad32/parser/uni_hid_parser_wii.c
index be2103e..4819639 100644
--- a/src/components/bluepad32/parser/uni_hid_parser_wii.c
+++ b/src/components/bluepad32/parser/uni_hid_parser_wii.c
@@ -19,6 +19,7 @@
#endif // ENABLE_EEPROM_DUMP
#include "parser/uni_hid_parser_wii.h"
+#include "parser/uni_hid_parser_imu.h"
#include "controller/uni_controller.h"
#include "hid_usage.h"
@@ -585,9 +586,7 @@ static void process_drm_ka(uni_hid_device_t* d, const uint8_t* report, uint16_t
uni_controller_t* ctl = &d->controller;
- ctl->gamepad.accel[0] = sx;
- ctl->gamepad.accel[1] = sy;
- ctl->gamepad.accel[2] = sz;
+ uni_imu_normalize_wii_accel(sx, sy, sz, ctl->gamepad.accel);
// Dpad works as dpad, useful to navigate menus.
ctl->gamepad.dpad |= (report[1] & 0x01) ? DPAD_DOWN : 0;
diff --git a/src/components/bluepad32/uni_hid_device.c b/src/components/bluepad32/uni_hid_device.c
index 67841e8..9fe7134 100644
--- a/src/components/bluepad32/uni_hid_device.c
+++ b/src/components/bluepad32/uni_hid_device.c
@@ -655,6 +655,7 @@ void uni_hid_device_guess_controller_type_from_pid_vid(uni_hid_device_t* d) {
d->report_parser.setup = uni_hid_parser_psmove_setup;
d->report_parser.init_report = uni_hid_parser_psmove_init_report;
d->report_parser.parse_input_report = uni_hid_parser_psmove_parse_input_report;
+ d->report_parser.parse_feature_report = uni_hid_parser_psmove_parse_feature_report;
d->report_parser.set_lightbar_color = uni_hid_parser_psmove_set_lightbar_color;
d->report_parser.play_dual_rumble = uni_hid_parser_psmove_play_dual_rumble;
logi("Device detected as PS Move: 0x%02x\n", type);
Reference in a new issue View git blame Copy permalink