#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.3-5V DC
   Connect GROUND to common ground

   History
   =======
   2015/MAR/03  - First release (KTOWN)
*/

/* Set the delay between fresh samples */
uint16_t BNO055_SAMPLERATE_DELAY_MS = 1000;

// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
//                                   id, address
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);

void setup(void)
{
  Serial.begin(115200);

  while (!Serial) delay(10);  // wait for serial port to open!

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

  bno.setExtCrystalUse(true); // this line is not in the original example sketch

  delay(1000);
}

void loop(void)
{
  //could add VECTOR_ACCELEROMETER, VECTOR_MAGNETOMETER,VECTOR_GRAVITY...
  sensors_event_t orientationData , angVelocityData , linearAccelData, magnetometerData, accelerometerData, gravityData;
  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
  bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
  bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
  bno.getEvent(&magnetometerData, Adafruit_BNO055::VECTOR_MAGNETOMETER);
  bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);
  bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);

  printEvent(&orientationData);
  printEvent(&angVelocityData);
  printEvent(&linearAccelData);
  printEvent(&magnetometerData);
  printEvent(&accelerometerData);
  printEvent(&gravityData);

  int8_t boardTemp = bno.getTemp();
  Serial.println();
  Serial.print(F("temperature: "));
  Serial.println(boardTemp);

  uint8_t system, gyro, accel, mag = 0;
  bno.getCalibration(&system, &gyro, &accel, &mag);
  Serial.println();
  Serial.print("Calibration: Sys=");
  Serial.print(system);
  Serial.print(" Gyro=");
  Serial.print(gyro);
  Serial.print(" Accel=");
  Serial.print(accel);
  Serial.print(" Mag=");
  Serial.println(mag);

  Serial.println("--");
  delay(BNO055_SAMPLERATE_DELAY_MS);
}

void printEvent(sensors_event_t* event) {
  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
  if (event->type == SENSOR_TYPE_ACCELEROMETER) {
    Serial.print("Accl:");
    x = event->acceleration.x;
    y = event->acceleration.y;
    z = event->acceleration.z;
  }
  else if (event->type == SENSOR_TYPE_ORIENTATION) {
    Serial.print("Orient:");
    x = event->orientation.x;
    y = event->orientation.y;
    z = event->orientation.z;
  }
  else if (event->type == SENSOR_TYPE_MAGNETIC_FIELD) {
    Serial.print("Mag:");
    x = event->magnetic.x;
    y = event->magnetic.y;
    z = event->magnetic.z;
  }
  else if (event->type == SENSOR_TYPE_GYROSCOPE) {
    Serial.print("Gyro:");
    x = event->gyro.x;
    y = event->gyro.y;
    z = event->gyro.z;
  }
  else if (event->type == SENSOR_TYPE_ROTATION_VECTOR) {
    Serial.print("Rot:");
    x = event->gyro.x;
    y = event->gyro.y;
    z = event->gyro.z;
  }
  else if (event->type == SENSOR_TYPE_LINEAR_ACCELERATION) {
    Serial.print("Linear:");
    x = event->acceleration.x;
    y = event->acceleration.y;
    z = event->acceleration.z;
  }
  else if (event->type == SENSOR_TYPE_GRAVITY) {
    Serial.print("Gravity:");
    x = event->acceleration.x;
    y = event->acceleration.y;
    z = event->acceleration.z;
  }
  else {
    Serial.print("Unk:");
  }

  Serial.print("\tx= ");
  Serial.print(x);
  Serial.print(" |\ty= ");
  Serial.print(y);
  Serial.print(" |\tz= ");
  Serial.println(z);
}

/*!
  * 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();  // resets the BNO055
  while(bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin failed");
    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);
}

/*!
  * 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
  * modified by Wolfgang Ewald 07/02/2026
  */

#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();
  delay(1000); // give time to reset and avoid "device not detected"
  while(bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin failed");
    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;
  BNO::sRegCalibState_t sCalibState;
  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
  sCalibState = bno.getCalStatus();           // read calibration status
  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.print("Calibration Status:         "); 
  Serial.print(" SYS: "); Serial.print(sCalibState.SYS);
  Serial.print("  ACC: "); Serial.print(sCalibState.ACC);
  Serial.print("  GYR: "); Serial.print(sCalibState.GYR);
  Serial.print("  MAG: "); Serial.println(sCalibState.MAG);
  Serial.println("======== analog data printend  ========");

  delay(1000);
}

/*!
  * 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
  * modified by Wolfgang Ewald 12/02/2026
  */

#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_2G);   // set range to 2g
  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_125);   // 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::eOprModeAMG);   // shift a non-fusion mode
}

#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;
  sAccAnalog = bno.getAxis(BNO::eAxisAcc);
  sMagAnalog = bno.getAxis(BNO::eAxisMag);
  sGyrAnalog = bno.getAxis(BNO::eAxisGyr);
  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.println("========  analog data print end  ========");

  delay(1000);
}

/*!
  * 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 reference comment
  *
  * 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(100);
  // 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(650); // comment Wolfgang: due to a bug the effective value is double the parameter. 
  // accelerometer (no motion) / (slow motion) settings, 2nd parameter unit seconds, no more than 336
  bno.setAccNmSet(BNO::eAccNmSmnmNm, 4); // better name would be setAccNmSmSet() for no and slow motion
  /*
   * 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(digitalPinToInterrupt(2), 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");
  }
}

#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();
  delay(1000);
  while(bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin faild");
    printLastOperateStatus(bno.lastOperateStatus);
    delay(2000);
  }
  Serial.println("bno begin success");
  bno.setOprMode(BNO::eOprModeConfig);
  // bno.setAccAmThres(500); // relvant for wakeup
  // bno.setAccIntAmDur(1);  // relevant for wakeup
  // bno.setAccNmThres(100);  // relevant for entering low power mode
  // bno.setAccNmSet(BNO::eAccNmSmnmNm, 4); // relevant for entering low power mode
  bno.setPowerMode(BNO::ePowerModeLowPower);
  bno.setOprMode(BNO::eOprModeNdof);
}

#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;
  BNO::sRegCalibState_t sCalibState;
  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
  sCalibState = bno.getCalStatus();           // read calibration status
  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.print("Calibration Status:         "); 
  Serial.print(" SYS: "); Serial.print(sCalibState.SYS);
  Serial.print("  ACC: "); Serial.print(sCalibState.ACC);
  Serial.print("  GYR: "); Serial.print(sCalibState.GYR);
  Serial.print("  MAG: "); Serial.println(sCalibState.MAG);
  Serial.println("======== analog data printend  ========");

  delay(1000);
}

#include "BNO055_support.h"		//Contains the bridge code between the API and Arduino
#include <Wire.h>

//The device address is set to BNO055_I2C_ADDR2 in this example. You can change this in the BNO055.h file in the code segment shown below.
// /* bno055 I2C Address */
// #define BNO055_I2C_ADDR1                0x28
// #define BNO055_I2C_ADDR2                0x29
// #define BNO055_I2C_ADDR                 BNO055_I2C_ADDR2

//This structure contains the details of the BNO055 device that is connected. (Updated after initialization)
struct bno055_t myBNO;

// Structures to hold the measured/calculated data
struct bno055_accel myAccelData;
struct bno055_gyro myGyroData;
struct bno055_mag myMagData;
struct bno055_euler myEulerData; 
struct bno055_quaternion myQuaternionData;
struct bno055_linear_accel myLinearAccelData;
struct bno055_gravity myGravityData;

BNO055_S16 myTemperatureData; // BNO055_S16 = short int

int factor = 1.0;  // for calculation measured values from raw data

void setup() {
  //Initialize I2C communication
  Wire.begin();

  //Initialization of the BNO055
  BNO_Init(&myBNO); // Assigning the structure to hold information about the device

  /* Some config examples. After initiation you are in config mode, for later changes go 
     into config mode before. If you apply changes, you need to repower the BNO055 or do
     a reset. */
  bno055_set_reset_sys(1); delay(800); // force reset a give rest time
  /* Some examples of config options */
  // bno055_set_operation_mode(OPERATION_MODE_CONFIG);  // change to config mode
  // bno055_set_powermode(POWER_MODE_LOW_POWER); // Altern.:  POWER_MODE_SUSPEND / POWER_MODE_LOW_POWER
  // bno055_set_axis_remap_value(REMAP_Y_Z); // swap y and z axis
  // bno055_set_x_remap_sign(1);  // set negative sign for x-axis
  // bno055_set_accel_range(ACCEL_RANGE_2G); // not available in fusion modes
  // bno055_set_gyro_range(GYRO_RANGE_125rps);  // you also need to change the factor below; not available in fusion modes
  bno055_set_operation_mode(OPERATION_MODE_NDOF); //  set NDoF mode 

  delay(1);

  //Initialize the Serial Port to view information on the Serial Monitor
  Serial.begin(115200);
}

template <typename T>
void printXYZ(const T *xyzData, float factor) {
  Serial.print(" x: "); Serial.print((float)xyzData->x / factor);
  Serial.print(" y: "); Serial.print((float)xyzData->y / factor);
  Serial.print(" z: "); Serial.println((float)xyzData->z / factor);
}

void printCalib() {
  unsigned char sysCalib=99, accelCalib=99, gyroCalib=99, magCalib=99;
  bno055_get_syscalib_status(&sysCalib);
  bno055_get_accelcalib_status(&accelCalib);
  bno055_get_gyrocalib_status(&gyroCalib);
  bno055_get_magcalib_status(&magCalib);
  Serial.print("Calibration:        "); 
  Serial.print("SYS: "); Serial.print(sysCalib);
  Serial.print(" ACC: "); Serial.print(accelCalib);
  Serial.print(" GYR: "); Serial.print(gyroCalib);
  Serial.print(" MAG: "); Serial.println(magCalib);
}

void loop() {

  bno055_read_accel_xyz(&myAccelData);
  bno055_read_gyro_xyz(&myGyroData);
  bno055_read_mag_xyz(&myMagData);
  bno055_read_gravity_xyz(&myGravityData);
  bno055_read_linear_accel_xyz(&myLinearAccelData);
  bno055_read_euler_hrp(&myEulerData); 
  bno055_read_quaternion_wxyz(&myQuaternionData);
  bno055_read_temperature_data(&myTemperatureData);
  
  Serial.print("Acceleration:      "); printXYZ(&myAccelData, 1.0);
  Serial.print("Gyroscope:         "); printXYZ(&myGyroData, 16.0);
  Serial.print("Magnetometer:      "); printXYZ(&myMagData, 16.0);
  Serial.print("Gravity:           "); printXYZ(&myGravityData, 1.0);
  Serial.print("Lin. Acceleration: "); printXYZ(&myLinearAccelData, 1.0);
  Serial.print("Euler Angles:      ");  
  Serial.print(" head: "); Serial.print((float)myEulerData.h / 16.0); 
  Serial.print(" roll: "); Serial.print((float)myEulerData.r / 16.0);
  Serial.print(" pitch: "); Serial.println((float)myEulerData.p / 16.0);
  Serial.print("Quaternion:        "); Serial.print(" w: "); Serial.print((float)myQuaternionData.w / 16384.0); 
  printXYZ(&myQuaternionData, 16384.0);
  Serial.print("Temperature:        "); Serial.println(myTemperatureData);
  printCalib();
  
  Serial.println();					//Extra line to differentiate between packets

  delay(1000);
}

#include "BNO055_support.h"    //Contains the bridge code between the API and Arduino
#include <Wire.h>
#define FUNC_ENABLE 1  // enable functions
#define FUNC_DISABLE 0 // disable functions
#define NO_MOTION_ENABLE 1
#define SLOW_MOTION_ENABLE 0

//This structure contains the details of the BNO055 device that is connected. (Updated after initialization)
struct bno055_t myBNO;

int intPin = 2;  // interrupt pin
unsigned char intSta = 0; // interupt status register
volatile bool intFlag = false;  // interrupt flag

void intHandle() {
  intFlag = true;
}

void setup() {
  //Initialize I2C communication
  Wire.begin();
  pinMode(intPin, INPUT);

  //Initialization of the BNO055
  BNO_Init(&myBNO); //Assigning the structure to hold information about the device

  /* Some config examples. After iniitiation you are in config mode, for later changes go 
     into config mode before. If you apply changes, you need to repower the BNO055 or do
     a reset. */
  bno055_set_reset_sys(true); delay(800); // force reset
  bno055_set_operation_mode(OPERATION_MODE_CONFIG);  // change to config mode
  
  bno055_set_intmsk_accel_anymotion(FUNC_ENABLE); // any motion interrupt propagated to interrupt pin
  bno055_set_intmsk_accel_nomotion(FUNC_ENABLE); // no motion interrupt propagated to interrupt pin
  bno055_set_accel_slow_no_motion_enable(NO_MOTION_ENABLE); // Alternative: SLOW_MOTION_ENABLE
  bno055_set_int_accel_anymotion(FUNC_ENABLE); // enable any motion interrupt
  bno055_set_int_accel_nomotion(FUNC_ENABLE);  // enable no/slow motion interrupt 
  
  /* activate axes for any motion / no (or slow) motion interrupt*/
  bno055_set_accel_an_nm_axis_enable(BNO055_ACCEL_AM_NM_X_AXIS, FUNC_ENABLE);
  bno055_set_accel_an_nm_axis_enable(BNO055_ACCEL_AM_NM_Y_AXIS, FUNC_ENABLE);
  bno055_set_accel_an_nm_axis_enable(BNO055_ACCEL_AM_NM_Z_AXIS, FUNC_ENABLE);

  bno055_set_accel_anymotion_threshold(40); // in 4g range 1 LSB = 7.81 mg
  bno055_set_accel_anymotion_duration(1); // parameter (max 3) = no samples above limit + 1 
  bno055_set_accel_slow_no_threshold(10); // in 4g range 1 LSB = 7.81 mg
  
  /* Parameter x (max. 63) for bno055_set_accel_slow_no_duration(x)
     In no motion mode: 
        x < 16 => duration[s] = x + 1 
        15 < x < 22 => duration[s] = 40 + (x - 16) * 8
        31 < x < 64 => duration[s] = 88 + (x - 32) * 8
     In slow motion mode: 
        number of samples above limit = x + 1 
  */
  bno055_set_accel_slow_no_duration(5); // interrupt after 6s of no motion

  attachInterrupt(digitalPinToInterrupt(intPin), intHandle, RISING);   // attach interrupt
  bno055_read_register(BNO055_INT_STA_ADDR, &intSta, sizeof(intSta));    // clear unexpected interrupt
  intFlag = false;
  bno055_set_reset_int(true);
  
  bno055_set_operation_mode(OPERATION_MODE_NDOF); //  set NDoF mode, 

  delay(1);

  //Initialize the Serial Port to view information on the Serial Monitor
  Serial.begin(115200); 
}


void loop() {
  if(intFlag) {
    intFlag = false;
    /* read interrupt state register and clear it */
    bno055_read_register(BNO055_INT_STA_ADDR, &intSta, sizeof(intSta)); 
    bno055_set_reset_int(true); // set interrupt pin low again
    Serial.println("interrupt detected");
    if(intSta & BNO055_INT_STAT_ACC_AM__MSK) // is any motion triggered?
      Serial.println("accelerometer any motion detected");
    if(intSta & BNO055_INT_STAT_ACC_NM__MSK) // is no motion triggered?
      Serial.println("accelerometer no motion detected");
  }
}