mirror of
https://github.com/valentderah/totk-pc-recomp.git
synced 2026-11-18 20:15:56 +00:00
The bias reset in sensors_ready only ran when sensor_pad was nonzero, but scan_pads zeroes it on every disconnect, so a pad connected after another one kept the old pad's bias, and an attempt still collecting finished on the new pad under the old pad's name. The bias was also loaded only once, at the first StartSixAxisSensor. - input.c pad_bias_select runs for every pad the sensor code sees: it drops the old bias and attempt, then loads the saved bias when gyro_bias_pad names this pad. scan_pads drops them on disconnect. The bias loads with gyro_calibration off too, so turning the switch on mid-run applies it. - The pad identity is the name plus an FNV-1a hash of SDL's serial when the pad reports one, so two pads of one model no longer share a bias; the hash keeps the Bluetooth address out of totk.ini and the log. - An attempt skips 0.5 s of samples (the button press shakes the pad), then averages 1 s. It is rejected when the largest per-axis standard deviation exceeds 0.04 rad/s; a steady bias of any size passes. The row then says the controller moved (new UI_MENU_GYRO_CALIBRATE_MOVED in all four catalogs) and the bias in effect stays. - The timeout counts 5 s without a sample, not 5 s from the start. - Calibrate gyro starts the sensor thread when the game has not yet (title screen) instead of doing nothing. - Row states are an enum (MOTION_CALIB_*); motion_calibrate_start takes the clock, and only the gamepad's fusion instance writes totk.ini, so --probe-services now checks a measured bias, a rejected attempt and the timeout. - The duplicate copy_name in motion_fusion.c is gone. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
355 lines
17 KiB
C
355 lines
17 KiB
C
// Adapted from MacNativeGames games/totk/runtime/motion_fusion.c @ b75be8cf (MIT, Copyright (c) 2026 MacNativeGames
|
|
// contributors).
|
|
// Six-axis sensor states (nn::hid::SixAxisSensorState) from gyro and accelerometer samples. Input per sample, in the
|
|
// Switch controller frame (x to the right, y toward the top edge, z out of the face): acceleration in G including
|
|
// gravity, as gravity reads (lying face up z = -1 G), and rotation rate in rad/s. Output per sample, as the Switch
|
|
// reports it: acceleration in G, angular velocity in revolutions per second, angle = the integrated revolutions per
|
|
// 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, 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"
|
|
|
|
// 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;
|
|
MotionState ring[MOTION_RING];
|
|
uint64_t produced, last_ns;
|
|
uint64_t first; // produced at the last restart: older samples are not read
|
|
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) {
|
|
MotionFusion *m = calloc(1, sizeof *m);
|
|
if (!m) rt_fatal("motion: out of memory");
|
|
plat_mutex_init(&m->lock);
|
|
m->restart = 1;
|
|
return m;
|
|
}
|
|
|
|
static MotionFusion *global;
|
|
|
|
static void global_init(void *arg) {
|
|
(void)arg;
|
|
global = motion_new();
|
|
global->persist = 1;
|
|
}
|
|
|
|
MotionFusion *motion_global(void) {
|
|
static PlatOnce once = PLAT_ONCE_INIT;
|
|
plat_once(&once, global_init, NULL);
|
|
return global;
|
|
}
|
|
|
|
static double norm3(const double v[3]) { return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); }
|
|
static void cross3(const double a[3], const double b[3], double out[3]) {
|
|
out[0] = a[1] * b[2] - a[2] * b[1];
|
|
out[1] = a[2] * b[0] - a[0] * b[2];
|
|
out[2] = a[0] * b[1] - a[1] * b[0];
|
|
}
|
|
// Rows, from the up row (the one the gravity correction keeps right).
|
|
static void orthonormalize(double R[3][3]) {
|
|
double n = norm3(R[2]);
|
|
for (int j = 0; j < 3; j++) R[2][j] /= n;
|
|
double d = R[0][0] * R[2][0] + R[0][1] * R[2][1] + R[0][2] * R[2][2];
|
|
for (int j = 0; j < 3; j++) R[0][j] -= d * R[2][j];
|
|
n = norm3(R[0]);
|
|
for (int j = 0; j < 3; j++) R[0][j] /= n;
|
|
cross3(R[2], R[0], R[1]);
|
|
}
|
|
// A fresh orientation: z up against the measured gravity, x as close to the controller's x as that allows.
|
|
static void align_to(double R[3][3], const double acc[3]) {
|
|
double n = norm3(acc), up[3] = {0, 0, 1}, x[3] = {1, 0, 0};
|
|
if (n > 0.5)
|
|
for (int j = 0; j < 3; j++) up[j] = -acc[j] / n;
|
|
if (fabs(up[0]) > 0.9) { x[0] = 0; x[1] = 1; }
|
|
memcpy(R[2], up, sizeof up);
|
|
memcpy(R[0], x, sizeof x);
|
|
orthonormalize(R);
|
|
}
|
|
// Body-frame rate: R = R * exp([w] dt), the rate plus the gravity pull.
|
|
static void integrate(double R[3][3], const double gyro[3], const double acc[3], double dt) {
|
|
double w[3] = {gyro[0], gyro[1], gyro[2]}, n = norm3(acc);
|
|
if (fabs(n - 1) < 0.15) { // not being accelerated: turn the estimated down toward the measured one
|
|
const double kp = 0.5;
|
|
double measured[3] = {acc[0] / n, acc[1] / n, acc[2] / n}, estimated[3] = {-R[2][0], -R[2][1], -R[2][2]}, e[3];
|
|
cross3(measured, estimated, e);
|
|
for (int j = 0; j < 3; j++) w[j] += kp * e[j];
|
|
}
|
|
double rate = norm3(w), theta = rate * dt;
|
|
if (theta < 1e-12) return;
|
|
double k[3] = {w[0] / rate, w[1] / rate, w[2] / rate}, s = sin(theta), c = 1 - cos(theta);
|
|
double K[3][3] = {{0, -k[2], k[1]}, {k[2], 0, -k[0]}, {-k[1], k[0], 0}}, E[3][3], out[3][3];
|
|
for (int i = 0; i < 3; i++)
|
|
for (int j = 0; j < 3; j++) {
|
|
double kk = 0;
|
|
for (int q = 0; q < 3; q++) kk += K[i][q] * K[q][j];
|
|
E[i][j] = (i == j) + s * K[i][j] + c * kk;
|
|
}
|
|
for (int i = 0; i < 3; i++)
|
|
for (int j = 0; j < 3; j++) {
|
|
double v = 0;
|
|
for (int q = 0; q < 3; q++) v += R[i][q] * E[q][j];
|
|
out[i][j] = v;
|
|
}
|
|
memcpy(R, out, sizeof out);
|
|
orthonormalize(R);
|
|
}
|
|
|
|
// 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
|
|
// 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);
|
|
m->restart = 0;
|
|
} else integrate(m->R, gyro, acc, dt);
|
|
MotionState *s = &m->ring[m->produced % MOTION_RING];
|
|
*s = (MotionState){.delta_ns = (int64_t)(dt * 1e9), .sampling = (int64_t)m->produced + 1, .attributes = 1};
|
|
for (int i = 0; i < 3; i++) {
|
|
double rps = gyro[i] / (2 * M_PI);
|
|
m->angle[i] += rps * dt;
|
|
s->acc[i] = (float)acc[i];
|
|
s->gyro[i] = (float)rps;
|
|
s->angle[i] = (float)m->angle[i];
|
|
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) {
|
|
if (max <= 0) return 0;
|
|
int n = 0;
|
|
plat_mutex_lock_shared(&m->lock);
|
|
if (m->produced > m->first && (now < m->last_ns || now - m->last_ns < STALE_NS)) {
|
|
uint64_t have = m->produced - m->first < MOTION_RING ? m->produced - m->first : MOTION_RING;
|
|
n = (int)(have < (uint64_t)max ? have : (uint64_t)max);
|
|
for (int i = 0; i < n; i++) out[i] = m->ring[(m->produced - 1 - (uint64_t)i) % MOTION_RING];
|
|
}
|
|
plat_mutex_unlock_shared(&m->lock);
|
|
return n;
|
|
}
|
|
|
|
void motion_restart(MotionFusion *m) {
|
|
plat_mutex_lock(&m->lock);
|
|
m->restart = 1;
|
|
m->first = m->produced;
|
|
m->last_ns = 0; // the first sample after the restart integrates nothing across the stop
|
|
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;
|
|
}
|