diff --git a/rpcs3/Emu/Cell/Modules/cellGem.cpp b/rpcs3/Emu/Cell/Modules/cellGem.cpp index eb043ed2bb..ac2914f797 100644 --- a/rpcs3/Emu/Cell/Modules/cellGem.cpp +++ b/rpcs3/Emu/Cell/Modules/cellGem.cpp @@ -270,23 +270,60 @@ public: return controllers[gem_num].status == CELL_GEM_STATUS_READY; } - bool is_controller_calibrating(u32 gem_num) + void update_calibration_status() { - gem_controller& gem = controllers[gem_num]; + std::scoped_lock lock(mtx); - if (gem.is_calibrating) + for (u32 gem_num = 0; gem_num < CELL_GEM_MAX_NUM; gem_num++) { - if ((get_guest_system_time() - gem.calibration_start_us) >= gem_controller::calibration_time_us) + gem_controller& controller = controllers[gem_num]; + if (!controller.is_calibrating) continue; + + bool controller_calibrated = true; + + // Request controller calibration + if (g_cfg.io.move == move_handler::real) { - gem.is_calibrating = false; - gem.calibration_start_us = 0; - gem.calibration_status_flags = CELL_GEM_FLAG_CALIBRATION_SUCCEEDED | CELL_GEM_FLAG_CALIBRATION_OCCURRED; - gem.calibrated_magnetometer = true; - gem.enabled_tracking = true; + std::lock_guard pad_lock(pad::g_pad_mutex); + const auto handler = pad::get_current_handler(); + const auto& pad = ::at32(handler->GetPads(), pad_num(gem_num)); + if (pad && pad->m_pad_handler == pad_handler::move) + { + if (!pad->move_data.calibration_requested || !pad->move_data.calibration_succeeded) + { + pad->move_data.calibration_requested = true; + controller_calibrated = false; + } + } + } + + // The calibration takes ~0.5 seconds on real hardware + if ((get_guest_system_time() - controller.calibration_start_us) < gem_controller::calibration_time_us) continue; + + if (!controller_calibrated) + { + cellGem.warning("Reached calibration timeout but ps move controller %d is still calibrating", gem_num); + } + + controller.is_calibrating = false; + controller.calibration_start_us = 0; + controller.calibration_status_flags = CELL_GEM_FLAG_CALIBRATION_SUCCEEDED | CELL_GEM_FLAG_CALIBRATION_OCCURRED; + controller.calibrated_magnetometer = true; + controller.enabled_tracking = true; + + // Reset controller calibration request + if (g_cfg.io.move == move_handler::real) + { + std::lock_guard pad_lock(pad::g_pad_mutex); + const auto handler = pad::get_current_handler(); + const auto& pad = ::at32(handler->GetPads(), pad_num(gem_num)); + if (pad && pad->m_pad_handler == pad_handler::move) + { + pad->move_data.calibration_requested = false; + pad->move_data.calibration_succeeded = false; + } } } - - return gem.is_calibrating; } void reset_controller(u32 gem_num) @@ -749,6 +786,11 @@ void gem_config_data::operator()() { while (!video_conversion_in_progress && thread_ctrl::state() != thread_state::aborting && !Emu.IsStopped()) { + if (state) + { + update_calibration_status(); + } + thread_ctrl::wait_for(1000); } @@ -1105,10 +1147,10 @@ static inline void pos_to_gem_state(u32 gem_num, gem_config::gem_controller& con // Calculate orientation if (g_cfg.io.move == move_handler::real) { - gem_state->quat[0] = move_data.quaternion[1]; // x - gem_state->quat[1] = move_data.quaternion[2]; // y - gem_state->quat[2] = move_data.quaternion[3]; // z - gem_state->quat[3] = move_data.quaternion[0]; // w + gem_state->quat[0] = move_data.quaternion[0]; // x + gem_state->quat[1] = move_data.quaternion[1]; // y + gem_state->quat[2] = move_data.quaternion[2]; // z + gem_state->quat[3] = move_data.quaternion[3]; // w } else { @@ -1595,13 +1637,15 @@ error_code cellGemCalibrate(u32 gem_num) return CELL_GEM_ERROR_INVALID_PARAMETER; } - if (gem.is_controller_calibrating(gem_num)) + auto& controller = gem.controllers[gem_num]; + + if (controller.is_calibrating) { return CELL_EBUSY; } - gem.controllers[gem_num].is_calibrating = true; - gem.controllers[gem_num].calibration_start_us = get_guest_system_time(); + controller.is_calibrating = true; + controller.calibration_start_us = get_guest_system_time(); return CELL_OK; } @@ -1762,12 +1806,14 @@ error_code cellGemEnableMagnetometer2(u32 gem_num, u32 enable) return CELL_GEM_NOT_CONNECTED; } - if (!gem.controllers[gem_num].calibrated_magnetometer) + auto& controller = gem.controllers[gem_num]; + + if (!controller.calibrated_magnetometer) { return CELL_GEM_NOT_CALIBRATED; } - gem.controllers[gem_num].enabled_magnetometer = !!enable; + controller.enabled_magnetometer = !!enable; return CELL_OK; } @@ -1845,11 +1891,13 @@ error_code cellGemForceRGB(u32 gem_num, f32 r, f32 g, f32 b) // color = color * (2.f / sum) //} - gem.controllers[gem_num].sphere_rgb = gem_config::gem_color(r, g, b); - gem.controllers[gem_num].enabled_tracking = false; + auto& controller = gem.controllers[gem_num]; + + controller.sphere_rgb = gem_config::gem_color(r, g, b); + controller.enabled_tracking = false; const auto [h, s, v] = ps_move_tracker::rgb_to_hsv(r, g, b); - gem.controllers[gem_num].hue = h; + controller.hue = h; return CELL_OK; } @@ -2377,7 +2425,7 @@ error_code cellGemGetState(u32 gem_num, u32 flag, u64 time_parameter, vm::ptr quaternion { 1.0f, 0.0f, 0.0f, 0.0f }; // quaternion orientation (x,y,z,w) of controller relative to default (facing the camera with buttons up) - f32 accelerometer_x = 0; // linear velocity in m/s² - f32 accelerometer_y = 0; // linear velocity in m/s² - f32 accelerometer_z = 0; // linear velocity in m/s² - f32 gyro_x = 0; // angular velocity in rad/s - f32 gyro_y = 0; // angular velocity in rad/s - f32 gyro_z = 0; // angular velocity in rad/s + f32 accelerometer_x = 0.0f; // linear velocity in m/s² + f32 accelerometer_y = 0.0f; // linear velocity in m/s² + f32 accelerometer_z = 0.0f; // linear velocity in m/s² + f32 gyro_x = 0.0f; // angular velocity in rad/s + f32 gyro_y = 0.0f; // angular velocity in rad/s + f32 gyro_z = 0.0f; // angular velocity in rad/s + f32 magnetometer_x = 0.0f; + f32 magnetometer_y = 0.0f; + f32 magnetometer_z = 0.0f; s16 temperature = 0; }; diff --git a/rpcs3/Input/ps_move_handler.cpp b/rpcs3/Input/ps_move_handler.cpp index 80742f0dc8..7b77fa2759 100644 --- a/rpcs3/Input/ps_move_handler.cpp +++ b/rpcs3/Input/ps_move_handler.cpp @@ -359,12 +359,7 @@ void ps_move_handler::check_add_device(hid_device* hidDevice, std::string_view p psmove_parse_calibration(calibration, *device); } - // Initialize Fusion - FusionAhrsInitialise(&device->ahrs); - device->ahrs.settings.convention = FusionConvention::FusionConventionEnu; - device->ahrs.settings.gain = 0.0f; // If gain is set, the algorithm tries to adjust the orientation over time. - FusionAhrsSetSettings(&device->ahrs, &device->ahrs.settings); - FusionAhrsReset(&device->ahrs); + device->reset_orientation(); // Activate if (send_output_report(device) == -1) @@ -720,57 +715,7 @@ void ps_move_handler::get_extended_info(const pad_ensemble& binding) pad->m_sensors[2].m_value = Clamp0To1023(512.0f + (MOTION_ONE_G * pad->move_data.accelerometer_z)); pad->m_sensors[3].m_value = Clamp0To1023(512.0f + (MOTION_ONE_G * pad->move_data.gyro_z * -1.0f)); - // Get elapsed time since last update - const u64 now_us = get_system_time(); - const float elapsed_sec = (dev->last_ahrs_update_time_us == 0) ? 0.0f : ((now_us - dev->last_ahrs_update_time_us) / 1'000'000.0f); - dev->last_ahrs_update_time_us = now_us; - - // The ps move handler's axis may differ from the Fusion axis, so we have to map them correctly. - // Don't ask how the axis work. It's basically been trial and error. - ensure(dev->ahrs.settings.convention == FusionConvention::FusionConventionEnu); // East-North-Up - - const FusionVector accelerometer{ - .axis { - .x = -pad->move_data.accelerometer_x, - .y = +pad->move_data.accelerometer_y, - .z = +pad->move_data.accelerometer_z - } - }; - - static constexpr f32 PI = 3.14159265f; - const auto rad_to_degree = [](f32 radians) -> f32 { return radians * 180.0f / PI; }; - const FusionVector gyroscope{ - .axis { - .x = +rad_to_degree(pad->move_data.gyro_x), - .y = +rad_to_degree(pad->move_data.gyro_z), - .z = -rad_to_degree(pad->move_data.gyro_y) - } - }; - - FusionVector magnetometer {}; - - // TODO: use magnetometer if possible - //if (dev->model == ps_move_model::ZCM1) - //{ - // const ps_move_input_report_ZCM1& input = dev->input_report_ZCM1; - // magnetometer = FusionVector{ - // .axis { - // .x = input.magnetometer_x2, - // .y = input.magnetometer_y, - // .z = input.magnetometer_z - // } - // }; - //} - - // Update Fusion - FusionAhrsUpdate(&dev->ahrs, gyroscope, accelerometer, magnetometer, elapsed_sec); - - // Get quaternion - const FusionQuaternion quaternion = FusionAhrsGetQuaternion(&dev->ahrs); - pad->move_data.quaternion[0] = quaternion.array[0]; - pad->move_data.quaternion[1] = quaternion.array[1]; - pad->move_data.quaternion[2] = quaternion.array[2]; - pad->move_data.quaternion[3] = quaternion.array[3]; + dev->update_orientation(pad->move_data); handle_external_device(binding); } @@ -911,3 +856,76 @@ u32 ps_move_handler::get_battery_level(const std::string& padId) // 0 to 5 return std::clamp(device->battery_level * 20, 0, 100); } + +void ps_move_device::reset_orientation() +{ + // Initialize Fusion + ahrs = {}; + FusionAhrsInitialise(&ahrs); + ahrs.settings.convention = FusionConvention::FusionConventionEnu; + ahrs.settings.gain = 0.0f; // If gain is set, the algorithm tries to adjust the orientation over time. + FusionAhrsSetSettings(&ahrs, &ahrs.settings); + FusionAhrsReset(&ahrs); +} + +void ps_move_device::update_orientation(ps_move_data& move_data) +{ + if (move_data.calibration_requested) + { + reset_orientation(); + + move_data.calibration_succeeded = true; + } + + // Get elapsed time since last update + const u64 now_us = get_system_time(); + const float elapsed_sec = (last_ahrs_update_time_us == 0) ? 0.0f : ((now_us - last_ahrs_update_time_us) / 1'000'000.0f); + last_ahrs_update_time_us = now_us; + + // The ps move handler's axis may differ from the Fusion axis, so we have to map them correctly. + // Don't ask how the axis work. It's basically been trial and error. + ensure(ahrs.settings.convention == FusionConvention::FusionConventionEnu); // East-North-Up + + const FusionVector accelerometer{ + .axis { + .x = -move_data.accelerometer_x, + .y = +move_data.accelerometer_y, + .z = +move_data.accelerometer_z + } + }; + + static constexpr f32 PI = 3.14159265f; + const auto rad_to_degree = [](f32 radians) -> f32 { return radians * 180.0f / PI; }; + const FusionVector gyroscope{ + .axis { + .x = +rad_to_degree(move_data.gyro_x), + .y = +rad_to_degree(move_data.gyro_z), + .z = -rad_to_degree(move_data.gyro_y) + } + }; + + FusionVector magnetometer {}; + + // TODO: use magnetometer if possible + //if (dev->model == ps_move_model::ZCM1) + //{ + // const ps_move_input_report_ZCM1& input = dev->input_report_ZCM1; + // magnetometer = FusionVector{ + // .axis { + // .x = input.magnetometer_x2, + // .y = input.magnetometer_y, + // .z = input.magnetometer_z + // } + // }; + //} + + // Update Fusion + FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, elapsed_sec); + + // Get quaternion + const FusionQuaternion quaternion = FusionAhrsGetQuaternion(&ahrs); + move_data.quaternion[0] = quaternion.array[1]; + move_data.quaternion[1] = quaternion.array[2]; + move_data.quaternion[2] = quaternion.array[3]; + move_data.quaternion[3] = quaternion.array[0]; +} diff --git a/rpcs3/Input/ps_move_handler.h b/rpcs3/Input/ps_move_handler.h index 434a3c81a2..d125fad14c 100644 --- a/rpcs3/Input/ps_move_handler.h +++ b/rpcs3/Input/ps_move_handler.h @@ -148,6 +148,9 @@ public: FusionAhrs ahrs {}; // Used to calculate quaternions from sensor data u64 last_ahrs_update_time_us = 0; // Last ahrs update + void update_orientation(ps_move_data& move_data); + void reset_orientation(); + const reports::ps_move_input_report_common& input_report_common() const; };