From 3340864786314d77e04a0dd2bf1ebffbc43bfc31 Mon Sep 17 00:00:00 2001
From: Sam Lantinga <[EMAIL REDACTED]>
Date: Mon, 27 Mar 2023 14:21:05 -0700
Subject: [PATCH] Increased precision for PS4 sensor data conversion
---
src/joystick/hidapi/SDL_hidapi_ps4.c | 111 ++++++++++++++++++---------
1 file changed, 75 insertions(+), 36 deletions(-)
diff --git a/src/joystick/hidapi/SDL_hidapi_ps4.c b/src/joystick/hidapi/SDL_hidapi_ps4.c
index e589b8b9859b..5bd2bdda0710 100644
--- a/src/joystick/hidapi/SDL_hidapi_ps4.c
+++ b/src/joystick/hidapi/SDL_hidapi_ps4.c
@@ -38,8 +38,6 @@
/* Define this if you want to log calibration data */
/*#define DEBUG_PS4_CALIBRATION*/
-#define GYRO_RES_PER_DEGREE 1024.0f
-#define ACCEL_RES_PER_G 8192.0f
#define BLUETOOTH_DISCONNECT_TIMEOUT_MS 500
#define LOAD16(A, B) (Sint16)((Uint16)(A) | (((Uint16)(B)) << 8))
@@ -118,7 +116,7 @@ typedef struct
typedef struct
{
Sint16 bias;
- float sensitivity;
+ float scale;
} IMUCalibrationData;
typedef struct
@@ -145,6 +143,10 @@ typedef struct
Uint8 led_red;
Uint8 led_green;
Uint8 led_blue;
+ Uint16 gyro_numerator;
+ Uint16 gyro_denominator;
+ Uint16 accel_numerator;
+ Uint16 accel_denominator;
Uint64 sensor_ticks;
Uint16 last_tick;
Uint16 valid_crc_packets; /* wrapping counter */
@@ -243,6 +245,11 @@ static SDL_bool HIDAPI_DriverPS4_InitDevice(SDL_HIDAPI_Device *device)
}
ctx->device = device;
+ ctx->gyro_numerator = 1;
+ ctx->gyro_denominator = 16;
+ ctx->accel_numerator = 1;
+ ctx->accel_denominator = 8192;
+
device->context = ctx;
if (device->serial && SDL_strlen(device->serial) == 12) {
@@ -314,6 +321,10 @@ static SDL_bool HIDAPI_DriverPS4_InitDevice(SDL_HIDAPI_Device *device)
if (size == 48 && data[2] == 0x27) {
Uint8 capabilities = data[4];
Uint8 device_type = data[5];
+ Uint16 gyro_numerator = LOAD16(data[10], data[11]);
+ Uint16 gyro_denominator = LOAD16(data[12], data[13]);
+ Uint16 accel_numerator = LOAD16(data[14], data[15]);
+ Uint16 accel_denominator = LOAD16(data[16], data[17]);
#ifdef DEBUG_PS4_PROTOCOL
HIDAPI_DumpPacket("PS4 capabilities: size = %d", data, size);
@@ -357,6 +368,15 @@ static SDL_bool HIDAPI_DriverPS4_InitDevice(SDL_HIDAPI_Device *device)
joystick_type = SDL_JOYSTICK_TYPE_UNKNOWN;
break;
}
+
+ if (gyro_numerator && gyro_denominator) {
+ ctx->gyro_numerator = gyro_numerator;
+ ctx->gyro_denominator = gyro_denominator;
+ }
+ if (accel_numerator && accel_denominator) {
+ ctx->accel_numerator = accel_numerator;
+ ctx->accel_denominator = accel_denominator;
+ }
} else if (device->vendor_id == USB_VENDOR_SONY) {
ctx->official_controller = SDL_TRUE;
ctx->sensors_supported = SDL_TRUE;
@@ -411,7 +431,7 @@ static int HIDAPI_DriverPS4_GetDevicePlayerIndex(SDL_HIDAPI_Device *device, SDL_
return -1;
}
-static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
+static SDL_bool HIDAPI_DriverPS4_LoadOfficialCalibrationData(SDL_HIDAPI_Device *device)
{
SDL_DriverPS4_Context *ctx = (SDL_DriverPS4_Context *)device->context;
int i, tries, size;
@@ -422,7 +442,7 @@ static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
#ifdef DEBUG_PS4_CALIBRATION
SDL_Log("Not an official controller, ignoring calibration\n");
#endif
- return;
+ return SDL_FALSE;
}
for (tries = 0; tries < 5; ++tries) {
@@ -432,7 +452,7 @@ static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
#ifdef DEBUG_PS4_CALIBRATION
SDL_Log("Short read of calibration data: %d, ignoring calibration\n", size);
#endif
- return;
+ return SDL_FALSE;
}
if (device->is_bluetooth) {
@@ -441,7 +461,7 @@ static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
#ifdef DEBUG_PS4_CALIBRATION
SDL_Log("Short read of calibration data: %d, ignoring calibration\n", size);
#endif
- return;
+ return SDL_FALSE;
}
}
@@ -471,6 +491,7 @@ static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
Sint16 sAccZPlus, sAccZMinus;
float flNumerator;
+ float flDenominator;
Sint16 sRange2g;
#ifdef DEBUG_PS4_CALIBRATION
@@ -507,36 +528,44 @@ static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
sAccZPlus = LOAD16(data[31], data[32]);
sAccZMinus = LOAD16(data[33], data[34]);
- flNumerator = (sGyroSpeedPlus + sGyroSpeedMinus) * GYRO_RES_PER_DEGREE;
- ctx->calibration[0].bias = sGyroPitchBias;
- ctx->calibration[0].sensitivity = flNumerator / (sGyroPitchPlus - sGyroPitchMinus);
+ flNumerator = (float)(sGyroSpeedPlus + sGyroSpeedMinus) * ctx->gyro_denominator / ctx->gyro_numerator;
+ flDenominator = (float)(SDL_abs(sGyroPitchPlus - sGyroPitchBias) + SDL_abs(sGyroPitchMinus - sGyroPitchBias));
+ if (flDenominator != 0.0f) {
+ ctx->calibration[0].bias = sGyroPitchBias;
+ ctx->calibration[0].scale = flNumerator / flDenominator;
+ }
- ctx->calibration[1].bias = sGyroYawBias;
- ctx->calibration[1].sensitivity = flNumerator / (sGyroYawPlus - sGyroYawMinus);
+ flDenominator = (float)(SDL_abs(sGyroYawPlus - sGyroYawBias) + SDL_abs(sGyroYawMinus - sGyroYawBias));
+ if (flDenominator != 0.0f) {
+ ctx->calibration[1].bias = sGyroYawBias;
+ ctx->calibration[1].scale = flNumerator / flDenominator;
+ }
- ctx->calibration[2].bias = sGyroRollBias;
- ctx->calibration[2].sensitivity = flNumerator / (sGyroRollPlus - sGyroRollMinus);
+ flDenominator = (float)(SDL_abs(sGyroRollPlus - sGyroRollBias) + SDL_abs(sGyroRollMinus - sGyroRollBias));
+ if (flDenominator != 0.0f) {
+ ctx->calibration[2].bias = sGyroRollBias;
+ ctx->calibration[2].scale = flNumerator / flDenominator;
+ }
sRange2g = sAccXPlus - sAccXMinus;
ctx->calibration[3].bias = sAccXPlus - sRange2g / 2;
- ctx->calibration[3].sensitivity = 2.0f * ACCEL_RES_PER_G / (float)sRange2g;
+ ctx->calibration[3].scale = (2.0f * ctx->accel_denominator / ctx->accel_numerator) / sRange2g;
sRange2g = sAccYPlus - sAccYMinus;
ctx->calibration[4].bias = sAccYPlus - sRange2g / 2;
- ctx->calibration[4].sensitivity = 2.0f * ACCEL_RES_PER_G / (float)sRange2g;
+ ctx->calibration[4].scale = (2.0f * ctx->accel_denominator / ctx->accel_numerator) / sRange2g;
sRange2g = sAccZPlus - sAccZMinus;
ctx->calibration[5].bias = sAccZPlus - sRange2g / 2;
- ctx->calibration[5].sensitivity = 2.0f * ACCEL_RES_PER_G / (float)sRange2g;
+ ctx->calibration[5].scale = (2.0f * ctx->accel_denominator / ctx->accel_numerator) / sRange2g;
ctx->hardware_calibration = SDL_TRUE;
for (i = 0; i < 6; ++i) {
- float divisor = (i < 3 ? 64.0f : 1.0f);
#ifdef DEBUG_PS4_CALIBRATION
- SDL_Log("calibration[%d] bias = %d, sensitivity = %f\n", i, ctx->calibration[i].bias, ctx->calibration[i].sensitivity);
+ SDL_Log("calibration[%d] bias = %d, sensitivity = %f\n", i, ctx->calibration[i].bias, ctx->calibration[i].scale);
#endif
/* Some controllers have a bad calibration */
- if ((SDL_abs(ctx->calibration[i].bias) > 1024) || (SDL_fabs(1.0f - ctx->calibration[i].sensitivity / divisor) > 0.5f)) {
+ if (SDL_abs(ctx->calibration[i].bias) > 1024 || SDL_fabs(1.0f - ctx->calibration[i].scale) > 0.5f) {
#ifdef DEBUG_PS4_CALIBRATION
SDL_Log("invalid calibration, ignoring\n");
#endif
@@ -548,29 +577,39 @@ static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
SDL_Log("Calibration data not available\n");
#endif
}
+ return ctx->hardware_calibration;
}
-static float HIDAPI_DriverPS4_ApplyCalibrationData(SDL_DriverPS4_Context *ctx, int index, Sint16 value)
+static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
{
- float result;
-
- if (ctx->hardware_calibration) {
- IMUCalibrationData *calibration = &ctx->calibration[index];
+ SDL_DriverPS4_Context *ctx = (SDL_DriverPS4_Context *)device->context;
+ int i;
- result = (value - calibration->bias) * calibration->sensitivity;
- } else if (index < 3) {
- result = value * 64.f;
- } else {
- result = value;
+ if (!HIDAPI_DriverPS4_LoadOfficialCalibrationData(device)) {
+ for (i = 0; i < SDL_arraysize(ctx->calibration); ++i) {
+ ctx->calibration[i].bias = 0;
+ ctx->calibration[i].scale = 1.0f;
+ }
}
- /* Convert the raw data to the units expected by SDL */
- if (index < 3) {
- result = (result / GYRO_RES_PER_DEGREE) * SDL_PI_F / 180.0f;
- } else {
- result = (result / ACCEL_RES_PER_G) * SDL_STANDARD_GRAVITY;
+ /* Scale the raw data to the units expected by SDL */
+ for (i = 0; i < SDL_arraysize(ctx->calibration); ++i) {
+ double scale = ctx->calibration[i].scale;
+
+ if (i < 3) {
+ scale *= ((double)ctx->gyro_numerator / ctx->gyro_denominator) * SDL_PI_D / 180.0;
+ } else {
+ scale *= ((double)ctx->accel_numerator / ctx->accel_denominator) * SDL_STANDARD_GRAVITY;
+ }
+ ctx->calibration[i].scale = (float)scale;
}
- return result;
+}
+
+static float HIDAPI_DriverPS4_ApplyCalibrationData(SDL_DriverPS4_Context *ctx, int index, Sint16 value)
+{
+ IMUCalibrationData *calibration = &ctx->calibration[index];
+
+ return ((float)value - calibration->bias) * calibration->scale;
}
static int HIDAPI_DriverPS4_UpdateEffects(SDL_HIDAPI_Device *device)