From f0d4aca03d9bc682e188a61257a5f320420a5958 Mon Sep 17 00:00:00 2001
From: Vicki Pfau <[EMAIL REDACTED]>
Date: Wed, 12 Nov 2025 16:46:32 -0800
Subject: [PATCH] switch2: Bring up IMU support
---
src/joystick/hidapi/SDL_hidapi_switch2.c | 156 ++++++++++++++++++++++-
1 file changed, 155 insertions(+), 1 deletion(-)
diff --git a/src/joystick/hidapi/SDL_hidapi_switch2.c b/src/joystick/hidapi/SDL_hidapi_switch2.c
index eadb24e2e380d..7fbb95d7b1c61 100644
--- a/src/joystick/hidapi/SDL_hidapi_switch2.c
+++ b/src/joystick/hidapi/SDL_hidapi_switch2.c
@@ -113,6 +113,19 @@ typedef struct
Uint8 left_trigger_zero;
Uint8 right_trigger_zero;
+ float gyro_bias_x;
+ float gyro_bias_y;
+ float gyro_bias_z;
+ float accel_bias_x;
+ float accel_bias_y;
+ float accel_bias_z;
+ bool sensors_enabled;
+ bool sensors_ready;
+ int sample_count;
+ Uint64 first_sensor_timestamp;
+ Uint64 sensor_ts_coeff;
+ float gyro_coeff;
+
bool player_lights;
int player_index;
@@ -436,6 +449,15 @@ static bool HIDAPI_DriverSwitch2_InitUSB(SDL_HIDAPI_Device *device)
HIDAPI_SetDeviceSerial(device, serial);
}
+ res = ReadFlashBlock(ctx, 0x13040, calibration_data);
+ if (res < 0) {
+ SDL_LogWarn(SDL_LOG_CATEGORY_INPUT, "Couldn't read factory calibration data: %d", res);
+ } else {
+ ctx->gyro_bias_x = *(float*)&calibration_data[4];
+ ctx->gyro_bias_y = *(float*)&calibration_data[8];
+ ctx->gyro_bias_z = *(float*)&calibration_data[12];
+ }
+
res = ReadFlashBlock(ctx, 0x13080, calibration_data);
if (res < 0) {
SDL_LogWarn(SDL_LOG_CATEGORY_INPUT, "Couldn't read factory calibration data: %d", res);
@@ -450,6 +472,15 @@ static bool HIDAPI_DriverSwitch2_InitUSB(SDL_HIDAPI_Device *device)
ParseStickCalibration(&ctx->right_stick, &calibration_data[0x28]);
}
+ res = ReadFlashBlock(ctx, 0x13100, calibration_data);
+ if (res < 0) {
+ SDL_LogWarn(SDL_LOG_CATEGORY_INPUT, "Couldn't read factory calibration data: %d", res);
+ } else {
+ ctx->accel_bias_x = *(float*)&calibration_data[12];
+ ctx->accel_bias_y = *(float*)&calibration_data[16];
+ ctx->accel_bias_z = *(float*)&calibration_data[20];
+ }
+
if (device->product_id == USB_PRODUCT_NINTENDO_SWITCH2_GAMECUBE_CONTROLLER) {
res = ReadFlashBlock(ctx, 0x13140, calibration_data);
if (res < 0) {
@@ -506,6 +537,9 @@ static bool HIDAPI_DriverSwitch2_InitDevice(SDL_HIDAPI_Device *device)
}
}
+ ctx->sensor_ts_coeff = 10000;
+ ctx->gyro_coeff = 34.8f;
+
// Sometimes the device handle isn't available during enumeration so we don't get the device name, so set it explicitly
switch (device->product_id) {
case USB_PRODUCT_NINTENDO_SWITCH2_GAMECUBE_CONTROLLER:
@@ -553,12 +587,26 @@ static bool HIDAPI_DriverSwitch2_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joy
SDL_PlayerLEDHintChanged, ctx);
// Initialize the joystick capabilities
+ if (!ctx->device->parent) {
+ SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO, 250.0f);
+ SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL, 250.0f);
+ }
switch (device->product_id) {
case USB_PRODUCT_NINTENDO_SWITCH2_GAMECUBE_CONTROLLER:
joystick->nbuttons = SDL_GAMEPAD_NUM_SWITCH2_GAMECUBE_BUTTONS;
break;
case USB_PRODUCT_NINTENDO_SWITCH2_JOYCON_LEFT:
+ if (ctx->device->parent) {
+ SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO_L, 250.0f);
+ SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL_L, 250.0f);
+ }
+ joystick->nbuttons = SDL_GAMEPAD_NUM_SWITCH2_JOYCON_BUTTONS;
+ break;
case USB_PRODUCT_NINTENDO_SWITCH2_JOYCON_RIGHT:
+ if (ctx->device->parent) {
+ SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO_R, 250.0f);
+ SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL_R, 250.0f);
+ }
joystick->nbuttons = SDL_GAMEPAD_NUM_SWITCH2_JOYCON_BUTTONS;
break;
case USB_PRODUCT_NINTENDO_SWITCH2_PRO:
@@ -621,7 +669,24 @@ static bool HIDAPI_DriverSwitch2_SendJoystickEffect(SDL_HIDAPI_Device *device, S
static bool HIDAPI_DriverSwitch2_SetJoystickSensorsEnabled(SDL_HIDAPI_Device *device, SDL_Joystick *joystick, bool enabled)
{
- return SDL_Unsupported();
+ SDL_DriverSwitch2_Context *ctx = (SDL_DriverSwitch2_Context *)device->context;
+ if (ctx->sensors_ready) {
+ Uint8 data[] = {
+ 0x0c, 0x91, 0x00, 0x04, 0x00, 0x04, 0x00, 0x00, 0x23, 0x00, 0x00, 0x00
+ };
+ unsigned char reply[12] = {0};
+
+ if (enabled) {
+ data[8] |= 4;
+ }
+ int res = SendBulkData(ctx, data, sizeof(data));
+ if (res < 0) {
+ return SDL_SetError("Couldn't set sensors enabled: %d\n", res);
+ }
+ RecvBulkData(ctx, reply, sizeof(reply));
+ }
+ ctx->sensors_enabled = true;
+ return true;
}
static void HandleGameCubeState(Uint64 timestamp, SDL_Joystick *joystick, SDL_DriverSwitch2_Context *ctx, Uint8 *data, int size)
@@ -1092,6 +1157,95 @@ static void HIDAPI_DriverSwitch2_HandleStatePacket(SDL_HIDAPI_Device *device, SD
break;
}
+ Uint64 sensor_timestamp = (Uint32) (data[0x2b] | (data[0x2c] << 8U) | (data[0x2d] << 16U) | (data[0x2e] << 24U));
+ if (sensor_timestamp && !ctx->sensors_ready) {
+ ctx->sample_count++;
+ if (ctx->sample_count >= 5 && !ctx->first_sensor_timestamp) {
+ ctx->first_sensor_timestamp = sensor_timestamp;
+ ctx->sample_count = 0;
+ } else if (ctx->sample_count == 100) {
+ // Calculate timestamp coefficient
+ // Timestamp are normally microseconds but sometimes it's something else for no apparent reason
+ Uint64 coeff = 1000 * (sensor_timestamp - ctx->first_sensor_timestamp) / (ctx->sample_count * 4);
+ if ((coeff + 100000) / 200000 == 5) {
+ // Within 10% of 1000
+ ctx->sensor_ts_coeff = 10000;
+ ctx->gyro_coeff = 34.8f;
+ ctx->sensors_ready = true;
+ } else {
+ ctx->sensor_ts_coeff = 10000000000 / coeff;
+ ctx->gyro_coeff = 40.0f;
+ ctx->sensors_ready = true;
+ }
+
+ if (ctx->sensors_ready && !ctx->sensors_enabled) {
+ Uint8 set_features[] = {
+ 0x0c, 0x91, 0x00, 0x04, 0x00, 0x04, 0x00, 0x00, 0x23, 0x00, 0x00, 0x00
+ };
+ unsigned char reply[12] = {0};
+
+ SendBulkData(ctx, set_features, sizeof(set_features));
+ RecvBulkData(ctx, reply, sizeof(reply));
+ }
+ }
+ }
+ if (ctx->sensors_enabled && sensor_timestamp && ctx->sensors_ready) {
+ sensor_timestamp = sensor_timestamp * ctx->sensor_ts_coeff / 10;
+ float accel_data[3];
+ float gyro_data[3];
+ const float g = 9.80665f;
+ const float accel_scale = g * 8.f / INT16_MAX;
+
+ accel_data[0] = (Sint16)(data[0x31] | (data[0x32] << 8)) * accel_scale;
+ accel_data[1] = (Sint16)(data[0x35] | (data[0x36] << 8)) * accel_scale;
+ accel_data[2] = (Sint16)(data[0x33] | (data[0x34] << 8)) * -accel_scale;
+
+ gyro_data[0] = (Sint16)(data[0x37] | (data[0x38] << 8)) * ctx->gyro_coeff / INT16_MAX - ctx->gyro_bias_x;
+ gyro_data[1] = (Sint16)(data[0x3b] | (data[0x3c] << 8)) * ctx->gyro_coeff / INT16_MAX - ctx->gyro_bias_z;
+ gyro_data[2] = (Sint16)(data[0x39] | (data[0x3a] << 8)) * -ctx->gyro_coeff / INT16_MAX + ctx->gyro_bias_y;
+
+ switch (ctx->device->product_id) {
+ case USB_PRODUCT_NINTENDO_SWITCH2_JOYCON_LEFT:
+ if (ctx->device->parent) {
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_ACCEL_L, sensor_timestamp, accel_data, 3);
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_GYRO_L, sensor_timestamp, gyro_data, 3);
+ } else {
+ float tmp = -accel_data[0];
+ accel_data[0] = accel_data[2];
+ accel_data[2] = tmp;
+
+ tmp = -gyro_data[0];
+ gyro_data[0] = gyro_data[2];
+ gyro_data[2] = tmp;
+
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_ACCEL, sensor_timestamp, accel_data, 3);
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_GYRO, sensor_timestamp, gyro_data, 3);
+ }
+ break;
+ case USB_PRODUCT_NINTENDO_SWITCH2_JOYCON_RIGHT:
+ if (ctx->device->parent) {
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_ACCEL_R, sensor_timestamp, accel_data, 3);
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_GYRO_R, sensor_timestamp, gyro_data, 3);
+ } else {
+ float tmp = accel_data[0];
+ accel_data[0] = -accel_data[2];
+ accel_data[2] = tmp;
+
+ tmp = gyro_data[0];
+ gyro_data[0] = -gyro_data[2];
+ gyro_data[2] = tmp;
+
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_ACCEL, sensor_timestamp, accel_data, 3);
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_GYRO, sensor_timestamp, gyro_data, 3);
+ }
+ break;
+ default:
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_ACCEL, sensor_timestamp, accel_data, 3);
+ SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_GYRO, sensor_timestamp, gyro_data, 3);
+ break;
+ }
+ }
+
SDL_memcpy(ctx->last_state, data, SDL_min(size, sizeof(ctx->last_state)));
}