/Adafruit_BNO055.cpp - Diff - BRIX5 - Research for Cognitive Interaction

Revision 5d8d461e Adafruit_BNO055.cpp

     /***************************************************************************
       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!
       Written by KTOWN for Adafruit Industries.
       MIT license, all text above must be included in any redistribution
      ***************************************************************************/
     /*!
      * @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
      */
     #if ARDUINO >= 100
     #include "Arduino.h"
-...
     #include "Adafruit_BNO055.h"
     /***************************************************************************
      CONSTRUCTOR
      ***************************************************************************/
     /**************************************************************************/
     /*!
         @brief  Instantiates a new Adafruit_BNO055 class
         @param  sensorID
         @param  address
     */
     /**************************************************************************/
      *  @brief  Instantiates a new Adafruit_BNO055 class
      *  @param  sensorID
      *  @param  address
      */
     Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) {
       _sensorID = sensorID;
       _address = address;
+    }
     /***************************************************************************
      PUBLIC FUNCTIONS
      ***************************************************************************/
     /**************************************************************************/
     /*!
         @brief  Sets up the HW
         @param  mode
         @return true if process is sucessfull
     */
     /**************************************************************************/
      *  @brief  Sets up the HW
      *  @param  mode
      *  @return true if process is sucessfull
      */
     bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) {
       /* Enable I2C */
       Wire.begin();
       // BNO055 clock stretches for 500us or more!
       /* BNO055 clock stretches for 500us or more! */
     #ifdef ESP8266
       Wire.setClockStretchLimit(1000); // Allow for 1000us of clock stretching
       /* Allow for 1000us of clock stretching */
       Wire.setClockStretchLimit(1000);
     #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
         /* hold on for boot */
         delay(1000);
         id = read8(BNO055_CHIP_ID_ADDR);
         if (id != BNO055_ID) {
           return false; // still not? ok bail
           /* still not? ok bail */
           return false;
+        }
+      }
-...
       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);
       */
       /* 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) */
-...
       return true;
+    }
     /**************************************************************************/
     /*!
         @brief  Puts the chip in the specified operating mode
         @param  mode
     */
     /**************************************************************************/
      *  @brief  Puts the chip in the specified operating mode
      *  @param  mode
      */
     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
     */
     /**************************************************************************/
      *  @brief  Changes the chip's axis remap
      *  @param  remapcode
      */
     void Adafruit_BNO055::setAxisRemap(
         adafruit_bno055_axis_remap_config_t remapcode) {
       adafruit_bno055_opmode_t modeback = _mode;
-...
       delay(20);
+    }
     /**************************************************************************/
     /*!
         @brief  Changes the chip's axis signs
         @param  remapsign
     */
     /**************************************************************************/
      *  @brief  Changes the chip's axis signs
      *  @param  remapsign
      */
     void Adafruit_BNO055::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
       adafruit_bno055_opmode_t modeback = _mode;
-...
       delay(20);
+    }
     /**************************************************************************/
     /*!
         @brief  Use the external 32.768KHz crystal
         @param  usextal boolean
     */
     /**************************************************************************/
      *  @brief  Use the external 32.768KHz crystal
      *  @param  usextal boolean
      */
     void Adafruit_BNO055::setExtCrystalUse(boolean usextal) {
       adafruit_bno055_opmode_t modeback = _mode;
-...
       delay(20);
+    }
     /**************************************************************************/
     /*!
         @brief  Gets the latest system status info
         @param  system_status
         @param  self_test_result
         @param  system_error
     */
     /**************************************************************************/
      *   @brief  Gets the latest system status info
      *   @param  system_status
      *   @param  self_test_result
      *   @param  system_error
      */
     void Adafruit_BNO055::getSystemStatus(uint8_t *system_status,
                                           uint8_t *self_test_result,
                                           uint8_t *system_error) {
       write8(BNO055_PAGE_ID_ADDR, 0);
       /* System Status (see section 4.3.58)
          ---------------------------------
 = Idle
 = System Error
 = Initializing Peripherals
 = System Iniitalization
 = Executing Self-Test
 = Sensor fusio algorithm running
 = System running without fusion algorithms */
 = System running without fusion algorithms
        */
       if (system_status != 0)
         *system_status = read8(BNO055_SYS_STAT_ADDR);
       /* Self Test Results (see section )
          --------------------------------
       /* Self Test Results
 = 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
 x0F = all good! */
 x0F = all good!
        */
       if (self_test_result != 0)
         *self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR);
       /* System Error (see section 4.3.59)
          ---------------------------------
 = No error
 = Peripheral initialization error
 = System initialization error
-...
 = BNO low power mode not available for selected operat ion mode
 = Accelerometer power mode not available
 = Fusion algorithm configuration error
          A = Sensor configuration error */
          A = Sensor configuration error
        */
       if (system_error != 0)
         *system_error = read8(BNO055_SYS_ERR_ADDR);
-...
       delay(200);
+    }
     /**************************************************************************/
     /*!
         @brief  Gets the chip revision numbers
     */
     /**************************************************************************/
      *  @brief  Gets the chip revision numbers
      */
     void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t *info) {
       uint8_t a, b;
-...
       info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a);
+    }
     /**************************************************************************/
     /*!
         @brief  Gets current calibration state.  Each value should be a uint8_t
                 pointer and it will be set to 0 if not calibrated and 3 if
                 fully calibrated.
         @param  sys
         @param  gyro
         @param  accel
         @param  mag
     */
     /**************************************************************************/
      *  @brief  Gets current calibration state.  Each value should be a uint8_t
      *          pointer and it will be set to 0 if not calibrated and 3 if
      *          fully calibrated.
      *  @param  sys
      *  @param  gyro
      *  @param  accel
      *  @param  mag
      */
     void Adafruit_BNO055::getCalibration(uint8_t *sys, uint8_t *gyro,
                                          uint8_t *accel, uint8_t *mag) {
       uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
-...
+      }
+    }
     /**************************************************************************/
     /*!
         @brief  Gets the temperature in degrees celsius
         @return temperature in degrees celsius
     */
     /**************************************************************************/
      *  @brief  Gets the temperature in degrees celsius
      *  @return temperature in degrees celsius
      */
     int8_t Adafruit_BNO055::getTemp() {
       int8_t temp = (int8_t)(read8(BNO055_TEMP_ADDR));
       return temp;
+    }
     /**************************************************************************/
     /*!
         @brief   Gets a vector reading from the specified source
         @param   vector_type
         @return  vector from specified source
     */
     /**************************************************************************/
      *  @brief   Gets a vector reading from the specified source
      *  @param   vector_type
      *  @return  vector from specified source
      */
     imu::Vector<3> Adafruit_BNO055::getVector(adafruit_vector_type_t vector_type) {
       imu::Vector<3> xyz;
       uint8_t buffer[6];
-...
       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 */
       /*!
        * 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 */
-...
       return xyz;
+    }
     /**************************************************************************/
     /*!
         @brief  Gets a quaternion reading from the specified source
         @return quaternion reading
     */
     /**************************************************************************/
      *  @brief  Gets a quaternion reading from the specified source
      *  @return quaternion reading
      */
     imu::Quaternion Adafruit_BNO055::getQuat() {
       uint8_t buffer[8];
       memset(buffer, 0, 8);
-...
       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
 .6.5.5 Orientation (Quaternion)  */
       /*!
        * 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
         @param  sensor
      *  @brief  Provides the sensor_t data for this sensor
      *  @param  sensor
     */
     /**************************************************************************/
     void Adafruit_BNO055::getSensor(sensor_t *sensor) {
       /* Clear the sensor_t object */
       memset(sensor, 0, sizeof(sensor_t));
-...
       sensor->resolution = 0.01F;
+    }
     /**************************************************************************/
     /*!
         @brief  Reads the sensor and returns the data as a sensors_event_t
         @param  event
         @return always returns true
     */
     /**************************************************************************/
      *  @brief  Reads the sensor and returns the data as a sensors_event_t
      *  @param  event
      *  @return always returns true
      */
     bool Adafruit_BNO055::getEvent(sensors_event_t *event) {
       /* Clear the event */
       memset(event, 0, sizeof(sensors_event_t));
-...
       return true;
+    }
     /**************************************************************************/
     /*!
         @brief  Reads the sensor's offset registers into a byte array
         @param  calibData
         @return true if read is successful
     */
     /**************************************************************************/
      *  @brief  Reads the sensor's offset registers into a byte array
      *  @param  calibData
      *  @return true if read is successful
      */
     bool Adafruit_BNO055::getSensorOffsets(uint8_t *calibData) {
       if (isFullyCalibrated()) {
         adafruit_bno055_opmode_t lastMode = _mode;
-...
       return false;
+    }
     /**************************************************************************/
     /*!
         @brief  Reads the sensor's offset registers into an offset struct
         @param  offsets_type
         @return true if read is successful
     */
     /**************************************************************************/
      *  @brief  Reads the sensor's offset registers into an offset struct
      *  @param  offsets_type
      *  @return true if read is successful
      */
     bool Adafruit_BNO055::getSensorOffsets(
         adafruit_bno055_offsets_t &offsets_type) {
       if (isFullyCalibrated()) {
-...
       return false;
+    }
     /**************************************************************************/
     /*!
       @brief  Writes an array of calibration values to the sensor's offset registers
       @param  calibData
     */
     /**************************************************************************/
      *  @brief  Writes an array of calibration values to the sensor's offset registers
      *  @param  calibData
      */
     void Adafruit_BNO055::setSensorOffsets(const uint8_t *calibData) {
       adafruit_bno055_opmode_t lastMode = _mode;
       setMode(OPERATION_MODE_CONFIG);
-...
       setMode(lastMode);
+    }
     /**************************************************************************/
     /*!
     @brief  Writes to the sensor's offset registers from an offset struct
     @param  offsets_type
      *  @brief  Writes to the sensor's offset registers from an offset struct
      *  @param  offsets_type
     */
     /**************************************************************************/
     void Adafruit_BNO055::setSensorOffsets(
         const adafruit_bno055_offsets_t &offsets_type) {
       adafruit_bno055_opmode_t lastMode = _mode;
-...
       setMode(lastMode);
+    }
     /**************************************************************************/
     /*!
         @brief  Checks of all cal status values are set to 3 (fully calibrated)
         @return status of calibration
     */
     /**************************************************************************/
      *  @brief  Checks of all cal status values are set to 3 (fully calibrated)
      *  @return status of calibration
      */
     bool Adafruit_BNO055::isFullyCalibrated() {
       uint8_t system, gyro, accel, mag;
       getCalibration(&system, &gyro, &accel, &mag);
-...
       return true;
+    }
     /***************************************************************************
      PRIVATE FUNCTIONS
      ***************************************************************************/
     /**************************************************************************/
     /*!
         @brief  Writes an 8 bit value over I2C
     */
     /**************************************************************************/
      *  @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
-...
       return true;
+    }
     /**************************************************************************/
     /*!
         @brief  Reads an 8 bit value over I2C
     */
     /**************************************************************************/
      *  @brief  Reads an 8 bit value over I2C
      */
     byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg) {
       byte value = 0;
-...
       return value;
+    }
     /**************************************************************************/
     /*!
         @brief  Reads the specified number of bytes over I2C
     */
     /**************************************************************************/
      *  @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);

Also available in: Unified diff