mirror of
https://github.com/valentderah/totk-pc-recomp.git
synced 2026-11-19 04:15:57 +00:00
365 lines
18 KiB
C
365 lines
18 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; 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
|
|
// 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
|
|
// 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
|
|
#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
|
|
|
|
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 (|acc| over 0.5 G), 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], x[3] = {1, 0, 0};
|
|
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
|
|
double kp = fabs(n - 1) < 0.05 && norm3(gyro) < STILL_RATE ? STILL_GAIN : 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 && norm3(acc) < 0.5) {
|
|
plat_mutex_unlock(&m->lock);
|
|
return;
|
|
}
|
|
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;
|
|
}
|