From 9ef748e84e4c3e9a6024902abc9f1398bb1a0ade Mon Sep 17 00:00:00 2001 From: Erik Nyquist Date: Fri, 27 Jan 2017 16:28:38 -0800 Subject: [PATCH] Use single dataReady() method, instead of accelDataReady/gyroDataReady 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 --- libraries/CurieIMU/keywords.txt | 4 ++ libraries/CurieIMU/src/BMI160.cpp | 78 ++++++++++++++++----------- libraries/CurieIMU/src/BMI160.h | 20 +++++-- libraries/CurieIMU/src/CurieIMU.cpp | 83 +++++++++++++++++++++++------ libraries/CurieIMU/src/CurieIMU.h | 8 +-- 5 files changed, 138 insertions(+), 55 deletions(-) diff --git a/libraries/CurieIMU/keywords.txt b/libraries/CurieIMU/keywords.txt index 1c64688b..05b1cb2b 100644 --- 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 index d1bf2a22..c8945b38 100644 --- a/libraries/CurieIMU/src/BMI160.cpp +++ b/libraries/CurieIMU/src/BMI160.cpp @@ -78,6 +78,11 @@ 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. @@ -85,8 +90,10 @@ uint8_t BMI160Class::reg_read_bits(uint8_t reg, unsigned pos, unsigned len) * 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 index 1384f1c4..dfdc7531 100644 --- 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 index 0def7306..e9e64760 100644 --- 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 index 8056b135..3a7a0dc4 100644 --- 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();