From dc13b194ce9c90f3be64c1d9bf217fa5045bfdb2 Mon Sep 17 00:00:00 2001 From: palxex Date: Sat, 7 Nov 2026 18:09:48 +0800 Subject: [PATCH 1/2] Add gyro bias calibration for the six-axis sensor The yaw axis has no gravity reference, so it drifts with the gyro's manufacturing bias. Two controllers of the same model can drift in opposite directions, and the sensor fusion had no way to estimate or remove the offset: aiming with motion turned slowly on its own and needed constant correction. A Calibrate gyro row on the Controls tab collects 250 gyro samples (about a second) while the controller is held still, averages them per axis into a bias, subtracts that bias from every sample before the fusion integrates it, and restarts the angle so the estimate starts clean. Collection has no motion threshold: a controller whose bias exceeds any absolute threshold would otherwise never finish, which is how the first version of this stalled with no way out. Five seconds without the expected samples ends the attempt, from the sample path and from the menu alike, so a sensor thread that stops feeding it is reported instead of leaving the row on "calibrating". The first raw sample and the final bias are logged, so a report carries the numbers that were measured. The row's value says where the controller stands: calibrating, ", calibrated" once it is done or when the bias was loaded from totk.ini at start, and "calibration failed" when an attempt timed out with no bias in effect. A controller change cancels a calibration that is in progress and clears the bias and the label, so neither is carried over to the new controller. The behaviour sits behind gyro_calibration (the Controls tab's "Gyro bias calibration", default on, live): off, the fusion takes the raw gyro, an attempt in progress is dropped, and nothing is loaded at start, while the saved keys stay in totk.ini so turning it back on restores the last calibration. The bias and the controller's name go to totk.ini when calibration completes, outside the fusion's lock, and are loaded at the next start only when the connected controller has the same name. Tested with a Switch Pro Controller on the 1.4.3 build: the log shows a z-axis bias of 0.139 rad/s (8 degrees per second), calibration finishes in about a second, the yaw drift is gone, and the row reports each state. --- runtime/cfg.c | 13 ++- runtime/input.c | 65 ++++++++++++++ runtime/input.h | 14 +++ runtime/motion_fusion.c | 194 +++++++++++++++++++++++++++++++++++++++- runtime/ui/menu.cpp | 16 ++++ runtime/ui_text.c | 31 +++++++ runtime/ui_text.h | 3 + totk.ini.example | 12 +++ 8 files changed, 343 insertions(+), 5 deletions(-) diff --git a/runtime/cfg.c b/runtime/cfg.c index bcbaadc3..ee59d716 100644 --- a/runtime/cfg.c +++ b/runtime/cfg.c @@ -49,7 +49,8 @@ static const char *const known[] = { "GAMEPLAY_MAP_ZOOM", "GAMEPLAY_MAP_ZOOM_SENSITIVITY", "GAMEPLAY_ROTATION_SENSITIVITY", "GAMEPLAY_TWEAKS_ENABLED", "GAMEPLAY_ULTRAHAND_C_AXIS", "GAMEPLAY_ULTRAHAND_DISTANCE", "GAMEPLAY_ULTRAHAND_ROTATE", "GAMEPLAY_WEAPON_SENSITIVITY", "GAMEPLAY_WEAPON_WHEEL", - "GPL", "GPL_DRAW_BUILD", "GPU", "GPU_INDEX", "GPU_NAME", "GRASS_RANGE", "HDR", "HDR_GAME_NITS", "HDR_HUE", + "GPL", "GPL_DRAW_BUILD", "GPU", "GPU_INDEX", "GPU_NAME", "GRASS_RANGE", + "GYRO_BIAS_PAD", "GYRO_BIAS_X", "GYRO_BIAS_Y", "GYRO_BIAS_Z", "GYRO_CALIBRATION", "HDR", "HDR_GAME_NITS", "HDR_HUE", "HDR_PEAK_NITS", "HDR_UI_NITS", "HIGHFPS_FIX", "INPUT", "INPUT_RECORD", "INPUT_SCRIPT", "KSPIN_US", "LANG", "LIB_PREBUILD", "LIB_PREBUILD_KEEP", "LIB_PREBUILD_THREADS", "LOCALE", "LOD_GUARD", "LSTICK_DEADZONE", "MAP_CLICK", "MEMORY", "MEMX", "MENU_LANG", "MENU_TEST", "MENU_FONT_FONTCONFIG", "MEM_RESERVE_MIB", "MENU_CURSOR", "MENU_CURSOR_LOG", "MENU_TEST_DIR", "MENU_TEST_SET", "MINIMAL_LAYOUT", "MIRROR_GAP", "MODS_DIR", "MODS_DISABLED", "MODS_ORDER", "MODS_REBUILD", "MOTION", "MOUSE_AIM_SCALE", "MOUSE_MODE", "MOUSE_DIRECT_PATCH", "MOUSE_SENS", "MOVIE_ASPECT", "MOVIE_HOST", "MOVIE_PACE", @@ -91,7 +92,8 @@ static const struct { const char *key, *section; } sections[] = { {"TEX_HEAP_EXTRA_MB", "mods"}, {"MENU_LANG", "language"}, {"MENU_FONT_FONTCONFIG", "language"}, {"LANG", "language"}, {"OPERATION_MODE", "language"}, {"VOLUME", "audio"}, {"VOLUME_MUSIC", "audio"}, {"VOLUME_SFX", "audio"}, {"VOLUME_VOICE", "audio"}, {"AUDIO_CHANNELS", "audio"}, - {"MOTION", "input"}, {"RUMBLE_STRENGTH", "input"}, {"RUMBLE_CURVE", "input"}, {"RUMBLE_TRIGGERS", "input"}, + {"MOTION", "input"}, {"GYRO_CALIBRATION", "input"}, {"RUMBLE_STRENGTH", "input"}, {"RUMBLE_CURVE", "input"}, {"RUMBLE_TRIGGERS", "input"}, + {"GYRO_BIAS_X", "input"}, {"GYRO_BIAS_Y", "input"}, {"GYRO_BIAS_Z", "input"}, {"GYRO_BIAS_PAD", "input"}, {"PAD_LAYOUT", "input"}, {"PROMPTS", "input"}, {"STICK_DEADZONE", "input"}, {"LSTICK_DEADZONE", "input"}, {"RSTICK_DEADZONE", "input"}, {"STICK_SENS", "input"}, {"MENU_CURSOR", "input"}, {"MAP_CLICK", "input"}, {"DIALOGUE_CLICK", "input"}, {"POINTER_IMAGE", "input"}, {"MOUSE_MODE", "input"}, {"MOUSE_SENS", "input"}, {"MOUSE_AIM_SCALE", "input"}, {"WALK_KEY", "input"}, {"WALK_SCALE", "input"}, {"INPUT_RECORD", "input"}, @@ -162,7 +164,12 @@ static const CfgMeta metas[] = { {"ASSUME_FOCUS", "1", "0|1", LIVE}, // read on every focus change (ov_system.c) // Sound, Controls, Menu {"VOLUME", "100", "0..100", LIVE}, {"VOLUME_MUSIC", "100", "0..100", LIVE}, {"VOLUME_SFX", "100", "0..100", LIVE}, {"VOLUME_VOICE", "100", "0..100", LIVE}, {"AUDIO_CHANNELS", "stereo", "auto|stereo|5.1", RESTART}, - {"MOTION", "1", "0|1", LIVE}, {"RUMBLE_STRENGTH", "130", "0..300", LIVE}, {"RUMBLE_CURVE", "0.6", "0.2..1.5", LIVE}, + {"MOTION", "1", "0|1", LIVE}, {"GYRO_CALIBRATION", "1", "0|1", LIVE}, + {"RUMBLE_STRENGTH", "130", "0..300", LIVE}, {"RUMBLE_CURVE", "0.6", "0.2..1.5", LIVE}, + // Written by the Calibrate gyro row (motion_fusion.c), not by hand; read at start when gyro_bias_pad names the + // connected controller. + {"GYRO_BIAS_X", "", NULL, RESTART}, {"GYRO_BIAS_Y", "", NULL, RESTART}, {"GYRO_BIAS_Z", "", NULL, RESTART}, + {"GYRO_BIAS_PAD", "", NULL, RESTART}, {"RUMBLE_TRIGGERS", "0", "0|1", LIVE}, {"PAD_LAYOUT", "auto", "auto|nintendo|xbox", LIVE}, {"PROMPTS", "nintendo", "nintendo|xbox|pc", RESTART}, {"STICK_DEADZONE", "8", "0..50", LIVE}, {"STICK_SENS", "1", "0.1..3", RESTART}, diff --git a/runtime/input.c b/runtime/input.c index 65f4de0a..465b0d9c 100644 --- a/runtime/input.c +++ b/runtime/input.c @@ -46,6 +46,7 @@ static volatile int32_t pad_layout_shown = -1; // pad_layout for the menu (any static SDL_JoystickID sensor_pad; // the gamepad whose sensors the motion thread set up, 0 none static int sensor_ok; // ... and it has both sensors, enabled static int motion_thread; // 0 not started, 1 running, -1 off (TOTK_MOTION=0, no SDL) +static char calib_pad_name[128]; // the controller the bias belongs to; empty when there is none // SDL buttons of the gamepad inputs (input_map.h PAD_*), by position; PAD_ZL and PAD_ZR are the triggers (axes). static const SDL_GamepadButton pad_sdl[PAD_COUNT] = { @@ -959,6 +960,12 @@ void input_motion_axes(const float a[3], const float g[3], double acc[3], double static int sensors_ready(void) { SDL_JoystickID id = SDL_GetGamepadID(pad); if (id == sensor_pad) return sensor_ok; + // A different controller: the old one's bias estimate does not apply. Only on a change, so the first call does + // not drop a bias that input_motion_start loaded from totk.ini for this controller. + if (sensor_pad) { + motion_bias_reset(motion_global()); + calib_pad_name[0] = 0; + } sensor_pad = id; // Nothing drains the SDL event queue: no event per sample (the data still updates, SDL_SendJoystickSensor) SDL_SetEventEnabled(SDL_EVENT_GAMEPAD_SENSOR_UPDATE, false); @@ -1007,10 +1014,37 @@ static void motion_main(void *arg) { } } +// Copy a controller name into cap bytes without splitting a UTF-8 sequence: a name cut mid-character would show as +// garbage in the menu and be written to totk.ini the same way. +static void copy_name(char *dst, size_t cap, const char *src) { + if (!src) src = ""; + size_t n = strlen(src); + if (n >= cap) { + n = cap - 1; + while (n > 0 && ((unsigned char)src[n] & 0xC0) == 0x80) n--; // back off to the start of the character + } + memcpy(dst, src, n); + dst[n] = 0; +} + void input_motion_start(void) { plat_mutex_lock(&lock); if (!state) start(); if (!motion_thread) { + // Load the calibrated bias from totk.ini when it was measured for the controller that is connected now and + // the switch is on; the keys stay in the file either way, so turning the switch back on restores the bias. + const char *name = pad && SDL_GamepadConnected(pad) ? SDL_GetGamepadName(pad) : NULL; + const char *calib_pad = cfg_get("TOTK_GYRO_BIAS_PAD"); + if (name && calib_pad && strcmp(calib_pad, name) == 0 && motion_calibration_enabled()) { + motion_bias_load(motion_global()); + copy_name(calib_pad_name, sizeof calib_pad_name, name); + rt_log("input: gyro bias loaded from totk.ini for %s", name); + } else { + // No usable bias for this controller: drop whatever an earlier start left behind, so the row does not + // report another controller's calibration. + calib_pad_name[0] = 0; + motion_bias_reset(motion_global()); + } if (!input_motion_enabled()) rt_log("input: motion off (TOTK_MOTION=0); it can be turned on while the game runs"); if (state != 1) motion_thread = -1; else { @@ -1020,3 +1054,34 @@ void input_motion_start(void) { } plat_mutex_unlock(&lock); } + +void input_motion_calibrate(void) { + plat_mutex_lock(&lock); + if (!input_gyro_calibration_enabled()) { + rt_log("input: calibrate gyro ignored: gyro_calibration is off"); + } else if (motion_thread != 1 || !pad) { + // Nothing would collect the samples; say so rather than leaving the row looking pressed. + rt_log("input: calibrate gyro ignored: %s", motion_thread != 1 ? "motion is off (TOTK_MOTION=0)" : "no gamepad"); + } else { + const char *name = SDL_GetGamepadName(pad); + copy_name(calib_pad_name, sizeof calib_pad_name, name); + motion_calibrate_start(motion_global(), calib_pad_name); + } + plat_mutex_unlock(&lock); +} + +int input_gyro_calibration_enabled(void) { return motion_calibration_enabled(); } + +int input_motion_calib_state(char *pad, size_t cap) { + plat_mutex_lock(&lock); + int st = 0; + if (motion_thread == 1) { + st = motion_calib_poll(motion_global(), kern_now_ns()); // also ends an attempt whose thread stalled + // Loaded from totk.ini at start, no attempt in this run; with the switch off the bias does not apply, so + // the row reads as before any calibration. + if (st == 0 && calib_pad_name[0] && input_gyro_calibration_enabled()) st = 2; + } + if (pad && cap) snprintf(pad, cap, "%s", calib_pad_name); + plat_mutex_unlock(&lock); + return st; +} diff --git a/runtime/input.h b/runtime/input.h index 86f26fa7..f2162fc4 100644 --- a/runtime/input.h +++ b/runtime/input.h @@ -114,6 +114,16 @@ int input_rumble_triggers_enabled(void); void input_motion_start(void); // TOTK_MOTION is not "0" (a live setting: read again when cfg_version changes). Off: the sensor reads at rest. int input_motion_enabled(void); +void input_motion_calibrate(void); // start collecting resting gyro samples for a bias estimate +// What the Controls tab's Calibrate gyro row shows: 0 no bias for this controller, 1 collecting, 2 calibrated +// (by this run or loaded from totk.ini), 3 the last attempt timed out. pad receives the controller the bias belongs +// to (empty when none), copied so the caller does not read a buffer another thread may be rewriting. +int input_motion_calib_state(char *pad, size_t cap); +// The bias's own switch (gyro_calibration, live). Off: no collection, the fusion takes the raw gyro, and the row's +// state reads as before any calibration while the saved keys stay in totk.ini. +int input_gyro_calibration_enabled(void); +// The fusion's own read of the same switch (motion_fusion.c); input.c reaches it for the load and the row's state. +int motion_calibration_enabled(void); // SDL sensor values (accelerometer m/s^2, gyro rad/s, SDL gamepad axes) to the Switch controller frame motion_feed // takes (acceleration in G as gravity reads, rotation rad/s). The mapping is documented in input_win.c. void input_motion_axes(const float sdl_acc[3], const float sdl_gyro[3], double acc_g[3], double gyro[3]); @@ -131,3 +141,7 @@ MotionFusion *motion_global(void); // the gamepad's, fed by input_win.c void motion_feed(MotionFusion *m, const double acc_g[3], const double gyro_rad_s[3], uint64_t now_ns); int motion_read(MotionFusion *m, MotionState *out, int max, uint64_t now_ns); // newest first; 0 = none in 200 ms void motion_restart(MotionFusion *m); // angle and direction start over at the next sample +void motion_calibrate_start(MotionFusion *m, const char *pad_name); // start collecting; pad_name saved with the bias +int motion_calib_poll(MotionFusion *m, uint64_t now_ns); // the timeout, for when motion_feed is not called +void motion_bias_load(MotionFusion *m); // load the bias from totk.ini (TOTK_GYRO_BIAS_*) +void motion_bias_reset(MotionFusion *m); // zero the bias (new controller) diff --git a/runtime/motion_fusion.c b/runtime/motion_fusion.c index 7bbf3be6..71552219 100644 --- a/runtime/motion_fusion.c +++ b/runtime/motion_fusion.c @@ -14,12 +14,14 @@ #include #include +#include "cfg.h" #include "host.h" #include "input.h" #include "platform/platform.h" -enum { MOTION_RING = 64 }; +enum { MOTION_RING = 64, CALIB_SAMPLES = 250 }; #define STALE_NS 200000000ull // no sample for 200 ms: no motion +#define CALIB_TIMEOUT_NS 5000000000ull // 5 s: give up if sensor data stops arriving mid-calibration struct MotionFusion { PlatMutex lock; @@ -29,6 +31,14 @@ struct MotionFusion { double R[3][3]; // controller -> reference; row i = reference axis i in controller coordinates double angle[3]; int restart; + double gyro_bias[3]; // estimated resting bias (rad/s), subtracted from the gyro + int calibrating; // 1: collecting resting samples for a new bias + double calib_sum[3]; // accumulated gyro samples during calibration + int calib_count; // samples collected so far + uint64_t calib_start_ns; // when calibration started (timeout check) + char calib_pad[128]; // controller name that was calibrated (for loading the bias on the next run) + int calib_result; // 0 no attempt or one in progress, 1 the last attempt finished, -1 it timed out + int bias_valid; // 1 when gyro_bias holds a real estimate (loaded or measured), so a zero is a value }; MotionFusion *motion_new(void) { @@ -107,11 +117,111 @@ static void integrate(double R[3][3], const double gyro[3], const double acc[3], orthonormalize(R); } -void motion_feed(MotionFusion *m, const double acc[3], const double gyro[3], uint64_t now) { +// Write the calibrated bias to totk.ini. Called with m->lock released: cfg_set takes cfg.c's own lock and cfg_save +// rewrites the file, and neither belongs inside the sensor thread's critical section. +static void save_bias(const double bias[3], const char *pad, int samples) { + char buf[32]; + snprintf(buf, sizeof buf, "%.6f", bias[0]); + cfg_set("TOTK_GYRO_BIAS_X", buf); + snprintf(buf, sizeof buf, "%.6f", bias[1]); + cfg_set("TOTK_GYRO_BIAS_Y", buf); + snprintf(buf, sizeof buf, "%.6f", bias[2]); + cfg_set("TOTK_GYRO_BIAS_Z", buf); + if (pad[0]) cfg_set("TOTK_GYRO_BIAS_PAD", pad); + // Saved now rather than at the menu's save tick, which may not run if the menu is closed first. + if (cfg_save()) + rt_log("motion: gyro bias calibrated from %d samples: %.5f %.5f %.5f rad/s", samples, bias[0], bias[1], bias[2]); + else + rt_log("motion: gyro bias %.5f %.5f %.5f rad/s could not be written to totk.ini; it applies to this run only", + bias[0], bias[1], bias[2]); +} + +// The one timeout decision, for the sample path and the menu's poll alike, so the two cannot drift apart. Called +// with m->lock held; the caller logs, with the lock released. +static int calib_check_timeout(MotionFusion *m, uint64_t now) { + if (!m->calibrating || now - m->calib_start_ns <= CALIB_TIMEOUT_NS) return 0; + m->calibrating = 0; + // A bias that is already in effect stays in effect, and stays reported as calibrated: the row describes the bias + // that is applied, and the failed attempt is in the log. A controller with no bias at all shows the failure. + m->calib_result = m->bias_valid ? 1 : -1; + return 1; +} + +// Copy a name into cap bytes without splitting a UTF-8 sequence; the same as input.c's copy_name, which this file +// cannot include from. +static void copy_name(char *dst, size_t cap, const char *src) { + if (!src) src = ""; + size_t n = strlen(src); + if (n >= cap) { + n = cap - 1; + while (n > 0 && ((unsigned char)src[n] & 0xC0) == 0x80) n--; // back off to the start of the character + } + memcpy(dst, src, n); + dst[n] = 0; +} + +// The bias's own switch (gyro_calibration, live, default 1): off, the fusion takes the raw gyro, an attempt in +// progress is dropped, and the menu's calibrate entry does nothing. The saved keys stay untouched, so turning it +// back on restores the last calibration. +int motion_calibration_enabled(void) { + static int on = -1; + static unsigned seen; + unsigned v = cfg_version(); + if (on < 0 || v != seen) { + seen = v; + on = cfg_bool("TOTK_GYRO_CALIBRATION"); + } + return on; +} + +void motion_feed(MotionFusion *m, const double acc[3], const double gyro_raw[3], uint64_t now) { + double saved[3] = {0, 0, 0}; + char saved_pad[sizeof m->calib_pad] = ""; + int save = 0, samples = 0, first = 0, logged_timeout = 0; + uint64_t saved_start = 0; plat_mutex_lock(&m->lock); double dt = m->last_ns && now > m->last_ns ? (double)(now - m->last_ns) / 1e9 : 0.004; if (dt > 0.05) dt = 0.004; // a gap (no controller, sensors were off): no integration across it m->last_ns = now; + // Calibration: collect gyro samples for CALIB_SAMPLES frames. The user holds the controller still, so the + // average of the raw gyro (which reads the bias at rest) is the bias estimate. No motion threshold: the bias + // itself can exceed any absolute threshold, which would block collection forever. calib_check_timeout decides + // the deadline for both this path and the menu's poll, so the two cannot drift apart. With the calibration + // switch off, an attempt in progress is dropped and no new one starts; the bias stops applying below. + if (m->calibrating && !motion_calibration_enabled()) { + m->calibrating = 0; + m->calib_result = 0; + } + if (m->calibrating) { + if (calib_check_timeout(m, now)) { + logged_timeout = 1; + samples = m->calib_count; + saved_start = m->calib_start_ns; + } + if (m->calibrating) { + for (int i = 0; i < 3; i++) m->calib_sum[i] += gyro_raw[i]; + m->calib_count++; + first = m->calib_count == 1; + if (m->calib_count >= CALIB_SAMPLES) { + for (int i = 0; i < 3; i++) m->gyro_bias[i] = m->calib_sum[i] / m->calib_count; + m->calibrating = 0; + m->calib_result = 1; + m->bias_valid = 1; + m->restart = 1; + // Hand the result to save_bias below, which runs with the lock released. + memcpy(saved, m->gyro_bias, sizeof saved); + snprintf(saved_pad, sizeof saved_pad, "%s", m->calib_pad); + samples = m->calib_count; + save = 1; + } + } + } + // Subtract the bias from the gyro for integration and storage. + // The bias applies only while the switch is on: off, the fusion sees the raw gyro again. + const int calib_on = motion_calibration_enabled(); + double gyro[3] = {gyro_raw[0] - (calib_on ? m->gyro_bias[0] : 0), + gyro_raw[1] - (calib_on ? m->gyro_bias[1] : 0), + gyro_raw[2] - (calib_on ? m->gyro_bias[2] : 0)}; if (m->restart) { align_to(m->R, acc); memset(m->angle, 0, sizeof m->angle); @@ -128,7 +238,17 @@ void motion_feed(MotionFusion *m, const double acc[3], const double gyro[3], uin for (int j = 0; j < 3; j++) s->dir[i * 3 + j] = (float)m->R[j][i]; // direction i = controller axis i in the reference frame } m->produced++; + // The logs run with the lock released: the log back end can be slow, and the menu thread holds this lock in + // motion_calib_poll. plat_mutex_unlock(&m->lock); + if (first) + rt_log("motion: calibration started, first raw gyro: %.5f %.5f %.5f rad/s", gyro_raw[0], gyro_raw[1], + gyro_raw[2]); + if (logged_timeout) { + double secs = (now - saved_start) / 1e9; // captured under the lock; another attempt may have begun since + rt_log("motion: gyro calibration timed out after %.1f s (%d samples): sensor data not arriving", secs, samples); + } + if (save) save_bias(saved, saved_pad, samples); } int motion_read(MotionFusion *m, MotionState *out, int max, uint64_t now) { @@ -152,3 +272,73 @@ void motion_restart(MotionFusion *m) { memset(m->angle, 0, sizeof m->angle); plat_mutex_unlock(&m->lock); } + +void motion_calibrate_start(MotionFusion *m, const char *pad_name) { + plat_mutex_lock(&m->lock); + m->calibrating = 1; + m->calib_result = 0; + m->calib_count = 0; + m->calib_sum[0] = m->calib_sum[1] = m->calib_sum[2] = 0; + m->calib_start_ns = kern_now_ns(); + // The caller passes the same name it put into its own row label; copied again here so the fusion keeps the name + // the bias belongs to without relying on another buffer's lifetime. Same UTF-8-safe cut as input.c's copy_name: + // a name cut mid-sequence would reach totk.ini and the next run's comparisons. + copy_name(m->calib_pad, sizeof m->calib_pad, pad_name); + plat_mutex_unlock(&m->lock); +} + +// One axis of totk.ini; a value that is not a number leaves the axis at zero and is named in the log, so a +// hand-edited or damaged line does not silently read as a calibrated zero. +static double bias_value(const char *key) { + const char *v = cfg_get(key); + if (!v || !*v) return 0; + char *end; + double d = strtod(v, &end); + if (end == v || *end || !isfinite(d)) { + rt_log("motion: %s = \"%s\" is not a number; that axis starts uncalibrated", key, v); + return 0; + } + return d; +} + +void motion_bias_load(MotionFusion *m) { + double bias[3] = {bias_value("TOTK_GYRO_BIAS_X"), bias_value("TOTK_GYRO_BIAS_Y"), + bias_value("TOTK_GYRO_BIAS_Z")}; + plat_mutex_lock(&m->lock); + memcpy(m->gyro_bias, bias, sizeof bias); + m->bias_valid = 1; + m->calib_result = 1; // 1 means "a bias is in effect", whether measured or loaded + plat_mutex_unlock(&m->lock); +} + +void motion_bias_reset(MotionFusion *m) { + plat_mutex_lock(&m->lock); + m->gyro_bias[0] = m->gyro_bias[1] = m->gyro_bias[2] = 0; + // A calibration that was running belonged to the previous controller: its samples must not complete against this + // one, and its name must not label a bias measured from another controller. + m->calibrating = 0; + m->calib_count = 0; + m->calib_sum[0] = m->calib_sum[1] = m->calib_sum[2] = 0; + m->calib_pad[0] = 0; + m->calib_result = 0; + m->bias_valid = 0; + plat_mutex_unlock(&m->lock); +} + +// The timeout, for the case motion_feed is not called at all (the sensor thread stalled): without this the row would +// read "Calibrating..." forever, which is the failure the timeout exists to end. Returns the row's state: 0 none, +// 1 collecting, 2 calibrated, 3 the last attempt timed out. Logs with the lock released: the menu thread holds this +// lock briefly on every redraw, and the sensor thread waits for it. +int motion_calib_poll(MotionFusion *m, uint64_t now) { + plat_mutex_lock(&m->lock); + int timed_out = calib_check_timeout(m, now); + int st = m->calibrating ? 1 : m->calib_result > 0 ? 2 : m->calib_result < 0 ? 3 : 0; + uint64_t start = m->calib_start_ns; + int count = m->calib_count; + plat_mutex_unlock(&m->lock); + if (timed_out) { + rt_log("motion: gyro calibration timed out after %.1f s (%d samples): the sensor thread stopped feeding it", + (now - start) / 1e9, count); + } + return st; +} diff --git a/runtime/ui/menu.cpp b/runtime/ui/menu.cpp index 44031fcd..00d346d3 100644 --- a/runtime/ui/menu.cpp +++ b/runtime/ui/menu.cpp @@ -928,6 +928,22 @@ static void tab_controls(float scale) { opt_choice("TOTK_PROMPTS", UI_MENU_PROMPTS, prompts, 3, UI_MENU_PROMPTS_DESC, note); bool ask = restart && opt_button("##restart", UI_MENU_RESTART, UI_MENU_RESTART_DESC); opt_toggle("TOTK_MOTION", UI_MENU_MOTION, UI_MENU_MOTION_DESC); + opt_toggle("TOTK_GYRO_CALIBRATION", UI_MENU_GYRO_CALIBRATION, UI_MENU_GYRO_CALIBRATION_DESC); + if (input_gyro_calibration_enabled()) { + // The row exists only while the switch is on: with the bias not applied there is nothing to calibrate and no + // state to show. + char pad[128] = ""; + int st = input_motion_calib_state(pad, sizeof pad); + char status[192] = ""; + if (st == 1) + snprintf(status, sizeof status, "%s", menu_text(UI_MENU_GYRO_CALIBRATE_WAIT)); + else if (st == 2) + snprintf(status, sizeof status, menu_text(UI_MENU_GYRO_CALIBRATED), pad); + else if (st == 3) + snprintf(status, sizeof status, "%s", menu_text(UI_MENU_GYRO_CALIBRATE_FAILED)); + if (opt_button("##gyro_calib", UI_MENU_GYRO_CALIBRATE, UI_MENU_GYRO_CALIBRATE_DESC, status)) + input_motion_calibrate(); + } opt_slider("TOTK_RUMBLE_STRENGTH", UI_MENU_RUMBLE_STRENGTH, 0, 300, UI_MENU_RUMBLE_STRENGTH_DESC, 10, "%.0f %%"); opt_slider("TOTK_RUMBLE_CURVE", UI_MENU_RUMBLE_CURVE, 0.2f, 1.5f, UI_MENU_RUMBLE_CURVE_DESC, 0.05f, "%.2f"); opt_toggle("TOTK_RUMBLE_TRIGGERS", UI_MENU_RUMBLE_TRIGGERS, UI_MENU_RUMBLE_TRIGGERS_DESC); diff --git a/runtime/ui_text.c b/runtime/ui_text.c index 54a7b690..0d8f2f2c 100644 --- a/runtime/ui_text.c +++ b/runtime/ui_text.c @@ -251,6 +251,14 @@ static const char *const text_en[UI_TEXT_COUNT] = { [UI_MENU_CHANNELS_AUTO] = "Automatic", [UI_MENU_CHANNELS_STEREO] = "Stereo", [UI_MENU_CHANNELS_51] = "5.1 surround", [UI_MENU_MOTION] = "Motion controls", [UI_MENU_MOTION_DESC] = "Aiming with the controller's gyroscope.", + [UI_MENU_GYRO_CALIBRATION] = "Gyro bias calibration", + [UI_MENU_GYRO_CALIBRATION_DESC] = "Subtracts the controller's resting drift, measured with the calibrate row " + "below. The measured values stay saved when this is off.", + [UI_MENU_GYRO_CALIBRATE] = "Calibrate gyro", + [UI_MENU_GYRO_CALIBRATE_DESC] = "Estimate and remove the gyroscope's resting drift. Hold the controller still.", + [UI_MENU_GYRO_CALIBRATE_WAIT] = "Calibrating...", + [UI_MENU_GYRO_CALIBRATED] = "%s, calibrated", + [UI_MENU_GYRO_CALIBRATE_FAILED] = "Calibration failed", [UI_MENU_RUMBLE_STRENGTH] = "Vibration strength", [UI_MENU_RUMBLE_STRENGTH_DESC] = "How strong the controller vibrates.", [UI_MENU_RUMBLE_CURVE] = "Vibration curve", @@ -782,6 +790,14 @@ static const char *const text_ru[UI_TEXT_COUNT] = { [UI_MENU_CHANNELS_AUTO] = "Автоматически", [UI_MENU_CHANNELS_STEREO] = "Стерео", [UI_MENU_CHANNELS_51] = "Объёмный 5.1", [UI_MENU_MOTION] = "Управление движением", [UI_MENU_MOTION_DESC] = "Прицеливание гироскопом контроллера.", + [UI_MENU_GYRO_CALIBRATION] = "Калибровка смещения гироскопа", + [UI_MENU_GYRO_CALIBRATION_DESC] = "Убирает покоящееся смещение контроллера, измеренное строкой калибровки ниже. " + "Измеренные значения сохраняются и при выключенном состоянии.", + [UI_MENU_GYRO_CALIBRATE] = "Калибровка гироскопа", + [UI_MENU_GYRO_CALIBRATE_DESC] = "Оценить и убрать покоящееся смещение гироскопа. Держите контроллер неподвижно.", + [UI_MENU_GYRO_CALIBRATE_WAIT] = "Калибровка...", + [UI_MENU_GYRO_CALIBRATED] = "%s, откалиброван", + [UI_MENU_GYRO_CALIBRATE_FAILED] = "Калибровка не удалась", [UI_MENU_RUMBLE_STRENGTH] = "Сила вибрации", [UI_MENU_RUMBLE_STRENGTH_DESC] = "Насколько сильно вибрирует контроллер.", [UI_MENU_RUMBLE_CURVE] = "Кривая вибрации", @@ -1232,6 +1248,14 @@ static const char *const text_pt_br[UI_TEXT_COUNT] = { [UI_MENU_CHANNELS_AUTO] = "Automático", [UI_MENU_CHANNELS_STEREO] = "Estéreo", [UI_MENU_CHANNELS_51] = "Surround 5.1", [UI_MENU_MOTION] = "Controles de movimento", [UI_MENU_MOTION_DESC] = "Mirar com o giroscópio do controle.", + [UI_MENU_GYRO_CALIBRATION] = "Calibração do desvio do giroscópio", + [UI_MENU_GYRO_CALIBRATION_DESC] = "Remove o desvio de repouso do controle, medido pela linha de calibração " + "abaixo. Os valores medidos ficam salvos mesmo com isto desligado.", + [UI_MENU_GYRO_CALIBRATE] = "Calibrar giroscópio", + [UI_MENU_GYRO_CALIBRATE_DESC] = "Estimar e remover o desvio de repouso do giroscópio. Segure o controle parado.", + [UI_MENU_GYRO_CALIBRATE_WAIT] = "Calibrando...", + [UI_MENU_GYRO_CALIBRATED] = "%s, calibrado", + [UI_MENU_GYRO_CALIBRATE_FAILED] = "Falha na calibração", [UI_MENU_RUMBLE_STRENGTH] = "Intensidade da vibração", [UI_MENU_RUMBLE_STRENGTH_DESC] = "Quão forte o controle vibra.", [UI_MENU_RUMBLE_CURVE] = "Curva da vibração", @@ -1845,6 +1869,13 @@ static const char *const text_zh_hans[UI_TEXT_COUNT] = { [UI_MENU_CHANNELS_AUTO] = "自动", [UI_MENU_CHANNELS_STEREO] = "立体声", [UI_MENU_CHANNELS_51] = "5.1 环绕声", [UI_MENU_MOTION] = "体感操作", [UI_MENU_MOTION_DESC] = "用手柄的陀螺仪瞄准。", + [UI_MENU_GYRO_CALIBRATION] = "陀螺仪偏置校准", + [UI_MENU_GYRO_CALIBRATION_DESC] = "减去下面校准行测得的手柄静止偏移。关闭时测量值仍然保留。", + [UI_MENU_GYRO_CALIBRATE] = "校准陀螺仪", + [UI_MENU_GYRO_CALIBRATE_DESC] = "估计并消除陀螺仪的静止偏移。请保持手柄静止。", + [UI_MENU_GYRO_CALIBRATE_WAIT] = "校准中...", + [UI_MENU_GYRO_CALIBRATED] = "%s,已校准", + [UI_MENU_GYRO_CALIBRATE_FAILED] = "校准失败", [UI_MENU_RUMBLE_STRENGTH] = "振动强度", [UI_MENU_RUMBLE_STRENGTH_DESC] = "手柄振动的强弱。", [UI_MENU_RUMBLE_CURVE] = "振动曲线", diff --git a/runtime/ui_text.h b/runtime/ui_text.h index 0c9ee4e6..7ebc2443 100644 --- a/runtime/ui_text.h +++ b/runtime/ui_text.h @@ -106,6 +106,9 @@ typedef enum { // Controls UI_MENU_MOTION, UI_MENU_MOTION_DESC, + UI_MENU_GYRO_CALIBRATION, UI_MENU_GYRO_CALIBRATION_DESC, + UI_MENU_GYRO_CALIBRATE, UI_MENU_GYRO_CALIBRATE_DESC, UI_MENU_GYRO_CALIBRATE_WAIT, + UI_MENU_GYRO_CALIBRATED, UI_MENU_GYRO_CALIBRATE_FAILED, UI_MENU_RUMBLE_STRENGTH, UI_MENU_RUMBLE_STRENGTH_DESC, UI_MENU_RUMBLE_CURVE, UI_MENU_RUMBLE_CURVE_DESC, UI_MENU_RUMBLE_TRIGGERS, UI_MENU_RUMBLE_TRIGGERS_DESC, diff --git a/totk.ini.example b/totk.ini.example index 255ecc0e..bfd7af13 100644 --- a/totk.ini.example +++ b/totk.ini.example @@ -343,6 +343,18 @@ [input] # Gamepad motion controls (0 = off) and vibration (0 = off). # motion = 1 +# Gyro bias calibration (1 = on): the Controls tab's "Calibrate gyro" row measures the controller's resting drift and +# subtracts it from the gyro readings, which removes the yaw drift. Off, the fusion takes the raw gyro, an attempt in +# progress is dropped and nothing is loaded at start; the four keys below stay in the file, so turning it back on +# restores the last calibration. +# gyro_calibration = 1 +# The measured bias and the controller it was measured from, written by the game when calibration finishes; they are +# not meant to be typed. The bias is loaded at the next start only when gyro_bias_pad names the controller that is +# connected, so calibrate each controller separately: another one starts with no bias until it is calibrated. +# gyro_bias_x = 0 +# gyro_bias_y = 0 +# gyro_bias_z = 0 +# gyro_bias_pad = Nintendo Switch Pro Controller # rumble = 1 # Rumble strength in percent (0 to 300; 0 = off) and the exponent on the amplitude (0.2 to 1.5; lower = weak pulses # stronger, 1 = linear). rumble_triggers = 1 also drives the impulse triggers of an Xbox pad. From 0efa904d9025166957596ebb166e9bea5f6fa0da Mon Sep 17 00:00:00 2001 From: Valiantsin Dzerakh Date: Sun, 15 Nov 2026 09:32:05 +0300 Subject: [PATCH 2/2] A new controller no longer inherits the previous one's gyro bias, and a calibration where the pad moved is rejected The bias reset in sensors_ready only ran when sensor_pad was nonzero, but scan_pads zeroes it on every disconnect, so a pad connected after another one kept the old pad's bias, and an attempt still collecting finished on the new pad under the old pad's name. The bias was also loaded only once, at the first StartSixAxisSensor. - input.c pad_bias_select runs for every pad the sensor code sees: it drops the old bias and attempt, then loads the saved bias when gyro_bias_pad names this pad. scan_pads drops them on disconnect. The bias loads with gyro_calibration off too, so turning the switch on mid-run applies it. - The pad identity is the name plus an FNV-1a hash of SDL's serial when the pad reports one, so two pads of one model no longer share a bias; the hash keeps the Bluetooth address out of totk.ini and the log. - An attempt skips 0.5 s of samples (the button press shakes the pad), then averages 1 s. It is rejected when the largest per-axis standard deviation exceeds 0.04 rad/s; a steady bias of any size passes. The row then says the controller moved (new UI_MENU_GYRO_CALIBRATE_MOVED in all four catalogs) and the bias in effect stays. - The timeout counts 5 s without a sample, not 5 s from the start. - Calibrate gyro starts the sensor thread when the game has not yet (title screen) instead of doing nothing. - Row states are an enum (MOTION_CALIB_*); motion_calibrate_start takes the clock, and only the gamepad's fusion instance writes totk.ini, so --probe-services now checks a measured bias, a rejected attempt and the timeout. - The duplicate copy_name in motion_fusion.c is gone. Co-Authored-By: Claude Opus 5.5 --- runtime/cfg.c | 4 +- runtime/input.c | 131 ++++++++++++++----------- runtime/input.h | 24 +++-- runtime/motion_fusion.c | 210 +++++++++++++++++++++------------------- runtime/ov_probe.c | 37 +++++++ runtime/ui/menu.cpp | 8 +- runtime/ui_text.c | 15 ++- runtime/ui_text.h | 2 +- totk.ini.example | 18 ++-- 9 files changed, 267 insertions(+), 182 deletions(-) diff --git a/runtime/cfg.c b/runtime/cfg.c index 5f138369..95450916 100644 --- a/runtime/cfg.c +++ b/runtime/cfg.c @@ -169,8 +169,8 @@ static const CfgMeta metas[] = { {"AUDIO_DEVICE", "", NULL, LIVE}, // audio_mixer.c output_worker: a device name, empty for the system default {"MOTION", "1", "0|1", LIVE}, {"GYRO_CALIBRATION", "1", "0|1", LIVE}, {"RUMBLE_STRENGTH", "130", "0..300", LIVE}, {"RUMBLE_CURVE", "0.6", "0.2..1.5", LIVE}, - // Written by the Calibrate gyro row (motion_fusion.c), not by hand; read at start when gyro_bias_pad names the - // connected controller. + // Written by the Calibrate gyro row (motion_fusion.c save_bias), not by hand; read by input.c pad_bias_select when a + // controller connects and gyro_bias_pad names it. {"GYRO_BIAS_X", "", NULL, RESTART}, {"GYRO_BIAS_Y", "", NULL, RESTART}, {"GYRO_BIAS_Z", "", NULL, RESTART}, {"GYRO_BIAS_PAD", "", NULL, RESTART}, {"RUMBLE_TRIGGERS", "0", "0|1", LIVE}, {"PAD_LAYOUT", "auto", "auto|nintendo|xbox", LIVE}, diff --git a/runtime/input.c b/runtime/input.c index bc824d3b..a7c4045e 100644 --- a/runtime/input.c +++ b/runtime/input.c @@ -425,6 +425,9 @@ static void scan_pads(void) { if (pad && SDL_GamepadConnected(pad)) return; if (pad) { SDL_CloseGamepad(pad); pad = NULL; sensor_pad = 0; sensor_ok = 0; + // Its bias and an attempt collecting from it go with it; the next pad gets its own in pad_bias_select. + motion_bias_reset(motion_global()); + calib_pad_name[0] = 0; plat_xchg32(&pad_layout_shown, -1); for (int i = 0; i < 4; i++) plat_xchg32(&pad_labels[i], PAD_LABEL_NONE); rt_log("input: gamepad disconnected"); @@ -957,17 +960,56 @@ void input_motion_axes(const float a[3], const float g[3], double acc[3], double gyro[2] = g[1]; } +// Copy a controller name into cap bytes without splitting a UTF-8 sequence: a name cut mid-character would show as +// garbage in the menu and be written to totk.ini the same way. +static void copy_name(char *dst, size_t cap, const char *src) { + if (!src) src = ""; + size_t n = strlen(src); + if (n >= cap) { + n = cap - 1; + while (n > 0 && ((unsigned char)src[n] & 0xC0) == 0x80) n--; // back off to the start of the character + } + memcpy(dst, src, n); + dst[n] = 0; +} + +// Under lock: the open gamepad's identity for the saved bias. Its name, plus a hash of SDL's serial when the pad +// reports one (Switch and PlayStation pads report their Bluetooth address), so two controllers of one model keep their +// own bias; the hash keeps the address out of totk.ini and the log, which players post with bug reports. +static void pad_identity(char *dst, size_t cap) { + const char *name = SDL_GetGamepadName(pad), *serial = SDL_GetGamepadSerial(pad); + char id[160]; + if (serial && *serial) { + uint32_t h = 2166136261u; // FNV-1a + for (const char *c = serial; *c; c++) h = (h ^ (uint8_t)*c) * 16777619u; + snprintf(id, sizeof id, "%s #%08x", name ? name : "", h); + } else snprintf(id, sizeof id, "%s", name ? name : ""); + copy_name(dst, cap, id); +} + +// Under lock, for each gamepad the sensor code sees (a first one, or one connected after another went away): the +// previous controller's bias and any attempt still collecting from it are dropped, then the bias saved in totk.ini is +// taken when gyro_bias_pad names this controller. It is taken with gyro_calibration off too: the switch only decides +// whether motion_feed subtracts it, so turning the switch on mid-run applies the saved bias. +static void pad_bias_select(void) { + char id[128]; + pad_identity(id, sizeof id); + motion_bias_reset(motion_global()); + calib_pad_name[0] = 0; + const char *saved = cfg_get("TOTK_GYRO_BIAS_PAD"); + if (saved && !strcmp(saved, id)) { + motion_bias_load(motion_global()); + copy_name(calib_pad_name, sizeof calib_pad_name, SDL_GetGamepadName(pad)); + rt_log("input: gyro bias loaded from totk.ini for %s", calib_pad_name); + } +} + // Under lock: (re)enable the sensors when the open gamepad changed; 1 if it has both and they are on. static int sensors_ready(void) { SDL_JoystickID id = SDL_GetGamepadID(pad); if (id == sensor_pad) return sensor_ok; - // A different controller: the old one's bias estimate does not apply. Only on a change, so the first call does - // not drop a bias that input_motion_start loaded from totk.ini for this controller. - if (sensor_pad) { - motion_bias_reset(motion_global()); - calib_pad_name[0] = 0; - } sensor_pad = id; + pad_bias_select(); // Nothing drains the SDL event queue: no event per sample (the data still updates, SDL_SendJoystickSensor) SDL_SetEventEnabled(SDL_EVENT_GAMEPAD_SENSOR_UPDATE, false); sensor_ok =SDL_GamepadHasSensor(pad, SDL_SENSOR_GYRO) && SDL_GamepadHasSensor(pad, SDL_SENSOR_ACCEL) && @@ -1015,58 +1057,43 @@ static void motion_main(void *arg) { } } -// Copy a controller name into cap bytes without splitting a UTF-8 sequence: a name cut mid-character would show as -// garbage in the menu and be written to totk.ini the same way. -static void copy_name(char *dst, size_t cap, const char *src) { - if (!src) src = ""; - size_t n = strlen(src); - if (n >= cap) { - n = cap - 1; - while (n > 0 && ((unsigned char)src[n] & 0xC0) == 0x80) n--; // back off to the start of the character +// Under lock: the sensor thread, once; -1 when SDL is not running. +static void motion_thread_start(void) { + if (!state) start(); + if (motion_thread) return; + if (!input_motion_enabled()) rt_log("input: motion off (TOTK_MOTION=0); it can be turned on while the game runs"); + if (state != 1) motion_thread = -1; + else { + if (!plat_thread_spawn(motion_main, NULL, "motion", 0)) rt_fatal("input: cannot start the motion thread"); + motion_thread = 1; } - memcpy(dst, src, n); - dst[n] = 0; } void input_motion_start(void) { plat_mutex_lock(&lock); - if (!state) start(); - if (!motion_thread) { - // Load the calibrated bias from totk.ini when it was measured for the controller that is connected now and - // the switch is on; the keys stay in the file either way, so turning the switch back on restores the bias. - const char *name = pad && SDL_GamepadConnected(pad) ? SDL_GetGamepadName(pad) : NULL; - const char *calib_pad = cfg_get("TOTK_GYRO_BIAS_PAD"); - if (name && calib_pad && strcmp(calib_pad, name) == 0 && motion_calibration_enabled()) { - motion_bias_load(motion_global()); - copy_name(calib_pad_name, sizeof calib_pad_name, name); - rt_log("input: gyro bias loaded from totk.ini for %s", name); - } else { - // No usable bias for this controller: drop whatever an earlier start left behind, so the row does not - // report another controller's calibration. - calib_pad_name[0] = 0; - motion_bias_reset(motion_global()); - } - if (!input_motion_enabled()) rt_log("input: motion off (TOTK_MOTION=0); it can be turned on while the game runs"); - if (state != 1) motion_thread = -1; - else { - if (!plat_thread_spawn(motion_main, NULL, "motion", 0)) rt_fatal("input: cannot start the motion thread"); - motion_thread = 1; - } - } + motion_thread_start(); plat_mutex_unlock(&lock); } void input_motion_calibrate(void) { plat_mutex_lock(&lock); - if (!input_gyro_calibration_enabled()) { - rt_log("input: calibrate gyro ignored: gyro_calibration is off"); - } else if (motion_thread != 1 || !pad) { - // Nothing would collect the samples; say so rather than leaving the row looking pressed. - rt_log("input: calibrate gyro ignored: %s", motion_thread != 1 ? "motion is off (TOTK_MOTION=0)" : "no gamepad"); - } else { - const char *name = SDL_GetGamepadName(pad); - copy_name(calib_pad_name, sizeof calib_pad_name, name); - motion_calibrate_start(motion_global(), calib_pad_name); + // The menu can run before the game starts the six-axis sensor (the title screen): the thread starts here then. + if (input_gyro_calibration_enabled() && input_motion_enabled()) motion_thread_start(); + const char *why = NULL; + if (!input_gyro_calibration_enabled()) why = "gyro_calibration is off"; + else if (!input_motion_enabled()) why = "motion is off (TOTK_MOTION=0)"; + else if (motion_thread != 1) why = "no SDL gamepad support"; + else if (!pad || !SDL_GamepadConnected(pad)) why = "no gamepad"; + // sensors_ready here, not at the sensor thread's next look at a new pad: that look selects the pad's bias and + // would cancel the attempt that starts below. + else if (!sensors_ready()) why = "the gamepad has no gyro and accelerometer"; + if (why) rt_log("input: calibrate gyro ignored: %s", why); + else { + char id[128]; + pad_identity(id, sizeof id); + copy_name(calib_pad_name, sizeof calib_pad_name, SDL_GetGamepadName(pad)); + motion_calibrate_start(motion_global(), id, kern_now_ns()); + rt_log("input: gyro calibration started on %s", calib_pad_name); } plat_mutex_unlock(&lock); } @@ -1075,13 +1102,7 @@ int input_gyro_calibration_enabled(void) { return motion_calibration_enabled(); int input_motion_calib_state(char *pad, size_t cap) { plat_mutex_lock(&lock); - int st = 0; - if (motion_thread == 1) { - st = motion_calib_poll(motion_global(), kern_now_ns()); // also ends an attempt whose thread stalled - // Loaded from totk.ini at start, no attempt in this run; with the switch off the bias does not apply, so - // the row reads as before any calibration. - if (st == 0 && calib_pad_name[0] && input_gyro_calibration_enabled()) st = 2; - } + int st = motion_thread == 1 ? motion_calib_poll(motion_global(), kern_now_ns()) : MOTION_CALIB_NONE; if (pad && cap) snprintf(pad, cap, "%s", calib_pad_name); plat_mutex_unlock(&lock); return st; diff --git a/runtime/input.h b/runtime/input.h index f2162fc4..84809336 100644 --- a/runtime/input.h +++ b/runtime/input.h @@ -114,15 +114,19 @@ int input_rumble_triggers_enabled(void); void input_motion_start(void); // TOTK_MOTION is not "0" (a live setting: read again when cfg_version changes). Off: the sensor reads at rest. int input_motion_enabled(void); -void input_motion_calibrate(void); // start collecting resting gyro samples for a bias estimate -// What the Controls tab's Calibrate gyro row shows: 0 no bias for this controller, 1 collecting, 2 calibrated -// (by this run or loaded from totk.ini), 3 the last attempt timed out. pad receives the controller the bias belongs -// to (empty when none), copied so the caller does not read a buffer another thread may be rewriting. +// The Controls tab's Calibrate gyro row: start a bias measurement on the open gamepad (starts the sensor thread when +// the game has not yet). Callable from any thread. +void input_motion_calibrate(void); +// What the Calibrate gyro row shows (MOTION_CALIB_*): NONE no bias for this controller, COLLECTING, DONE a bias in +// effect (measured in this run or loaded from totk.ini), FAILED the last attempt got no samples and no bias is in +// effect, MOVED the last attempt was rejected because the controller moved. pad receives the name of the controller the +// bias belongs to (empty when none), copied so the caller does not read a buffer another thread may be rewriting. +enum { MOTION_CALIB_NONE, MOTION_CALIB_COLLECTING, MOTION_CALIB_DONE, MOTION_CALIB_FAILED, MOTION_CALIB_MOVED }; int input_motion_calib_state(char *pad, size_t cap); // The bias's own switch (gyro_calibration, live). Off: no collection, the fusion takes the raw gyro, and the row's // state reads as before any calibration while the saved keys stay in totk.ini. int input_gyro_calibration_enabled(void); -// The fusion's own read of the same switch (motion_fusion.c); input.c reaches it for the load and the row's state. +// The fusion's own read of the same switch (motion_fusion.c); input.c reaches it for the row's state. int motion_calibration_enabled(void); // SDL sensor values (accelerometer m/s^2, gyro rad/s, SDL gamepad axes) to the Switch controller frame motion_feed // takes (acceleration in G as gravity reads, rotation rad/s). The mapping is documented in input_win.c. @@ -141,7 +145,9 @@ MotionFusion *motion_global(void); // the gamepad's, fed by input_win.c void motion_feed(MotionFusion *m, const double acc_g[3], const double gyro_rad_s[3], uint64_t now_ns); int motion_read(MotionFusion *m, MotionState *out, int max, uint64_t now_ns); // newest first; 0 = none in 200 ms void motion_restart(MotionFusion *m); // angle and direction start over at the next sample -void motion_calibrate_start(MotionFusion *m, const char *pad_name); // start collecting; pad_name saved with the bias -int motion_calib_poll(MotionFusion *m, uint64_t now_ns); // the timeout, for when motion_feed is not called -void motion_bias_load(MotionFusion *m); // load the bias from totk.ini (TOTK_GYRO_BIAS_*) -void motion_bias_reset(MotionFusion *m); // zero the bias (new controller) +// Start a bias measurement at now_ns: samples while the controller rests, then the bias or a rejection (MOTION_CALIB_*). +// pad is the controller's identity, written to totk.ini with the bias. +void motion_calibrate_start(MotionFusion *m, const char *pad, uint64_t now_ns); +int motion_calib_poll(MotionFusion *m, uint64_t now_ns); // MOTION_CALIB_*; ends an attempt whose samples stopped +void motion_bias_load(MotionFusion *m); // take the bias from totk.ini (TOTK_GYRO_BIAS_*) +void motion_bias_reset(MotionFusion *m); // no bias and no attempt (another controller) diff --git a/runtime/motion_fusion.c b/runtime/motion_fusion.c index 71552219..0f6c59a0 100644 --- a/runtime/motion_fusion.c +++ b/runtime/motion_fusion.c @@ -7,9 +7,11 @@ // axis, direction = the controller's three axes in a reference frame whose z is up. The orientation is integrated from // the gyro and, while the controller is not being accelerated (|a| within 15 % of 1 G), pulled toward the measured // gravity (Mahony, gain 0.5/s: about 2 s), as the Switch's sensor fusion does; yaw has no reference and drifts with the -// gyro's bias. The newest MOTION_RING samples are kept; reads are newest first. A restart (StartSixAxisSensor) drops -// the samples before it: reads return nothing until the next sample, which starts a fresh frame and angle; sampling -// numbers go on. The feeding thread writes, guest threads read, both under the instance's SRW lock. +// gyro's bias, so a bias measured with the controller at rest (the Calibrate gyro row, motion_calibrate_start) is +// subtracted from every sample while gyro_calibration is on. The newest MOTION_RING samples are kept; reads are newest +// first. A restart (StartSixAxisSensor) drops the samples before it: reads return nothing until the next sample, which +// starts a fresh frame and angle; sampling numbers go on. The feeding thread writes, guest threads read, both under the +// instance's SRW lock. #include #include #include @@ -19,9 +21,16 @@ #include "input.h" #include "platform/platform.h" -enum { MOTION_RING = 64, CALIB_SAMPLES = 250 }; +// A calibration skips CALIB_SETTLE samples (0.5 s at the sensor thread's 4 ms), then averages CALIB_SAMPLES (1 s). +enum { MOTION_RING = 64, CALIB_SETTLE = 125, CALIB_SAMPLES = 250 }; #define STALE_NS 200000000ull // no sample for 200 ms: no motion -#define CALIB_TIMEOUT_NS 5000000000ull // 5 s: give up if sensor data stops arriving mid-calibration +#define CALIB_TIMEOUT_NS 5000000000ull // 5 s without a sample: the attempt gives up +// Largest per-axis standard deviation of the averaged samples (rad/s, about 2.3 deg/s) that still counts as holding +// still. The spread, not the mean, tells a moved controller from a biased one: a factory bias shifts every sample +// alike (0.14 rad/s on one Pro Controller's z axis) and passes, while the noise of a resting pad or a steady hand stays +// near 0.01-0.02 rad/s. A jolt that stays under the limit moves the mean by at most the limit times the square root of +// the share of the second it lasts. +#define CALIB_MAX_SPREAD 0.04 struct MotionFusion { PlatMutex lock; @@ -31,14 +40,15 @@ struct MotionFusion { double R[3][3]; // controller -> reference; row i = reference axis i in controller coordinates double angle[3]; int restart; - double gyro_bias[3]; // estimated resting bias (rad/s), subtracted from the gyro - int calibrating; // 1: collecting resting samples for a new bias - double calib_sum[3]; // accumulated gyro samples during calibration - int calib_count; // samples collected so far - uint64_t calib_start_ns; // when calibration started (timeout check) - char calib_pad[128]; // controller name that was calibrated (for loading the bias on the next run) - int calib_result; // 0 no attempt or one in progress, 1 the last attempt finished, -1 it timed out - int bias_valid; // 1 when gyro_bias holds a real estimate (loaded or measured), so a zero is a value + double gyro_bias[3]; // the controller's resting gyro (rad/s), subtracted from every sample while the switch is on + int bias_valid; // gyro_bias holds a measured or loaded estimate, so a zero is a value + int calibrating; // an attempt is collecting samples + int calib_count; // samples of the attempt so far, the skipped settle samples included + double calib_sum[3], calib_sq[3]; // sum and sum of squares of the averaged samples, per axis + uint64_t calib_start_ns; // when the attempt started (the timeout) + char calib_pad[128]; // the controller the attempt measures (input.c pad_identity), written with the bias + int calib_result; // MOTION_CALIB_NONE, _DONE, _FAILED or _MOVED: the outcome the row shows between attempts + int persist; // the gamepad's instance: a finished calibration is written to totk.ini }; MotionFusion *motion_new(void) { @@ -54,6 +64,7 @@ static MotionFusion *global; static void global_init(void *arg) { (void)arg; global = motion_new(); + global->persist = 1; } MotionFusion *motion_global(void) { @@ -119,7 +130,7 @@ static void integrate(double R[3][3], const double gyro[3], const double acc[3], // Write the calibrated bias to totk.ini. Called with m->lock released: cfg_set takes cfg.c's own lock and cfg_save // rewrites the file, and neither belongs inside the sensor thread's critical section. -static void save_bias(const double bias[3], const char *pad, int samples) { +static void save_bias(const double bias[3], const char *pad, double spread) { char buf[32]; snprintf(buf, sizeof buf, "%.6f", bias[0]); cfg_set("TOTK_GYRO_BIAS_X", buf); @@ -127,39 +138,29 @@ static void save_bias(const double bias[3], const char *pad, int samples) { cfg_set("TOTK_GYRO_BIAS_Y", buf); snprintf(buf, sizeof buf, "%.6f", bias[2]); cfg_set("TOTK_GYRO_BIAS_Z", buf); - if (pad[0]) cfg_set("TOTK_GYRO_BIAS_PAD", pad); + cfg_set("TOTK_GYRO_BIAS_PAD", pad); // Saved now rather than at the menu's save tick, which may not run if the menu is closed first. if (cfg_save()) - rt_log("motion: gyro bias calibrated from %d samples: %.5f %.5f %.5f rad/s", samples, bias[0], bias[1], bias[2]); + rt_log("motion: gyro bias calibrated from %d samples: %.5f %.5f %.5f rad/s (spread %.4f rad/s)", CALIB_SAMPLES, + bias[0], bias[1], bias[2], spread); else rt_log("motion: gyro bias %.5f %.5f %.5f rad/s could not be written to totk.ini; it applies to this run only", bias[0], bias[1], bias[2]); } -// The one timeout decision, for the sample path and the menu's poll alike, so the two cannot drift apart. Called -// with m->lock held; the caller logs, with the lock released. +// The one timeout decision, for the sample path and the menu's poll alike: CALIB_TIMEOUT_NS without a sample since the +// attempt started or since its last sample, so a slow sensor tick only lengthens the attempt. now may trail a sample +// the sensor thread stored meanwhile; that is no timeout. Called with m->lock held; the caller logs, with the lock +// released. A bias that is already in effect stays in effect and the row keeps reporting it; a controller with no +// bias shows the failure. static int calib_check_timeout(MotionFusion *m, uint64_t now) { - if (!m->calibrating || now - m->calib_start_ns <= CALIB_TIMEOUT_NS) return 0; + uint64_t since = m->last_ns > m->calib_start_ns ? m->last_ns : m->calib_start_ns; + if (!m->calibrating || now <= since || now - since <= CALIB_TIMEOUT_NS) return 0; m->calibrating = 0; - // A bias that is already in effect stays in effect, and stays reported as calibrated: the row describes the bias - // that is applied, and the failed attempt is in the log. A controller with no bias at all shows the failure. - m->calib_result = m->bias_valid ? 1 : -1; + m->calib_result = m->bias_valid ? MOTION_CALIB_DONE : MOTION_CALIB_FAILED; return 1; } -// Copy a name into cap bytes without splitting a UTF-8 sequence; the same as input.c's copy_name, which this file -// cannot include from. -static void copy_name(char *dst, size_t cap, const char *src) { - if (!src) src = ""; - size_t n = strlen(src); - if (n >= cap) { - n = cap - 1; - while (n > 0 && ((unsigned char)src[n] & 0xC0) == 0x80) n--; // back off to the start of the character - } - memcpy(dst, src, n); - dst[n] = 0; -} - // The bias's own switch (gyro_calibration, live, default 1): off, the fusion takes the raw gyro, an attempt in // progress is dropped, and the menu's calibrate entry does nothing. The saved keys stay untouched, so turning it // back on restores the last calibration. @@ -174,54 +175,68 @@ int motion_calibration_enabled(void) { return on; } +// One calibration sample, m->lock held. The first CALIB_SETTLE samples are skipped: the button press that started the +// attempt shakes the controller. The next CALIB_SAMPLES are averaged per axis into the bias, unless their spread shows +// the controller moved (CALIB_MAX_SPREAD). Returns 1 when the attempt ended with a new bias, -1 when it was rejected, +// 0 while it goes on; *spread receives the largest per-axis standard deviation of a finished attempt. +static int calib_sample(MotionFusion *m, const double g[3], double *spread) { + if (++m->calib_count <= CALIB_SETTLE) return 0; + for (int i = 0; i < 3; i++) { + m->calib_sum[i] += g[i]; + m->calib_sq[i] += g[i] * g[i]; + } + if (m->calib_count < CALIB_SETTLE + CALIB_SAMPLES) return 0; + m->calibrating = 0; + double mean[3], worst = 0; + for (int i = 0; i < 3; i++) { + mean[i] = m->calib_sum[i] / CALIB_SAMPLES; + double var = m->calib_sq[i] / CALIB_SAMPLES - mean[i] * mean[i]; + double sd = var > 0 ? sqrt(var) : 0; + if (sd > worst) worst = sd; + } + *spread = worst; + if (worst > CALIB_MAX_SPREAD) { + m->calib_result = MOTION_CALIB_MOVED; // the bias in effect, if any, stays + return -1; + } + memcpy(m->gyro_bias, mean, sizeof mean); + m->bias_valid = 1; + m->calib_result = MOTION_CALIB_DONE; + m->restart = 1; // the angle and direction start over from the new bias + return 1; +} + void motion_feed(MotionFusion *m, const double acc[3], const double gyro_raw[3], uint64_t now) { - double saved[3] = {0, 0, 0}; + const int calib_on = motion_calibration_enabled(); // read before the lock: a settings change takes cfg.c's lock + double saved[3] = {0, 0, 0}, spread = 0; char saved_pad[sizeof m->calib_pad] = ""; - int save = 0, samples = 0, first = 0, logged_timeout = 0; - uint64_t saved_start = 0; + int ended = 0, timed_out = 0, samples = 0; + uint64_t started = 0; plat_mutex_lock(&m->lock); double dt = m->last_ns && now > m->last_ns ? (double)(now - m->last_ns) / 1e9 : 0.004; if (dt > 0.05) dt = 0.004; // a gap (no controller, sensors were off): no integration across it - m->last_ns = now; - // Calibration: collect gyro samples for CALIB_SAMPLES frames. The user holds the controller still, so the - // average of the raw gyro (which reads the bias at rest) is the bias estimate. No motion threshold: the bias - // itself can exceed any absolute threshold, which would block collection forever. calib_check_timeout decides - // the deadline for both this path and the menu's poll, so the two cannot drift apart. With the calibration - // switch off, an attempt in progress is dropped and no new one starts; the bias stops applying below. - if (m->calibrating && !motion_calibration_enabled()) { + // Calibration: the player holds the controller still, so the raw gyro reads the bias. With the switch off an + // attempt in progress is dropped. + if (m->calibrating && !calib_on) { m->calibrating = 0; - m->calib_result = 0; + m->calib_result = m->bias_valid ? MOTION_CALIB_DONE : MOTION_CALIB_NONE; } if (m->calibrating) { if (calib_check_timeout(m, now)) { - logged_timeout = 1; + timed_out = 1; samples = m->calib_count; - saved_start = m->calib_start_ns; - } - if (m->calibrating) { - for (int i = 0; i < 3; i++) m->calib_sum[i] += gyro_raw[i]; - m->calib_count++; - first = m->calib_count == 1; - if (m->calib_count >= CALIB_SAMPLES) { - for (int i = 0; i < 3; i++) m->gyro_bias[i] = m->calib_sum[i] / m->calib_count; - m->calibrating = 0; - m->calib_result = 1; - m->bias_valid = 1; - m->restart = 1; - // Hand the result to save_bias below, which runs with the lock released. - memcpy(saved, m->gyro_bias, sizeof saved); - snprintf(saved_pad, sizeof saved_pad, "%s", m->calib_pad); - samples = m->calib_count; - save = 1; - } + started = m->calib_start_ns; + } else if ((ended = calib_sample(m, gyro_raw, &spread)) != 0) { + memcpy(saved, ended > 0 ? m->gyro_bias : m->calib_sum, sizeof saved); + if (ended < 0) + for (int i = 0; i < 3; i++) saved[i] /= CALIB_SAMPLES; + snprintf(saved_pad, sizeof saved_pad, "%s", m->calib_pad); } } - // Subtract the bias from the gyro for integration and storage. - // The bias applies only while the switch is on: off, the fusion sees the raw gyro again. - const int calib_on = motion_calibration_enabled(); - double gyro[3] = {gyro_raw[0] - (calib_on ? m->gyro_bias[0] : 0), - gyro_raw[1] - (calib_on ? m->gyro_bias[1] : 0), - gyro_raw[2] - (calib_on ? m->gyro_bias[2] : 0)}; + m->last_ns = now; // after the timeout check, which measures the gap before this sample + // The bias applies only while the switch is on; with no bias measured it is zero and the gyro passes unchanged. + double gyro[3]; + for (int i = 0; i < 3; i++) gyro[i] = gyro_raw[i] - (calib_on ? m->gyro_bias[i] : 0); if (m->restart) { align_to(m->R, acc); memset(m->angle, 0, sizeof m->angle); @@ -241,14 +256,17 @@ void motion_feed(MotionFusion *m, const double acc[3], const double gyro_raw[3], // The logs run with the lock released: the log back end can be slow, and the menu thread holds this lock in // motion_calib_poll. plat_mutex_unlock(&m->lock); - if (first) - rt_log("motion: calibration started, first raw gyro: %.5f %.5f %.5f rad/s", gyro_raw[0], gyro_raw[1], - gyro_raw[2]); - if (logged_timeout) { - double secs = (now - saved_start) / 1e9; // captured under the lock; another attempt may have begun since - rt_log("motion: gyro calibration timed out after %.1f s (%d samples): sensor data not arriving", secs, samples); + if (timed_out) + rt_log("motion: gyro calibration timed out after %.1f s (%d samples): sensor data not arriving", + (now - started) / 1e9, samples); + if (ended < 0) + rt_log("motion: gyro calibration rejected, the controller moved: spread %.4f rad/s over the limit %.2f " + "(mean %.5f %.5f %.5f rad/s)", spread, CALIB_MAX_SPREAD, saved[0], saved[1], saved[2]); + if (ended > 0) { + if (m->persist) save_bias(saved, saved_pad, spread); + else rt_log("motion: gyro bias calibrated: %.5f %.5f %.5f rad/s (spread %.4f rad/s)", saved[0], saved[1], + saved[2], spread); } - if (save) save_bias(saved, saved_pad, samples); } int motion_read(MotionFusion *m, MotionState *out, int max, uint64_t now) { @@ -273,17 +291,14 @@ void motion_restart(MotionFusion *m) { plat_mutex_unlock(&m->lock); } -void motion_calibrate_start(MotionFusion *m, const char *pad_name) { +void motion_calibrate_start(MotionFusion *m, const char *pad, uint64_t now) { plat_mutex_lock(&m->lock); m->calibrating = 1; - m->calib_result = 0; m->calib_count = 0; - m->calib_sum[0] = m->calib_sum[1] = m->calib_sum[2] = 0; - m->calib_start_ns = kern_now_ns(); - // The caller passes the same name it put into its own row label; copied again here so the fusion keeps the name - // the bias belongs to without relying on another buffer's lifetime. Same UTF-8-safe cut as input.c's copy_name: - // a name cut mid-sequence would reach totk.ini and the next run's comparisons. - copy_name(m->calib_pad, sizeof m->calib_pad, pad_name); + memset(m->calib_sum, 0, sizeof m->calib_sum); + memset(m->calib_sq, 0, sizeof m->calib_sq); + m->calib_start_ns = now; + snprintf(m->calib_pad, sizeof m->calib_pad, "%s", pad ? pad : ""); plat_mutex_unlock(&m->lock); } @@ -302,43 +317,38 @@ static double bias_value(const char *key) { } void motion_bias_load(MotionFusion *m) { - double bias[3] = {bias_value("TOTK_GYRO_BIAS_X"), bias_value("TOTK_GYRO_BIAS_Y"), - bias_value("TOTK_GYRO_BIAS_Z")}; + double bias[3] = {bias_value("TOTK_GYRO_BIAS_X"), bias_value("TOTK_GYRO_BIAS_Y"), bias_value("TOTK_GYRO_BIAS_Z")}; plat_mutex_lock(&m->lock); memcpy(m->gyro_bias, bias, sizeof bias); m->bias_valid = 1; - m->calib_result = 1; // 1 means "a bias is in effect", whether measured or loaded + m->calib_result = MOTION_CALIB_DONE; plat_mutex_unlock(&m->lock); } void motion_bias_reset(MotionFusion *m) { plat_mutex_lock(&m->lock); - m->gyro_bias[0] = m->gyro_bias[1] = m->gyro_bias[2] = 0; - // A calibration that was running belonged to the previous controller: its samples must not complete against this + memset(m->gyro_bias, 0, sizeof m->gyro_bias); + // An attempt that was collecting belongs to the previous controller: its samples must not complete against this // one, and its name must not label a bias measured from another controller. m->calibrating = 0; m->calib_count = 0; - m->calib_sum[0] = m->calib_sum[1] = m->calib_sum[2] = 0; m->calib_pad[0] = 0; - m->calib_result = 0; + m->calib_result = MOTION_CALIB_NONE; m->bias_valid = 0; plat_mutex_unlock(&m->lock); } -// The timeout, for the case motion_feed is not called at all (the sensor thread stalled): without this the row would -// read "Calibrating..." forever, which is the failure the timeout exists to end. Returns the row's state: 0 none, -// 1 collecting, 2 calibrated, 3 the last attempt timed out. Logs with the lock released: the menu thread holds this -// lock briefly on every redraw, and the sensor thread waits for it. +// Also ends an attempt whose samples stopped (the sensor thread no longer calls motion_feed): without this the row +// would read "Calibrating..." forever. Logs with the lock released: the menu thread calls this on every redraw. int motion_calib_poll(MotionFusion *m, uint64_t now) { plat_mutex_lock(&m->lock); int timed_out = calib_check_timeout(m, now); - int st = m->calibrating ? 1 : m->calib_result > 0 ? 2 : m->calib_result < 0 ? 3 : 0; + int st = m->calibrating ? MOTION_CALIB_COLLECTING : m->calib_result; uint64_t start = m->calib_start_ns; int count = m->calib_count; plat_mutex_unlock(&m->lock); - if (timed_out) { + if (timed_out) rt_log("motion: gyro calibration timed out after %.1f s (%d samples): the sensor thread stopped feeding it", (now - start) / 1e9, count); - } return st; } diff --git a/runtime/ov_probe.c b/runtime/ov_probe.c index 15026546..510c0a66 100644 --- a/runtime/ov_probe.c +++ b/runtime/ov_probe.c @@ -618,6 +618,43 @@ static void probe_feedback(uint32_t pid, uint32_t style) { near3(ms[0].dir, 1, 0, 0, 0.01), "fusion restart: %d states in the frame of the Start, %d after one sample (sampling %lld, angle %.4f rev)", same, next, (long long)ms[0].sampling, (double)ms[0].angle[2]); + // Gyro bias calibration on the private instance (not written to totk.ini): a large steady bias with resting noise + // is measured and then subtracted, so a resting controller reads no rotation and its yaw holds; samples that swing + // (the controller moved) are rejected and leave the measured bias in effect; an attempt without samples times out. + const double bias[3] = {-0.031, -0.011, -0.141}; + double g[3]; + motion_calibrate_start(m, "probe pad", t); + for (int i = 0; i < 375; i++) { + for (int j = 0; j < 3; j++) g[j] = bias[j] + (i & 1 ? 0.01 : -0.01); + motion_feed(m, down, g, t += 4000000); + } + int done = motion_calib_poll(m, t); + for (int i = 0; i < 250; i++) motion_feed(m, down, bias, t += 4000000); + motion_read(m, ms, 1, t + 1000000); + CHECK(done == MOTION_CALIB_DONE && near3(ms[0].gyro, 0, 0, 0, 1e-4) && fabs(ms[0].angle[2]) < 1e-3 && + near3(ms[0].dir, 1, 0, 0, 0.001), + "calibration: state %d, resting gyro %.5f %.5f %.5f rev/s, yaw %.5f rev after 1 s", done, + (double)ms[0].gyro[0], (double)ms[0].gyro[1], (double)ms[0].gyro[2], (double)ms[0].angle[2]); + motion_calibrate_start(m, "probe pad", t); + for (int i = 0; i < 375; i++) { + for (int j = 0; j < 3; j++) g[j] = bias[j] + (j == 0 && (i & 1) ? 0.2 : 0); + motion_feed(m, down, g, t += 4000000); + } + int moved = motion_calib_poll(m, t); + motion_feed(m, down, bias, t += 4000000); + motion_read(m, ms, 1, t + 1000000); + CHECK(moved == MOTION_CALIB_MOVED && near3(ms[0].gyro, 0, 0, 0, 1e-4), + "calibration of a moving controller: state %d, resting gyro %.5f %.5f %.5f rev/s", moved, + (double)ms[0].gyro[0], (double)ms[0].gyro[1], (double)ms[0].gyro[2]); + motion_bias_reset(m); + motion_calibrate_start(m, "probe pad", t); + int waiting = motion_calib_poll(m, t + 1000000000ull), failed = motion_calib_poll(m, t + 6000000000ull); + motion_feed(m, down, bias, t += 4000000); + motion_read(m, ms, 1, t + 1000000); + CHECK(waiting == MOTION_CALIB_COLLECTING && failed == MOTION_CALIB_FAILED && + fabs(ms[0].gyro[2] - bias[2] / (2 * M_PI)) < 1e-6, + "calibration without samples: state %d after 1 s, %d after 6 s; after a reset the raw gyro passes", waiting, + failed); printf("hid feedback: vibration handles %#x %#x, six-axis %#x, at rest when stopped; fusion quarter turn %.4f rev\n", vh[0], vh[1], sh[0], quarter); } diff --git a/runtime/ui/menu.cpp b/runtime/ui/menu.cpp index 4b3584bd..b7aee9dd 100644 --- a/runtime/ui/menu.cpp +++ b/runtime/ui/menu.cpp @@ -967,12 +967,14 @@ static void tab_controls(float scale) { char pad[128] = ""; int st = input_motion_calib_state(pad, sizeof pad); char status[192] = ""; - if (st == 1) + if (st == MOTION_CALIB_COLLECTING) snprintf(status, sizeof status, "%s", menu_text(UI_MENU_GYRO_CALIBRATE_WAIT)); - else if (st == 2) + else if (st == MOTION_CALIB_DONE) snprintf(status, sizeof status, menu_text(UI_MENU_GYRO_CALIBRATED), pad); - else if (st == 3) + else if (st == MOTION_CALIB_FAILED) snprintf(status, sizeof status, "%s", menu_text(UI_MENU_GYRO_CALIBRATE_FAILED)); + else if (st == MOTION_CALIB_MOVED) + snprintf(status, sizeof status, "%s", menu_text(UI_MENU_GYRO_CALIBRATE_MOVED)); if (opt_button("##gyro_calib", UI_MENU_GYRO_CALIBRATE, UI_MENU_GYRO_CALIBRATE_DESC, status)) input_motion_calibrate(); } diff --git a/runtime/ui_text.c b/runtime/ui_text.c index 1627d321..bd2de95f 100644 --- a/runtime/ui_text.c +++ b/runtime/ui_text.c @@ -262,10 +262,12 @@ static const char *const text_en[UI_TEXT_COUNT] = { [UI_MENU_GYRO_CALIBRATION_DESC] = "Subtracts the controller's resting drift, measured with the calibrate row " "below. The measured values stay saved when this is off.", [UI_MENU_GYRO_CALIBRATE] = "Calibrate gyro", - [UI_MENU_GYRO_CALIBRATE_DESC] = "Estimate and remove the gyroscope's resting drift. Hold the controller still.", + [UI_MENU_GYRO_CALIBRATE_DESC] = "Measures the gyroscope's resting drift and removes it. Put the controller down or " + "hold it still for two seconds.", [UI_MENU_GYRO_CALIBRATE_WAIT] = "Calibrating...", [UI_MENU_GYRO_CALIBRATED] = "%s, calibrated", [UI_MENU_GYRO_CALIBRATE_FAILED] = "Calibration failed", + [UI_MENU_GYRO_CALIBRATE_MOVED] = "Controller moved: hold it still and try again", [UI_MENU_RUMBLE_STRENGTH] = "Vibration strength", [UI_MENU_RUMBLE_STRENGTH_DESC] = "How strong the controller vibrates.", [UI_MENU_RUMBLE_CURVE] = "Vibration curve", @@ -809,10 +811,12 @@ static const char *const text_ru[UI_TEXT_COUNT] = { [UI_MENU_GYRO_CALIBRATION_DESC] = "Убирает покоящееся смещение контроллера, измеренное строкой калибровки ниже. " "Измеренные значения сохраняются и при выключенном состоянии.", [UI_MENU_GYRO_CALIBRATE] = "Калибровка гироскопа", - [UI_MENU_GYRO_CALIBRATE_DESC] = "Оценить и убрать покоящееся смещение гироскопа. Держите контроллер неподвижно.", + [UI_MENU_GYRO_CALIBRATE_DESC] = "Измеряет смещение гироскопа в покое и убирает его. Положите контроллер или " + "держите его неподвижно две секунды.", [UI_MENU_GYRO_CALIBRATE_WAIT] = "Калибровка...", [UI_MENU_GYRO_CALIBRATED] = "%s, откалиброван", [UI_MENU_GYRO_CALIBRATE_FAILED] = "Калибровка не удалась", + [UI_MENU_GYRO_CALIBRATE_MOVED] = "Контроллер сдвинулся: держите его неподвижно и повторите", [UI_MENU_RUMBLE_STRENGTH] = "Сила вибрации", [UI_MENU_RUMBLE_STRENGTH_DESC] = "Насколько сильно вибрирует контроллер.", [UI_MENU_RUMBLE_CURVE] = "Кривая вибрации", @@ -1273,10 +1277,12 @@ static const char *const text_pt_br[UI_TEXT_COUNT] = { [UI_MENU_GYRO_CALIBRATION_DESC] = "Remove o desvio de repouso do controle, medido pela linha de calibração " "abaixo. Os valores medidos ficam salvos mesmo com isto desligado.", [UI_MENU_GYRO_CALIBRATE] = "Calibrar giroscópio", - [UI_MENU_GYRO_CALIBRATE_DESC] = "Estimar e remover o desvio de repouso do giroscópio. Segure o controle parado.", + [UI_MENU_GYRO_CALIBRATE_DESC] = "Mede o desvio de repouso do giroscópio e o remove. Apoie o controle ou " + "segure-o parado por dois segundos.", [UI_MENU_GYRO_CALIBRATE_WAIT] = "Calibrando...", [UI_MENU_GYRO_CALIBRATED] = "%s, calibrado", [UI_MENU_GYRO_CALIBRATE_FAILED] = "Falha na calibração", + [UI_MENU_GYRO_CALIBRATE_MOVED] = "O controle se moveu: segure-o parado e tente de novo", [UI_MENU_RUMBLE_STRENGTH] = "Intensidade da vibração", [UI_MENU_RUMBLE_STRENGTH_DESC] = "Quão forte o controle vibra.", [UI_MENU_RUMBLE_CURVE] = "Curva da vibração", @@ -1902,10 +1908,11 @@ static const char *const text_zh_hans[UI_TEXT_COUNT] = { [UI_MENU_GYRO_CALIBRATION] = "陀螺仪偏置校准", [UI_MENU_GYRO_CALIBRATION_DESC] = "减去下面校准行测得的手柄静止偏移。关闭时测量值仍然保留。", [UI_MENU_GYRO_CALIBRATE] = "校准陀螺仪", - [UI_MENU_GYRO_CALIBRATE_DESC] = "估计并消除陀螺仪的静止偏移。请保持手柄静止。", + [UI_MENU_GYRO_CALIBRATE_DESC] = "测量并消除陀螺仪的静止偏移。请放下手柄或保持静止两秒。", [UI_MENU_GYRO_CALIBRATE_WAIT] = "校准中...", [UI_MENU_GYRO_CALIBRATED] = "%s,已校准", [UI_MENU_GYRO_CALIBRATE_FAILED] = "校准失败", + [UI_MENU_GYRO_CALIBRATE_MOVED] = "手柄移动了:请保持静止后重试", [UI_MENU_RUMBLE_STRENGTH] = "振动强度", [UI_MENU_RUMBLE_STRENGTH_DESC] = "手柄振动的强弱。", [UI_MENU_RUMBLE_CURVE] = "振动曲线", diff --git a/runtime/ui_text.h b/runtime/ui_text.h index 7fa11c4f..9bf7ab40 100644 --- a/runtime/ui_text.h +++ b/runtime/ui_text.h @@ -110,7 +110,7 @@ typedef enum { UI_MENU_MOTION, UI_MENU_MOTION_DESC, UI_MENU_GYRO_CALIBRATION, UI_MENU_GYRO_CALIBRATION_DESC, UI_MENU_GYRO_CALIBRATE, UI_MENU_GYRO_CALIBRATE_DESC, UI_MENU_GYRO_CALIBRATE_WAIT, - UI_MENU_GYRO_CALIBRATED, UI_MENU_GYRO_CALIBRATE_FAILED, + UI_MENU_GYRO_CALIBRATED, UI_MENU_GYRO_CALIBRATE_FAILED, UI_MENU_GYRO_CALIBRATE_MOVED, UI_MENU_RUMBLE_STRENGTH, UI_MENU_RUMBLE_STRENGTH_DESC, UI_MENU_RUMBLE_CURVE, UI_MENU_RUMBLE_CURVE_DESC, UI_MENU_RUMBLE_TRIGGERS, UI_MENU_RUMBLE_TRIGGERS_DESC, diff --git a/totk.ini.example b/totk.ini.example index 2260e4c3..8cf1fb13 100644 --- a/totk.ini.example +++ b/totk.ini.example @@ -351,18 +351,20 @@ [input] # Gamepad motion controls (0 = off) and vibration (0 = off). # motion = 1 -# Gyro bias calibration (1 = on): the Controls tab's "Calibrate gyro" row measures the controller's resting drift and -# subtracts it from the gyro readings, which removes the yaw drift. Off, the fusion takes the raw gyro, an attempt in -# progress is dropped and nothing is loaded at start; the four keys below stay in the file, so turning it back on -# restores the last calibration. +# Gyro bias calibration (1 = on): the Controls tab's "Calibrate gyro" row measures the controller's resting drift over +# two seconds and subtracts it from the gyro readings, which removes the yaw drift; an attempt where the controller +# moved is rejected and the row says so. Nothing changes until a controller is calibrated. Off, the fusion takes the raw +# gyro and an attempt in progress is dropped; the four keys below stay in the file, so turning it back on restores the +# last calibration. # gyro_calibration = 1 -# The measured bias and the controller it was measured from, written by the game when calibration finishes; they are -# not meant to be typed. The bias is loaded at the next start only when gyro_bias_pad names the controller that is -# connected, so calibrate each controller separately: another one starts with no bias until it is calibrated. +# The measured bias in rad/s and the controller it was measured from, written by the game when calibration finishes; +# they are not meant to be typed. The bias applies only while gyro_bias_pad names the connected controller (its name, +# plus a hash of its serial number when it reports one), so another controller starts with no bias. One controller's +# bias is kept: calibrating another one replaces it. # gyro_bias_x = 0 # gyro_bias_y = 0 # gyro_bias_z = 0 -# gyro_bias_pad = Nintendo Switch Pro Controller +# gyro_bias_pad = Nintendo Switch Pro Controller #1a2b3c4d # rumble = 1 # Rumble strength in percent (0 to 300; 0 = off) and the exponent on the amplitude (0.2 to 1.5; lower = weak pulses # stronger, 1 = linear). rumble_triggers = 1 also drives the impulse triggers of an Xbox pad.