From be8db88917e8f190579b0f36c3e4be8c81c87dd4 Mon Sep 17 00:00:00 2001 From: Erik Nyquist Date: Sun, 17 Jul 2016 00:33:17 -0700 Subject: [PATCH] Issue #149: Provide scaled IMU data Add implementations of readAccelerometer and readGyro which return values that are scaled according to the range set with setAccelerometerRange and setGyroRange --- .../examples/Accelerometer/Accelerometer.ino | 22 +---- libraries/CurieIMU/examples/Gyro/Gyro.ino | 22 +---- libraries/CurieIMU/keywords.txt | 3 + libraries/CurieIMU/src/BMI160.h | 3 + libraries/CurieIMU/src/CurieIMU.cpp | 92 +++++++++++++++++++ libraries/CurieIMU/src/CurieIMU.h | 11 ++- 6 files changed, 114 insertions(+), 39 deletions(-) diff --git a/libraries/CurieIMU/examples/Accelerometer/Accelerometer.ino b/libraries/CurieIMU/examples/Accelerometer/Accelerometer.ino index 22069c61..a2553b4d 100644 --- a/libraries/CurieIMU/examples/Accelerometer/Accelerometer.ino +++ b/libraries/CurieIMU/examples/Accelerometer/Accelerometer.ino @@ -23,16 +23,10 @@ void setup() { } void loop() { - int axRaw, ayRaw, azRaw; // raw accelerometer values - float ax, ay, az; + float ax, ay, az; //scaled accelerometer values - // read raw accelerometer measurements from device - CurieIMU.readAccelerometer(axRaw, ayRaw, azRaw); - - // convert the raw accelerometer data to G's - ax = convertRawAcceleration(axRaw); - ay = convertRawAcceleration(ayRaw); - az = convertRawAcceleration(azRaw); + // read accelerometer measurements from device, scaled to the configured range + CurieIMU.readAccelerometerScaled(ax, ay, az); // display tab-separated accelerometer x/y/z values Serial.print("a:\t"); @@ -44,16 +38,6 @@ void loop() { Serial.println(); } -float convertRawAcceleration(int aRaw) { - // since we are using 2G range - // -2g maps to a raw value of -32768 - // +2g maps to a raw value of 32767 - - float a = (aRaw * 2.0) / 32768.0; - - return a; -} - /* Copyright (c) 2016 Intel Corporation. All rights reserved. diff --git a/libraries/CurieIMU/examples/Gyro/Gyro.ino b/libraries/CurieIMU/examples/Gyro/Gyro.ino index a7752659..56d0b64e 100644 --- a/libraries/CurieIMU/examples/Gyro/Gyro.ino +++ b/libraries/CurieIMU/examples/Gyro/Gyro.ino @@ -23,16 +23,10 @@ void setup() { } void loop() { - int gxRaw, gyRaw, gzRaw; // raw gyro values - float gx, gy, gz; + float gx, gy, gz; //scaled Gyro values - // read raw gyro measurements from device - CurieIMU.readGyro(gxRaw, gyRaw, gzRaw); - - // convert the raw gyro data to degrees/second - gx = convertRawGyro(gxRaw); - gy = convertRawGyro(gyRaw); - gz = convertRawGyro(gzRaw); + // read gyro measurements from device, scaled to the configured range + CurieIMU.readGyroScaled(gx, gy, gz); // display tab-separated gyro x/y/z values Serial.print("g:\t"); @@ -44,16 +38,6 @@ void loop() { Serial.println(); } -float convertRawGyro(int gRaw) { - // since we are using 250 degrees/seconds range - // -250 maps to a raw value of -32768 - // +250 maps to a raw value of 32767 - - float g = (gRaw * 250.0) / 32768.0; - - return g; -} - /* Copyright (c) 2016 Intel Corporation. All rights reserved. diff --git a/libraries/CurieIMU/keywords.txt b/libraries/CurieIMU/keywords.txt index 874baf4f..1c64688b 100644 --- a/libraries/CurieIMU/keywords.txt +++ b/libraries/CurieIMU/keywords.txt @@ -59,11 +59,14 @@ setStepCountEnabled KEYWORD1 resetStepCount KEYWORD1 readMotionSensor KEYWORD1 +readMotionSensorScaled KEYWORD1 readAcceleration KEYWORD1 readRotation KEYWORD1 readAccelerometer KEYWORD1 +readAccelerometerScaled KEYWORD1 readGyro KEYWORD1 +readGyroScaled KEYWORD1 readTemperature KEYWORD1 shockDetected KEYWORD1 diff --git a/libraries/CurieIMU/src/BMI160.h b/libraries/CurieIMU/src/BMI160.h index 2c9f3654..b452965a 100644 --- a/libraries/CurieIMU/src/BMI160.h +++ b/libraries/CurieIMU/src/BMI160.h @@ -35,6 +35,9 @@ THE SOFTWARE. #include "Arduino.h" +#define BMI160_SENSOR_RANGE 65535.0f +#define BMI160_SENSOR_LOW 32768.0f + #define BMI160_SPI_READ_BIT 7 #define BMI160_RA_CHIP_ID 0x00 diff --git a/libraries/CurieIMU/src/CurieIMU.cpp b/libraries/CurieIMU/src/CurieIMU.cpp index 0a91cc73..67b6630f 100644 --- a/libraries/CurieIMU/src/CurieIMU.cpp +++ b/libraries/CurieIMU/src/CurieIMU.cpp @@ -232,14 +232,19 @@ void CurieIMUClass::setGyroRange(int range) if (range >= 2000) { bmiRange = BMI160_GYRO_RANGE_2000; + gyro_range = 2000.0f; } else if (range >= 1000) { bmiRange = BMI160_GYRO_RANGE_1000; + gyro_range = 1000.0f; } else if (range >= 500) { bmiRange = BMI160_GYRO_RANGE_500; + gyro_range = 500.0f; } else if (range >= 250) { bmiRange = BMI160_GYRO_RANGE_250; + gyro_range = 250.0f; } else { bmiRange = BMI160_GYRO_RANGE_125; + gyro_range = 125.0f; } setFullScaleGyroRange(bmiRange); @@ -277,12 +282,16 @@ void CurieIMUClass::setAccelerometerRange(int range) if (range <= 2) { bmiRange = BMI160_ACCEL_RANGE_2G; + accel_range = 2.0f; } else if (range <= 4) { bmiRange = BMI160_ACCEL_RANGE_4G; + accel_range = 4.0f; } else if (range <= 8) { bmiRange = BMI160_ACCEL_RANGE_8G; + accel_range = 8.0f; } else { bmiRange = BMI160_ACCEL_RANGE_16G; + accel_range = 16.0f; } setFullScaleAccelRange(bmiRange); @@ -1556,6 +1565,18 @@ void CurieIMUClass::setStepDetectionMode(int mode) BMI160Class::setStepDetectionMode((BMI160StepMode)mode); } +float CurieIMUClass::convertRaw(int16_t raw, float range_abs) +{ + float slope; + float val; + + /* Input range will be -32768 to 32767 + * Output range must be -range_abs to range_abs */ + val = (float) raw; + slope = (range_abs * 2.0f) / BMI160_SENSOR_RANGE; + return -(range_abs) + slope * (val + BMI160_SENSOR_LOW); +} + void CurieIMUClass::readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy, int& gz) { short sax, say, saz, sgx, sgy, sgz; @@ -1570,6 +1591,21 @@ void CurieIMUClass::readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy gz = sgz; } +void CurieIMUClass::readMotionSensorScaled(float& ax, float& ay, float& az, + float& gx, float& gy, float& gz) +{ + int16_t sax, say, saz, sgx, sgy, sgz; + + getMotion6(&sax, &say, &saz, &sgx, &sgy, &sgz); + + ax = convertRaw(sax, accel_range); + ay = convertRaw(say, accel_range); + az = convertRaw(saz, accel_range); + gx = convertRaw(sgx, gyro_range); + gy = convertRaw(sgy, gyro_range); + gz = convertRaw(sgz, gyro_range); +} + void CurieIMUClass::readAccelerometer(int& x, int& y, int& z) { short sx, sy, sz; @@ -1581,6 +1617,17 @@ void CurieIMUClass::readAccelerometer(int& x, int& y, int& z) z = sz; } +void CurieIMUClass::readAccelerometerScaled(float& x, float& y, float& z) +{ + int16_t sx, sy, sz; + + getAcceleration(&sx, &sy, &sz); + + x = convertRaw(sx, accel_range); + y = convertRaw(sy, accel_range); + z = convertRaw(sz, accel_range); +} + void CurieIMUClass::readGyro(int& x, int& y, int& z) { short sx, sy, sz; @@ -1592,6 +1639,17 @@ void CurieIMUClass::readGyro(int& x, int& y, int& z) z = sz; } +void CurieIMUClass::readGyroScaled(float& x, float& y, float& z) +{ + int16_t sx, sy, sz; + + getRotation(&sx, &sy, &sz); + + x = convertRaw(sx, gyro_range); + y = convertRaw(sy, gyro_range); + z = convertRaw(sz, gyro_range); +} + int CurieIMUClass::readAccelerometer(int axis) { if (axis == X_AXIS) { @@ -1605,6 +1663,23 @@ int CurieIMUClass::readAccelerometer(int axis) return 0; } +float CurieIMUClass::readAccelerometerScaled(int axis) +{ + int16_t raw; + + if (axis == X_AXIS) { + raw = getAccelerationX(); + } else if (axis == Y_AXIS) { + raw = getAccelerationY(); + } else if (axis == Z_AXIS) { + raw = getAccelerationZ(); + } else { + return 0; + } + + return convertRaw(raw, accel_range); +} + int CurieIMUClass::readGyro(int axis) { if (axis == X_AXIS) { @@ -1618,6 +1693,23 @@ int CurieIMUClass::readGyro(int axis) return 0; } +float CurieIMUClass::readGyroScaled(int axis) +{ + int16_t raw; + + if (axis == X_AXIS) { + raw = getRotationX(); + } else if (axis == Y_AXIS) { + raw = getRotationY(); + } else if (axis == Z_AXIS) { + raw = getRotationZ(); + } else { + return 0; + } + + return convertRaw(raw, gyro_range); +} + int CurieIMUClass::readTemperature() { return getTemperature(); diff --git a/libraries/CurieIMU/src/CurieIMU.h b/libraries/CurieIMU/src/CurieIMU.h index c27aa314..8056b135 100644 --- a/libraries/CurieIMU/src/CurieIMU.h +++ b/libraries/CurieIMU/src/CurieIMU.h @@ -187,11 +187,15 @@ class CurieIMUClass : public BMI160Class { void setStepDetectionMode(int mode); void readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy, int& gz); + void readMotionSensorScaled(float& ax, float& ay, float& az, float& gx, float& gy, float& gz); void readAccelerometer(int& x, int& y, int& z); + void readAccelerometerScaled(float& x, float& y, float& z); void readGyro(int& x, int& y, int& z); - + void readGyroScaled(float& x, float& y, float& z); int readAccelerometer(int axis); + float readAccelerometerScaled(int axis); int readGyro(int axis); + float readGyroScaled(int axis); int readTemperature(); bool shockDetected(int axis, int direction); @@ -205,6 +209,9 @@ class CurieIMUClass : public BMI160Class { private: 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(); @@ -231,6 +238,8 @@ class CurieIMUClass : public BMI160Class { int getDoubleTapDetectionDuration(); void setDoubleTapDetectionDuration(int duration); + float convertRaw(int16_t raw, float range_abs); + void enableInterrupt(int feature, bool enabled); void (*_user_callback)(void);