diff --git a/src/joystick/hidapi/SDL_hidapi_switch.c b/src/joystick/hidapi/SDL_hidapi_switch.c index 8bfebb6df7..074803ecc1 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