Raspberries/upm
Public Access
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bmi160: add C driver and example. Add SPI support, other improvments.

Browse Source 
This adds SPI support to the BMI160, as well as a C driver and a C
example.  In addition, some changes were made to more properly detect
and handle errors.

Functions supplied by the bosch_bmi160 driver source code is also
exported and made available to callers who want more than what the
basic driver support.  Bus access methods (I2C and SPI) are also now
exposed to both C and C++.

Signed-off-by: Jon Trulson <jtrulson@ics.com>
This commit is contained in:
Jon Trulson
2016-09-27 16:39:09 -06:00
parent e7c80217c2
commit 0086626173
12 changed files with 1203 additions and 547 deletions
Download Patch File Download Diff File Expand all files Collapse all files

389
src/bmi160/bmi160.cxx
View File

@@ -27,354 +27,57 @@
#include <stdexcept>
#include <string>
// we have to do it the old skool way
#include <mraa/i2c.h>
#include "bmi160.hpp"
extern "C" {
#include "bosch_bmi160.h"
}
// We do not need this define anyway. It conflicts with mraa::SUCCESS.
#undef SUCCESS
using namespace upm;
using namespace std;
static mraa_i2c_context i2cContext = NULL;
// Our bmi160 info structure
struct bmi160_t s_bmi160;
// bus read and write functions for use with the bmi driver code
s8 bmi160_i2c_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
BMI160::BMI160(int bus, int address, int csPin, bool enableMag) :
m_bmi160(bmi160_init(bus, address, csPin, enableMag))
{
if (!i2cContext)
{
throw std::runtime_error(std::string(__FUNCTION__) +
": i2c context is NULL");
}
int retries = 10;
// There seems to be some occasional flakyness with reads when
// moving the sensor around
while (retries >= 0)
{
int rv = mraa_i2c_read_bytes_data(i2cContext, reg_addr, reg_data, cnt);
if (rv < 0)
{
usleep(100000);
retries--;
}
else
return 0;
}
throw std::runtime_error(std::string(__FUNCTION__) +
": mraa_i2c_read_bytes_data() failed");
return 0;
}
s8 bmi160_i2c_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
{
if (!i2cContext)
{
throw std::runtime_error(std::string(__FUNCTION__) +
": i2c context is NULL");
}
// FIXME fprintf(stderr, "%s: %02x: cnt %d\n", __FUNCTION__, reg_addr, cnt);
uint8_t buffer[cnt + 1];
buffer[0] = reg_addr;
for (int i=0; i<cnt; i++)
buffer[i+1] = reg_data[i];
mraa_result_t rv = mraa_i2c_write(i2cContext, buffer, cnt+1);
if (rv != MRAA_SUCCESS)
{
throw std::runtime_error(std::string(__FUNCTION__) +
": mraa_i2c_write() failed");
}
return 0;
}
// delay for some milliseconds
void bmi160_delay_ms(u32 msek)
{
usleep(msek * 1000);
}
BMI160::BMI160(int bus, uint8_t address)
{
m_addr = address;
// We need to use the C MRAA interface to avoid issue with C++ <-> C
// calling convention issues, also we need a global
// mraa_i2c_context
if (!(i2cContext = mraa_i2c_init(bus)))
{
throw std::invalid_argument(std::string(__FUNCTION__) +
": mraa_i2c_init() failed");
}
if (mraa_i2c_address(i2cContext, m_addr) != MRAA_SUCCESS)
{
throw std::runtime_error(std::string(__FUNCTION__) +
": mraa_i2c_address() failed");
return;
}
// init the driver interface functions
s_bmi160.bus_write = bmi160_i2c_bus_write;
s_bmi160.bus_read = bmi160_i2c_bus_read;
s_bmi160.delay_msec = bmi160_delay_ms;
s_bmi160.dev_addr = m_addr;
// Init our driver interface pointers
bmi160_init(&s_bmi160);
m_accelX = 0.0;
m_accelY = 0.0;
m_accelZ = 0.0;
m_gyroX = 0.0;
m_gyroY = 0.0;
m_gyroZ = 0.0;
m_magX = 0.0;
m_magY = 0.0;
m_magZ = 0.0;
m_accelScale = 1.0;
m_gyroScale = 1.0;
m_magEnabled = false;
if (!init())
{
throw std::runtime_error(std::string(__FUNCTION__) +
": init() failed");
}
if (!m_bmi160)
throw std::runtime_error(string(__FUNCTION__)
+ ": bmi160_init() failed");
}
BMI160::~BMI160()
{
mraa_i2c_stop(i2cContext);
i2cContext = NULL;
bmi160_close(m_bmi160);
}
bool BMI160::init()
{
// This should be interesting...
const u32 C_BMI160_THIRTY_U8X = 30;
enableMagnetometer(true);
/*Set the accel mode as Normal write in the register 0x7E*/
bmi160_set_command_register(ACCEL_MODE_NORMAL);
/* bmi160_delay_ms in ms*/
bmi160_delay_ms(C_BMI160_THIRTY_U8X);
/*Set the gyro mode as Normal write in the register 0x7E*/
bmi160_set_command_register(GYRO_MODE_NORMAL);
/* bmi160_delay_ms in ms*/
bmi160_delay_ms(C_BMI160_THIRTY_U8X);
/* Set the accel bandwidth as OSRS4 */
bmi160_set_accel_bw(BMI160_ACCEL_OSR4_AVG1);
bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
/* Set the gryo bandwidth as Normal */
bmi160_set_gyro_bw(BMI160_GYRO_NORMAL_MODE);
bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
/* set gyro data rate as 200Hz*/
bmi160_set_gyro_output_data_rate(BMI160_GYRO_OUTPUT_DATA_RATE_200HZ);
bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
/* set accel data rate as 200Hz*/
bmi160_set_accel_output_data_rate(BMI160_ACCEL_OUTPUT_DATA_RATE_200HZ,
BMI160_ACCEL_OSR4_AVG1);
bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
setAccelerometerScale(ACCEL_RANGE_2G);
setGyroscopeScale(GYRO_RANGE_125);
return true;
}
void BMI160::update()
{
struct bmi160_gyro_t gyroxyz;
struct bmi160_accel_t accelxyz;
struct bmi160_mag_xyz_s32_t magxyz;
// read gyro data
bmi160_read_gyro_xyz(&gyroxyz);
// read accel data
bmi160_read_accel_xyz(&accelxyz);
// read mag data
if (m_magEnabled)
bmi160_bmm150_mag_compensate_xyz(&magxyz);
// read the sensor time
u32 v_sensor_time;
bmi160_get_sensor_time(&v_sensor_time);
m_sensorTime = (unsigned int)v_sensor_time;
m_accelX = float(accelxyz.x);
m_accelY = float(accelxyz.y);
m_accelZ = float(accelxyz.z);
m_gyroX = float(gyroxyz.x);
m_gyroY = float(gyroxyz.y);
m_gyroZ = float(gyroxyz.z);
if (m_magEnabled)
{
m_magX = float(magxyz.x);
m_magY = float(magxyz.y);
m_magZ = float(magxyz.z);
}
bmi160_update(m_bmi160);
}
void BMI160::setAccelerometerScale(ACCEL_RANGE_T scale)
void BMI160::setAccelerometerScale(BMI160_ACC_RANGE_T scale)
{
s8 v_range = BMI160_ACCEL_RANGE_2G;
// store scaling factor
switch (scale)
{
case ACCEL_RANGE_2G:
v_range = BMI160_ACCEL_RANGE_2G;
m_accelScale = 16384.0;
break;
case ACCEL_RANGE_4G:
v_range = BMI160_ACCEL_RANGE_4G;
m_accelScale = 8192.0;
break;
case ACCEL_RANGE_8G:
v_range = BMI160_ACCEL_RANGE_8G;
m_accelScale = 4096.0;
break;
case ACCEL_RANGE_16G:
v_range = BMI160_ACCEL_RANGE_16G;
m_accelScale = 2048.0;
break;
default: // should never occur, but...
m_accelScale = 1.0; // set a safe, though incorrect value
throw std::logic_error(string(__FUNCTION__) +
": internal error, unsupported scale");
break;
}
bmi160_set_accel_range(v_range);
return;
bmi160_set_accelerometer_scale(m_bmi160, scale);
}
void BMI160::setGyroscopeScale(GYRO_RANGE_T scale)
void BMI160::setGyroscopeScale(BMI160_GYRO_RANGE_T scale)
{
u8 v_range = BMI160_GYRO_RANGE_2000_DEG_SEC;
// store scaling factor
switch (scale)
{
case GYRO_RANGE_125:
v_range = BMI160_GYRO_RANGE_125_DEG_SEC;
m_gyroScale = 262.4;
break;
case GYRO_RANGE_250:
v_range = BMI160_GYRO_RANGE_250_DEG_SEC;
m_gyroScale = 131.2;
break;
case GYRO_RANGE_500:
v_range = BMI160_GYRO_RANGE_500_DEG_SEC;
m_gyroScale = 65.6;
break;
case GYRO_RANGE_1000:
v_range = BMI160_GYRO_RANGE_1000_DEG_SEC;
m_gyroScale = 32.8;
break;
case GYRO_RANGE_2000:
v_range = BMI160_GYRO_RANGE_2000_DEG_SEC;
m_gyroScale = 16.4;
break;
default: // should never occur, but...
m_gyroScale = 1.0; // set a safe, though incorrect value
throw std::logic_error(string(__FUNCTION__) +
": internal error, unsupported scale");
break;
}
bmi160_set_gyro_range(v_range);
return;
bmi160_set_gyroscope_scale(m_bmi160, scale);
}
void BMI160::getAccelerometer(float *x, float *y, float *z)
{
if (x)
*x = m_accelX / m_accelScale;
if (y)
*y = m_accelY / m_accelScale;
if (z)
*z = m_accelZ / m_accelScale;
bmi160_get_accelerometer(m_bmi160, x, y, z);
}
void BMI160::getGyroscope(float *x, float *y, float *z)
{
if (x)
*x = m_gyroX / m_gyroScale;
if (y)
*y = m_gyroY / m_gyroScale;
if (z)
*z = m_gyroZ / m_gyroScale;
bmi160_get_gyroscope(m_bmi160, x, y, z);
}
void BMI160::getMagnetometer(float *x, float *y, float *z)
{
if (x)
*x = m_magX;
if (y)
*y = m_magY;
if (z)
*z = m_magZ;
bmi160_get_magnetometer(m_bmi160, x, y, z);
}
float *BMI160::getAccelerometer()
{
float *values = new float[3]; // x, y, and then z
static float values[3]; // x, y, and then z
getAccelerometer(&values[0], &values[1], &values[2]);
@@ -383,7 +86,7 @@ float *BMI160::getAccelerometer()
float *BMI160::getGyroscope()
{
float *values = new float[3]; // x, y, and then z
static float values[3]; // x, y, and then z
getGyroscope(&values[0], &values[1], &values[2]);
@@ -392,7 +95,7 @@ float *BMI160::getGyroscope()
float *BMI160::getMagnetometer()
{
float *values = new float[3]; // x, y, and then z
static float values[3]; // x, y, and then z
getMagnetometer(&values[0], &values[1], &values[2]);
@@ -401,33 +104,41 @@ float *BMI160::getMagnetometer()
void BMI160::enableMagnetometer(bool enable)
{
// butchered from support example
if (!enable)
{
bmi160_set_bmm150_mag_and_secondary_if_power_mode(MAG_SUSPEND_MODE);
bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
bmi160_set_if_mode(0x00);
bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
m_magEnabled = false;
m_magX = 0;
m_magY = 0;
m_magZ = 0;
}
else
{
u8 v_bmm_chip_id_u8 = BMI160_INIT_VALUE;
/* Init the magnetometer */
bmi160_bmm150_mag_interface_init(&v_bmm_chip_id_u8);
/* bmi160_delay_ms in ms*/
bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
m_magEnabled = true;
}
bmi160_enable_magnetometer(m_bmi160, enable);
}
unsigned int BMI160::getSensorTime()
{
return m_sensorTime;
return bmi160_get_time(m_bmi160);
}
string BMI160::busRead(int addr, int reg, int len)
{
u8 dev_addr = (u8)(addr & 0xff);
u8 reg_addr = (u8)(reg & 0xff);
u8 cnt = (u8)(len & 0xff);
u8 *data = new u8[cnt];
if (bmi160_bus_read(dev_addr, reg_addr, data, cnt))
{
delete [] data;
throw std::runtime_error(string(__FUNCTION__)
+ ": bmi160_bus_read() failed");
}
string dataStr((char *)data, cnt);
delete [] data;
return dataStr;
}
void BMI160::busWrite(int addr, int reg, string data)
{
u8 dev_addr = (u8)(addr & 0xff);
u8 reg_addr = (u8)(reg & 0xff);
if (bmi160_bus_write(dev_addr, reg_addr, (u8 *)data.data(), data.size()))
throw std::runtime_error(string(__FUNCTION__)
+ ": bmi160_bus_write() failed");
}