From 60fb1b55417a2955e8a5d31d1c482d6c1cbab7ed Mon Sep 17 00:00:00 2001
From: Aubrey Hesselgren <[EMAIL REDACTED]>
Date: Wed, 9 Apr 2025 10:46:41 -0700
Subject: [PATCH] Fix IMU orientations for 8bitdo wireless 2 controller in
bluetooth mode
Fix IMU timing stamp - simulate the timing stamp from the controller hardware's perspective, as opposed to the receiving computer's perspective. Do this by advancing a time stamp by a fixed rate, based on observation of average rate over bluetooth.
---
src/joystick/hidapi/SDL_hidapi_8bitdo.c | 45 ++++++++++++++++++-------
1 file changed, 33 insertions(+), 12 deletions(-)
diff --git a/src/joystick/hidapi/SDL_hidapi_8bitdo.c b/src/joystick/hidapi/SDL_hidapi_8bitdo.c
index 9b64657a9d1f0..ca60118006137 100644
--- a/src/joystick/hidapi/SDL_hidapi_8bitdo.c
+++ b/src/joystick/hidapi/SDL_hidapi_8bitdo.c
@@ -42,9 +42,8 @@ enum
SDL_GAMEPAD_NUM_8BITDO_BUTTONS,
};
-#define ABITDO_GYRO_SCALE 14.2842f
#define ABITDO_ACCEL_SCALE 4096.f
-
+#define SENSOR_INTERVAL_NS 8000000ULL
typedef struct
{
@@ -63,6 +62,7 @@ typedef struct
float accelScale;
float gyroScale;
Uint8 last_state[USB_PACKET_LENGTH];
+ Uint64 sensor_timestamp; // Microseconds. Simulate onboard clock. Advance by known rate: SENSOR_INTERVAL_NS == 8ms = 125 Hz
} SDL_Driver8BitDo_Context;
#pragma pack(push,1)
@@ -149,6 +149,10 @@ static void HIDAPI_Driver8BitDo_SetDevicePlayerIndex(SDL_HIDAPI_Device *device,
{
}
+#ifndef DEG2RAD
+#define DEG2RAD(x) ((float)(x) * (float)(SDL_PI_F / 180.f))
+#endif
+
static bool HIDAPI_Driver8BitDo_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joystick)
{
SDL_Driver8BitDo_Context *ctx = (SDL_Driver8BitDo_Context *)device->context;
@@ -163,12 +167,12 @@ static bool HIDAPI_Driver8BitDo_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joys
joystick->nhats = 1;
if (ctx->sensors_supported) {
- SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO, 250.0f);
- SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL, 250.0f);
+ SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO, 125.0f);
+ SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL, 125.0f);
ctx->accelScale = SDL_STANDARD_GRAVITY / ABITDO_ACCEL_SCALE;
- ctx->gyroScale = SDL_PI_F / 180.0f / ABITDO_GYRO_SCALE;
+ ctx->gyroScale = DEG2RAD(2048) / INT16_MAX; // Hardware senses +/- 2048 Degrees per second mapped to +/- INT16_MAX
}
return true;
@@ -357,15 +361,32 @@ static void HIDAPI_Driver8BitDo_HandleStatePacket(SDL_Joystick *joystick, SDL_Dr
float values[3];
ABITDO_SENSORS *sensors = (ABITDO_SENSORS *)&data[15];
- sensor_timestamp = timestamp;
- values[0] = (sensors->sGyroX) * (ctx->gyroScale);
- values[1] = (sensors->sGyroZ) * (ctx->gyroScale);
- values[2] = (sensors->sGyroY) * (ctx->gyroScale);
+ // Note: we cannot use the time stamp of the receiving computer due to packet delay creating "spiky" timings.
+ // The imu time stamp is intended to be the sample time of the on-board hardware.
+ // In the absence of time stamp data from the data[], we can simulate that by
+ // advancing a time stamp by the observed/known imu clock rate. This is 8ms = 125 Hz
+ sensor_timestamp = ctx->sensor_timestamp;
+ ctx->sensor_timestamp += SENSOR_INTERVAL_NS;
+
+ // This device's IMU values are reported differently from SDL
+ // Thus we perform a rotation of the coordinate system to match the SDL standard.
+
+ // By observation of this device:
+ // Hardware x is reporting roll (rotation about the power jack's axis)
+ // Hardware y is reporting pitch (rotation about the horizontal axis)
+ // Hardware z is reporting yaw (rotation about the joysticks' center axis)
+ values[0] = -sensors->sGyroY * ctx->gyroScale; // Rotation around pitch axis
+ values[1] = sensors->sGyroZ * ctx->gyroScale; // Rotation around yaw axis
+ values[2] = -sensors->sGyroX * ctx->gyroScale; // Rotation around roll axis
SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_GYRO, sensor_timestamp, values, 3);
- values[0] = (sensors->sAccelX) * (ctx->accelScale);
- values[1] = (sensors->sAccelZ) * (ctx->accelScale);
- values[2] = (sensors->sAccelY) * (ctx->accelScale);
+ // By observation of this device:
+ // Accelerometer X is positive when front of the controller points toward the sky.
+ // Accelerometer y is positive when left side of the controller points toward the sky.
+ // Accelerometer Z is positive when sticks point toward the sky.
+ values[0] = -sensors->sAccelY * ctx->accelScale; // Acceleration along pitch axis
+ values[1] = sensors->sAccelZ * ctx->accelScale; // Acceleration along yaw axis
+ values[2] = -sensors->sAccelX * ctx->accelScale; // Acceleration along roll axis
SDL_SendJoystickSensor(timestamp, joystick, SDL_SENSOR_ACCEL, sensor_timestamp, values, 3);
}