mirror of
https://github.com/valentderah/totk-pc-recomp.git
synced 2026-11-21 21:05:56 +00:00
Merge task/sixaxis: drop six-axis samples from before StartSixAxisSensor (#104)
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
commit
be4d8f5245
2 files changed
+22
-5
No files matched your search
@@ -7,8 +7,9 @@
|
||||
// axis, direction = the controller's three axes in a reference frame whose z is up. The orientation is integrated from
|
||||
// the gyro and, while the controller is not being accelerated (|a| within 15 % of 1 G), pulled toward the measured
|
||||
// gravity (Mahony, gain 0.5/s: about 2 s), as the Switch's sensor fusion does; yaw has no reference and drifts with the
|
||||
// gyro's bias. The newest MOTION_RING samples are kept; reads are newest first. The feeding thread writes, guest
|
||||
// threads read, both under the instance's SRW lock.
|
||||
// gyro's bias. The newest MOTION_RING samples are kept; reads are newest first. A restart (StartSixAxisSensor) drops
|
||||
// the samples before it: reads return nothing until the next sample, which starts a fresh frame and angle; sampling
|
||||
// numbers go on. The feeding thread writes, guest threads read, both under the instance's SRW lock.
|
||||
#include <windows.h>
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
@@ -24,6 +25,7 @@ struct MotionFusion {
|
||||
SRWLOCK lock;
|
||||
MotionState ring[MOTION_RING];
|
||||
uint64_t produced, last_ns;
|
||||
uint64_t first; // produced at the last restart: older samples are not read
|
||||
double R[3][3]; // controller -> reference; row i = reference axis i in controller coordinates
|
||||
double angle[3];
|
||||
int restart;
|
||||
@@ -131,8 +133,8 @@ int motion_read(MotionFusion *m, MotionState *out, int max, uint64_t now) {
|
||||
if (max <= 0) return 0;
|
||||
int n = 0;
|
||||
AcquireSRWLockShared(&m->lock);
|
||||
if (m->produced && (now < m->last_ns || now - m->last_ns < STALE_NS)) {
|
||||
uint64_t have = m->produced < MOTION_RING ? m->produced : MOTION_RING;
|
||||
if (m->produced > m->first && (now < m->last_ns || now - m->last_ns < STALE_NS)) {
|
||||
uint64_t have = m->produced - m->first < MOTION_RING ? m->produced - m->first : MOTION_RING;
|
||||
n = (int)(have < (uint64_t)max ? have : (uint64_t)max);
|
||||
for (int i = 0; i < n; i++) out[i] = m->ring[(m->produced - 1 - (uint64_t)i) % MOTION_RING];
|
||||
}
|
||||
@@ -143,5 +145,8 @@ int motion_read(MotionFusion *m, MotionState *out, int max, uint64_t now) {
|
||||
void motion_restart(MotionFusion *m) {
|
||||
AcquireSRWLockExclusive(&m->lock);
|
||||
m->restart = 1;
|
||||
m->first = m->produced;
|
||||
m->last_ns = 0; // the first sample after the restart integrates nothing across the stop
|
||||
memset(m->angle, 0, sizeof m->angle);
|
||||
ReleaseSRWLockExclusive(&m->lock);
|
||||
}
|
||||
+13
-1
@@ -552,9 +552,21 @@ static void probe_feedback(uint32_t pid, uint32_t style) {
|
||||
near3(ms[0].dir + 3, -1, 0, 0, 0.01) && near3(ms[0].dir + 6, 0, 0, 1, 0.01),
|
||||
"fusion quarter turn: %d states, sampling %lld, angle %.4f rev, x axis (%.3f %.3f %.3f)", n,
|
||||
(long long)ms[0].sampling, (double)ms[0].angle[2], (double)ms[0].dir[0], (double)ms[0].dir[1], (double)ms[0].dir[2]);
|
||||
double quarter = ms[0].angle[2];
|
||||
CHECK(motion_read(m, ms, 16, t + 300000000ull) == 0, "fusion states 300 ms after the last sample");
|
||||
// StartSixAxisSensor again in the middle of the turn (the bow drawn again): the states from before it are gone, and
|
||||
// the first one after it starts a fresh angle in a fresh frame; sampling numbers go on.
|
||||
for (int i = 0; i < 250; i++) motion_feed(m, down, turn, t += 4000000);
|
||||
motion_restart(m);
|
||||
int same = motion_read(m, ms, 16, t + 1000000);
|
||||
motion_feed(m, down, turn, t += 4000000);
|
||||
int next = motion_read(m, ms, 16, t + 1000000);
|
||||
CHECK(same == 0 && next == 1 && ms[0].sampling == 502 && fabs(ms[0].angle[2]) < 0.002 &&
|
||||
near3(ms[0].dir, 1, 0, 0, 0.01),
|
||||
"fusion restart: %d states in the frame of the Start, %d after one sample (sampling %lld, angle %.4f rev)", same,
|
||||
next, (long long)ms[0].sampling, (double)ms[0].angle[2]);
|
||||
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], (double)ms[0].angle[2]);
|
||||
vh[0], vh[1], sh[0], quarter);
|
||||
}
|
||||
|
||||
static void probe_input(void) {
|
||||
|
||||
Reference in new issue
Block a user