Add gyro bias calibration for the six-axis sensor

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.
This commit is contained in:
palxex committed 2026-11-15 02:26:30 +08:00
1 parent 3858ad4b51
commit dc13b194ce
8 files changed
+343 -5

No files matched your search

+14
View File
@@ -114,6 +114,16 @@ 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);
void input_motion_calibrate(void); // start collecting resting gyro samples for a bias estimate
// What the Controls tab's Calibrate gyro row shows: 0 no bias for this controller, 1 collecting, 2 calibrated
// (by this run or loaded from totk.ini), 3 the last attempt timed out. pad receives the controller the bias belongs
// to (empty when none), copied so the caller does not read a buffer another thread may be rewriting.
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 load and 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 +141,7 @@ 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
void motion_calibrate_start(MotionFusion *m, const char *pad_name); // start collecting; pad_name saved with the bias
int motion_calib_poll(MotionFusion *m, uint64_t now_ns); // the timeout, for when motion_feed is not called
void motion_bias_load(MotionFusion *m); // load the bias from totk.ini (TOTK_GYRO_BIAS_*)
void motion_bias_reset(MotionFusion *m); // zero the bias (new controller)