Skip to content

Commit be8db88

Browse files
eriknyquisteriknyquist
committed
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
1 parent 12b2376 commit be8db88

File tree

6 files changed

+114
-39
lines changed

6 files changed

+114
-39
lines changed

‎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

‎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

‎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

‎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

‎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);
@@ -1556,6 +1565,18 @@ void CurieIMUClass::setStepDetectionMode(int mode)
15561565
BMI160Class::setStepDetectionMode((BMI160StepMode)mode);
15571566
}
15581567

1568+
float CurieIMUClass::convertRaw(int16_t raw, float range_abs)
1569+
{
1570+
float slope;
1571+
float val;
1572+
1573+
/* Input range will be -32768 to 32767
1574+
* Output range must be -range_abs to range_abs */
1575+
val = (float) raw;
1576+
slope = (range_abs * 2.0f) / BMI160_SENSOR_RANGE;
1577+
return -(range_abs) + slope * (val + BMI160_SENSOR_LOW);
1578+
}
1579+
15591580
void CurieIMUClass::readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy, int& gz)
15601581
{
15611582
short sax, say, saz, sgx, sgy, sgz;
@@ -1570,6 +1591,21 @@ void CurieIMUClass::readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy
15701591
gz = sgz;
15711592
}
15721593

1594+
void CurieIMUClass::readMotionSensorScaled(float& ax, float& ay, float& az,
1595+
float& gx, float& gy, float& gz)
1596+
{
1597+
int16_t sax, say, saz, sgx, sgy, sgz;
1598+
1599+
getMotion6(&sax, &say, &saz, &sgx, &sgy, &sgz);
1600+
1601+
ax = convertRaw(sax, accel_range);
1602+
ay = convertRaw(say, accel_range);
1603+
az = convertRaw(saz, accel_range);
1604+
gx = convertRaw(sgx, gyro_range);
1605+
gy = convertRaw(sgy, gyro_range);
1606+
gz = convertRaw(sgz, gyro_range);
1607+
}
1608+
15731609
void CurieIMUClass::readAccelerometer(int& x, int& y, int& z)
15741610
{
15751611
short sx, sy, sz;
@@ -1581,6 +1617,17 @@ void CurieIMUClass::readAccelerometer(int& x, int& y, int& z)
15811617
z = sz;
15821618
}
15831619

1620+
void CurieIMUClass::readAccelerometerScaled(float& x, float& y, float& z)
1621+
{
1622+
int16_t sx, sy, sz;
1623+
1624+
getAcceleration(&sx, &sy, &sz);
1625+
1626+
x = convertRaw(sx, accel_range);
1627+
y = convertRaw(sy, accel_range);
1628+
z = convertRaw(sz, accel_range);
1629+
}
1630+
15841631
void CurieIMUClass::readGyro(int& x, int& y, int& z)
15851632
{
15861633
short sx, sy, sz;
@@ -1592,6 +1639,17 @@ void CurieIMUClass::readGyro(int& x, int& y, int& z)
15921639
z = sz;
15931640
}
15941641

1642+
void CurieIMUClass::readGyroScaled(float& x, float& y, float& z)
1643+
{
1644+
int16_t sx, sy, sz;
1645+
1646+
getRotation(&sx, &sy, &sz);
1647+
1648+
x = convertRaw(sx, gyro_range);
1649+
y = convertRaw(sy, gyro_range);
1650+
z = convertRaw(sz, gyro_range);
1651+
}
1652+
15951653
int CurieIMUClass::readAccelerometer(int axis)
15961654
{
15971655
if (axis == X_AXIS) {
@@ -1605,6 +1663,23 @@ int CurieIMUClass::readAccelerometer(int axis)
16051663
return 0;
16061664
}
16071665

1666+
float CurieIMUClass::readAccelerometerScaled(int axis)
1667+
{
1668+
int16_t raw;
1669+
1670+
if (axis == X_AXIS) {
1671+
raw = getAccelerationX();
1672+
} else if (axis == Y_AXIS) {
1673+
raw = getAccelerationY();
1674+
} else if (axis == Z_AXIS) {
1675+
raw = getAccelerationZ();
1676+
} else {
1677+
return 0;
1678+
}
1679+
1680+
return convertRaw(raw, accel_range);
1681+
}
1682+
16081683
int CurieIMUClass::readGyro(int axis)
16091684
{
16101685
if (axis == X_AXIS) {
@@ -1618,6 +1693,23 @@ int CurieIMUClass::readGyro(int axis)
16181693
return 0;
16191694
}
16201695

1696+
float CurieIMUClass::readGyroScaled(int axis)
1697+
{
1698+
int16_t raw;
1699+
1700+
if (axis == X_AXIS) {
1701+
raw = getRotationX();
1702+
} else if (axis == Y_AXIS) {
1703+
raw = getRotationY();
1704+
} else if (axis == Z_AXIS) {
1705+
raw = getRotationZ();
1706+
} else {
1707+
return 0;
1708+
}
1709+
1710+
return convertRaw(raw, gyro_range);
1711+
}
1712+
16211713
int CurieIMUClass::readTemperature()
16221714
{
16231715
return getTemperature();

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