diff --git a/test/gamepadutils.c b/test/gamepadutils.c index 94390cc2c0..4c2590e7b4 100644 --- a/test/gamepadutils.c +++ b/test/gamepadutils.c @@ -30,6 +30,217 @@ #include "gamepad_wired.h" #include "gamepad_wireless.h" +#include + +#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) { diff --git a/test/gamepadutils.h b/test/gamepadutils.h index c08261d171..f6eeaf010e 100644 --- a/test/gamepadutils.h +++ b/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 */ diff --git a/test/testcontroller.c b/test/testcontroller.c index b717b30eb9..65ce004523 100644 --- a/test/testcontroller.c +++ b/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);