diff --git a/runtime/cfg.c b/runtime/cfg.c index dda29abd..95450916 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", "LOD_STEADY", "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", @@ -92,7 +93,8 @@ static const struct { const char *key, *section; } sections[] = { {"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"}, {"AUDIO_DEVICE", "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"}, @@ -165,7 +167,12 @@ static const CfgMeta metas[] = { // 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}, {"AUDIO_DEVICE", "", NULL, LIVE}, // audio_mixer.c output_worker: a device name, empty for the system default - {"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 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}, {"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 2e2f48e5..a7c4045e 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] = { @@ -424,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"); @@ -956,11 +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; 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) && @@ -1008,16 +1057,53 @@ static void motion_main(void *arg) { } } +// 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; + } +} + void input_motion_start(void) { plat_mutex_lock(&lock); - if (!state) start(); - if (!motion_thread) { - 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); + // 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); } + +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 = 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 86f26fa7..84809336 100644 --- a/runtime/input.h +++ b/runtime/input.h @@ -114,6 +114,20 @@ 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); +// 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 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 +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 +// 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 7bbf3be6..0f6c59a0 100644 --- a/runtime/motion_fusion.c +++ b/runtime/motion_fusion.c @@ -7,19 +7,30 @@ // 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 +#include "cfg.h" #include "host.h" #include "input.h" #include "platform/platform.h" -enum { MOTION_RING = 64 }; +// 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 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; @@ -29,6 +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]; // 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) { @@ -44,6 +64,7 @@ static MotionFusion *global; static void global_init(void *arg) { (void)arg; global = motion_new(); + global->persist = 1; } MotionFusion *motion_global(void) { @@ -107,11 +128,115 @@ 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, double spread) { + 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); + 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 (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: 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) { + 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; + m->calib_result = m->bias_valid ? MOTION_CALIB_DONE : MOTION_CALIB_FAILED; + return 1; +} + +// 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; +} + +// 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) { + 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 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: 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 = m->bias_valid ? MOTION_CALIB_DONE : MOTION_CALIB_NONE; + } + if (m->calibrating) { + if (calib_check_timeout(m, now)) { + timed_out = 1; + samples = m->calib_count; + 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); + } + } + 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); @@ -128,7 +253,20 @@ 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 (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); + } } int motion_read(MotionFusion *m, MotionState *out, int max, uint64_t now) { @@ -152,3 +290,65 @@ 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, uint64_t now) { + plat_mutex_lock(&m->lock); + m->calibrating = 1; + m->calib_count = 0; + 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); +} + +// 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 = MOTION_CALIB_DONE; + plat_mutex_unlock(&m->lock); +} + +void motion_bias_reset(MotionFusion *m) { + plat_mutex_lock(&m->lock); + 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_pad[0] = 0; + m->calib_result = MOTION_CALIB_NONE; + m->bias_valid = 0; + plat_mutex_unlock(&m->lock); +} + +// 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 ? 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) + 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 43b122c2..b7aee9dd 100644 --- a/runtime/ui/menu.cpp +++ b/runtime/ui/menu.cpp @@ -960,6 +960,24 @@ 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 == MOTION_CALIB_COLLECTING) + snprintf(status, sizeof status, "%s", menu_text(UI_MENU_GYRO_CALIBRATE_WAIT)); + else if (st == MOTION_CALIB_DONE) + snprintf(status, sizeof status, menu_text(UI_MENU_GYRO_CALIBRATED), pad); + 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(); + } 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 50802d6d..bd2de95f 100644 --- a/runtime/ui_text.c +++ b/runtime/ui_text.c @@ -258,6 +258,16 @@ static const char *const text_en[UI_TEXT_COUNT] = { [UI_MENU_AUDIO_DEVICE_DEFAULT] = "System default", [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] = "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", @@ -797,6 +807,16 @@ static const char *const text_ru[UI_TEXT_COUNT] = { [UI_MENU_AUDIO_DEVICE_DEFAULT] = "Системное по умолчанию", [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_GYRO_CALIBRATE_MOVED] = "Контроллер сдвинулся: держите его неподвижно и повторите", [UI_MENU_RUMBLE_STRENGTH] = "Сила вибрации", [UI_MENU_RUMBLE_STRENGTH_DESC] = "Насколько сильно вибрирует контроллер.", [UI_MENU_RUMBLE_CURVE] = "Кривая вибрации", @@ -1253,6 +1273,16 @@ static const char *const text_pt_br[UI_TEXT_COUNT] = { [UI_MENU_AUDIO_DEVICE_DEFAULT] = "Padrão do sistema", [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] = "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", @@ -1875,6 +1905,14 @@ static const char *const text_zh_hans[UI_TEXT_COUNT] = { [UI_MENU_AUDIO_DEVICE_DEFAULT] = "系统默认", [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_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 0f85f056..9bf7ab40 100644 --- a/runtime/ui_text.h +++ b/runtime/ui_text.h @@ -108,6 +108,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_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 8c27f7a4..8cf1fb13 100644 --- a/totk.ini.example +++ b/totk.ini.example @@ -351,6 +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 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 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 #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.