BRIX5

/*!

 * @file Adafruit_BNO055.cpp

 *

 *  @mainpage Adafruit BNO055 Orientation Sensor

 *

 *  @section intro_sec Introduction

 *

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

 *  ------> https://www.adafruit.com/product/2472

 *

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

 *

 *  @section author Author

 *

 *  K.Townsend (Adafruit Industries)

 *

 *  @section license License

 *

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

 */

#include "Arduino.h"

#include <limits.h>

#include <math.h>

#include "Adafruit_BNO055.h"

/*!

 *  @brief  Instantiates a new Adafruit_BNO055 class

 *  @param  sensorID

 *          sensor ID

 *  @param  address

 *          i2c address

 *  @param  *theWire

 *          Wire object

 */

Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address,

                                 TwoWire *theWire) {

  _sensorID = sensorID;

  _address = address;

  _wire = theWire;

}

/*!

 *  @brief  Sets up the HW

 *  @param  mode

 *          mode values

 *           [OPERATION_MODE_CONFIG,

 *            OPERATION_MODE_ACCONLY,

 *            OPERATION_MODE_MAGONLY,

 *            OPERATION_MODE_GYRONLY,

 *            OPERATION_MODE_ACCMAG,

 *            OPERATION_MODE_ACCGYRO,

 *            OPERATION_MODE_MAGGYRO,

 *            OPERATION_MODE_AMG,

 *            OPERATION_MODE_IMUPLUS,

 *            OPERATION_MODE_COMPASS,

 *            OPERATION_MODE_M4G,

 *            OPERATION_MODE_NDOF_FMC_OFF,

 *            OPERATION_MODE_NDOF]

 *  @return true if process is successful

 */

bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) {

#if defined(ARDUINO_SAMD_ZERO) && (_address == BNO055_ADDRESS_A)

#error                                                                         \

    "On an arduino Zero, BNO055's ADR pin must be high. Fix that, then delete this line."

  _address = BNO055_ADDRESS_B;

#endif

  /* Enable I2C */

  _wire->begin();

  // BNO055 clock stretches for 500us or more!

#ifdef ESP8266

  _wire->setClockStretchLimit(1000); // Allow for 1000us of clock stretching

#endif

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

  */

  /* Configure axis mapping (see section 3.4) */

  /*

  write8(BNO055_AXIS_MAP_CONFIG_ADDR, REMAP_CONFIG_P2); // P0-P7, Default is P1

  delay(10);

  write8(BNO055_AXIS_MAP_SIGN_ADDR, REMAP_SIGN_P2); // P0-P7, Default is P1

  delay(10);

  */

  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

 *  @param  mode

 *          mode values

 *           [OPERATION_MODE_CONFIG,

 *            OPERATION_MODE_ACCONLY,

 *            OPERATION_MODE_MAGONLY,

 *            OPERATION_MODE_GYRONLY,

 *            OPERATION_MODE_ACCMAG,

 *            OPERATION_MODE_ACCGYRO,

 *            OPERATION_MODE_MAGGYRO,

 *            OPERATION_MODE_AMG,

 *            OPERATION_MODE_IMUPLUS,

 *            OPERATION_MODE_COMPASS,

 *            OPERATION_MODE_M4G,

 *            OPERATION_MODE_NDOF_FMC_OFF,

 *            OPERATION_MODE_NDOF]

 */

void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) {

  _mode = mode;

  write8(BNO055_OPR_MODE_ADDR, _mode);

  delay(30);

}

/*!

 *  @brief  Changes the chip's axis remap

 *  @param  remapcode

 *          remap code possible values

 *          [REMAP_CONFIG_P0

 *           REMAP_CONFIG_P1 (default)

 *           REMAP_CONFIG_P2

 *           REMAP_CONFIG_P3

 *           REMAP_CONFIG_P4

 *           REMAP_CONFIG_P5

 *           REMAP_CONFIG_P6

 *           REMAP_CONFIG_P7]

 */

void Adafruit_BNO055::setAxisRemap(

    adafruit_bno055_axis_remap_config_t remapcode) {

  adafruit_bno055_opmode_t modeback = _mode;

  setMode(OPERATION_MODE_CONFIG);

  delay(25);

  write8(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);

  delay(10);

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

  setMode(modeback);

  delay(20);

}

/*!

 *  @brief  Changes the chip's axis signs

 *  @param  remapsign

 *          remap sign possible values

 *          [REMAP_SIGN_P0

 *           REMAP_SIGN_P1 (default)

 *           REMAP_SIGN_P2

 *           REMAP_SIGN_P3

 *           REMAP_SIGN_P4

 *           REMAP_SIGN_P5

 *           REMAP_SIGN_P6

 *           REMAP_SIGN_P7]

 */

void Adafruit_BNO055::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {

  adafruit_bno055_opmode_t modeback = _mode;

  setMode(OPERATION_MODE_CONFIG);

  delay(25);

  write8(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);

  delay(10);

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

  setMode(modeback);

  delay(20);

}

/*!

 *  @brief  Use the external 32.768KHz crystal

 *  @param  usextal

 *          use external crystal boolean