RPCS3/rpcs3
1
0
Fork 0
mirror of https://github.com/RPCS3/rpcs3.git synced 2026-10-12 02:09:17 +00:00
Code Issues Packages Projects Releases Wiki Activity

ps move: Only update ahrs orientation if a new input report arrived and handle ZCM1 half frames

Browse Source
This commit is contained in:
Megamouse
2026-10-03 19:35:09 +02:00
parent a664f405bc
commit 103d41944c
4 changed files with 169 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rpcs3/Emu/Io/PadHandler.cpp
+52 -17
View File
@@ -987,8 +987,9 @@ void PadDevice::reset_orientation()
FusionAhrsInitialise(ahrs.get());
set_fusion_settings(ahrs.get(), fusionAhrsDefaultSettings.sampleRate);
// Start measuring the sample period from scratch
// Start measuring the sample period from scratch and discard samples from before the reset
last_ahrs_update_time_us = 0;
imu_sample_count = 0;
}
void PadDevice::update_orientation(ps_move_data& move_data)
@@ -998,17 +999,50 @@ void PadDevice::update_orientation(ps_move_data& move_data)
reset_orientation();
}
// Get elapsed time since last update
const u64 now_us = get_system_time();
const f32 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;
// We need a valid sample period. Skip the first update after a reset.
if (elapsed_sec <= 0.0f)
if (!queues_imu_samples)
{
return;
// Use the current sensor values and the elapsed time since the last update
const u64 now_us = get_system_time();
const f32 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;
// We need a valid sample period. Skip the first update after a reset.
if (elapsed_sec > 0.0f)
{
imu_samples[0] = { move_data.accelerometer, move_data.gyro };
imu_sample_count = 1;
imu_sample_delta_time = elapsed_sec;
}
}
// Feed each queued sample to the AHRS
const u32 sample_count = std::exchange(imu_sample_count, 0);
f32 elapsed_sec = 0.0f;
for (u32 i = 0; i < sample_count; i++)
{
if (update_ahrs(move_data, ::at32(imu_samples, i), imu_sample_delta_time))
{
elapsed_sec += imu_sample_delta_time;
}
}
if (elapsed_sec > 0.0f)
{
move_data.update_orientation(elapsed_sec);
}
}
bool PadDevice::update_ahrs(ps_move_data& move_data, const imu_sample& sample, f32 elapsed_sec)
{
if (!ahrs || !(elapsed_sec > 0.0f))
{
return false;
}
const ps_move_data::vect<3>& accel = sample.accelerometer;
const ps_move_data::vect<3>& gyro = sample.gyro;
// The sensor data in move_data uses the following device frame (see set_raw_orientation and the PS Move handler):
// x: right, y: forward (towards the sphere), z: up (buttons)
// The accelerometer, gyro and magnetometer share this frame.
@@ -1018,17 +1052,17 @@ void PadDevice::update_orientation(ps_move_data& move_data)
const FusionVector accelerometer{
.axis {
.x = move_data.accelerometer.x(),
.y = move_data.accelerometer.y(),
.z = move_data.accelerometer.z()
.x = accel.x(),
.y = accel.y(),
.z = accel.z()
}
};
const FusionVector gyroscope{
.axis {
.x = PadHandlerBase::rad_to_degree(move_data.gyro.x()),
.y = PadHandlerBase::rad_to_degree(move_data.gyro.y()),
.z = PadHandlerBase::rad_to_degree(move_data.gyro.z())
.x = PadHandlerBase::rad_to_degree(gyro.x()),
.y = PadHandlerBase::rad_to_degree(gyro.y()),
.z = PadHandlerBase::rad_to_degree(gyro.z())
}
};
@@ -1064,7 +1098,7 @@ void PadDevice::update_orientation(ps_move_data& move_data)
{
// Discard this update and keep the last valid orientation
*ahrs = ahrs_backup;
return;
return false;
}
// Convert the quaternion from the device frame (x: right, y: forward, z: up) to the cellGem frame (x: right, y: up, z: backward).
@@ -1075,5 +1109,6 @@ void PadDevice::update_orientation(ps_move_data& move_data)
move_data.quaternion[1] = quaternion.array[3]; // y = z
move_data.quaternion[2] = -quaternion.array[2]; // z = -y
move_data.quaternion[3] = quaternion.array[0]; // w
move_data.update_orientation(elapsed_sec);
return true;
}
rpcs3/Emu/Io/PadHandler.h
+17
View File
@@ -53,8 +53,25 @@ public:
std::shared_ptr<FusionAhrs> ahrs; // Used to calculate quaternions from sensor data
u64 last_ahrs_update_time_us = 0; // Last ahrs update
// Sensor samples (accelerometer in G, gyro in rad/s) for the next orientation update.
// By default, update_orientation uses the current values in move_data and the time since the last update.
// Handlers that set queues_imu_samples queue their own samples instead (e.g. once per input report).
struct imu_sample
{
ps_move_data::vect<3> accelerometer{};
ps_move_data::vect<3> gyro{};
};
bool queues_imu_samples = false;
std::array<imu_sample, 2> imu_samples{};
u32 imu_sample_count = 0;
f32 imu_sample_delta_time = 0.0f; // Seconds per sample
void update_orientation(ps_move_data& move_data);
void reset_orientation();
private:
// Feeds one sensor sample to the AHRS and updates move_data.quaternion. Returns false if the sample was discarded.
bool update_ahrs(ps_move_data& move_data, const imu_sample& sample, f32 elapsed_sec);
};
struct pad_ensemble
rpcs3/Input/ps_move_handler.cpp
+97 -42
View File
@@ -3,6 +3,7 @@
#include "ps_move_calibration.h"
#include "Emu/Io/pad_config.h"
#include "Emu/Cell/Modules/cellGem.h"
#include "Emu/Cell/timers.hpp"
LOG_CHANNEL(move_log, "Move");
@@ -347,6 +348,12 @@ void ps_move_handler::check_add_device(hid_device* hidDevice, hid_enumerated_dev
device->calibration.is_valid = psmove_parse_calibration(calibration, *device);
}
// We queue the IMU samples once per input report (see get_extended_info)
device->queues_imu_samples = true;
device->has_new_input_report = false;
device->last_input_report_time_us = 0;
device->imu_sample_count = 0;
// Activate
if (send_output_report(device) == -1)
{
@@ -405,6 +412,7 @@ ps_move_handler::DataStatus ps_move_handler::get_data(ps_move_device* device)
// Get the new data
std::memcpy(report, buf.data(), report_size);
device->has_new_input_report = true;
//move_log.error("%s", fmt::buf_to_hexstring(buf.data(), buf.size(), 64));
@@ -665,68 +673,115 @@ void ps_move_handler::get_extended_info(const pad_ensemble& binding)
const ps_move_input_report_common& input = dev->input_report_common();
// Get the time since the last input report. We only queue new IMU samples once per report.
// Skip the first report and long gaps (e.g. reconnects), since there is no meaningful delta.
f32 report_delta_time = 0.0f;
if (std::exchange(dev->has_new_input_report, false))
{
const u64 now_us = get_system_time();
const u64 last_us = std::exchange(dev->last_input_report_time_us, now_us);
if (last_us != 0 && (now_us - last_us) <= 250'000)
{
report_delta_time = (now_us - last_us) / 1'000'000.0f;
}
}
// The default position is flat on the ground, pointing forward.
// The accelerometers constantly measure G forces.
// The gyros measure changes in orientation and will reset when the device isn't moved anymore.
f32 accel_x = input.accel_x_1; // Increases if the device is rolled to the left
f32 accel_y = input.accel_y_1; // Increases if the device is pitched upwards
f32 accel_z = input.accel_z_1; // Increases if the device is moved upwards
f32 gyro_x = input.gyro_x_1; // Increases if the device is pitched upwards
f32 gyro_y = input.gyro_y_1; // Increases if the device is rolled to the right
f32 gyro_z = input.gyro_z_1; // Increases if the device is yawed to the left
if (dev->model == ps_move_model::ZCM1)
// accel x: Increases if the device is rolled to the left
// accel y: Increases if the device is pitched upwards
// accel z: Increases if the device is moved upwards
// gyro x: Increases if the device is pitched upwards
// gyro y: Increases if the device is rolled to the right
// gyro z: Increases if the device is yawed to the left
const auto get_calibrated_sample = [dev](s16 ax, s16 ay, s16 az, s16 gx, s16 gy, s16 gz)
{
const auto decode_16bit = [](s16 val)
const auto decode = [model = dev->model](s16 val)
{
if (model != ps_move_model::ZCM1)
{
// The ZCM2 uses two's complement
return static_cast<f32>(val);
}
// The ZCM1 uses an offset of 0x8000
const u8* data = reinterpret_cast<const u8*>(&val);
const u8 low = data[0] & 0xFF;
const u8 high = data[1] & 0xFF;
const s32 res = (low | (high << 8)) - zero_shift;
return static_cast<f32>(res);
};
accel_x = decode_16bit(input.accel_x_1);
accel_y = decode_16bit(input.accel_y_1);
accel_z = decode_16bit(input.accel_z_1);
gyro_x = decode_16bit(input.gyro_x_1);
gyro_y = decode_16bit(input.gyro_y_1);
gyro_z = decode_16bit(input.gyro_z_1);
}
// Apply calibration. We need the calibrated values for the sixaxis sensors as well, regardless of the orientation setting.
if (dev->calibration.is_valid)
{
accel_x = accel_x * dev->calibration.accel_x_factor + dev->calibration.accel_x_offset;
accel_y = accel_y * dev->calibration.accel_y_factor + dev->calibration.accel_y_offset;
accel_z = accel_z * dev->calibration.accel_z_factor + dev->calibration.accel_z_offset;
gyro_x = (gyro_x - dev->calibration.gyro_x_offset) * dev->calibration.gyro_x_gain;
gyro_y = (gyro_y - dev->calibration.gyro_y_offset) * dev->calibration.gyro_y_gain;
gyro_z = (gyro_z - dev->calibration.gyro_z_offset) * dev->calibration.gyro_z_gain;
}
else
{
constexpr f32 MOVE_ONE_G = 4096.0f; // This is just a rough estimate and probably depends on the device
f32 accel_x = decode(ax);
f32 accel_y = decode(ay);
f32 accel_z = decode(az);
f32 gyro_x = decode(gx);
f32 gyro_y = decode(gy);
f32 gyro_z = decode(gz);
accel_x /= MOVE_ONE_G;
accel_y /= MOVE_ONE_G;
accel_z /= MOVE_ONE_G;
gyro_x /= MOVE_ONE_G;
gyro_y /= MOVE_ONE_G;
gyro_z /= MOVE_ONE_G;
}
// Apply calibration. We need the calibrated values for the sixaxis sensors as well, regardless of the orientation setting.
if (dev->calibration.is_valid)
{
accel_x = accel_x * dev->calibration.accel_x_factor + dev->calibration.accel_x_offset;
accel_y = accel_y * dev->calibration.accel_y_factor + dev->calibration.accel_y_offset;
accel_z = accel_z * dev->calibration.accel_z_factor + dev->calibration.accel_z_offset;
gyro_x = (gyro_x - dev->calibration.gyro_x_offset) * dev->calibration.gyro_x_gain;
gyro_y = (gyro_y - dev->calibration.gyro_y_offset) * dev->calibration.gyro_y_gain;
gyro_z = (gyro_z - dev->calibration.gyro_z_offset) * dev->calibration.gyro_z_gain;
}
else
{
constexpr f32 MOVE_ONE_G = 4096.0f; // This is just a rough estimate and probably depends on the device
accel_x /= MOVE_ONE_G;
accel_y /= MOVE_ONE_G;
accel_z /= MOVE_ONE_G;
gyro_x /= MOVE_ONE_G;
gyro_y /= MOVE_ONE_G;
gyro_z /= MOVE_ONE_G;
}
ps_move_device::imu_sample sample{};
sample.accelerometer = ps_move_data::vect<3>({accel_x, accel_y, accel_z});
sample.gyro = ps_move_data::vect<3>({gyro_x, gyro_y, gyro_z});
return sample;
};
// The ZCM1 reports two half-frames per report. The 1st half-frame contains slightly older data than the 2nd half-frame.
// The ZCM2 only has one frame. Its second set of values is just a copy of the first one.
const bool has_two_half_frames = dev->model == ps_move_model::ZCM1;
const ps_move_device::imu_sample sample_1 = get_calibrated_sample(input.accel_x_1, input.accel_y_1, input.accel_z_1, input.gyro_x_1, input.gyro_y_1, input.gyro_z_1);
const ps_move_device::imu_sample sample_2 = has_two_half_frames
? get_calibrated_sample(input.accel_x_2, input.accel_y_2, input.accel_z_2, input.gyro_x_2, input.gyro_y_2, input.gyro_z_2)
: sample_1;
// Use the latest sample for everything that only needs the current state
const f32 accel_x = sample_2.accelerometer.x();
const f32 accel_y = sample_2.accelerometer.y();
const f32 accel_z = sample_2.accelerometer.z();
const f32 gyro_z = sample_2.gyro.z();
if (!device->config || !device->config->orientation_enabled)
{
pad->move_data.reset_sensors();
dev->imu_sample_count = 0;
}
else
{
pad->move_data.accelerometer.x() = accel_x;
pad->move_data.accelerometer.y() = accel_y;
pad->move_data.accelerometer.z() = accel_z;
pad->move_data.gyro.x() = gyro_x;
pad->move_data.gyro.y() = gyro_y;
pad->move_data.gyro.z() = gyro_z;
pad->move_data.accelerometer = sample_2.accelerometer;
pad->move_data.gyro = sample_2.gyro;
// Queue the samples of this report for the orientation update. Each half-frame covers half of the report interval.
if (report_delta_time > 0.0f)
{
dev->imu_samples = { sample_1, sample_2 };
dev->imu_sample_count = has_two_half_frames ? 2 : 1;
dev->imu_sample_delta_time = report_delta_time / dev->imu_sample_count;
}
if (dev->model == ps_move_model::ZCM1)
{
rpcs3/Input/ps_move_handler.h
+3
View File
@@ -137,6 +137,9 @@ public:
u32 external_device_id = 0;
ps_move_calibration calibration{};
bool has_new_input_report = false; // Set by get_data, consumed by get_extended_info
u64 last_input_report_time_us = 0;
const reports::ps_move_input_report_common& input_report_common() const;
};