mirror
/
SDL
mirror of https://github.com/libsdl-org/SDL.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

Read Switch controller gyro/accel sensitivity coeffs (SDL2) 

These vary by controller, so using the stored values should improve the accuracy of the sensor data.
This commit is contained in:
ceski 2025-06-13 08:04:13 -07:00 committed by Sam Lantinga
parent 027d46f03a
commit 9eb50a906a
1 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/joystick/hidapi/SDL_hidapi_switch.c
View File

@ -62,9 +62,7 @@
#define SWITCH_GYRO_SCALE 14.2842f #define SWITCH_GYRO_SCALE 14.2842f
#define SWITCH_ACCEL_SCALE 4096.f #define SWITCH_ACCEL_SCALE 4096.f
#define SWITCH_GYRO_SCALE_OFFSET 13371.0f
#define SWITCH_GYRO_SCALE_MULT 936.0f #define SWITCH_GYRO_SCALE_MULT 936.0f
#define SWITCH_ACCEL_SCALE_OFFSET 16384.0f
#define SWITCH_ACCEL_SCALE_MULT 4.0f #define SWITCH_ACCEL_SCALE_MULT 4.0f
typedef enum typedef enum
@ -925,6 +923,8 @@ static SDL_bool LoadIMUCalibration(SDL_DriverSwitch_Context *ctx)
if (WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply)) { if (WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply)) {
Uint8 *pIMUScale; Uint8 *pIMUScale;
Sint16 sAccelRawX, sAccelRawY, sAccelRawZ, sGyroRawX, sGyroRawY, sGyroRawZ; 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 */ /* IMU scale gives us multipliers for converting raw values to real world values */
pIMUScale = reply->spiReadData.rgucReadData; pIMUScale = reply->spiReadData.rgucReadData;
@ -933,10 +933,18 @@ static SDL_bool LoadIMUCalibration(SDL_DriverSwitch_Context *ctx)
sAccelRawY = (pIMUScale[3] << 8) | pIMUScale[2]; sAccelRawY = (pIMUScale[3] << 8) | pIMUScale[2];
sAccelRawZ = (pIMUScale[5] << 8) | pIMUScale[4]; 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]; sGyroRawX = (pIMUScale[13] << 8) | pIMUScale[12];
sGyroRawY = (pIMUScale[15] << 8) | pIMUScale[14]; sGyroRawY = (pIMUScale[15] << 8) | pIMUScale[14];
sGyroRawZ = (pIMUScale[17] << 8) | pIMUScale[16]; 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 */ /* Check for user calibration data. If it's present and set, it'll override the factory settings */
readParams.unAddress = k_unSPIIMUUserScaleStartOffset; readParams.unAddress = k_unSPIIMUUserScaleStartOffset;
readParams.ucLength = k_unSPIIMUUserScaleLength; readParams.ucLength = k_unSPIIMUUserScaleLength;
@ -953,14 +961,14 @@ static SDL_bool LoadIMUCalibration(SDL_DriverSwitch_Context *ctx)
} }
/* Accelerometer scale */ /* Accelerometer scale */
ctx->m_IMUScaleData.fAccelScaleX = SWITCH_ACCEL_SCALE_MULT / (SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawX) * 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 / (SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawY) * 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 / (SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawZ) * SDL_STANDARD_GRAVITY; ctx->m_IMUScaleData.fAccelScaleZ = SWITCH_ACCEL_SCALE_MULT / ((float)sAccelSensCoeffZ - (float)sAccelRawZ) * SDL_STANDARD_GRAVITY;
/* Gyro scale */ /* Gyro scale */
ctx->m_IMUScaleData.fGyroScaleX = SWITCH_GYRO_SCALE_MULT / (SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawX) * (float)M_PI / 180.0f; ctx->m_IMUScaleData.fGyroScaleX = SWITCH_GYRO_SCALE_MULT / ((float)sGyroSensCoeffX - (float)sGyroRawX) * (float)M_PI / 180.0f;
ctx->m_IMUScaleData.fGyroScaleY = SWITCH_GYRO_SCALE_MULT / (SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawY) * (float)M_PI / 180.0f; ctx->m_IMUScaleData.fGyroScaleY = SWITCH_GYRO_SCALE_MULT / ((float)sGyroSensCoeffY - (float)sGyroRawY) * (float)M_PI / 180.0f;
ctx->m_IMUScaleData.fGyroScaleZ = SWITCH_GYRO_SCALE_MULT / (SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawZ) * (float)M_PI / 180.0f; ctx->m_IMUScaleData.fGyroScaleZ = SWITCH_GYRO_SCALE_MULT / ((float)sGyroSensCoeffZ - (float)sGyroRawZ) * (float)M_PI / 180.0f;
} else { } else {
/* Use default values */ /* Use default values */