adafruit industries

protonstorm · Post by **protonstorm** » Wed Jun 17, 2015 5:40 am

Hi Adafruiters

I recently got the BNO055 sensor (9-DOF IMU) which seems to be a really great IMU - ease of use, update rate, precision.
But after some tinkering around, I have a question:

How can I find magnetic north with this sensor? I'm using the Unified Sensor Driver from Adafruit, and the Euler angles returned always seem to init to 0. Is there a way to get the absolute x angle (magnetic north) with this sensor?

Thanks
protonstorm

adafruit_support_bill · Wed Jun 17, 2015 6:20 am

Take a look at the RawData example in the library. If you edit it to use VECTOR_MAGNETOMETER , it will return the raw magnetometer data.

protonstorm · Post by **protonstorm** » Wed Jun 17, 2015 6:25 am

Thanks for your reply.

According to the BNO055 library, VECTOR_MAGNETOMETER returns micro Teslas. I got the BNO055 to get around all the math, I suppose getting absolute heading from uT returned is not a simple task?

adafruit_support_bill · Wed Jun 17, 2015 6:33 pm

Not so difficult:

heading = atan2(magnetometer.y, magnetometer.x);

protonstorm · Post by **protonstorm** » Thu Jun 18, 2015 10:29 am

Thanks for your reply

As far as I understand this only works if the sensor is level, no?

adafruit_support_bill · Thu Jun 18, 2015 7:29 pm

That is correct. A tilt-compensated heading calculation is a bit more involved:
http://www.timzaman.com/2011/04/heading ... d-compass/

protonstorm · Post by **protonstorm** » Fri Jun 19, 2015 9:18 am

Ah, that doesn't look too hard though.

Thanks!

frankjoshua · Post by **frankjoshua** » Mon Jun 22, 2015 6:40 pm

Did any one get this working yet?

I have tried this but no joy so far. Also I don't need tilt compensation but it would be nice.

Code: Select all

imu::Vector<3> vector;
float mCompassHeading;
void updateCompassHeading(){

  vector = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
  mCompassHeading = atan2(vector.y(), vector.x());

// Correct for when signs are reversed.
  if(mCompassHeading < 0)
    mCompassHeading += 2*PI;
// Convert radians to degrees
   mCompassHeading = mCompassHeading * 180/M_PI; 
}

I have been working on this for a few weeks and this is as close as I have been so far. I can't be the only one in the world who needs a magnetic heading.

protonstorm · Post by **protonstorm** » Mon Jun 22, 2015 8:24 pm

Yeah I been trying it this way, too. The reason it doesn't work is (I think) that the sensor is put into a Fusion Mode by default. Which means that it's output for Euler is already "fused" with other sensors.
You can use a different fusion mode which gives you compass heading (but gyro is off in this mode) like this:
(See the .h file of the library near the end)

Change:

Code: Select all

 bool  begin               ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );

to:

Code: Select all

 bool  begin               ( adafruit_bno055_opmode_t mode = OPERATION_MODE_COMPASS );

This will lead to absolute euler angles. Note that the sensor needs to be calibrated after each power cycle by doing some random figure 8 patterns (until somebody implements saving of calibration in the library.....I might do it if I find the time)

Edit: It just occured to me that the manual way (getting the magnetometer data) might actually work if you also do the calibration patterns. Will try this out tomorrow.

Hope this helps a bit.
Protonstorm

davegun · Post by **davegun** » Tue Jun 23, 2015 7:34 pm

I got it to work! The magnetic compass points north and is tilt compensated. I also found out how to calibrate the sensor so it points north on startup without calibration. I ended up modifying the library to make this work. It's not the best solution, and I think it could be written more efficiently, but it works well. I post it here:
http://forums.adafruit.com/viewtopic.php?f=19&t=73014

This is a very cool sensor and it's rock solid. Once it's calibrated for your project, it points north right at starup without having to twirl it around.

Dave

protonstorm · Post by **protonstorm** » Tue Jun 23, 2015 10:16 pm

@davegun I basically just did all this, too. I think your way is really not very efficient but probably works well enough ;)
The issue I have atm is, that most of my configuration values are 0 (And yours seem not) I don't know if thats a good thing or if I'm missing something. Did you run accross this issue? I see you wait for all the CALIB_STAT bits to be 1, not just the SYS bits, so that might be my mistake.

The main thing I learned so far is that with OPERATION_MODE_NDOF as default the sensor will initialize at 0,0,0. After a couple 8-figures the x component will actually show the actual compass heading (+/- 2.5°) So you can use this with the original library as a compass, but you will have to move it around a bit first.

If you start the sensor with OPERATION_MODE_COMPASS:

Code: Select all

if (!bno.begin(Adafruit_BNO055::OPERATION_MODE_COMPASS))

the vector will show some initial (kinda wrong) heading and after a couple 8-figures it will represent true north with a higher accuracy (in my experiments it's like +/-0.2°)

I plan to extend their library to support calibration cleanly.

davegun · Post by **davegun** » Tue Jun 23, 2015 10:58 pm

Protonstorm,

The magnetometer is very sensitive so I have found you need to find your calibration values once you have the sensor mounted in your project. If you change or add anything, you may need to redo the calibration values. If you just want to calibrate the compass, you should only need to wait until the magnetic calibration status of 3. To get full calibration, it only takes a few figure 8's. It takes a lot more fiddling around to get all 3 sensors calibrated. I have been calibrating all 3, but only use the magnetic calibration data.

You must not be getting good calibration values. My values range all over the place, as an example, here are the values for my current project:

Code: Select all

/**********************************
// Writes calibration data to 9DOF sensor//
void setCal(){
  byte calData;
  bno.setMode( bno.OPERATION_MODE_CONFIG );    // Put into CONFIG_Mode
  delay(25);
  calData = bno.setCalvalMRL(39);
  calData = bno.setCalvalMRM(3);
  calData = bno.setCalvalMOXL(151);
  calData = bno.setCalvalMOXM(255);
  calData = bno.setCalvalMOYL(70);
  calData = bno.setCalvalMOYM(254);
  calData = bno.setCalvalMOZL(44);
  calData = bno.setCalvalMOZM(0);
  bno.setMode( bno.OPERATION_MODE_NDOF );    // Put into NDOF Mode
  delay(25);
}

Once you get these values based on your installation, it should work every time without recalibration.

If you don't have good calibration values, it will act just as you said. I will show a wrong direction until you move it around. Sometime it only takes a little moving, other times a few figure 8s.

Dave

protonstorm · Post by **protonstorm** » Wed Jun 24, 2015 5:26 pm

@Dave
stupid me, after searching for like an hour, I saw that I need to change into config mode for the read too, not just for write. I got meaningful values now :)

protonstorm · Post by **protonstorm** » Wed Jun 24, 2015 6:38 pm

I finished my version of the library update. Maybe somebody wants to test it. I will probably contribute it to Github or get in contact with Kevin Townsend first.

To use this, overwrite the library cpp/h files and use the sketch (read the comments!)

Adafruit_BNO055.h:

Code: Select all

/***************************************************************************
  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

#ifdef __AVR_ATtiny85__
 #include <TinyWireM.h>
 #define Wire TinyWireM
#else
 #include <Wire.h>
#endif

#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 enum
	{ 
	  /* bit masks to read the CALIB_STAT register */
	  CALIB_STAT_SYS = 0xC0, 
	  CALIB_STAT_GYR = 0x30, 
	  CALIB_STAT_ACC = 0x0C,
	  CALIB_STAT_MAG = 0x03
	} adafruit_bno055_calib_stat_t; 

    typedef enum
    {
      VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
      VECTOR_MAGNETOMETER  = BNO055_MAG_DATA_X_LSB_ADDR,
      VECTOR_GYROSCOPE     = BNO055_GYRO_DATA_X_LSB_ADDR,
      VECTOR_EULER         = BNO055_EULER_H_LSB_ADDR,
      VECTOR_LINEARACCEL   = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
      VECTOR_GRAVITY       = BNO055_GRAVITY_DATA_X_LSB_ADDR
    } adafruit_vector_type_t;

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

    bool  begin               ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF );
	bool  getCalibState 	  ( adafruit_bno055_calib_stat_t );
	void  getCalibData		  ( byte* pbuffer);
	void  setCalibData		  ( byte* pbuffer);
    void  setMode             ( adafruit_bno055_opmode_t mode );
    void  getRevInfo          ( adafruit_bno055_rev_info_t* );
    void  displayRevInfo      ( void );
    void  setExtCrystalUse    ( boolean usextal );
    void  getSystemStatus     ( uint8_t *system_status,
                                uint8_t *self_test_result,
                                uint8_t *system_error);
    void  displaySystemStatus ( void );

    imu::Vector<3>  getVector ( adafruit_vector_type_t vector_type );
    imu::Quaternion getQuat   ( void );
    int8_t          getTemp   ( 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_BNO055.cpp:

Code: Select all

/***************************************************************************
  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)
  {
    delay(1000); // hold on for boot
    id = read8(BNO055_CHIP_ID_ADDR);
    if(id != BNO055_ID) {
      return false;  // still not? ok bail
    }
  }

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

  /* Reset */
  write8(BNO055_SYS_TRIGGER_ADDR, 0x20);
  while (read8(BNO055_CHIP_ID_ADDR) != BNO055_ID)
  {
    delay(10);
  }
  delay(50);
 
  /* Set to normal power mode */
  write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
  delay(10);

  write8(BNO055_PAGE_ID_ADDR, 0);
  
  /* Set the output units */
  /*
  uint8_t unitsel = (0 << 7) | // Orientation = Android
                    (0 << 4) | // Temperature = Celsius
                    (0 << 2) | // Euler = Degrees
                    (1 << 1) | // Gyro = Rads
                    (0 << 0);  // Accelerometer = m/s^2
  write8(BNO055_UNIT_SEL_ADDR, unitsel);
  */

  write8(BNO055_SYS_TRIGGER_ADDR, 0x0);
  delay(10);
  /* Set the requested operating mode (see section 3.3) */
  setMode(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  Use the external 32.768KHz crystal
*/
/**************************************************************************/
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
{
  adafruit_bno055_opmode_t modeback = _mode;

  /* Switch to config mode (just in case since this is the default) */
  setMode(OPERATION_MODE_CONFIG);
  delay(25);
  write8(BNO055_PAGE_ID_ADDR, 0);
  if (usextal) {
    write8(BNO055_SYS_TRIGGER_ADDR, 0x80);
  } else {
    write8(BNO055_SYS_TRIGGER_ADDR, 0x00);
  }
  delay(10);
  /* Set the requested operating mode (see section 3.3) */
  setMode(modeback);
  delay(20);
}


/**************************************************************************/
/*!
    @brief  Gets the latest system status info
*/
/**************************************************************************/
void Adafruit_BNO055::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error)
{
  adafruit_bno055_opmode_t backupmode = _mode;

  setMode(OPERATION_MODE_CONFIG);
  delay(20);
  write8(BNO055_PAGE_ID_ADDR, 0);

  write8(BNO055_SYS_TRIGGER_ADDR, read8(BNO055_SYS_TRIGGER_ADDR) | 0x1);
  delay(1000);
  
  /* 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 */
  
  if (system_status != 0)
    *system_status    = read8(BNO055_SYS_STAT_ADDR);
  
  /* 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! */
  
  if (self_test_result != 0)
    *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);

  /* 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 */
  
  if (system_error != 0)
    *system_error     = read8(BNO055_SYS_ERR_ADDR);

  setMode(backupmode);
  delay(20);
}

/**************************************************************************/
/*!
    @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));

  /* Check the accelerometer revision */
  info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR);

  /* Check the magnetometer revision */
  info->mag_rev   = read8(BNO055_MAG_REV_ID_ADDR);

  /* Check the gyroscope revision */
  info->gyro_rev  = read8(BNO055_GYRO_REV_ID_ADDR);

  /* Check the SW revision */
  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  Reads the calibration state (CALIB_STAT)
*/
/**************************************************************************/
bool Adafruit_BNO055::getCalibState(adafruit_bno055_calib_stat_t mask)
{
/* See section 3.10 */
uint8_t a;
a = read8(BNO055_CALIB_STAT_ADDR);
return ((a & mask) == mask);
}

/**************************************************************************/
/*!
    @brief  Reads the calibration data 
*/
/**************************************************************************/
void Adafruit_BNO055::getCalibData(byte * pbuffer)
{
	adafruit_bno055_opmode_t backupmode = _mode;
	setMode(OPERATION_MODE_CONFIG);
	delay(25);
    readLen(ACCEL_OFFSET_X_LSB_ADDR, pbuffer, 22); //read the whole range of offsets
	setMode(backupmode); //return to previous opmode
	delay(25);
}

/**************************************************************************/
/*!
    @brief  Writes previously acquired calibration data 
*/
/**************************************************************************/
void Adafruit_BNO055::setCalibData(byte * pbuffer)
{
	adafruit_bno055_opmode_t backupmode = _mode;
	setMode(OPERATION_MODE_CONFIG);
	delay(25);
	for(uint8_t i = 0; i < 22; i++)
	{
		Serial.println(pbuffer[i]);
		write8((adafruit_bno055_reg_t)((uint8_t)ACCEL_OFFSET_X_LSB_ADDR+i), pbuffer[i]); //write the whole range of offsets
	}
	setMode(backupmode); //return to previous opmode
	delay(25);
}

/**************************************************************************/
/*!
    @brief  Gets teh temperature in degrees celsius
*/
/**************************************************************************/
int8_t Adafruit_BNO055::getTemp(void)
{
  int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
  return temp;
}

/**************************************************************************/
/*!
    @brief  Gets a vector reading from the specified source
*/
/**************************************************************************/
imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type)
{
  imu::Vector<3> xyz;
  uint8_t buffer[6];
  memset (buffer, 0, 6);
  
  int16_t x, y, z;
  x = y = z = 0;
  
  /* Read vector data (6 bytes) */
  readLen((adafruit_bno055_reg_t)vector_type, buffer, 6);
  
  x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
  y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
  z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);

  /* Convert the value to an appropriate range (section 3.6.4) */
  /* and assign the value to the Vector type */
  switch(vector_type)
  {
    case VECTOR_MAGNETOMETER:
      /* 1uT = 16 LSB */
      xyz[0] = ((double)x)/16.0;
      xyz[1] = ((double)y)/16.0;
      xyz[2] = ((double)z)/16.0;
      break;
    case VECTOR_GYROSCOPE:
      /* 1rps = 900 LSB */
      xyz[0] = ((double)x)/900.0;
      xyz[1] = ((double)y)/900.0;
      xyz[2] = ((double)z)/900.0;
      break;
    case VECTOR_EULER:
      /* 1 degree = 16 LSB */
      xyz[0] = ((double)x)/16.0;
      xyz[1] = ((double)y)/16.0;
      xyz[2] = ((double)z)/16.0;
      break;
    case VECTOR_ACCELEROMETER:
    case VECTOR_LINEARACCEL:
    case VECTOR_GRAVITY:
      /* 1m/s^2 = 100 LSB */
      xyz[0] = ((double)x)/100.0;
      xyz[1] = ((double)y)/100.0;
      xyz[2] = ((double)z)/100.0;
      break;
  }
  
  return xyz;
}

/**************************************************************************/
/*!
    @brief  Gets a quaternion reading from the specified source
*/
/**************************************************************************/
imu::Quaternion Adafruit_BNO055::getQuat(void)
{
  uint8_t buffer[8];
  memset (buffer, 0, 8);
  
  int16_t x, y, z, w;
  x = y = z = w = 0;
  
  /* Read quat data (8 bytes) */
  readLen(BNO055_QUATERNION_DATA_W_LSB_ADDR, buffer, 8);
  w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
  x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
  y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
  z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);

  /* Assign to Quaternion */
  /* See http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_12~1.pdf
     3.6.5.5 Orientation (Quaternion)  */
  const double scale = (1.0 / (1<<14));
  imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
  return quat;
}

/**************************************************************************/
/*!
    @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)
{
  /* 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 = millis();

  /* Get a Euler angle sample for orientation */
  imu::Vector<3> euler = getVector(Adafruit_BNO055::VECTOR_EULER);
  event->orientation.x = euler.x();
  event->orientation.y = euler.y();
  event->orientation.z = euler.z();

  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;
}

Example Sketch for calibration:

Code: Select all

/*******************************************************************************************************************************************************************************************
*  Adafruit BNO055 Sensor Calibration sample
*  Refer to http://www.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
*  Section 3.10 on how to calibrate your sensor:
*   -Run this sketch
*   -To calibrate the Gyroscope, just let the sensor sit for a couple of seconds until GYR Calibration shows 1
*   -To calibrate the Magnetometer, move your sensor in random figure 8 patterns until MAG Calibration shows 1
*   -If you want to calibrate the acceleration sensor, move the sensor to 6 stable positions slowly, 3 of those should be in the XY/XZ/YZ plane
*   -Once you are happy with the readings you get, copy the c_data = .... line from serial.console to the setup() of your sketch and write it to the sensor with "bno.setCalibData(c_data);"
********************************************************************************************************************************************************************************************/

#include <Adafruit_Sensor.h>
#include <utility/imumaths.h>
#include <Wire.h>

#include <Adafruit_BNO055.h>
Adafruit_BNO055 bno = Adafruit_BNO055(55);

void setup()
{
  Serial.begin(9600);
  Serial.println("Orientation Sensor Calibration"); Serial.println("");

  /* Initialise the sensor */
  if (!bno.begin(Adafruit_BNO055::OPERATION_MODE_NDOF)) //if you want to calibrate using another mode, set it here. OPERATION_MODE_COMPASS for a precise tilt compensated compass (Section 3.3.2 / 3.3.3)
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while (1);
  }
  
  //You will need the next two lines in your own sketch, _immediatly_ after bno.begin() to use a predefined calibration
  //byte c_data[22] = {0, 0, 0, 0, 0, 0, 172, 250, 112, 255, 52, 253, 0, 0, 253, 255, 255, 255, 232, 3, 240, 2}; //replace this line with the serial output of this sketch
  //bno.setCalibData(c_data);
  
  delay(1000);
  bno.setExtCrystalUse(true);
}

void loop()
{
  sensors_event_t event;
  bno.getEvent(&event);

  //print the euler angles for reference
  Serial.print("X: "); //"heading"
  Serial.print(event.orientation.x, 4);
  Serial.print(" Y: ");
  Serial.print(event.orientation.y, 4);
  Serial.print(" Z: ");
  Serial.print(event.orientation.z, 4);
  Serial.println();

  //show the calibration states for all 3 sensors + system. Optimally, in NDOF mode you want all these to read "1" before you assume good calibration (Note: Some sensors are off in other modes)
  bool bMAG = bno.getCalibState(Adafruit_BNO055::CALIB_STAT_MAG);
  Serial.print("MAG Calibration: ");
  Serial.println(bMAG);
  bool bGYR = bno.getCalibState(Adafruit_BNO055::CALIB_STAT_GYR);
  Serial.print("GYR Calibration: ");
  Serial.println(bGYR);
  bool bACC = bno.getCalibState(Adafruit_BNO055::CALIB_STAT_ACC);
  Serial.print("ACC Calibration: ");
  Serial.println(bACC);
  bool b = bno.getCalibState(Adafruit_BNO055::CALIB_STAT_SYS);
  Serial.print("SYS Calibration: ");
  Serial.println(b);

  byte buffer[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  bno.getCalibData(buffer);
  //print the actual line to paste into your sketch (see setup() above)
  Serial.print("byte c_data[22] = {");
  for (int i = 0; i < 22; i++)
  {
    Serial.print(buffer[i]); 
    if(i!=21) Serial.print(", ");
  }
  Serial.println("};");
  
  delay(500);
}

davegun · Post by **davegun** » Wed Jun 24, 2015 11:00 pm

protonstorm,

Very nice!! Your version has much cleaner than mine :) I'm going to look at it closer, I'm sure I will learn something.

Without calibration, this sensor is just about useless unless you only need to make a bunny jump around the screen. With calibration, it enables the sensor to perform as it should and do amazing things. It would be nice to see your modifications added to the official library!

I have copied the files, and will give it a test when I can. I'm in the middle of a project now, so it my be a bit.

Thank You!!

Dave

PS - I made a short video showing a demo of the compass using the BNO055. This shows how stable it is even when tilted. The NNO055 is under the Ultimate GPS so it isn't visible. Note the red indicator, this is a north pointing arrow. I have calibration values that are loaded at startup, so it gives correct magnetic values when it powers on.

http://youtu.be/815veaVXh2g

adafruit industries

BNO055 IMU - Find magnetic north

BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north

Re: BNO055 IMU - Find magnetic north