Commit 1429e525fbaa1db9b76c1cdb3340d772c767c11d
2023-06-21T10:28:45
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)
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113
diff --git a/src/joystick/hidapi/SDL_hidapi_switch.c b/src/joystick/hidapi/SDL_hidapi_switch.c
index 8ab0ea3..f53857c 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;
}