gitea/SDL3_fork
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Accelerometer Tolerance is now calibrated before Gyro Drift.

Browse Source
This commit is contained in:
Aubrey Hesselgren
2025-07-21 16:39:32 -07:00
committed by Sam Lantinga
parent 07ef532681
commit 6bfc54508c
3 changed files with 187 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
test/gamepadutils.c
View File

@@ -1031,8 +1031,10 @@ struct GyroDisplay
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] */
EGyroCalibrationPhase current_calibration_phase;
float calibration_phase_progress_fraction; /* [0..1] */
float accelerometer_noise_sq; /* Distance between last noise and new noise. Used to indicate motion.*/
float accelerometer_noise_tolerance_sq; /* Maximum amount of noise detected during the Noise Profiling Phase */
GamepadButton *reset_gyro_button;
GamepadButton *calibrate_gyro_button;
@@ -1049,6 +1051,10 @@ GyroDisplay *CreateGyroDisplay(SDL_Renderer *renderer)
ctx->gyro_quaternion = quat_identity;
ctx->reported_sensor_rate_hz = 0;
ctx->next_reported_sensor_time = 0;
ctx->current_calibration_phase = GYRO_CALIBRATION_PHASE_OFF;
ctx->calibration_phase_progress_fraction = 0.0f; /* [0..1] */
ctx->accelerometer_noise_sq = 0.0f;
ctx->accelerometer_noise_tolerance_sq = ACCELEROMETER_NOISE_THRESHOLD; /* Will be overwritten but this avoids divide by zero. */
ctx->reset_gyro_button = CreateGamepadButton(renderer, "Reset View");
ctx->calibrate_gyro_button = CreateGamepadButton(renderer, "Recalibrate Drift");
}
@@ -1362,17 +1368,7 @@ 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)
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, EGyroCalibrationPhase calibration_phase, float drift_calibration_progress_frac, float accelerometer_noise_sq, float accelerometer_noise_tolerance_sq)
{
if (!ctx) {
return;
@@ -1391,8 +1387,10 @@ void SetGamepadDisplayIMUValues(GyroDisplay *ctx, float *gyro_drift_solution, fl
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;
ctx->current_calibration_phase = calibration_phase;
ctx->calibration_phase_progress_fraction = drift_calibration_progress_frac;
ctx->accelerometer_noise_sq = accelerometer_noise_sq;
ctx->accelerometer_noise_tolerance_sq = accelerometer_noise_tolerance_sq;
}
extern GamepadButton *GetGyroResetButton(GyroDisplay *ctx)
@@ -1713,7 +1711,7 @@ 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;
const float text_offset_x = ctx->area.x + ctx->area.w / 4.0f + 35.0f;
/* Anchor to bottom left of principle rect. */
float text_y_pos = ctx->area.y + ctx->area.h - new_line_height * 2;
/*
@@ -1759,7 +1757,7 @@ void RenderGyroDriftCalibrationButton(GyroDisplay *ctx, GamepadDisplay *gamepad_
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;
@@ -1769,24 +1767,46 @@ void RenderGyroDriftCalibrationButton(GyroDisplay *ctx, GamepadDisplay *gamepad_
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) {
/* Button label vs state */
if (ctx->current_calibration_phase == GYRO_CALIBRATION_PHASE_OFF) {
SDL_strlcpy(label_text, "Start Gyro Calibration", sizeof(label_text));
} else if (ctx->current_calibration_phase == GYRO_CALIBRATION_PHASE_NOISE_PROFILING) {
SDL_snprintf(label_text, sizeof(label_text), "Noise Progress: %3.0f%% ", ctx->calibration_phase_progress_fraction * 100.0f);
} else if (ctx->current_calibration_phase == GYRO_CALIBRATION_PHASE_DRIFT_PROFILING) {
SDL_snprintf(label_text, sizeof(label_text), "Drift Progress: %3.0f%% ", ctx->calibration_phase_progress_fraction * 100.0f);
} else if (ctx->current_calibration_phase == GYRO_CALIBRATION_PHASE_COMPLETE) {
SDL_strlcpy(label_text, "Recalibrate Gyro", sizeof(label_text));
}
float flNoiseFraction = SDL_clamp(SDL_sqrtf(ctx->accelerometer_noise_sq) / ACCELEROMETER_NOISE_THRESHOLD, 0.0f, 1.0f);
bool bTooMuchNoise = (flNoiseFraction == 1.0f);
SetGamepadButtonLabel(start_calibration_button, label_text);
SetGamepadButtonArea(start_calibration_button, &recalibrate_button_area);
RenderGamepadButton(start_calibration_button);
const float flAbsoluteMaxAccelerationG = 0.125f;
bool bExtremeNoise = ctx->accelerometer_noise_sq > (flAbsoluteMaxAccelerationG * flAbsoluteMaxAccelerationG);
/* Explicit warning message if we detect too much movement */
if (ctx->current_calibration_phase == GYRO_CALIBRATION_PHASE_OFF) {
if (bExtremeNoise)
{
SDL_strlcpy(label_text, "GamePad Must Be Still", sizeof(label_text));
SDLTest_DrawString(ctx->renderer, recalibrate_button_area.x, recalibrate_button_area.y + recalibrate_button_area.h + new_line_height, label_text);
SDL_strlcpy(label_text, "Place GamePad On Table", sizeof(label_text));
SDLTest_DrawString(ctx->renderer, recalibrate_button_area.x, recalibrate_button_area.y + recalibrate_button_area.h + new_line_height * 2, label_text);
}
}
if (ctx->current_calibration_phase == GYRO_CALIBRATION_PHASE_NOISE_PROFILING
|| ctx->current_calibration_phase == GYRO_CALIBRATION_PHASE_DRIFT_PROFILING)
{
float flAbsoluteNoiseFraction = SDL_clamp(ctx->accelerometer_noise_sq / (flAbsoluteMaxAccelerationG * flAbsoluteMaxAccelerationG), 0.0f, 1.0f);
float flAbsoluteToleranceFraction = SDL_clamp(ctx->accelerometer_noise_tolerance_sq / (flAbsoluteMaxAccelerationG * flAbsoluteMaxAccelerationG), 0.0f, 1.0f);
float flRelativeNoiseFraction = SDL_clamp(ctx->accelerometer_noise_sq / ctx->accelerometer_noise_tolerance_sq, 0.0f, 1.0f);
bool bTooMuchNoise = (flAbsoluteNoiseFraction == 1.0f);
float noise_bar_height = gamepad_display->button_height;
SDL_FRect noise_bar_rect;
@@ -1795,21 +1815,35 @@ void RenderGyroDriftCalibrationButton(GyroDisplay *ctx, GamepadDisplay *gamepad_
noise_bar_rect.w = recalibrate_button_area.w;
noise_bar_rect.h = noise_bar_height;
//SDL_strlcpy(label_text, "Place GamePad On Table", sizeof(label_text));
SDL_snprintf(label_text, sizeof(label_text), "Noise Tolerance: %3.3fG ", SDL_sqrtf(ctx->accelerometer_noise_tolerance_sq) );
SDLTest_DrawString(ctx->renderer, recalibrate_button_area.x, recalibrate_button_area.y + recalibrate_button_area.h + new_line_height * 2, label_text);
/* 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 */
float noise_bar_fill_width = flAbsoluteNoiseFraction * 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);
/* Set the color based on the noise value vs the tolerance */
Uint8 red = (Uint8)(flRelativeNoiseFraction * 255.0f);
Uint8 green = (Uint8)((1.0f - flRelativeNoiseFraction) * 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 */
float tolerance_bar_fill_width = flAbsoluteToleranceFraction * noise_bar_rect.w; /* Scale the width based on the noise value */
SDL_FRect tolerance_bar_rect;
tolerance_bar_rect.x = noise_bar_rect.x + (noise_bar_rect.w - tolerance_bar_fill_width) * 0.5f;
tolerance_bar_rect.y = noise_bar_rect.y;
tolerance_bar_rect.w = tolerance_bar_fill_width;
tolerance_bar_rect.h = noise_bar_height;
SDL_SetRenderDrawColor(ctx->renderer, 128, 128, 0, 255);
SDL_RenderRect(ctx->renderer, &tolerance_bar_rect); /* draw the tolerance rectangle */
SDL_SetRenderDrawColor(ctx->renderer, 100, 100, 100, 255); /* gray box */
SDL_RenderRect(ctx->renderer, &noise_bar_rect); /* draw the outline rectangle */
@@ -1828,7 +1862,7 @@ void RenderGyroDriftCalibrationButton(GyroDisplay *ctx, GamepadDisplay *gamepad_
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;
float drift_bar_fill_width = bTooMuchNoise ? 1.0f : ctx->calibration_phase_progress_fraction * 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;
@@ -1947,14 +1981,14 @@ void RenderGyroDisplay(GyroDisplay *ctx, GamepadDisplay *gamepadElements, SDL_Ga
SDL_GetRenderDrawColor(ctx->renderer, &r, &g, &b, &a);
RenderSensorTimingInfo(ctx, gamepadElements);
RenderGyroDriftCalibrationButton(ctx, gamepadElements);
bool bHasCachedDriftSolution = BHasCachedGyroDriftSolution(ctx);
if (bHasCachedDriftSolution) {
/* Render Gyro calibration phases */
if (ctx->current_calibration_phase == GYRO_CALIBRATION_PHASE_COMPLETE) {
float bottom = RenderEulerReadout(ctx, gamepadElements);
RenderGyroGizmo(ctx, gamepad, bottom);
}
SDL_SetRenderDrawColor(ctx->renderer, r, g, b, a);
}