Increased precision for PS4 sensor data conversion

main
Sam Lantinga 2023-03-27 14:21:05 -07:00
parent 975039ce0d
commit 3340864786
1 changed files with 77 additions and 38 deletions

View File

@ -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;
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].sensitivity = flNumerator / (sGyroPitchPlus - sGyroPitchMinus);
ctx->calibration[0].scale = flNumerator / flDenominator;
}
flDenominator = (float)(SDL_abs(sGyroYawPlus - sGyroYawBias) + SDL_abs(sGyroYawMinus - sGyroYawBias));
if (flDenominator != 0.0f) {
ctx->calibration[1].bias = sGyroYawBias;
ctx->calibration[1].sensitivity = flNumerator / (sGyroYawPlus - sGyroYawMinus);
ctx->calibration[1].scale = flNumerator / flDenominator;
}
flDenominator = (float)(SDL_abs(sGyroRollPlus - sGyroRollBias) + SDL_abs(sGyroRollMinus - sGyroRollBias));
if (flDenominator != 0.0f) {
ctx->calibration[2].bias = sGyroRollBias;
ctx->calibration[2].sensitivity = flNumerator / (sGyroRollPlus - sGyroRollMinus);
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 void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
{
SDL_DriverPS4_Context *ctx = (SDL_DriverPS4_Context *)device->context;
int i;
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;
}
}
/* 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;
}
}
static float HIDAPI_DriverPS4_ApplyCalibrationData(SDL_DriverPS4_Context *ctx, int index, Sint16 value)
{
float result;
if (ctx->hardware_calibration) {
IMUCalibrationData *calibration = &ctx->calibration[index];
result = (value - calibration->bias) * calibration->sensitivity;
} else if (index < 3) {
result = value * 64.f;
} else {
result = value;
}
/* 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;
}
return result;
return ((float)value - calibration->bias) * calibration->scale;
}
static int HIDAPI_DriverPS4_UpdateEffects(SDL_HIDAPI_Device *device)