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:
Valiantsin DzerakhandClaude Opus 5.5 committed 2026-10-31 00:26:39 +03:00
commit be4d8f5245
2 files changed
+22 -5

No files matched your search

+9 -4
View File
@@ -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
View File
@@ -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) {