mirror of
https://github.com/valentderah/totk-pc-recomp.git
synced 2026-11-19 20:25:57 +00:00
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 <noreply@anthropic.com>
This commit is contained in:
1 parent
706144d673
commit
45a0cd1f6c
5 files changed
+240
-17
No files matched your search
+3
-3
@@ -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.
|
||||
|
||||
@@ -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]);
|
||||
|
||||
+136
-14
@@ -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 <math.h>
|
||||
#include <stdlib.h>
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in new issue
Block a user