Issue arduino#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

Diff for: 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 for: 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 for: 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,16 @@ void CurieIMUClass::setStepDetectionMode(int mode)
     BMI160Class::setStepDetectionMode((BMI160StepMode)mode);
 }
 
+float convertRaw(int16_t raw, float range)
+{
+    float slope;
+    float val;
+
+    val = (float) raw;
+    slope = (range * 2.0f) / BMI160_SENSOR_RANGE;
+    return -(range) + 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 +1589,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 +1615,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 +1637,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 +1661,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 +1691,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 for: 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();