From 45a0cd1f6c238482a8e6135a80bc437bdd5282a9 Mon Sep 17 00:00:00 2001 From: Valiantsin Dzerakh Date: Wed, 18 Nov 2026 09:06:30 +0300 Subject: [PATCH] Learn the gyro drift left after calibration while the controller lies still and subtract it, so the aim stops sliding on a table, from pull request #380 (#292, #1267) The calibration removes the bias measured once; a bias that moves as the controller warms up, or a controller never calibrated, still turns the aim slowly. motion_fusion.c drift_track learns it from 256 ms windows in which the gyro stays within 0.04 rad/s and the acceleration within 0.03 G peak to peak, after half a second of rest, with a time constant of 0.25 s until the estimate settles and 30 s after. The estimate sits on top of the calibrated bias and is cleared with a new controller, a new or loaded calibration and a change of gyro_calibration. New setting gyro_auto_bias (live, default 1). Taken from #380: the rest window by peak to peak, the two time constants, the per-axis limit. Left out: the 0.006 rad/s dead zone, which ate slow aiming; the one global state shared by every fusion instance; the reset at every StartSixAxisSensor; the 0.5 G acceleration gate, which let a controller in motion count as still. The first estimate is limited to 0.1 rad/s from zero instead of 0.15; a larger bias is the calibration's job. Also: a sample without a reading at a restart (#1274) no longer returns before the calibration's log and save. --probe-services checks the drift with the switch off and on, after a calibration whose bias then moved, a slow aim in the hand, and the levelling of #1274 with a bias on every axis. Co-authored-by: airquark <97342227+airquark@users.noreply.github.com> Co-Authored-By: Claude Opus 5.5 --- runtime/cfg.c | 6 +- runtime/input.h | 3 + runtime/motion_fusion.c | 150 ++++++++++++++++++++++++++++++++++++---- runtime/ov_probe.c | 92 ++++++++++++++++++++++++ totk.ini.example | 6 ++ 5 files changed, 240 insertions(+), 17 deletions(-) diff --git a/runtime/cfg.c b/runtime/cfg.c index 7784a7e7..121c7040 100644 --- a/runtime/cfg.c +++ b/runtime/cfg.c @@ -50,7 +50,7 @@ static const char *const known[] = { "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", - "GYRO_BIAS_PAD", "GYRO_BIAS_X", "GYRO_BIAS_Y", "GYRO_BIAS_Z", "GYRO_CALIBRATION", "HDR", "HDR_GAME_NITS", "HDR_HUE", + "GYRO_AUTO_BIAS", "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", "HUD_WIDTH", "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_NO_CODE", "MODS_ORDER", "MODS_REBUILD", "MODS_WITHOUT_CODE", "MOTION", "MOUSE_AIM_SCALE", "MOUSE_MODE", "MOUSE_DIRECT_PATCH", "MOUSE_SENS", "MOVIE_ASPECT", "MOVIE_HOST", "MOVIE_PACE", @@ -94,7 +94,7 @@ 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"}, {"GYRO_CALIBRATION", "input"}, {"RUMBLE_STRENGTH", "input"}, {"RUMBLE_CURVE", "input"}, {"RUMBLE_TRIGGERS", "input"}, + {"MOTION", "input"}, {"GYRO_CALIBRATION", "input"}, {"GYRO_AUTO_BIAS", "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"}, @@ -170,7 +170,7 @@ 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}, {"GYRO_CALIBRATION", "1", "0|1", LIVE}, + {"MOTION", "1", "0|1", LIVE}, {"GYRO_CALIBRATION", "1", "0|1", LIVE}, {"GYRO_AUTO_BIAS", "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. diff --git a/runtime/input.h b/runtime/input.h index f59edbc1..e2b61a6a 100644 --- a/runtime/input.h +++ b/runtime/input.h @@ -149,6 +149,9 @@ int input_motion_calib_state(char *pad, size_t cap); 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); +// The drift tracker's switch (gyro_auto_bias, live): the drift left after calibration, learned while the controller +// lies still, is subtracted too. +int motion_auto_bias_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]); diff --git a/runtime/motion_fusion.c b/runtime/motion_fusion.c index b981577f..e54e14f5 100644 --- a/runtime/motion_fusion.c +++ b/runtime/motion_fusion.c @@ -8,13 +8,14 @@ // 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; held still (|a| within 5 % of 1 G, // turning slower than STILL_RATE) the pull is STILL_GAIN, so a down the gyro got wrong while the controller moved is -// right again before the next aim. The game splits the aim into pitch and yaw by the reported down: a wrong one turns -// a tilt up or down into a turn to the side as well. Yaw has no reference and drifts with the gyro's bias, so a bias +// right again before the next aim. The game splits the aim into pitch and yaw by the reported down: a wrong one turns a +// tilt up or down into a turn to the side as well. Yaw has no reference and drifts with the 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 first sample with a reading (|a| -// over 0.5 G: a sensor just switched on reads zero until its first report), which starts a fresh frame, levelled by -// that reading, and a fresh angle; sampling numbers go on. The feeding thread writes, guest threads read, both under +// while gyro_calibration is on, and the drift left after it is learned while the controller lies still and subtracted +// too while gyro_auto_bias is on (drift_track). The newest MOTION_RING samples are kept; reads are newest first. A +// restart (StartSixAxisSensor) drops the samples before it: reads return nothing until the first sample with a reading +// (|a| over 0.5 G: a sensor just switched on reads zero until its first report), which starts a fresh frame, levelled +// by that reading, and a fresh angle; sampling numbers go on. The feeding thread writes, guest threads read, both under // the instance's SRW lock. #include #include @@ -37,6 +38,27 @@ enum { MOTION_RING = 64, CALIB_SETTLE = 125, CALIB_SAMPLES = 250 }; #define CALIB_MAX_SPREAD 0.04 #define STILL_GAIN 4.0 // 1/s: a wrong down is 2 % of itself after a second held still #define STILL_RATE 0.5 // rad/s, about 30 deg/s: a slow aim still counts as held still +// The drift tracker (drift_track), after PR #380's estimator: a window of REST_WIN samples (256 ms) is still when every +// axis of the gyro stays within REST_GYRO_PTP peak to peak and of the acceleration within REST_ACC_PTP, |a| is within +// REST_GRAVITY of 1 G, and the window's mean rate is within DRIFT_FIRST_MAX (before the estimate settled) or +// DRIFT_TRACK_MAX (after) of the estimate in use. A Pro Controller on a table reads 0.004-0.006 rad/s of noise and +// passes; held in a hand, even steadily, it reads 0.013-0.025 rad/s and mostly does not, so the tracker learns from a +// controller put down rather than from an aim held still. After REST_HOLD_S still the estimate moves toward the +// window's mean with a time constant of DRIFT_TAU_FIRST until it has settled, then DRIFT_TAU: a bias changes only +// slowly as the controller warms up, and a hand that slips past the gate shifts it little. No dead zone: a slow aim +// keeps every bit of its rate. +enum { REST_WIN = 64 }; +#define REST_GYRO_PTP 0.04 // rad/s, per axis over the window +#define REST_ACC_PTP 0.03 // G, per axis over the window +#define REST_GRAVITY 0.25 // G: controllers' accelerometers read 1 G only roughly +#define REST_HOLD_S 0.5 +#define DRIFT_FIRST_MAX 0.1 // rad/s, 5.7 deg/s; a larger factory bias (one Pro: 0.14 on z) needs the calibration +#define DRIFT_TRACK_MAX 0.05 // rad/s +#define DRIFT_TAU_FIRST 0.25 // s +#define DRIFT_TAU 30.0 // s +#define DRIFT_SETTLE_S 1.5 // learning this long with the estimate within DRIFT_SETTLED of the window: settled +#define DRIFT_SETTLED 0.01 // rad/s +#define DRIFT_LIMIT 0.3 // rad/s per axis struct MotionFusion { PlatMutex lock; @@ -55,6 +77,16 @@ struct MotionFusion { 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 + // The drift tracker: the estimate (rad/s, on top of gyro_bias when gyro_calibration is on), whether it settled, + // and the rest window it is learned from. The estimate belongs to the controller and to the bias it was learned + // against: kept through restarts, cleared with a new controller, a new calibration or a change of + // gyro_calibration (drift_clear). + double drift[3]; + int drift_settled; + int drift_base; // gyro_calibration the estimate was learned with, -1 none + double rest_s, learn_s; + double win_g[REST_WIN][3], win_a[REST_WIN][3]; + int win_n, win_i; }; MotionFusion *motion_new(void) { @@ -62,6 +94,7 @@ MotionFusion *motion_new(void) { if (!m) rt_fatal("motion: out of memory"); plat_mutex_init(&m->lock); m->restart = 1; + m->drift_base = -1; return m; } @@ -134,6 +167,79 @@ static void integrate(double R[3][3], const double gyro[3], const double acc[3], orthonormalize(R); } +// The tracker's own switch (gyro_auto_bias, live, default 1). Off, nothing is learned or subtracted. +int motion_auto_bias_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_AUTO_BIAS"); + } + return on; +} + +// m->lock held: forget the estimate and the rest window. +static void drift_clear(MotionFusion *m) { + memset(m->drift, 0, sizeof m->drift); + m->drift_settled = 0; + m->drift_base = -1; + m->rest_s = m->learn_s = 0; + m->win_n = m->win_i = 0; +} + +// Largest per-axis peak-to-peak of a full window. +static double win_ptp(const double w[REST_WIN][3]) { + double worst = 0; + for (int i = 0; i < 3; i++) { + double lo = w[0][i], hi = w[0][i]; + for (int j = 1; j < REST_WIN; j++) { + if (w[j][i] < lo) lo = w[j][i]; + if (w[j][i] > hi) hi = w[j][i]; + } + if (hi - lo > worst) worst = hi - lo; + } + return worst; +} + +// m->lock held, one sample with a reading: gyro is the rate with the calibrated bias taken off when base is 1. Moves +// m->drift toward the rate of a controller lying still. Returns 1 when the estimate settled with this sample. +static int drift_track(MotionFusion *m, const double acc[3], const double gyro[3], int base, double dt) { + if (m->drift_base != base) { // nothing learned yet, or learned against the other baseline: start over + drift_clear(m); + m->drift_base = base; + } + memcpy(m->win_g[m->win_i], gyro, sizeof m->win_g[0]); + memcpy(m->win_a[m->win_i], acc, sizeof m->win_a[0]); + m->win_i = (m->win_i + 1) % REST_WIN; + if (m->win_n < REST_WIN) m->win_n++; + if (m->win_n < REST_WIN) return 0; + double mean[3] = {0, 0, 0}, a[3] = {0, 0, 0}, dev = 0; + for (int j = 0; j < REST_WIN; j++) + for (int i = 0; i < 3; i++) mean[i] += m->win_g[j][i] / REST_WIN, a[i] += m->win_a[j][i] / REST_WIN; + for (int i = 0; i < 3; i++) + if (fabs(mean[i] - m->drift[i]) > dev) dev = fabs(mean[i] - m->drift[i]); + int still = fabs(norm3(a) - 1) < REST_GRAVITY && win_ptp(m->win_g) < REST_GYRO_PTP && + win_ptp(m->win_a) < REST_ACC_PTP && dev < (m->drift_settled ? DRIFT_TRACK_MAX : DRIFT_FIRST_MAX); + if (!still) { + m->rest_s = 0; + if (!m->drift_settled) m->learn_s = 0; + return 0; + } + m->rest_s += dt; + if (m->rest_s < REST_HOLD_S) return 0; + double k = dt / (m->drift_settled ? DRIFT_TAU : DRIFT_TAU_FIRST); + for (int i = 0; i < 3; i++) { + m->drift[i] += k * (mean[i] - m->drift[i]); + if (m->drift[i] > DRIFT_LIMIT) m->drift[i] = DRIFT_LIMIT; + if (m->drift[i] < -DRIFT_LIMIT) m->drift[i] = -DRIFT_LIMIT; + } + m->learn_s += dt; + if (m->drift_settled || m->learn_s < DRIFT_SETTLE_S || dev >= DRIFT_SETTLED) return 0; + m->drift_settled = 1; + return 1; +} + // 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) { @@ -209,18 +315,24 @@ static int calib_sample(MotionFusion *m, const double g[3], double *spread) { m->bias_valid = 1; m->calib_result = MOTION_CALIB_DONE; m->restart = 1; // the angle and direction start over from the new bias + drift_clear(m); // what it learned was the old bias's remainder 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; + // Read before the lock: a settings change takes cfg.c's lock. + const int calib_on = motion_calibration_enabled(), auto_on = motion_auto_bias_enabled(); + double saved[3] = {0, 0, 0}, spread = 0, drift[3] = {0, 0, 0}; char saved_pad[sizeof m->calib_pad] = ""; - int ended = 0, timed_out = 0, samples = 0; + int ended = 0, timed_out = 0, samples = 0, settled = 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 + if (dt > 0.05) { // a gap (no controller, sensors were off): no integration and no rest across it + dt = 0.004; + m->rest_s = 0; + m->win_n = m->win_i = 0; + } // 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) { @@ -243,10 +355,14 @@ void motion_feed(MotionFusion *m, const double acc[3], const double gyro_raw[3], // 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 && norm3(acc) < 0.5) { - plat_mutex_unlock(&m->lock); - return; - } + // A sample without a reading (a sensor just switched on reads zero) neither starts a frame nor counts as rest. + const int reading = norm3(acc) >= 0.5; + if (!auto_on) { + if (m->drift_base >= 0) drift_clear(m); + } else if (reading && (settled = drift_track(m, acc, gyro, calib_on, dt))) + memcpy(drift, m->drift, sizeof drift); + for (int i = 0; i < 3; i++) gyro[i] -= m->drift[i]; // zero while the switch is off + if (m->restart && !reading) goto unlock; // no state yet; the logs below still run if (m->restart) { align_to(m->R, acc); memset(m->angle, 0, sizeof m->angle); @@ -263,9 +379,13 @@ void motion_feed(MotionFusion *m, const double acc[3], const double gyro_raw[3], 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++; +unlock: // 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 (settled) + rt_log("motion: gyro drift at rest learned: %.5f %.5f %.5f rad/s, taken off %s", drift[0], drift[1], drift[2], + calib_on ? "on top of the calibrated bias" : "the raw gyro"); if (timed_out) rt_log("motion: gyro calibration timed out after %.1f s (%d samples): sensor data not arriving", (now - started) / 1e9, samples); @@ -332,6 +452,7 @@ void motion_bias_load(MotionFusion *m) { memcpy(m->gyro_bias, bias, sizeof bias); m->bias_valid = 1; m->calib_result = MOTION_CALIB_DONE; + drift_clear(m); plat_mutex_unlock(&m->lock); } @@ -345,6 +466,7 @@ void motion_bias_reset(MotionFusion *m) { m->calib_pad[0] = 0; m->calib_result = MOTION_CALIB_NONE; m->bias_valid = 0; + drift_clear(m); // the drift belonged to that controller too plat_mutex_unlock(&m->lock); } diff --git a/runtime/ov_probe.c b/runtime/ov_probe.c index 59ff861c..b5315f08 100644 --- a/runtime/ov_probe.c +++ b/runtime/ov_probe.c @@ -244,6 +244,12 @@ static char *lang(void) { } static void set_env(const char *name, const char *value) { plat_setenv(name, value); } // NULL removes it +// A live setting read through a cfg_version cache: the environment wins, and the cfg_set (empty = the default) moves +// the version so the cache reads it again. +static void set_live(const char *name, const char *value) { + set_env(name, value); + cfg_set(name, value ? value : ""); +} static void probe_oe(void) { char *keep_lang = cfg_get("TOTK_LANG") ? strdup(cfg_get("TOTK_LANG")) : NULL; @@ -523,6 +529,26 @@ static void probe_time(void) { static int near3(const float *v, double x, double y, double z, double tol) { return fabs(v[0] - x) <= tol && fabs(v[1] - y) <= tol && fabs(v[2] - z) <= tol; } +// A gyro sample: rate plus noise. Uniform noise of half-width amp from a fixed LCG, so every run reads the same. +static void noisy(const double rate[3], double amp, uint32_t *seed, double out[3]) { + for (int i = 0; i < 3; i++) { + *seed = *seed * 1664525u + 1013904223u; + out[i] = rate[i] + amp * ((double)(*seed >> 8) / (1 << 24) * 2 - 1); + } +} + +// Feed secs of samples at the sensor thread's 4 ms; returns the yaw (angle z, revolutions) the last state reads. +static double feed_for(MotionFusion *m, const double acc[3], const double rate[3], double amp, uint32_t *seed, + double secs, uint64_t *t) { + MotionState s; + double g[3]; + for (int i = 0; i < (int)(secs * 250 + 0.5); i++) { + noisy(rate, amp, seed, g); + motion_feed(m, acc, g, *t += 4000000); + } + return motion_read(m, &s, 1, *t + 1000000) == 1 ? s.angle[2] : NAN; +} + static int at_rest(const MotionState *s) { return !s->attributes && !s->sampling && near3(s->acc, 0, 0, -1, 0) && near3(s->gyro, 0, 0, 0, 0) && near3(s->dir, 1, 0, 0, 0) && near3(s->dir + 3, 0, 1, 0, 0) && near3(s->dir + 6, 0, 0, 1, 0); @@ -670,6 +696,72 @@ static void probe_feedback(uint32_t pid, uint32_t style) { CHECK(before == 0 && upright_y && ms[0].dir[8] > 0.99, "fusion start and levelling: %d states before a reading, %d after (y up %d), z up %.3f after 1 s still", before, first, upright_y, (double)ms[0].dir[8]); + + // Drift at rest (gyro_auto_bias). Table noise is +-0.006 rad/s, a hand's +-0.03; drift is the yaw rate a + // controller lying still reads, in rad/s, over the 10 s after the tracker had 10 s to learn. + char *keep_auto = cfg_get("TOTK_GYRO_AUTO_BIAS") ? strdup(cfg_get("TOTK_GYRO_AUTO_BIAS")) : NULL; + char *keep_cal = cfg_get("TOTK_GYRO_CALIBRATION") ? strdup(cfg_get("TOTK_GYRO_CALIBRATION")) : NULL; + set_live("TOTK_GYRO_CALIBRATION", "1"); + const double drift_b[3] = {0.02, -0.03, 0.05}, rev = 2 * M_PI; + uint32_t seed = 1; + double drift_rate[2]; + for (int on = 0; on < 2; on++) { + set_live("TOTK_GYRO_AUTO_BIAS", on ? "1" : "0"); + MotionFusion *d = motion_new(); + uint64_t td = t; + feed_for(d, down, drift_b, 0.006, &seed, 10, &td); + double y0 = feed_for(d, down, drift_b, 0.006, &seed, 0.004, &td); + drift_rate[on] = (feed_for(d, down, drift_b, 0.006, &seed, 10, &td) - y0) * rev / 10; + } + CHECK(fabs(drift_rate[0] - drift_b[2]) < 0.002 && fabs(drift_rate[1]) < 0.001, + "drift at rest: yaw %.5f rad/s with gyro_auto_bias off, %.5f on (bias %.3f)", drift_rate[0], drift_rate[1], + drift_b[2]); + // The #1267 case: calibrated, then the bias moved by 0.01 rad/s (the controller warmed up). The tracker takes the + // remainder on top of the calibrated bias, and a new calibration starts it over. + MotionFusion *c = motion_new(); + uint64_t tc = t; + motion_calibrate_start(c, "probe pad", tc); + feed_for(c, down, drift_b, 0.006, &seed, 1.6, &tc); + int cal = motion_calib_poll(c, tc); + const double warm[3] = {drift_b[0], drift_b[1], drift_b[2] - 0.01}; + double c0 = feed_for(c, down, warm, 0.006, &seed, 0.004, &tc); + double c_early = (feed_for(c, down, warm, 0.006, &seed, 0.5, &tc) - c0) * rev / 0.5; + feed_for(c, down, warm, 0.006, &seed, 10, &tc); + c0 = feed_for(c, down, warm, 0.006, &seed, 0.004, &tc); + double c_late = (feed_for(c, down, warm, 0.006, &seed, 10, &tc) - c0) * rev / 10; + CHECK(cal == MOTION_CALIB_DONE && fabs(c_early + 0.01) < 0.002 && fabs(c_late) < 0.001, + "drift after calibration: state %d, yaw %.5f rad/s right after, %.5f once learned", cal, c_early, c_late); + // In the hand: a slow aim, 0.05 rad/s (3 deg/s) of yaw with a hand's noise, for 10 s. Nothing is learned from it, + // so the full turn reaches the game. + MotionFusion *h = motion_new(); + uint64_t th = t; + const double pan[3] = {0, 0, 0.05}; + double h0 = feed_for(h, down, pan, 0.03, &seed, 0.004, &th); + double turned = (feed_for(h, down, pan, 0.03, &seed, 10, &th) - h0) * rev; + CHECK(fabs(turned - 0.5) < 0.01, "slow aim in the hand: %.4f rad of the 0.5 turned", turned); + // Levelling and drift together: the frame starts standing on the bottom edge, then the controller lies flat with + // a bias on every axis. The bias tilts the fused down by bias / gain (0.05 / 4 rad, 0.7 degrees) until it is + // learned; then the down is level, and yaw holds. + MotionFusion *l = motion_new(); + uint64_t tl = t; + feed_for(l, standing, drift_b, 0.006, &seed, 0.004, &tl); + feed_for(l, down, drift_b, 0.006, &seed, 1, &tl); + motion_read(l, ms, 1, tl + 1000000); + double up_1s = ms[0].dir[8]; + feed_for(l, down, drift_b, 0.006, &seed, 10, &tl); + double l0 = feed_for(l, down, drift_b, 0.006, &seed, 0.004, &tl); + double l_rate = (feed_for(l, down, drift_b, 0.006, &seed, 10, &tl) - l0) * rev / 10; + motion_read(l, ms, 1, tl + 1000000); + CHECK(up_1s > 0.99 && ms[0].dir[8] > 0.99999 && fabs(l_rate) < 0.001, + "levelling with drift: z up %.5f after 1 s, %.6f after 21 s, yaw %.5f rad/s", up_1s, (double)ms[0].dir[8], + l_rate); + printf("hid drift at rest: yaw %.5f rad/s off, %.5f on; after calibration %.5f, then %.5f; slow aim %.4f of 0.5 " + "rad; levelled z %.5f, yaw %.5f rad/s\n", + drift_rate[0], drift_rate[1], c_early, c_late, turned, up_1s, l_rate); + set_live("TOTK_GYRO_AUTO_BIAS", keep_auto); + set_live("TOTK_GYRO_CALIBRATION", keep_cal); + free(keep_auto); + free(keep_cal); 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/totk.ini.example b/totk.ini.example index ee6c07ee..bc062f21 100644 --- a/totk.ini.example +++ b/totk.ini.example @@ -380,6 +380,12 @@ # 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 +# Gyro drift at rest (1 = on): whenever the controller lies still (on a table, not held) for half a second, the game +# learns the drift left in its gyro, after the calibration above if there is one, and subtracts it, as the Switch's +# hid service does for its own controllers. It catches a bias that changes as the controller warms up and works +# without a calibration. The log says "gyro drift at rest learned" once it has settled. A controller held in the hand +# is not learned from, and there is no dead zone, so slow aiming is untouched. Off, nothing is learned or subtracted. +# gyro_auto_bias = 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