Skip to content

Commit 1974340

Browse files
eriknyquistsys_maker
eriknyquist
authored and
sys_maker
committed
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
1 parent 5671917 commit 1974340

File tree

6 files changed

+114
-39
lines changed

6 files changed

+114
-39
lines changed

Diff for: ‎libraries/CurieIMU/examples/Accelerometer/Accelerometer.ino

+3-19
Original file line numberDiff line numberDiff line change
@@ -23,16 +23,10 @@ void setup() {
2323
}
2424

2525
void loop() {
26-
int axRaw, ayRaw, azRaw; // raw accelerometer values
27-
float ax, ay, az;
26+
float ax, ay, az; //scaled accelerometer values
2827

29-
// read raw accelerometer measurements from device
30-
CurieIMU.readAccelerometer(axRaw, ayRaw, azRaw);
31-
32-
// convert the raw accelerometer data to G's
33-
ax = convertRawAcceleration(axRaw);
34-
ay = convertRawAcceleration(ayRaw);
35-
az = convertRawAcceleration(azRaw);
28+
// read accelerometer measurements from device, scaled to the configured range
29+
CurieIMU.readAccelerometerScaled(ax, ay, az);
3630

3731
// display tab-separated accelerometer x/y/z values
3832
Serial.print("a:\t");
@@ -44,16 +38,6 @@ void loop() {
4438
Serial.println();
4539
}
4640

47-
float convertRawAcceleration(int aRaw) {
48-
// since we are using 2G range
49-
// -2g maps to a raw value of -32768
50-
// +2g maps to a raw value of 32767
51-
52-
float a = (aRaw * 2.0) / 32768.0;
53-
54-
return a;
55-
}
56-
5741
/*
5842
Copyright (c) 2016 Intel Corporation. All rights reserved.
5943

Diff for: ‎libraries/CurieIMU/examples/Gyro/Gyro.ino

+3-19
Original file line numberDiff line numberDiff line change
@@ -23,16 +23,10 @@ void setup() {
2323
}
2424

2525
void loop() {
26-
int gxRaw, gyRaw, gzRaw; // raw gyro values
27-
float gx, gy, gz;
26+
float gx, gy, gz; //scaled Gyro values
2827

29-
// read raw gyro measurements from device
30-
CurieIMU.readGyro(gxRaw, gyRaw, gzRaw);
31-
32-
// convert the raw gyro data to degrees/second
33-
gx = convertRawGyro(gxRaw);
34-
gy = convertRawGyro(gyRaw);
35-
gz = convertRawGyro(gzRaw);
28+
// read gyro measurements from device, scaled to the configured range
29+
CurieIMU.readGyroScaled(gx, gy, gz);
3630

3731
// display tab-separated gyro x/y/z values
3832
Serial.print("g:\t");
@@ -44,16 +38,6 @@ void loop() {
4438
Serial.println();
4539
}
4640

47-
float convertRawGyro(int gRaw) {
48-
// since we are using 250 degrees/seconds range
49-
// -250 maps to a raw value of -32768
50-
// +250 maps to a raw value of 32767
51-
52-
float g = (gRaw * 250.0) / 32768.0;
53-
54-
return g;
55-
}
56-
5741
/*
5842
Copyright (c) 2016 Intel Corporation. All rights reserved.
5943

Diff for: ‎libraries/CurieIMU/keywords.txt

+3
Original file line numberDiff line numberDiff line change
@@ -59,11 +59,14 @@ setStepCountEnabled KEYWORD1
5959
resetStepCount KEYWORD1
6060

6161
readMotionSensor KEYWORD1
62+
readMotionSensorScaled KEYWORD1
6263
readAcceleration KEYWORD1
6364
readRotation KEYWORD1
6465

6566
readAccelerometer KEYWORD1
67+
readAccelerometerScaled KEYWORD1
6668
readGyro KEYWORD1
69+
readGyroScaled KEYWORD1
6770
readTemperature KEYWORD1
6871

6972
shockDetected KEYWORD1

Diff for: ‎libraries/CurieIMU/src/BMI160.h

+3
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,9 @@ THE SOFTWARE.
3535

3636
#include "Arduino.h"
3737

38+
#define BMI160_SENSOR_RANGE 65535.0f
39+
#define BMI160_SENSOR_LOW 32768.0f
40+
3841
#define BMI160_SPI_READ_BIT 7
3942

4043
#define BMI160_RA_CHIP_ID 0x00

Diff for: ‎libraries/CurieIMU/src/CurieIMU.cpp

+92
Original file line numberDiff line numberDiff line change
@@ -232,14 +232,19 @@ void CurieIMUClass::setGyroRange(int range)
232232

233233
if (range >= 2000) {
234234
bmiRange = BMI160_GYRO_RANGE_2000;
235+
gyro_range = 2000.0f;
235236
} else if (range >= 1000) {
236237
bmiRange = BMI160_GYRO_RANGE_1000;
238+
gyro_range = 1000.0f;
237239
} else if (range >= 500) {
238240
bmiRange = BMI160_GYRO_RANGE_500;
241+
gyro_range = 500.0f;
239242
} else if (range >= 250) {
240243
bmiRange = BMI160_GYRO_RANGE_250;
244+
gyro_range = 250.0f;
241245
} else {
242246
bmiRange = BMI160_GYRO_RANGE_125;
247+
gyro_range = 125.0f;
243248
}
244249

245250
setFullScaleGyroRange(bmiRange);
@@ -277,12 +282,16 @@ void CurieIMUClass::setAccelerometerRange(int range)
277282

278283
if (range <= 2) {
279284
bmiRange = BMI160_ACCEL_RANGE_2G;
285+
accel_range = 2.0f;
280286
} else if (range <= 4) {
281287
bmiRange = BMI160_ACCEL_RANGE_4G;
288+
accel_range = 4.0f;
282289
} else if (range <= 8) {
283290
bmiRange = BMI160_ACCEL_RANGE_8G;
291+
accel_range = 8.0f;
284292
} else {
285293
bmiRange = BMI160_ACCEL_RANGE_16G;
294+
accel_range = 16.0f;
286295
}
287296

288297
setFullScaleAccelRange(bmiRange);
@@ -1546,6 +1555,18 @@ void CurieIMUClass::setStepDetectionMode(int mode)
15461555
BMI160Class::setStepDetectionMode((BMI160StepMode)mode);
15471556
}
15481557

1558+
float CurieIMUClass::convertRaw(int16_t raw, float range_abs)
1559+
{
1560+
float slope;
1561+
float val;
1562+
1563+
/* Input range will be -32768 to 32767
1564+
* Output range must be -range_abs to range_abs */
1565+
val = (float) raw;
1566+
slope = (range_abs * 2.0f) / BMI160_SENSOR_RANGE;
1567+
return -(range_abs) + slope * (val + BMI160_SENSOR_LOW);
1568+
}
1569+
15491570
void CurieIMUClass::readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy, int& gz)
15501571
{
15511572
short sax, say, saz, sgx, sgy, sgz;
@@ -1560,6 +1581,21 @@ void CurieIMUClass::readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy
15601581
gz = sgz;
15611582
}
15621583

1584+
void CurieIMUClass::readMotionSensorScaled(float& ax, float& ay, float& az,
1585+
float& gx, float& gy, float& gz)
1586+
{
1587+
int16_t sax, say, saz, sgx, sgy, sgz;
1588+
1589+
getMotion6(&sax, &say, &saz, &sgx, &sgy, &sgz);
1590+
1591+
ax = convertRaw(sax, accel_range);
1592+
ay = convertRaw(say, accel_range);
1593+
az = convertRaw(saz, accel_range);
1594+
gx = convertRaw(sgx, gyro_range);
1595+
gy = convertRaw(sgy, gyro_range);
1596+
gz = convertRaw(sgz, gyro_range);
1597+
}
1598+
15631599
void CurieIMUClass::readAccelerometer(int& x, int& y, int& z)
15641600
{
15651601
short sx, sy, sz;
@@ -1571,6 +1607,17 @@ void CurieIMUClass::readAccelerometer(int& x, int& y, int& z)
15711607
z = sz;
15721608
}
15731609

1610+
void CurieIMUClass::readAccelerometerScaled(float& x, float& y, float& z)
1611+
{
1612+
int16_t sx, sy, sz;
1613+
1614+
getAcceleration(&sx, &sy, &sz);
1615+
1616+
x = convertRaw(sx, accel_range);
1617+
y = convertRaw(sy, accel_range);
1618+
z = convertRaw(sz, accel_range);
1619+
}
1620+
15741621
void CurieIMUClass::readGyro(int& x, int& y, int& z)
15751622
{
15761623
short sx, sy, sz;
@@ -1582,6 +1629,17 @@ void CurieIMUClass::readGyro(int& x, int& y, int& z)
15821629
z = sz;
15831630
}
15841631

1632+
void CurieIMUClass::readGyroScaled(float& x, float& y, float& z)
1633+
{
1634+
int16_t sx, sy, sz;
1635+
1636+
getRotation(&sx, &sy, &sz);
1637+
1638+
x = convertRaw(sx, gyro_range);
1639+
y = convertRaw(sy, gyro_range);
1640+
z = convertRaw(sz, gyro_range);
1641+
}
1642+
15851643
int CurieIMUClass::readAccelerometer(int axis)
15861644
{
15871645
if (axis == X_AXIS) {
@@ -1595,6 +1653,23 @@ int CurieIMUClass::readAccelerometer(int axis)
15951653
return 0;
15961654
}
15971655

1656+
float CurieIMUClass::readAccelerometerScaled(int axis)
1657+
{
1658+
int16_t raw;
1659+
1660+
if (axis == X_AXIS) {
1661+
raw = getAccelerationX();
1662+
} else if (axis == Y_AXIS) {
1663+
raw = getAccelerationY();
1664+
} else if (axis == Z_AXIS) {
1665+
raw = getAccelerationZ();
1666+
} else {
1667+
return 0;
1668+
}
1669+
1670+
return convertRaw(raw, accel_range);
1671+
}
1672+
15981673
int CurieIMUClass::readGyro(int axis)
15991674
{
16001675
if (axis == X_AXIS) {
@@ -1608,6 +1683,23 @@ int CurieIMUClass::readGyro(int axis)
16081683
return 0;
16091684
}
16101685

1686+
float CurieIMUClass::readGyroScaled(int axis)
1687+
{
1688+
int16_t raw;
1689+
1690+
if (axis == X_AXIS) {
1691+
raw = getRotationX();
1692+
} else if (axis == Y_AXIS) {
1693+
raw = getRotationY();
1694+
} else if (axis == Z_AXIS) {
1695+
raw = getRotationZ();
1696+
} else {
1697+
return 0;
1698+
}
1699+
1700+
return convertRaw(raw, gyro_range);
1701+
}
1702+
16111703
int CurieIMUClass::readTemperature()
16121704
{
16131705
return getTemperature();

Diff for: ‎libraries/CurieIMU/src/CurieIMU.h

+10-1
Original file line numberDiff line numberDiff line change
@@ -187,11 +187,15 @@ class CurieIMUClass : public BMI160Class {
187187
void setStepDetectionMode(int mode);
188188

189189
void readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy, int& gz);
190+
void readMotionSensorScaled(float& ax, float& ay, float& az, float& gx, float& gy, float& gz);
190191
void readAccelerometer(int& x, int& y, int& z);
192+
void readAccelerometerScaled(float& x, float& y, float& z);
191193
void readGyro(int& x, int& y, int& z);
192-
194+
void readGyroScaled(float& x, float& y, float& z);
193195
int readAccelerometer(int axis);
196+
float readAccelerometerScaled(int axis);
194197
int readGyro(int axis);
198+
float readGyroScaled(int axis);
195199
int readTemperature();
196200

197201
bool shockDetected(int axis, int direction);
@@ -205,6 +209,9 @@ class CurieIMUClass : public BMI160Class {
205209
private:
206210
int serial_buffer_transfer(uint8_t *buf, unsigned tx_cnt, unsigned rx_cnt);
207211

212+
float accel_range;
213+
float gyro_range;
214+
208215
float getFreefallDetectionThreshold();
209216
void setFreefallDetectionThreshold(float threshold);
210217
float getShockDetectionThreshold();
@@ -231,6 +238,8 @@ class CurieIMUClass : public BMI160Class {
231238
int getDoubleTapDetectionDuration();
232239
void setDoubleTapDetectionDuration(int duration);
233240

241+
float convertRaw(int16_t raw, float range_abs);
242+
234243
void enableInterrupt(int feature, bool enabled);
235244

236245
void (*_user_callback)(void);

0 commit comments

Comments
 (0)