v1.1.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Groups Pages
motion.h
1 /* -*- mode: C; c-basic-offset: 4; intent-tabs-mode: nil -*-
2  *
3  * This file is part of the public interface to the Sifteo SDK.
4  * Copyright <c> 2012 Sifteo, Inc. All rights reserved.
5  */
6 
7 #pragma once
8 #ifdef NOT_USERSPACE
9 # error This is a userspace-only header, not allowed by the current build.
10 #endif
11 
12 #include <sifteo/abi.h>
13 #include <sifteo/cube.h>
14 #include <sifteo/math.h>
15 
16 namespace Sifteo {
17 
60 template < unsigned tSize = 32 >
61 struct MotionBuffer {
62  struct SysType {
63  _SYSMotionBufferHeader header;
64  _SYSByte4 samples[tSize];
65  } sys;
66 
68  static const unsigned TICK_NS = _SYS_MOTION_TIMESTAMP_NS;
69 
71  static const unsigned TICK_US = _SYS_MOTION_TIMESTAMP_NS / 1000;
72 
74  static const unsigned TICK_HZ = _SYS_MOTION_TIMESTAMP_HZ;
75 
76  // Implicit conversions
77  operator _SYSMotionBuffer* () { return reinterpret_cast<_SYSMotionBuffer*>(&sys); }
78  operator const _SYSMotionBuffer* () const { return reinterpret_cast<const _SYSMotionBuffer*>(&sys); }
79 
100  void attach(_SYSCubeID id, unsigned hz=100)
101  {
102  STATIC_ASSERT(id < _SYS_NUM_CUBE_SLOTS);
103  STATIC_ASSERT(tSize <= _SYS_MOTION_MAX_ENTRIES);
104  bzero(sys);
105  sys.header.last = tSize - 1;
106  sys.header.rate = TICK_HZ / hz;
107 
108  /*
109  * If this ASSERT fails, the buffer is too close to the top of RAM. This
110  * usually means you've allocated the MotionBuffer on the stack, which isn't
111  * recommended. See the warning in the MotionBuffer class comments.
112  */
113  ASSERT(reinterpret_cast<uintptr_t>(this) + sizeof(_SYSMotionBuffer) <= 0x18000);
114 
115  _SYS_setMotionBuffer(id, *this);
116  }
117 
136  Int3 integrate(unsigned duration)
137  {
138  _SYSInt3 result;
139  _SYS_motion_integrate(*this, duration, &result);
140  return vec(result.x, result.y, result.z);
141  }
142 };
143 
156 private:
157  _SYSMotionBuffer *buffer;
158  uint32_t tickCounter;
159  Byte3 lastAccel;
160  uint8_t head;
161 
162 public:
164  static const unsigned TICK_NS = MotionBuffer<>::TICK_NS;
165 
167  static const unsigned TICK_US = MotionBuffer<>::TICK_US;
168 
170  static const unsigned TICK_HZ = MotionBuffer<>::TICK_HZ;
171 
180  MotionIterator(_SYSMotionBuffer *buffer)
181  : buffer(buffer), tickCounter(0), lastAccel(vec(0,0,0)),
182  head(buffer->header.tail) {}
183 
191  bool next()
192  {
193  unsigned tail = buffer->header.tail;
194  unsigned last = buffer->header.last;
195  unsigned head = this->head;
196 
197  if (head > last)
198  head = 0;
199 
200  if (head == tail)
201  return false;
202 
203  _SYSByte4 sample = buffer->samples[head];
204  this->head = head + 1;
205 
206  lastAccel = vec(sample.x, sample.y, sample.z);
207  tickCounter += unsigned(uint8_t(sample.w)) + 1;
208 
209  return true;
210  }
211 
218  Byte3 accel() const {
219  return lastAccel;
220  }
221 
232  uint32_t ticks() const {
233  return tickCounter;
234  }
235 
245  float seconds() const {
246  return ticks() * (1.0f / TICK_HZ);
247  }
248 
253  void setTicks(uint32_t value) {
254  tickCounter = value;
255  }
256 
267  void adjustTicks(int32_t value) {
268  tickCounter += value;
269  }
270 };
271 
281 public:
282  _SYSMotionMedian sys;
283 
284  // Implicit conversions
285  operator _SYSMotionMedian* () { return &sys; }
286  operator const _SYSMotionMedian* () const { return &sys; }
287 
307  void calculate(_SYSMotionBuffer *mbuf, unsigned duration) {
308  _SYS_motion_median(mbuf, duration, *this);
309  }
310 
313 
315  MotionMedian(_SYSMotionBuffer *mbuf, unsigned duration) {
316  calculate(mbuf, duration);
317  }
318 
320  Byte3 median() const {
321  return vec(sys.axes[0].median, sys.axes[1].median, sys.axes[2].median);
322  }
323 
325  Byte3 minimum() const {
326  return vec(sys.axes[0].minimum, sys.axes[1].minimum, sys.axes[2].minimum);
327  }
328 
330  Byte3 maximum() const {
331  return vec(sys.axes[0].maximum, sys.axes[1].maximum, sys.axes[2].maximum);
332  }
333 
335  Int3 range() const {
336  return Int3(maximum()) - Int3(minimum());
337  }
338 };
339 
340 
352 public:
353  static const int kFilterLatency = MotionBuffer<>::TICK_HZ / 30;
354  static const int kTiltThresholdMin = 15;
355  static const int kTiltThresholdMax = 26;
356  static const int kShakeThresholdMin = 1000;
357  static const int kShakeThresholdMax = 50000;
358 
368 
378 
386 
388  bool shake;
389 
396  void attach(_SYSCubeID id)
397  {
398  buffer.attach(id);
399  bzero(median);
400  bzero(tilt);
401  shake = false;
402  }
403 
411  Byte3 physicalTilt() const {
412  return tilt;
413  }
414 
425  Byte3 virtualTilt(Side orientation) const {
426  return tilt.zRotateI(orientation);
427  }
428 
432  enum ChangeFlags {
433  Shake_Begin = 1 << 0,
434  Shake_End = 1 << 1,
436 
437  Tilt_XNeg = 1 << 2,
438  Tilt_XZero = 1 << 3,
439  Tilt_XPos = 1 << 4,
441 
442  Tilt_YNeg = 1 << 5,
443  Tilt_YZero = 1 << 6,
444  Tilt_YPos = 1 << 7,
446 
447  Tilt_ZNeg = 1 << 8,
448  Tilt_ZZero = 1 << 9,
449  Tilt_ZPos = 1 << 10,
451 
453  };
454 
469  unsigned update(int latency = kFilterLatency)
470  {
471  unsigned changed = 0;
472 
473  median.calculate(buffer, latency);
474  auto m = median.median();
475  int wobble = median.range().len2();
476 
477  // Shake hysteresis
478  if (wobble >= kShakeThresholdMax) {
479  if (!shake) changed |= Shake_Begin;
480  shake = true;
481 
482  } else if (wobble < kShakeThresholdMin) {
483  if (shake) changed |= Shake_End;
484  shake = false;
485 
486  // Only update tilt state when wobble is low.
487  // Each tilt axis has hysteresis.
488 
489  if (m.x <= -kTiltThresholdMax) {
490  if (tilt.x != -1) changed |= Tilt_XNeg;
491  tilt.x = -1;
492  } else if (m.x >= kTiltThresholdMax) {
493  if (tilt.x != 1) changed |= Tilt_XPos;
494  tilt.x = 1;
495  } else if (abs(m.x) < kTiltThresholdMin) {
496  if (tilt.x != 0) changed |= Tilt_XZero;
497  tilt.x = 0;
498  }
499 
500  if (m.y <= -kTiltThresholdMax) {
501  if (tilt.y != -1) changed |= Tilt_YNeg;
502  tilt.y = -1;
503  } else if (m.y >= kTiltThresholdMax) {
504  if (tilt.y != 1) changed |= Tilt_YPos;
505  tilt.y = 1;
506  } else if (abs(m.y) < kTiltThresholdMin) {
507  if (tilt.y != 0) changed |= Tilt_YZero;
508  tilt.y = 0;
509  }
510 
511  if (m.z <= -kTiltThresholdMax) {
512  if (tilt.z != -1) changed |= Tilt_ZNeg;
513  tilt.z = -1;
514  } else if (m.z >= kTiltThresholdMax) {
515  if (tilt.z != 1) changed |= Tilt_ZPos;
516  tilt.z = 1;
517  } else if (abs(m.z) < kTiltThresholdMin) {
518  if (tilt.z != 0) changed |= Tilt_ZZero;
519  tilt.z = 0;
520  }
521  }
522 
523  return changed;
524  }
525 };
526 
527 
532 }; // namespace Sifteo