mirror/rpcs3
1
0
Fork 0
mirror of https://github.com/RPCS3/rpcs3.git synced 2025-03-14 10:21:21 +00:00
Code Issues Packages Projects Releases Wiki Activity

cellGem: request orientation reset during calibration

Browse Source
This commit is contained in:
Megamouse 2024-12-20 23:38:43 +01:00
parent 8678402c25
commit 1cf927aef9
4 changed files with 169 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
rpcs3/Emu/Cell/Modules/cellGem.cpp
View File

@ -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<false>::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<Ce
return CELL_GEM_COMPUTING_AVAILABLE_COLORS;
}
if (gem.is_controller_calibrating(gem_num))
if (controller.is_calibrating)
{
return CELL_GEM_SPHERE_CALIBRATING;
}
@ -2574,14 +2622,15 @@ error_code cellGemInvalidateCalibration(s32 gem_num)
return CELL_GEM_ERROR_INVALID_PARAMETER;
}
gem.controllers[gem_num].calibrated_magnetometer = false;
auto& controller = gem.controllers[gem_num];
// TODO: does this really stop an ongoing calibration ?
gem.controllers[gem_num].is_calibrating = false;
gem.controllers[gem_num].calibration_start_us = 0;
gem.controllers[gem_num].calibration_status_flags = 0;
gem.controllers[gem_num].hue_set = false;
gem.controllers[gem_num].enabled_tracking = false;
controller.calibrated_magnetometer = false;
controller.is_calibrating = false;
controller.calibration_start_us = 0;
controller.calibration_status_flags = 0;
controller.hue_set = false;
controller.enabled_tracking = false;
return CELL_OK;
}

18
rpcs3/Emu/Io/pad_types.h
View File

@ -466,13 +466,19 @@ struct ps_move_data
bool external_device_read_requested = false;
bool external_device_write_requested = false;
bool calibration_requested = false;
bool calibration_succeeded = false;
std::array<f32, 4> 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;
};

132
rpcs3/Input/ps_move_handler.cpp
View File

@ -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<u32>(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];
}

3
rpcs3/Input/ps_move_handler.h
View File

@ -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;
};