From 1429e525fbaa1db9b76c1cdb3340d772c767c11d Mon Sep 17 00:00:00 2001
From: Sam Lantinga <[EMAIL REDACTED]>
Date: Wed, 21 Jun 2023 10:28:45 -0700
Subject: [PATCH] Use default sensor calibration if we can't read it from the
Nintendo Switch controller
Fixes https://github.com/libsdl-org/SDL/issues/7830
(cherry picked from commit 3694dabe7c16bb8e76f6f4820a1987ce16c4a3c9)
(cherry picked from commit dfbdaca2b8a2bc4c7e1871698066b62b42806a46)
---
src/joystick/hidapi/SDL_hidapi_switch.c | 84 ++++++++++++-------------
1 file changed, 42 insertions(+), 42 deletions(-)
diff --git a/src/joystick/hidapi/SDL_hidapi_switch.c b/src/joystick/hidapi/SDL_hidapi_switch.c
index 8ab0ea3d3514..f53857ccc60a 100644
--- a/src/joystick/hidapi/SDL_hidapi_switch.c
+++ b/src/joystick/hidapi/SDL_hidapi_switch.c
@@ -841,66 +841,66 @@ static SDL_bool LoadStickCalibration(SDL_DriverSwitch_Context *ctx)
static SDL_bool LoadIMUCalibration(SDL_DriverSwitch_Context *ctx)
{
- Uint8 *pIMUScale;
SwitchSubcommandInputPacket_t *reply = NULL;
- Sint16 sAccelRawX, sAccelRawY, sAccelRawZ, sGyroRawX, sGyroRawY, sGyroRawZ;
/* Read Calibration Info */
SwitchSPIOpData_t readParams;
readParams.unAddress = k_unSPIIMUScaleStartOffset;
readParams.ucLength = k_unSPIIMUScaleLength;
- if (!WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply)) {
- const float accelScale = SDL_STANDARD_GRAVITY / SWITCH_ACCEL_SCALE;
- const float gyroScale = (float)M_PI / 180.0f / SWITCH_GYRO_SCALE;
+ if (WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply)) {
+ Uint8 *pIMUScale;
+ Sint16 sAccelRawX, sAccelRawY, sAccelRawZ, sGyroRawX, sGyroRawY, sGyroRawZ;
- ctx->m_IMUScaleData.fAccelScaleX = accelScale;
- ctx->m_IMUScaleData.fAccelScaleY = accelScale;
- ctx->m_IMUScaleData.fAccelScaleZ = accelScale;
-
- ctx->m_IMUScaleData.fGyroScaleX = gyroScale;
- ctx->m_IMUScaleData.fGyroScaleY = gyroScale;
- ctx->m_IMUScaleData.fGyroScaleZ = gyroScale;
+ /* IMU scale gives us multipliers for converting raw values to real world values */
+ pIMUScale = reply->spiReadData.rgucReadData;
- return SDL_FALSE;
- }
+ sAccelRawX = (pIMUScale[1] << 8) | pIMUScale[0];
+ sAccelRawY = (pIMUScale[3] << 8) | pIMUScale[2];
+ sAccelRawZ = (pIMUScale[5] << 8) | pIMUScale[4];
- /* IMU scale gives us multipliers for converting raw values to real world values */
- pIMUScale = reply->spiReadData.rgucReadData;
+ sGyroRawX = (pIMUScale[13] << 8) | pIMUScale[12];
+ sGyroRawY = (pIMUScale[15] << 8) | pIMUScale[14];
+ sGyroRawZ = (pIMUScale[17] << 8) | pIMUScale[16];
- sAccelRawX = (pIMUScale[1] << 8) | pIMUScale[0];
- sAccelRawY = (pIMUScale[3] << 8) | pIMUScale[2];
- sAccelRawZ = (pIMUScale[5] << 8) | pIMUScale[4];
+ /* 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;
+ if (WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply) && (pIMUScale[0] | pIMUScale[1] << 8) == 0xA1B2) {
+ pIMUScale = reply->spiReadData.rgucReadData;
- sGyroRawX = (pIMUScale[13] << 8) | pIMUScale[12];
- sGyroRawY = (pIMUScale[15] << 8) | pIMUScale[14];
- sGyroRawZ = (pIMUScale[17] << 8) | pIMUScale[16];
+ sAccelRawX = (pIMUScale[3] << 8) | pIMUScale[2];
+ sAccelRawY = (pIMUScale[5] << 8) | pIMUScale[4];
+ sAccelRawZ = (pIMUScale[7] << 8) | pIMUScale[6];
- /* 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;
- if (WriteSubcommand(ctx, k_eSwitchSubcommandIDs_SPIFlashRead, (uint8_t *)&readParams, sizeof(readParams), &reply) && (pIMUScale[0] | pIMUScale[1] << 8) == 0xA1B2) {
- pIMUScale = reply->spiReadData.rgucReadData;
+ sGyroRawX = (pIMUScale[15] << 8) | pIMUScale[14];
+ sGyroRawY = (pIMUScale[17] << 8) | pIMUScale[16];
+ sGyroRawZ = (pIMUScale[19] << 8) | pIMUScale[18];
+ }
- sAccelRawX = (pIMUScale[3] << 8) | pIMUScale[2];
- sAccelRawY = (pIMUScale[5] << 8) | pIMUScale[4];
- sAccelRawZ = (pIMUScale[7] << 8) | pIMUScale[6];
+ /* 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;
- sGyroRawX = (pIMUScale[15] << 8) | pIMUScale[14];
- sGyroRawY = (pIMUScale[17] << 8) | pIMUScale[16];
- sGyroRawZ = (pIMUScale[19] << 8) | pIMUScale[18];
- }
+ /* Gyro scale */
+ ctx->m_IMUScaleData.fGyroScaleX = SWITCH_GYRO_SCALE_MULT / (SWITCH_GYRO_SCALE_OFFSET - (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.fGyroScaleZ = SWITCH_GYRO_SCALE_MULT / (SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawZ) * (float)M_PI / 180.0f;
- /* 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;
+ } else {
+ /* Use default values */
+ const float accelScale = SDL_STANDARD_GRAVITY / SWITCH_ACCEL_SCALE;
+ const float gyroScale = (float)M_PI / 180.0f / SWITCH_GYRO_SCALE;
- /* Gyro scale */
- ctx->m_IMUScaleData.fGyroScaleX = SWITCH_GYRO_SCALE_MULT / (float)(SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawX) * (float)M_PI / 180.0f;
- ctx->m_IMUScaleData.fGyroScaleY = SWITCH_GYRO_SCALE_MULT / (float)(SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawY) * (float)M_PI / 180.0f;
- ctx->m_IMUScaleData.fGyroScaleZ = SWITCH_GYRO_SCALE_MULT / (float)(SWITCH_GYRO_SCALE_OFFSET - (float)sGyroRawZ) * (float)M_PI / 180.0f;
+ ctx->m_IMUScaleData.fAccelScaleX = accelScale;
+ ctx->m_IMUScaleData.fAccelScaleY = accelScale;
+ ctx->m_IMUScaleData.fAccelScaleZ = accelScale;
+ ctx->m_IMUScaleData.fGyroScaleX = gyroScale;
+ ctx->m_IMUScaleData.fGyroScaleY = gyroScale;
+ ctx->m_IMUScaleData.fGyroScaleZ = gyroScale;
+ }
return SDL_TRUE;
}