From 7d89fde2f0fd1cde07285d08b303aeb3c8cfe9a2 Mon Sep 17 00:00:00 2001 From: Valiantsin Dzerakh Date: Sat, 31 Oct 2026 00:22:08 +0300 Subject: [PATCH] Drop the six-axis samples from before StartSixAxisSensor motion_restart only flagged the next sample to start a fresh frame, so a read in the frame of the Start returned the previous session's 16 states as live, and for the next 60 ms 15 of the 16 were from before it, with the angle jumping back to zero between two reads. Motion aim was dead when the bow was first drawn and lagged after that. A restart now hides the older samples and zeroes the angle; sampling numbers keep counting. --probe-services checks a restart in the middle of a turn: 16 old states in the Start frame before, none after. Reported with the cause and a harness in #104. Co-Authored-By: Claude Opus 5.5 --- runtime/motion_fusion.c | 13 +++++++++---- runtime/ov_probe.c | 14 +++++++++++++- 2 files changed, 22 insertions(+), 5 deletions(-) diff --git a/runtime/motion_fusion.c b/runtime/motion_fusion.c index 0b4c56b7..76df5064 100644 --- a/runtime/motion_fusion.c +++ b/runtime/motion_fusion.c @@ -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 #include #include @@ -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); } diff --git a/runtime/ov_probe.c b/runtime/ov_probe.c index 4668bf0a..c0ffa181 100644 --- a/runtime/ov_probe.c +++ b/runtime/ov_probe.c @@ -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) {