Use single dataReady() method, instead of accelDataReady/gyroDataReady

eriknyquist · eriknyquist · commit ee08a03b21d5 · 2017-02-22T11:12:51.000-08:00
This method is a bit more flexible. There is a version with no
parameters, which will return true if all enabled sensors have new
data. There is one overload, that takes a single 'flags' parameter,
which returns true if all enabled *and* selected sensors have new data.
Sensors are selectable through bit flags. For example;

    CurieIMU.dataReady() : this will return true if both the accel and gyro
    are enabled, and both have new data ready.

    CurieIMU.dataReady(ACCEL | GYRO) : this will return true if both the
    accel and gyro are enabled, and both have new data ready.

    CurieIMU.dataReady(GYRO) : this will return true if the gyro is
    enabled and has new data ready.

    CurieIMU.dataReady(ACCEL) : this will return true if the accel is
    enabled and has new data ready.

This also involved changing the 'begin' method to allow selective
enabling of sensors, e.g.

    CurieIMU.begin() : same as it always has, initializes IMU, enabling
    both accel and gyro

    CurieIMU.begin(ACCEL | GYRO) : initializes IMU, enabling both accel
    and gyro

    CurieIMU.begin(ACCEL) : initializes IMU, enabling accel only
diff --git a/libraries/CurieIMU/keywords.txt b/libraries/CurieIMU/keywords.txt
@@ -14,6 +14,7 @@ CurieIMUClass	KEYWORD1
 
 begin	KEYWORD1
 
+dataReady   KEYWORD1
 getGyroRate	KEYWORD1
 setGyroRate	KEYWORD1
 getAccelerometerRate	KEYWORD1
@@ -84,6 +85,9 @@ CurieIMU	KEYWORD2
 # Constants (LITERAL1)
 #######################################
 
+ACCEL   LITERAL1
+GYRO    LITERAL1
+
 X_AXIS	LITERAL1
 Y_AXIS	LITERAL1
 Z_AXIS	LITERAL1
diff --git a/libraries/CurieIMU/src/BMI160.cpp b/libraries/CurieIMU/src/BMI160.cpp
@@ -78,15 +78,22 @@ uint8_t BMI160Class::reg_read_bits(uint8_t reg, unsigned pos, unsigned len)
     return b;
 }
 
+int BMI160Class::isBitSet(uint8_t value, unsigned bit)
+{
+    return value & (1 << bit);
+}
+
 /******************************************************************************/
 
 /** Power on and prepare for general usage.
  * This will activate the device and take it out of sleep mode (which must be done
  * after start-up). This function also sets both the accelerometer and the gyroscope
  * to default range settings, namely +/- 2g and +/- 250 degrees/sec.
  */
-void BMI160Class::initialize()
+void BMI160Class::initialize(unsigned int flags)
 {
+    sensors_enabled = 0;
+
     /* Issue a soft-reset to bring the device into a clean state */
     reg_write(BMI160_RA_CMD, BMI160_CMD_SOFT_RESET);
     delay(1);
@@ -95,26 +102,36 @@ void BMI160Class::initialize()
     reg_read(0x7F);
     delay(1);
 
-    /* Power up the accelerometer */
-    reg_write(BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL);
-    delay(1);
-    /* Wait for power-up to complete */
-    while (0x1 != reg_read_bits(BMI160_RA_PMU_STATUS,
+    if (flags & ACCEL) {
+        /* Power up the accelerometer */
+        reg_write(BMI160_RA_CMD, BMI160_CMD_ACC_MODE_NORMAL);
+        delay(1);
+
+        /* Wait for power-up to complete */
+        while (0x1 != reg_read_bits(BMI160_RA_PMU_STATUS,
                                 BMI160_ACC_PMU_STATUS_BIT,
                                 BMI160_ACC_PMU_STATUS_LEN))
+            delay(1);
+
+        sensors_enabled |= ACCEL;
+    }
+
+    if (flags & GYRO) {
+        /* Power up the gyroscope */
+        reg_write(BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL);
         delay(1);
 
-    /* Power up the gyroscope */
-    reg_write(BMI160_RA_CMD, BMI160_CMD_GYR_MODE_NORMAL);
-    delay(1);
-    /* Wait for power-up to complete */
-    while (0x1 != reg_read_bits(BMI160_RA_PMU_STATUS,
+        /* Wait for power-up to complete */
+        while (0x1 != reg_read_bits(BMI160_RA_PMU_STATUS,
                                 BMI160_GYR_PMU_STATUS_BIT,
                                 BMI160_GYR_PMU_STATUS_LEN))
-        delay(1);
+            delay(1);
+
+        sensors_enabled |= GYRO;
+    }
 
-    setFullScaleGyroRange(BMI160_GYRO_RANGE_250);
-    setFullScaleAccelRange(BMI160_ACCEL_RANGE_2G);
+    setFullScaleGyroRange(BMI160_GYRO_RANGE_250, 250.0f);
+    setFullScaleAccelRange(BMI160_ACCEL_RANGE_2G, 2.0f);
 
     /* Only PIN1 interrupts currently supported - map all interrupts to PIN1 */
     reg_write(BMI160_RA_INT_MAP_0, 0xFF);
@@ -131,6 +148,17 @@ uint8_t BMI160Class::getDeviceID() {
     return reg_read(BMI160_RA_CHIP_ID);
 }
 
+/* Checks if the specified sensors are enabled (sensors are specified using
+ * bit flags defined by CurieIMUSensor enum). If 0 is passed, checks if *any*
+ * sensors are enabled */
+bool BMI160Class::isEnabled(unsigned int sensors)
+{
+    if (sensors == 0)
+        return sensors_enabled > 0;
+
+   return (sensors_enabled & sensors) > 0;
+}
+
 /** Verify the SPI connection.
  * Make sure the device is connected and responds as expected.
  * @return True if connection is valid, false otherwise
@@ -330,10 +358,11 @@ uint8_t BMI160Class::getFullScaleGyroRange() {
  * @param range New full-scale gyroscope range value
  * @see getFullScaleGyroRange()
  */
-void BMI160Class::setFullScaleGyroRange(uint8_t range) {
+void BMI160Class::setFullScaleGyroRange(uint8_t range, float real) {
     reg_write_bits(BMI160_RA_GYRO_RANGE, range,
                    BMI160_GYRO_RANGE_SEL_BIT,
                    BMI160_GYRO_RANGE_SEL_LEN);
+    gyro_range = real;
 }
 
 /** Get full-scale accelerometer range.
@@ -362,10 +391,11 @@ uint8_t BMI160Class::getFullScaleAccelRange() {
  * @see getFullScaleAccelRange()
  * @see BMI160AccelRange
  */
-void BMI160Class::setFullScaleAccelRange(uint8_t range) {
+void BMI160Class::setFullScaleAccelRange(uint8_t range, float real) {
     reg_write_bits(BMI160_RA_ACCEL_RANGE, range,
                    BMI160_ACCEL_RANGE_SEL_BIT,
                    BMI160_ACCEL_RANGE_SEL_LEN);
+    accel_range = real;
 }
 
 /** Get accelerometer offset compensation enabled value.
@@ -2386,19 +2416,3 @@ uint8_t BMI160Class::getRegister(uint8_t reg) {
 void BMI160Class::setRegister(uint8_t reg, uint8_t data) {
     reg_write(reg, data);
 }
-
-/** Check if new gyroscope data is available
- * @return True if new data is available, else false.
- */
-bool BMI160Class::gyroDataReady()
-{
-    return reg_read_bits(BMI160_RA_STATUS, BMI160_STATUS_DRDY_GYR, 1);
-}
-
-/** Check if new accelerometer data is available
- * @return True if new data is available, else false.
- */
-bool BMI160Class::accelDataReady()
-{
-    return reg_read_bits(BMI160_RA_STATUS, BMI160_STATUS_DRDY_ACC, 1);
-}
diff --git a/libraries/CurieIMU/src/BMI160.h b/libraries/CurieIMU/src/BMI160.h
@@ -267,6 +267,12 @@ THE SOFTWARE.
 
 #define BMI160_RA_CMD               0x7E
 
+/* Bit flags for selecting individual sensors */
+typedef enum {
+    GYRO = 0x1,
+    ACCEL = 0x2
+} CurieIMUSensor;
+
 /**
  * Interrupt Latch Mode options
  * @see setInterruptLatch()
@@ -471,9 +477,10 @@ typedef enum {
 
 class BMI160Class {
     public:
-        void initialize();
+        void initialize(unsigned int flags);
         bool testConnection();
 
+        bool isEnabled(unsigned int sensors);
         uint8_t getGyroRate();
         void setGyroRate(uint8_t rate);
 
@@ -487,9 +494,9 @@ class BMI160Class {
         void setAccelDLPFMode(uint8_t bandwidth);
 
         uint8_t getFullScaleGyroRange();
-        void setFullScaleGyroRange(uint8_t range);
+        void setFullScaleGyroRange(uint8_t range, float real);
         uint8_t getFullScaleAccelRange();
-        void setFullScaleAccelRange(uint8_t range);
+        void setFullScaleAccelRange(uint8_t range, float real);
 
         void autoCalibrateGyroOffset();
         bool getGyroOffsetEnabled();
@@ -640,6 +647,7 @@ class BMI160Class {
 
         uint8_t getDeviceID();
 
+        int isBitSet(uint8_t value, unsigned bit);
         uint8_t getRegister(uint8_t reg);
         void setRegister(uint8_t reg, uint8_t data);
 
@@ -653,8 +661,10 @@ class BMI160Class {
         void setInterruptLatch(uint8_t latch);
         void resetInterrupt();
 
-        bool gyroDataReady();
-        bool accelDataReady();
+        /* Use a bitmask to track which sensors are enabled */
+        unsigned sensors_enabled;
+        float accel_range;
+        float gyro_range;
 
     protected:
         virtual int serial_buffer_transfer(uint8_t *buf, unsigned tx_cnt, unsigned rx_cnt) = 0;
diff --git a/libraries/CurieIMU/src/CurieIMU.cpp b/libraries/CurieIMU/src/CurieIMU.cpp
@@ -32,7 +32,8 @@
  * on the Curie module, before calling BMI160::initialize() to activate the
  * BMI160 accelerometer and gyroscpoe with default settings.
  */
-bool CurieIMUClass::begin()
+
+bool CurieIMUClass::configure_imu(unsigned int sensors)
 {
     ss_spi_init(SPI_SENSING_1, 2000, SPI_BUSMODE_0, SPI_8_BIT, SPI_SE_1);
 
@@ -41,7 +42,7 @@ bool CurieIMUClass::begin()
     serial_buffer_transfer(&dummy_reg, 1, 1);
 
     /* The SPI interface is ready - now invoke the base class initialization */
-    BMI160Class::initialize();
+    initialize(sensors);
 
     /** Verify the SPI connection.
      * MakgetGyroRatee sure the device is connected and responds as expected.
@@ -50,16 +51,66 @@ bool CurieIMUClass::begin()
     if (CURIE_IMU_CHIP_ID != getDeviceID())
         return false;
 
-    accel_range = (float) getAccelerometerRange();
-    gyro_range = (float) getGyroRange();
     return true;
 }
 
+bool CurieIMUClass::begin()
+{
+    return configure_imu(GYRO | ACCEL);
+}
+
+bool CurieIMUClass::begin(unsigned int sensors)
+{
+    return configure_imu(sensors);
+}
+
 void CurieIMUClass::end()
 {
     ss_spi_disable(SPI_SENSING_1);
 }
 
+bool CurieIMUClass::dataReady()
+{
+    uint8_t stat;
+
+    /* If no sensors are enabled */
+    if (!isEnabled(0))
+        return false;
+
+    /* Read status register */
+    stat = getRegister(BMI160_RA_STATUS);
+
+    if (isEnabled(GYRO) && !isBitSet(stat, BMI160_STATUS_DRDY_GYR))
+        return false;
+
+    if (isEnabled(ACCEL) && !isBitSet(stat, BMI160_STATUS_DRDY_ACC))
+        return false;
+
+    return true;
+}
+
+bool CurieIMUClass::dataReady(unsigned int sensors)
+{
+    uint8_t stat;
+
+    /* If no sensors enabled, or no data requested */
+    if (sensors == 0 || !isEnabled(0))
+        return false;
+
+    /* Read status register */
+    stat = getRegister(BMI160_RA_STATUS);
+
+    if ((sensors & GYRO) && isEnabled(GYRO) &&
+        !isBitSet(stat, BMI160_STATUS_DRDY_GYR))
+        return false;
+
+    if ((sensors & ACCEL) && isEnabled(ACCEL) &&
+        !isBitSet(stat, BMI160_STATUS_DRDY_ACC))
+        return false;
+
+    return true;
+}
+
 int CurieIMUClass::getGyroRate()
 {
     int rate;
@@ -227,25 +278,26 @@ int CurieIMUClass::getGyroRange()
 void CurieIMUClass::setGyroRange(int range)
 {
     BMI160GyroRange bmiRange;
+    float real;
 
     if (range >= 2000) {
         bmiRange = BMI160_GYRO_RANGE_2000;
-        gyro_range = 2000.0f;
+        real = 2000.0f;
     } else if (range >= 1000) {
         bmiRange = BMI160_GYRO_RANGE_1000;
-        gyro_range = 1000.0f;
+        real = 1000.0f;
     } else if (range >= 500) {
         bmiRange = BMI160_GYRO_RANGE_500;
-        gyro_range = 500.0f;
+        real = 500.0f;
     } else if (range >= 250) {
         bmiRange = BMI160_GYRO_RANGE_250;
-        gyro_range = 250.0f;
+        real = 250.0f;
     } else {
         bmiRange = BMI160_GYRO_RANGE_125;
-        gyro_range = 125.0f;
+        real = 125.0f;
     }
 
-    setFullScaleGyroRange(bmiRange);
+    setFullScaleGyroRange(bmiRange, real);
 }
 
 int CurieIMUClass::getAccelerometerRange()
@@ -277,22 +329,23 @@ int CurieIMUClass::getAccelerometerRange()
 void CurieIMUClass::setAccelerometerRange(int range)
 {
     BMI160AccelRange bmiRange;
+    float real;
 
     if (range <= 2) {
         bmiRange = BMI160_ACCEL_RANGE_2G;
-        accel_range = 2.0f;
+        real = 2.0f;
     } else if (range <= 4) {
         bmiRange = BMI160_ACCEL_RANGE_4G;
-        accel_range = 4.0f;
+        real = 4.0f;
     } else if (range <= 8) {
         bmiRange = BMI160_ACCEL_RANGE_8G;
-        accel_range = 8.0f;
+        real = 8.0f;
     } else {
         bmiRange = BMI160_ACCEL_RANGE_16G;
-        accel_range = 16.0f;
+        real = 16.0f;
     }
 
-    setFullScaleAccelRange(bmiRange);
+    setFullScaleAccelRange(bmiRange, real);
 }
 
 void CurieIMUClass::autoCalibrateGyroOffset()
diff --git a/libraries/CurieIMU/src/CurieIMU.h b/libraries/CurieIMU/src/CurieIMU.h
@@ -94,9 +94,13 @@ class CurieIMUClass : public BMI160Class {
     friend void bmi160_pin1_isr(void);
 
     public:
+        bool begin(unsigned int sensors);
         bool begin(void);
         void end(void);
 
+        bool dataReady();
+        bool dataReady(unsigned int sensors);
+
         // supported values: 25, 50, 100, 200, 400, 800, 1600, 3200 (Hz)
         int getGyroRate();
         void setGyroRate(int rate);
@@ -207,11 +211,9 @@ class CurieIMUClass : public BMI160Class {
         void detachInterrupt(void);
 
     private:
+        bool configure_imu(unsigned int sensors);
         int serial_buffer_transfer(uint8_t *buf, unsigned tx_cnt, unsigned rx_cnt);
 
-        float accel_range;
-        float gyro_range;
-
         float getFreefallDetectionThreshold();
         void setFreefallDetectionThreshold(float threshold);
         float getShockDetectionThreshold();
