diff --git a/inc/MicroBitAccelerometer.h b/inc/MicroBitAccelerometer.h index a9bc583..4d26cae 100644 --- a/inc/MicroBitAccelerometer.h +++ b/inc/MicroBitAccelerometer.h @@ -39,7 +39,42 @@ /* * Accelerometer events */ -#define MICROBIT_ACCELEROMETER_EVT_DATA_UPDATE 1 +#define MICROBIT_ACCELEROMETER_EVT_DATA_UPDATE 1 + +/* + * Gesture events + */ +#define MICROBIT_ACCELEROMETER_EVT_TILT_UP 1 +#define MICROBIT_ACCELEROMETER_EVT_TILT_DOWN 2 +#define MICROBIT_ACCELEROMETER_EVT_TILT_LEFT 3 +#define MICROBIT_ACCELEROMETER_EVT_TILT_RIGHT 4 +#define MICROBIT_ACCELEROMETER_EVT_FACE_UP 5 +#define MICROBIT_ACCELEROMETER_EVT_FACE_DOWN 6 +#define MICROBIT_ACCELEROMETER_EVT_FREEFALL 7 +#define MICROBIT_ACCELEROMETER_EVT_WHEEE 8 +#define MICROBIT_ACCELEROMETER_EVT_SICK 9 +#define MICROBIT_ACCELEROMETER_EVT_UNCONSCIOUS 10 +#define MICROBIT_ACCELEROMETER_EVT_SHAKE 11 + +/* + * Gesture recogniser constants + */ +#define MICROBIT_ACCELEROMETER_REST_TOLERANCE 200 +#define MICROBIT_ACCELEROMETER_TILT_TOLERANCE 200 +#define MICROBIT_ACCELEROMETER_FREEFALL_TOLERANCE 200 +#define MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE 1000 +#define MICROBIT_ACCELEROMETER_WHEEE_TOLERANCE 3000 +#define MICROBIT_ACCELEROMETER_SICK_TOLERANCE 5000 +#define MICROBIT_ACCELEROMETER_UNCONSCIOUS_TOLERANCE 8000 +#define MICROBIT_ACCELEROMETER_GESTURE_DAMPING 10 +#define MICROBIT_ACCELEROMETER_SHAKE_DAMPING 10 + +#define MICROBIT_ACCELEROMETER_REST_THRESHOLD (MICROBIT_ACCELEROMETER_REST_TOLERANCE * MICROBIT_ACCELEROMETER_REST_TOLERANCE) +#define MICROBIT_ACCELEROMETER_FREEFALL_THRESHOLD (MICROBIT_ACCELEROMETER_FREEFALL_TOLERANCE * MICROBIT_ACCELEROMETER_FREEFALL_TOLERANCE) +#define MICROBIT_ACCELEROMETER_WHEEE_THRESHOLD (MICROBIT_ACCELEROMETER_WHEEE_TOLERANCE * MICROBIT_ACCELEROMETER_WHEEE_TOLERANCE) +#define MICROBIT_ACCELEROMETER_SICK_THRESHOLD (MICROBIT_ACCELEROMETER_SICK_TOLERANCE * MICROBIT_ACCELEROMETER_SICK_TOLERANCE) +#define MICROBIT_ACCELEROMETER_UNCONSCIOUS_THRESHOLD (MICROBIT_ACCELEROMETER_UNCONSCIOUS_TOLERANCE * MICROBIT_ACCELEROMETER_UNCONSCIOUS_TOLERANCE) +#define MICROBIT_ACCELEROMETER_SHAKE_COUNT_THRESHOLD 4 struct MMA8653Sample { @@ -63,6 +98,32 @@ struct MMA8653SampleRangeConfig extern const MMA8653SampleRangeConfig MMA8653SampleRange[]; extern const MMA8653SampleRateConfig MMA8653SampleRate[]; +enum BasicGesture +{ + NONE, + UP, + DOWN, + LEFT, + RIGHT, + FACE_UP, + FACE_DOWN, + FREEFALL, + WHEEE, + SICK, + UNCONSCIOUS, + SHAKE +}; + +struct ShakeHistory +{ + uint16_t shaken:1, + x:1, + y:1, + z:1, + count:4, + timer:8; +}; + /** * Class definition for MicroBit Accelerometer. * @@ -76,11 +137,15 @@ class MicroBitAccelerometer : public MicroBitComponent * Used to track asynchronous events in the event bus. */ + MMA8653Sample sample; // The last sample read. + DigitalIn int1; // Data ready interrupt. uint16_t address; // I2C address of this accelerometer. uint16_t samplePeriod; // The time between samples, in milliseconds. uint8_t sampleRange; // The sample range of the accelerometer in g. - MMA8653Sample sample; // The last sample read. - DigitalIn int1; // Data ready interrupt. + uint8_t sigma; // the number of ticks that the instantaneous gesture has been stable. + BasicGesture gesture; // the current, filtered orientation of the device. + BasicGesture iGesture; // the last instantaneous orientation recorded. + ShakeHistory shake; // State information needed to detect shake events. public: @@ -192,6 +257,17 @@ class MicroBitAccelerometer : public MicroBitComponent */ int getZ(); + /** + * Reads the last recorded gesture detected. + * @return The last gesture detected. + * + * Example: + * @code + * if (uBit.accelerometer.getGesture() == SHAKE) + * @endcode + */ + BasicGesture getGesture(); + /** * periodic callback from MicroBit idle thread. * Check if any data is ready for reading by checking the interrupt flag on the accelerometer @@ -224,6 +300,27 @@ class MicroBitAccelerometer : public MicroBitComponent * @return MICROBIT_OK on success, MICROBIT_INVALID_PARAMETER or MICROBIT_I2C_ERROR if the the read request failed. */ int readCommand(uint8_t reg, uint8_t* buffer, int length); + + /** + * Updates the basic gesture recognizer. This performs instantaneous pose recognition, and also some low pass filtering to promote + * stability. + */ + void updateGesture(); + + /** + * Service function. Calculates the current scalar acceleration of the device (x^2 + y^2 + z^2). + * It does not, however, square root the result, as this is a relatively high cost operation. + * This is left to application code should it be needed. + * @return the sum of the square of the acceleration of the device across all axes. + */ + int instantaneousAcceleration2(); + + /** + * Service function. Determines the best guess posture of the device based on instantaneous data. + * This makes no use of historic data, and forms this input to th filter implemented in updateGesture(). + * @return A best guess of the curret posture of the device, based on instanataneous data. + */ + BasicGesture instantaneousPosture(); }; #endif diff --git a/inc/MicroBitComponent.h b/inc/MicroBitComponent.h index 3278498..c6928b4 100644 --- a/inc/MicroBitComponent.h +++ b/inc/MicroBitComponent.h @@ -42,6 +42,7 @@ #define MICROBIT_ID_IO_P20 25 //SDA #define MICROBIT_ID_BUTTON_AB 26 // Button A+B multibutton +#define MICROBIT_ID_GESTURE 27 // Gesture events #define MICROBIT_ID_NOTIFY 1023 // Notfication channel, for general purpose synchronisation #define MICROBIT_ID_NOTIFY_ONE 1022 // Notfication channel, for general purpose synchronisation diff --git a/source/MicroBitAccelerometer.cpp b/source/MicroBitAccelerometer.cpp index d4ba4db..55cb10a 100644 --- a/source/MicroBitAccelerometer.cpp +++ b/source/MicroBitAccelerometer.cpp @@ -144,6 +144,16 @@ MicroBitAccelerometer::MicroBitAccelerometer(uint16_t id, uint16_t address) : sa this->samplePeriod = 20; this->sampleRange = 2; + // Initialise gesture history + this->sigma = 0; + this->gesture = NONE; + this->iGesture = NONE; + this->shake.x = 0; + this->shake.y = 0; + this->shake.z = 0; + this->shake.count = 0; + this->shake.timer = 0; + // Configure and enable the accelerometer. if (this->configure() == MICROBIT_OK) uBit.flags |= MICROBIT_FLAG_ACCELEROMETER_RUNNING; @@ -211,12 +221,136 @@ int MicroBitAccelerometer::update() sample.y *= this->sampleRange; sample.z *= this->sampleRange; + // Update gesture tracking + updateGesture(); + // Indicate that a new sample is available MicroBitEvent e(id, MICROBIT_ACCELEROMETER_EVT_DATA_UPDATE); return MICROBIT_OK; }; +/** + * Service function. Calculates the current scalar acceleration of the device (x^2 + y^2 + z^2). + * It does not, however, square root the result, as this is a relatively high cost operation. + * This is left to application code should it be needed. + * + * @return the sum of the square of the acceleration of the device across all axes. + */ +int +MicroBitAccelerometer::instantaneousAcceleration2() +{ + // Use pythagoras theorem to determine the combined force acting on the device. + return (int)sample.x*(int)sample.x + (int)sample.y*(int)sample.y + (int)sample.z*(int)sample.z; +} + +/** + * Service function. Determines the best guess posture of the device based on instantaneous data. + * This makes no use of historic data (except for shake), and forms this input to the filter implemented in updateGesture(). + * + * @return A best guess of the current posture of the device, based on instantaneous data. + */ +BasicGesture +MicroBitAccelerometer::instantaneousPosture() +{ + int force = instantaneousAcceleration2(); + bool shakeDetected = false; + + // Test for shake events. + if ((sample.x < -MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && shake.x) || (sample.x > MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && !shake.x)) + { + shakeDetected = true; + shake.x = !shake.x; + } + + if ((sample.y < -MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && shake.y) || (sample.y > MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && !shake.y)) + { + shakeDetected = true; + shake.y = !shake.y; + } + + if ((sample.z < -MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && shake.z) || (sample.z > MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && !shake.z)) + { + shakeDetected = true; + shake.z = !shake.z; + } + + if (shakeDetected && shake.count < MICROBIT_ACCELEROMETER_SHAKE_COUNT_THRESHOLD && ++shake.count == MICROBIT_ACCELEROMETER_SHAKE_COUNT_THRESHOLD) + shake.shaken = 1; + + if (++shake.timer >= MICROBIT_ACCELEROMETER_SHAKE_DAMPING) + { + shake.timer = 0; + if (shake.count > 0) + { + if(--shake.count == 0) + shake.shaken = 0; + } + } + + if (shake.shaken) + return SHAKE; + + if (force < MICROBIT_ACCELEROMETER_FREEFALL_THRESHOLD) + return FREEFALL; + + if (force > MICROBIT_ACCELEROMETER_UNCONSCIOUS_THRESHOLD) + return UNCONSCIOUS; + + if (force > MICROBIT_ACCELEROMETER_SICK_THRESHOLD) + return SICK; + + if (force > MICROBIT_ACCELEROMETER_WHEEE_THRESHOLD) + return WHEEE; + + // Determine our posture. + if (sample.x < (-1000 + MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return LEFT; + + if (sample.x > (1000 - MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return RIGHT; + + if (sample.y < (-1000 + MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return DOWN; + + if (sample.y > (1000 - MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return UP; + + if (sample.z < (-1000 + MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return FACE_UP; + + if (sample.z > (1000 - MICROBIT_ACCELEROMETER_TILT_TOLERANCE)) + return FACE_DOWN; + + return NONE; +} + +void +MicroBitAccelerometer::updateGesture() +{ + // Determine what it looks like we're doing based on the latest sample... + BasicGesture g = instantaneousPosture(); + + // Perform some low pass filtering to remove hysteresis (data flapping) effects + if (g == iGesture) + { + if (sigma < MICROBIT_ACCELEROMETER_GESTURE_DAMPING) + sigma++; + } + else + { + iGesture = g; + sigma = 0; + } + + // If we've reached threshold, update our record and raise the relevant event... + if (iGesture != gesture && sigma >= MICROBIT_ACCELEROMETER_GESTURE_DAMPING) + { + gesture = iGesture; + MicroBitEvent e(MICROBIT_ID_GESTURE, gesture); + } +} + /** * Attempts to set the sample rate of the accelerometer to the specified value (in ms). * n.b. the requested rate may not be possible on the hardware. In this case, the @@ -306,6 +440,19 @@ int MicroBitAccelerometer::getZ() return sample.z; } +/** + * Reads the last recorded gesture detected. + * @return The last gesture detected. + * + * Example: + * @code + * if (uBit.accelerometer.getGesture() == SHAKE) + * @endcode + */ +BasicGesture MicroBitAccelerometer::getGesture() +{ + return gesture; +} /** * periodic callback from MicroBit clock.