Skip to content

Instantly share code, notes, and snippets.

@lolzballs
Created September 16, 2017 07:11
Show Gist options
  • Save lolzballs/f18f5eb50d4a6e4612bb9725d712300d to your computer and use it in GitHub Desktop.
Save lolzballs/f18f5eb50d4a6e4612bb9725d712300d to your computer and use it in GitHub Desktop.
/*
****************************************************************************
* Copyright (C) 2015 - 2016 Bosch Sensortec GmbH
*
* bno055_support.c
* Date: 2016/03/14
* Revision: 1.0.4 $
*
* Usage: Sensor Driver support file for BNO055 sensor
*
****************************************************************************
* License:
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* Neither the name of the copyright holder nor the names of the
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER
* OR CONTRIBUTORS BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
* OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
*
* The information provided is believed to be accurate and reliable.
* The copyright holder assumes no responsibility
* for the consequences of use
* of such information nor for any infringement of patents or
* other rights of third parties which may result from its use.
* No license is granted by implication or otherwise under any patent or
* patent rights of the copyright holder.
**************************************************************************/
/*---------------------------------------------------------------------------*
Includes
*---------------------------------------------------------------------------*/
#include "bno055.h"
/*----------------------------------------------------------------------------*
* The following APIs are used for reading and writing of
* sensor data using I2C communication
*----------------------------------------------------------------------------*/
#ifdef BNO055_API
#define BNO055_I2C_BUS_WRITE_ARRAY_INDEX ((u8)1)
/* \Brief: The API is used as I2C bus read
* \Return : Status of the I2C read
* \param dev_addr : The device address of the sensor
* \param reg_addr : Address of the first register,
* will data is going to be read
* \param reg_data : This data read from the sensor,
* which is hold in an array
* \param cnt : The no of byte of data to be read
*/
s8 BNO055_I2C_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt);
/* \Brief: The API is used as SPI bus write
* \Return : Status of the SPI write
* \param dev_addr : The device address of the sensor
* \param reg_addr : Address of the first register,
* will data is going to be written
* \param reg_data : It is a value hold in the array,
* will be used for write the value into the register
* \param cnt : The no of byte of data to be write
*/
s8 BNO055_I2C_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt);
/*
* \Brief: I2C init routine
*/
s8 I2C_routine(void);
#endif
/********************End of I2C APIs declarations***********************/
/* Brief : The delay routine
* \param : delay in ms
*/
void BNO055_delay_msek(u32 msek);
/* This API is an example for reading sensor data
* \param: None
* \return: communication result
*/
s32 bno055_data_readout_template(void);
/*----------------------------------------------------------------------------*
* struct bno055_t parameters can be accessed by using BNO055
* BNO055_t having the following parameters
* Bus write function pointer: BNO055_WR_FUNC_PTR
* Bus read function pointer: BNO055_RD_FUNC_PTR
* Burst read function pointer: BNO055_BRD_FUNC_PTR
* Delay function pointer: delay_msec
* I2C address: dev_addr
* Chip id of the sensor: chip_id
*---------------------------------------------------------------------------*/
struct bno055_t bno055;
/* This API is an example for reading sensor data
* \param: None
* \return: communication result
*/
s32 bno055_data_readout_template(void)
{
/* Variable used to return value of
communication routine*/
s32 comres = BNO055_ERROR;
/* variable used to set the power mode of the sensor*/
u8 power_mode = BNO055_INIT_VALUE;
/*********read raw accel data***********/
/* variable used to read the accel x data */
s16 accel_datax = BNO055_INIT_VALUE;
/* variable used to read the accel y data */
s16 accel_datay = BNO055_INIT_VALUE;
/* variable used to read the accel z data */
s16 accel_dataz = BNO055_INIT_VALUE;
/* variable used to read the accel xyz data */
struct bno055_accel_t accel_xyz;
/*********read raw mag data***********/
/* variable used to read the mag x data */
s16 mag_datax = BNO055_INIT_VALUE;
/* variable used to read the mag y data */
s16 mag_datay = BNO055_INIT_VALUE;
/* variable used to read the mag z data */
s16 mag_dataz = BNO055_INIT_VALUE;
/* structure used to read the mag xyz data */
struct bno055_mag_t mag_xyz;
/***********read raw gyro data***********/
/* variable used to read the gyro x data */
s16 gyro_datax = BNO055_INIT_VALUE;
/* variable used to read the gyro y data */
s16 gyro_datay = BNO055_INIT_VALUE;
/* variable used to read the gyro z data */
s16 gyro_dataz = BNO055_INIT_VALUE;
/* structure used to read the gyro xyz data */
struct bno055_gyro_t gyro_xyz;
/*************read raw Euler data************/
/* variable used to read the euler h data */
s16 euler_data_h = BNO055_INIT_VALUE;
/* variable used to read the euler r data */
s16 euler_data_r = BNO055_INIT_VALUE;
/* variable used to read the euler p data */
s16 euler_data_p = BNO055_INIT_VALUE;
/* structure used to read the euler hrp data */
struct bno055_euler_t euler_hrp;
/************read raw quaternion data**************/
/* variable used to read the quaternion w data */
s16 quaternion_data_w = BNO055_INIT_VALUE;
/* variable used to read the quaternion x data */
s16 quaternion_data_x = BNO055_INIT_VALUE;
/* variable used to read the quaternion y data */
s16 quaternion_data_y = BNO055_INIT_VALUE;
/* variable used to read the quaternion z data */
s16 quaternion_data_z = BNO055_INIT_VALUE;
/* structure used to read the quaternion wxyz data */
struct bno055_quaternion_t quaternion_wxyz;
/************read raw linear acceleration data***********/
/* variable used to read the linear accel x data */
s16 linear_accel_data_x = BNO055_INIT_VALUE;
/* variable used to read the linear accel y data */
s16 linear_accel_data_y = BNO055_INIT_VALUE;
/* variable used to read the linear accel z data */
s16 linear_accel_data_z = BNO055_INIT_VALUE;
/* structure used to read the linear accel xyz data */
struct bno055_linear_accel_t linear_acce_xyz;
/*****************read raw gravity sensor data****************/
/* variable used to read the gravity x data */
s16 gravity_data_x = BNO055_INIT_VALUE;
/* variable used to read the gravity y data */
s16 gravity_data_y = BNO055_INIT_VALUE;
/* variable used to read the gravity z data */
s16 gravity_data_z = BNO055_INIT_VALUE;
/* structure used to read the gravity xyz data */
struct bno055_gravity_t gravity_xyz;
/*************read accel converted data***************/
/* variable used to read the accel x data output as m/s2 or mg */
double d_accel_datax = BNO055_INIT_VALUE;
/* variable used to read the accel y data output as m/s2 or mg */
double d_accel_datay = BNO055_INIT_VALUE;
/* variable used to read the accel z data output as m/s2 or mg */
double d_accel_dataz = BNO055_INIT_VALUE;
/* structure used to read the accel xyz data output as m/s2 or mg */
struct bno055_accel_double_t d_accel_xyz;
/******************read mag converted data********************/
/* variable used to read the mag x data output as uT*/
double d_mag_datax = BNO055_INIT_VALUE;
/* variable used to read the mag y data output as uT*/
double d_mag_datay = BNO055_INIT_VALUE;
/* variable used to read the mag z data output as uT*/
double d_mag_dataz = BNO055_INIT_VALUE;
/* structure used to read the mag xyz data output as uT*/
struct bno055_mag_double_t d_mag_xyz;
/*****************read gyro converted data************************/
/* variable used to read the gyro x data output as dps or rps */
double d_gyro_datax = BNO055_INIT_VALUE;
/* variable used to read the gyro y data output as dps or rps */
double d_gyro_datay = BNO055_INIT_VALUE;
/* variable used to read the gyro z data output as dps or rps */
double d_gyro_dataz = BNO055_INIT_VALUE;
/* structure used to read the gyro xyz data output as dps or rps */
struct bno055_gyro_double_t d_gyro_xyz;
/*******************read euler converted data*******************/
/* variable used to read the euler h data output
as degree or radians*/
double d_euler_data_h = BNO055_INIT_VALUE;
/* variable used to read the euler r data output
as degree or radians*/
double d_euler_data_r = BNO055_INIT_VALUE;
/* variable used to read the euler p data output
as degree or radians*/
double d_euler_data_p = BNO055_INIT_VALUE;
/* structure used to read the euler hrp data output
as as degree or radians */
struct bno055_euler_double_t d_euler_hpr;
/*********read linear acceleration converted data**********/
/* variable used to read the linear accel x data output as m/s2*/
double d_linear_accel_datax = BNO055_INIT_VALUE;
/* variable used to read the linear accel y data output as m/s2*/
double d_linear_accel_datay = BNO055_INIT_VALUE;
/* variable used to read the linear accel z data output as m/s2*/
double d_linear_accel_dataz = BNO055_INIT_VALUE;
/* structure used to read the linear accel xyz data output as m/s2*/
struct bno055_linear_accel_double_t d_linear_accel_xyz;
/********************Gravity converted data**********************/
/* variable used to read the gravity sensor x data output as m/s2*/
double d_gravity_data_x = BNO055_INIT_VALUE;
/* variable used to read the gravity sensor y data output as m/s2*/
double d_gravity_data_y = BNO055_INIT_VALUE;
/* variable used to read the gravity sensor z data output as m/s2*/
double d_gravity_data_z = BNO055_INIT_VALUE;
/* structure used to read the gravity xyz data output as m/s2*/
struct bno055_gravity_double_t d_gravity_xyz;
/*---------------------------------------------------------------------------*
*********************** START INITIALIZATION ************************
*--------------------------------------------------------------------------*/
#ifdef BNO055_API
/* Based on the user need configure I2C interface.
* It is example code to explain how to use the bno055 API*/
I2C_routine();
#endif
/*--------------------------------------------------------------------------*
* This API used to assign the value/reference of
* the following parameters
* I2C address
* Bus Write
* Bus read
* Chip id
* Page id
* Accel revision id
* Mag revision id
* Gyro revision id
* Boot loader revision id
* Software revision id
*-------------------------------------------------------------------------*/
comres = bno055_init(&bno055);
/* For initializing the BNO sensor it is required to the operation mode
of the sensor as NORMAL
Normal mode can set from the register
Page - page0
register - 0x3E
bit positions - 0 and 1*/
power_mode = BNO055_POWER_MODE_NORMAL;
/* set the power mode as NORMAL*/
comres += bno055_set_power_mode(power_mode);
/*----------------------------------------------------------------*
************************* END INITIALIZATION *************************
*-----------------------------------------------------------------*/
/************************* START READ RAW SENSOR DATA****************/
/* Using BNO055 sensor we can read the following sensor data and
virtual sensor data
Sensor data:
Accel
Mag
Gyro
Virtual sensor data
Euler
Quaternion
Linear acceleration
Gravity sensor */
/* For reading sensor raw data it is required to set the
operation modes of the sensor
operation mode can set from the register
page - page0
register - 0x3D
bit - 0 to 3
for sensor data read following operation mode have to set
* SENSOR MODE
*0x01 - BNO055_OPERATION_MODE_ACCONLY
*0x02 - BNO055_OPERATION_MODE_MAGONLY
*0x03 - BNO055_OPERATION_MODE_GYRONLY
*0x04 - BNO055_OPERATION_MODE_ACCMAG
*0x05 - BNO055_OPERATION_MODE_ACCGYRO
*0x06 - BNO055_OPERATION_MODE_MAGGYRO
*0x07 - BNO055_OPERATION_MODE_AMG
based on the user need configure the operation mode*/
comres += bno055_set_operation_mode(BNO055_OPERATION_MODE_AMG);
/* Raw accel X, Y and Z data can read from the register
page - page 0
register - 0x08 to 0x0D*/
comres += bno055_read_accel_x(&accel_datax);
comres += bno055_read_accel_y(&accel_datay);
comres += bno055_read_accel_z(&accel_dataz);
comres += bno055_read_accel_xyz(&accel_xyz);
/* Raw mag X, Y and Z data can read from the register
page - page 0
register - 0x0E to 0x13*/
comres += bno055_read_mag_x(&mag_datax);
comres += bno055_read_mag_y(&mag_datay);
comres += bno055_read_mag_z(&mag_dataz);
comres += bno055_read_mag_xyz(&mag_xyz);
/* Raw gyro X, Y and Z data can read from the register
page - page 0
register - 0x14 to 0x19*/
comres += bno055_read_gyro_x(&gyro_datax);
comres += bno055_read_gyro_y(&gyro_datay);
comres += bno055_read_gyro_z(&gyro_dataz);
comres += bno055_read_gyro_xyz(&gyro_xyz);
/************************* END READ RAW SENSOR DATA****************/
/************************* START READ RAW FUSION DATA ********
For reading fusion data it is required to set the
operation modes of the sensor
operation mode can set from the register
page - page0
register - 0x3D
bit - 0 to 3
for sensor data read following operation mode have to set
*FUSION MODE
*0x08 - BNO055_OPERATION_MODE_IMUPLUS
*0x09 - BNO055_OPERATION_MODE_COMPASS
*0x0A - BNO055_OPERATION_MODE_M4G
*0x0B - BNO055_OPERATION_MODE_NDOF_FMC_OFF
*0x0C - BNO055_OPERATION_MODE_NDOF
based on the user need configure the operation mode*/
comres += bno055_set_operation_mode(BNO055_OPERATION_MODE_NDOF);
/* Raw Euler H, R and P data can read from the register
page - page 0
register - 0x1A to 0x1E */
comres += bno055_read_euler_h(&euler_data_h);
comres += bno055_read_euler_r(&euler_data_r);
comres += bno055_read_euler_p(&euler_data_p);
comres += bno055_read_euler_hrp(&euler_hrp);
/* Raw Quaternion W, X, Y and Z data can read from the register
page - page 0
register - 0x20 to 0x27 */
comres += bno055_read_quaternion_w(&quaternion_data_w);
comres += bno055_read_quaternion_x(&quaternion_data_x);
comres += bno055_read_quaternion_y(&quaternion_data_y);
comres += bno055_read_quaternion_z(&quaternion_data_z);
comres += bno055_read_quaternion_wxyz(&quaternion_wxyz);
/* Raw Linear accel X, Y and Z data can read from the register
page - page 0
register - 0x28 to 0x2D */
comres += bno055_read_linear_accel_x(&linear_accel_data_x);
comres += bno055_read_linear_accel_y(&linear_accel_data_y);
comres += bno055_read_linear_accel_z(&linear_accel_data_z);
comres += bno055_read_linear_accel_xyz(&linear_acce_xyz);
/* Raw Gravity sensor X, Y and Z data can read from the register
page - page 0
register - 0x2E to 0x33 */
comres += bno055_read_gravity_x(&gravity_data_x);
comres += bno055_read_gravity_y(&gravity_data_y);
comres += bno055_read_gravity_z(&gravity_data_z);
comres += bno055_read_gravity_xyz(&gravity_xyz);
/************************* END READ RAW FUSION DATA ************/
/******************START READ CONVERTED SENSOR DATA****************/
/* API used to read accel data output as double - m/s2 and mg
float functions also available in the BNO055 API */
comres += bno055_convert_double_accel_x_msq(&d_accel_datax);
comres += bno055_convert_double_accel_x_mg(&d_accel_datax);
comres += bno055_convert_double_accel_y_msq(&d_accel_datay);
comres += bno055_convert_double_accel_y_mg(&d_accel_datay);
comres += bno055_convert_double_accel_z_msq(&d_accel_dataz);
comres += bno055_convert_double_accel_z_mg(&d_accel_dataz);
comres += bno055_convert_double_accel_xyz_msq(&d_accel_xyz);
comres += bno055_convert_double_accel_xyz_mg(&d_accel_xyz);
/* API used to read mag data output as double - uT(micro Tesla)
float functions also available in the BNO055 API */
comres += bno055_convert_double_mag_x_uT(&d_mag_datax);
comres += bno055_convert_double_mag_y_uT(&d_mag_datay);
comres += bno055_convert_double_mag_z_uT(&d_mag_dataz);
comres += bno055_convert_double_mag_xyz_uT(&d_mag_xyz);
/* API used to read gyro data output as double - dps and rps
float functions also available in the BNO055 API */
comres += bno055_convert_double_gyro_x_dps(&d_gyro_datax);
comres += bno055_convert_double_gyro_y_dps(&d_gyro_datay);
comres += bno055_convert_double_gyro_z_dps(&d_gyro_dataz);
comres += bno055_convert_double_gyro_x_rps(&d_gyro_datax);
comres += bno055_convert_double_gyro_y_rps(&d_gyro_datay);
comres += bno055_convert_double_gyro_z_rps(&d_gyro_dataz);
comres += bno055_convert_double_gyro_xyz_dps(&d_gyro_xyz);
comres += bno055_convert_double_gyro_xyz_rps(&d_gyro_xyz);
/* API used to read Euler data output as double - degree and radians
float functions also available in the BNO055 API */
comres += bno055_convert_double_euler_h_deg(&d_euler_data_h);
comres += bno055_convert_double_euler_r_deg(&d_euler_data_r);
comres += bno055_convert_double_euler_p_deg(&d_euler_data_p);
comres += bno055_convert_double_euler_h_rad(&d_euler_data_h);
comres += bno055_convert_double_euler_r_rad(&d_euler_data_r);
comres += bno055_convert_double_euler_p_rad(&d_euler_data_p);
comres += bno055_convert_double_euler_hpr_deg(&d_euler_hpr);
comres += bno055_convert_double_euler_hpr_rad(&d_euler_hpr);
/* API used to read Linear acceleration data output as m/s2
float functions also available in the BNO055 API */
comres += bno055_convert_double_linear_accel_x_msq(
&d_linear_accel_datax);
comres += bno055_convert_double_linear_accel_y_msq(
&d_linear_accel_datay);
comres += bno055_convert_double_linear_accel_z_msq(
&d_linear_accel_dataz);
comres += bno055_convert_double_linear_accel_xyz_msq(
&d_linear_accel_xyz);
/* API used to read Gravity sensor data output as m/s2
float functions also available in the BNO055 API */
comres += bno055_convert_gravity_double_x_msq(&d_gravity_data_x);
comres += bno055_convert_gravity_double_y_msq(&d_gravity_data_y);
comres += bno055_convert_gravity_double_z_msq(&d_gravity_data_z);
comres += bno055_convert_double_gravity_xyz_msq(&d_gravity_xyz);
/*-----------------------------------------------------------------------*
************************* START DE-INITIALIZATION ***********************
*-------------------------------------------------------------------------*/
/* For de - initializing the BNO sensor it is required
to the operation mode of the sensor as SUSPEND
Suspend mode can set from the register
Page - page0
register - 0x3E
bit positions - 0 and 1*/
power_mode = BNO055_POWER_MODE_SUSPEND;
/* set the power mode as SUSPEND*/
comres += bno055_set_power_mode(power_mode);
/*---------------------------------------------------------------------*
************************* END DE-INITIALIZATION **********************
*---------------------------------------------------------------------*/
return comres;
}
#ifdef BNO055_API
/*--------------------------------------------------------------------------*
* The following API is used to map the I2C bus read, write, delay and
* device address with global structure bno055_t
*-------------------------------------------------------------------------*/
/*-------------------------------------------------------------------------*
* By using bno055 the following structure parameter can be accessed
* Bus write function pointer: BNO055_WR_FUNC_PTR
* Bus read function pointer: BNO055_RD_FUNC_PTR
* Delay function pointer: delay_msec
* I2C address: dev_addr
*--------------------------------------------------------------------------*/
s8 I2C_routine(void)
{
bno055.bus_write = BNO055_I2C_bus_write;
bno055.bus_read = BNO055_I2C_bus_read;
bno055.delay_msec = BNO055_delay_msek;
bno055.dev_addr = BNO055_I2C_ADDR1;
return BNO055_INIT_VALUE;
}
/************** I2C buffer length******/
#define I2C_BUFFER_LEN 8
#define I2C0 5
/*-------------------------------------------------------------------*
*
* This is a sample code for read and write the data by using I2C
* Use either I2C based on your need
* The device address defined in the bno055.h file
*
*--------------------------------------------------------------------*/
/* \Brief: The API is used as I2C bus write
* \Return : Status of the I2C write
* \param dev_addr : The device address of the sensor
* \param reg_addr : Address of the first register,
* will data is going to be written
* \param reg_data : It is a value hold in the array,
* will be used for write the value into the register
* \param cnt : The no of byte of data to be write
*/
s8 BNO055_I2C_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
{
assert(cnt + 1 < I2C_BUFFER_LEN);
u8 array[I2C_BUFFER_LEN + 1];
array[0] = reg_addr;
memcpy(array + 1, reg_data, cnt);
if (write(_fd, array, cnt + 1)) {
return -1;
}
return 0;
}
/* \Brief: The API is used as I2C bus read
* \Return : Status of the I2C read
* \param dev_addr : The device address of the sensor
* \param reg_addr : Address of the first register,
* will data is going to be read
* \param reg_data : This data read from the sensor,
* which is hold in an array
* \param cnt : The no of byte of data to be read
*/
s8 BNO055_I2C_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
{
assert(cnt < I2C_BUFFER_LEN);
if (write(_fd, &reg_addr, 1) < 0) {
return -1;
}
if (read(_fd, reg_data, cnt) < 0) {
return -1;
}
return 0;
}
/* Brief : The delay routine
* \param : delay in ms
*/
void BNO055_delay_msek(u32 msek)
{
/*Here you can write your own delay routine*/
}
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment