From 558a89fdb6b17e2968af221115974261c07d3d49 Mon Sep 17 00:00:00 2001
From: ceski <[EMAIL REDACTED]>
Date: Fri, 13 Jun 2025 07:46:14 -0700
Subject: [PATCH] Read Switch controller gyro/accel sensitivity coeffs (SDL3)
These vary by controller, so using the stored values should improve the accuracy of the sensor data.
---
src/joystick/hidapi/SDL_hidapi_switch.c | 24 ++++++++++++++++--------
1 file changed, 16 insertions(+), 8 deletions(-)
diff --git a/src/joystick/hidapi/SDL_hidapi_switch.c b/src/joystick/hidapi/SDL_hidapi_switch.c
index 8bfebb6df7446..074803ecc188d 100644
--- a/src/joystick/hidapi/SDL_hidapi_switch.c
+++ b/src/joystick/hidapi/SDL_hidapi_switch.c
@@ -58,9 +58,7 @@
#define SWITCH_GYRO_SCALE 14.2842f
#define SWITCH_ACCEL_SCALE 4096.f
-#define SWITCH_GYRO_SCALE_OFFSET 13371.0f
#define SWITCH_GYRO_SCALE_MULT 936.0f
-#define SWITCH_ACCEL_SCALE_OFFSET 16384.0f
#define SWITCH_ACCEL_SCALE_MULT 4.0f
enum
@@ -1044,6 +1042,8 @@ static bool LoadIMUCalibration(SDL_DriverSwitch_Context *ctx)
if (WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply)) {
Uint8 *pIMUScale;
Sint16 sAccelRawX, sAccelRawY, sAccelRawZ, sGyroRawX, sGyroRawY, sGyroRawZ;
+ Sint16 sAccelSensCoeffX, sAccelSensCoeffY, sAccelSensCoeffZ;
+ Sint16 sGyroSensCoeffX, sGyroSensCoeffY, sGyroSensCoeffZ;
// IMU scale gives us multipliers for converting raw values to real world values
pIMUScale = reply->spiReadData.rgucReadData;
@@ -1052,10 +1052,18 @@ static bool LoadIMUCalibration(SDL_DriverSwitch_Context *ctx)
sAccelRawY = (pIMUScale[3] << 8) | pIMUScale[2];
sAccelRawZ = (pIMUScale[5] << 8) | pIMUScale[4];
+ sAccelSensCoeffX = (pIMUScale[7] << 8) | pIMUScale[6];
+ sAccelSensCoeffY = (pIMUScale[9] << 8) | pIMUScale[8];
+ sAccelSensCoeffZ = (pIMUScale[11] << 8) | pIMUScale[10];
+
sGyroRawX = (pIMUScale[13] << 8) | pIMUScale[12];
sGyroRawY = (pIMUScale[15] << 8) | pIMUScale[14];
sGyroRawZ = (pIMUScale[17] << 8) | pIMUScale[16];
+ sGyroSensCoeffX = (pIMUScale[19] << 8) | pIMUScale[18];
+ sGyroSensCoeffY = (pIMUScale[21] << 8) | pIMUScale[20];
+ sGyroSensCoeffZ = (pIMUScale[23] << 8) | pIMUScale[22];
+
// Check for user calibration data. If it's present and set, it'll override the factory settings
readParams.unAddress = k_unSPIIMUUserScaleStartOffset;
readParams.ucLength = k_unSPIIMUUserScaleLength;
@@ -1072,14 +1080,14 @@ static bool LoadIMUCalibration(SDL_DriverSwitch_Context *ctx)
}
// Accelerometer scale
- ctx->m_IMUScaleData.fAccelScaleX = SWITCH_ACCEL_SCALE_MULT / (SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawX) * SDL_STANDARD_GRAVITY;
- ctx->m_IMUScaleData.fAccelScaleY = SWITCH_ACCEL_SCALE_MULT / (SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawY) * SDL_STANDARD_GRAVITY;
- ctx->m_IMUScaleData.fAccelScaleZ = SWITCH_ACCEL_SCALE_MULT / (SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawZ) * SDL_STANDARD_GRAVITY;
+ ctx->m_IMUScaleData.fAccelScaleX = SWITCH_ACCEL_SCALE_MULT / ((float)sAccelSensCoeffX - (float)sAccelRawX) * SDL_STANDARD_GRAVITY;
+ ctx->m_IMUScaleData.fAccelScaleY = SWITCH_ACCEL_SCALE_MULT / ((float)sAccelSensCoeffY - (float)sAccelRawY) * SDL_STANDARD_GRAVITY;
+ ctx->m_IMUScaleData.fAccelScaleZ = SWITCH_ACCEL_SCALE_MULT / ((float)sAccelSensCoeffZ - (float)sAccelRawZ) * SDL_STANDARD_GRAVITY;
// Gyro scale
- ctx->m_IMUScaleData.fGyroScaleX = SWITCH_GYRO_SCALE_MULT / (SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawX) * SDL_PI_F / 180.0f;
- ctx->m_IMUScaleData.fGyroScaleY = SWITCH_GYRO_SCALE_MULT / (SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawY) * SDL_PI_F / 180.0f;
- ctx->m_IMUScaleData.fGyroScaleZ = SWITCH_GYRO_SCALE_MULT / (SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawZ) * SDL_PI_F / 180.0f;
+ ctx->m_IMUScaleData.fGyroScaleX = SWITCH_GYRO_SCALE_MULT / ((float)sGyroSensCoeffX - (float)sGyroRawX) * SDL_PI_F / 180.0f;
+ ctx->m_IMUScaleData.fGyroScaleY = SWITCH_GYRO_SCALE_MULT / ((float)sGyroSensCoeffY - (float)sGyroRawY) * SDL_PI_F / 180.0f;
+ ctx->m_IMUScaleData.fGyroScaleZ = SWITCH_GYRO_SCALE_MULT / ((float)sGyroSensCoeffZ - (float)sGyroRawZ) * SDL_PI_F / 180.0f;
} else {
// Use default values