From 6bfc54508c55f70cccc6ac49a6e3d8e1f31acc0d Mon Sep 17 00:00:00 2001 From: Aubrey Hesselgren Date: Mon, 21 Jul 2025 16:39:32 -0700 Subject: [PATCH] Accelerometer Tolerance is now calibrated before Gyro Drift. --- test/gamepadutils.c | 106 +++++++++++++++++++++------------ test/gamepadutils.h | 18 ++++-- test/testcontroller.c | 134 ++++++++++++++++++++++++++++++++---------- 3 files changed, 187 insertions(+), 71 deletions(-) diff --git a/test/gamepadutils.c b/test/gamepadutils.c index 3b68342223..c27df6d790 100644 --- a/test/gamepadutils.c +++ b/test/gamepadutils.c @@ -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); } diff --git a/test/gamepadutils.h b/test/gamepadutils.h index 157bc9a0be..743d4c39ff 100644 --- a/test/gamepadutils.h +++ b/test/gamepadutils.h @@ -142,16 +142,26 @@ 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.5f + +/* This is used as the initial noise tolernace threshold. It's set very close to zero to avoid divide by zero while we're evaluating the noise profile. Each controller may have a very different noise profile.*/ +#define ACCELEROMETER_NOISE_THRESHOLD 1e-6f + +/* Gyro Calibration Phases */ +typedef enum +{ + GYRO_CALIBRATION_PHASE_OFF, /* Calibration has not yet been evaluated - signal to the user to put the controller on a flat surface before beginning the calibration process */ + GYRO_CALIBRATION_PHASE_NOISE_PROFILING, /* Find the max accelerometer noise for a fixed period */ + GYRO_CALIBRATION_PHASE_DRIFT_PROFILING, /* Find the drift while the accelerometer is below the accelerometer noise tolerance */ + GYRO_CALIBRATION_PHASE_COMPLETE, /* Calibration has finished */ +} EGyroCalibrationPhase; + 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 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); extern GamepadButton *GetGyroResetButton(GyroDisplay *ctx); extern GamepadButton *GetGyroCalibrateButton(GyroDisplay *ctx); extern void RenderGyroDisplay(GyroDisplay *ctx, GamepadDisplay *gamepadElements, SDL_Gamepad *gamepad); diff --git a/test/testcontroller.c b/test/testcontroller.c index 0530d1a992..2c41e01d8f 100644 --- a/test/testcontroller.c +++ b/test/testcontroller.c @@ -156,23 +156,39 @@ typedef struct 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 accelerometer_length_squared; /* The current length squared from last packet to this packet */ + float accelerometer_tolerance_squared; /* In phase one of calibration we calculate this as the largest accelerometer_length_squared over the time period */ + 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. */ + + EGyroCalibrationPhase calibration_phase; /* [ GYRO_CALIBRATION_PHASE_OFF, GYRO_CALIBRATION_PHASE_NOISE_PROFILING, GYRO_CALIBRATION_PHASE_DRIFT_PROFILING,GYRO_CALIBRATION_PHASE_COMPLETE ] */ + Uint64 calibration_phase_start_time_ticks_ns; /* Set each time a calibration phase begins so that we can a real time number for evaluation of drift. Previously we would use a fixed number of packets but given that gyro polling rates vary wildly this made the duration very different. */ + 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) +/* First stage of calibration - get the noise profile of the accelerometer */ +void BeginNoiseCalibrationPhase(IMUState *imustate) { - imustate->is_calibrating_drift = true; + imustate->accelerometer_tolerance_squared = ACCELEROMETER_NOISE_THRESHOLD; + imustate->calibration_phase = GYRO_CALIBRATION_PHASE_NOISE_PROFILING; + imustate->calibration_phase_start_time_ticks_ns = SDL_GetTicksNS(); +} + +/* Reset the Drift calculation state */ +void BeginDriftCalibrationPhase(IMUState *imustate) +{ + imustate->calibration_phase = GYRO_CALIBRATION_PHASE_DRIFT_PROFILING; + imustate->calibration_phase_start_time_ticks_ns = SDL_GetTicksNS(); imustate->gyro_drift_sample_count = 0; SDL_zeroa(imustate->gyro_drift_solution); SDL_zeroa(imustate->gyro_drift_accumulator); } + +/* Initial/full reset of state */ void ResetIMUState(IMUState *imustate) { imustate->gyro_packet_number = 0; @@ -180,10 +196,13 @@ void ResetIMUState(IMUState *imustate) imustate->starting_time_stamp_ns = SDL_GetTicksNS(); imustate->integrated_rotation = quat_identity; imustate->accelerometer_length_squared = 0.0f; + imustate->accelerometer_tolerance_squared = ACCELEROMETER_NOISE_THRESHOLD; + imustate->calibration_phase = GYRO_CALIBRATION_PHASE_OFF; + imustate->calibration_phase_start_time_ticks_ns = SDL_GetTicksNS(); imustate->integrated_rotation = quat_identity; SDL_zeroa(imustate->last_accel_data); SDL_zeroa(imustate->gyro_drift_solution); - StartGyroDriftCalibration(imustate); + SDL_zeroa(imustate->gyro_drift_accumulator); } void ResetGyroOrientation(IMUState *imustate) @@ -191,8 +210,40 @@ 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 +/* More time = more accurate drift correction*/ +#define SDL_GAMEPAD_IMU_NOISE_SETTLING_PERIOD_NS (1 * SDL_NS_PER_SECOND) +#define SDL_GAMEPAD_IMU_NOISE_EVALUATION_PERIOD_NS (4 * SDL_NS_PER_SECOND) +#define SDL_GAMEPAD_IMU_NOISE_PROFILING_PHASE_DURATION_NS (SDL_GAMEPAD_IMU_NOISE_SETTLING_PERIOD_NS + SDL_GAMEPAD_IMU_NOISE_EVALUATION_PERIOD_NS) +#define SDL_GAMEPAD_IMU_CALIBRATION_PHASE_DURATION_NS (5 * SDL_NS_PER_SECOND) + +/* + * Find the maximum accelerometer noise over the duration of the GYRO_CALIBRATION_PHASE_NOISE_PROFILING phase. + */ +void CalibrationPhase_NoiseProfiling(IMUState *imustate) +{ + /* If we have really large movement (i.e. greater than a fraction of G), then we want to start noise evaluation over. The frontend will warn the user to put down the controller. */ + const float flAbsoluteMaxAccelerationG = 0.125f; + if (imustate->accelerometer_length_squared > (flAbsoluteMaxAccelerationG * flAbsoluteMaxAccelerationG) ) { + BeginNoiseCalibrationPhase(imustate); + return; + } + + Uint64 now = SDL_GetTicksNS(); + Uint64 delta_ns = now - imustate->calibration_phase_start_time_ticks_ns; + + /* Nuanced behavior - give the evaluation system some time to settle after placing the controller down before _actually_ evaluating, as the accelerometer could still be "ringing" after the user has placed it down, resulting in exaggerated tolerances */ + if (delta_ns > SDL_GAMEPAD_IMU_NOISE_SETTLING_PERIOD_NS) { + /* Get the largest noise spike in the period of evaluation */ + if (imustate->accelerometer_length_squared > imustate->accelerometer_tolerance_squared) { + imustate->accelerometer_tolerance_squared = imustate->accelerometer_length_squared; + } + } + + /* Switch phase if we go over the time limit */ + if (delta_ns >= SDL_GAMEPAD_IMU_NOISE_PROFILING_PHASE_DURATION_NS) { + BeginDriftCalibrationPhase(imustate); + } +} /* * Average drift _per packet_ as opposed to _per second_ @@ -200,36 +251,22 @@ void ResetGyroOrientation(IMUState *imustate) */ void FinalizeDriftSolution(IMUState *imustate) { - if (imustate->gyro_drift_sample_count >= SDL_GAMEPAD_IMU_MIN_GYRO_DRIFT_SAMPLE_COUNT) { + if (imustate->gyro_drift_sample_count >= 0) { 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; + imustate->calibration_phase = GYRO_CALIBRATION_PHASE_COMPLETE; ResetGyroOrientation(imustate); } -/* Sample gyro packet in order to calculate drift*/ -void SampleGyroPacketForDrift( IMUState *imustate ) +void CalibrationPhase_DriftProfiling(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) { + if (imustate->accelerometer_length_squared > imustate->accelerometer_tolerance_squared) { /* Reset the drift calibration if the accelerometer has moved significantly */ - StartGyroDriftCalibration(imustate); + BeginDriftCalibrationPhase(imustate); } else { /* Sensor is stationary enough to evaluate for drift.*/ ++imustate->gyro_drift_sample_count; @@ -238,12 +275,33 @@ void SampleGyroPacketForDrift( IMUState *imustate ) 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) { + /* Finish phase if we go over the time limit */ + Uint64 now = SDL_GetTicksNS(); + Uint64 delta_ns = now - imustate->calibration_phase_start_time_ticks_ns; + if (delta_ns >= SDL_GAMEPAD_IMU_CALIBRATION_PHASE_DURATION_NS) { FinalizeDriftSolution(imustate); } } } +/* Sample gyro packet in order to calculate drift*/ +void SampleGyroPacketForDrift(IMUState *imustate) +{ + /* 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]; + + if (imustate->calibration_phase == GYRO_CALIBRATION_PHASE_NOISE_PROFILING) + CalibrationPhase_NoiseProfiling(imustate); + + if (imustate->calibration_phase == GYRO_CALIBRATION_PHASE_DRIFT_PROFILING) + CalibrationPhase_DriftProfiling(imustate); +} + void ApplyDriftSolution(float *gyro_data, const float *drift_solution) { gyro_data[0] -= drift_solution[0]; @@ -1444,7 +1502,18 @@ static void HandleGamepadSensorEvent( SDL_Event* event ) float display_euler_angles[3]; 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; + /* Show how far we are through the current phase. When off, just default to zero progress */ + Uint64 now = SDL_GetTicksNS(); + float duration = 0.0f; + if (controller->imu_state->calibration_phase == GYRO_CALIBRATION_PHASE_NOISE_PROFILING) { + duration = SDL_GAMEPAD_IMU_NOISE_PROFILING_PHASE_DURATION_NS; + } else if (controller->imu_state->calibration_phase == GYRO_CALIBRATION_PHASE_DRIFT_PROFILING) { + duration = SDL_GAMEPAD_IMU_CALIBRATION_PHASE_DURATION_NS; + } + + Uint64 delta_ns = now - controller->imu_state->calibration_phase_start_time_ticks_ns; + float drift_calibration_progress_frac = duration > 0.0f ? ((float)delta_ns / (float)duration) : 0.0f; + int reported_polling_rate_hz = sensorTimeStampDelta_ns > 0 ? (int)(SDL_NS_PER_SECOND / sensorTimeStampDelta_ns) : 0; /* Send the results to the frontend */ @@ -1454,8 +1523,11 @@ static void HandleGamepadSensorEvent( SDL_Event* event ) &controller->imu_state->integrated_rotation, reported_polling_rate_hz, controller->imu_state->imu_estimated_sensor_rate, + controller->imu_state->calibration_phase, drift_calibration_progress_frac, - controller->imu_state->accelerometer_length_squared + controller->imu_state->accelerometer_length_squared, + controller->imu_state->accelerometer_tolerance_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) */ @@ -2145,7 +2217,7 @@ SDL_AppResult SDLCALL SDL_AppEvent(void *appstate, SDL_Event *event) 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); + BeginNoiseCalibrationPhase(controller->imu_state); } else if (GamepadButtonContains(setup_mapping_button, event->button.x, event->button.y)) { SetDisplayMode(CONTROLLER_MODE_BINDING); }