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

Author: eriknyquist

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.
 

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.
 

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

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

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);
@@ -1546,6 +1555,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;
@@ -1560,6 +1581,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;
@@ -1571,6 +1607,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;
@@ -1582,6 +1629,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) {
@@ -1595,6 +1653,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) {
@@ -1608,6 +1683,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();

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);