From 913b611ccd5acd671fd99462b3dbf8b2899060e5 Mon Sep 17 00:00:00 2001
From: Aubrey Hesselgren <[EMAIL REDACTED]>
Date: Fri, 13 Jun 2025 14:01:52 -0700
Subject: [PATCH] Added tools for evaluating gyroscope accuracy and IMU polling
rates. (#13209)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
* Added tools to Test Controller for evaluating gyroscope accuracy and IMU polling rates.
This adds a visual suite to the testcontroller tool to help validate IMU data from new gamepad drivers and HID implementations.
The 3D gizmo renders accumulated rotation using quaternion integration of gyroscope packets. If a controller is rotated 90° in real space, the gizmo should reflect a 90° change, allowing quick detection of incorrect sensitivity or misaligned axes.
Also includes:
- Euler angle readout (pitch, yaw, roll)
- Real-time drift calibration display with noise gating and progress
- Accelerometer vector overlay
- Live polling rate estimation to verify update frequency
Intended for developers working on controller firmware or SDL backend support to confirm correctness of IMU data processing.
---
test/gamepadutils.c | 623 +++++++++++++++++++++++++++++++++++++++++-
test/gamepadutils.h | 32 ++-
test/testcontroller.c | 363 +++++++++++++++++++++++-
3 files changed, 999 insertions(+), 19 deletions(-)
diff --git a/test/gamepadutils.c b/test/gamepadutils.c
index 94390cc2c025c..4c2590e7b4909 100644
--- a/test/gamepadutils.c
+++ b/test/gamepadutils.c
@@ -30,6 +30,217 @@
#include "gamepad_wired.h"
#include "gamepad_wireless.h"
+#include <limits.h>
+
+#define RAD_TO_DEG (180.0f / SDL_PI_F)
+
+/* Used to draw a 3D cube to represent the gyroscope orientation */
+typedef struct
+{
+ float x, y, z;
+} Vector3;
+
+struct Quaternion
+{
+ float x, y, z, w;
+};
+
+static const Vector3 debug_cube_vertices[] = {
+ { -1.0f, -1.0f, -1.0f },
+ { 1.0f, -1.0f, -1.0f },
+ { 1.0f, 1.0f, -1.0f },
+ { -1.0f, 1.0f, -1.0f },
+ { -1.0f, -1.0f, 1.0f },
+ { 1.0f, -1.0f, 1.0f },
+ { 1.0f, 1.0f, 1.0f },
+ { -1.0f, 1.0f, 1.0f },
+};
+
+static const int debug_cube_edges[][2] = {
+ { 0, 1 }, { 1, 2 }, { 2, 3 }, { 3, 0 }, /* bottom square */
+ { 4, 5 }, { 5, 6 }, { 6, 7 }, { 7, 4 }, /* top square */
+ { 0, 4 }, { 1, 5 }, { 2, 6 }, { 3, 7 }, /* verticals */
+};
+
+static Vector3 RotateVectorByQuaternion(const Vector3 *v, const Quaternion *q) {
+ /* v' = q * v * q^-1 */
+ float x = v->x, y = v->y, z = v->z;
+ float qx = q->x, qy = q->y, qz = q->z, qw = q->w;
+
+ /* Calculate quaternion *vector */
+ float ix = qw * x + qy * z - qz * y;
+ float iy = qw * y + qz * x - qx * z;
+ float iz = qw * z + qx * y - qy * x;
+ float iw = -qx * x - qy * y - qz * z;
+
+ /* Result = result * conjugate(q) */
+ Vector3 out;
+ out.x = ix * qw + iw * -qx + iy * -qz - iz * -qy;
+ out.y = iy * qw + iw * -qy + iz * -qx - ix * -qz;
+ out.z = iz * qw + iw * -qz + ix * -qy - iy * -qx;
+ return out;
+}
+
+#ifdef GYRO_ISOMETRIC_PROJECTION
+static SDL_FPoint ProjectVec3ToRect(const Vector3 *v, const SDL_FRect *rect)
+{
+ SDL_FPoint out;
+ /* Simple orthographic projection using X and Y; scale to fit into rect */
+ out.x = rect->x + (rect->w / 2.0f) + (v->x * (rect->w / 2.0f));
+ out.y = rect->y + (rect->h / 2.0f) - (v->y * (rect->h / 2.0f)); /* Y inverted */
+ return out;
+}
+#else
+static SDL_FPoint ProjectVec3ToRect(const Vector3 *v, const SDL_FRect *rect)
+{
+ const float verticalFOV_deg = 40.0f;
+ const float cameraZ = 4.0f; /* Camera is at(0, 0, +4), looking toward origin */
+ float aspect = rect->w / rect->h;
+
+ float fovScaleY = SDL_tanf((verticalFOV_deg * SDL_PI_F / 180.0f) * 0.5f);
+ float fovScaleX = fovScaleY * aspect;
+
+ float relZ = cameraZ - v->z;
+ 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;
+
+ /* Convert to screen space */
+ SDL_FPoint out;
+ out.x = rect->x + (rect->w / 2.0f) + (ndc_x * rect->w / 2.0f);
+ out.y = rect->y + (rect->h / 2.0f) - (ndc_y * rect->h / 2.0f); /* flip Y */
+ return out;
+}
+#endif
+
+void DrawGyroDebugCube(SDL_Renderer *renderer, const Quaternion *orientation, const SDL_FRect *rect)
+{
+ SDL_FPoint projected[8];
+ int i;
+ for (i = 0; i < 8; ++i) {
+ Vector3 rotated = RotateVectorByQuaternion(&debug_cube_vertices[i], orientation);
+ projected[i] = ProjectVec3ToRect(&rotated, rect);
+ }
+
+ for (i = 0; i < 12; ++i) {
+ const SDL_FPoint p0 = projected[debug_cube_edges[i][0]];
+ const SDL_FPoint p1 = projected[debug_cube_edges[i][1]];
+ SDL_RenderLine(renderer, p0.x, p0.y, p1.x, p1.y);
+ }
+}
+
+#define CIRCLE_SEGMENTS 64
+
+static Vector3 kCirclePoints3D_XY_Plane[CIRCLE_SEGMENTS];
+static Vector3 kCirclePoints3D_XZ_Plane[CIRCLE_SEGMENTS];
+static Vector3 kCirclePoints3D_YZ_Plane[CIRCLE_SEGMENTS];
+
+void InitCirclePoints3D(void)
+{
+ int i;
+ for (i = 0; i < CIRCLE_SEGMENTS; ++i) {
+ float theta = ((float)i / CIRCLE_SEGMENTS) * SDL_PI_F * 2.0f;
+ kCirclePoints3D_XY_Plane[i].x = SDL_cosf(theta);
+ kCirclePoints3D_XY_Plane[i].y = SDL_sinf(theta);
+ kCirclePoints3D_XY_Plane[i].z = 0.0f;
+ }
+
+ for (i = 0; i < CIRCLE_SEGMENTS; ++i) {
+ float theta = ((float)i / CIRCLE_SEGMENTS) * SDL_PI_F * 2.0f;
+ kCirclePoints3D_XZ_Plane[i].x = SDL_cosf(theta);
+ kCirclePoints3D_XZ_Plane[i].y = 0.0f;
+ kCirclePoints3D_XZ_Plane[i].z = SDL_sinf(theta);
+ }
+
+ for (i = 0; i < CIRCLE_SEGMENTS; ++i) {
+ float theta = ((float)i / CIRCLE_SEGMENTS) * SDL_PI_F * 2.0f;
+ kCirclePoints3D_YZ_Plane[i].x = 0.0f;
+ kCirclePoints3D_YZ_Plane[i].y = SDL_cosf(theta);
+ kCirclePoints3D_YZ_Plane[i].z = SDL_sinf(theta);
+ }
+}
+
+void DrawGyroCircle(
+ SDL_Renderer *renderer,
+ const Vector3 *circlePoints,
+ int numSegments,
+ const Quaternion *orientation,
+ const SDL_FRect *bounds,
+ Uint8 r, Uint8 g, Uint8 b, Uint8 a)
+{
+ SDL_SetRenderDrawColor(renderer, r, g, b, a);
+
+ SDL_FPoint lastScreenPt = { 0 };
+ bool hasLast = false;
+ int i;
+ for (i = 0; i <= numSegments; ++i) {
+ int index = i % numSegments;
+
+ Vector3 rotated = RotateVectorByQuaternion(&circlePoints[index], orientation);
+ SDL_FPoint screenPtVec2 = ProjectVec3ToRect(&rotated, bounds);
+ SDL_FPoint screenPt;
+ screenPt.x = screenPtVec2.x;
+ screenPt.y = screenPtVec2.y;
+
+
+ if (hasLast) {
+ SDL_RenderLine(renderer, lastScreenPt.x, lastScreenPt.y, screenPt.x, screenPt.y);
+ }
+
+ lastScreenPt = screenPt;
+ hasLast = true;
+ }
+}
+
+void DrawGyroDebugCircle(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);
+ DrawGyroCircle(renderer, kCirclePoints3D_YZ_Plane, CIRCLE_SEGMENTS, orientation, bounds, GYRO_COLOR_RED); /* X axis - pitch */
+ DrawGyroCircle(renderer, kCirclePoints3D_XZ_Plane, CIRCLE_SEGMENTS, orientation, bounds, GYRO_COLOR_GREEN); /* Y axis - yaw */
+ DrawGyroCircle(renderer, kCirclePoints3D_XY_Plane, CIRCLE_SEGMENTS, orientation, bounds, GYRO_COLOR_BLUE); /* Z axis - Roll */
+
+ /* 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 */
+ Uint8 r, g, b, a;
+ SDL_GetRenderDrawColor(renderer, &r, &g, &b, &a);
+
+ const float flGravity = 9.81f;
+ Vector3 vAccel;
+ vAccel.x = accel_data[0] / flGravity;
+ vAccel.y = accel_data[1] / flGravity;
+ vAccel.z = accel_data[2] / flGravity;
+
+ Vector3 origin = { 0.0f, 0.0f, 0.0f };
+ Vector3 rotated_accel = RotateVectorByQuaternion(&vAccel, gyro_quaternion);
+
+ /* Project the origin and rotated vector to screen space */
+ SDL_FPoint origin_screen = ProjectVec3ToRect(&origin, bounds);
+ SDL_FPoint accel_screen = ProjectVec3ToRect(&rotated_accel, bounds);
+
+ /* Draw the line from origin to the rotated accelerometer vector */
+ SDL_SetRenderDrawColor(renderer, GYRO_COLOR_ORANGE);
+ SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, accel_screen.x, accel_screen.y);
+
+ const float head_width = 4.0f;
+ SDL_FRect arrow_head_rect;
+ arrow_head_rect.x = accel_screen.x - head_width * 0.5f;
+ arrow_head_rect.y = accel_screen.y - head_width * 0.5f;
+ arrow_head_rect.w = head_width;
+ arrow_head_rect.h = head_width;
+ SDL_RenderRect(renderer, &arrow_head_rect);
+
+ /* Restore current color */
+ SDL_SetRenderDrawColor(renderer, r, g, b, a);
+}
/* This is indexed by gamepad element */
static const struct
@@ -683,7 +894,6 @@ void DestroyGamepadImage(GamepadImage *ctx)
}
}
-
static const char *gamepad_button_names[] = {
"South",
"East",
@@ -736,6 +946,8 @@ struct GamepadDisplay
float accel_data[3];
float gyro_data[3];
+ float gyro_drift_correction_data[3];
+
Uint64 last_sensor_update;
ControllerDisplayMode display_mode;
@@ -760,10 +972,68 @@ GamepadDisplay *CreateGamepadDisplay(SDL_Renderer *renderer)
ctx->element_highlighted = SDL_GAMEPAD_ELEMENT_INVALID;
ctx->element_selected = SDL_GAMEPAD_ELEMENT_INVALID;
+
+ SDL_zeroa(ctx->accel_data);
+ SDL_zeroa(ctx->gyro_data);
+ SDL_zeroa(ctx->gyro_drift_correction_data);
}
return ctx;
}
+struct GyroDisplay
+{
+ SDL_Renderer *renderer;
+
+ /* Main drawing area */
+ SDL_FRect area;
+
+ /* 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*/
+ 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. */
+ float drift_calibration_progress_frac; /* [0..1] */
+ float accelerometer_noise_sq; /* Distance between last noise and new noise. Used to indicate motion.*/
+
+ GamepadButton *reset_gyro_button;
+ GamepadButton *calibrate_gyro_button;
+};
+
+GyroDisplay *CreateGyroDisplay(SDL_Renderer *renderer)
+{
+ GyroDisplay *ctx = SDL_calloc(1, sizeof(*ctx));
+ {
+ ctx->renderer = renderer;
+ ctx->estimated_sensor_rate_hz = 0;
+ SDL_zeroa(ctx->gyro_drift_solution);
+ Quaternion quat_identity = { 0.0f, 0.0f, 0.0f, 1.0f };
+ ctx->gyro_quaternion = quat_identity;
+
+ ctx->reset_gyro_button = CreateGamepadButton(renderer, "Reset View");
+ ctx->calibrate_gyro_button = CreateGamepadButton(renderer, "Recalibrate Drift");
+ }
+
+ return ctx;
+}
+
+void SetGyroDisplayArea(GyroDisplay *ctx, const SDL_FRect *area)
+{
+ if (!ctx) {
+ return;
+ }
+
+ 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);
+ reset_button_area.h = GetGamepadButtonLabelHeight(ctx->reset_gyro_button) + BUTTON_PADDING;
+ reset_button_area.x = area->x + area->w - reset_button_area.w - BUTTON_PADDING;
+ reset_button_area.y = area->y + area->h - reset_button_area.h - BUTTON_PADDING;
+ SetGamepadButtonArea(ctx->reset_gyro_button, &reset_button_area);
+}
+
void SetGamepadDisplayDisplayMode(GamepadDisplay *ctx, ControllerDisplayMode display_mode)
{
if (!ctx) {
@@ -781,6 +1051,16 @@ void SetGamepadDisplayArea(GamepadDisplay *ctx, const SDL_FRect *area)
SDL_copyp(&ctx->area, area);
}
+void SetGamepadDisplayGyroDriftCorrection(GamepadDisplay *ctx, float *gyro_drift_correction)
+{
+ if (!ctx) {
+ return;
+ }
+
+ ctx->gyro_drift_correction_data[0] = gyro_drift_correction[0];
+ ctx->gyro_drift_correction_data[1] = gyro_drift_correction[1];
+ ctx->gyro_drift_correction_data[2] = gyro_drift_correction[2];
+}
static bool GetBindingString(const char *label, const char *mapping, char *text, size_t size)
{
@@ -1044,6 +1324,50 @@ static void RenderGamepadElementHighlight(GamepadDisplay *ctx, int element, cons
}
}
+bool BHasCachedGyroDriftSolution(GyroDisplay *ctx)
+{
+ if (!ctx) {
+ return false;
+ }
+ return (ctx->gyro_drift_solution[0] != 0.0f ||
+ ctx->gyro_drift_solution[1] != 0.0f ||
+ ctx->gyro_drift_solution[2] != 0.0f);
+}
+
+void SetGamepadDisplayIMUValues(GyroDisplay *ctx, float *gyro_drift_solution, float *euler_displacement_angles, Quaternion *gyro_quaternion, int reported_senor_rate_hz, int estimated_sensor_rate_hz, float drift_calibration_progress_frac, float accelerometer_noise_sq)
+{
+ if (!ctx) {
+ 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;
+
+ 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;
+ ctx->accelerometer_noise_sq = accelerometer_noise_sq;
+}
+
+extern GamepadButton *GetGyroResetButton(GyroDisplay *ctx)
+{
+ if (!ctx) {
+ return NULL;
+ }
+ return ctx->reset_gyro_button;
+}
+
+extern GamepadButton *GetGyroCalibrateButton(GyroDisplay *ctx)
+{
+ if (!ctx) {
+ return NULL;
+ }
+ return ctx->calibrate_gyro_button;
+}
+
void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad)
{
float x, y;
@@ -1285,8 +1609,10 @@ void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad)
has_accel = SDL_GamepadHasSensor(gamepad, SDL_SENSOR_ACCEL);
has_gyro = SDL_GamepadHasSensor(gamepad, SDL_SENSOR_GYRO);
+
if (has_accel || has_gyro) {
- const int SENSOR_UPDATE_INTERVAL_MS = 100;
+ const float gyro_sensor_rate = has_gyro ? SDL_GetGamepadSensorDataRate(gamepad, SDL_SENSOR_GYRO) : 0;
+ const int SENSOR_UPDATE_INTERVAL_MS = gyro_sensor_rate > 0.0f ? (int)( 1000.0f / gyro_sensor_rate ) : 100;
Uint64 now = SDL_GetTicks();
if (now >= ctx->last_sensor_update + SENSOR_UPDATE_INTERVAL_MS) {
@@ -1296,26 +1622,37 @@ void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad)
if (has_gyro) {
SDL_GetGamepadSensorData(gamepad, SDL_SENSOR_GYRO, ctx->gyro_data, SDL_arraysize(ctx->gyro_data));
}
- ctx->last_sensor_update = now;
}
if (has_accel) {
SDL_strlcpy(text, "Accelerometer:", sizeof(text));
SDLTest_DrawString(ctx->renderer, x + center - SDL_strlen(text) * FONT_CHARACTER_SIZE, y, text);
- SDL_snprintf(text, sizeof(text), "(%.2f,%.2f,%.2f)", ctx->accel_data[0], ctx->accel_data[1], ctx->accel_data[2]);
+ SDL_snprintf(text, sizeof(text), "[%.2f,%.2f,%.2f]m/s%s", ctx->accel_data[0], ctx->accel_data[1], ctx->accel_data[2], SQUARED_UTF8 );
SDLTest_DrawString(ctx->renderer, x + center + 2.0f, y, text);
-
y += ctx->button_height + 2.0f;
}
if (has_gyro) {
SDL_strlcpy(text, "Gyro:", sizeof(text));
SDLTest_DrawString(ctx->renderer, x + center - SDL_strlen(text) * FONT_CHARACTER_SIZE, y, text);
- SDL_snprintf(text, sizeof(text), "(%.2f,%.2f,%.2f)", ctx->gyro_data[0], ctx->gyro_data[1], ctx->gyro_data[2]);
+ 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 a smoothed version of the above for the sake of 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 )
+ {
+ y += ctx->button_height + 2.0f;
+ SDL_strlcpy(text, "Gyro Drift:", sizeof(text));
+ 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_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);
+ }
- y += ctx->button_height + 2.0f;
}
+
+ ctx->last_sensor_update = now;
}
}
SDL_free(mapping);
@@ -1332,6 +1669,260 @@ void DestroyGamepadDisplay(GamepadDisplay *ctx)
SDL_free(ctx);
}
+void RenderSensorTimingInfo(GyroDisplay *ctx, GamepadDisplay *gamepad_display)
+{
+ /* Sensor timing section */
+ char text[128];
+ const float new_line_height = gamepad_display->button_height + 2.0f;
+ const float text_offset_x = ctx->area.x + ctx->area.w / 4.0f + 40.0f;
+ /* Anchor to bottom left of principle rect. */
+ float text_y_pos = ctx->area.y + ctx->area.h - new_line_height * 2;
+ /*
+ * Display rate of gyro as reported by the HID implementation.
+ * This could be based on a hardware time stamp (PS5), or it could be generated by the HID implementation.
+ * One should expect this to match the estimated rate below, assuming a wired connection.
+ */
+
+ SDL_strlcpy(text, "HID Sensor Time:", sizeof(text));
+ SDLTest_DrawString(ctx->renderer, text_offset_x - SDL_strlen(text) * FONT_CHARACTER_SIZE, text_y_pos, text);
+ if (ctx->reported_sensor_rate_hz > 0) {
+ /* Convert to micro seconds */
+ const int delta_time_us = (int)1e6 / ctx->reported_sensor_rate_hz;
+ SDL_snprintf(text, sizeof(text), "%d%ss %dhz", delta_time_us, MICRO_UTF8, ctx->reported_sensor_rate_hz);
+ } else {
+ SDL_snprintf(text, sizeof(text), "????%ss ???hz", MICRO_UTF8);
+ }
+ SDLTest_DrawString(ctx->renderer, text_offset_x + 2.0f, text_y_pos, text);
+
+ /*
+ * Display the instrumentation's count of all sensor packets received over time.
+ * This may represent a more accurate polling rate for the IMU
+ * But only when using a wired connection.
+ * It does not necessarily reflect the rate at which the IMU is sampled.
+ */
+
+ text_y_pos += new_line_height;
+ SDL_strlcpy(text, "Est.Sensor Time:", sizeof(text));
+ SDLTest_DrawString(ctx->renderer, text_offset_x - SDL_strlen(text) * FONT_CHARACTER_SIZE, text_y_pos, text);
+ if (ctx->estimated_sensor_rate_hz > 0) {
+ /* Convert to micro seconds */
+ const int delta_time_us = (int)1e6 / ctx->estimated_sensor_rate_hz;
+ SDL_snprintf(text, sizeof(text), "%d%ss %dhz", delta_time_us, MICRO_UTF8, ctx->estimated_sensor_rate_hz);
+ } else {
+ SDL_snprintf(text, sizeof(text), "????%ss ???hz", MICRO_UTF8);
+ }
+ SDLTest_DrawString(ctx->renderer, text_offset_x + 2.0f, text_y_pos, text);
+}
+
+void RenderGyroDriftCalibrationButton(GyroDisplay *ctx, GamepadDisplay *gamepad_display )
+{
+ char label_text[128];
+ float log_y = ctx->area.y + BUTTON_PADDING;
+ const float new_line_height = gamepad_display->button_height + 2.0f;
+ GamepadButton *start_calibration_button = GetGyroCalibrateButton(ctx);
+ bool bHasCachedDriftSolution = BHasCachedGyroDriftSolution(ctx);
+
+ /* Show the recalibration progress bar. */
+ float recalibrate_button_width = GetGamepadButtonLabelWidth(start_calibration_button) + 2 * BUTTON_PADDING;
+ SDL_FRect recalibrate_button_area;
+ recalibrate_button_area.x = ctx->area.x + ctx->area.w - recalibrate_button_width - BUTTON_PADDING;
+ recalibrate_button_area.y = log_y + FONT_CHARACTER_SIZE * 0.5f - gamepad_display->button_height * 0.5f;
+ recalibrate_button_area.w = GetGamepadButtonLabelWidth(start_calibration_button) + 2.0f * BUTTON_PADDING;
+ recalibrate_button_area.h = gamepad_display->button_height + BUTTON_PADDING * 2.0f;
+
+ if (!bHasCachedDriftSolution) {
+ SDL_snprintf(label_text, sizeof(label_text), "Progress: %3.0f%% ", ctx->drift_calibration_progress_frac * 100.0f);
+ } else {
+ SDL_strlcpy(label_text, "Calibrate Drift", sizeof(label_text));
+ }
+
+ SetGamepadButtonLabel(start_calibration_button, label_text);
+ SetGamepadButtonArea(start_calibration_button, &recalibrate_button_area);
+ RenderGamepadButton(start_calibration_button);
+
+ /* Above button */
+ SDL_strlcpy(label_text, "Gyro Orientation:", sizeof(label_text));
+ SDLTest_DrawString(ctx->renderer, recalibrate_button_area.x, recalibrate_button_area.y - new_line_height, label_text);
+
+ if (!bHasCachedDriftSolution) {
+
+ float flNoiseFraction = SDL_clamp(SDL_sqrtf(ctx->accelerometer_noise_sq) / ACCELEROMETER_NOISE_THRESHOLD, 0.0f, 1.0f);
+ bool bTooMuchNoise = (flNoiseFraction == 1.0f);
+
+ float noise_bar_height = gamepad_display->button_height;
+ SDL_FRect noise_bar_rect;
+ noise_bar_rect.x = recalibrate_button_area.x;
+ noise_bar_rect.y = recalibrate_button_area.y + recalibrate_button_area.h + BUTTON_PADDING;
+ noise_bar_rect.w = recalibrate_button_area.w;
+ noise_bar_rect.h = noise_bar_height;
+
+ /* Adjust the noise bar rectangle based on the accelerometer noise value */
+
+ float noise_bar_fill_width = flNoiseFraction * noise_bar_rect.w; /* Scale the width based on the noise value */
+ SDL_FRect noise_bar_fill_rect;
+ noise_bar_fill_rect.x = noise_bar_rect.x + (noise_bar_rect.w - noise_bar_fill_width) * 0.5f;
+ noise_bar_fill_rect.y = noise_bar_rect.y;
+ noise_bar_fill_rect.w = noise_bar_fill_width;
+ noise_bar_fill_rect.h = noise_bar_height;
+
+ /* Set the color based on the noise value */
+ Uint8 red = (Uint8)(flNoiseFraction * 255.0f);
+ Uint8 green = (Uint8)((1.0f - flNoiseFraction) * 255.0f);
+ SDL_SetRenderDrawColor(ctx->renderer, red, green, 0, 255); /* red when high noise, green when low noise */
+ SDL_RenderFillRect(ctx->renderer, &noise_bar_fill_rect); /* draw the filled rectangle */
+
+ SDL_SetRenderDrawColor(ctx->renderer, 100, 100, 100, 255); /* gray box */
+ SDL_RenderRect(ctx->renderer, &noise_bar_rect); /* draw the outline rectangle */
+
+ /* Explicit warning message if we detect too much movement */
+ if (bTooMuchNoise) {
+ SDL_strlcpy(label_text, "Place GamePad Down!", sizeof(label_text));
+ SDLTest_DrawString(ctx->renderer, recalibrate_button_area.x, noise_bar_rect.y + noise_bar_rect.h + new_line_height, label_text);
+ }
+
+ /* Drift progress bar */
+ /* Demonstrate how far we are through the drift progress, and how it resets when there's "high noise", i.e if flNoiseFraction == 1.0f */
+ SDL_FRect progress_bar_rect;
+ progress_bar_rect.x = recalibrate_button_area.x + BUTTON_PADDING;
+ progress_bar_rect.y = recalibrate_button_area.y + recalibrate_button_area.h * 0.5f + BUTTON_PADDING * 0.5f;
+ progress_bar_rect.w = recalibrate_button_area.w - BUTTON_PADDING * 2.0f;
+ progress_bar_rect.h = BUTTON_PADDING * 0.5f;
+
+ /* Adjust the drift bar rectangle based on the drift calibration progress fraction */
+ float drift_bar_fill_width = bTooMuchNoise ? 1.0f : ctx->drift_calibration_progress_frac * progress_bar_rect.w;
+ SDL_FRect progress_bar_fill;
+ progress_bar_fill.x = progress_bar_rect.x;
+ progress_bar_fill.y = progress_bar_rect.y;
+ progress_bar_fill.w = drift_bar_fill_width;
+ progress_bar_fill.h = progress_bar_rect.h;
+
+ /* 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*/
+ SDL_RenderRect(ctx->renderer, &progress_bar_rect); /* draw the outline rectangle*/
+
+ /* If there is too much movement, we are going to draw two diagonal red lines between the progress rect corners.*/
+ if (bTooMuchNoise) {
+ SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_RED); /* red */
+ SDL_RenderFillRect(ctx->renderer, &progress_bar_fill); /* draw the filled rectangle */
+ }
+ }
+}
+
+float RenderEulerReadout(GyroDisplay *ctx, GamepadDisplay *gamepad_display )
+{
+ /* Get the mater button's width and base our width off that */
+ GamepadButton *master_button = GetGyroCalibrateButton(ctx);
+ SDL_FRect gyro_calibrate_button_rect;
+ GetGamepadButtonArea(master_button, &gyro_calibrate_button_rect);
+
+ char text[128];
+ float log_y = gyro_calibrate_button_rect.y + gyro_calibrate_button_rect.h + BUTTON_PADDING;
+ const float new_line_height = gamepad_display->button_height + 2.0f;
+ float log_gyro_euler_text_x = gyro_calibrate_button_rect.x;
+
+ /* Pitch Readout */
+ 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 */
+ 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 */
+ 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);
+
+ return log_y + new_line_height; /* Return the next y position for further rendering */
+}
+
+/* Draws the 3D cube, circles and accel arrow, positioning itself relative to the calibrate button. */
+void RenderGyroGizmo(GyroDisplay *ctx, SDL_Gamepad *gamepad, float top)
+{
+ /* Get the calibrate button's on-screen area: */
+ GamepadButton *btn = GetGyroCalibrateButton(ctx);
+ SDL_FRect btnArea;
+ GetGamepadButtonArea(btn, &btnArea);
+
+ float gizmoSize = btnArea.w;
+ /* Position it centered horizontally above the button with a small gap */
+ SDL_FRect gizmoRect;
+ gizmoRect.x = btnArea.x + (btnArea.w - gizmoSize) * 0.5f;
+ gizmoRect.y = top;
+ gizmoRect.w = gizmoSize;
+ gizmoRect.h = gizmoSize;
+
+ /* Draw the rotated cube */
+ DrawGyroDebugCube(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect);
+
+ /* Overlay the XYZ circles */
+ DrawGyroDebugCircle(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect);
+
+ /* If we have accel, draw that arrow too */
+ if (SDL_GamepadHasSensor(gamepad, SDL_SENSOR_ACCEL)) {
+ float accel[3];
+ SDL_GetGamepadSensorData(gamepad, SDL_SENSOR_ACCEL, accel, SDL_arraysize(accel));
+ DrawAccelerometerDebugArrow(ctx->renderer, &ctx->gyro_quaternion, accel, &gizmoRect);
+ }
+
+ /* Follow the size of the main button, but position it below the gizmo */
+ GamepadButton *reset_button = GetGyroResetButton(ctx);
+ if (reset_button) {
+ SDL_FRect reset_area;
+ GetGamepadButtonArea(reset_button, &reset_area);
+ /* Position the reset button below the gizmo */
+ reset_area.x = btnArea.x;
+ reset_area.y = gizmoRect.y + gizmoRect.h + BUTTON_PADDING * 0.5f;
+ reset_area.w = btnArea.w;
+ reset_area.h = btnArea.h;
+ SetGamepadButtonArea(reset_button, &reset_area);
+ RenderGamepadButton(reset_button);
+ }
+}
+
+void RenderGyroDisplay(GyroDisplay *ctx, GamepadDisplay *gamepadElements, SDL_Gamepad *gamepad)
+{
+ if (!ctx)
+ return;
+
+ bool bHasAccelerometer = SDL_GamepadHasSensor(gamepad, SDL_SENSOR_ACCEL);
+ bool bHasGyroscope = SDL_GamepadHasSensor(gamepad, SDL_SENSOR_GYRO);
+ bool bHasIMU = bHasAccelerometer || bHasGyroscope;
+ if (!bHasIMU)
+ return;
+
+ Uint8 r, g, b, a;
+ SDL_GetRenderDrawColor(ctx->renderer, &r, &g, &b, &a);
+
+ RenderSensorTimingInfo(ctx, gamepadElements);
+
+ RenderGyroDriftCalibrationButton(ctx, gamepadElements);
+
+ bool bHasCachedDriftSolution = BHasCachedGyroDriftSolution(ctx);
+ if (bHasCachedDriftSolution) {
+ float bottom = RenderEulerReadout(ctx, gamepadElements);
+ RenderGyroGizmo(ctx, gamepad, bottom);
+
+ }
+ SDL_SetRenderDrawColor(ctx->renderer, r, g, b, a);
+}
+
+void DestroyGyroDisplay(GyroDisplay *ctx)
+{
+ if (!ctx) {
+ return;
+ }
+ DestroyGamepadButton(ctx->reset_gyro_button);
+ DestroyGamepadButton(ctx->calibrate_gyro_button);
+ SDL_free(ctx);
+}
+
+
struct GamepadTypeDisplay
{
SDL_Renderer *renderer;
@@ -1965,13 +2556,25 @@ GamepadButton *CreateGamepadButton(SDL_Renderer *renderer, const char *label)
ctx->background = CreateTexture(renderer, gamepad_button_background_bmp, gamepad_button_background_bmp_len);
SDL_GetTextureSize(ctx->background, &ctx->background_width, &ctx->background_height);
- ctx->label = SDL_strdup(label);
- ctx->label_width = (float)(FONT_CHARACTER_SIZE * SDL_strlen(label));
- ctx->label_height = (float)FONT_CHARACTER_SIZE;
+ SetGamepadButtonLabel(ctx, label);
}
return ctx;
}
+void SetGamepadButtonLabel(GamepadButton *ctx, const char *label)
+{
+ if (!ctx) {
+ return;
+ }
+
+ if (ctx->label) {
+ SDL_free(ctx->label);
+ }
+
+ ctx->label = SDL_strdup(label);
+ ctx->label_width = (float)(FONT_CHARACTER_SIZE * SDL_strlen(label));
+ ctx->label_height = (float)FONT_CHARACTER_SIZE;
+}
void SetGamepadButtonArea(GamepadButton *ctx, const SDL_FRect *area)
{
if (!ctx) {
diff --git a/test/gamepadutils.h b/test/gamepadutils.h
index c08261d1718c5..f6eeaf010ee94 100644
--- a/test/gamepadutils.h
+++ b/test/gamepadutils.h
@@ -48,7 +48,19 @@ enum
#define PRESSED_COLOR 175, 238, 238, SDL_ALPHA_OPAQUE
#define PRESSED_TEXTURE_MOD 175, 238, 238
#define SELECTED_COLOR 224, 255, 224, SDL_ALPHA_OPAQUE
-
+#define GYRO_COLOR_RED 255, 0, 0, SDL_ALPHA_OPAQUE
+#define GYRO_COLOR_GREEN 0, 255, 0, SDL_ALPHA_OPAQUE
+#define GYRO_COLOR_BLUE 0, 0, 255, SDL_ALPHA_OPAQUE
+#define GYRO_COLOR_ORANGE
(Patch may be truncated, please check the link at the top of this post.)