BNO055 Intelligent 9-axis Absolute Orientation Sensor Module Wiki - DFRobot

Introduction

BNO055 is a new sensor IC for implementing an intelligent 9-axis Absolute Orientation Sensor. It is a System in Package, integrating a triaxial 14-bit accelerometer, a triaxial 16-bit gyroscope, a triaxial geomagnetic sensor and a 32-bit microcontroller.
With a size of 5.2 x 3.8 x 1.1mm, the BNO055 is significantly smaller than comparable discrete or system-on-board solutions and also is the sensor-hub product of the smallest size that supports Windows 8.1 at present.
BNO055 is able to provide not only single data of the three kinds of sensors (accelerometer/gyroscope/geomagnetic), but also integrated data, such as quaternions, Euler angles or vectors. Besides, the built-in MCU frees the users from the complexities of algorithm processing, which provides application support in many aspects for smart phone, wearable device and so on.

Features

Specification

Dimension

Note: the sensor default I2C address is 0X28.

Board Overview

Front View

Back View

Silkscreen Function Description
VCC +
GND -
SCL I2C Clock
SDA I2C Data
INT Interrupt Pin
BOOT Lead Mode select pin
PS1 Protocol Select Pin 1
PS2 Protocol Select Pin 2
BL-IND Lead Program Guide
RST Reset Pin
ADDR I2C Address Select
PS1 PS2 Function
0 0 Standard/Fast 12C Interface
0 1 HID OVER I2C
1 0 UART Interface
1 1 Reserved

Note: the PS1 and SP2 are default to be 0, 0.

API Functions

class DFRobot_BNO055 {

public:
  /**
   * @brief global axis declare (excepet eular and quaternion)
   */
  typedef enum {
    eAxisAcc,
    eAxisMag,
    eAxisGyr,
    eAxisLia,
    eAxisGrv
  } eAxis_t;

  /**
   * @brief global single axis declare
   */
  typedef enum {
    eSingleAxisX,
    eSingleAxisY,
    eSingleAxisZ
  } eSingleAxis_t;

  /**
   * @brief enum interrupt
   */
  typedef enum {
    eIntGyrAm = 0x04,
    eIntGyrHighRate = 0x08,
    eIntAccHighG = 0x20,
    eIntAccAm = 0x40,
    eIntAccNm = 0x80,
    eIntAll = 0xec
  } eInt_t;

  /**
   * @brief Operation mode enum
   */
  typedef enum {
    eOprModeConfig,
    eOprModeAccOnly,
    eOprModeMagOnly,
    eOprModeGyroOnly,
    eOprModeAccMag,
    eOprModeAccGyro,
    eOprModeMagGyro,
    eOprModeAMG,
    eOprModeImu,
    eOprModeCompass,
    eOprModeM4G,
    eOprModeNdofFmcOff,
    eOprModeNdof
  } eOprMode_t;

  /**
   * @brief Poewr mode enum
   */
  typedef enum {
    ePowerModeNormal,
    ePowerModeLowPower,
    ePowerModeSuspend
  } ePowerMode_t;

  /**
   * @brief axis analog data struct
   */
  typedef struct {
    float   x, y, z;
  } sAxisAnalog_t;

  /**
   * @brief eular analog data struct
   */
  typedef struct {
    float   head, roll, pitch;
  } sEulAnalog_t;

  /**
   * @brief qua analog data struct
   */
  typedef struct {
    float   w, x, y, z;
  } sQuaAnalog_t;

  /**
   * @brief enum accelerometer range, unit G
   */
  typedef enum {
    eAccRange_2G,
    eAccRange_4G,
    eAccRange_8G,
    eAccRange_16G
  } eAccRange_t;

  /**
   * @brief enum accelerometer band width, unit HZ
   */
  typedef enum {
    eAccBandWidth_7_81,    // 7.81HZ
    eAccBandWidth_15_63,   // 16.63HZ
    eAccBandWidth_31_25,
    eAccBandWidth_62_5,
    eAccBandWidth_125,
    eAccBandWidth_250,
    eAccBandWidth_500,
    eAccBandWidth_1000
  } eAccBandWidth_t;

  /**
   * @brief enum accelerometer power mode
   */
  typedef enum {
    eAccPowerModeNormal,
    eAccPowerModeSuspend,
    eAccPowerModeLowPower1,
    eAccPowerModeStandby,
    eAccPowerModeLowPower2,
    eAccPowerModeDeepSuspend
  } eAccPowerMode_t;

  /**
   * @brief enum magnetometer data output rate, unit HZ
   */
  typedef enum {
    eMagDataRate_2,
    eMagDataRate_6,
    eMagDataRate_8,
    eMagDataRate_10,
    eMagDataRate_15,
    eMagDataRate_20,
    eMagDataRate_25,
    eMagDataRate_30
  } eMagDataRate_t;

  /**
   * @brief enum magnetometer operation mode
   */
  typedef enum {
    eMagOprModeLowPower,
    eMagOprModeRegular,
    eMagOprModeEnhancedRegular,
    eMagOprModeHighAccuracy
  } eMagOprMode_t;

  /**
   * @brief enum magnetometer power mode
   */
  typedef enum {
    eMagPowerModeNormal,
    eMagPowerModeSleep,
    eMagPowerModeSuspend,
    eMagPowerModeForce
  } eMagPowerMode_t;

  /**
   * @brief enum gyroscope range, unit dps
   */
  typedef enum {
    eGyrRange_2000,
    eGyrRange_1000,
    eGyrRange_500,
    eGyrRange_250,
    eGyrRange_125
  } eGyrRange_t;

  /**
   * @brief enum gyroscope band width, unit HZ
   */
  typedef enum {
    eGyrBandWidth_523,
    eGyrBandWidth_230,
    eGyrBandWidth_116,
    eGyrBandWidth_47,
    eGyrBandWidth_23,
    eGyrBandWidth_12,
    eGyrBandWidth_64,
    eGyrBandWidth_32
  } eGyrBandWidth_t;

  /**
   * @brief enum gyroscope power mode
   */
  typedef enum {
    eGyrPowerModeNormal,
    eGyrPowerModeFastPowerUp,
    eGyrPowerModeDeepSuspend,
    eGyrPowerModeSuspend,
    eGyrPowerModeAdvancedPowersave
  } eGyrPowerMode_t;

  /**
   * @brief Enum accelerometer interrupt settings
   */
  typedef enum {
    eAccIntSetAmnmXAxis = (0x01 << 2),
    eAccIntSetAmnmYAxis = (0x01 << 3),
    eAccIntSetAmnmZAxis = (0x01 << 4),
    eAccIntSetHgXAxis = (0x01 << 5),
    eAccIntSetHgYAxis = (0x01 << 6),
    eAccIntSetHgZAxis = (0x01 << 7),
    eAccIntSetAll = 0xfc
  } eAccIntSet_t;

  /**
   * @brief Enum accelerometer slow motion mode or no motion mode
   */
  typedef enum {
    eAccNmSmnmSm,  // slow motion mode
    eAccNmSmnmNm   // no motion mode
  } eAccNmSmnm_t;

  /**
   * @brief Enum gyroscope interrupt settings
   */
  typedef enum {
    eGyrIntSetAmXAxis = (0x01 << 0),
    eGyrIntSetAmYAxis = (0x01 << 1),
    eGyrIntSetAmZAxis = (0x01 << 2),
    eGyrIntSetHrXAxis = (0x01 << 3),
    eGyrIntSetHrYAxis = (0x01 << 4),
    eGyrIntSetHrZAxis = (0x01 << 5),
    eGyrIntSetAmFilt = (0x01 << 6),
    eGyrIntSetHrFilt = (0x01 << 7),
    eGyrIntSetAll = 0x3f
  } eGyrIntSet_t;

  /**
   * @brief Declare sensor status
   */
  typedef enum {
    eStatusOK,    // everything OK
    eStatusErr,   // unknow error
    eStatusErrDeviceNotDetect,    // device not detected
    eStatusErrDeviceReadyTimeOut, // device ready time out
    eStatusErrDeviceStatus,       // device internal status error
    eStatusErrParameter           // function parameter error
  } eStatus_t;

  /**
   * @brief begin Sensor begin
   * @return Sensor status
   */
  eStatus_t   begin();

  /**
   * @brief getAxisAnalog Get axis analog data
   * @param eAxis One axis type from eAxis_t
   * @return Struct sAxisAnalog_t, contains axis analog data, members unit depend on eAxis:
   *                case eAxisAcc, unit mg
   *                case eAxisLia, unit mg
   *                case eAxisGrv, unit mg
   *                case eAxisMag, unit ut
   *                case eAxisGyr, unit dps
   */
  sAxisAnalog_t getAxis(eAxis_t eAxis);

  /**
   * @brief getEulAnalog Get euler analog data
   * @return Struct sEulAnalog_t, contains euler analog data
   */
  sEulAnalog_t  getEul();

  /**
   * @brief getQuaAnalog Get quaternion analog data
   * @return Struct sQuaAnalog_t, contains quaternion analog data
   */
  sQuaAnalog_t  getQua();

  /**
   * @brief setAccOffset Set axis offset data
   * @param eAxis One axis type from eAxis_t, only support accelerometer, magnetometer and gyroscope
   * @param sOffset Struct sAxisAnalog_t, contains axis analog data, members unit depend on eAxis:
   *                case eAxisAcc, unit mg, members can't out of acc range
   *                case eAxisMag, unit ut, members can't out of mag range
   *                case eAxisGyr, unit dps, members can't out of gyr range
   */
  void    setAxisOffset(eAxis_t eAxis, sAxisAnalog_t sOffset);

  /**
   * @brief setOprMode Set operation mode
   * @param eOpr One operation mode from eOprMode_t
   */
  void    setOprMode(eOprMode_t eMode);

  /**
   * @brief setPowerMode Set power mode
   * @param eMode One power mode from ePowerMode_t
   */
  void    setPowerMode(ePowerMode_t eMode);

  /**
   * @brief Reset sensor
   */
  void    reset();

  /**
   * @brief setAccRange Set accelerometer measurement range, default value is 4g
   * @param eRange One range enum from eAccRange_t
   */
  void    setAccRange(eAccRange_t eRange);

  /**
   * @brief setAccBandWidth Set accelerometer band width, default value is 62.5hz
   * @param eBand One band enum from eAccBandWidth_t
   */
  void    setAccBandWidth(eAccBandWidth_t eBand);

  /**
   * @brief setAccPowerMode Set accelerometer power mode, default value is eAccPowerModeNormal
   * @param eMode One mode enum from eAccPowerMode_t
   */
  void    setAccPowerMode(eAccPowerMode_t eMode);

  /**
   * @brief setMagDataRate Set magnetometer data output rate, default value is 20hz
   * @param eRate One rate enum from eMagDataRate_t
   */
  void    setMagDataRate(eMagDataRate_t eRate);

  /**
   * @brief setMagOprMode Set magnetometer operation mode, default value is eMagOprModeRegular
   * @param eMode One mode enum from eMagOprMode_t
   */
  void    setMagOprMode(eMagOprMode_t eMode);

  /**
   * @brief setMagPowerMode Set magnetometer power mode, default value is eMagePowerModeForce
   * @param eMode One mode enum from eMagPowerMode_t
   */
  void    setMagPowerMode(eMagPowerMode_t eMode);

  /**
   * @brief setGyrRange Set gyroscope range, default value is 2000
   * @param eRange One range enum from eGyrRange_t
   */
  void    setGyrRange(eGyrRange_t eRange);

  /**
   * @brief setGyrBandWidth Set gyroscope band width, default value is 32HZ
   * @param eBandWidth One band width enum from eGyrBandWidth_t
   */
  void    setGyrBandWidth(eGyrBandWidth_t eBandWidth);

  /**
   * @brief setGyrPowerMode Set gyroscope power mode, default value is eGyrPowerModeNormal
   * @param eMode One power mode enum from eGyrPowerMode_t
   */
  void    setGyrPowerMode(eGyrPowerMode_t eMode);

  /**
   * @brief getIntState Get interrupt state, interrupt auto clear after read
   * @return If result > 0, at least one interrupt triggered. Result & eIntXXX (from eInt_t) to test is triggered
   */
  uint8_t   getIntState();

  /**
   * @brief setIntMask Set interrupt mask enable, there will generate a interrupt signal (raising) on INT pin if corresponding interrupt enabled
   * @param eInt One or more interrupt flags to set, input them through operate or
   */
  void    setIntMaskEnable(eInt_t eInt);

  /**
   * @brief setIntMaskDisable Set corresponding interrupt mask disable
   * @param eInt One or more interrupt flags to set, input them through operate or
   */
  void    setIntMaskDisable(eInt_t eInt);

  /**
   * @brief setIntEnEnable Set corresponding interrupt enable
   * @param eInt One or more interrupt flags to set, input them through operate or
   */
  void    setIntEnable(eInt_t eInt);

  /**
   * @brief setIntEnDisable Set corresponding interrupt disable
   * @param eInt One or more interrupt flags to set, input them through operate or
   */
  void    setIntDisable(eInt_t eInt);

  /**
   * @brief setAccAmThres Set accelerometer any motion threshold
   * @param thres Threshold to set, unit mg, value is dependent on accelerometer range selected,
   *        case 2g, no more than 1991
   *        case 4g, no more than 3985
   *        case 8g, no more than 7968
   *        case 16g, no more than 15937
   *        Attenion: The set value will be slightly biased according to datasheet
   */
  void    setAccAmThres(uint16_t thres);

  /**
   * @brief setAccIntDur Set accelerometer interrupt duration,
   *        any motion interrupt triggers if duration (dur + 1) consecutive data points are above the any motion interrupt
   *        threshold define in any motion threshold
   * @param dur Duration to set, range form 1 to 4
   */
  void    setAccIntAmDur(uint8_t dur);

  /**
   * @brief setAccIntEnable Set accelerometer interrupt enable
   * @param eInt One or more interrupt flags to set, input them through operate or
   */
  void    setAccIntEnable(eAccIntSet_t eInt);

  /**
   * @brief setAccIntDisable Set accelerometer interrupt disable
   * @param eInt One or more interrupt flags to set, input them through operate or
   */
  void    setAccIntDisable(eAccIntSet_t eInt);

  /**
   * @brief setAccHighGDuration Set accelerometer high-g interrupt, the high-g interrupt delay according to [dur + 1] * 2 ms
   * @param dur Duration from 2ms to 512ms
   */
  void    setAccHighGDuration(uint16_t dur);

  /**
   * @brief setAccHighGThres Set accelerometer high-g threshold
   * @param thres Threshold to set, unit mg, value is dependent on accelerometer range selected,
   *        case 2g, no more than 1991
   *        case 4g, no more than 3985
   *        case 8g, no more than 7968
   *        case 16g, no more than 15937
   *        Attenion: The set value will be slightly biased according to datasheet
   */
  void    setAccHighGThres(uint16_t thres);

  /**
   * @brief setAccNmThres Set accelerometer no motion threshold
   * @param thres Threshold to set, unit mg, value is dependent on accelerometer range selected,
   *        case 2g, no more than 1991
   *        case 4g, no more than 3985
   *        case 8g, no more than 7968
   *        case 16g, no more than 15937
   *        Attenion: The set value will be slightly biased according to datasheet
   */
  void    setAccNmThres(uint16_t thres);

  /**
   * @brief setAccNmSet Set accelerometer slow motion or no motion mode and duration
   * @param eSmnm Enum of eAccNmSmnm_t
   * @param dur Interrupt trigger delay (unit seconds), no more than 344.
   *            Attenion: The set value will be slightly biased according to datasheet
   */
  void    setAccNmSet(eAccNmSmnm_t eSmnm, uint16_t dur);

  /**
   * @brief setGyrIntEnable Set corresponding gyroscope interrupt enable
   * @param eInt One or more interrupt flags to set, input them through operate or
   */
  void    setGyrIntEnable(eGyrIntSet_t eInt);

  /**
   * @brief setGyrIntDisable Set corresponding gyroscope interrupt disable
   * @param eInt One or more interrupt flags to set, input them through operate or
   */
  void    setGyrIntDisable(eGyrIntSet_t eInt);

  /**
   * @brief setGyrHrSet Set gyroscope high rate settings
   * @param eSingleAxis Single axis to set
   * @param thres High rate threshold to set, unit degree/seconds, value is dependent on gyroscope range selected,
   *        case 2000, no more than 1937
   *        case 1000, no more than 968
   *        case 500, no more than 484
   *        case 250, no more than 242
   *        case 125, no more than 121
   *        Attenion: The set value will be slightly biased according to datasheet
   * @param dur High rate duration to set, unit ms, duration from 2.5ms to 640ms
   *            Attenion: The set value will be slightly biased according to datasheet
   */
  void    setGyrHrSet(eSingleAxis_t eSingleAxis, uint16_t thres, uint16_t dur);

  /**
   * @brief setGyrAmThres Set gyroscope any motion threshold
   * @param thres Threshold to set, unit mg, value is dependent on accelerometer range selected,
   *        case 2000, no more than 128
   *        case 1000, no more than 64
   *        case 500, no more than 32
   *        case 250, no more than 16
   *        case 125, no more than 8
   *        Attenion: The set value will be slightly biased according to datasheet
   */
  void    setGyrAmThres(uint8_t thres);

  /**
   * @brief lastOpreateStatus Show last operate status
   */
  eStatus_t   lastOpreateStatus;
};

class DFRobot_BNO055_IIC : public DFRobot_BNO055 {
public:

  /**
   * @brief DFRobot_BNO055_IIC class constructor
   * @param pWire select One TwoWire peripheral
   * @param addr Sensor address
   */
  DFRobot_BNO055_IIC(TwoWire *pWire, uint8_t addr);
};

Tutorial

Visit the I2C address of BNO055 via I2C interface to get the related position data.

Requirements

Connection Diagram

Connection Diagram

BMX160 Tutorial

Function: read the pitch angle, roll angle and yaw angle of BNO055 sensor via I2C interface, and print out the data through the serial port. Using this demo with a small visual software Euler angle visual tool.exe we specifically designed, you can directly observe the attitude variation of 10DOF. As shown below.

/*!
  * imu_show.ino
  *
  * Download this demo to show attitude on [imu_show](https://github.com/DFRobot/DFRobot_IMU_Show)
  * Attitude will show on imu_show
  *
  * Product: http://www.dfrobot.com.cn/goods-1860.html
  * Copyright   [DFRobot](http://www.dfrobot.com), 2016
  * Copyright   GNU Lesser General Public License
  *
  * version  V1.0
  * date  07/03/2019
  */

#include "DFRobot_BNO055.h"
#include "Wire.h"

typedef DFRobot_BNO055_IIC    BNO;    // ******** use abbreviations instead of full names ********

BNO   bno(&Wire, 0x28);    // input TwoWire interface and IIC address

// show last sensor operate status
void printLastOperateStatus(BNO::eStatus_t eStatus)
{
  switch(eStatus) {
  case BNO::eStatusOK:    Serial.println("everything ok"); break;
  case BNO::eStatusErr:   Serial.println("unknow error"); break;
  case BNO::eStatusErrDeviceNotDetect:    Serial.println("device not detected"); break;
  case BNO::eStatusErrDeviceReadyTimeOut: Serial.println("device ready time out"); break;
  case BNO::eStatusErrDeviceStatus:       Serial.println("device internal status error"); break;
  default: Serial.println("unknow status"); break;
  }
}

void setup()
{
  Serial.begin(115200);
  bno.reset();
  while(bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin faild");
    printLastOperateStatus(bno.lastOperateStatus);
    delay(2000);
  }
  Serial.println("bno begin success");
}

void loop()
{
  BNO::sEulAnalog_t   sEul;
  sEul = bno.getEul();
  Serial.print("pitch:");
  Serial.print(sEul.pitch, 3);
  Serial.print(" ");
  Serial.print("roll:");
  Serial.print(sEul.roll, 3);
  Serial.print(" ");
  Serial.print("yaw:");
  Serial.print(sEul.head, 3);
  Serial.println(" ");
  delay(80);
}

If we compare 10DOF to an airplane whose nose points at due east, the positive direction of X axis will be the direction of the nose, the positive direction of Y axis will be the direction of the left wing, which is due north. Z axis is perpendicular to the plane XOY that formed by X and Y axes. When the 10 DOF’s direction of X, Y, and Z totally coincides with the above-mentioned direction, the values of the pitch, roll and yaw angle are 0°. Here we define: pitch is the angle between the nose and XOY when the airplane noses up or down along the Y axis, and nose up is positive while nose down is negative; roll is the angle between the body and XOY when the airplane rolls along the X axis; yaw is the angle between the nose and XOZ when the airplane moves along the Z axis.

warning_yellow.png Please note that you need to close the serial port occupied by the printer when using the test software to observe the sensor’s movement posture.

Sample Code

Function: get the acceleration data of the sensor’s movement on X, Z and Y, and print it out through the serial port.

/*!
  * read_data.ino
  *
  * Download this demo to test read data from bno055
  * Data will print on your serial monitor
  *
  * Product: http://www.dfrobot.com.cn/goods-1860.html
  * Copyright   [DFRobot](http://www.dfrobot.com), 2016
  * Copyright   GNU Lesser General Public License
  *
  * version  V1.0
  * date  07/03/2019
  */

#include "DFRobot_BNO055.h"
#include "Wire.h"

typedef DFRobot_BNO055_IIC    BNO;    // ******** use abbreviations instead of full names ********

BNO   bno(&Wire, 0x28);    // input TwoWire interface and IIC address

// show last sensor operate status
void printLastOperateStatus(BNO::eStatus_t eStatus)
{
  switch(eStatus) {
  case BNO::eStatusOK:   Serial.println("everything ok"); break;
  case BNO::eStatusErr:  Serial.println("unknow error"); break;
  case BNO::eStatusErrDeviceNotDetect:   Serial.println("device not detected"); break;
  case BNO::eStatusErrDeviceReadyTimeOut:    Serial.println("device ready time out"); break;
  case BNO::eStatusErrDeviceStatus:    Serial.println("device internal status error"); break;
  default: Serial.println("unknow status"); break;
  }
}

void setup()
{
  Serial.begin(115200);
  bno.reset();
  while(bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin faild");
    printLastOperateStatus(bno.lastOperateStatus);
    delay(2000);
  }
  Serial.println("bno begin success");
}

#define printAxisData(sAxis) \
  Serial.print(" x: "); \
  Serial.print(sAxis.x); \
  Serial.print(" y: "); \
  Serial.print(sAxis.y); \
  Serial.print(" z: "); \
  Serial.println(sAxis.z)

void loop()
{
  BNO::sAxisAnalog_t   sAccAnalog, sMagAnalog, sGyrAnalog, sLiaAnalog, sGrvAnalog;
  BNO::sEulAnalog_t    sEulAnalog;
  BNO::sQuaAnalog_t    sQuaAnalog;
  sAccAnalog = bno.getAxis(BNO::eAxisAcc);    // read acceleration
  sMagAnalog = bno.getAxis(BNO::eAxisMag);    // read geomagnetic
  sGyrAnalog = bno.getAxis(BNO::eAxisGyr);    // read gyroscope
  sLiaAnalog = bno.getAxis(BNO::eAxisLia);    // read linear acceleration
  sGrvAnalog = bno.getAxis(BNO::eAxisGrv);    // read gravity vector
  sEulAnalog = bno.getEul();                  // read euler angle
  sQuaAnalog = bno.getQua();                  // read quaternion
  Serial.println();
  Serial.println("======== analog data print start ========");
  Serial.print("acc analog: (unit mg)       "); printAxisData(sAccAnalog);
  Serial.print("mag analog: (unit ut)       "); printAxisData(sMagAnalog);
  Serial.print("gyr analog: (unit dps)      "); printAxisData(sGyrAnalog);
  Serial.print("lia analog: (unit mg)       "); printAxisData(sLiaAnalog);
  Serial.print("grv analog: (unit mg)       "); printAxisData(sGrvAnalog);
  Serial.print("eul analog: (unit degree)   "); Serial.print(" head: "); Serial.print(sEulAnalog.head); Serial.print(" roll: "); Serial.print(sEulAnalog.roll);  Serial.print(" pitch: "); Serial.println(sEulAnalog.pitch);
  Serial.print("qua analog: (no unit)       "); Serial.print(" w: "); Serial.print(sQuaAnalog.w); printAxisData(sQuaAnalog);
  Serial.println("========  analog data print end  ========");

  delay(1000);
}

Sample Code

Function: moitor the sensor interrupts, including high/low speed interrupt, and fast tilt interrupt.

/*!
  * interrupt.ino
  *
  * Download this demo to test bno055 interrupt
  * Connect bno055 int pin to arduino pin 2
  * If there occurs interrupt, it will printr on you serial monitor, more detail please refer to comments
  *
  * Product: http://www.dfrobot.com.cn/goods-1860.html
  * Copyright   [DFRobot](http://www.dfrobot.com), 2016
  * Copyright   GNU Lesser General Public License
  *
  * version  V1.0
  * date  07/03/2019
  */

#include "DFRobot_BNO055.h"
#include "Wire.h"

typedef DFRobot_BNO055_IIC    BNO;    // ******** use abbreviations instead of full names ********

BNO   bno(&Wire, 0x28);    // input TwoWire interface and IIC address

// show last sensor operate status
void printLastOperateStatus(BNO::eStatus_t eStatus)
{
  switch(eStatus) {
  case BNO::eStatusOK:    Serial.println("everything ok"); break;
  case BNO::eStatusErr:   Serial.println("unknow error"); break;
  case BNO::eStatusErrDeviceNotDetect:    Serial.println("device not detected"); break;
  case BNO::eStatusErrDeviceReadyTimeOut: Serial.println("device ready time out"); break;
  case BNO::eStatusErrDeviceStatus:       Serial.println("device internal status error"); break;
  default: Serial.println("unknow status"); break;
  }
}

bool intFlag = false;

void intHandle()
{
  intFlag = true;
}

void setup()
{
  Serial.begin(115200);
  bno.reset();
  while(bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin faild");
    printLastOperateStatus(bno.lastOperateStatus);
    delay(2000);
  }
  Serial.println("bno begin success");

  bno.setOprMode(BNO::eOprModeConfig);    // set to config mode

  bno.setIntMaskEnable(BNO::eIntAll);    // set interrupt mask enable, signal to int pin when interrupt
  // bno.setIntMaskDisable(BNO::eIntAccAm | BNO::eIntAccNm);    // set interrupt mask disable, no signal to int pin when interrupt

  bno.setIntEnable(BNO::eIntAll);   // set interrupt enable
  // bno.setIntDisable(BNO::eIntAccAm | BNO::eIntAccNm);    // set interrupt disable

  bno.setAccIntEnable(BNO::eAccIntSetAll);    // set accelerometer interrupt enable
  // bno.setAccIntDisable(BNO::eAccIntSetAmnmXAxis | BNO::eAccIntSetHgXAxis);   // set accelerometer interrupt disable

  /* accelerometer any motion threshold to set, unit mg, value is dependent on accelerometer range selected,
   * case 2g, no more than 1991
   * case 4g, no more than 3985
   * case 8g, no more than 7968
   * case 16g, no more than 15937
   * attenion: The set value will be slightly biased according to datasheet
   * tips: default accelerometer range is 4g
   */
  // how to trig this: still --> fast move
  bno.setAccAmThres(200);
  // any motion interrupt triggers if duration consecutive data points are above the any motion interrupt
  // threshold define in any motion threshold
  bno.setAccIntAmDur(1);
  // set high-g duration, value from 2ms to 512ms
  bno.setAccHighGDuration(80);
  /*
   * accelerometer high-g threshold to set, unit mg, value is dependent on accelerometer range selected,
   * case 2g, no more than 1991
   * case 4g, no more than 3985
   * case 8g, no more than 7968
   * case 16g, no more than 15937
   * Attenion: The set value will be slightly biased according to datasheet
   */
  // how to trig this: still --> (very) fast move
  bno.setAccHighGThres(900);
  // accelerometer (no motion) / (slow motion) settings, 2nd parameter unit seconds, no more than 344
  bno.setAccNmSet(BNO::eAccNmSmnmNm, 4);
  /*
   * accelerometer no motion threshold to set, unit mg, value is dependent on accelerometer range selected,
   * case 2g, no more than 1991
   * case 4g, no more than 3985
   * case 8g, no more than 7968
   * case 16g, no more than 15937
   * Attenion: The set value will be slightly biased according to datasheet
   */
  // hot to trig this: any motion --> still --> still
  bno.setAccNmThres(100);

  bno.setGyrIntEnable((BNO::eGyrIntSet_t) (BNO::eGyrIntSetHrXAxis | BNO::eGyrIntSetHrYAxis | BNO::eGyrIntSetHrZAxis));    // set gyroscope interrupt enable, in most cases, this is enough.
  // bno.setGyrIntEnable(BNO::eGyrIntSetAmYAxis | BNO::eGyrIntSetAmYAxis | BNO::eGyrIntSetAmZAxis);    // set gyroscope interrupt enable
  // bno.setGyrIntDisable(BNO::eGyrIntSetHrXAxis | BNO::eGyrIntSetAmXAxis);    // set gyroscope interrupt disable

  /*
   * 2nd parameter, high rate threshold to set, unit degree/seconds, value is dependent on gyroscope range selected,
   * case 2000, no more than 1937
   * case 1000, no more than 968
   * case 500, no more than 484
   * case 250, no more than 242
   * case 125, no more than 121
   * Attenion: The set value will be slightly biased according to datasheet
   * 3rd parameter, high rate duration to set, unit ms, duration from 2.5ms to 640ms
   * Attenion: The set value will be slightly biased according to datasheet
   */
  // how to trigger this: still --> fast tilt
  bno.setGyrHrSet(BNO::eSingleAxisX, 300, 80);
  bno.setGyrHrSet(BNO::eSingleAxisY, 300, 80);
  bno.setGyrHrSet(BNO::eSingleAxisZ, 300, 80);
  /*
   * gyroscope any motion threshold to set, unit mg, value is dependent on accelerometer range selected,
   * case 2000, no more than 128
   * case 1000, no more than 64
   * case 500, no more than 32
   * case 250, no more than 16
   * case 125, no more than 8
   * Attenion: The set value will be slightly biased according to datasheet
   * tips: default range is 2000
   */
  // how to trigger this: still --> fast tilt
  bno.setGyrAmThres(20);

  bno.setOprMode(BNO::eOprModeNdof);    // configure done

  attachInterrupt(0, intHandle, RISING);   // attach interrupt
  bno.getIntState();    // clear unexpected interrupt
  intFlag = false;
}

void loop()
{
  if(intFlag) {
    intFlag = false;
    uint8_t   intSta = bno.getIntState();   // interrupt auto clear after read

    Serial.println("interrupt detected");
    if(intSta & BNO::eIntAccAm)
      Serial.println("accelerometer any motion detected");
    if(intSta & BNO::eIntAccNm)
      Serial.println("accelerometer no motion detected");
    if(intSta & BNO::eIntAccHighG)
      Serial.println("acceleromter high-g detected");
    if(intSta & BNO::eIntGyrHighRate)
      Serial.println("gyroscope high rate detected");
    if(intSta & BNO::eIntGyrAm)
      Serial.println("gyroscope any motion detected");
  }
}

Sample Code

Function: sensor configuration.

/*!
  * config.ino
  *
  * Download this demo to test config to bno055
  * Data will print on your serial monitor
  *
  * Product: http://www.dfrobot.com.cn/goods-1860.html
  * Copyright   [DFRobot](http://www.dfrobot.com), 2016
  * Copyright   GNU Lesser General Public License
  *
  * version  V1.0
  * date  07/03/2019
  */

#include "DFRobot_BNO055.h"
#include "Wire.h"

typedef DFRobot_BNO055_IIC    BNO;    // ******** use abbreviations instead of full names ********

BNO   bno(&Wire, 0x28);    // input TwoWire interface and IIC address

// show last sensor operate status
void printLastOperateStatus(BNO::eStatus_t eStatus)
{
  switch(eStatus) {
  case BNO::eStatusOK:    Serial.println("everything ok"); break;
  case BNO::eStatusErr:   Serial.println("unknow error"); break;
  case BNO::eStatusErrDeviceNotDetect:    Serial.println("device not detected"); break;
  case BNO::eStatusErrDeviceReadyTimeOut: Serial.println("device ready time out"); break;
  case BNO::eStatusErrDeviceStatus:       Serial.println("device internal status error"); break;
  default: Serial.println("unknow status"); break;
  }
}

void setup()
{
  Serial.begin(115200);
  bno.reset();
  while(bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin faild");
    printLastOperateStatus(bno.lastOperateStatus);
    delay(2000);
  }
  Serial.println("bno begin success");

  bno.setPowerMode(BNO::ePowerModeNormal);    // set to normal power mode
  bno.setOprMode(BNO::eOprModeConfig);    // must set sensor to config-mode before configure
  bno.setAccPowerMode(BNO::eAccPowerModeNormal);    // set acc to normal power mode
  bno.setGyrPowerMode(BNO::eGyrPowerModeNormal);    // set gyr to normal power mode
  bno.setMagPowerMode(BNO::eMagPowerModeForce);     // set mag to force power mode

  // accelerometer normal configure
  bno.setAccRange(BNO::eAccRange_4G);   // set range to 4g
  bno.setAccBandWidth(BNO::eAccBandWidth_62_5);   // set band width 62.5HZ
  bno.setAccPowerMode(BNO::eAccPowerModeNormal);  // set accelerometer power mode

  // magnetometer normal configure
  bno.setMagDataRate(BNO::eMagDataRate_20);   // set output data rate 20HZ
  bno.setMagPowerMode(BNO::eMagPowerModeForce);   // set power mode
  bno.setMagOprMode(BNO::eMagOprModeRegular); // set operate mode

  // gyroscope normal configure
  bno.setGyrRange(BNO::eGyrRange_2000);   // set range
  bno.setGyrBandWidth(BNO::eGyrBandWidth_32);   // set band width
  bno.setGyrPowerMode(BNO::eGyrPowerModeNormal);    // set power mode

  BNO::sAxisAnalog_t    sOffsetAcc;   // unit mg, members can't out of acc range
  BNO::sAxisAnalog_t    sOffsetMag;   // unit ut, members can't out of mag range
  BNO::sAxisAnalog_t    sOffsetGyr;   // unit dps, members can't out of gyr range
  sOffsetAcc.x = 1;
  sOffsetAcc.y = 1;
  sOffsetAcc.z = 1;
  sOffsetMag.x = 1;
  sOffsetMag.y = 1;
  sOffsetMag.z = 1;
  sOffsetGyr.x = 1;
  sOffsetGyr.y = 1;
  sOffsetGyr.z = 1;
  bno.setAxisOffset(BNO::eAxisAcc, sOffsetAcc);   // set offset
  bno.setAxisOffset(BNO::eAxisMag, sOffsetMag);
  bno.setAxisOffset(BNO::eAxisGyr, sOffsetGyr);

  bno.setOprMode(BNO::eOprModeNdof);   // shift to other operate mode, reference datasheet for more detail
}

#define printAxisData(sAxis) \
  Serial.print(" x: "); \
  Serial.print(sAxis.x); \
  Serial.print(" y: "); \
  Serial.print(sAxis.y); \
  Serial.print(" z: "); \
  Serial.println(sAxis.z)

void loop()
{
  BNO::sAxisAnalog_t   sAccAnalog, sMagAnalog, sGyrAnalog, sLiaAnalog, sGrvAnalog;
  BNO::sEulAnalog_t    sEulAnalog;
  BNO::sQuaAnalog_t    sQuaAnalog;
  sAccAnalog = bno.getAxis(BNO::eAxisAcc);
  sMagAnalog = bno.getAxis(BNO::eAxisMag);
  sGyrAnalog = bno.getAxis(BNO::eAxisGyr);
  sLiaAnalog = bno.getAxis(BNO::eAxisLia);
  sGrvAnalog = bno.getAxis(BNO::eAxisGrv);
  sEulAnalog = bno.getEul();
  sQuaAnalog = bno.getQua();
  Serial.println();
  Serial.println("======== analog data print start ========");
  Serial.print("acc analog: (unit mg)       "); printAxisData(sAccAnalog);
  Serial.print("mag analog: (unit ut)       "); printAxisData(sMagAnalog);
  Serial.print("gyr analog: (unit dps)      "); printAxisData(sGyrAnalog);
  Serial.print("lia analog: (unit mg)       "); printAxisData(sLiaAnalog);
  Serial.print("grv analog: (unit mg)       "); printAxisData(sGrvAnalog);
  Serial.print("eul analog: (unit degree)   "); Serial.print(" head: "); Serial.print(sEulAnalog.head); Serial.print(" roll: "); Serial.print(sEulAnalog.roll);  Serial.print(" pitch: "); Serial.println(sEulAnalog.pitch);
  Serial.print("qua analog: (no unit)       "); Serial.print(" w: "); Serial.print(sQuaAnalog.w); printAxisData(sQuaAnalog);
  Serial.println("========  analog data print end  ========");

  delay(1000);
}

Expected Results

Result

FAQ

For any questions, advice or cool ideas to share, please visit the DFRobot Forum.

More Documents