Gyro instrumentation for test controller (#13287)

This adds several minor changes to the gyro instruments.

* The HID Sensor Time display is now throttled to 10hz.
* Calibration for the gyro is now time based, not sample count based. Different polling rates will have drift calibrated over the same space of time.
* Pitch/Yaw/Roll readout: Yaw is prioritized, and then pitch, and then roll. This gives a more human-readable pitch/yaw/roll display, closely matching game engines.
* Pitch/Yaw/Roll text is colorized to match the axes in the 3D gizmo.
* Added set of axes to the 3D gizmo to show the "Left Hand Space" positive axis directions.

test/gamepadutils.c

@@ -101,8 +101,9 @@ static SDL_FPoint ProjectVec3ToRect(const Vector3 *v, const SDL_FRect *rect)
     float fovScaleX = fovScaleY * aspect;
 
     float relZ = cameraZ - v->z;
-    if (relZ < 0.01f)
+    if (relZ < 0.01f) {
         relZ = 0.01f; /* Prevent division by 0 or negative depth */
+    }
 
     float ndc_x = (v->x / relZ) / fovScaleX;
     float ndc_y = (v->y / relZ) / fovScaleY;
@@ -207,6 +208,39 @@ void DrawGyroDebugCircle(SDL_Renderer *renderer, const Quaternion *orientation,
     SDL_SetRenderDrawColor(renderer, r, g, b, a);
 }
 
+
+void DrawGyroDebugAxes(SDL_Renderer *renderer, const Quaternion *orientation, const SDL_FRect *bounds)
+{
+    /* Store current color */
+    Uint8 r, g, b, a;
+    SDL_GetRenderDrawColor(renderer, &r, &g, &b, &a);
+
+    Vector3 origin = { 0.0f, 0.0f, 0.0f };
+
+    Vector3 right = { 1.0f, 0.0f, 0.0f };
+    Vector3 up = { 0.0f, 1.0f, 0.0f };
+    Vector3 back = { 0.0f, 0.0f, 1.0f };
+
+    Vector3 world_right = RotateVectorByQuaternion(&right, orientation);
+    Vector3 world_up = RotateVectorByQuaternion(&up, orientation);
+    Vector3 world_back = RotateVectorByQuaternion(&back, orientation);
+
+    SDL_FPoint origin_screen = ProjectVec3ToRect(&origin, bounds);
+    SDL_FPoint right_screen = ProjectVec3ToRect(&world_right, bounds);
+    SDL_FPoint up_screen = ProjectVec3ToRect(&world_up, bounds);
+    SDL_FPoint back_screen = ProjectVec3ToRect(&world_back, bounds);
+
+    SDL_SetRenderDrawColor(renderer, GYRO_COLOR_RED);
+    SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, right_screen.x, right_screen.y);
+    SDL_SetRenderDrawColor(renderer, GYRO_COLOR_GREEN);
+    SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, up_screen.x, up_screen.y);
+    SDL_SetRenderDrawColor(renderer, GYRO_COLOR_BLUE);
+    SDL_RenderLine(renderer, origin_screen.x, origin_screen.y, back_screen.x, back_screen.y);
+    
+    /* Restore current color */
+    SDL_SetRenderDrawColor(renderer, r, g, b, a);
+}
+
 void DrawAccelerometerDebugArrow(SDL_Renderer *renderer, const Quaternion *gyro_quaternion, const float *accel_data, const SDL_FRect *bounds)
 {
     /* Store current color */
@@ -990,6 +1024,8 @@ struct GyroDisplay
     /* This part displays extra info from the IMUstate in order to figure out actual polling rates. */
     float gyro_drift_solution[3];
     int reported_sensor_rate_hz;           /*hz - comes from HIDsdl implementation. Could be fixed, platform time, or true sensor time*/
+    Uint64 next_reported_sensor_time;      /* SDL ticks used to throttle the display */
+
     int estimated_sensor_rate_hz;          /*hz - our estimation of the actual polling rate by observing packets received*/
     float euler_displacement_angles[3];    /* pitch, yaw, roll */
     Quaternion gyro_quaternion;            /* Rotation since startup/reset, comprised of each gyro speed packet times sensor delta time. */
@@ -1009,7 +1045,8 @@ GyroDisplay *CreateGyroDisplay(SDL_Renderer *renderer)
         SDL_zeroa(ctx->gyro_drift_solution);
         Quaternion quat_identity = { 0.0f, 0.0f, 0.0f, 1.0f };
         ctx->gyro_quaternion = quat_identity;
-
+        ctx->reported_sensor_rate_hz = 0;
+        ctx->next_reported_sensor_time = 0;
         ctx->reset_gyro_button = CreateGamepadButton(renderer, "Reset View");
         ctx->calibrate_gyro_button = CreateGamepadButton(renderer, "Recalibrate Drift");
     }
@@ -1024,7 +1061,6 @@ void SetGyroDisplayArea(GyroDisplay *ctx, const SDL_FRect *area)
     }
 
     SDL_copyp(&ctx->area, area);
-
     /* Place the reset button to the bottom right of the gyro display area.*/
     SDL_FRect reset_button_area;
     reset_button_area.w = SDL_max(MINIMUM_BUTTON_WIDTH, GetGamepadButtonLabelWidth(ctx->reset_gyro_button) + 2 * BUTTON_PADDING);
@@ -1340,12 +1376,17 @@ void SetGamepadDisplayIMUValues(GyroDisplay *ctx, float *gyro_drift_solution, fl
         return;
     }
 
-    SDL_memcpy(ctx->gyro_drift_solution, gyro_drift_solution, sizeof(ctx->gyro_drift_solution));
-    ctx->estimated_sensor_rate_hz = estimated_sensor_rate_hz;
-
-    if (reported_senor_rate_hz != 0)
-        ctx->reported_sensor_rate_hz = reported_senor_rate_hz;
+    const int SENSOR_UPDATE_INTERVAL_MS = 100;
+    Uint64 now = SDL_GetTicks();
+    if (now > ctx->next_reported_sensor_time) {
+        ctx->estimated_sensor_rate_hz = estimated_sensor_rate_hz;
+        if (reported_senor_rate_hz != 0) {
+            ctx->reported_sensor_rate_hz = reported_senor_rate_hz;
+        }
+        ctx->next_reported_sensor_time = now + SENSOR_UPDATE_INTERVAL_MS;
+    }
 
+    SDL_memcpy(ctx->gyro_drift_solution, gyro_drift_solution, sizeof(ctx->gyro_drift_solution));
     SDL_memcpy(ctx->euler_displacement_angles, euler_displacement_angles, sizeof(ctx->euler_displacement_angles));
     ctx->gyro_quaternion = *gyro_quaternion;
     ctx->drift_calibration_progress_frac = drift_calibration_progress_frac;
@@ -1637,7 +1678,7 @@ void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad)
                 SDLTest_DrawString(ctx->renderer, x + center - SDL_strlen(text) * FONT_CHARACTER_SIZE, y, text);
                 SDL_snprintf(text, sizeof(text), "[%.2f,%.2f,%.2f]%s/s", ctx->gyro_data[0] * RAD_TO_DEG, ctx->gyro_data[1] * RAD_TO_DEG, ctx->gyro_data[2] * RAD_TO_DEG, DEGREE_UTF8);
                 SDLTest_DrawString(ctx->renderer, x + center + 2.0f, y, text);
-
+           
 
                 /* Display the testcontroller tool's evaluation of drift. This is also useful to get an average rate of turn in calibrated turntable tests. */
                 if (ctx->gyro_drift_correction_data[0] != 0.0f && ctx->gyro_drift_correction_data[2] != 0.0f && ctx->gyro_drift_correction_data[2] != 0.0f )
@@ -1648,10 +1689,7 @@ void RenderGamepadDisplay(GamepadDisplay *ctx, SDL_Gamepad *gamepad)
                     SDL_snprintf(text, sizeof(text), "[%.2f,%.2f,%.2f]%s/s", ctx->gyro_drift_correction_data[0] * RAD_TO_DEG, ctx->gyro_drift_correction_data[1] * RAD_TO_DEG, ctx->gyro_drift_correction_data[2] * RAD_TO_DEG, DEGREE_UTF8);
                     SDLTest_DrawString(ctx->renderer, x + center + 2.0f, y, text);
                 }
-
             }
-
-
         }
     }
     SDL_free(mapping);
@@ -1797,7 +1835,6 @@ void RenderGyroDriftCalibrationButton(GyroDisplay *ctx, GamepadDisplay *gamepad_
 
         /* Set the color based on the drift calibration progress fraction */
         SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_GREEN);        /* red when too much noise, green when low noise*/
-
         /* Now draw the bars with the filled, then empty rectangles */
         SDL_RenderFillRect(ctx->renderer, &progress_bar_fill);          /* draw the filled rectangle*/
         SDL_SetRenderDrawColor(ctx->renderer, 100, 100, 100, 255);      /* gray box*/
@@ -1823,20 +1860,26 @@ float RenderEulerReadout(GyroDisplay *ctx, GamepadDisplay *gamepad_display )
     const float new_line_height = gamepad_display->button_height + 2.0f;
     float log_gyro_euler_text_x = gyro_calibrate_button_rect.x;
 
+    Uint8 r, g, b, a;
+    SDL_GetRenderDrawColor(ctx->renderer, &r, &g, &b, &a);
     /* Pitch Readout */
+    SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_RED);
     SDL_snprintf(text, sizeof(text), "Pitch: %6.2f%s", ctx->euler_displacement_angles[0], DEGREE_UTF8);
     SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text);
 
     /* Yaw Readout */
+    SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_GREEN);
     log_y += new_line_height;
     SDL_snprintf(text, sizeof(text), "  Yaw: %6.2f%s", ctx->euler_displacement_angles[1], DEGREE_UTF8);
     SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text);
 
     /* Roll Readout */
+    SDL_SetRenderDrawColor(ctx->renderer, GYRO_COLOR_BLUE);
     log_y += new_line_height;
     SDL_snprintf(text, sizeof(text), " Roll: %6.2f%s", ctx->euler_displacement_angles[2], DEGREE_UTF8);
     SDLTest_DrawString(ctx->renderer, log_gyro_euler_text_x + 2.0f, log_y, text);
 
+    SDL_SetRenderDrawColor(ctx->renderer, r, g, b, a);
     return log_y + new_line_height; /* Return the next y position for further rendering */
 }
 
@@ -1859,6 +1902,9 @@ void RenderGyroGizmo(GyroDisplay *ctx, SDL_Gamepad *gamepad, float top)
     /* Draw the rotated cube */
     DrawGyroDebugCube(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect);
 
+    /* Draw positive axes */
+    DrawGyroDebugAxes(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect);
+
     /* Overlay the XYZ circles */
     DrawGyroDebugCircle(ctx->renderer, &ctx->gyro_quaternion, &gizmoRect);
 
@@ -1906,7 +1952,6 @@ void RenderGyroDisplay(GyroDisplay *ctx, GamepadDisplay *gamepadElements, SDL_Ga
     if (bHasCachedDriftSolution) {
         float bottom = RenderEulerReadout(ctx, gamepadElements);
         RenderGyroGizmo(ctx, gamepad, bottom);
-
     }
     SDL_SetRenderDrawColor(ctx->renderer, r, g, b, a);
 }

test/testcontroller.c

@@ -53,62 +53,59 @@ struct Quaternion
 
 static Quaternion quat_identity = { 0.0f, 0.0f, 0.0f, 1.0f };
 
-Quaternion QuaternionFromEuler(float roll, float pitch, float yaw)
+Quaternion QuaternionFromEuler(float pitch, float yaw, float roll)
 {
-    Quaternion q;
+    float cx = SDL_cosf(pitch * 0.5f);
+    float sx = SDL_sinf(pitch * 0.5f);
     float cy = SDL_cosf(yaw * 0.5f);
     float sy = SDL_sinf(yaw * 0.5f);
-    float cp = SDL_cosf(pitch * 0.5f);
-    float sp = SDL_sinf(pitch * 0.5f);
-    float cr = SDL_cosf(roll * 0.5f);
-    float sr = SDL_sinf(roll * 0.5f);
+    float cz = SDL_cosf(roll * 0.5f);
+    float sz = SDL_sinf(roll * 0.5f);
 
-    q.w = cr * cp * cy + sr * sp * sy;
-    q.x = sr * cp * cy - cr * sp * sy;
-    q.y = cr * sp * cy + sr * cp * sy;
-    q.z = cr * cp * sy - sr * sp * cy;
+    Quaternion q;
+    q.w = cx * cy * cz + sx * sy * sz;
+    q.x = sx * cy * cz - cx * sy * sz;
+    q.y = cx * sy * cz + sx * cy * sz;
+    q.z = cx * cy * sz - sx * sy * cz;
 
     return q;
 }
 
-static void EulerFromQuaternion(Quaternion q, float *roll, float *pitch, float *yaw)
-{
-    float sinr_cosp = 2.0f * (q.w * q.x + q.y * q.z);
-    float cosr_cosp = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
-    float roll_rad = SDL_atan2f(sinr_cosp, cosr_cosp);
+#define RAD_TO_DEG (180.0f / SDL_PI_F)
 
-    float sinp = 2.0f * (q.w * q.y - q.z * q.x);
-    float pitch_rad;
-    if (SDL_fabsf(sinp) >= 1.0f) {
-        pitch_rad = SDL_copysignf(SDL_PI_F / 2.0f, sinp);
-    } else {
-        pitch_rad = SDL_asinf(sinp);
-    }
+/* Decomposes quaternion into Yaw (Y), Pitch (X), Roll (Z) using Y-X-Z order in a left-handed system */
+void QuaternionToYXZ(Quaternion q, float *pitch, float *yaw, float *roll)
+{
+    /* Precalculate repeated expressions */
+    float qxx = q.x * q.x;
+    float qyy = q.y * q.y;
+    float qzz = q.z * q.z;
 
-    float siny_cosp = 2.0f * (q.w * q.z + q.x * q.y);
-    float cosy_cosp = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
-    float yaw_rad = SDL_atan2f(siny_cosp, cosy_cosp);
+    float qxy = q.x * q.y;
+    float qxz = q.x * q.z;
+    float qyz = q.y * q.z;
+    float qwx = q.w * q.x;
+    float qwy = q.w * q.y;
+    float qwz = q.w * q.z;
 
-    if (roll)
-        *roll = roll_rad;
-    if (pitch)
-        *pitch = pitch_rad;
-    if (yaw)
-        *yaw = yaw_rad;
-}
+    /* Yaw (around Y) */
+    if (yaw) {
+        *yaw = SDL_atan2f(2.0f * (qwy + qxz), 1.0f - 2.0f * (qyy + qzz)) * RAD_TO_DEG;
+    }
 
-static void EulerDegreesFromQuaternion(Quaternion q, float *pitch, float *yaw, float *roll)
-{
-    float pitch_rad, yaw_rad, roll_rad;
-    EulerFromQuaternion(q, &pitch_rad, &yaw_rad, &roll_rad);
+    /* Pitch (around X) */
+    float sinp = 2.0f * (qwx - qyz);
     if (pitch) {
-        *pitch = pitch_rad * (180.0f / SDL_PI_F);
-    }
-    if (yaw) {
-        *yaw = yaw_rad * (180.0f / SDL_PI_F);
+        if (SDL_fabsf(sinp) >= 1.0f) {
+            *pitch = SDL_copysignf(90.0f, sinp); /* Clamp to avoid domain error */
+        } else {
+            *pitch = SDL_asinf(sinp) * RAD_TO_DEG;
+        }
     }
+
+    /* Roll (around Z) */
     if (roll) {
-        *roll = roll_rad * (180.0f / SDL_PI_F);
+        *roll = SDL_atan2f(2.0f * (qwz + qxy), 1.0f - 2.0f * (qxx + qzz)) * RAD_TO_DEG;
     }
 }
 
@@ -1375,7 +1372,16 @@ static void HandleGamepadGyroEvent(SDL_Event *event)
     SDL_memcpy(controller->imu_state->gyro_data, event->gsensor.data, sizeof(controller->imu_state->gyro_data));
 }
 
+/* Two strategies for evaluating polling rate - one based on a fixed packet count, and one using a fixed time window.
+ * Smaller values in either will give you a more responsive polling rate estimate, but this may fluctuate more.
+ * Larger values in either will give you a more stable average but they will require more time to evaluate.
+ * Generally, wired connections tend to give much more stable 
+ */
+/* #define SDL_USE_FIXED_PACKET_COUNT_FOR_ESTIMATION */
 #define SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT 2048
+#define SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_TIME_NS (SDL_NS_PER_SECOND * 2)
+
+
 static void EstimatePacketRate()
 {
     Uint64 now_ns = SDL_GetTicksNS();
@@ -1384,17 +1390,22 @@ static void EstimatePacketRate()
     }
 
     /* Require a significant sample size before averaging rate. */
+#ifdef SDL_USE_FIXED_PACKET_COUNT_FOR_ESTIMATION
     if (controller->imu_state->imu_packet_counter >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT) {
         Uint64 deltatime_ns = now_ns - controller->imu_state->starting_time_stamp_ns;
-        controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * 1000000000ULL) / deltatime_ns);
+        controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * SDL_NS_PER_SECOND) / deltatime_ns);
+        controller->imu_state->imu_packet_counter = 0;
     }
-
-    /* Flush sampled data after a brief period so that the imu_estimated_sensor_rate value can be read.*/
-    if (controller->imu_state->imu_packet_counter >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_COUNT * 2) {
-        controller->imu_state->starting_time_stamp_ns = now_ns;
+#else
+    Uint64 deltatime_ns = now_ns - controller->imu_state->starting_time_stamp_ns;
+    if (deltatime_ns >= SDL_GAMEPAD_IMU_MIN_POLLING_RATE_ESTIMATION_TIME_NS) {
+        controller->imu_state->imu_estimated_sensor_rate = (Uint16)((controller->imu_state->imu_packet_counter * SDL_NS_PER_SECOND) / deltatime_ns);
         controller->imu_state->imu_packet_counter = 0;
     }
-    ++controller->imu_state->imu_packet_counter;
+#endif
+    else {
+        ++controller->imu_state->imu_packet_counter;
+    }
 }
 
 static void UpdateGamepadOrientation( Uint64 delta_time_ns )
@@ -1409,13 +1420,11 @@ static void UpdateGamepadOrientation( Uint64 delta_time_ns )
 
 static void HandleGamepadSensorEvent( SDL_Event* event )
 {
-    if (!controller) {
-        return;
-    }
+    if (!controller)
+        return;   
 
-    if (controller->id != event->gsensor.which) {
+    if (controller->id != event->gsensor.which)
         return;
-    }
 
     if (event->gsensor.sensor == SDL_SENSOR_GYRO) {
         HandleGamepadGyroEvent(event);
@@ -1428,13 +1437,12 @@ static void HandleGamepadSensorEvent( SDL_Event* event )
     accelerometer and gyro events are received before progressing.
     */
     if ( controller->imu_state->accelerometer_packet_number == controller->imu_state->gyro_packet_number ) {
-
         EstimatePacketRate();
         Uint64 sensorTimeStampDelta_ns = event->gsensor.sensor_timestamp - controller->imu_state->last_sensor_time_stamp_ns ;
         UpdateGamepadOrientation(sensorTimeStampDelta_ns);
 
         float display_euler_angles[3];
-        EulerDegreesFromQuaternion(controller->imu_state->integrated_rotation, &display_euler_angles[0], &display_euler_angles[1], &display_euler_angles[2]);
+        QuaternionToYXZ(controller->imu_state->integrated_rotation, &display_euler_angles[0], &display_euler_angles[1], &display_euler_angles[2]);
 
         float drift_calibration_progress_frac = controller->imu_state->gyro_drift_sample_count / (float)SDL_GAMEPAD_IMU_MIN_GYRO_DRIFT_SAMPLE_COUNT;
         int reported_polling_rate_hz = sensorTimeStampDelta_ns > 0 ? (int)(SDL_NS_PER_SECOND / sensorTimeStampDelta_ns) : 0;
@@ -2073,7 +2081,6 @@ SDL_AppResult SDLCALL SDL_AppEvent(void *appstate, SDL_Event *event)
                 event->gsensor.data[1],
                 event->gsensor.data[2],
                 event->gsensor.sensor_timestamp);
-
 #endif /* VERBOSE_SENSORS */
         HandleGamepadSensorEvent(event);
         break;