9 # error This is a userspace-only header, not allowed by the current build.
12 #include <sifteo/abi.h>
13 #include <sifteo/cube.h>
14 #include <sifteo/math.h>
60 template <
unsigned tSize = 32 >
63 _SYSMotionBufferHeader header;
64 _SYSByte4 samples[tSize];
68 static const unsigned TICK_NS = _SYS_MOTION_TIMESTAMP_NS;
71 static const unsigned TICK_US = _SYS_MOTION_TIMESTAMP_NS / 1000;
74 static const unsigned TICK_HZ = _SYS_MOTION_TIMESTAMP_HZ;
77 operator _SYSMotionBuffer* () {
return reinterpret_cast<_SYSMotionBuffer*
>(&sys); }
78 operator const _SYSMotionBuffer* ()
const {
return reinterpret_cast<const _SYSMotionBuffer*
>(&sys); }
100 void attach(_SYSCubeID
id,
unsigned hz=100)
105 sys.header.last = tSize - 1;
106 sys.header.rate =
TICK_HZ / hz;
113 ASSERT(reinterpret_cast<uintptr_t>(
this) +
sizeof(_SYSMotionBuffer) <= 0x18000);
115 _SYS_setMotionBuffer(
id, *
this);
139 _SYS_motion_integrate(*
this, duration, &result);
140 return vec(result.x, result.y, result.z);
157 _SYSMotionBuffer *buffer;
158 uint32_t tickCounter;
181 : buffer(buffer), tickCounter(0), lastAccel(
vec(0,0,0)),
182 head(buffer->header.tail) {}
193 unsigned tail = buffer->header.tail;
194 unsigned last = buffer->header.last;
195 unsigned head = this->head;
203 _SYSByte4 sample = buffer->samples[head];
204 this->head = head + 1;
206 lastAccel =
vec(sample.x, sample.y, sample.z);
207 tickCounter += unsigned(uint8_t(sample.w)) + 1;
268 tickCounter += value;
282 _SYSMotionMedian sys;
285 operator _SYSMotionMedian* () {
return &sys; }
286 operator const _SYSMotionMedian* ()
const {
return &sys; }
307 void calculate(_SYSMotionBuffer *mbuf,
unsigned duration) {
308 _SYS_motion_median(mbuf, duration, *
this);
321 return vec(sys.axes[0].median, sys.axes[1].median, sys.axes[2].median);
326 return vec(sys.axes[0].minimum, sys.axes[1].minimum, sys.axes[2].minimum);
331 return vec(sys.axes[0].maximum, sys.axes[1].maximum, sys.axes[2].maximum);
354 static const int kTiltThresholdMin = 15;
355 static const int kTiltThresholdMax = 26;
356 static const int kShakeThresholdMin = 1000;
357 static const int kShakeThresholdMax = 50000;
469 unsigned update(
int latency = kFilterLatency)
471 unsigned changed = 0;
478 if (wobble >= kShakeThresholdMax) {
482 }
else if (wobble < kShakeThresholdMin) {
489 if (m.x <= -kTiltThresholdMax) {
492 }
else if (m.x >= kTiltThresholdMax) {
495 }
else if (
abs(m.x) < kTiltThresholdMin) {
500 if (m.y <= -kTiltThresholdMax) {
503 }
else if (m.y >= kTiltThresholdMax) {
506 }
else if (
abs(m.y) < kTiltThresholdMin) {
511 if (m.z <= -kTiltThresholdMax) {
514 }
else if (m.z >= kTiltThresholdMax) {
517 }
else if (
abs(m.z) < kTiltThresholdMin) {