Merge task/gyro: gyro through Steam Input, levelled from the first reading, optional drift learning at rest (#1282, #292, #1267)

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
Valiantsin DzerakhandClaude Opus 5.5 committed 2026-11-18 09:12:06 +03:00
commit c3ec7fce0e
7 files changed
+466 -47

No files matched your search

+20
View File
@@ -246,6 +246,26 @@ Windows does not present the Switch 2 Pro Controller as a gamepad SDL can open (
Bridges that output a ViGEm DualShock 4 (ProCon2Windows, connectyourpro as tested) give buttons and rumble but no motion: their virtual DualShock 4 reports a gyro and an accelerometer that never change, so motion aiming and Ultrahand rotation do nothing. Run one bridge at a time; Steam Input can also claim the controller, so close Steam if the bridge does not find it.
## Steam Input and motion
A game Steam starts sees Steam's virtual gamepad in place of the controllers Steam Input reads (Steam lists them in
`SDL_GAMECONTROLLER_IGNORE_DEVICES`), and that virtual pad has no gyro or accelerometer. With `motion = 1` the game
clears that list before SDL starts (`runtime/input.c`, `steam_reveal`): buttons and sticks still come from the virtual
pad, so Steam's remapping applies and nothing is read twice, and when the virtual pad has no sensors the motion thread
opens the real controller behind it for its gyro and accelerometer only (`motion_source`). The log then shows
`input: motion from "<name>" (<vendor>:<product>), the controller behind Steam Input's virtual gamepad`. This covers the
Steam Controller and the Steam Deck's built-in controls, which Steam Input cannot be turned off for, and other
controllers Steam Input wraps. A controller Steam hid serves for buttons only while no other gamepad is there (Steam's
virtual pad can come up after the game starts); once the virtual pad is up the buttons move to it. With `motion = 0`
at start Steam's list stays in force.
Steam switches the controller's gyro off while the game's layout binds nothing to it: the reports keep coming with the
gyro at zero, and the game's request for it (SDL renews it every 3 s) does not win. The layout needs a gyro binding,
for example Gyro Behavior "As Mouse"; its activation only decides when Steam itself moves the mouse, the game gets the
gyro all the time. Pick an activation the game does not need (a back button), or Steam's mouse turns the camera on top
of the game's own motion. Under Steam the Steam Controller's gyro arrives already corrected, so it keeps a bias of its
own (`gyro_bias_pad` ends in "(under Steam)").
## Scripted input
`TOTK_INPUT_SCRIPT=<file>` replays presses. It is ORed with real input and still applies under `TOTK_INPUT=none`. One event per line, `#` starts a comment:
+3 -3
View File
@@ -50,7 +50,7 @@ static const char *const known[] = {
"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",
"GYRO_BIAS_PAD", "GYRO_BIAS_X", "GYRO_BIAS_Y", "GYRO_BIAS_Z", "GYRO_CALIBRATION", "HDR", "HDR_GAME_NITS", "HDR_HUE",
"GYRO_AUTO_BIAS", "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", "HUD_WIDTH", "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_NO_CODE", "MODS_ORDER", "MODS_REBUILD", "MODS_WITHOUT_CODE", "MOTION", "MOUSE_AIM_SCALE", "MOUSE_MODE", "MOUSE_DIRECT_PATCH", "MOUSE_SENS", "MOVIE_ASPECT", "MOVIE_HOST", "MOVIE_PACE",
@@ -94,7 +94,7 @@ 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"}, {"GYRO_CALIBRATION", "input"}, {"RUMBLE_STRENGTH", "input"}, {"RUMBLE_CURVE", "input"}, {"RUMBLE_TRIGGERS", "input"},
{"MOTION", "input"}, {"GYRO_CALIBRATION", "input"}, {"GYRO_AUTO_BIAS", "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"},
@@ -170,7 +170,7 @@ 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}, {"GYRO_CALIBRATION", "1", "0|1", LIVE},
{"MOTION", "1", "0|1", LIVE}, {"GYRO_CALIBRATION", "1", "0|1", LIVE}, {"GYRO_AUTO_BIAS", "0", "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.
+179 -29
View File
@@ -43,6 +43,17 @@ static int pad_east_a; // ... and labels its east face button A (N
static int pad_layout = -1; // PAD_LAYOUT_* in effect for the open gamepad, -1 to pick again; live (cfg_version)
static unsigned pad_layout_seen;
static volatile int32_t pad_layout_shown = -1; // pad_layout for the menu (any thread), -1 without a gamepad
// Steam Input: a game Steam starts gets SDL_GAMECONTROLLER_IGNORE_DEVICES, the vendor/product pairs of the controllers
// Steam reads itself, and sees Steam's virtual gamepad in their place. That pad has no motion sensors, so a Steam
// Controller, a Steam Deck or a DualSense under Steam Input gives no gyro. steam_reveal keeps the list here and clears
// the hint: the buttons and sticks still come from the virtual pad (scan_pads skips the listed devices, so Steam's
// remapping holds and nothing is read twice), and only the sensors come from the real controller (motion_source).
enum { STEAM_HIDDEN_MAX = 64 };
static uint16_t steam_hidden[STEAM_HIDDEN_MAX][2]; // vendor, product
static int steam_hidden_n;
static SDL_Gamepad *motion_extra; // the hidden controller opened for its sensors only, NULL none
static int pad_hidden; // the open gamepad is a controller Steam hid (scan_pads' fallback)
static uint64_t next_motion_scan;
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)
@@ -424,9 +435,81 @@ static void stick(SDL_GamepadAxis ax, SDL_GamepadAxis ay, float dead, int32_t ou
input_pad_stick(SDL_GetGamepadAxis(pad, ax), -(int32_t)SDL_GetGamepadAxis(pad, ay), dead, out);
}
// Open the first gamepad when none is open or the open one went away; logs once per change.
// Before SDL_Init: keep the controllers Steam hid (steam_hidden) and let SDL open them. The list is
// "0xVVVV/0xPPPP,..." (SDL's own format). With motion off (TOTK_MOTION=0 at start) Steam's list stays in force.
static void steam_reveal(void) {
const char *e = getenv("SDL_GAMECONTROLLER_IGNORE_DEVICES");
if (!e || !*e || !cfg_bool("TOTK_MOTION")) return;
for (const char *s = e; *s && steam_hidden_n < STEAM_HIDDEN_MAX;) {
unsigned v, p;
int used = 0;
if (sscanf(s, " %x/%x%n", &v, &p, &used) == 2 && used > 0) {
steam_hidden[steam_hidden_n][0] = (uint16_t)v;
steam_hidden[steam_hidden_n][1] = (uint16_t)p;
steam_hidden_n++;
s += used;
}
const char *c = strchr(s, ',');
if (!c) break;
s = c + 1;
}
if (!steam_hidden_n) return;
// The environment outranks a hint of normal priority, so this one overrides it
SDL_SetHintWithPriority(SDL_HINT_GAMECONTROLLER_IGNORE_DEVICES, "", SDL_HINT_OVERRIDE);
rt_log("input: Steam Input hides %d controller type(s); buttons stay on Steam's virtual gamepad, motion is read "
"from the controller itself", steam_hidden_n);
}
// The ids the device itself reports. SDL_GetGamepadVendorForID and ...ProductForID give a Steam virtual pad the ids of
// the controller behind it, which would make it look like a controller Steam hid; the GUID keeps the device's own.
static void raw_ids(SDL_JoystickID id, uint16_t *v, uint16_t *p) {
SDL_GetJoystickGUIDInfo(SDL_GetGamepadGUIDForID(id), v, p, NULL, NULL);
}
static int steam_hidden_id(SDL_JoystickID id) {
uint16_t v, p;
raw_ids(id, &v, &p);
for (int i = 0; i < steam_hidden_n; i++)
if (steam_hidden[i][0] == v && steam_hidden[i][1] == p) return 1;
return 0;
}
// Steam's virtual gamepad: Valve's vendor id with product 0x11ff in the device's own ids (raw_ids; SDL's
// SDL_IsJoystickSteamVirtualGamepad is not public)
static int steam_virtual(SDL_Gamepad *g) {
uint16_t v, p;
raw_ids(SDL_GetGamepadID(g), &v, &p);
return v == 0x28de && p == 0x11ff;
}
static int has_motion(SDL_Gamepad *g) {
return SDL_GamepadHasSensor(g, SDL_SENSOR_GYRO) && SDL_GamepadHasSensor(g, SDL_SENSOR_ACCEL);
}
// A gamepad Steam did not hide is connected (Steam's virtual pad, or another controller).
static int unhidden_pad_present(void) {
int n = 0, found = 0;
SDL_JoystickID *ids = SDL_GetGamepads(&n);
for (int i = 0; ids && i < n && !found; i++) found = !steam_hidden_id(ids[i]);
SDL_free(ids);
return found;
}
// Open the first gamepad when none is open or the open one went away; logs once per change. A controller Steam hid
// (steam_hidden) is taken only when no other gamepad is there, e.g. while Steam's virtual pad is not up yet, and gives
// way to the virtual pad once it is (pad_physical looks again every RESCAN_MS while pad_hidden): otherwise Steam's
// remapping would be lost for the session. The hidden controller then stays open for its sensors (motion_extra), so
// its bias and orientation go on.
static void scan_pads(void) {
if (pad && SDL_GamepadConnected(pad)) return;
if (pad && SDL_GamepadConnected(pad)) {
if (!pad_hidden || !unhidden_pad_present()) return;
rt_log("input: Steam Input's virtual gamepad is up: buttons move to it from \"%s\"", SDL_GetGamepadName(pad));
if (!motion_extra && has_motion(pad)) motion_extra = pad;
else SDL_CloseGamepad(pad);
pad = NULL;
pad_hidden = 0;
}
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.
@@ -438,9 +521,16 @@ static void scan_pads(void) {
}
int n = 0;
SDL_JoystickID *ids = SDL_GetGamepads(&n);
for (int i = 0; ids && i < n && !pad; i++) pad = SDL_OpenGamepad(ids[i]);
for (int pass = 0; pass < 2 && !pad; pass++)
for (int i = 0; ids && i < n && !pad; i++)
if (steam_hidden_id(ids[i]) == pass) {
// The controller motion_source holds for its sensors serves as the pad too: one open handle
if (motion_extra && SDL_GetGamepadID(motion_extra) == ids[i]) { pad = motion_extra; motion_extra = NULL; }
else pad = SDL_OpenGamepad(ids[i]);
}
SDL_free(ids);
if (!pad) return;
pad_hidden = steam_hidden_n && steam_hidden_id(SDL_GetGamepadID(pad));
pad_rumble = SDL_GetBooleanProperty(SDL_GetGamepadProperties(pad), SDL_PROP_GAMEPAD_CAP_RUMBLE_BOOLEAN, false);
pad_east_a = SDL_GetGamepadButtonLabel(pad, SDL_GAMEPAD_BUTTON_EAST) == SDL_GAMEPAD_BUTTON_LABEL_A;
for (int i = 0; i < 4; i++) {
@@ -457,7 +547,10 @@ static void scan_pads(void) {
}
pad_layout = -1;
int motion = SDL_GamepadHasSensor(pad, SDL_SENSOR_GYRO) && SDL_GamepadHasSensor(pad, SDL_SENSOR_ACCEL);
rt_log("input: gamepad \"%s\" (%s, %s, east button %s)", SDL_GetGamepadName(pad), pad_rumble ? "rumble" : "no rumble",
uint16_t rv, rp;
raw_ids(SDL_GetGamepadID(pad), &rv, &rp);
rt_log("input: gamepad \"%s\" %04x:%04x%s (%s, %s, east button %s)", SDL_GetGamepadName(pad), rv, rp,
steam_virtual(pad) ? " Steam Input" : "", pad_rumble ? "rumble" : "no rumble",
motion ? "gyro and accelerometer" : "no motion sensors", pad_east_a ? "labelled A" : "not labelled A");
}
@@ -532,6 +625,7 @@ static void start(void) {
binds_refresh();
const char *e = cfg_get("TOTK_INPUT");
if (e && !strcmp(e, "none")) { state = -1; rt_log("input: disabled (TOTK_INPUT=none)"); return; }
steam_reveal();
if (!SDL_Init(SDL_INIT_GAMEPAD)) { state = -1; rt_log("input: SDL gamepad init failed (%s), keyboard only", SDL_GetError()); }
else state = 1;
if (state == 1) test_pad_attach();
@@ -654,7 +748,7 @@ static void wheel_route(uint64_t *bp, int32_t sl[2], int32_t sr[2]) {
// Under lock, state 1: the open gamepad's inputs by position (pd[PAD_*] != 0 held); 0 without a connected gamepad.
static int pad_physical(uint8_t pd[PAD_COUNT]) {
if ((!pad || !SDL_GamepadConnected(pad)) && plat_now_ns() / 1000000u >= next_scan) { next_scan = plat_now_ns() / 1000000u + RESCAN_MS; scan_pads(); }
if ((!pad || !SDL_GamepadConnected(pad) || pad_hidden) && plat_now_ns() / 1000000u >= next_scan) { next_scan = plat_now_ns() / 1000000u + RESCAN_MS; scan_pads(); }
SDL_UpdateGamepads();
if (!pad || !SDL_GamepadConnected(pad)) return 0;
for (int p = PAD_SOUTH; p < PAD_ZL; p++) pd[p] = SDL_GetGamepadButton(pad, pad_sdl[p]);
@@ -983,17 +1077,24 @@ static void copy_name(char *dst, size_t cap, const char *src) {
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
// Under lock: the identity of src (motion_source) 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);
static void pad_identity(SDL_Gamepad *src, char *dst, size_t cap) {
const char *name = SDL_GetGamepadName(src), *serial = SDL_GetGamepadSerial(src);
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 : "");
// Steam's virtual pad takes the name of the controller behind it but sends Steam's corrected motion: its bias
// is its own (pad_bias_select would otherwise load the raw controller's)
// A controller read past Steam Input sends what Steam set it to: on the Steam Controller that is motion the
// controller already corrected, so the bias of the same controller read with Steam closed does not apply either
if (steam_virtual(src)) strncat(id, " (Steam Input)", sizeof id - strlen(id) - 1);
else if (steam_hidden_n && steam_hidden_id(SDL_GetGamepadID(src)))
strncat(id, " (under Steam)", sizeof id - strlen(id) - 1);
copy_name(dst, cap, id);
}
@@ -1001,35 +1102,82 @@ static void pad_identity(char *dst, size_t cap) {
// 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) {
static void pad_bias_select(SDL_Gamepad *src) {
char id[128];
pad_identity(id, sizeof id);
pad_identity(src, 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));
copy_name(calib_pad_name, sizeof calib_pad_name, SDL_GetGamepadName(src));
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);
// Under lock: the gamepad the sensors are read from, NULL without one. The open pad when it has a gyro and an
// accelerometer, or when Steam hid nothing. Otherwise (Steam's virtual pad, which has neither) the first controller
// Steam hid that has both, looked for once a RESCAN_MS; one without them is not opened again until it reconnects.
static SDL_Gamepad *motion_source(void) {
static SDL_JoystickID rejected[8];
if (motion_extra && !SDL_GamepadConnected(motion_extra)) {
rt_log("input: \"%s\" (motion only) disconnected", SDL_GetGamepadName(motion_extra));
SDL_CloseGamepad(motion_extra);
motion_extra = NULL;
}
int pad_on = pad && SDL_GamepadConnected(pad);
// Newer Steam clients give the virtual pad motion too, but already corrected by Steam (and gated by the layout's
// gyro settings), so the bias calibrated on the raw controller would count twice and the camera drift. Under
// Steam Input the real controller's raw sensors come first; the virtual pad's are the fallback.
if (pad_on && has_motion(pad) && !(steam_hidden_n && steam_virtual(pad))) {
if (motion_extra) { SDL_CloseGamepad(motion_extra); motion_extra = NULL; }
return pad;
}
if (!motion_extra && steam_hidden_n && plat_now_ns() / 1000000u >= next_motion_scan) {
next_motion_scan = plat_now_ns() / 1000000u + RESCAN_MS;
int n = 0;
SDL_JoystickID *ids = SDL_GetGamepads(&n), skip = pad_on ? SDL_GetGamepadID(pad) : 0;
for (int i = 0; ids && i < n && !motion_extra; i++) {
int seen = 0;
for (int r = 0; r < 8; r++) seen |= rejected[r] == ids[i];
if (ids[i] == skip || seen || !steam_hidden_id(ids[i])) continue;
SDL_Gamepad *g = SDL_OpenGamepad(ids[i]);
if (g && has_motion(g)) {
motion_extra = g;
rt_log("input: motion from \"%s\" (%04x:%04x), the controller behind Steam Input's virtual gamepad",
SDL_GetGamepadName(g), SDL_GetGamepadVendor(g), SDL_GetGamepadProduct(g));
} else {
if (g) SDL_CloseGamepad(g);
memmove(rejected + 1, rejected, sizeof rejected - sizeof rejected[0]);
rejected[0] = ids[i];
}
}
SDL_free(ids);
}
return motion_extra ? motion_extra : pad_on ? pad : NULL;
}
// Under lock: (re)enable the sensors when the motion source changed; 1 if it has both and they are on.
static int sensors_ready(SDL_Gamepad *src) {
SDL_JoystickID id = SDL_GetGamepadID(src);
if (id == sensor_pad) return sensor_ok;
sensor_pad = id;
pad_bias_select();
pad_bias_select(src);
// 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) &&
SDL_SetGamepadSensorEnabled(pad, SDL_SENSOR_GYRO, true) &&
SDL_SetGamepadSensorEnabled(pad, SDL_SENSOR_ACCEL, true);
if (sensor_ok) rt_log("input: motion sensors on (gyro %.0f Hz, accelerometer %.0f Hz)",
(double)SDL_GetGamepadSensorDataRate(pad, SDL_SENSOR_GYRO),
(double)SDL_GetGamepadSensorDataRate(pad, SDL_SENSOR_ACCEL));
sensor_ok = has_motion(src) &&
SDL_SetGamepadSensorEnabled(src, SDL_SENSOR_GYRO, true) &&
SDL_SetGamepadSensorEnabled(src, SDL_SENSOR_ACCEL, true);
if (sensor_ok) {
rt_log("input: motion sensors on (gyro %.0f Hz, accelerometer %.0f Hz)",
(double)SDL_GetGamepadSensorDataRate(src, SDL_SENSOR_GYRO),
(double)SDL_GetGamepadSensorDataRate(src, SDL_SENSOR_ACCEL));
motion_restart(motion_global()); // another controller: its own orientation, from its first reading
} else if (has_motion(src)) // after PR #1024: the SDL error, not "no sensors", when enabling them failed
rt_log("input: \"%s\" has a gyro and an accelerometer but SDL could not turn them on (%s): the six-axis sensor "
"reads a controller at rest", SDL_GetGamepadName(src), SDL_GetError());
else rt_log("input: \"%s\" has no gyro and accelerometer: the six-axis sensor reads a controller at rest",
SDL_GetGamepadName(pad));
SDL_GetGamepadName(src));
return sensor_ok;
}
@@ -1054,10 +1202,11 @@ static void motion_main(void *arg) {
float a[3], g[3];
int have = 0;
plat_mutex_lock(&lock);
if (pad && SDL_GamepadConnected(pad)) {
SDL_Gamepad *src = motion_source();
if (src) {
SDL_UpdateGamepads();
have = sensors_ready() && SDL_GetGamepadSensorData(pad, SDL_SENSOR_ACCEL, a, 3) &&
SDL_GetGamepadSensorData(pad, SDL_SENSOR_GYRO, g, 3);
have = sensors_ready(src) && SDL_GetGamepadSensorData(src, SDL_SENSOR_ACCEL, a, 3) &&
SDL_GetGamepadSensorData(src, SDL_SENSOR_GYRO, g, 3);
}
plat_mutex_unlock(&lock);
if (!have || !input_motion_enabled()) continue;
@@ -1090,18 +1239,19 @@ void input_motion_calibrate(void) {
// 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;
SDL_Gamepad *src = motion_thread == 1 ? motion_source() : 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";
else if (!src) 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";
else if (!sensors_ready(src)) 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));
pad_identity(src, id, sizeof id);
copy_name(calib_pad_name, sizeof calib_pad_name, SDL_GetGamepadName(src));
motion_calibrate_start(motion_global(), id, kern_now_ns());
rt_log("input: gyro calibration started on %s", calib_pad_name);
}
+3
View File
@@ -149,6 +149,9 @@ int input_motion_calib_state(char *pad, size_t cap);
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);
// The drift tracker's switch (gyro_auto_bias, live): the drift left after calibration, learned while the controller
// lies still, is subtracted too.
int motion_auto_bias_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]);
+148 -15
View File
@@ -6,12 +6,17 @@
// 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.
// 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, and the drift left after it is learned while the controller lies still and subtracted
// too while gyro_auto_bias is on (drift_track). 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>
@@ -31,6 +36,29 @@ enum { MOTION_RING = 64, CALIB_SETTLE = 125, CALIB_SAMPLES = 250 };
// 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
// The drift tracker (drift_track), after PR #380's estimator: a window of REST_WIN samples (256 ms) is still when every
// axis of the gyro stays within REST_GYRO_PTP peak to peak and of the acceleration within REST_ACC_PTP, |a| is within
// REST_GRAVITY of 1 G, and the window's mean rate is within DRIFT_FIRST_MAX (before the estimate settled) or
// DRIFT_TRACK_MAX (after) of the estimate in use. A Pro Controller on a table reads 0.004-0.006 rad/s of noise and
// passes; held in a hand, even steadily, it reads 0.013-0.025 rad/s and mostly does not, so the tracker learns from a
// controller put down rather than from an aim held still. After REST_HOLD_S still the estimate moves toward the
// window's mean with a time constant of DRIFT_TAU_FIRST until it has settled, then DRIFT_TAU: a bias changes only
// slowly as the controller warms up, and a hand that slips past the gate shifts it little. No dead zone: a slow aim
// keeps every bit of its rate.
enum { REST_WIN = 64 };
#define REST_GYRO_PTP 0.04 // rad/s, per axis over the window
#define REST_ACC_PTP 0.03 // G, per axis over the window
#define REST_GRAVITY 0.25 // G: controllers' accelerometers read 1 G only roughly
#define REST_HOLD_S 0.5
#define DRIFT_FIRST_MAX 0.1 // rad/s, 5.7 deg/s; a larger factory bias (one Pro: 0.14 on z) needs the calibration
#define DRIFT_TRACK_MAX 0.05 // rad/s
#define DRIFT_TAU_FIRST 0.25 // s
#define DRIFT_TAU 30.0 // s
#define DRIFT_SETTLE_S 1.5 // learning this long with the estimate within DRIFT_SETTLED of the window: settled
#define DRIFT_SETTLED 0.01 // rad/s
#define DRIFT_LIMIT 0.3 // rad/s per axis
struct MotionFusion {
PlatMutex lock;
@@ -49,6 +77,16 @@ struct MotionFusion {
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
// The drift tracker: the estimate (rad/s, on top of gyro_bias when gyro_calibration is on), whether it settled,
// and the rest window it is learned from. The estimate belongs to the controller and to the bias it was learned
// against: kept through restarts, cleared with a new controller, a new calibration or a change of
// gyro_calibration (drift_clear).
double drift[3];
int drift_settled;
int drift_base; // gyro_calibration the estimate was learned with, -1 none
double rest_s, learn_s;
double win_g[REST_WIN][3], win_a[REST_WIN][3];
int win_n, win_i;
};
MotionFusion *motion_new(void) {
@@ -56,6 +94,7 @@ MotionFusion *motion_new(void) {
if (!m) rt_fatal("motion: out of memory");
plat_mutex_init(&m->lock);
m->restart = 1;
m->drift_base = -1;
return m;
}
@@ -89,11 +128,11 @@ static void orthonormalize(double R[3][3]) {
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.
// 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] = {0, 0, 1}, x[3] = {1, 0, 0};
if (n > 0.5)
for (int j = 0; j < 3; j++) up[j] = -acc[j] / n;
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);
@@ -103,7 +142,7 @@ static void align_to(double R[3][3], const double acc[3]) {
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 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];
@@ -128,6 +167,80 @@ static void integrate(double R[3][3], const double gyro[3], const double acc[3],
orthonormalize(R);
}
// The tracker's own switch (gyro_auto_bias, live, default 0: subtracting a learned bias made aiming feel worse in
// play, cause unknown). Off, nothing is learned or subtracted.
int motion_auto_bias_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_AUTO_BIAS");
}
return on;
}
// m->lock held: forget the estimate and the rest window.
static void drift_clear(MotionFusion *m) {
memset(m->drift, 0, sizeof m->drift);
m->drift_settled = 0;
m->drift_base = -1;
m->rest_s = m->learn_s = 0;
m->win_n = m->win_i = 0;
}
// Largest per-axis peak-to-peak of a full window.
static double win_ptp(const double w[REST_WIN][3]) {
double worst = 0;
for (int i = 0; i < 3; i++) {
double lo = w[0][i], hi = w[0][i];
for (int j = 1; j < REST_WIN; j++) {
if (w[j][i] < lo) lo = w[j][i];
if (w[j][i] > hi) hi = w[j][i];
}
if (hi - lo > worst) worst = hi - lo;
}
return worst;
}
// m->lock held, one sample with a reading: gyro is the rate with the calibrated bias taken off when base is 1. Moves
// m->drift toward the rate of a controller lying still. Returns 1 when the estimate settled with this sample.
static int drift_track(MotionFusion *m, const double acc[3], const double gyro[3], int base, double dt) {
if (m->drift_base != base) { // nothing learned yet, or learned against the other baseline: start over
drift_clear(m);
m->drift_base = base;
}
memcpy(m->win_g[m->win_i], gyro, sizeof m->win_g[0]);
memcpy(m->win_a[m->win_i], acc, sizeof m->win_a[0]);
m->win_i = (m->win_i + 1) % REST_WIN;
if (m->win_n < REST_WIN) m->win_n++;
if (m->win_n < REST_WIN) return 0;
double mean[3] = {0, 0, 0}, a[3] = {0, 0, 0}, dev = 0;
for (int j = 0; j < REST_WIN; j++)
for (int i = 0; i < 3; i++) mean[i] += m->win_g[j][i] / REST_WIN, a[i] += m->win_a[j][i] / REST_WIN;
for (int i = 0; i < 3; i++)
if (fabs(mean[i] - m->drift[i]) > dev) dev = fabs(mean[i] - m->drift[i]);
int still = fabs(norm3(a) - 1) < REST_GRAVITY && win_ptp(m->win_g) < REST_GYRO_PTP &&
win_ptp(m->win_a) < REST_ACC_PTP && dev < (m->drift_settled ? DRIFT_TRACK_MAX : DRIFT_FIRST_MAX);
if (!still) {
m->rest_s = 0;
if (!m->drift_settled) m->learn_s = 0;
return 0;
}
m->rest_s += dt;
if (m->rest_s < REST_HOLD_S) return 0;
double k = dt / (m->drift_settled ? DRIFT_TAU : DRIFT_TAU_FIRST);
for (int i = 0; i < 3; i++) {
m->drift[i] += k * (mean[i] - m->drift[i]);
if (m->drift[i] > DRIFT_LIMIT) m->drift[i] = DRIFT_LIMIT;
if (m->drift[i] < -DRIFT_LIMIT) m->drift[i] = -DRIFT_LIMIT;
}
m->learn_s += dt;
if (m->drift_settled || m->learn_s < DRIFT_SETTLE_S || dev >= DRIFT_SETTLED) return 0;
m->drift_settled = 1;
return 1;
}
// 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) {
@@ -203,18 +316,24 @@ static int calib_sample(MotionFusion *m, const double g[3], double *spread) {
m->bias_valid = 1;
m->calib_result = MOTION_CALIB_DONE;
m->restart = 1; // the angle and direction start over from the new bias
drift_clear(m); // what it learned was the old bias's remainder
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;
// Read before the lock: a settings change takes cfg.c's lock.
const int calib_on = motion_calibration_enabled(), auto_on = motion_auto_bias_enabled();
double saved[3] = {0, 0, 0}, spread = 0, drift[3] = {0, 0, 0};
char saved_pad[sizeof m->calib_pad] = "";
int ended = 0, timed_out = 0, samples = 0;
int ended = 0, timed_out = 0, samples = 0, settled = 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
if (dt > 0.05) { // a gap (no controller, sensors were off): no integration and no rest across it
dt = 0.004;
m->rest_s = 0;
m->win_n = m->win_i = 0;
}
// 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) {
@@ -237,6 +356,14 @@ void motion_feed(MotionFusion *m, const double acc[3], const double gyro_raw[3],
// 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);
// A sample without a reading (a sensor just switched on reads zero) neither starts a frame nor counts as rest.
const int reading = norm3(acc) >= 0.5;
if (!auto_on) {
if (m->drift_base >= 0) drift_clear(m);
} else if (reading && (settled = drift_track(m, acc, gyro, calib_on, dt)))
memcpy(drift, m->drift, sizeof drift);
for (int i = 0; i < 3; i++) gyro[i] -= m->drift[i]; // zero while the switch is off
if (m->restart && !reading) goto unlock; // no state yet; the logs below still run
if (m->restart) {
align_to(m->R, acc);
memset(m->angle, 0, sizeof m->angle);
@@ -253,9 +380,13 @@ void motion_feed(MotionFusion *m, const double acc[3], const double gyro_raw[3],
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++;
unlock:
// 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 (settled)
rt_log("motion: gyro drift at rest learned: %.5f %.5f %.5f rad/s, taken off %s", drift[0], drift[1], drift[2],
calib_on ? "on top of the calibrated bias" : "the raw gyro");
if (timed_out)
rt_log("motion: gyro calibration timed out after %.1f s (%d samples): sensor data not arriving",
(now - started) / 1e9, samples);
@@ -322,6 +453,7 @@ void motion_bias_load(MotionFusion *m) {
memcpy(m->gyro_bias, bias, sizeof bias);
m->bias_valid = 1;
m->calib_result = MOTION_CALIB_DONE;
drift_clear(m);
plat_mutex_unlock(&m->lock);
}
@@ -335,6 +467,7 @@ void motion_bias_reset(MotionFusion *m) {
m->calib_pad[0] = 0;
m->calib_result = MOTION_CALIB_NONE;
m->bias_valid = 0;
drift_clear(m); // the drift belonged to that controller too
plat_mutex_unlock(&m->lock);
}
+107
View File
@@ -244,6 +244,12 @@ static char *lang(void) {
}
static void set_env(const char *name, const char *value) { plat_setenv(name, value); } // NULL removes it
// A live setting read through a cfg_version cache: the environment wins, and the cfg_set (empty = the default) moves
// the version so the cache reads it again.
static void set_live(const char *name, const char *value) {
set_env(name, value);
cfg_set(name, value ? value : "");
}
static void probe_oe(void) {
char *keep_lang = cfg_get("TOTK_LANG") ? strdup(cfg_get("TOTK_LANG")) : NULL;
@@ -523,6 +529,26 @@ static void probe_time(void) {
static int near3(const float *v, double x, double y, double z, double tol) {
return fabs(v[0] - x) <= tol && fabs(v[1] - y) <= tol && fabs(v[2] - z) <= tol;
}
// A gyro sample: rate plus noise. Uniform noise of half-width amp from a fixed LCG, so every run reads the same.
static void noisy(const double rate[3], double amp, uint32_t *seed, double out[3]) {
for (int i = 0; i < 3; i++) {
*seed = *seed * 1664525u + 1013904223u;
out[i] = rate[i] + amp * ((double)(*seed >> 8) / (1 << 24) * 2 - 1);
}
}
// Feed secs of samples at the sensor thread's 4 ms; returns the yaw (angle z, revolutions) the last state reads.
static double feed_for(MotionFusion *m, const double acc[3], const double rate[3], double amp, uint32_t *seed,
double secs, uint64_t *t) {
MotionState s;
double g[3];
for (int i = 0; i < (int)(secs * 250 + 0.5); i++) {
noisy(rate, amp, seed, g);
motion_feed(m, acc, g, *t += 4000000);
}
return motion_read(m, &s, 1, *t + 1000000) == 1 ? s.angle[2] : NAN;
}
static int at_rest(const MotionState *s) {
return !s->attributes && !s->sampling && near3(s->acc, 0, 0, -1, 0) && near3(s->gyro, 0, 0, 0, 0) &&
near3(s->dir, 1, 0, 0, 0) && near3(s->dir + 3, 0, 1, 0, 0) && near3(s->dir + 6, 0, 0, 1, 0);
@@ -655,6 +681,87 @@ static void probe_feedback(uint32_t pid, uint32_t style) {
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);
// A sensor just switched on reads zero: the frame starts at the first reading, here standing on the bottom edge
// (controller y up). Then the down is wrong by a quarter turn (laid flat without turning): a second held still
// levels it to within 8 degrees.
MotionFusion *u = motion_new();
const double standing[3] = {0, -1, 0};
motion_feed(u, still, still, t);
int before = motion_read(u, ms, 16, t + 1000000);
motion_feed(u, standing, still, t += 4000000);
int first = motion_read(u, ms, 16, t + 1000000);
int upright_y = first == 1 && near3(ms[0].dir + 3, 0, 0, 1, 0.01);
for (int i = 0; i < 250; i++) motion_feed(u, down, still, t += 4000000);
motion_read(u, ms, 16, t + 1000000);
CHECK(before == 0 && upright_y && ms[0].dir[8] > 0.99,
"fusion start and levelling: %d states before a reading, %d after (y up %d), z up %.3f after 1 s still",
before, first, upright_y, (double)ms[0].dir[8]);
// Drift at rest (gyro_auto_bias). Table noise is +-0.006 rad/s, a hand's +-0.03; drift is the yaw rate a
// controller lying still reads, in rad/s, over the 10 s after the tracker had 10 s to learn.
char *keep_auto = cfg_get("TOTK_GYRO_AUTO_BIAS") ? strdup(cfg_get("TOTK_GYRO_AUTO_BIAS")) : NULL;
char *keep_cal = cfg_get("TOTK_GYRO_CALIBRATION") ? strdup(cfg_get("TOTK_GYRO_CALIBRATION")) : NULL;
set_live("TOTK_GYRO_CALIBRATION", "1");
const double drift_b[3] = {0.02, -0.03, 0.05}, rev = 2 * M_PI;
uint32_t seed = 1;
double drift_rate[2];
for (int on = 0; on < 2; on++) {
set_live("TOTK_GYRO_AUTO_BIAS", on ? "1" : "0");
MotionFusion *d = motion_new();
uint64_t td = t;
feed_for(d, down, drift_b, 0.006, &seed, 10, &td);
double y0 = feed_for(d, down, drift_b, 0.006, &seed, 0.004, &td);
drift_rate[on] = (feed_for(d, down, drift_b, 0.006, &seed, 10, &td) - y0) * rev / 10;
}
CHECK(fabs(drift_rate[0] - drift_b[2]) < 0.002 && fabs(drift_rate[1]) < 0.001,
"drift at rest: yaw %.5f rad/s with gyro_auto_bias off, %.5f on (bias %.3f)", drift_rate[0], drift_rate[1],
drift_b[2]);
// The #1267 case: calibrated, then the bias moved by 0.01 rad/s (the controller warmed up). The tracker takes the
// remainder on top of the calibrated bias, and a new calibration starts it over.
MotionFusion *c = motion_new();
uint64_t tc = t;
motion_calibrate_start(c, "probe pad", tc);
feed_for(c, down, drift_b, 0.006, &seed, 1.6, &tc);
int cal = motion_calib_poll(c, tc);
const double warm[3] = {drift_b[0], drift_b[1], drift_b[2] - 0.01};
double c0 = feed_for(c, down, warm, 0.006, &seed, 0.004, &tc);
double c_early = (feed_for(c, down, warm, 0.006, &seed, 0.5, &tc) - c0) * rev / 0.5;
feed_for(c, down, warm, 0.006, &seed, 10, &tc);
c0 = feed_for(c, down, warm, 0.006, &seed, 0.004, &tc);
double c_late = (feed_for(c, down, warm, 0.006, &seed, 10, &tc) - c0) * rev / 10;
CHECK(cal == MOTION_CALIB_DONE && fabs(c_early + 0.01) < 0.002 && fabs(c_late) < 0.001,
"drift after calibration: state %d, yaw %.5f rad/s right after, %.5f once learned", cal, c_early, c_late);
// In the hand: a slow aim, 0.05 rad/s (3 deg/s) of yaw with a hand's noise, for 10 s. Nothing is learned from it,
// so the full turn reaches the game.
MotionFusion *h = motion_new();
uint64_t th = t;
const double pan[3] = {0, 0, 0.05};
double h0 = feed_for(h, down, pan, 0.03, &seed, 0.004, &th);
double turned = (feed_for(h, down, pan, 0.03, &seed, 10, &th) - h0) * rev;
CHECK(fabs(turned - 0.5) < 0.01, "slow aim in the hand: %.4f rad of the 0.5 turned", turned);
// Levelling and drift together: the frame starts standing on the bottom edge, then the controller lies flat with
// a bias on every axis. The bias tilts the fused down by bias / gain (0.05 / 4 rad, 0.7 degrees) until it is
// learned; then the down is level, and yaw holds.
MotionFusion *l = motion_new();
uint64_t tl = t;
feed_for(l, standing, drift_b, 0.006, &seed, 0.004, &tl);
feed_for(l, down, drift_b, 0.006, &seed, 1, &tl);
motion_read(l, ms, 1, tl + 1000000);
double up_1s = ms[0].dir[8];
feed_for(l, down, drift_b, 0.006, &seed, 10, &tl);
double l0 = feed_for(l, down, drift_b, 0.006, &seed, 0.004, &tl);
double l_rate = (feed_for(l, down, drift_b, 0.006, &seed, 10, &tl) - l0) * rev / 10;
motion_read(l, ms, 1, tl + 1000000);
CHECK(up_1s > 0.99 && ms[0].dir[8] > 0.99999 && fabs(l_rate) < 0.001,
"levelling with drift: z up %.5f after 1 s, %.6f after 21 s, yaw %.5f rad/s", up_1s, (double)ms[0].dir[8],
l_rate);
printf("hid drift at rest: yaw %.5f rad/s off, %.5f on; after calibration %.5f, then %.5f; slow aim %.4f of 0.5 "
"rad; levelled z %.5f, yaw %.5f rad/s\n",
drift_rate[0], drift_rate[1], c_early, c_late, turned, up_1s, l_rate);
set_live("TOTK_GYRO_AUTO_BIAS", keep_auto);
set_live("TOTK_GYRO_CALIBRATION", keep_cal);
free(keep_auto);
free(keep_cal);
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);
}
+6
View File
@@ -380,6 +380,12 @@
# 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
# Gyro drift at rest (1 = on, off by default): whenever the controller lies still (on a table, not held) for half a second, the game
# learns the drift left in its gyro, after the calibration above if there is one, and subtracts it, as the Switch's
# hid service does for its own controllers. It catches a bias that changes as the controller warms up and works
# without a calibration. The log says "gyro drift at rest learned" once it has settled. A controller held in the hand
# is not learned from, and there is no dead zone, so slow aiming is untouched. Off, nothing is learned or subtracted.
# gyro_auto_bias = 0
# 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