kc3-lang/SDL

diff --git a/src/joystick/hidapi/SDL_hidapi_ps4.c b/src/joystick/hidapi/SDL_hidapi_ps4.c
index 5a73b3c..24452a2 100644
--- a/src/joystick/hidapi/SDL_hidapi_ps4.c
+++ b/src/joystick/hidapi/SDL_hidapi_ps4.c
@@ -42,8 +42,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))
@@ -122,7 +120,7 @@ typedef struct
 typedef struct
 {
     Sint16 bias;
-    float sensitivity;
+    float scale;
 } IMUCalibrationData;
 
 typedef struct
@@ -149,6 +147,10 @@ typedef struct
     Uint8 led_red;
     Uint8 led_green;
     Uint8 led_blue;
+    Uint16 gyro_numerator;
+    Uint16 gyro_denominator;
+    Uint16 accel_numerator;
+    Uint16 accel_denominator;
     Uint16 last_timestamp;
     Uint64 timestamp;
     Uint16 valid_crc_packets; /* wrapping counter */
@@ -247,6 +249,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) {
@@ -318,6 +325,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);
@@ -361,6 +372,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;
@@ -415,7 +435,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;
@@ -426,7 +446,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) {
@@ -436,7 +456,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) {
@@ -445,7 +465,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;
             }
         }
 
@@ -475,6 +495,7 @@ static void HIDAPI_DriverPS4_LoadCalibrationData(SDL_HIDAPI_Device *device)
         Sint16 sAccZPlus, sAccZMinus;
 
         float flNumerator;
+        float flDenominator;
         Sint16 sRange2g;
 
 #ifdef DEBUG_PS4_CALIBRATION
@@ -511,36 +532,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
@@ -552,29 +581,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) * (float)M_PI / 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) * M_PI / 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)