Diff: source/drivers/MicroBitAccelerometer.cpp

Revision:

1:8aa5cdb4ab67

Child:

6:2e1c2e0d8c7a

diff -r fb15f7887843 -r 8aa5cdb4ab67 source/drivers/MicroBitAccelerometer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/drivers/MicroBitAccelerometer.cpp	Thu Apr 07 01:33:22 2016 +0100
@@ -0,0 +1,710 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2016 British Broadcasting Corporation.
+This software is provided by Lancaster University by arrangement with the BBC.
+
+Permission is hereby granted, free of charge, to any person obtaining a
+copy of this software and associated documentation files (the "Software"),
+to deal in the Software without restriction, including without limitation
+the rights to use, copy, modify, merge, publish, distribute, sublicense,
+and/or sell copies of the Software, and to permit persons to whom the
+Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+*/
+
+/**
+ * Class definition for MicroBit Accelerometer.
+ *
+ * Represents an implementation of the Freescale MMA8653 3 axis accelerometer
+ * Also includes basic data caching and on demand activation.
+ */
+#include "MicroBitConfig.h"
+#include "MicroBitAccelerometer.h"
+#include "ErrorNo.h"
+#include "MicroBitConfig.h"
+#include "MicroBitEvent.h"
+#include "MicroBitCompat.h"
+#include "MicroBitFiber.h"
+
+/**
+  * Configures the accelerometer for G range and sample rate defined
+  * in this object. The nearest values are chosen to those defined
+  * that are supported by the hardware. The instance variables are then
+  * updated to reflect reality.
+  *
+  * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR if the accelerometer could not be configured.
+  */
+int MicroBitAccelerometer::configure()
+{
+    const MMA8653SampleRangeConfig  *actualSampleRange;
+    const MMA8653SampleRateConfig  *actualSampleRate;
+    int result;
+
+    // First find the nearest sample rate to that specified.
+    actualSampleRate = &MMA8653SampleRate[MMA8653_SAMPLE_RATES-1];
+    for (int i=MMA8653_SAMPLE_RATES-1; i>=0; i--)
+    {
+        if(MMA8653SampleRate[i].sample_period < this->samplePeriod * 1000)
+            break;
+
+        actualSampleRate = &MMA8653SampleRate[i];
+    }
+
+    // Now find the nearest sample range to that specified.
+    actualSampleRange = &MMA8653SampleRange[MMA8653_SAMPLE_RANGES-1];
+    for (int i=MMA8653_SAMPLE_RANGES-1; i>=0; i--)
+    {
+        if(MMA8653SampleRange[i].sample_range < this->sampleRange)
+            break;
+
+        actualSampleRange = &MMA8653SampleRange[i];
+    }
+
+    // OK, we have the correct data. Update our local state.
+    this->samplePeriod = actualSampleRate->sample_period / 1000;
+    this->sampleRange = actualSampleRange->sample_range;
+
+    // Now configure the accelerometer accordingly.
+    // First place the device into standby mode, so it can be configured.
+    result = writeCommand(MMA8653_CTRL_REG1, 0x00);
+    if (result != 0)
+        return MICROBIT_I2C_ERROR;
+
+    // Enable high precisiosn mode. This consumes a bit more power, but still only 184 uA!
+    result = writeCommand(MMA8653_CTRL_REG2, 0x10);
+    if (result != 0)
+        return MICROBIT_I2C_ERROR;
+
+    // Enable the INT1 interrupt pin.
+    result = writeCommand(MMA8653_CTRL_REG4, 0x01);
+    if (result != 0)
+        return MICROBIT_I2C_ERROR;
+
+    // Select the DATA_READY event source to be routed to INT1
+    result = writeCommand(MMA8653_CTRL_REG5, 0x01);
+    if (result != 0)
+        return MICROBIT_I2C_ERROR;
+
+    // Configure for the selected g range.
+    result = writeCommand(MMA8653_XYZ_DATA_CFG, actualSampleRange->xyz_data_cfg);
+    if (result != 0)
+        return MICROBIT_I2C_ERROR;
+
+    // Bring the device back online, with 10bit wide samples at the requested frequency.
+    result = writeCommand(MMA8653_CTRL_REG1, actualSampleRate->ctrl_reg1 | 0x01);
+    if (result != 0)
+        return MICROBIT_I2C_ERROR;
+
+    return MICROBIT_OK;
+}
+
+/**
+  * Issues a standard, 2 byte I2C command write to the accelerometer.
+  *
+  * Blocks the calling thread until complete.
+  *
+  * @param reg The address of the register to write to.
+  *
+  * @param value The value to write.
+  *
+  * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR if the the write request failed.
+  */
+int MicroBitAccelerometer::writeCommand(uint8_t reg, uint8_t value)
+{
+    uint8_t command[2];
+    command[0] = reg;
+    command[1] = value;
+
+    return i2c.write(address, (const char *)command, 2);
+}
+
+/**
+  * Issues a read command, copying data into the specified buffer.
+  *
+  * Blocks the calling thread until complete.
+  *
+  * @param reg The address of the register to access.
+  *
+  * @param buffer Memory area to read the data into.
+  *
+  * @param length The number of bytes to read.
+  *
+  * @return MICROBIT_OK on success, MICROBIT_INVALID_PARAMETER or MICROBIT_I2C_ERROR if the the read request failed.
+  */
+int MicroBitAccelerometer::readCommand(uint8_t reg, uint8_t* buffer, int length)
+{
+    int result;
+
+    if (buffer == NULL || length <= 0 )
+        return MICROBIT_INVALID_PARAMETER;
+
+    result = i2c.write(address, (const char *)&reg, 1, true);
+    if (result !=0)
+        return MICROBIT_I2C_ERROR;
+
+    result = i2c.read(address, (char *)buffer, length);
+    if (result !=0)
+        return MICROBIT_I2C_ERROR;
+
+    return MICROBIT_OK;
+}
+
+/**
+  * Constructor.
+  * Create a software abstraction of an accelerometer.
+  *
+  * @param _i2c an instance of MicroBitI2C used to communicate with the onboard accelerometer.
+  *
+  * @param address the default I2C address of the accelerometer. Defaults to: MMA8653_DEFAULT_ADDR.
+  *
+  * @param id the unique EventModel id of this component. Defaults to: MICROBIT_ID_ACCELEROMETER
+  *
+  * @code
+  * MicroBitI2C i2c = MicroBitI2C(I2C_SDA0, I2C_SCL0);
+  *
+  * MicroBitAccelerometer accelerometer = MicroBitAccelerometer(i2c);
+  * @endcode
+ */
+MicroBitAccelerometer::MicroBitAccelerometer(MicroBitI2C& _i2c, uint16_t address, uint16_t id) : sample(), int1(MICROBIT_PIN_ACCEL_DATA_READY), i2c(_i2c)
+{
+    // Store our identifiers.
+    this->id = id;
+    this->status = 0;
+    this->address = address;
+
+    // Update our internal state for 50Hz at +/- 2g (50Hz has a period af 20ms).
+    this->samplePeriod = 20;
+    this->sampleRange = 2;
+
+    // Initialise gesture history
+    this->sigma = 0;
+    this->lastGesture = GESTURE_NONE;
+    this->currentGesture = GESTURE_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)
+        status |= MICROBIT_COMPONENT_RUNNING;
+}
+
+/**
+  * Attempts to read the 8 bit ID from the accelerometer, this can be used for
+  * validation purposes.
+  *
+  * @return the 8 bit ID returned by the accelerometer, or MICROBIT_I2C_ERROR if the request fails.
+  *
+  * @code
+  * accelerometer.whoAmI();
+  * @endcode
+  */
+int MicroBitAccelerometer::whoAmI()
+{
+    uint8_t data;
+    int result;
+
+    result = readCommand(MMA8653_WHOAMI, &data, 1);
+    if (result !=0)
+        return MICROBIT_I2C_ERROR;
+
+    return (int)data;
+}
+
+/**
+  * Reads the acceleration data from the accelerometer, and stores it in our buffer.
+  * This only happens if the accelerometer indicates that it has new data via int1.
+  *
+  * On first use, this member function will attempt to add this component to the
+  * list of fiber components in order to constantly update the values stored
+  * by this object.
+  *
+  * This technique is called lazy instantiation, and it means that we do not
+  * obtain the overhead from non-chalantly adding this component to fiber components.
+  *
+  * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR if the read request fails.
+  */
+int MicroBitAccelerometer::updateSample()
+{
+    if(!(status & MICROBIT_ACCEL_ADDED_TO_IDLE))
+    {
+        fiber_add_idle_component(this);
+        status |= MICROBIT_ACCEL_ADDED_TO_IDLE;
+    }
+
+    // Poll interrupt line from accelerometer.
+    // n.b. Default is Active LO. Interrupt is cleared in data read.
+    if(!int1)
+    {
+        int8_t data[6];
+        int result;
+
+        result = readCommand(MMA8653_OUT_X_MSB, (uint8_t *)data, 6);
+        if (result !=0)
+            return MICROBIT_I2C_ERROR;
+
+        // read MSB values...
+        sample.x = data[0];
+        sample.y = data[2];
+        sample.z = data[4];
+
+        // Normalize the data in the 0..1024 range.
+        sample.x *= 8;
+        sample.y *= 8;
+        sample.z *= 8;
+
+#if CONFIG_ENABLED(USE_ACCEL_LSB)
+        // Add in LSB values.
+        sample.x += (data[1] / 64);
+        sample.y += (data[3] / 64);
+        sample.z += (data[5] / 64);
+#endif
+
+        // Scale into millig (approx!)
+        sample.x *= this->sampleRange;
+        sample.y *= this->sampleRange;
+        sample.z *= this->sampleRange;
+
+        // Indicate that pitch and roll data is now stale, and needs to be recalculated if needed.
+        status &= ~MICROBIT_ACCEL_PITCH_ROLL_VALID;
+
+        // Update gesture tracking
+        updateGesture();
+
+        // Indicate that a new sample is available
+        MicroBitEvent e(id, MICROBIT_ACCELEROMETER_EVT_DATA_UPDATE);
+    }
+
+    return MICROBIT_OK;
+};
+
+/**
+  * A service function.
+  * It 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::instantaneousAccelerationSquared()
+{
+    updateSample();
+
+    // 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 a 'best guess' posture of the device based on instantaneous data.
+ *
+ * This makes no use of historic data, and forms this input to the filter implemented in updateGesture().
+ *
+ * @return A 'best guess' of the current posture of the device, based on instanataneous data.
+ */
+BasicGesture MicroBitAccelerometer::instantaneousPosture()
+{
+    int force = instantaneousAccelerationSquared();
+    bool shakeDetected = false;
+
+    // Test for shake events.
+    // We detect a shake by measuring zero crossings in each axis. In other words, if we see a strong acceleration to the left followed by
+    // a string acceleration to the right, then we can infer a shake. Similarly, we can do this for each acxis (left/right, up/down, in/out).
+    //
+    // If we see enough zero crossings in succession (MICROBIT_ACCELEROMETER_SHAKE_COUNT_THRESHOLD), then we decide that the device
+    // has been shaken.
+    if ((getX() < -MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && shake.x) || (getX() > MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && !shake.x))
+    {
+        shakeDetected = true;
+        shake.x = !shake.x;
+    }
+
+    if ((getY() < -MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && shake.y) || (getY() > MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && !shake.y))
+    {
+        shakeDetected = true;
+        shake.y = !shake.y;
+    }
+
+    if ((getZ() < -MICROBIT_ACCELEROMETER_SHAKE_TOLERANCE && shake.z) || (getZ() > 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 GESTURE_SHAKE;
+
+    if (force < MICROBIT_ACCELEROMETER_FREEFALL_THRESHOLD)
+        return GESTURE_FREEFALL;
+
+    if (force > MICROBIT_ACCELEROMETER_3G_THRESHOLD)
+        return GESTURE_3G;
+
+    if (force > MICROBIT_ACCELEROMETER_6G_THRESHOLD)
+        return GESTURE_6G;
+
+    if (force > MICROBIT_ACCELEROMETER_8G_THRESHOLD)
+        return GESTURE_8G;
+
+    // Determine our posture.
+    if (getX() < (-1000 + MICROBIT_ACCELEROMETER_TILT_TOLERANCE))
+        return GESTURE_LEFT;
+
+    if (getX() > (1000 - MICROBIT_ACCELEROMETER_TILT_TOLERANCE))
+        return GESTURE_RIGHT;
+
+    if (getY() < (-1000 + MICROBIT_ACCELEROMETER_TILT_TOLERANCE))
+        return GESTURE_DOWN;
+
+    if (getY() > (1000 - MICROBIT_ACCELEROMETER_TILT_TOLERANCE))
+        return GESTURE_UP;
+
+    if (getZ() < (-1000 + MICROBIT_ACCELEROMETER_TILT_TOLERANCE))
+        return GESTURE_FACE_UP;
+
+    if (getZ() > (1000 - MICROBIT_ACCELEROMETER_TILT_TOLERANCE))
+        return GESTURE_FACE_DOWN;
+
+    return GESTURE_NONE;
+}
+
+/**
+  * Updates the basic gesture recognizer. This performs instantaneous pose recognition, and also some low pass filtering to promote
+  * stability.
+  */
+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 reduce jitter from any detected effects
+    if (g == currentGesture)
+    {
+        if (sigma < MICROBIT_ACCELEROMETER_GESTURE_DAMPING)
+            sigma++;
+    }
+    else
+    {
+        currentGesture = g;
+        sigma = 0;
+    }
+
+    // If we've reached threshold, update our record and raise the relevant event...
+    if (currentGesture != lastGesture && sigma >= MICROBIT_ACCELEROMETER_GESTURE_DAMPING)
+    {
+        lastGesture = currentGesture;
+        MicroBitEvent e(MICROBIT_ID_GESTURE, lastGesture);
+    }
+}
+
+/**
+  * Attempts to set the sample rate of the accelerometer to the specified value (in ms).
+  *
+  * @param period the requested time between samples, in milliseconds.
+  *
+  * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR is the request fails.
+  *
+  * @code
+  * // sample rate is now 20 ms.
+  * accelerometer.setPeriod(20);
+  * @endcode
+  *
+  * @note The requested rate may not be possible on the hardware. In this case, the
+  * nearest lower rate is chosen.
+  */
+int MicroBitAccelerometer::setPeriod(int period)
+{
+    this->samplePeriod = period;
+    return this->configure();
+}
+
+/**
+  * Reads the currently configured sample rate of the accelerometer.
+  *
+  * @return The time between samples, in milliseconds.
+  */
+int MicroBitAccelerometer::getPeriod()
+{
+    return (int)samplePeriod;
+}
+
+/**
+  * Attempts to set the sample range of the accelerometer to the specified value (in g).
+  *
+  * @param range The requested sample range of samples, in g.
+  *
+  * @return MICROBIT_OK on success, MICROBIT_I2C_ERROR is the request fails.
+  *
+  * @code
+  * // the sample range of the accelerometer is now 8G.
+  * accelerometer.setRange(8);
+  * @endcode
+  *
+  * @note The requested range may not be possible on the hardware. In this case, the
+  * nearest lower range is chosen.
+  */
+int MicroBitAccelerometer::setRange(int range)
+{
+    this->sampleRange = range;
+    return this->configure();
+}
+
+/**
+  * Reads the currently configured sample range of the accelerometer.
+  *
+  * @return The sample range, in g.
+  */
+int MicroBitAccelerometer::getRange()
+{
+    return (int)sampleRange;
+}
+
+/**
+  * Reads the value of the X axis from the latest update retrieved from the accelerometer.
+  *
+  * @param system The coordinate system to use. By default, a simple cartesian system is provided.
+  *
+  * @return The force measured in the X axis, in milli-g.
+  *
+  * @code
+  * accelerometer.getX();
+  * @endcode
+  */
+int MicroBitAccelerometer::getX(MicroBitCoordinateSystem system)
+{
+    updateSample();
+
+    switch (system)
+    {
+        case SIMPLE_CARTESIAN:
+            return -sample.x;
+
+        case NORTH_EAST_DOWN:
+            return sample.y;
+
+        case RAW:
+        default:
+            return sample.x;
+    }
+}
+
+/**
+  * Reads the value of the Y axis from the latest update retrieved from the accelerometer.
+  *
+  * @return The force measured in the Y axis, in milli-g.
+  *
+  * @code
+  * accelerometer.getY();
+  * @endcode
+  */
+int MicroBitAccelerometer::getY(MicroBitCoordinateSystem system)
+{
+    updateSample();
+
+    switch (system)
+    {
+        case SIMPLE_CARTESIAN:
+            return -sample.y;
+
+        case NORTH_EAST_DOWN:
+            return -sample.x;
+
+        case RAW:
+        default:
+            return sample.y;
+    }
+}
+
+/**
+  * Reads the value of the Z axis from the latest update retrieved from the accelerometer.
+  *
+  * @return The force measured in the Z axis, in milli-g.
+  *
+  * @code
+  * accelerometer.getZ();
+  * @endcode
+  */
+int MicroBitAccelerometer::getZ(MicroBitCoordinateSystem system)
+{
+    updateSample();
+
+    switch (system)
+    {
+        case NORTH_EAST_DOWN:
+            return -sample.z;
+
+        case SIMPLE_CARTESIAN:
+        case RAW:
+        default:
+            return sample.z;
+    }
+}
+
+/**
+  * Provides a rotation compensated pitch of the device, based on the latest update retrieved from the accelerometer.
+  *
+  * @return The pitch of the device, in degrees.
+  *
+  * @code
+  * accelerometer.getPitch();
+  * @endcode
+  */
+int MicroBitAccelerometer::getPitch()
+{
+    return (int) ((360*getPitchRadians()) / (2*PI));
+}
+
+/**
+  * Provides a rotation compensated pitch of the device, based on the latest update retrieved from the accelerometer.
+  *
+  * @return The pitch of the device, in radians.
+  *
+  * @code
+  * accelerometer.getPitchRadians();
+  * @endcode
+  */
+float MicroBitAccelerometer::getPitchRadians()
+{
+    if (!(status & MICROBIT_ACCEL_PITCH_ROLL_VALID))
+        recalculatePitchRoll();
+
+    return pitch;
+}
+
+/**
+  * Provides a rotation compensated roll of the device, based on the latest update retrieved from the accelerometer.
+  *
+  * @return The roll of the device, in degrees.
+  *
+  * @code
+  * accelerometer.getRoll();
+  * @endcode
+  */
+int MicroBitAccelerometer::getRoll()
+{
+    return (int) ((360*getRollRadians()) / (2*PI));
+}
+
+/**
+  * Provides a rotation compensated roll of the device, based on the latest update retrieved from the accelerometer.
+  *
+  * @return The roll of the device, in radians.
+  *
+  * @code
+  * accelerometer.getRollRadians();
+  * @endcode
+  */
+float MicroBitAccelerometer::getRollRadians()
+{
+    if (!(status & MICROBIT_ACCEL_PITCH_ROLL_VALID))
+        recalculatePitchRoll();
+
+    return roll;
+}
+
+/**
+  * Recalculate roll and pitch values for the current sample.
+  *
+  * @note We only do this at most once per sample, as the necessary trigonemteric functions are rather
+  *       heavyweight for a CPU without a floating point unit.
+  */
+void MicroBitAccelerometer::recalculatePitchRoll()
+{
+    float x = (float) getX(NORTH_EAST_DOWN);
+    float y = (float) getY(NORTH_EAST_DOWN);
+    float z = (float) getZ(NORTH_EAST_DOWN);
+
+    roll = atan2(getY(NORTH_EAST_DOWN), getZ(NORTH_EAST_DOWN));
+    pitch = atan(-x / (y*sin(roll) + z*cos(roll)));
+    status |= MICROBIT_ACCEL_PITCH_ROLL_VALID;
+}
+
+/**
+  * Retrieves the last recorded gesture.
+  *
+  * @return The last gesture that was detected.
+  *
+  * Example:
+  * @code
+  * MicroBitDisplay display;
+  *
+  * if (accelerometer.getGesture() == SHAKE)
+  *     display.scroll("SHAKE!");
+  * @endcode
+  */
+BasicGesture MicroBitAccelerometer::getGesture()
+{
+    return lastGesture;
+}
+
+/**
+  * A periodic callback invoked by the fiber scheduler idle thread.
+  *
+  * Internally calls updateSample().
+  */
+void MicroBitAccelerometer::idleTick()
+{
+    updateSample();
+}
+
+/**
+  * Returns 0 or 1. 1 indicates data is waiting to be read, zero means data is not ready to be read.
+  *
+  * We check if any data is ready for reading by checking the interrupt flag on the accelerometer.
+  */
+int MicroBitAccelerometer::isIdleCallbackNeeded()
+{
+    return !int1;
+}
+
+/**
+  * Destructor for MicroBitAccelerometer, where we deregister from the array of fiber components.
+  */
+MicroBitAccelerometer::~MicroBitAccelerometer()
+{
+    fiber_remove_idle_component(this);
+}
+
+const MMA8653SampleRangeConfig MMA8653SampleRange[MMA8653_SAMPLE_RANGES] = {
+    {2, 0},
+    {4, 1},
+    {8, 2}
+};
+
+const MMA8653SampleRateConfig MMA8653SampleRate[MMA8653_SAMPLE_RATES] = {
+    {1250,      0x00},
+    {2500,      0x08},
+    {5000,      0x10},
+    {10000,     0x18},
+    {20000,     0x20},
+    {80000,     0x28},
+    {160000,    0x30},
+    {640000,    0x38}
+};