mirror of
https://github.com/RPCSX/rpcsx.git
synced 2025-12-06 07:12:14 +01:00
241 lines
9.8 KiB
C++
241 lines
9.8 KiB
C++
|
#include "stdafx.h"
|
||
|
|
#include "ps_move_calibration.h"
|
||
|
|
|
||
|
|
LOG_CHANNEL(move_log, "Move");
|
||
|
|
|
||
|
|
// This is basically the same as in ps move api
|
||
|
|
|
||
|
|
static inline s32 psmove_calibration_decode_16bit_unsigned(const u8* data, u32 offset)
|
||
|
|
{
|
||
|
|
const u8 low = data[offset] & 0xFF;
|
||
|
|
const u8 high = (data[offset + 1]) & 0xFF;
|
||
|
|
return (low | (high << 8)) - zero_shift;
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline s16 psmove_calibration_decode_16bit_signed(const u8* data, u32 offset)
|
||
|
|
{
|
||
|
|
const u16 low = data[offset] & 0xFF;
|
||
|
|
const u16 high = (data[offset + 1]) & 0xFF;
|
||
|
|
return static_cast<s16>(low | (high << 8));
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline u32 psmove_calibration_decode_12bits(const u8* data, u32 offset)
|
||
|
|
{
|
||
|
|
const u8 low = data[offset] & 0xFF;
|
||
|
|
const u8 high = (data[offset + 1]) & 0xFF;
|
||
|
|
return low | (high << 8);
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline f32 psmove_calibration_decode_float(const u8* data, u32 offset)
|
||
|
|
{
|
||
|
|
union
|
||
|
|
{
|
||
|
|
uint32_t u32;
|
||
|
|
float f;
|
||
|
|
} v {
|
||
|
|
.u32 = static_cast<u32>( (data[offset] & 0xFF) |
|
||
|
|
((data[offset + 1] & 0xFF) << 8) |
|
||
|
|
((data[offset + 2] & 0xFF) << 16) |
|
||
|
|
((data[offset + 3] & 0xFF) << 24))
|
||
|
|
};
|
||
|
|
|
||
|
|
return v.f;
|
||
|
|
}
|
||
|
|
|
||
|
|
static void psmove_dump_calibration(const reports::ps_move_calibration_blob& calibration, const ps_move_device& device)
|
||
|
|
{
|
||
|
|
int t, x, y, z;
|
||
|
|
float fx, fy, fz;
|
||
|
|
|
||
|
|
const u8* data = calibration.data.data();
|
||
|
|
|
||
|
|
switch (device.model)
|
||
|
|
{
|
||
|
|
case ps_move_model::ZCM1:
|
||
|
|
t = psmove_calibration_decode_12bits(data, 0x02);
|
||
|
|
move_log.error("Temperature: 0x%04X", t);
|
||
|
|
for (int orientation = 0; orientation < 6; orientation++)
|
||
|
|
{
|
||
|
|
x = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * orientation);
|
||
|
|
y = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * orientation + 2);
|
||
|
|
z = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * orientation + 4);
|
||
|
|
move_log.error("Orientation #%d: (%5d | %5d | %5d)", orientation, x, y, z);
|
||
|
|
}
|
||
|
|
|
||
|
|
t = psmove_calibration_decode_12bits(data, 0x42);
|
||
|
|
move_log.error("Temperature: 0x%04X", t);
|
||
|
|
for (int orientation = 0; orientation < 3; orientation++)
|
||
|
|
{
|
||
|
|
x = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * orientation);
|
||
|
|
y = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * orientation + 2);
|
||
|
|
z = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * orientation + 4);
|
||
|
|
move_log.error("Gyro %c, 80 rpm: (%5d | %5d | %5d)", "XYZ"[orientation], x, y, z);
|
||
|
|
}
|
||
|
|
|
||
|
|
t = psmove_calibration_decode_12bits(data, 0x28);
|
||
|
|
x = psmove_calibration_decode_16bit_unsigned(data, 0x2a);
|
||
|
|
y = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 2);
|
||
|
|
z = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 4);
|
||
|
|
move_log.error("Temperature: 0x%04X", t);
|
||
|
|
move_log.error("Gyro, 0 rpm (@0x2a): (%5d | %5d | %5d)", x, y, z);
|
||
|
|
|
||
|
|
t = psmove_calibration_decode_12bits(data, 0x30);
|
||
|
|
x = psmove_calibration_decode_16bit_unsigned(data, 0x32);
|
||
|
|
y = psmove_calibration_decode_16bit_unsigned(data, 0x32 + 2);
|
||
|
|
z = psmove_calibration_decode_16bit_unsigned(data, 0x32 + 4);
|
||
|
|
move_log.error("Temperature: 0x%04X", t);
|
||
|
|
move_log.error("Gyro, 0 rpm (@0x32): (%5d | %5d | %5d)", x, y, z);
|
||
|
|
|
||
|
|
t = psmove_calibration_decode_12bits(data, 0x5c);
|
||
|
|
fx = psmove_calibration_decode_float(data, 0x5e);
|
||
|
|
fy = psmove_calibration_decode_float(data, 0x5e + 4);
|
||
|
|
fz = psmove_calibration_decode_float(data, 0x5e + 8);
|
||
|
|
move_log.error("Temperature: 0x%04X", t);
|
||
|
|
move_log.error("Vector @0x5e: (%f | %f | %f)", fx, fy, fz);
|
||
|
|
|
||
|
|
fx = psmove_calibration_decode_float(data, 0x6a);
|
||
|
|
fy = psmove_calibration_decode_float(data, 0x6a + 4);
|
||
|
|
fz = psmove_calibration_decode_float(data, 0x6a + 8);
|
||
|
|
move_log.error("Vector @0x6a: (%f | %f | %f)", fx, fy, fz);
|
||
|
|
|
||
|
|
move_log.error("byte @0x3f: 0x%02x", static_cast<u8>(data[0x3f]));
|
||
|
|
move_log.error("float @0x76: %f", psmove_calibration_decode_float(data, 0x76));
|
||
|
|
move_log.error("float @0x7a: %f", psmove_calibration_decode_float(data, 0x7a));
|
||
|
|
break;
|
||
|
|
case ps_move_model::ZCM2:
|
||
|
|
for (int orientation = 0; orientation < 6; orientation++)
|
||
|
|
{
|
||
|
|
x = psmove_calibration_decode_16bit_signed(data, 0x04 + 6 * orientation);
|
||
|
|
y = psmove_calibration_decode_16bit_signed(data, 0x04 + 6 * orientation + 2);
|
||
|
|
z = psmove_calibration_decode_16bit_signed(data, 0x04 + 6 * orientation + 4);
|
||
|
|
move_log.error("Orientation #%d: (%5d | %5d | %5d)", orientation, x, y, z);
|
||
|
|
}
|
||
|
|
|
||
|
|
x = psmove_calibration_decode_16bit_signed(data, 0x26);
|
||
|
|
y = psmove_calibration_decode_16bit_signed(data, 0x26 + 2);
|
||
|
|
z = psmove_calibration_decode_16bit_signed(data, 0x26 + 4);
|
||
|
|
move_log.error("Gyro Bias?, 0 rpm (@0x26): (%5d | %5d | %5d)", x, y, z);
|
||
|
|
|
||
|
|
for (int orientation = 0; orientation < 6; orientation++)
|
||
|
|
{
|
||
|
|
x = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * orientation);
|
||
|
|
y = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * orientation + 2);
|
||
|
|
z = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * orientation + 4);
|
||
|
|
move_log.error("Gyro %c, 90 rpm: (%5d | %5d | %5d)", "XYZXYZ"[orientation], x, y, z);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void psmove_calibration_get_usb_accel_values(const reports::ps_move_calibration_blob& calibration, ps_move_device& device)
|
||
|
|
{
|
||
|
|
const u8* data = calibration.data.data();
|
||
|
|
|
||
|
|
int x1 = 0, x2 = 0, y1 = 0, y2 = 0, z1 = 0, z2 = 0;
|
||
|
|
|
||
|
|
switch (device.model)
|
||
|
|
{
|
||
|
|
case ps_move_model::ZCM1:
|
||
|
|
// Minimum (negative) value of each axis
|
||
|
|
x1 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * 1);
|
||
|
|
y1 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * 5 + 2);
|
||
|
|
z1 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * 2 + 4);
|
||
|
|
|
||
|
|
// Maximum (positive) value of each axis
|
||
|
|
x2 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * 3);
|
||
|
|
y2 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * 4 + 2);
|
||
|
|
z2 = psmove_calibration_decode_16bit_unsigned(data, 0x04 + 6 * 0 + 4);
|
||
|
|
break;
|
||
|
|
case ps_move_model::ZCM2:
|
||
|
|
// Minimum (negative) value of each axis
|
||
|
|
x1 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6 * 1);
|
||
|
|
y1 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6 * 3 + 2);
|
||
|
|
z1 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6 * 5 + 4);
|
||
|
|
|
||
|
|
// Maximum (positive) value of each axis
|
||
|
|
x2 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6 * 0);
|
||
|
|
y2 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6 * 2 + 2);
|
||
|
|
z2 = psmove_calibration_decode_16bit_signed(data, 0x02 + 6 * 4 + 4);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
device.calibration.accel_x_factor = 2.0f / static_cast<float>(x2 - x1);
|
||
|
|
device.calibration.accel_y_factor = 2.0f / static_cast<float>(y2 - y1);
|
||
|
|
device.calibration.accel_z_factor = 2.0f / static_cast<float>(z2 - z1);
|
||
|
|
|
||
|
|
device.calibration.accel_x_offset = -(device.calibration.accel_x_factor * static_cast<float>(x1)) - 1.0f;
|
||
|
|
device.calibration.accel_y_offset = -(device.calibration.accel_y_factor * static_cast<float>(x1)) - 1.0f;
|
||
|
|
device.calibration.accel_z_offset = -(device.calibration.accel_z_factor * static_cast<float>(x1)) - 1.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
void psmove_calibration_get_usb_gyro_values(const reports::ps_move_calibration_blob& calibration, ps_move_device& device)
|
||
|
|
{
|
||
|
|
const u8* data = calibration.data.data();
|
||
|
|
|
||
|
|
constexpr f32 PI = 3.14159265f;
|
||
|
|
constexpr f32 rpm_to_rad_per_sec = (2.0f * PI) / 60.0f;
|
||
|
|
|
||
|
|
switch (device.model)
|
||
|
|
{
|
||
|
|
case ps_move_model::ZCM1:
|
||
|
|
{
|
||
|
|
const int bx = psmove_calibration_decode_16bit_unsigned(data, 0x2a);
|
||
|
|
const int by = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 2);
|
||
|
|
const int bz = psmove_calibration_decode_16bit_unsigned(data, 0x2a + 4);
|
||
|
|
|
||
|
|
const int x = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * 0) - bx;
|
||
|
|
const int y = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * 1 + 2) - by;
|
||
|
|
const int z = psmove_calibration_decode_16bit_unsigned(data, 0x46 + 8 * 2 + 4) - bz;
|
||
|
|
|
||
|
|
constexpr f32 calibration_rpm = 80.0f;
|
||
|
|
constexpr f32 factor = calibration_rpm * rpm_to_rad_per_sec;
|
||
|
|
|
||
|
|
// Per frame drift taken into account using adjusted gain values
|
||
|
|
device.calibration.gyro_x_gain = factor / static_cast<float>(x);
|
||
|
|
device.calibration.gyro_y_gain = factor / static_cast<float>(y);
|
||
|
|
device.calibration.gyro_z_gain = factor / static_cast<float>(z);
|
||
|
|
device.calibration.gyro_x_offset = 0;
|
||
|
|
device.calibration.gyro_y_offset = 0;
|
||
|
|
device.calibration.gyro_z_offset = 0;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case ps_move_model::ZCM2:
|
||
|
|
{
|
||
|
|
// Minimum (negative) value of each axis
|
||
|
|
const int x1 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * 3);
|
||
|
|
const int y1 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * 4 + 2);
|
||
|
|
const int z1 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * 5 + 4);
|
||
|
|
|
||
|
|
// Maximum (positive) values of each axis
|
||
|
|
const int x2 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * 0);
|
||
|
|
const int y2 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * 1 + 2);
|
||
|
|
const int z2 = psmove_calibration_decode_16bit_signed(data, 0x30 + 6 * 2 + 4);
|
||
|
|
|
||
|
|
const int dx = psmove_calibration_decode_16bit_signed(data, 0x26);
|
||
|
|
const int dy = psmove_calibration_decode_16bit_signed(data, 0x26 + 2);
|
||
|
|
const int dz = psmove_calibration_decode_16bit_signed(data, 0x26 + 4);
|
||
|
|
|
||
|
|
constexpr f32 calibration_rpm = 90.0f;
|
||
|
|
constexpr f32 calibration_hi = calibration_rpm * rpm_to_rad_per_sec;
|
||
|
|
constexpr f32 calibration_low = -calibration_rpm * rpm_to_rad_per_sec;
|
||
|
|
constexpr f32 factor = calibration_hi - calibration_low;
|
||
|
|
|
||
|
|
// Compute the gain value (the slope of the gyro reading/angular speed line)
|
||
|
|
device.calibration.gyro_x_gain = factor / static_cast<f32>(x2 - x1);
|
||
|
|
device.calibration.gyro_y_gain = factor / static_cast<f32>(y2 - y1);
|
||
|
|
device.calibration.gyro_z_gain = factor / static_cast<f32>(z2 - z1);
|
||
|
|
device.calibration.gyro_x_offset = static_cast<f32>(dx);
|
||
|
|
device.calibration.gyro_y_offset = static_cast<f32>(dy);
|
||
|
|
device.calibration.gyro_z_offset = static_cast<f32>(dz);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void psmove_parse_calibration(const reports::ps_move_calibration_blob& calibration, ps_move_device& device)
|
||
|
|
{
|
||
|
|
psmove_dump_calibration(calibration, device);
|
||
|
|
psmove_calibration_get_usb_accel_values(calibration, device);
|
||
|
|
psmove_calibration_get_usb_gyro_values(calibration, device);
|
||
|
|
}
|