mirror of
https://github.com/valentderah/totk-pc-recomp.git
synced 2026-11-18 20:15:56 +00:00
The yaw axis has no gravity reference, so it drifts with the gyro's manufacturing bias. Two controllers of the same model can drift in opposite directions, and the sensor fusion had no way to estimate or remove the offset: aiming with motion turned slowly on its own and needed constant correction. A Calibrate gyro row on the Controls tab collects 250 gyro samples (about a second) while the controller is held still, averages them per axis into a bias, subtracts that bias from every sample before the fusion integrates it, and restarts the angle so the estimate starts clean. Collection has no motion threshold: a controller whose bias exceeds any absolute threshold would otherwise never finish, which is how the first version of this stalled with no way out. Five seconds without the expected samples ends the attempt, from the sample path and from the menu alike, so a sensor thread that stops feeding it is reported instead of leaving the row on "calibrating". The first raw sample and the final bias are logged, so a report carries the numbers that were measured. The row's value says where the controller stands: calibrating, "<controller>, calibrated" once it is done or when the bias was loaded from totk.ini at start, and "calibration failed" when an attempt timed out with no bias in effect. A controller change cancels a calibration that is in progress and clears the bias and the label, so neither is carried over to the new controller. The behaviour sits behind gyro_calibration (the Controls tab's "Gyro bias calibration", default on, live): off, the fusion takes the raw gyro, an attempt in progress is dropped, and nothing is loaded at start, while the saved keys stay in totk.ini so turning it back on restores the last calibration. The bias and the controller's name go to totk.ini when calibration completes, outside the fusion's lock, and are loaded at the next start only when the connected controller has the same name. Tested with a Switch Pro Controller on the 1.4.3 build: the log shows a z-axis bias of 0.139 rad/s (8 degrees per second), calibration finishes in about a second, the yaw drift is gone, and the row reports each state.
345 lines
16 KiB
C
345 lines
16 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. 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, CALIB_SAMPLES = 250 };
|
|
#define STALE_NS 200000000ull // no sample for 200 ms: no motion
|
|
#define CALIB_TIMEOUT_NS 5000000000ull // 5 s: give up if sensor data stops arriving mid-calibration
|
|
|
|
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]; // estimated resting bias (rad/s), subtracted from the gyro
|
|
int calibrating; // 1: collecting resting samples for a new bias
|
|
double calib_sum[3]; // accumulated gyro samples during calibration
|
|
int calib_count; // samples collected so far
|
|
uint64_t calib_start_ns; // when calibration started (timeout check)
|
|
char calib_pad[128]; // controller name that was calibrated (for loading the bias on the next run)
|
|
int calib_result; // 0 no attempt or one in progress, 1 the last attempt finished, -1 it timed out
|
|
int bias_valid; // 1 when gyro_bias holds a real estimate (loaded or measured), so a zero is a value
|
|
};
|
|
|
|
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();
|
|
}
|
|
|
|
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, int samples) {
|
|
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);
|
|
if (pad[0]) 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", samples, bias[0], bias[1], bias[2]);
|
|
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, so the two cannot drift apart. Called
|
|
// with m->lock held; the caller logs, with the lock released.
|
|
static int calib_check_timeout(MotionFusion *m, uint64_t now) {
|
|
if (!m->calibrating || now - m->calib_start_ns <= CALIB_TIMEOUT_NS) return 0;
|
|
m->calibrating = 0;
|
|
// A bias that is already in effect stays in effect, and stays reported as calibrated: the row describes the bias
|
|
// that is applied, and the failed attempt is in the log. A controller with no bias at all shows the failure.
|
|
m->calib_result = m->bias_valid ? 1 : -1;
|
|
return 1;
|
|
}
|
|
|
|
// Copy a name into cap bytes without splitting a UTF-8 sequence; the same as input.c's copy_name, which this file
|
|
// cannot include from.
|
|
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;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
void motion_feed(MotionFusion *m, const double acc[3], const double gyro_raw[3], uint64_t now) {
|
|
double saved[3] = {0, 0, 0};
|
|
char saved_pad[sizeof m->calib_pad] = "";
|
|
int save = 0, samples = 0, first = 0, logged_timeout = 0;
|
|
uint64_t saved_start = 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: collect gyro samples for CALIB_SAMPLES frames. The user holds the controller still, so the
|
|
// average of the raw gyro (which reads the bias at rest) is the bias estimate. No motion threshold: the bias
|
|
// itself can exceed any absolute threshold, which would block collection forever. calib_check_timeout decides
|
|
// the deadline for both this path and the menu's poll, so the two cannot drift apart. With the calibration
|
|
// switch off, an attempt in progress is dropped and no new one starts; the bias stops applying below.
|
|
if (m->calibrating && !motion_calibration_enabled()) {
|
|
m->calibrating = 0;
|
|
m->calib_result = 0;
|
|
}
|
|
if (m->calibrating) {
|
|
if (calib_check_timeout(m, now)) {
|
|
logged_timeout = 1;
|
|
samples = m->calib_count;
|
|
saved_start = m->calib_start_ns;
|
|
}
|
|
if (m->calibrating) {
|
|
for (int i = 0; i < 3; i++) m->calib_sum[i] += gyro_raw[i];
|
|
m->calib_count++;
|
|
first = m->calib_count == 1;
|
|
if (m->calib_count >= CALIB_SAMPLES) {
|
|
for (int i = 0; i < 3; i++) m->gyro_bias[i] = m->calib_sum[i] / m->calib_count;
|
|
m->calibrating = 0;
|
|
m->calib_result = 1;
|
|
m->bias_valid = 1;
|
|
m->restart = 1;
|
|
// Hand the result to save_bias below, which runs with the lock released.
|
|
memcpy(saved, m->gyro_bias, sizeof saved);
|
|
snprintf(saved_pad, sizeof saved_pad, "%s", m->calib_pad);
|
|
samples = m->calib_count;
|
|
save = 1;
|
|
}
|
|
}
|
|
}
|
|
// Subtract the bias from the gyro for integration and storage.
|
|
// The bias applies only while the switch is on: off, the fusion sees the raw gyro again.
|
|
const int calib_on = motion_calibration_enabled();
|
|
double gyro[3] = {gyro_raw[0] - (calib_on ? m->gyro_bias[0] : 0),
|
|
gyro_raw[1] - (calib_on ? m->gyro_bias[1] : 0),
|
|
gyro_raw[2] - (calib_on ? m->gyro_bias[2] : 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 (first)
|
|
rt_log("motion: calibration started, first raw gyro: %.5f %.5f %.5f rad/s", gyro_raw[0], gyro_raw[1],
|
|
gyro_raw[2]);
|
|
if (logged_timeout) {
|
|
double secs = (now - saved_start) / 1e9; // captured under the lock; another attempt may have begun since
|
|
rt_log("motion: gyro calibration timed out after %.1f s (%d samples): sensor data not arriving", secs, samples);
|
|
}
|
|
if (save) save_bias(saved, saved_pad, samples);
|
|
}
|
|
|
|
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_name) {
|
|
plat_mutex_lock(&m->lock);
|
|
m->calibrating = 1;
|
|
m->calib_result = 0;
|
|
m->calib_count = 0;
|
|
m->calib_sum[0] = m->calib_sum[1] = m->calib_sum[2] = 0;
|
|
m->calib_start_ns = kern_now_ns();
|
|
// The caller passes the same name it put into its own row label; copied again here so the fusion keeps the name
|
|
// the bias belongs to without relying on another buffer's lifetime. Same UTF-8-safe cut as input.c's copy_name:
|
|
// a name cut mid-sequence would reach totk.ini and the next run's comparisons.
|
|
copy_name(m->calib_pad, sizeof m->calib_pad, pad_name);
|
|
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 = 1; // 1 means "a bias is in effect", whether measured or loaded
|
|
plat_mutex_unlock(&m->lock);
|
|
}
|
|
|
|
void motion_bias_reset(MotionFusion *m) {
|
|
plat_mutex_lock(&m->lock);
|
|
m->gyro_bias[0] = m->gyro_bias[1] = m->gyro_bias[2] = 0;
|
|
// A calibration that was running belonged 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_sum[0] = m->calib_sum[1] = m->calib_sum[2] = 0;
|
|
m->calib_pad[0] = 0;
|
|
m->calib_result = 0;
|
|
m->bias_valid = 0;
|
|
plat_mutex_unlock(&m->lock);
|
|
}
|
|
|
|
// The timeout, for the case motion_feed is not called at all (the sensor thread stalled): without this the row would
|
|
// read "Calibrating..." forever, which is the failure the timeout exists to end. Returns the row's state: 0 none,
|
|
// 1 collecting, 2 calibrated, 3 the last attempt timed out. Logs with the lock released: the menu thread holds this
|
|
// lock briefly on every redraw, and the sensor thread waits for it.
|
|
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 ? 1 : m->calib_result > 0 ? 2 : m->calib_result < 0 ? 3 : 0;
|
|
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;
|
|
}
|