Skip to content

Commit

Permalink
Read motion sensor scale from Switch controllers (#5555)
Browse files Browse the repository at this point in the history 
* 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
  • Loading branch information
@JibbSmart
JibbSmart committed Apr 26, 2022
1 parent 76afb85 commit d7c07d6
Showing 1 changed file with 100 additions and 20 deletions.
120 changes: 100 additions & 20 deletions src/joystick/hidapi/SDL_hidapi_switch.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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;


Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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 */
Expand Down

0 comments on commit d7c07d6

Please sign in to comment.