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) {