mirror of
https://github.com/valentderah/totk-pc-recomp.git
synced 2026-11-19 04:15:57 +00:00
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:
commit
c3ec7fce0e
7 files changed
+466
-47
No files matched your search
@@ -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
@@ -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
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in new issue
Block a user