Merge pull request #1197: a Calibrate gyro button on the Controls tab removes the aim drift of a controller with a gyro bias; each controller keeps its own, and a calibration where the pad moved is rejected (palxex)

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
Valiantsin DzerakhandClaude Opus 5.5 committed 2026-11-15 09:32:40 +03:00
commit 33bfd29ebb
9 files changed
+440 -17

No files matched your search

+10 -3
View File
@@ -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},
+94 -8
View File
@@ -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;
}
+20
View File
@@ -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)
+206 -6
View File
@@ -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 <math.h>
#include <stdlib.h>
#include <string.h>
#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;
}
+37
View File
@@ -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);
}
+18
View File
@@ -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);
+38
View File
@@ -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] = "振动曲线",
+3
View File
@@ -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,
+14
View File
@@ -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.