Added tools for evaluating gyroscope accuracy and IMU polling rates. (#13209)

* 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

@@ -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) {

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       255, 128, 0, SDL_ALPHA_OPAQUE
+
+/* Shared layout constants */
+#define BUTTON_PADDING          12.0f
+#define MINIMUM_BUTTON_WIDTH 96.0f
+
+/*  Symbol */
+#define DEGREE_UTF8 "\xC2\xB0"
+#define SQUARED_UTF8 "\xC2\xB2"
+#define MICRO_UTF8   "\xC2\xB5"
 /* Gamepad image display */
 
 extern GamepadImage *CreateGamepadImage(SDL_Renderer *renderer);
@@ -78,6 +90,7 @@ typedef struct GamepadDisplay GamepadDisplay;
 extern GamepadDisplay *CreateGamepadDisplay(SDL_Renderer *renderer);
 extern void SetGamepadDisplayDisplayMode(GamepadDisplay *ctx, ControllerDisplayMode display_mode);
 extern void SetGamepadDisplayArea(GamepadDisplay *ctx, const SDL_FRect *area);
+extern void SetGamepadDisplayGyroDriftCorrection(GamepadDisplay *ctx, float *gyro_drift_correction);
 extern int GetGamepadDisplayElementAt(GamepadDisplay *ctx, SDL_Gamepad *gamepad, float x, float y);
 extern void SetGamepadDisplayHighlight(GamepadDisplay *ctx, int element, bool pressed);
 extern void SetGamepadDisplaySelected(GamepadDisplay *ctx, int element);
@@ -118,6 +131,7 @@ extern void DestroyJoystickDisplay(JoystickDisplay *ctx);
 typedef struct GamepadButton GamepadButton;
 
 extern GamepadButton *CreateGamepadButton(SDL_Renderer *renderer, const char *label);
+extern void SetGamepadButtonLabel(GamepadButton *ctx, const char *label);
 extern void SetGamepadButtonArea(GamepadButton *ctx, const SDL_FRect *area);
 extern void GetGamepadButtonArea(GamepadButton *ctx, SDL_FRect *area);
 extern void SetGamepadButtonHighlight(GamepadButton *ctx, bool highlight, bool pressed);
@@ -127,6 +141,22 @@ extern bool GamepadButtonContains(GamepadButton *ctx, float x, float y);
 extern void RenderGamepadButton(GamepadButton *ctx);
 extern void DestroyGamepadButton(GamepadButton *ctx);
 
+/* Gyro element Display */
+/* If you want to calbirate against a known rotation (i.e. a turn table test) Increase ACCELEROMETER_NOISE_THRESHOLD to about 5, or drift correction will be constantly reset.*/
+#define ACCELEROMETER_NOISE_THRESHOLD 0.125f
+typedef struct Quaternion Quaternion;
+typedef struct GyroDisplay GyroDisplay;
+
+extern void InitCirclePoints3D();
+extern GyroDisplay *CreateGyroDisplay(SDL_Renderer *renderer);
+extern void SetGyroDisplayArea(GyroDisplay *ctx, const SDL_FRect *area);
+extern bool BHasCachedGyroDriftSolution(GyroDisplay *ctx);
+extern 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); 
+extern GamepadButton *GetGyroResetButton(GyroDisplay *ctx);
+extern GamepadButton *GetGyroCalibrateButton(GyroDisplay *ctx);
+extern void RenderGyroDisplay(GyroDisplay *ctx, GamepadDisplay *gamepadElements, SDL_Gamepad *gamepad);
+extern void DestroyGyroDisplay(GyroDisplay *ctx);
+
 /* Working with mappings and bindings */
 
 /* Return whether a mapping has any bindings */

test/testcontroller.c

@@ -32,12 +32,9 @@
 #define TITLE_HEIGHT 48.0f
 #define PANEL_SPACING 25.0f
 #define PANEL_WIDTH 250.0f
-#define MINIMUM_BUTTON_WIDTH 96.0f
-#define BUTTON_MARGIN 16.0f
-#define BUTTON_PADDING 12.0f
 #define GAMEPAD_WIDTH 512.0f
 #define GAMEPAD_HEIGHT 560.0f
-
+#define BUTTON_MARGIN  16.0f
 #define SCREEN_WIDTH  (PANEL_WIDTH + PANEL_SPACING + GAMEPAD_WIDTH + PANEL_SPACING + PANEL_WIDTH)
 #define SCREEN_HEIGHT (TITLE_HEIGHT + GAMEPAD_HEIGHT)
 
@@ -49,6 +46,228 @@ typedef struct
     int m_nFarthestValue;
 } AxisState;
 
+struct Quaternion
+{
+    float x, y, z, w;
+};
+
+static Quaternion quat_identity = { 0.0f, 0.0f, 0.0f, 1.0f };
+
+Quaternion QuaternionFromEuler(float roll, float pitch, float yaw)
+{
+    Quaternion q;
+    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);
+
+    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;
+
+    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);
+
+    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);
+    }
+
+    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);
+
+    if (roll)
+        *roll = roll_rad;
+    if (pitch)
+        *pitch = pitch_rad;
+    if (yaw)
+        *yaw = yaw_rad;
+}
+
+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);
+    if (pitch) {
+        *pitch = pitch_rad * (180.0f / SDL_PI_F);
+    }
+    if (yaw) {
+        *yaw = yaw_rad * (180.0f / SDL_PI_F);
+    }
+    if (roll) {
+        *roll = roll_rad * (180.0f / SDL_PI_F);
+    }
+}
+
+Quaternion MultiplyQuaternion(Quaternion a, Quaternion b)
+{
+    Quaternion q;
+    q.x = a.x * b.w + a.y * b.z - a.z * b.y + a.w * b.x;
+    q.y = -a.x * b.z + a.y * b.w + a.z * b.x + a.w * b.y;
+    q.z = a.x * b.y - a.y * b.x + a.z * b.w + a.w * b.z;
+    q.w = -a.x * b.x - a.y * b.y - a.z * b.z + a.w * b.w;
+    return q;
+}
+
+void NormalizeQuaternion(Quaternion *q)
+{
+    float mag = SDL_sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
+    if (mag > 0.0f) {
+        q->x /= mag;
+        q->y /= mag;
+        q->z /= mag;
+        q->w /= mag;
+    }
+}
+
+float Normalize180(float angle)
+{
+    angle = SDL_fmodf(angle + 180.0f, 360.0f);
+    if (angle < 0.0f) {
+        angle += 360.0f;
+    }
+    return angle - 180.0f;
+}
+
+typedef struct
+{
+    Uint64 gyro_packet_number;
+    Uint64 accelerometer_packet_number;
+    /* When both gyro and accelerometer events have been processed, we can increment this and use it to calculate polling rate over time.*/
+    Uint64 imu_packet_counter; 
+
+    Uint64 starting_time_stamp_ns; /* Use this to help estimate how many packets are received over a duration */
+    Uint16 imu_estimated_sensor_rate; /* in Hz, used to estimate how many packets are received over a duration */
+
+    Uint64 last_sensor_time_stamp_ns;/* Comes from the event data/HID implementation. Official PS5/Edge gives true hardware time stamps. Others are simulated. Nanoseconds  i.e. 1e9 */
+
+    /* Fresh data copied from sensor events. */
+    float accel_data[3]; /* Meters per second squared, i.e. 9.81f means 9.81 meters per second squared */
+    float gyro_data[3]; /* Degrees per second, i.e. 100.0f means 100 degrees per second */
+
+    float last_accel_data[3];/* Needed to detect motion (and inhibit drift calibration) */
+    float accelerometer_length_squared;
+    float gyro_drift_accumulator[3];
+    bool is_calibrating_drift; /* Starts on, but can be turned back on by the user to restart the drift calibration. */
+    int gyro_drift_sample_count;
+    float gyro_drift_solution[3]; /* Non zero if calibration is complete. */
+
+    Quaternion integrated_rotation; /* Used to help test whether the time stamps and gyro degrees per second are set up correctly by the HID implementation */
+} IMUState;
+
+/* Reset the Drift calculation state */
+void StartGyroDriftCalibration(IMUState *imustate)
+{
+    imustate->is_calibrating_drift = true;
+    imustate->gyro_drift_sample_count = 0;
+    SDL_zeroa(imustate->gyro_drift_solution);
+    SDL_zeroa(imustate->gyro_drift_accumulator);
+}
+void ResetIMUState(IMUState *imustate)
+{
+    imustate->gyro_packet_number = 0;
+    imustate->accelerometer_packet_number = 0;
+    imustate->starting_time_stamp_ns = SDL_GetTicksNS();
+    imustate->integrated_rotation = quat_identity;
+    imustate->accelerometer_length_squared = 0.0f;
+    imustate->integrated_rotation = quat_identity;
+    SDL_zeroa(imustate->last_accel_data);
+    SDL_zeroa(imustate->gyro_drift_solution);
+    StartGyroDriftCalibration(imustate);
+}
+
+void ResetGyroOrientation(IMUState *imustate)
+{
+    imustate->integrated_rotation = quat_identity;
+}
+
+/* More samples = more accurate drift correction, but also more time to calibrate.*/
+#define SDL_GAMEPAD_IMU_MIN_GYRO_DRIFT_SAMPLE_COUNT 1024
+
+/*
+ * Average drift _per packet_ as opposed to _per second_
+ * This reduces a small amount of overhead when applying the drift correction.
+ */
+void FinalizeDriftSolution(IMUState *imustate)
+{
+    if (imustate->gyro_drift_sample_count >= SDL_GAMEPAD_IMU_MIN_GYRO_DRIFT_SAMPLE_COUNT) {
+        imustate->gyro_drift_solution[0] = imustate->gyro_drift_accumulator[0] / (float)imustate->gyro_drift_sample_count;
+        imustate->gyro_drift_solution[1] = imustate->gyro_drift_accumulator[1] / (float)imustate->gyro_drift_sample_count;
+        imustate->gyro_drift_solution[2] = imustate->gyro_drift_accumulator[2] / (float)imustate->gyro_drift_sample_count;
+    }
+
+    imustate->is_calibrating_drift = false;
+    ResetGyroOrientation(imustate);
+}
+
+/* Sample gyro packet in order to calculate drift*/
+void SampleGyroPacketForDrift( IMUState *imustate )
+{
+    if ( !imustate->is_calibrating_drift )
+        return;
+
+    /* Get the length squared difference of the last accelerometer data vs. the new one */
+    float accelerometer_difference[3];
+    accelerometer_difference[0] = imustate->accel_data[0] - imustate->last_accel_data[0];
+    accelerometer_difference[1] = imustate->accel_data[1] - imustate->last_accel_data[1];
+    accelerometer_difference[2] = imustate->accel_data[2] - imustate->last_accel_data[2];
+    SDL_memcpy(imustate->last_accel_data, imustate->accel_data, sizeof(imustate->last_accel_data));
+
+    imustate->accelerometer_length_squared = accelerometer_difference[0] * accelerometer_difference[0] + accelerometer_difference[1] * accelerometer_difference[1] + accelerometer_difference[2] * accelerometer_difference[2];
+
+    /* Ideal threshold will vary considerably depending on IMU. PS5 needs a low value (0.05f). Nintendo Switch needs a higher value (0.15f). */
+    const float flAccelerometerMovementThreshold = ACCELEROMETER_NOISE_THRESHOLD;
+    if (imustate->accelerometer_length_squared > flAccelerometerMovementThreshold * flAccelerometerMovementThreshold) {
+        /* Reset the drift calibration if the accelerometer has moved significantly */
+        StartGyroDriftCalibration(imustate);
+    } else {
+        /* Sensor is stationary enough to evaluate for drift.*/
+        ++imustate->gyro_drift_sample_count;
+
+        imustate->gyro_drift_accumulator[0] += imustate->gyro_data[0];
+        imustate->gyro_drift_accumulator[1] += imustate->gyro_data[1];
+        imustate->gyro_drift_accumulator[2] += imustate->gyro_data[2];
+
+        if (imustate->gyro_drift_sample_count >= SDL_GAMEPAD_IMU_MIN_GYRO_DRIFT_SAMPLE_COUNT) {
+            FinalizeDriftSolution(imustate);
+        }
+    }    
+}
+
+void ApplyDriftSolution(float *gyro_data, const float *drift_solution)
+{
+    gyro_data[0] -= drift_solution[0];
+    gyro_data[1] -= drift_solution[1];
+    gyro_data[2] -= drift_solution[2];
+}
+
+void UpdateGyroRotation(IMUState *imustate, Uint64 sensorTimeStampDelta_ns)
+{
+    float sensorTimeDeltaTimeSeconds = SDL_NS_TO_SECONDS((float)sensorTimeStampDelta_ns);
+    /* Integrate speeds to get Rotational Displacement*/
+    float pitch  = imustate->gyro_data[0] * sensorTimeDeltaTimeSeconds;
+    float yaw = imustate->gyro_data[1] * sensorTimeDeltaTimeSeconds;
+    float roll  = imustate->gyro_data[2] * sensorTimeDeltaTimeSeconds;
+
+    /* Use quaternions to avoid gimbal lock*/
+    Quaternion delta_rotation = QuaternionFromEuler(pitch, yaw, roll);
+    imustate->integrated_rotation = MultiplyQuaternion(imustate->integrated_rotation, delta_rotation);
+    NormalizeQuaternion(&imustate->integrated_rotation);
+}
+
 typedef struct
 {
     SDL_JoystickID id;
@@ -56,6 +275,7 @@ typedef struct
     SDL_Joystick *joystick;
     int num_axes;
     AxisState *axis_state;
+    IMUState *imu_state;
 
     SDL_Gamepad *gamepad;
     char *mapping;
@@ -71,6 +291,7 @@ static SDL_Renderer *screen = NULL;
 static ControllerDisplayMode display_mode = CONTROLLER_MODE_TESTING;
 static GamepadImage *image = NULL;
 static GamepadDisplay *gamepad_elements = NULL;
+static GyroDisplay *gyro_elements = NULL;
 static GamepadTypeDisplay *gamepad_type = NULL;
 static JoystickDisplay *joystick_elements = NULL;
 static GamepadButton *setup_mapping_button = NULL;
@@ -265,6 +486,8 @@ static void ClearButtonHighlights(void)
     ClearGamepadImage(image);
     SetGamepadDisplayHighlight(gamepad_elements, SDL_GAMEPAD_ELEMENT_INVALID, false);
     SetGamepadTypeDisplayHighlight(gamepad_type, SDL_GAMEPAD_TYPE_UNSELECTED, false);
+    SetGamepadButtonHighlight(GetGyroResetButton( gyro_elements ), false, false);
+    SetGamepadButtonHighlight(GetGyroCalibrateButton(gyro_elements), false, false);
     SetGamepadButtonHighlight(setup_mapping_button, false, false);
     SetGamepadButtonHighlight(done_mapping_button, false, false);
     SetGamepadButtonHighlight(cancel_button, false, false);
@@ -276,6 +499,8 @@ static void ClearButtonHighlights(void)
 static void UpdateButtonHighlights(float x, float y, bool button_down)
 {
     ClearButtonHighlights();
+    SetGamepadButtonHighlight(GetGyroResetButton(gyro_elements), GamepadButtonContains(GetGyroResetButton(gyro_elements), x, y), button_down);
+    SetGamepadButtonHighlight(GetGyroCalibrateButton(gyro_elements), GamepadButtonContains(GetGyroCalibrateButton(gyro_elements), x, y), button_down);
 
     if (display_mode == CONTROLLER_MODE_TESTING) {
         SetGamepadButtonHighlight(setup_mapping_button, GamepadButtonContains(setup_mapping_button, x, y), button_down);
@@ -915,6 +1140,8 @@ static void AddController(SDL_JoystickID id, bool verbose)
     if (new_controller->joystick) {
         new_controller->num_axes = SDL_GetNumJoystickAxes(new_controller->joystick);
         new_controller->axis_state = (AxisState *)SDL_calloc(new_controller->num_axes, sizeof(*new_controller->axis_state));
+        new_controller->imu_state = (IMUState *)SDL_calloc(1, sizeof(*new_controller->imu_state));
+        ResetIMUState(new_controller->imu_state);
     }
 
     joystick = new_controller->joystick;
@@ -959,6 +1186,9 @@ static void DelController(SDL_JoystickID id)
     if (controllers[i].axis_state) {
         SDL_free(controllers[i].axis_state);
     }
+    if (controllers[i].imu_state) {
+        SDL_free(controllers[i].imu_state);
+    }
     if (controllers[i].joystick) {
         SDL_CloseJoystick(controllers[i].joystick);
     }
@@ -1133,6 +1363,97 @@ static void HandleGamepadRemoved(SDL_JoystickID id)
         controllers[i].gamepad = NULL;
     }
 }
+static void HandleGamepadAccelerometerEvent(SDL_Event *event)
+{
+    controller->imu_state->accelerometer_packet_number++;
+    SDL_memcpy(controller->imu_state->accel_data, event->gsensor.data, sizeof(controller->imu_state->accel_data));
+}
+
+static void HandleGamepadGyroEvent(SDL_Event *event)
+{
+    controller->imu_state->gyro_packet_number++;
+    SDL_memcpy(controller->imu_state->gyro_data, event->gsensor.data, sizeof(controller->imu_state->gyro_data));
+}
+
+#define SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT 2048
+static void EstimatePacketRate()
+{
+    Uint64 now_ns = SDL_GetTicksNS();
+    if (controller->imu_state->imu_packet_counter == 0) {
+        controller->imu_state->starting_time_stamp_ns = now_ns;
+    }
+
+    /* Require a significant sample size before averaging rate. */
+    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);
+    }
+
+    /* 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;
+        controller->imu_state->imu_packet_counter = 0;
+    }
+    ++controller->imu_state->imu_packet_counter;
+}
+
+static void UpdateGamepadOrientation( Uint64 delta_time_ns )
+{
+    if (!controller || !controller->imu_state)
+        return;
+
+    SampleGyroPacketForDrift(controller->imu_state);
+    ApplyDriftSolution(controller->imu_state->gyro_data, controller->imu_state->gyro_drift_solution);
+    UpdateGyroRotation(controller->imu_state, delta_time_ns);
+}
+
+static void HandleGamepadSensorEvent( SDL_Event* event )
+{
+    if (!controller)
+        return;   
+
+    if (controller->id != event->gsensor.which)
+        return;
+
+    if (event->gsensor.sensor == SDL_SENSOR_GYRO) {
+        HandleGamepadGyroEvent(event);
+    } else if (event->gsensor.sensor == SDL_SENSOR_ACCEL) {
+        HandleGamepadAccelerometerEvent(event);
+    }  
+
+    /*
+    This is where we can update the quaternion because we need to have a drift solution, which requires both
+    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]);
+
+        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;
+
+        /* Send the results to the frontend */
+        SetGamepadDisplayIMUValues(gyro_elements,
+            controller->imu_state->gyro_drift_solution,
+            display_euler_angles,
+            &controller->imu_state->integrated_rotation,
+            reported_polling_rate_hz,
+            controller->imu_state->imu_estimated_sensor_rate,
+            drift_calibration_progress_frac,
+            controller->imu_state->accelerometer_length_squared
+        );
+
+        /* Also show the gyro correction next to the gyro speed - this is useful in turntable tests as you can use a turntable to calibrate for drift, and that drift correction is functionally the same as the turn table speed (ignoring drift) */
+        SetGamepadDisplayGyroDriftCorrection(gamepad_elements, controller->imu_state->gyro_drift_solution);
+
+        controller->imu_state->last_sensor_time_stamp_ns = event->gsensor.sensor_timestamp;
+    }
+}
 
 static Uint16 ConvertAxisToRumble(Sint16 axisval)
 {
@@ -1296,7 +1617,9 @@ static void VirtualGamepadMouseDown(float x, float y)
     int element = GetGamepadImageElementAt(image, x, y);
 
     if (element == SDL_GAMEPAD_ELEMENT_INVALID) {
-        SDL_FPoint point = { x, y };
+        SDL_FPoint point;
+        point.x = x;
+        point.y = y;
         SDL_FRect touchpad;
         GetGamepadTouchpadArea(image, &touchpad);
         if (SDL_PointInRectFloat(&point, &touchpad)) {
@@ -1738,8 +2061,9 @@ SDL_AppResult SDLCALL SDL_AppEvent(void *appstate, SDL_Event *event)
         break;
 #endif /* VERBOSE_TOUCHPAD */
 
-#ifdef VERBOSE_SENSORS
+
     case SDL_EVENT_GAMEPAD_SENSOR_UPDATE:
+#ifdef VERBOSE_SENSORS
         SDL_Log("Gamepad %" SDL_PRIu32 " sensor %s: %.2f, %.2f, %.2f (%" SDL_PRIu64 ")",
                 event->gsensor.which,
                 GetSensorName((SDL_SensorType) event->gsensor.sensor),
@@ -1747,8 +2071,10 @@ SDL_AppResult SDLCALL SDL_AppEvent(void *appstate, SDL_Event *event)
                 event->gsensor.data[1],
                 event->gsensor.data[2],
                 event->gsensor.sensor_timestamp);
-        break;
+        
 #endif /* VERBOSE_SENSORS */
+        HandleGamepadSensorEvent(event);
+        break;
 
 #ifdef VERBOSE_AXES
     case SDL_EVENT_GAMEPAD_AXIS_MOTION:
@@ -1807,7 +2133,11 @@ SDL_AppResult SDLCALL SDL_AppEvent(void *appstate, SDL_Event *event)
         }
 
         if (display_mode == CONTROLLER_MODE_TESTING) {
-            if (GamepadButtonContains(setup_mapping_button, event->button.x, event->button.y)) {
+            if (GamepadButtonContains(GetGyroResetButton(gyro_elements), event->button.x, event->button.y)) {
+                ResetGyroOrientation(controller->imu_state);
+            } else if (GamepadButtonContains(GetGyroCalibrateButton(gyro_elements), event->button.x, event->button.y)) {
+                StartGyroDriftCalibration(controller->imu_state);
+            } else if (GamepadButtonContains(setup_mapping_button, event->button.x, event->button.y)) {
                 SetDisplayMode(CONTROLLER_MODE_BINDING);
             }
         } else if (display_mode == CONTROLLER_MODE_BINDING) {
@@ -1886,6 +2216,10 @@ SDL_AppResult SDLCALL SDL_AppEvent(void *appstate, SDL_Event *event)
                 SDL_ReloadGamepadMappings();
             } else if (event->key.key == SDLK_ESCAPE) {
                 done = true;
+            } else if (event->key.key == SDLK_SPACE) {
+                if (controller && controller->imu_state) {
+                    ResetGyroOrientation(controller->imu_state);
+                }
             }
         } else if (display_mode == CONTROLLER_MODE_BINDING) {
             if (event->key.key == SDLK_C && (event->key.mod & SDL_KMOD_CTRL)) {
@@ -1994,6 +2328,7 @@ SDL_AppResult SDLCALL SDL_AppIterate(void *appstate)
 
         if (display_mode == CONTROLLER_MODE_TESTING) {
             RenderGamepadButton(setup_mapping_button);
+            RenderGyroDisplay(gyro_elements, gamepad_elements, controller->gamepad);
         } else if (display_mode == CONTROLLER_MODE_BINDING) {
             DrawBindingTips(screen);
             RenderGamepadButton(done_mapping_button);
@@ -2148,6 +2483,17 @@ SDL_AppResult SDLCALL SDL_AppInit(void **appstate, int argc, char *argv[])
     area.h = GAMEPAD_HEIGHT;
     SetGamepadDisplayArea(gamepad_elements, &area);
 
+    gyro_elements = CreateGyroDisplay(screen);
+    const float vidReservedHeight = 24.0f;
+    /* Bottom right of the screen */
+    area.w = SCREEN_WIDTH * 0.375f;
+    area.h = SCREEN_HEIGHT * 0.475f;
+    area.x = SCREEN_WIDTH - area.w;
+    area.y = SCREEN_HEIGHT - area.h - vidReservedHeight;
+
+    SetGyroDisplayArea(gyro_elements, &area);
+    InitCirclePoints3D();
+
     gamepad_type = CreateGamepadTypeDisplay(screen);
     area.x = 0;
     area.y = TITLE_HEIGHT;
@@ -2227,6 +2573,7 @@ void SDLCALL SDL_AppQuit(void *appstate, SDL_AppResult result)
     SDL_free(controller_name);
     DestroyGamepadImage(image);
     DestroyGamepadDisplay(gamepad_elements);
+    DestroyGyroDisplay(gyro_elements);
     DestroyGamepadTypeDisplay(gamepad_type);
     DestroyJoystickDisplay(joystick_elements);
     DestroyGamepadButton(setup_mapping_button);