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.
2025-09-05 19:08:12 +00:00 · 2025-06-26 19:56:06 -07:00
parent 89eef1bd34
commit e960bf6904
2 changed files with 123 additions and 71 deletions
--- 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;
    }

+    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));
-    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;
@@ -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);
 }
--- 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)
+#define RAD_TO_DEG (180.0f / SDL_PI_F)
+
+/* 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)
 {
-    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);
+    /* Precalculate repeated expressions */
+    float qxx = q.x * q.x;
+    float qyy = q.y * q.y;
+    float qzz = q.z * q.z;

-    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 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;

-    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);
-    }
+    /* Yaw (around Y) */
    if (yaw) {
-        *yaw = yaw_rad * (180.0f / SDL_PI_F);
+        *yaw = SDL_atan2f(2.0f * (qwy + qxz), 1.0f - 2.0f * (qyy + qzz)) * RAD_TO_DEG;
    }
+
+    /* Pitch (around X) */
+    float sinp = 2.0f * (qwx - qyz);
+    if (pitch) {
+        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);
-    }
-
-    /* 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_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;
+#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;
+    }
+#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;