Commit d7c07d6b09de6f9ae3c7bb3eb1765c7a10af4b3b
2022-04-27T00:57:17
Read motion sensor scale from Switch controllers (#5555) * Read IMU scale data from Switch controllers. Up until now, SDL has used hard-coded scaling which isn't correct with some supported controllers. * Moved declarations to beginning of code blocks to better fit with SDL style requirements
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 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174
diff --git a/src/joystick/hidapi/SDL_hidapi_switch.c b/src/joystick/hidapi/SDL_hidapi_switch.c
index 4bee69b..13b5e29 100644
--- a/src/joystick/hidapi/SDL_hidapi_switch.c
+++ b/src/joystick/hidapi/SDL_hidapi_switch.c
@@ -63,6 +63,11 @@
#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
+
typedef enum {
k_eSwitchInputReportIDs_SubcommandReply = 0x21,
k_eSwitchInputReportIDs_FullControllerState = 0x30,
@@ -114,6 +119,14 @@ typedef enum {
#define k_unSPIStickCalibrationEndOffset 0x604E
#define k_unSPIStickCalibrationLength (k_unSPIStickCalibrationEndOffset - k_unSPIStickCalibrationStartOffset + 1)
+#define k_unSPIIMUScaleStartOffset 0x6020
+#define k_unSPIIMUScaleEndOffset 0x6037
+#define k_unSPIIMUScaleLength (k_unSPIIMUScaleEndOffset - k_unSPIIMUScaleStartOffset + 1)
+
+#define k_unSPIIMUUserScaleStartOffset 0x8026
+#define k_unSPIIMUUserScaleEndOffset 0x8039
+#define k_unSPIIMUUserScaleLength (k_unSPIIMUUserScaleEndOffset - k_unSPIIMUUserScaleStartOffset + 1)
+
#pragma pack(1)
typedef struct
{
@@ -266,6 +279,16 @@ typedef struct {
Sint16 sMax;
} axis[2];
} m_StickExtents[2];
+
+ struct IMUScaleData {
+ float fAccelScaleX;
+ float fAccelScaleY;
+ float fAccelScaleZ;
+
+ float fGyroScaleX;
+ float fGyroScaleY;
+ float fGyroScaleZ;
+ } m_IMUScaleData;
} SDL_DriverSwitch_Context;
@@ -769,6 +792,72 @@ static SDL_bool LoadStickCalibration(SDL_DriverSwitch_Context *ctx, Uint8 input_
return SDL_TRUE;
}
+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;
+
+ 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_FALSE;
+ }
+
+ /* IMU scale gives us multipliers for converting raw values to real world values */
+ pIMUScale = reply->spiReadData.rgucReadData;
+
+ sAccelRawX = ((pIMUScale[1] << 8) & 0xF00) | pIMUScale[0];
+ sAccelRawY = ((pIMUScale[3] << 8) & 0xF00) | pIMUScale[2];
+ sAccelRawZ = ((pIMUScale[5] << 8) & 0xF00) | pIMUScale[4];
+
+ sGyroRawX = ((pIMUScale[13] << 8) & 0xF00) | pIMUScale[12];
+ sGyroRawY = ((pIMUScale[15] << 8) & 0xF00) | pIMUScale[14];
+ sGyroRawZ = ((pIMUScale[17] << 8) & 0xF00) | pIMUScale[16];
+
+ /* 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;
+
+ sAccelRawX = ((pIMUScale[3] << 8) & 0xF00) | pIMUScale[2];
+ sAccelRawY = ((pIMUScale[5] << 8) & 0xF00) | pIMUScale[4];
+ sAccelRawZ = ((pIMUScale[7] << 8) & 0xF00) | pIMUScale[6];
+
+ sGyroRawX = ((pIMUScale[15] << 8) & 0xF00) | pIMUScale[14];
+ sGyroRawY = ((pIMUScale[17] << 8) & 0xF00) | pIMUScale[16];
+ sGyroRawZ = ((pIMUScale[19] << 8) & 0xF00) | pIMUScale[18];
+ }
+
+ /* Accelerometer scale */
+ ctx->m_IMUScaleData.fAccelScaleX = SWITCH_ACCEL_SCALE_MULT / (float)(SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawX) * SDL_STANDARD_GRAVITY;
+ ctx->m_IMUScaleData.fAccelScaleY = SWITCH_ACCEL_SCALE_MULT / (float)(SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawY) * SDL_STANDARD_GRAVITY;
+ ctx->m_IMUScaleData.fAccelScaleZ = SWITCH_ACCEL_SCALE_MULT / (float)(SWITCH_ACCEL_SCALE_OFFSET - (float)sAccelRawZ) * SDL_STANDARD_GRAVITY;
+
+ /* 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;
+
+ return SDL_TRUE;
+}
+
+
static Sint16 ApplyStickCalibrationCentered(SDL_DriverSwitch_Context *ctx, int nStick, int nAxis, Sint16 sRawValue, Sint16 sCenter)
{
sRawValue -= sCenter;
@@ -914,6 +1003,11 @@ HIDAPI_DriverSwitch_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joysti
goto error;
}
+ if (!LoadIMUCalibration(ctx)) {
+ SDL_SetError("Couldn't load sensor calibration");
+ goto error;
+ }
+
if (!SetVibrationEnabled(ctx, 1)) {
SDL_SetError("Couldn't enable vibration");
goto error;
@@ -1146,20 +1240,6 @@ HIDAPI_DriverSwitch_SetJoystickSensorsEnabled(SDL_HIDAPI_Device *device, SDL_Joy
return 0;
}
-static float
-HIDAPI_DriverSwitch_ScaleGyro(Sint16 value)
-{
- float result = (value / SWITCH_GYRO_SCALE) * (float)M_PI / 180.0f;
- return result;
-}
-
-static float
-HIDAPI_DriverSwitch_ScaleAccel(Sint16 value)
-{
- float result = (value / SWITCH_ACCEL_SCALE) * SDL_STANDARD_GRAVITY;
- return result;
-}
-
static void HandleInputOnlyControllerState(SDL_Joystick *joystick, SDL_DriverSwitch_Context *ctx, SwitchInputOnlyControllerStatePacket_t *packet)
{
Sint16 axis;
@@ -1357,13 +1437,13 @@ static void SendSensorUpdate(SDL_Joystick *joystick, SDL_DriverSwitch_Context *c
* users will want consistent axis mappings across devices.
*/
if (type == SDL_SENSOR_GYRO) {
- data[0] = -HIDAPI_DriverSwitch_ScaleGyro(values[1]);
- data[1] = HIDAPI_DriverSwitch_ScaleGyro(values[2]);
- data[2] = -HIDAPI_DriverSwitch_ScaleGyro(values[0]);
+ data[0] = -(ctx->m_IMUScaleData.fGyroScaleY * (float)values[1]);
+ data[1] = ctx->m_IMUScaleData.fGyroScaleZ * (float)values[2];
+ data[2] = -(ctx->m_IMUScaleData.fGyroScaleX * (float)values[0]);
} else {
- data[0] = -HIDAPI_DriverSwitch_ScaleAccel(values[1]);
- data[1] = HIDAPI_DriverSwitch_ScaleAccel(values[2]);
- data[2] = -HIDAPI_DriverSwitch_ScaleAccel(values[0]);
+ data[0] = -(ctx->m_IMUScaleData.fAccelScaleY * (float)values[1]);
+ data[1] = ctx->m_IMUScaleData.fAccelScaleZ * (float)values[2];
+ data[2] = -(ctx->m_IMUScaleData.fAccelScaleX * (float)values[0]);
}
/* Right Joy-Con flips some axes, so let's flip them back for consistency */