encounter
/
SDL
mirror of https://github.com/encounter/SDL.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Jibb Smart 2022-04-27 00:57:17 +08:00 committed by GitHub
parent 76afb8583b
commit d7c07d6b09
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 100 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 */