BRIX5

/***************************************************************************

  This is a library for the BNO055 orientation sensor

  Designed specifically to work with the Adafruit BNO055 Breakout.

  Pick one up today in the adafruit shop!

  ------> http://www.adafruit.com/products

  These sensors use I2C to communicate, 2 pins are required to interface.

  Adafruit invests time and resources providing this open source code,

  please support Adafruit andopen-source hardware by purchasing products

  from Adafruit!

  Written by KTOWN for Adafruit Industries.

  MIT license, all text above must be included in any redistribution

 ***************************************************************************/

#if ARDUINO >= 100

 #include "Arduino.h"

#else

 #include "WProgram.h"

#endif

#include <math.h>

#include <limits.h>

#include "Adafruit_BNO055.h"

/***************************************************************************

 CONSTRUCTOR

 ***************************************************************************/

/**************************************************************************/

/*!

    @brief  Instantiates a new Adafruit_BNO055 class

*/

/**************************************************************************/

Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)

{

  _sensorID = sensorID;

  _address = address;

}

/***************************************************************************

 PUBLIC FUNCTIONS

 ***************************************************************************/

/**************************************************************************/

/*!

    @brief  Sets up the HW

*/

/**************************************************************************/

bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode)

{

  /* Enable I2C */

  Wire.begin();

  /* Make sure we have the right device */

  uint8_t id = read8(BNO055_CHIP_ID_ADDR);

  if(id != BNO055_ID)

  {

    return false;

  }

  /* Switch to config mode (just in case since this is the default) */

  setMode(OPERATION_MODE_CONFIG);

  /* Set to normal power mode */

  write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);

  delay(10);

  /* Set the requested operating mode (see section 3.3) */

  write8(BNO055_OPR_MODE_ADDR, mode);

  delay(20);

  return true;

}

/**************************************************************************/

/*!

    @brief  Puts the chip in the specified operating mode

*/

/**************************************************************************/

void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode)

{

  _mode = mode;

  write8(BNO055_OPR_MODE_ADDR, _mode);

  delay(30);

}

/**************************************************************************/

/*!

    @brief  Gets the latest system status info

*/

/**************************************************************************/

void Adafruit_BNO055::getSystemStatus(adafruit_bno055_system_status_t * status)

{

  memset(status, 0, sizeof(adafruit_bno055_system_status_t));

  /* Read the system status register */

  status->system_status    = read8(BNO055_SYS_STAT_ADDR);

  status->self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);

  status->system_error     = read8(BNO055_SYS_ERR_ADDR);

}

/**************************************************************************/

/*!

    @brief  Displays system status info via Serial.print

*/

/**************************************************************************/

void Adafruit_BNO055::displaySystemStatus(void)

{

  adafruit_bno055_system_status_t status;

  getSystemStatus(&status);

  /* System Status (see section 4.3.58)

     ---------------------------------

     0 = Idle

     1 = System Error

     2 = Initializing Peripherals

     3 = System Iniitalization

     4 = Executing Self-Test

     5 = Sensor fusio algorithm running

     6 = System running without fusion algorithms */

  Serial.print("System Status:          0x");

  Serial.println(status.system_status, HEX);

  /* Self Test Results (see section )

     --------------------------------

     1 = test passed, 0 = test failed

     Bit 0 = Accelerometer self test

     Bit 1 = Magnetometer self test

     Bit 2 = Gyroscope self test

     Bit 3 = MCU self test

     0x0F = all good! */

  Serial.print("Self Test Results:      0x");

  Serial.println(status.self_test_result, HEX);

  /* System Error (see section 4.3.59)

     ---------------------------------

     0 = No error

     1 = Peripheral initialization error

     2 = System initialization error

     3 = Self test result failed

     4 = Register map value out of range

     5 = Register map address out of range

     6 = Register map write error

     7 = BNO low power mode not available for selected operat ion mode

     8 = Accelerometer power mode not available

     9 = Fusion algorithm configuration error

     A = Sensor configuration error */

  Serial.print("System Error:           0x");

  Serial.println(status.system_error, HEX);

}

/**************************************************************************/

/*!

    @brief  Gets the chip revision numbers

*/

/**************************************************************************/

void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info)

{

  uint8_t a, b;

  memset(info, 0, sizeof(adafruit_bno055_rev_info_t));

  info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);

  info->mag_rev   = read8(BNO055_MAG_REV_ID_ADDR);

  info->gyro_rev  = read8(BNO055_GYRO_REV_ID_ADDR);

  info->bl_rev    = read8(BNO055_BL_REV_ID_ADDR);

  a = read8(BNO055_SW_REV_ID_LSB_ADDR);

  b = read8(BNO055_SW_REV_ID_MSB_ADDR);

  info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);

}

/**************************************************************************/

/*!

    @brief  Displays the chip revision numbers via Serial.print

*/

/**************************************************************************/

void Adafruit_BNO055::displayRevInfo(void)

{

  adafruit_bno055_rev_info_t info;

  getRevInfo(&info);

  /* Check the accelerometer revision */

  Serial.print("Accelerometer Revision: 0x");

  Serial.println(info.accel_rev, HEX);

  /* Check the magnetometer revision */

  Serial.print("Magnetometer Revision:  0x");

  Serial.println(info.mag_rev, HEX);

  /* Check the gyroscope revision */

  Serial.print("Gyroscope Revision:     0x");

  Serial.println(info.gyro_rev, HEX);

  /* Check the SW revision */

  Serial.print("SW Revision:            0x");

  Serial.println(info.sw_rev, HEX);

  /* Check the bootloader revision */

  Serial.print("Bootloader Revision:    0x");

  Serial.println(info.bl_rev, HEX);

}

/**************************************************************************/

/*!

    @brief  Gets a new heading/roll/pitch sample in Euler angles

*/

/**************************************************************************/

imu::Vector<3> Adafruit_BNO055::getEuler(void)

{

  imu::Vector<3> hrp;

  uint8_t buffer[6];

  memset (buffer, 0, 6);

  int16_t h, r, p;

  h = r = p = 0;

  /* Read HRP data (6 bytes) */

  readLen(BNO055_EULER_H_LSB_ADDR, buffer, 6);

  h = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);

  r = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);

  p = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);

  /* Assign to Vector */

  hrp[0] = (double)h;

  hrp[1] = (double)r;

  hrp[2] = (double)p;

  return hrp;

}

/**************************************************************************/

/*!

    @brief  Gets a new accelerometer sample

*/

/**************************************************************************/

imu::Vector<3> Adafruit_BNO055::getAccel(void)

{

  imu::Vector<3> xyz;

  uint8_t buffer[6];

  memset (buffer, 0, 6);

  int16_t x, y, z;

  x = y = z = 0;

  /* Read accel data (6 bytes) */

  readLen(BNO055_ACCEL_DATA_X_LSB_ADDR, buffer, 6);

  x = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);

  y = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);

  z = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);

  /* Assign to Vector */

  xyz[0] = (double)x;

  xyz[1] = (double)y;

  xyz[2] = (double)z;

  return xyz;

}

/**************************************************************************/

/*!

    @brief  Provides the sensor_t data for this sensor

*/

/**************************************************************************/

void Adafruit_BNO055::getSensor(sensor_t *sensor)

{

  /* Clear the sensor_t object */

  memset(sensor, 0, sizeof(sensor_t));

  /* Insert the sensor name in the fixed length char array */

  strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1);

  sensor->name[sizeof(sensor->name)- 1] = 0;

  sensor->version     = 1;

  sensor->sensor_id   = _sensorID;

  sensor->type        = SENSOR_TYPE_ORIENTATION;

  sensor->min_delay   = 0;

  sensor->max_value   = 0.0F;

  sensor->min_value   = 0.0F;

  sensor->resolution  = 0.01F;

}

/**************************************************************************/

/*!

    @brief  Reads the sensor and returns the data as a sensors_event_t

*/

/**************************************************************************/

bool Adafruit_BNO055::getEvent(sensors_event_t *event)

{

  float orientation;

  /* Clear the event */

  memset(event, 0, sizeof(sensors_event_t));

  event->version   = sizeof(sensors_event_t);

  event->sensor_id = _sensorID;

  event->type      = SENSOR_TYPE_ORIENTATION;

  event->timestamp = 0;

  /* 

  getPressure(&pressure_kPa);

  event->pressure = pressure_kPa / 100.0F;

  */

  return true;

}

/***************************************************************************

 PRIVATE FUNCTIONS

 ***************************************************************************/

/**************************************************************************/

/*!

    @brief  Writes an 8 bit value over I2C

*/

/**************************************************************************/

bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value)

{

  Wire.beginTransmission(_address);

  #if ARDUINO >= 100

    Wire.write((uint8_t)reg);

    Wire.write((uint8_t)value);

  #else

    Wire.send(reg);

    Wire.send(value);

  #endif

  Wire.endTransmission();

  /* ToDo: Check for error! */

  return true;

}

/**************************************************************************/

/*!

    @brief  Reads an 8 bit value over I2C

*/

/**************************************************************************/

byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg )

{

  byte value = 0;

  Wire.beginTransmission(_address);

  #if ARDUINO >= 100

    Wire.write((uint8_t)reg);

  #else

    Wire.send(reg);

  #endif

  Wire.endTransmission();

  Wire.requestFrom(_address, (byte)1);

  #if ARDUINO >= 100

    value = Wire.read();

  #else

    value = Wire.receive();

  #endif

  return value;

}

/**************************************************************************/

/*!

    @brief  Reads the specified number of bytes over I2C

*/

/**************************************************************************/

bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len)

{

  Wire.beginTransmission(_address);

  #if ARDUINO >= 100

    Wire.write((uint8_t)reg);

  #else

    Wire.send(reg);

  #endif

  Wire.endTransmission();

  Wire.requestFrom(_address, (byte)len);

  /* Wait until data is available */

  while (Wire.available() < len);

  for (uint8_t i = 0; i < len; i++)

  {

    #if ARDUINO >= 100

      buffer[i] = Wire.read();

    #else

      buffer[i] = Wire.receive();

    #endif

  }

  /* ToDo: Check for errors! */

  return true;

}

/***************************************************************************

  This is a library for the BNO055 orientation sensor

  Designed specifically to work with the Adafruit BNO055 Breakout.

  Pick one up today in the adafruit shop!

  ------> http://www.adafruit.com/products

  These sensors use I2C to communicate, 2 pins are required to interface.

  Adafruit invests time and resources providing this open source code,

  please support Adafruit andopen-source hardware by purchasing products

  from Adafruit!

  Written by KTOWN for Adafruit Industries.

  MIT license, all text above must be included in any redistribution

 ***************************************************************************/

#ifndef __ADAFRUIT_BNO055_H__

#define __ADAFRUIT_BNO055_H__

#if (ARDUINO >= 100)

 #include "Arduino.h"

#else

 #include "WProgram.h"

#endif

#include "Wire.h"

#include <Adafruit_Sensor.h>

#include <utility/imumaths.h>

#define BNO055_ADDRESS_A (0x28)

#define BNO055_ADDRESS_B (0x29)

#define BNO055_ID        (0xA0)

class Adafruit_BNO055 /* : public Adafruit_Sensor */

{

  public:

    typedef enum

    {

      /* Page id register definition */

      BNO055_PAGE_ID_ADDR				                  = 0X07,

      /* PAGE0 REGISTER DEFINITION START*/

      BNO055_CHIP_ID_ADDR                         = 0x00,

      BNO055_ACCEL_REV_ID_ADDR			              = 0x01,

      BNO055_MAG_REV_ID_ADDR                      = 0x02,

      BNO055_GYRO_REV_ID_ADDR                     = 0x03,

      BNO055_SW_REV_ID_LSB_ADDR			              = 0x04,

      BNO055_SW_REV_ID_MSB_ADDR			              = 0x05,

      BNO055_BL_REV_ID_ADDR				                = 0X06,

      /* Accel data register */

      BNO055_ACCEL_DATA_X_LSB_ADDR			          = 0X08,

      BNO055_ACCEL_DATA_X_MSB_ADDR			          = 0X09,

      BNO055_ACCEL_DATA_Y_LSB_ADDR			          = 0X0A,

      BNO055_ACCEL_DATA_Y_MSB_ADDR			          = 0X0B,

      BNO055_ACCEL_DATA_Z_LSB_ADDR			          = 0X0C,

      BNO055_ACCEL_DATA_Z_MSB_ADDR			          = 0X0D,

      /* Mag data register */

      BNO055_MAG_DATA_X_LSB_ADDR			            = 0X0E,

      BNO055_MAG_DATA_X_MSB_ADDR			            = 0X0F,

      BNO055_MAG_DATA_Y_LSB_ADDR			            = 0X10,

      BNO055_MAG_DATA_Y_MSB_ADDR			            = 0X11,

      BNO055_MAG_DATA_Z_LSB_ADDR			            = 0X12,

      BNO055_MAG_DATA_Z_MSB_ADDR			            = 0X13,

      /* Gyro data registers */

      BNO055_GYRO_DATA_X_LSB_ADDR			            = 0X14,

      BNO055_GYRO_DATA_X_MSB_ADDR			            = 0X15,

      BNO055_GYRO_DATA_Y_LSB_ADDR			            = 0X16,

      BNO055_GYRO_DATA_Y_MSB_ADDR			            = 0X17,

      BNO055_GYRO_DATA_Z_LSB_ADDR			            = 0X18,

      BNO055_GYRO_DATA_Z_MSB_ADDR			            = 0X19,

      /* Euler data registers */

      BNO055_EULER_H_LSB_ADDR			                = 0X1A,

      BNO055_EULER_H_MSB_ADDR			                = 0X1B,

      BNO055_EULER_R_LSB_ADDR			                = 0X1C,

      BNO055_EULER_R_MSB_ADDR			                = 0X1D,

      BNO055_EULER_P_LSB_ADDR			                = 0X1E,

      BNO055_EULER_P_MSB_ADDR			                = 0X1F,

      /* Quaternion data registers */

      BNO055_QUATERNION_DATA_W_LSB_ADDR	          = 0X20,

      BNO055_QUATERNION_DATA_W_MSB_ADDR	          = 0X21,

      BNO055_QUATERNION_DATA_X_LSB_ADDR	          = 0X22,

      BNO055_QUATERNION_DATA_X_MSB_ADDR	          = 0X23,

      BNO055_QUATERNION_DATA_Y_LSB_ADDR	          = 0X24,

      BNO055_QUATERNION_DATA_Y_MSB_ADDR	          = 0X25,

      BNO055_QUATERNION_DATA_Z_LSB_ADDR	          = 0X26,

      BNO055_QUATERNION_DATA_Z_MSB_ADDR	          = 0X27,

      /* Linear acceleration data registers */

      BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR			    = 0X28,

      BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR			    = 0X29,

      BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR			    = 0X2A,

      BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR			    = 0X2B,

      BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR			    = 0X2C,

      BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR			    = 0X2D,

      /* Gravity data registers */

      BNO055_GRAVITY_DATA_X_LSB_ADDR			        = 0X2E,

      BNO055_GRAVITY_DATA_X_MSB_ADDR			        = 0X2F,

      BNO055_GRAVITY_DATA_Y_LSB_ADDR			        = 0X30,

      BNO055_GRAVITY_DATA_Y_MSB_ADDR			        = 0X31,

      BNO055_GRAVITY_DATA_Z_LSB_ADDR			        = 0X32,

      BNO055_GRAVITY_DATA_Z_MSB_ADDR			        = 0X33,

      /* Temperature data register */

      BNO055_TEMP_ADDR					                  = 0X34,

      /* Status registers */

      BNO055_CALIB_STAT_ADDR				              = 0X35,

      BNO055_SELFTEST_RESULT_ADDR			            = 0X36,

      BNO055_INTR_STAT_ADDR				                = 0X37,

      BNO055_SYS_CLK_STAT_ADDR			              = 0X38,

      BNO055_SYS_STAT_ADDR				                = 0X39,

      BNO055_SYS_ERR_ADDR					                = 0X3A,

      /* Unit selection register */

      BNO055_UNIT_SEL_ADDR				                = 0X3B,

      BNO055_DATA_SELECT_ADDR				              = 0X3C,

      /* Mode registers */

      BNO055_OPR_MODE_ADDR				                = 0X3D,

      BNO055_PWR_MODE_ADDR				                = 0X3E,

      BNO055_SYS_TRIGGER_ADDR				              = 0X3F,

      BNO055_TEMP_SOURCE_ADDR				              = 0X40,

      /* Axis remap registers */

      BNO055_AXIS_MAP_CONFIG_ADDR			            = 0X41,

      BNO055_AXIS_MAP_SIGN_ADDR			              = 0X42,

      /* SIC registers */

      BNO055_SIC_MATRIX_0_LSB_ADDR		            = 0X43,

      BNO055_SIC_MATRIX_0_MSB_ADDR		            = 0X44,

      BNO055_SIC_MATRIX_1_LSB_ADDR		            = 0X45,

      BNO055_SIC_MATRIX_1_MSB_ADDR		            = 0X46,

      BNO055_SIC_MATRIX_2_LSB_ADDR		            = 0X47,

      BNO055_SIC_MATRIX_2_MSB_ADDR		            = 0X48,

      BNO055_SIC_MATRIX_3_LSB_ADDR		            = 0X49,

      BNO055_SIC_MATRIX_3_MSB_ADDR		            = 0X4A,

      BNO055_SIC_MATRIX_4_LSB_ADDR		            = 0X4B,

      BNO055_SIC_MATRIX_4_MSB_ADDR		            = 0X4C,

      BNO055_SIC_MATRIX_5_LSB_ADDR		            = 0X4D,

      BNO055_SIC_MATRIX_5_MSB_ADDR		            = 0X4E,

      BNO055_SIC_MATRIX_6_LSB_ADDR		            = 0X4F,

      BNO055_SIC_MATRIX_6_MSB_ADDR		            = 0X50,

      BNO055_SIC_MATRIX_7_LSB_ADDR		            = 0X51,

      BNO055_SIC_MATRIX_7_MSB_ADDR		            = 0X52,

      BNO055_SIC_MATRIX_8_LSB_ADDR		            = 0X53,

      BNO055_SIC_MATRIX_8_MSB_ADDR		            = 0X54,

      /* Accelerometer Offset registers */

      ACCEL_OFFSET_X_LSB_ADDR				              = 0X55,

      ACCEL_OFFSET_X_MSB_ADDR				              = 0X56,

      ACCEL_OFFSET_Y_LSB_ADDR				              = 0X57,

      ACCEL_OFFSET_Y_MSB_ADDR				              = 0X58,

      ACCEL_OFFSET_Z_LSB_ADDR				              = 0X59,

      ACCEL_OFFSET_Z_MSB_ADDR				              = 0X5A,

      /* Magnetometer Offset registers */

      MAG_OFFSET_X_LSB_ADDR				                = 0X5B,

      MAG_OFFSET_X_MSB_ADDR				                = 0X5C,

      MAG_OFFSET_Y_LSB_ADDR				                = 0X5D,

      MAG_OFFSET_Y_MSB_ADDR				                = 0X5E,

      MAG_OFFSET_Z_LSB_ADDR				                = 0X5F,

      MAG_OFFSET_Z_MSB_ADDR				                = 0X60,

      /* Gyroscope Offset register s*/

      GYRO_OFFSET_X_LSB_ADDR				              = 0X61,

      GYRO_OFFSET_X_MSB_ADDR				              = 0X62,

      GYRO_OFFSET_Y_LSB_ADDR				              = 0X63,

      GYRO_OFFSET_Y_MSB_ADDR				              = 0X64,

      GYRO_OFFSET_Z_LSB_ADDR				              = 0X65,

      GYRO_OFFSET_Z_MSB_ADDR				              = 0X66,

      /* Radius registers */

      ACCEL_RADIUS_LSB_ADDR				                = 0X67,

      ACCEL_RADIUS_MSB_ADDR				                = 0X68,

      MAG_RADIUS_LSB_ADDR					                = 0X69,

      MAG_RADIUS_MSB_ADDR					                = 0X6A

    } adafruit_bno055_reg_t;

    typedef enum

    {

      POWER_MODE_NORMAL	                          = 0X00,

      POWER_MODE_LOWPOWER	                        = 0X01,

      POWER_MODE_SUSPEND	                        = 0X02

    } adafruit_bno055_powermode_t;

    typedef enum

    {

      /* Operation mode settings*/

      OPERATION_MODE_CONFIG			                  = 0X00,

      OPERATION_MODE_ACCONLY			                = 0X01,

      OPERATION_MODE_MAGONLY			                = 0X02,

      OPERATION_MODE_GYRONLY			                = 0X03,

      OPERATION_MODE_ACCMAG			                  = 0X04,

      OPERATION_MODE_ACCGYRO			                = 0X05,

      OPERATION_MODE_MAGGYRO			                = 0X06,

      OPERATION_MODE_AMG				                  = 0X07,

      OPERATION_MODE_IMUPLUS			                = 0X08,

      OPERATION_MODE_COMPASS			                = 0X09,

      OPERATION_MODE_M4G				                  = 0X0A,

      OPERATION_MODE_NDOF_FMC_OFF		              = 0X0B,

      OPERATION_MODE_NDOF				                  = 0X0C

    } adafruit_bno055_opmode_t;

    typedef struct

    {

      uint8_t  accel_rev;

      uint8_t  mag_rev;

      uint8_t  gyro_rev;

      uint16_t sw_rev;

      uint8_t  bl_rev;

    } adafruit_bno055_rev_info_t;

    typedef struct

    {

      uint8_t system_status;      

      uint8_t self_test_result;

      uint8_t system_error;

    } adafruit_bno055_system_status_t;

    Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A );

    bool  begin               ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );

    void  setMode             ( adafruit_bno055_opmode_t mode );

    void  getRevInfo          ( adafruit_bno055_rev_info_t* );

    void  displayRevInfo      ( void );

    void  getSystemStatus     ( adafruit_bno055_system_status_t* );

    void  displaySystemStatus ( void );

    imu::Vector<3> getEuler   ( void );

    imu::Vector<3> getAccel   ( void );

    /* Adafruit_Sensor implementation */

    bool  getEvent  ( sensors_event_t* );

    void  getSensor ( sensor_t* );

  private:

    byte  read8   ( adafruit_bno055_reg_t );

    bool  readLen ( adafruit_bno055_reg_t, byte* buffer, uint8_t len );

    bool  write8  ( adafruit_bno055_reg_t, byte value );

    uint8_t _address;

    int32_t _sensorID;

    adafruit_bno055_opmode_t _mode;

};

#endif

#Adafruit Unified BNO055 Driver (AHRS/Orientation) #

This driver is for the Adafruit BNO055 Breakout (http://www.adafruit.com/products/xxxx),

and is based on Adafruit's Unified Sensor Library (Adafruit_Sensor).

## What is the Adafruit Unified Sensor Library? ##

The Adafruit Unified Sensor Library ([Adafruit_Sensor](https://github.com/adafruit/Adafruit_Sensor)) provides a common interface and data type for any supported sensor.  It defines some basic information about the sensor (sensor limits, etc.), and returns standard SI units of a specific type and scale for each supported sensor type.

It provides a simple abstraction layer between your application and the actual sensor HW, allowing you to drop in any comparable sensor with only one or two lines of code to change in your project (essentially the constructor since the functions to read sensor data and get information about the sensor are defined in the base Adafruit_Sensor class).

This is imporant useful for two reasons:

1.) You can use the data right away because it's already converted to SI units that you understand and can compare, rather than meaningless values like 0..1023.

2.) Because SI units are standardised in the sensor library, you can also do quick sanity checks when working with new sensors, or drop in any comparable sensor if you need better sensitivity or if a lower cost unit becomes available, etc. 

Light sensors will always report units in lux, gyroscopes will always report units in rad/s, etc. ... freeing you up to focus on the data, rather than digging through the datasheet to understand what the sensor's raw numbers really mean.

## About this Driver ##

Adafruit invests time and resources providing this open source code.  Please support Adafruit and open-source hardware by purchasing products from Adafruit!

Written by Kevin (KTOWN) Townsend for Adafruit Industries.

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor),

   which provides a common 'type' for sensor data and some helper functions.

   To use this driver you will also need to download the Adafruit_Sensor

   library and include it in your libraries folder.

   You should also assign a unique ID to this sensor for use with

   the Adafruit Sensor API so that you can identify this particular

   sensor in any data logs, etc.  To assign a unique ID, simply

   provide an appropriate value in the constructor below (12345

   is used by default in this example).

   Connections

   ===========

   Connect SCL to analog 5

   Connect SDA to analog 4

   Connect VDD to 3.3V DC

   Connect GROUND to common ground

   History

   =======

   2015/MAR/03  - First release (KTOWN)

*/

/* Set the delay between fresh samples */

#define BNO055_SAMPLERATE_DELAY_MS (500)

Adafruit_BNO055 bno = Adafruit_BNO055(55);

/**************************************************************************/

/*

    Displays some basic information on this sensor from the unified

    sensor API sensor_t type (see Adafruit_Sensor for more information)

*/

/**************************************************************************/

void displaySensorDetails(void)

{

  sensor_t sensor;

  bno.getSensor(&sensor);

  Serial.println("------------------------------------");

  Serial.print  ("Sensor:       "); Serial.println(sensor.name);

  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);

  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);

  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" xxx");

  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" xxx");

  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" xxx");  

  Serial.println("------------------------------------");

  Serial.println("");

  delay(500);

}

/**************************************************************************/

/*

    Arduino setup function (automatically called at startup)

*/

/**************************************************************************/

void setup(void) 

{

  Serial.begin(9600);

  Serial.println("Orientation Sensor Test"); Serial.println("");

  /* Initialise the sensor */

  if(!bno.begin())

  {

    /* There was a problem detecting the BNO055 ... check your connections */

    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");

    while(1);

  }

  delay(1000);

  /* Display some basic information on this sensor */

  displaySensorDetails();

  /* Display system info (optional) */

  bno.displaySystemStatus();

  Serial.println("");

  /* Display chip revision details (optional) */

  bno.displayRevInfo();

  Serial.println("");

}

/**************************************************************************/

/*

    Arduino loop function, called once 'setup' is complete (your own code

    should go here)

*/

/**************************************************************************/

void loop(void) 

{

  imu::Vector<3> euler = bno.getEuler();

  Serial.print("X: ");

  Serial.println((int)euler.x(), DEC);

  Serial.print("Y: ");

  Serial.println((int)euler.y(), DEC);

  Serial.print("Z: ");

  Serial.println((int)euler.z(), DEC);

  Serial.println("");

  delay(BNO055_SAMPLERATE_DELAY_MS);

}

/*

    Inertial Measurement Unit Maths Library

    Copyright (C) 2013-2014  Samuel Cowen

	www.camelsoftware.com

    This program is free software: you can redistribute it and/or modify

    it under the terms of the GNU General Public License as published by

    the Free Software Foundation, either version 3 of the License, or

    (at your option) any later version.

    This program is distributed in the hope that it will be useful,

    but WITHOUT ANY WARRANTY; without even the implied warranty of

    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License

    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#ifndef IMUMATH_H

#define IMUMATH_H

#include "vector.h"

#include "matrix.h"

#include "quaternion.h"

#endif

/*

    Inertial Measurement Unit Maths Library

    Copyright (C) 2013-2014  Samuel Cowen

	www.camelsoftware.com

    This program is free software: you can redistribute it and/or modify

    it under the terms of the GNU General Public License as published by

    the Free Software Foundation, either version 3 of the License, or

    (at your option) any later version.

    This program is distributed in the hope that it will be useful,

    but WITHOUT ANY WARRANTY; without even the implied warranty of

    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License

    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#ifndef IMUMATH_MATRIX_HPP

#define IMUMATH_MATRIX_HPP

#include <stdlib.h>

#include <string.h>

#include <stdint.h>

#include <math.h>

namespace imu

{

template <uint8_t N> class Matrix

{

public:

	Matrix()

	{

		int r = sizeof(double)*N;

        _cell = (double*)malloc(r*r);

        memset(_cell, 0, r*r);

	}

    Matrix(const Matrix &v)

    {

        int r = sizeof(double)*N;

        _cell = (double*)malloc(r*r);

        memset(_cell, 0, r*r);

        for (int x = 0; x < N; x++ )

        {

            for(int y = 0; y < N; y++)

            {

                _cell[x*N+y] = v._cell[x*N+y];

            }

        }

    }

    ~Matrix()

    {

        free(_cell);

    }

    void operator = (Matrix m)

    {

        for(int x = 0; x < N; x++)

        {

            for(int y = 0; y < N; y++)

            {

                cell(x, y) = m.cell(x, y);

            }

        }

    }

    Vector<N> row_to_vector(int y)

    {

        Vector<N> ret;

        for(int i = 0; i < N; i++)

        {

            ret[i] = _cell[y*N+i];

        }

        return ret;

    }

    Vector<N> col_to_vector(int x)

    {

        Vector<N> ret;

        for(int i = 0; i < N; i++)

        {

            ret[i] = _cell[i*N+x];

        }

        return ret;

    }

    void vector_to_row(Vector<N> v, int row)

    {

        for(int i = 0; i < N; i++)

        {

            cell(row, i) = v(i);

        }

    }

    void vector_to_col(Vector<N> v, int col)

    {

        for(int i = 0; i < N; i++)

        {

            cell(i, col) = v(i);

        }

    }

    double& operator ()(int x, int y)

    {

        return _cell[x*N+y];

    }

    double& cell(int x, int y)

    {

        return _cell[x*N+y];

    }

    Matrix operator + (Matrix m)

    {

        Matrix ret;

        for(int x = 0; x < N; x++)

        {

            for(int y = 0; y < N; y++)

            {

                ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y];

            }

        }

        return ret;

    }

    Matrix operator - (Matrix m)

    {

        Matrix ret;

        for(int x = 0; x < N; x++)

        {

            for(int y = 0; y < N; y++)

            {

                ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y];

            }

        }

        return ret;

    }

    Matrix operator * (double scalar)

    {

        Matrix ret;

        for(int x = 0; x < N; x++)

        {

            for(int y = 0; y < N; y++)

            {

                ret._cell[x*N+y] = _cell[x*N+y] * scalar;

            }

        }

        return ret;

    }

    Matrix operator * (Matrix m)

    {

        Matrix ret;

        for(int x = 0; x < N; x++)

        {

            for(int y = 0; y < N; y++)

            {

                Vector<N> row = row_to_vector(x);

                Vector<N> col = m.col_to_vector(y);

                ret.cell(x, y) = row.dot(col);

            }

        }

        return ret;

    }

    Matrix transpose()

    {

        Matrix ret;

        for(int x = 0; x < N; x++)

        {

            for(int y = 0; y < N; y++)

            {

                ret.cell(y, x) = cell(x, y);

            }

        }

        return ret;

    }

    Matrix<N-1> minor_matrix(int row, int col)

    {

        int colCount = 0, rowCount = 0;

        Matrix<N-1> ret;

        for(int i = 0; i < N; i++ )

        {

            if( i != row )

            {

                for(int j = 0; j < N; j++ )

                {

                    if( j != col )

                    {

                        ret(rowCount, colCount) = cell(i, j);

                        colCount++;

                    }

                }

                rowCount++;

            }

        }

        return ret;

    }

    double determinant()

    {

        if(N == 1)

            return cell(0, 0);

        float det = 0.0;

        for(int i = 0; i < N; i++ )

        {

            Matrix<N-1> minor = minor_matrix(0, i);

            det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant();

        }

        return det;

    }

    Matrix invert()

    {

        Matrix ret;

        float det = determinant();

        for(int x = 0; x < N; x++)

        {

            for(int y = 0; y < N; y++)

            {

                Matrix<N-1> minor = minor_matrix(y, x);

                ret(x, y) = det*minor.determinant();

                if( (x+y)%2 == 1)

                    ret(x, y) = -ret(x, y);

            }

        }

        return ret;

    }

private:

    double* _cell;

};

};

#endif

/*

    Inertial Measurement Unit Maths Library

    Copyright (C) 2013-2014  Samuel Cowen

	www.camelsoftware.com

    This program is free software: you can redistribute it and/or modify

    it under the terms of the GNU General Public License as published by

    the Free Software Foundation, either version 3 of the License, or

    (at your option) any later version.

    This program is distributed in the hope that it will be useful,

    but WITHOUT ANY WARRANTY; without even the implied warranty of

    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License

    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#ifndef IMUMATH_QUATERNION_HPP

#define IMUMATH_QUATERNION_HPP

#include <stdlib.h>

#include <string.h>

#include <stdint.h>

#include <math.h>

#include "vector.h"

namespace imu

{

class Quaternion

{

public:

    Quaternion()

    {

        _w = 1.0;

        _x = _y = _z = 0.0;

    }

    Quaternion(double iw, double ix, double iy, double iz)

    {

        _w = iw;

        _x = ix;

        _y = iy;

        _z = iz;

    }

    Quaternion(double w, Vector<3> vec)

    {

        _w = w;

        _x = vec.x();

        _y = vec.y();

        _z = vec.z();

    }

    double& w()

    {

        return _w;

    }

    double& x()

    {

        return _x;

    }

    double& y()

    {

        return _y;

    }

    double& z()

    {

        return _z;

    }

    double magnitude()

    {

        double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);

        return sqrt(res);

    }

    void normalize()

    {

		double mag = magnitude();

        *this = this->scale(1/mag);

    }

    Quaternion conjugate()

    {

        Quaternion q;

        q.w() = _w;

        q.x() = -_x;

        q.y() = -_y;

        q.z() = -_z;

        return q;

    }

    void fromAxisAngle(Vector<3> axis, double theta)

    {

        _w = cos(theta/2);

        //only need to calculate sine of half theta once

        double sht = sin(theta/2);

        _x = axis.x() * sht;

        _y = axis.y() * sht;

        _z = axis.z() * sht;

    }

    void fromMatrix(Matrix<3> m)

    {

        float tr = m(0, 0) + m(1, 1) + m(2, 2);

        float S = 0.0;

        if (tr > 0)

        {

            S = sqrt(tr+1.0) * 2;

            _w = 0.25 * S;

            _x = (m(2, 1) - m(1, 2)) / S;

            _y = (m(0, 2) - m(2, 0)) / S;

            _z = (m(1, 0) - m(0, 1)) / S;

        }

        else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2)))

        {

            S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;

            _w = (m(2, 1) - m(1, 2)) / S;

            _x = 0.25 * S;

            _y = (m(0, 1) + m(1, 0)) / S;

            _z = (m(0, 2) + m(2, 0)) / S;

        }

        else if (m(1, 1) < m(2, 2))

        {

            S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;

            _w = (m(0, 2) - m(2, 0)) / S;

            _x = (m(0, 1) + m(1, 0)) / S;

            _y = 0.25 * S;

            _z = (m(1, 2) + m(2, 1)) / S;

        }

        else

        {

            S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;

            _w = (m(1, 0) - m(0, 1)) / S;

            _x = (m(0, 2) + m(2, 0)) / S;

            _y = (m(1, 2) + m(2, 1)) / S;

            _z = 0.25 * S;

        }

    }

    void toAxisAngle(Vector<3>& axis, float& angle)

    {

        float sqw = sqrt(1-_w*_w);

        if(sqw == 0) //it's a singularity and divide by zero, avoid

            return;

        angle = 2 * acos(_w);

        axis.x() = _x / sqw;

        axis.y() = _y / sqw;

        axis.z() = _z / sqw;

    }

    Matrix<3> toMatrix()

    {

        Matrix<3> ret;

        ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z));

        ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z);

        ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y);

        ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z);

        ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z));

        ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x));

        ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y);

        ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x);

        ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y));

        return ret;

    }

    Vector<3> toEuler()

    {

        Vector<3> ret;

        double sqw = _w*_w;

        double sqx = _x*_x;

        double sqy = _y*_y;

        double sqz = _z*_z;

        ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));

        ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));

        ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));

        return ret;

    }

    Vector<3> toAngularVelocity(float dt)

    {

        Vector<3> ret;

        Quaternion one(1.0, 0.0, 0.0, 0.0);

        Quaternion delta = one - *this;

        Quaternion r = (delta/dt);

        r = r * 2;

        r = r * one;

        ret.x() = r.x();

        ret.y() = r.y();

        ret.z() = r.z();

        return ret;

    }

    Vector<3> rotateVector(Vector<2> v)

    {

        Vector<3> ret(v.x(), v.y(), 0.0);

        return rotateVector(ret);

    }

    Vector<3> rotateVector(Vector<3> v)

    {

        Vector<3> qv(this->x(), this->y(), this->z());

        Vector<3> t;

        t = qv.cross(v) * 2.0;

        return v + (t * _w) + qv.cross(t);

    }

    Quaternion operator * (Quaternion q)

    {

        Quaternion ret;

        ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z));

        ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y));

        ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x));

        ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w));

        return ret;

    }

    Quaternion operator + (Quaternion q)

    {

        Quaternion ret;

        ret._w = _w + q._w;

        ret._x = _x + q._x;

        ret._y = _y + q._y;

        ret._z = _z + q._z;

        return ret;

    }

    Quaternion operator - (Quaternion q)

    {

        Quaternion ret;

        ret._w = _w - q._w;

        ret._x = _x - q._x;

        ret._y = _y - q._y;

        ret._z = _z - q._z;

        return ret;

    }

    Quaternion operator / (float scalar)

    {

        Quaternion ret;

        ret._w = this->_w/scalar;

        ret._x = this->_x/scalar;

        ret._y = this->_y/scalar;

        ret._z = this->_z/scalar;

        return ret;

    }

    Quaternion operator * (float scalar)

    {

        Quaternion ret;

        ret._w = this->_w*scalar;

        ret._x = this->_x*scalar;

        ret._y = this->_y*scalar;

        ret._z = this->_z*scalar;

        return ret;

    }

	Quaternion scale(double scalar)

	{

        Quaternion ret;

        ret._w = this->_w*scalar;

        ret._x = this->_x*scalar;

        ret._y = this->_y*scalar;

        ret._z = this->_z*scalar;

        return ret;

    }

private:

    double _w, _x, _y, _z;

};

};

#endif

/*

    Inertial Measurement Unit Maths Library

    Copyright (C) 2013-2014  Samuel Cowen

	www.camelsoftware.com

    This program is free software: you can redistribute it and/or modify

    it under the terms of the GNU General Public License as published by

    the Free Software Foundation, either version 3 of the License, or

    (at your option) any later version.

    This program is distributed in the hope that it will be useful,

    but WITHOUT ANY WARRANTY; without even the implied warranty of

    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License

    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

#ifndef IMUMATH_VECTOR_HPP

#define IMUMATH_VECTOR_HPP

#include <stdlib.h>

#include <string.h>

#include <stdint.h>

#include <math.h>

namespace imu

{

template <uint8_t N> class Vector

{

public:

	Vector()

	{

		p_vec = (double*)malloc(sizeof(double)*N+1);

        memset(p_vec, 0, sizeof(double)*N);

	}

	Vector(double a)

	{

		p_vec = (double*)malloc(sizeof(double)*N+1);

        memset(p_vec, 0, sizeof(double)*N);

		p_vec[0] = a;

	}

	Vector(double a, double b)

	{

		p_vec = (double*)malloc(sizeof(double)*N+1);

        memset(p_vec, 0, sizeof(double)*N);

		p_vec[0] = a;

		p_vec[1] = b;

	}

	Vector(double a, double b, double c)

	{

		p_vec = (double*)malloc(sizeof(double)*N+1);

        memset(p_vec, 0, sizeof(double)*N);

		p_vec[0] = a;

		p_vec[1] = b;

		p_vec[2] = c;

	}

    Vector(double a, double b, double c, double d)

    {

		p_vec = (double*)malloc(sizeof(double)*N+1);

        memset(p_vec, 0, sizeof(double)*N);

        p_vec[0] = a;

		p_vec[1] = b;

		p_vec[2] = c;

		p_vec[3] = d;

    }

    Vector(const Vector<N> &v)

    {

        p_vec = (double*)malloc(sizeof(double)*N);

        memset(p_vec, 0, sizeof(double)*N);

        for (int x = 0; x < N; x++ )

            p_vec[x] = v.p_vec[x];

    }

    ~Vector()

    {

        free(p_vec);

    }