bmi160: add C driver and example. Add SPI support, other improvments.
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
595
src/bmi160/bmi160.c
Normal file
595
src/bmi160/bmi160.c
Normal file
@@ -0,0 +1,595 @@
|
||||
/*
|
||||
* Author: Jon Trulson <jtrulson@ics.com>
|
||||
* Copyright (c) 2016 Intel Corporation.
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining
|
||||
* a copy of this software and associated documentation files (the
|
||||
* "Software"), to deal in the Software without restriction, including
|
||||
* without limitation the rights to use, copy, modify, merge, publish,
|
||||
* distribute, sublicense, and/or sell copies of the Software, and to
|
||||
* permit persons to whom the Software is furnished to do so, subject to
|
||||
* the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be
|
||||
* included in all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
||||
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
|
||||
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
||||
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
|
||||
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
|
||||
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include <mraa/i2c.h>
|
||||
#include <mraa/spi.h>
|
||||
#include <mraa/gpio.h>
|
||||
|
||||
#include "bmi160.h"
|
||||
|
||||
#include <upm_utilities.h>
|
||||
|
||||
// we have to do it the old skool way. Note, this also means that
|
||||
// only one instance of the bmi160 driver can be active at a time.
|
||||
static mraa_i2c_context i2cContext = NULL;
|
||||
static mraa_spi_context spiContext = NULL;
|
||||
/* this is used for chip-select when using SPI */
|
||||
static mraa_gpio_context gpioContext = NULL;
|
||||
|
||||
// whether we are doing I2C or SPI
|
||||
static bool isSPI = false;
|
||||
|
||||
// Our bmi160 info structure
|
||||
struct bmi160_t s_bmi160;
|
||||
|
||||
// For SPI, these are our CS on/off functions, if needed
|
||||
static void bmi160_cs_on()
|
||||
{
|
||||
if (gpioContext)
|
||||
mraa_gpio_write(gpioContext, 0);
|
||||
}
|
||||
|
||||
static void bmi160_cs_off()
|
||||
{
|
||||
if (gpioContext)
|
||||
mraa_gpio_write(gpioContext, 1);
|
||||
}
|
||||
|
||||
// i2c bus read and write functions for use with the bmi driver code
|
||||
s8 bmi160_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
|
||||
{
|
||||
printf("%s: ENTER reg %02x cnt %02x.\n", __FUNCTION__,
|
||||
reg_addr, cnt);
|
||||
|
||||
if (isSPI)
|
||||
{
|
||||
if (!spiContext)
|
||||
{
|
||||
printf("%s: spiContext is NULL.\n", __FUNCTION__);
|
||||
return 1;
|
||||
}
|
||||
|
||||
reg_addr |= 0x80; // needed for read
|
||||
|
||||
uint8_t sbuf[cnt + 1];
|
||||
memset((char *)sbuf, 0, cnt + 1);
|
||||
sbuf[0] = reg_addr;
|
||||
|
||||
bmi160_cs_on();
|
||||
|
||||
if (mraa_spi_transfer_buf(spiContext, sbuf, sbuf, cnt + 1))
|
||||
{
|
||||
bmi160_cs_off();
|
||||
printf("%s: mraa_spi_transfer_buf() failed.\n", __FUNCTION__);
|
||||
return 1;
|
||||
}
|
||||
bmi160_cs_off();
|
||||
|
||||
// now copy it into user buffer
|
||||
int i;
|
||||
for (i=0; i<cnt; i++)
|
||||
reg_data[i] = sbuf[i + 1];
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// doing I2C
|
||||
|
||||
if (!i2cContext)
|
||||
{
|
||||
printf("%s: i2cContext is NULL.\n", __FUNCTION__);
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (mraa_i2c_read_bytes_data(i2cContext, reg_addr, reg_data, cnt) < 0)
|
||||
{
|
||||
printf("%s: mraa_i2c_read_bytes() failed.\n", __FUNCTION__);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
s8 bmi160_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
|
||||
{
|
||||
printf("%s: ENTER reg %02x cnt %02x.\n", __FUNCTION__,
|
||||
reg_addr, cnt);
|
||||
|
||||
if (isSPI)
|
||||
{
|
||||
if (!spiContext)
|
||||
{
|
||||
printf("%s: spiContext is NULL.\n", __FUNCTION__);
|
||||
return 1;
|
||||
}
|
||||
|
||||
reg_addr &= 0x7f; // mask off 0x80 for writing
|
||||
|
||||
uint8_t sbuf[cnt + 1];
|
||||
memset((char *)sbuf, 0, cnt + 1);
|
||||
sbuf[0] = reg_addr;
|
||||
|
||||
// copy in the data to write...
|
||||
int i;
|
||||
for (i=0; i<cnt; i++)
|
||||
sbuf[i + 1] = reg_data[i];
|
||||
|
||||
bmi160_cs_on();
|
||||
|
||||
if (mraa_spi_transfer_buf(spiContext, sbuf, sbuf, cnt + 1))
|
||||
{
|
||||
bmi160_cs_off();
|
||||
printf("%s: mraa_spi_transfer_buf() failed.\n", __FUNCTION__);
|
||||
return 1;
|
||||
}
|
||||
bmi160_cs_off();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// I2C...
|
||||
if (!i2cContext)
|
||||
{
|
||||
printf("%s: i2cContext is NULL.\n", __FUNCTION__);
|
||||
return 1;
|
||||
}
|
||||
|
||||
uint8_t buffer[cnt + 1];
|
||||
|
||||
buffer[0] = reg_addr;
|
||||
int i;
|
||||
for (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)
|
||||
{
|
||||
printf("%s: mraa_i2c_write() failed.\n", __FUNCTION__);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// delay for some milliseconds
|
||||
void bmi160_delay_ms(u32 msek)
|
||||
{
|
||||
upm_delay_ms(msek);
|
||||
}
|
||||
|
||||
|
||||
bmi160_context bmi160_init(unsigned int bus, int address, int cs_pin,
|
||||
bool enable_mag)
|
||||
{
|
||||
bmi160_context dev =
|
||||
(bmi160_context)malloc(sizeof(struct _bmi160_context));
|
||||
|
||||
if (!dev)
|
||||
return NULL;
|
||||
|
||||
// zero out context
|
||||
memset((void *)dev, 0, sizeof(struct _bmi160_context));
|
||||
|
||||
// make sure MRAA is initialized
|
||||
int mraa_rv;
|
||||
if ((mraa_rv = mraa_init()) != MRAA_SUCCESS)
|
||||
{
|
||||
printf("%s: mraa_init() failed (%d).\n", __FUNCTION__, mraa_rv);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (address > 0)
|
||||
{
|
||||
// we are doing I2C
|
||||
isSPI = false;
|
||||
|
||||
if (!(i2cContext = mraa_i2c_init(bus)))
|
||||
{
|
||||
printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (mraa_i2c_address(i2cContext, address) != MRAA_SUCCESS)
|
||||
{
|
||||
printf("%s: mraa_i2c_address() failed.\n", __FUNCTION__);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// we are doing SPI
|
||||
isSPI = true;
|
||||
printf("DOING SPI!\n");//FIXME
|
||||
if (!(spiContext = mraa_spi_init(bus)))
|
||||
{
|
||||
printf("%s: mraa_spi_init() failed.\n", __FUNCTION__);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Only create cs context if we are actually using a valid pin.
|
||||
// A hardware controlled pin should specify cs as -1.
|
||||
if (cs_pin >= 0)
|
||||
{
|
||||
if (!(gpioContext = mraa_gpio_init(cs_pin)))
|
||||
{
|
||||
printf("%s: mraa_gpio_init() failed.\n", __FUNCTION__);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
mraa_gpio_dir(gpioContext, MRAA_GPIO_OUT);
|
||||
bmi160_cs_off();
|
||||
}
|
||||
|
||||
if (mraa_spi_mode(spiContext, MRAA_SPI_MODE0))
|
||||
{
|
||||
printf("%s: mraa_spi_mode() failed.\n", __FUNCTION__);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (mraa_spi_frequency(spiContext, 5000000))
|
||||
{
|
||||
printf("%s: mraa_spi_frequency() failed.\n", __FUNCTION__);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
// init the driver interface functions
|
||||
s_bmi160.bus_write = bmi160_bus_write;
|
||||
s_bmi160.bus_read = bmi160_bus_read;
|
||||
s_bmi160.delay_msec = bmi160_delay_ms;
|
||||
if (isSPI)
|
||||
s_bmi160.dev_addr = 0;
|
||||
else
|
||||
s_bmi160.dev_addr = address & 0xff;
|
||||
|
||||
// Init our driver interface pointers
|
||||
if (bmi160_init_bus(&s_bmi160))
|
||||
{
|
||||
printf("%s: bmi160_bus_init() failed.\n", __FUNCTION__);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// bmi160_init_bus will read the chip Id and deposit into our
|
||||
// interface struct. So, check it out and make sure it's correct.
|
||||
printf("%s: CHIP_ID = %02x\n", __FUNCTION__, s_bmi160.chip_id);
|
||||
if (s_bmi160.chip_id != BMI160_CHIP_ID)
|
||||
{
|
||||
printf("%s: Error: expected chip id %02x, but got %02x.\n",
|
||||
__FUNCTION__, BMI160_CHIP_ID, s_bmi160.chip_id);
|
||||
bmi160_close(dev);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
dev->accelScale = 1.0;
|
||||
dev->gyroScale = 1.0;
|
||||
dev->magEnabled = false;
|
||||
|
||||
// This should be interesting...
|
||||
const u32 C_BMI160_THIRTY_U8X = 30;
|
||||
|
||||
bmi160_enable_magnetometer(dev, enable_mag);
|
||||
|
||||
/* 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);
|
||||
|
||||
bmi160_set_accelerometer_scale(dev, BMI160_ACC_RANGE_2G);
|
||||
bmi160_set_gyroscope_scale(dev, BMI160_GYRO_RANGE_125);
|
||||
|
||||
return dev;
|
||||
}
|
||||
|
||||
void bmi160_close(bmi160_context dev)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
if (i2cContext)
|
||||
mraa_i2c_stop(i2cContext);
|
||||
i2cContext = NULL;
|
||||
|
||||
if (spiContext)
|
||||
mraa_spi_stop(spiContext);
|
||||
spiContext = NULL;
|
||||
|
||||
if (gpioContext)
|
||||
mraa_gpio_close(gpioContext);
|
||||
gpioContext = NULL;
|
||||
|
||||
free(dev);
|
||||
}
|
||||
|
||||
void bmi160_update(const bmi160_context dev)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
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 (dev->magEnabled)
|
||||
bmi160_bmm150_mag_compensate_xyz(&magxyz);
|
||||
|
||||
// read the sensor time
|
||||
u32 v_sensor_time;
|
||||
bmi160_get_sensor_time(&v_sensor_time);
|
||||
dev->sensorTime = (unsigned int)v_sensor_time;
|
||||
|
||||
dev->accelX = (float)accelxyz.x;
|
||||
dev->accelY = (float)accelxyz.y;
|
||||
dev->accelZ = (float)accelxyz.z;
|
||||
|
||||
dev->gyroX = (float)gyroxyz.x;
|
||||
dev->gyroY = (float)gyroxyz.y;
|
||||
dev->gyroZ = (float)gyroxyz.z;
|
||||
|
||||
if (dev->magEnabled)
|
||||
{
|
||||
dev->magX = (float)magxyz.x;
|
||||
dev->magY = (float)magxyz.y;
|
||||
dev->magZ = (float)magxyz.z;
|
||||
}
|
||||
}
|
||||
|
||||
void bmi160_set_accelerometer_scale(const bmi160_context dev,
|
||||
BMI160_ACC_RANGE_T scale)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
s8 v_range = BMI160_ACCEL_RANGE_2G;
|
||||
// store scaling factor
|
||||
|
||||
switch (scale)
|
||||
{
|
||||
case BMI160_ACC_RANGE_2G:
|
||||
v_range = BMI160_ACCEL_RANGE_2G;
|
||||
dev->accelScale = 16384.0;
|
||||
break;
|
||||
|
||||
case BMI160_ACC_RANGE_4G:
|
||||
v_range = BMI160_ACCEL_RANGE_4G;
|
||||
dev->accelScale = 8192.0;
|
||||
break;
|
||||
|
||||
case BMI160_ACC_RANGE_8G:
|
||||
v_range = BMI160_ACCEL_RANGE_8G;
|
||||
dev->accelScale = 4096.0;
|
||||
break;
|
||||
|
||||
case BMI160_ACC_RANGE_16G:
|
||||
v_range = BMI160_ACCEL_RANGE_16G;
|
||||
dev->accelScale = 2048.0;
|
||||
break;
|
||||
|
||||
default: // should never occur, but...
|
||||
dev->accelScale = 1.0; // set a safe, though incorrect value
|
||||
printf("%s: internal error, unsupported scale.\n", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
|
||||
bmi160_set_accel_range(v_range);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void bmi160_set_gyroscope_scale(const bmi160_context dev,
|
||||
BMI160_GYRO_RANGE_T scale)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
u8 v_range = BMI160_GYRO_RANGE_2000_DEG_SEC;
|
||||
|
||||
// store scaling factor
|
||||
|
||||
switch (scale)
|
||||
{
|
||||
case BMI160_GYRO_RANGE_125:
|
||||
v_range = BMI160_GYRO_RANGE_125_DEG_SEC;
|
||||
dev->gyroScale = 262.4;
|
||||
break;
|
||||
|
||||
case BMI160_GYRO_RANGE_250:
|
||||
v_range = BMI160_GYRO_RANGE_250_DEG_SEC;
|
||||
dev->gyroScale = 131.2;
|
||||
break;
|
||||
|
||||
case BMI160_GYRO_RANGE_500:
|
||||
v_range = BMI160_GYRO_RANGE_500_DEG_SEC;
|
||||
dev->gyroScale = 65.6;
|
||||
break;
|
||||
|
||||
case BMI160_GYRO_RANGE_1000:
|
||||
v_range = BMI160_GYRO_RANGE_1000_DEG_SEC;
|
||||
dev->gyroScale = 32.8;
|
||||
break;
|
||||
|
||||
case BMI160_GYRO_RANGE_2000:
|
||||
v_range = BMI160_GYRO_RANGE_2000_DEG_SEC;
|
||||
dev->gyroScale = 16.4;
|
||||
break;
|
||||
|
||||
default: // should never occur, but...
|
||||
dev->gyroScale = 1.0; // set a safe, though incorrect value
|
||||
printf("%s: internal error, unsupported scale.\n", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
|
||||
bmi160_set_gyro_range(v_range);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void bmi160_get_accelerometer(const bmi160_context dev, float *x, float *y,
|
||||
float *z)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
if (x)
|
||||
*x = dev->accelX / dev->accelScale;
|
||||
|
||||
if (y)
|
||||
*y = dev->accelY / dev->accelScale;
|
||||
|
||||
if (z)
|
||||
*z = dev->accelZ / dev->accelScale;
|
||||
}
|
||||
|
||||
void bmi160_get_gyroscope(const bmi160_context dev, float *x, float *y,
|
||||
float *z)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
if (x)
|
||||
*x = dev->gyroX / dev->gyroScale;
|
||||
|
||||
if (y)
|
||||
*y = dev->gyroY / dev->gyroScale;
|
||||
|
||||
if (z)
|
||||
*z = dev->gyroZ / dev->gyroScale;
|
||||
}
|
||||
|
||||
void bmi160_get_magnetometer(const bmi160_context dev, float *x, float *y,
|
||||
float *z)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
if (x)
|
||||
*x = dev->magX;
|
||||
|
||||
if (y)
|
||||
*y = dev->magY;
|
||||
|
||||
if (z)
|
||||
*z = dev->magZ;
|
||||
}
|
||||
|
||||
#if 0
|
||||
float *bmi160_get_ccelerometer()
|
||||
{
|
||||
float *values = new float[3]; // x, y, and then z
|
||||
|
||||
getAccelerometer(&values[0], &values[1], &values[2]);
|
||||
|
||||
return values;
|
||||
}
|
||||
|
||||
float *bmi160_getGyroscope()
|
||||
{
|
||||
float *values = new float[3]; // x, y, and then z
|
||||
|
||||
getGyroscope(&values[0], &values[1], &values[2]);
|
||||
|
||||
return values;
|
||||
}
|
||||
|
||||
float *bmi160_getMagnetometer()
|
||||
{
|
||||
float *values = new float[3]; // x, y, and then z
|
||||
|
||||
getMagnetometer(&values[0], &values[1], &values[2]);
|
||||
|
||||
return values;
|
||||
}
|
||||
#endif
|
||||
|
||||
void bmi160_enable_magnetometer(const bmi160_context dev, bool enable)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
// 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);
|
||||
|
||||
dev->magEnabled = false;
|
||||
dev->magX = 0;
|
||||
dev->magY = 0;
|
||||
dev->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);
|
||||
|
||||
dev->magEnabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int bmi160_get_time(const bmi160_context dev)
|
||||
{
|
||||
assert(dev != NULL);
|
||||
|
||||
return dev->sensorTime;
|
||||
}
|
||||
Reference in New Issue
Block a user