Justin Jordan
Library for Bosch Sensortech BMI160 IMU
Dependents: MAX32630FTHR_BALANCE_BOT MPSMAX_copy MAX32630FTHR_BALANCE_BOT SELF_BALANCING_BOT
Diff: bmi160.cpp
- Revision:
- 16:12782f5d4aa4
- Parent:
- 15:dc35ccc0b08e
diff -r dc35ccc0b08e -r 12782f5d4aa4 bmi160.cpp
--- a/bmi160.cpp Tue Dec 20 21:20:22 2016 +0000
+++ b/bmi160.cpp Tue Dec 20 23:57:54 2016 +0000
@@ -34,7 +34,14 @@
#include "bmi160.h"
-const struct BMI160::AccConfig BMI160::DEFAULT_ACC_CONFIG = {SENS_2G, ACC_US_OFF, ACC_BWP_2, ACC_ODR_8};
+const struct BMI160::AccConfig BMI160::DEFAULT_ACC_CONFIG = {SENS_2G,
+ ACC_US_OFF,
+ ACC_BWP_2,
+ ACC_ODR_8};
+
+const struct BMI160::GyroConfig BMI160::DEFAULT_GYRO_CONFIG = {DPS_2000,
+ GYRO_BWP_2,
+ GYRO_ODR_8};
//*****************************************************************************
@@ -66,7 +73,7 @@
//*****************************************************************************
-int32_t BMI160::setAccConfig(const AccConfig &config)
+int32_t BMI160::setSensorConfig(const AccConfig &config)
{
uint8_t data[2];
@@ -79,7 +86,19 @@
//*****************************************************************************
-int32_t BMI160::getAccConfig(AccConfig &config)
+int32_t BMI160::setSensorConfig(const GyroConfig &config)
+{
+ uint8_t data[2];
+
+ data[0] = ((config.bwp << GYRO_BWP_POS) | (config.odr << GYRO_ODR_POS));
+ data[1] = config.range;
+
+ return writeBlock(GYR_CONF, GYR_RANGE, data);
+}
+
+
+//*****************************************************************************
+int32_t BMI160::getSensorConfig(AccConfig &config)
{
uint8_t data[2];
int32_t rtnVal = readBlock(ACC_CONF, ACC_RANGE, data);
@@ -92,7 +111,7 @@
((data[0] & ACC_US_MASK) >> ACC_US_POS));
config.bwp = static_cast<BMI160::AccBandWidthParam>(
((data[0] & ACC_BWP_MASK) >> ACC_BWP_POS));
- config.odr = static_cast<BMI160::AccOutPutDataRate>(
+ config.odr = static_cast<BMI160::AccOutputDataRate>(
((data[0] & ACC_ODR_MASK) >> ACC_ODR_POS));
}
@@ -101,7 +120,27 @@
//*****************************************************************************
-int32_t BMI160::getAccAxis(SensorAxis axis, AxisData &data, AccRange range)
+int32_t BMI160::getSensorConfig(GyroConfig &config)
+{
+ uint8_t data[2];
+ int32_t rtnVal = readBlock(GYR_CONF, GYR_RANGE, data);
+
+ if(rtnVal == RTN_NO_ERROR)
+ {
+ config.range = static_cast<BMI160::GyroRange>(
+ (data[1] & GYRO_RANGE_MASK));
+ config.bwp = static_cast<BMI160::GyroBandWidthParam>(
+ ((data[0] & GYRO_BWP_MASK) >> GYRO_BWP_POS));
+ config.odr = static_cast<BMI160::GyroOutputDataRate>(
+ ((data[0] & GYRO_ODR_MASK) >> GYRO_ODR_POS));
+ }
+
+ return rtnVal;
+}
+
+
+//*****************************************************************************
+int32_t BMI160::getSensorAxis(SensorAxis axis, AxisData &data, AccRange range)
{
uint8_t localData[2];
int32_t rtnVal;
@@ -150,10 +189,66 @@
return rtnVal;
}
+
+
+//*****************************************************************************
+int32_t BMI160::getSensorAxis(SensorAxis axis, AxisData &data, GyroRange range)
+{
+ uint8_t localData[2];
+ int32_t rtnVal;
+
+ switch(axis)
+ {
+ case X_AXIS:
+ rtnVal = readBlock(DATA_8, DATA_9, localData);
+ break;
+
+ case Y_AXIS:
+ rtnVal = readBlock(DATA_10, DATA_11, localData);
+ break;
+
+ case Z_AXIS:
+ rtnVal = readBlock(DATA_12, DATA_13, localData);
+ break;
+
+ default:
+ rtnVal = -1;
+ break;
+ }
+
+ if(rtnVal == RTN_NO_ERROR)
+ {
+ data.raw = ((localData[1] << 8) | localData[0]);
+ switch(range)
+ {
+ case DPS_2000:
+ data.scaled = (data.raw/SENS_2000_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_1000:
+ data.scaled = (data.raw/SENS_1000_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_500:
+ data.scaled = (data.raw/SENS_500_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_250:
+ data.scaled = (data.raw/SENS_250_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_125:
+ data.scaled = (data.raw/SENS_125_DPS_LSB_PER_DPS);
+ break;
+ }
+ }
+
+ return rtnVal;
+}
//*****************************************************************************
-int32_t BMI160::getAccXYZ(SensorData &data, AccRange range)
+int32_t BMI160::getSensorXYZ(SensorData &data, AccRange range)
{
uint8_t localData[6];
int32_t rtnVal = readBlock(DATA_14, DATA_19, localData);
@@ -196,8 +291,60 @@
}
+//*****************************************************************************
+int32_t BMI160::getSensorXYZ(SensorData &data, GyroRange range)
+{
+ uint8_t localData[6];
+ int32_t rtnVal = readBlock(DATA_8, DATA_13, localData);
+
+ if(rtnVal == RTN_NO_ERROR)
+ {
+ data.xAxis.raw = ((localData[1] << 8) | localData[0]);
+ data.yAxis.raw = ((localData[3] << 8) | localData[2]);
+ data.zAxis.raw = ((localData[5] << 8) | localData[4]);
+
+ switch(range)
+ {
+ case DPS_2000:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_1000:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_500:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_250:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_125:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ break;
+ }
+ }
+
+ return rtnVal;
+}
+
+
//*****************************************************************************
-int32_t BMI160::getAccXYZandSensorTime(SensorData &data, SensorTime &sensorTime, AccRange range)
+int32_t BMI160::getSensorXYZandSensorTime(SensorData &data,
+ SensorTime &sensorTime,
+ AccRange range)
{
uint8_t localData[9];
int32_t rtnVal = readBlock(DATA_14, SENSORTIME_2, localData);
@@ -244,6 +391,149 @@
//*****************************************************************************
+int32_t BMI160::getSensorXYZandSensorTime(SensorData &data,
+ SensorTime &sensorTime,
+ GyroRange range)
+{
+ uint8_t localData[16];
+ int32_t rtnVal = readBlock(DATA_8, SENSORTIME_2, localData);
+ if(rtnVal == RTN_NO_ERROR)
+ {
+ data.xAxis.raw = ((localData[1] << 8) | localData[0]);
+ data.yAxis.raw = ((localData[3] << 8) | localData[2]);
+ data.zAxis.raw = ((localData[5] << 8) | localData[4]);
+
+ switch(range)
+ {
+ case DPS_2000:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_1000:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_500:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_250:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_125:
+ data.xAxis.scaled = (data.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ data.yAxis.scaled = (data.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ data.zAxis.scaled = (data.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ break;
+ }
+
+ sensorTime.raw = ((localData[14] << 16) | (localData[13] << 8) |
+ localData[12]);
+ sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
+ }
+
+ return rtnVal;
+}
+
+
+//*****************************************************************************
+int32_t BMI160::getGyroAccXYZandSensorTime(SensorData &accData,
+ SensorData &gyroData,
+ SensorTime &sensorTime,
+ AccRange accRange,
+ GyroRange gyroRange)
+{
+ uint8_t localData[16];
+ int32_t rtnVal = readBlock(DATA_8, SENSORTIME_2, localData);
+ if(rtnVal == RTN_NO_ERROR)
+ {
+ gyroData.xAxis.raw = ((localData[1] << 8) | localData[0]);
+ gyroData.yAxis.raw = ((localData[3] << 8) | localData[2]);
+ gyroData.zAxis.raw = ((localData[5] << 8) | localData[4]);
+
+ accData.xAxis.raw = ((localData[7] << 8) | localData[6]);
+ accData.yAxis.raw = ((localData[9] << 8) | localData[8]);
+ accData.zAxis.raw = ((localData[11] << 8) | localData[10]);
+
+ switch(gyroRange)
+ {
+ case DPS_2000:
+ gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_2000_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_1000:
+ gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_1000_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_500:
+ gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_500_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_250:
+ gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_250_DPS_LSB_PER_DPS);
+ break;
+
+ case DPS_125:
+ gyroData.xAxis.scaled = (gyroData.xAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ gyroData.yAxis.scaled = (gyroData.yAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ gyroData.zAxis.scaled = (gyroData.zAxis.raw/SENS_125_DPS_LSB_PER_DPS);
+ break;
+ }
+
+ switch(accRange)
+ {
+ case SENS_2G:
+ accData.xAxis.scaled = (accData.xAxis.raw/SENS_2G_LSB_PER_G);
+ accData.yAxis.scaled = (accData.yAxis.raw/SENS_2G_LSB_PER_G);
+ accData.zAxis.scaled = (accData.zAxis.raw/SENS_2G_LSB_PER_G);
+ break;
+
+ case SENS_4G:
+ accData.xAxis.scaled = (accData.xAxis.raw/SENS_4G_LSB_PER_G);
+ accData.yAxis.scaled = (accData.yAxis.raw/SENS_4G_LSB_PER_G);
+ accData.zAxis.scaled = (accData.zAxis.raw/SENS_4G_LSB_PER_G);
+ break;
+
+ case SENS_8G:
+ accData.xAxis.scaled = (accData.xAxis.raw/SENS_8G_LSB_PER_G);
+ accData.yAxis.scaled = (accData.yAxis.raw/SENS_8G_LSB_PER_G);
+ accData.zAxis.scaled = (accData.zAxis.raw/SENS_8G_LSB_PER_G);
+ break;
+
+ case SENS_16G:
+ accData.xAxis.scaled = (accData.xAxis.raw/SENS_16G_LSB_PER_G);
+ accData.yAxis.scaled = (accData.yAxis.raw/SENS_16G_LSB_PER_G);
+ accData.zAxis.scaled = (accData.zAxis.raw/SENS_16G_LSB_PER_G);
+ break;
+ }
+
+ sensorTime.raw = ((localData[14] << 16) | (localData[13] << 8) |
+ localData[12]);
+ sensorTime.seconds = (sensorTime.raw * SENSOR_TIME_LSB);
+ }
+
+ return rtnVal;
+}
+
+
+//*****************************************************************************
int32_t BMI160::getSensorTime(SensorTime &sensorTime)
{
uint8_t localData[3];