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 @@ -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..9f073b4 100644 --- a/src/components/bluepad32/parser/uni_hid_parser_switch.c +++ b/src/components/bluepad32/parser/uni_hid_parser_switch.c @@ -51,13 +51,15 @@ 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 #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 +74,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 +114,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 +141,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 +327,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 +339,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 +462,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 +740,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 +773,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 +847,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 +874,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 +1206,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 +1249,10 @@ 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 * 1003 + 127) / 255); +} + static struct switch_rumble_amp_data find_rumble_amp(uint16_t amp) { unsigned int i = 0; if (amp > rumble_amps[0].amp) { @@ -1259,6 +1309,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 +1417,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 +1431,22 @@ 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, 453, 135, + switch_magnitude_to_amp(weak_magnitude)); + switch_encode_rumble(req.rumble_right, 453, 99, + 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 +1459,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 +1485,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);