From e960bf6904551e75eadcae80a948d2c4e83a74d6 Mon Sep 17 00:00:00 2001
From: Aubrey Hesselgren <[EMAIL REDACTED]>
Date: Thu, 26 Jun 2025 19:56:06 -0700
Subject: [PATCH] Gyro instrumentation for test controller (#13287)
This adds several minor changes to the gyro instruments.
* The HID Sensor Time display is now throttled to 10hz.
* Calibration for the gyro is now time based, not sample count based. Different polling rates will have drift calibrated over the same space of time.
* Pitch/Yaw/Roll readout: Yaw is prioritized, and then pitch, and then roll. This gives a more human-readable pitch/yaw/roll display, closely matching game engines.
* Pitch/Yaw/Roll text is colorized to match the axes in the 3D gizmo.
* Added set of axes to the 3D gizmo to show the "Left Hand Space" positive axis directions.
---
test/gamepadutils.c | 73 +++++++++++++++++++++-----
test/testcontroller.c | 117 ++++++++++++++++++++++--------------------
2 files changed, 121 insertions(+), 69 deletions(-)
diff --git a/test/gamepadutils.c b/test/gamepadutils.c
index 68d8e448fd0f5..c5295c4c54dd0 100644
--- a/test/gamepadutils.c
+++ b/test/gamepadutils.c
@@ -101,8 +101,9 @@ static SDL_FPoint ProjectVec3ToRect(const Vector3 *v, const SDL_FRect *rect)
float fovScaleX = fovScaleY * aspect;
float relZ = cameraZ - v->z;
- if (relZ < 0.01f)
+ if (relZ < 0.01f) {
relZ = 0.01f; /* Prevent division by 0 or negative depth */
+ }
float ndc_x = (v->x / relZ) / fovScaleX;
float ndc_y = (v->y / relZ) / fovScaleY;
@@ -207,6 +208,39 @@ void DrawGyroDebugCircle(SDL_Renderer *renderer, const Quaternion *orientation,
SDL_SetRenderDrawColor(renderer, r, g, b, a);
}
+
+void DrawGyroDebugAxes(SDL_Renderer *renderer, const Quaternion *orientation, const SDL_FRect *bounds)
+{
+ /* Store current color */
+ Uint8 r, g, b, a;
+ SDL_GetRenderDrawColor(renderer, &r, &g, &b, &a);
+
+ Vector3 origin = { 0.0f, 0.0f, 0.0f };
+
+ Vector3 right = { 1.0f, 0.0f, 0.0f };
+ Vector3 up = { 0.0f, 1.0f, 0.0f };
+ Vector3 back = { 0.0f, 0.0f, 1.0f };
+
+ Vector3 world_right = RotateVectorByQuaternion(&right, orientation);
+ Vector3 world_up = RotateVectorByQuaternion(&up, orientation);
+ Vector3 world_back = RotateVectorByQuaternion(&back, orientation);
+
+ SDL_FPoint origin_screen = ProjectVec3ToRect(&origin, bounds);
+ SDL_FPoint right_screen = ProjectVec3ToRect(&world_right, bounds);
+ SDL_FPoint up_screen = ProjectVec3ToRect(&world_up, bounds);
+ SDL_FPoint back_screen = ProjectVec3ToRect(&world_back, bounds);
+
+ SDL_SetRenderDrawColor(renderer, GYRO_COLOR_RED);
+ SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, right_screen.x, right_screen.y);
+ SDL_SetRenderDrawColor(renderer, GYRO_COLOR_GREEN);
+ SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, up_screen.x, up_screen.y);
+ SDL_SetRenderDrawColor(renderer, GYRO_COLOR_BLUE);
+ SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, back_screen.x, back_screen.y);
+
+ /* Restore current color */
+ SDL_SetRenderDrawColor(renderer, r, g, b, a);
+}
+
void DrawAccelerometerDebugArrow(SDL_Renderer *renderer, const Quaternion *gyro_quaternion, const float *accel_data, const SDL_FRect *bounds)
{
/* Store current color */
@@ -990,6 +1024,8 @@ struct GyroDisplay
/* This part displays extra info from the IMUstate in order to figure out actual polling rates. */
float gyro_drift_solution[3];
int reported_sensor_rate_hz; /*hz - comes from HIDsdl implementation. Could be fixed, platform time, or true sensor time*/
+ Uint64 next_reported_sensor_time; /* SDL ticks used to throttle the display */
+
int estimated_sensor_rate_hz; /*hz - our estimation of the actual polling rate by observing packets received*/
float euler_displacement_angles[3]; /* pitch, yaw, roll */
Quaternion gyro_quaternion; /* Rotation since startup/reset, comprised of each gyro speed packet times sensor delta time. */
@@ -1009,7 +1045,8 @@ GyroDisplay *CreateGyroDisplay(SDL_Renderer *renderer)
SDL_zeroa(ctx->gyro_drift_solution);
Quaternion quat_identity = { 0.0f, 0.0f, 0.0f, 1.0f };
ctx->gyro_quaternion = quat_identity;
-
+ ctx->reported_sensor_rate_hz = 0;
+ ctx->next_reported_sensor_time = 0;
ctx->reset_gyro_button = CreateGamepadButton(renderer, "Reset View");
ctx->calibrate_gyro_button = CreateGamepadButton(renderer, "Recalibrate Drift");
}
@@ -1024,7 +1061,6 @@ void SetGyroDisplayArea(GyroDisplay *ctx, const SDL_FRect *area)
}
SDL_copyp(&ctx->area, area);
-
/* Place the reset button to the bottom right of the gyro display area.*/
SDL_FRect reset_button_area;
reset_button_area.w = SDL_max(MINIMUM_BUTTON_WIDTH, GetGamepadButtonLabelWidth(ctx->reset_gyro_button) + 2 * BUTTON_PADDING);
@@ -1340,12 +1376,17 @@ void SetGamepadDisplayIMUValues(GyroDisplay *ctx, float *gyro_drift_solution, fl
return;
}
- SDL_memcpy(ctx->gyro_drift_solution, gyro_drift_solution, sizeof(ctx->gyro_drift_solution));
- ctx->estimated_sensor_rate_hz = estimated_sensor_rate_hz;
-
- if (reported_senor_rate_hz != 0)
- ctx->reported_sensor_rate_hz = reported_senor_rate_hz;
+ const int SENSOR_UPDATE_INTERVAL_MS = 100;
+ Uint64 now = SDL_GetTicks();
+ if (now > ctx->next_reported_sensor_time) {
+ ctx->estimated_sensor_rate_hz = estimated_sensor_rate_hz;
+ if (reported_senor_rate_hz != 0) {
+ ctx->reported_sensor_rate_hz = reported_senor_rate_hz;
+ }
+ ctx->next_reported_sensor_time = now + SENSOR_UPDATE_INTERVAL_MS;
+ }
+ SDL_memcpy(ctx->gyro_drift_solution, gyro_drift_solution, sizeof(ctx->gyro_drift_solution));
SDL_memcpy(ctx->euler_displacement_angles, euler_displacement_angles, sizeof(ctx->euler_displacement_angles));
ctx->gyro_quaternion = *gyro_quaternion;
ctx->drift_calibration_progress_frac = drift_calibration_progress_frac;
@@ -1637,7 +1678,7 @@ void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad)
SDLTest_DrawString(ctx->renderer, x + center - SDL_strlen(text) * FONT_CHARACTER_SIZE, y, text);
SDL_snprintf(text, sizeof(text), "[%.2f,%.2f,%.2f]%s/s", ctx->gyro_data[0] * RAD_TO_DEG, ctx->gyro_data[1] * RAD_TO_DEG, ctx->gyro_data[2] * RAD_TO_DEG, DEGREE_UTF8);
SDLTest_DrawString(ctx->renderer, x + center + 2.0f, y, text);
-
+
/* Display the testcontroller tool's evaluation of drift. This is also useful to get an average rate of turn in calibrated turntable tests. */
if (ctx->gyro_drift_correction_data[0] != 0.0f && ctx->gyro_drift_correction_data[2] != 0.0f && ctx->gyro_drift_correction_data[2] != 0.0f )
@@ -1648,10 +1689,7 @@ void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad)
SDL_snprintf(text, sizeof(text), "[%.2f,%.2f,%.2f]%s/s", ctx->gyro_drift_correction_data[0] * RAD_TO_DEG, ctx->gyro_drift_correction_data[1] * RAD_TO_DEG, ctx->gyro_drift_correction_data[2] * RAD_TO_DEG, DEGREE_UTF8);
SDLTest_DrawString(ctx->renderer, x + center + 2.0f, y, text);
}
-
}
-
-
}
}
SDL_free(mapping);
@@ -1797,7 +1835,6 @@ void RenderGyroDriftCalibrationButton(GyroDisplay *ctx, GamepadDisplay *gamepad_
/* Set the color based on the drift calibration progress fraction */
SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_GREEN); /* red when too much noise, green when low noise*/
-
/* Now draw the bars with the filled, then empty rectangles */
SDL_RenderFillRect(ctx->renderer, &progress_bar_fill); /* draw the filled rectangle*/
SDL_SetRenderDrawColor(ctx->renderer, 100, 100, 100, 255); /* gray box*/
@@ -1823,20 +1860,26 @@ float RenderEulerReadout(GyroDisplay *ctx, GamepadDisplay *gamepad_display )
const float new_line_height = gamepad_display->button_height + 2.0f;
float log_gyro_euler_text_x = gyro_calibrate_button_rect.x;
+ Uint8 r, g, b, a;
+ SDL_GetRenderDrawColor(ctx->renderer, &r, &g, &b, &a);
/* Pitch Readout */
+ SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_RED);
SDL_snprintf(text, sizeof(text), "Pitch: %6.2f%s", ctx->euler_displacement_angles[0], DEGREE_UTF8);
SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text);
/* Yaw Readout */
+ SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_GREEN);
log_y += new_line_height;
SDL_snprintf(text, sizeof(text), " Yaw: %6.2f%s", ctx->euler_displacement_angles[1], DEGREE_UTF8);
SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text);
/* Roll Readout */
+ SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_BLUE);
log_y += new_line_height;
SDL_snprintf(text, sizeof(text), " Roll: %6.2f%s", ctx->euler_displacement_angles[2], DEGREE_UTF8);
SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text);
+ SDL_SetRenderDrawColor(ctx->renderer, r, g, b, a);
return log_y + new_line_height; /* Return the next y position for further rendering */
}
@@ -1859,6 +1902,9 @@ void RenderGyroGizmo(GyroDisplay *ctx, SDL_Gamepad *gamepad, float top)
/* Draw the rotated cube */
DrawGyroDebugCube(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect);
+ /* Draw positive axes */
+ DrawGyroDebugAxes(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect);
+
/* Overlay the XYZ circles */
DrawGyroDebugCircle(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect);
@@ -1906,7 +1952,6 @@ void RenderGyroDisplay(GyroDisplay *ctx, GamepadDisplay *gamepadElements, SDL_Ga
if (bHasCachedDriftSolution) {
float bottom = RenderEulerReadout(ctx, gamepadElements);
RenderGyroGizmo(ctx, gamepad, bottom);
-
}
SDL_SetRenderDrawColor(ctx->renderer, r, g, b, a);
}
diff --git a/test/testcontroller.c b/test/testcontroller.c
index 7602253f86a6d..0530d1a992e3e 100644
--- a/test/testcontroller.c
+++ b/test/testcontroller.c
@@ -53,62 +53,59 @@ struct Quaternion
static Quaternion quat_identity = { 0.0f, 0.0f, 0.0f, 1.0f };
-Quaternion QuaternionFromEuler(float roll, float pitch, float yaw)
+Quaternion QuaternionFromEuler(float pitch, float yaw, float roll)
{
- Quaternion q;
+ float cx = SDL_cosf(pitch * 0.5f);
+ float sx = SDL_sinf(pitch * 0.5f);
float cy = SDL_cosf(yaw * 0.5f);
float sy = SDL_sinf(yaw * 0.5f);
- float cp = SDL_cosf(pitch * 0.5f);
- float sp = SDL_sinf(pitch * 0.5f);
- float cr = SDL_cosf(roll * 0.5f);
- float sr = SDL_sinf(roll * 0.5f);
+ float cz = SDL_cosf(roll * 0.5f);
+ float sz = SDL_sinf(roll * 0.5f);
- q.w = cr * cp * cy + sr * sp * sy;
- q.x = sr * cp * cy - cr * sp * sy;
- q.y = cr * sp * cy + sr * cp * sy;
- q.z = cr * cp * sy - sr * sp * cy;
+ Quaternion q;
+ q.w = cx * cy * cz + sx * sy * sz;
+ q.x = sx * cy * cz - cx * sy * sz;
+ q.y = cx * sy * cz + sx * cy * sz;
+ q.z = cx * cy * sz - sx * sy * cz;
return q;
}
-static void EulerFromQuaternion(Quaternion q, float *roll, float *pitch, float *yaw)
-{
- float sinr_cosp = 2.0f * (q.w * q.x + q.y * q.z);
- float cosr_cosp = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
- float roll_rad = SDL_atan2f(sinr_cosp, cosr_cosp);
+#define RAD_TO_DEG (180.0f / SDL_PI_F)
- float sinp = 2.0f * (q.w * q.y - q.z * q.x);
- float pitch_rad;
- if (SDL_fabsf(sinp) >= 1.0f) {
- pitch_rad = SDL_copysignf(SDL_PI_F / 2.0f, sinp);
- } else {
- pitch_rad = SDL_asinf(sinp);
- }
+/* Decomposes quaternion into Yaw (Y), Pitch (X), Roll (Z) using Y-X-Z order in a left-handed system */
+void QuaternionToYXZ(Quaternion q, float *pitch, float *yaw, float *roll)
+{
+ /* Precalculate repeated expressions */
+ float qxx = q.x * q.x;
+ float qyy = q.y * q.y;
+ float qzz = q.z * q.z;
- float siny_cosp = 2.0f * (q.w * q.z + q.x * q.y);
- float cosy_cosp = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
- float yaw_rad = SDL_atan2f(siny_cosp, cosy_cosp);
+ float qxy = q.x * q.y;
+ float qxz = q.x * q.z;
+ float qyz = q.y * q.z;
+ float qwx = q.w * q.x;
+ float qwy = q.w * q.y;
+ float qwz = q.w * q.z;
- if (roll)
- *roll = roll_rad;
- if (pitch)
- *pitch = pitch_rad;
- if (yaw)
- *yaw = yaw_rad;
-}
+ /* Yaw (around Y) */
+ if (yaw) {
+ *yaw = SDL_atan2f(2.0f * (qwy + qxz), 1.0f - 2.0f * (qyy + qzz)) * RAD_TO_DEG;
+ }
-static void EulerDegreesFromQuaternion(Quaternion q, float *pitch, float *yaw, float *roll)
-{
- float pitch_rad, yaw_rad, roll_rad;
- EulerFromQuaternion(q, &pitch_rad, &yaw_rad, &roll_rad);
+ /* Pitch (around X) */
+ float sinp = 2.0f * (qwx - qyz);
if (pitch) {
- *pitch = pitch_rad * (180.0f / SDL_PI_F);
- }
- if (yaw) {
- *yaw = yaw_rad * (180.0f / SDL_PI_F);
+ if (SDL_fabsf(sinp) >= 1.0f) {
+ *pitch = SDL_copysignf(90.0f, sinp); /* Clamp to avoid domain error */
+ } else {
+ *pitch = SDL_asinf(sinp) * RAD_TO_DEG;
+ }
}
+
+ /* Roll (around Z) */
if (roll) {
- *roll = roll_rad * (180.0f / SDL_PI_F);
+ *roll = SDL_atan2f(2.0f * (qwz + qxy), 1.0f - 2.0f * (qxx + qzz)) * RAD_TO_DEG;
}
}
@@ -1375,7 +1372,16 @@ static void HandleGamepadGyroEvent(SDL_Event *event)
SDL_memcpy(controller->imu_state->gyro_data, event->gsensor.data, sizeof(controller->imu_state->gyro_data));
}
+/* Two strategies for evaluating polling rate - one based on a fixed packet count, and one using a fixed time window.
+ * Smaller values in either will give you a more responsive polling rate estimate, but this may fluctuate more.
+ * Larger values in either will give you a more stable average but they will require more time to evaluate.
+ * Generally, wired connections tend to give much more stable
+ */
+/* #define SDL_USE_FIXED_PACKET_COUNT_FOR_ESTIMATION */
#define SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT 2048
+#define SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_TIME_NS (SDL_NS_PER_SECOND * 2)
+
+
static void EstimatePacketRate()
{
Uint64 now_ns = SDL_GetTicksNS();
@@ -1384,17 +1390,22 @@ static void EstimatePacketRate()
}
/* Require a significant sample size before averaging rate. */
+#ifdef SDL_USE_FIXED_PACKET_COUNT_FOR_ESTIMATION
if (controller->imu_state->imu_packet_counter >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT) {
Uint64 deltatime_ns = now_ns - controller->imu_state->starting_time_stamp_ns;
- controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * 1000000000ULL) / deltatime_ns);
+ controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * SDL_NS_PER_SECOND) / deltatime_ns);
+ controller->imu_state->imu_packet_counter = 0;
}
-
- /* Flush sampled data after a brief period so that the imu_estimated_sensor_rate value can be read.*/
- if (controller->imu_state->imu_packet_counter >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT * 2) {
- controller->imu_state->starting_time_stamp_ns = now_ns;
+#else
+ Uint64 deltatime_ns = now_ns - controller->imu_state->starting_time_stamp_ns;
+ if (deltatime_ns >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_TIME_NS) {
+ controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * SDL_NS_PER_SECOND) / deltatime_ns);
controller->imu_state->imu_packet_counter = 0;
}
- ++controller->imu_state->imu_packet_counter;
+#endif
+ else {
+ ++controller->imu_state->imu_packet_counter;
+ }
}
static void UpdateGamepadOrientation( Uint64 delta_time_ns )
@@ -1409,13 +1420,11 @@ static void UpdateGamepadOrientation( Uint64 delta_time_ns )
static void HandleGamepadSensorEvent( SDL_Event* event )
{
- if (!controller) {
- return;
- }
+ if (!controller)
+ return;
- if (controller->id != event->gsensor.which) {
+ if (controller->id != event->gsensor.which)
return;
- }
if (event->gsensor.sensor == SDL_SENSOR_GYRO) {
HandleGamepadGyroEvent(event);
@@ -1428,13 +1437,12 @@ static void HandleGamepadSensorEvent( SDL_Event* event )
accelerometer and gyro events are received before progressing.
*/
if ( controller->imu_state->accelerometer_packet_number == controller->imu_state->gyro_packet_number ) {
-
EstimatePacketRate();
Uint64 sensorTimeStampDelta_ns = event->gsensor.sensor_timestamp - controller->imu_state->last_sensor_time_stamp_ns ;
UpdateGamepadOrientation(sensorTimeStampDelta_ns);
float display_euler_angles[3];
- EulerDegreesFromQuaternion(controller->imu_state->integrated_rotation, &display_euler_angles[0], &display_euler_angles[1], &display_euler_angles[2]);
+ QuaternionToYXZ(controller->imu_state->integrated_rotation, &display_euler_angles[0], &display_euler_angles[1], &display_euler_angles[2]);
float drift_calibration_progress_frac = controller->imu_state->gyro_drift_sample_count / (float)SDL_GAMEPAD_IMU_MIN_GYRO_DRIFT_SAMPLE_COUNT;
int reported_polling_rate_hz = sensorTimeStampDelta_ns > 0 ? (int)(SDL_NS_PER_SECOND / sensorTimeStampDelta_ns) : 0;
@@ -2073,7 +2081,6 @@ SDL_AppResult SDLCALL SDL_AppEvent(void *appstate, SDL_Event *event)
event->gsensor.data[1],
event->gsensor.data[2],
event->gsensor.sensor_timestamp);
-
#endif /* VERBOSE_SENSORS */
HandleGamepadSensorEvent(event);
break;