0

BNO055 library adapted to i2c_t3 lib for Teensy
Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by amundsen on Tue Aug 16, 2016 2:09 pm

I wanted to know for sure why my sensors were not always detected.

So I have tried this sketch (from the guy selling an alternative breakout board). It's only a sketch. There's no library required outside I2C_t3. With the latter, it's easy to set the I2C bus and sensor ID and test all sensors.

I have tested the sketch with settings corresponding to each of my three boards (including one on the Wire1 bus), with the circuit switched off and on ten times in a row. Whatever the board chosen it's always found when switching on. So it's now 100% sure it's a software issue. Also, it means the I2C_t3 library is reliable. What this means in the end is that either the issue comes from my modified BNO-055 library or it comes from the original Adafruit BNO-055 library. :-(

amundsen
 
Posts: 52
Joined: Thu Dec 15, 2011 3:24 am

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by amundsen on Wed Aug 17, 2016 9:06 am

I have solved the issue. It came from the original Adafruit_BNO055.cpp file. Every time the function write8(BNO055_SYS_TRIGGER_ADDR, xxxx) is used, it is followed by a delay except after the first call, within the definition of the begin function (line 80 in the original version, line 101 in the i2C_t3 version). So I have added a delay of 30 ms. Delays of 10 or 20 ms somewhat helped but not all the time. With 30 ms, I could switch the circuit on and off several tens of times in a row without issue.

Delay times might have to be increased in other calls too but tests should be performed to confirm this is required first.

So here is the corrected code for Adafruit_BNO055_t3_V2.cpp:

Code: Select all | TOGGLE FULL SIZE
/***************************************************************************
 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.
 
 !!!!!MODIFIED VERSION TO USE I2C_T3 LIBRARY FOR TEENSY INSTEAD OF WIRE
 IN ORDER TO USE TEENSY'S BOTH I2C PORTS
 Roald Baudoux - 2016-03
 
 Updated 2016-03-15: i2c_bus type defined to set choice of Wire or Wire1
 Updated 2016-08-17: added delay of 30 ms after function call "write8(BNO055_SYS_TRIGGER_ADDR, 0x20);"
 
 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_t3.h" // MODIFIED RB

/***************************************************************************
 CONSTRUCTOR
 ***************************************************************************/

/**************************************************************************/
/*!
 @brief  Instantiates a new Adafruit_BNO055 class
 
 !!!!! MODIFIED TO BE COMPATIBLE  WITH I2C_T3 library instead of Wire
 // Params : I2C bus, sensor ID, address, I2C mode (master/slave), pins, pullups, freq, opMode (interrupt/DMA/immediate)
 */
/**************************************************************************/
// OLD : Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address)
Adafruit_BNO055::Adafruit_BNO055(i2c_bus iBus, int32_t sensorID, uint8_t aAddress, i2c_mode iMode, i2c_pins pins, i2c_pullup pullup, i2c_rate iRate, i2c_op_mode opeMode)
{
    _iBus = iBus; // ADDED RB 0 for Wire() and 1 for Wire1();
    _sensorID = sensorID;
    _aAddress = aAddress;
    _iMode = iMode;         // ADDED RB
    _pins = pins;           // ADDED RB
    _pullup = pullup;       // ADDED RB
    _iRate = iRate;           // ADDED RB
    _opeMode = opeMode;       // ADDED RB
}

/***************************************************************************
 PUBLIC FUNCTIONS
 ***************************************************************************/

/**************************************************************************/
/*!
 @brief  Sets up the HW
 */
/**************************************************************************/
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t aMode)
{
    /* Enable I2C */
   
    if (_iBus == WIRE_BUS)
    {
        Wire.begin(_iMode, _aAddress, _pins, _pullup, _iRate, _opeMode);
    }
    else
    {
        Wire1.begin(_iMode, _aAddress, _pins, _pullup, _iRate, _opeMode);
    }
    /* Make sure we have the right device */
    uint8_t id = read8(BNO055_CHIP_ID_ADDR);
    if(id != BNO055_ID)
    {
        delay(1000);
        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);
    delay(30); // ADDED RB

    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(aMode);
    delay(20);
   
    return true;
}

/**************************************************************************/
/*!
 @brief  Puts the chip in the specified operating mode
 */
/**************************************************************************/
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t aMode)
{
    _aMode = aMode;
    write8(BNO055_OPR_MODE_ADDR, _aMode);
    delay(30);
}

/**************************************************************************/
/*!
 @brief  Use the external 32.768KHz crystal
 */
/**************************************************************************/
void Adafruit_BNO055::setExtCrystalUse(boolean usextal)
{
    adafruit_bno055_opmode_t modeback = _aMode;
   
    /* 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)
{
    write8(BNO055_PAGE_ID_ADDR, 0);
   
    /* 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);
   
    delay(200);
}

/**************************************************************************/
/*!
 @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  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.
 */
/**************************************************************************/
void Adafruit_BNO055::getCalibration(uint8_t* sys, uint8_t* gyro, uint8_t* accel, uint8_t* mag) {
    uint8_t calData = read8(BNO055_CALIB_STAT_ADDR);
    if (sys != NULL) {
        *sys = (calData >> 6) & 0x03;
    }
    if (gyro != NULL) {
        *gyro = (calData >> 4) & 0x03;
    }
    if (accel != NULL) {
        *accel = (calData >> 2) & 0x03;
    }
    if (mag != NULL) {
        *mag = calData & 0x03;
    }
}

/**************************************************************************/
/*!
 @brief  Gets the 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;
}

/**************************************************************************/
/*!
 @brief  Reads the sensor's offset registers into a byte array
 */
/**************************************************************************/
bool Adafruit_BNO055::getSensorOffsets(uint8_t* calibData)
{
    if (isFullyCalibrated())
    {
        adafruit_bno055_opmode_t lastMode = _aMode;
        setMode(OPERATION_MODE_CONFIG);
       
        readLen(ACCEL_OFFSET_X_LSB_ADDR, calibData, NUM_BNO055_OFFSET_REGISTERS);
       
        setMode(lastMode);
        return true;
    }
    return false;
}

/**************************************************************************/
/*!
 @brief  Reads the sensor's offset registers into an offset struct
 */
/**************************************************************************/
bool Adafruit_BNO055::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type)
{
    if (isFullyCalibrated())
    {
        adafruit_bno055_opmode_t lastMode = _aMode;
        setMode(OPERATION_MODE_CONFIG);
        delay(25);
       
        offsets_type.accel_offset_x = (read8(ACCEL_OFFSET_X_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_X_LSB_ADDR));
        offsets_type.accel_offset_y = (read8(ACCEL_OFFSET_Y_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Y_LSB_ADDR));
        offsets_type.accel_offset_z = (read8(ACCEL_OFFSET_Z_MSB_ADDR) << 8) | (read8(ACCEL_OFFSET_Z_LSB_ADDR));
       
        offsets_type.gyro_offset_x = (read8(GYRO_OFFSET_X_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_X_LSB_ADDR));
        offsets_type.gyro_offset_y = (read8(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Y_LSB_ADDR));
        offsets_type.gyro_offset_z = (read8(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (read8(GYRO_OFFSET_Z_LSB_ADDR));
       
        offsets_type.mag_offset_x = (read8(MAG_OFFSET_X_MSB_ADDR) << 8) | (read8(MAG_OFFSET_X_LSB_ADDR));
        offsets_type.mag_offset_y = (read8(MAG_OFFSET_Y_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Y_LSB_ADDR));
        offsets_type.mag_offset_z = (read8(MAG_OFFSET_Z_MSB_ADDR) << 8) | (read8(MAG_OFFSET_Z_LSB_ADDR));
       
        offsets_type.accel_radius = (read8(ACCEL_RADIUS_MSB_ADDR) << 8) | (read8(ACCEL_RADIUS_LSB_ADDR));
        offsets_type.mag_radius = (read8(MAG_RADIUS_MSB_ADDR) << 8) | (read8(MAG_RADIUS_LSB_ADDR));
       
        setMode(lastMode);
        return true;
    }
    return false;
}


/**************************************************************************/
/*!
 @brief  Writes an array of calibration values to the sensor's offset registers
 */
/**************************************************************************/
void Adafruit_BNO055::setSensorOffsets(const uint8_t* calibData)
{
    adafruit_bno055_opmode_t lastMode = _aMode;
    setMode(OPERATION_MODE_CONFIG);
    delay(25);
   
    /* A writeLen() would make this much cleaner */
    write8(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
    write8(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
    write8(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
    write8(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
    write8(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
    write8(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
   
    write8(GYRO_OFFSET_X_LSB_ADDR, calibData[6]);
    write8(GYRO_OFFSET_X_MSB_ADDR, calibData[7]);
    write8(GYRO_OFFSET_Y_LSB_ADDR, calibData[8]);
    write8(GYRO_OFFSET_Y_MSB_ADDR, calibData[9]);
    write8(GYRO_OFFSET_Z_LSB_ADDR, calibData[10]);
    write8(GYRO_OFFSET_Z_MSB_ADDR, calibData[11]);
   
    write8(MAG_OFFSET_X_LSB_ADDR, calibData[12]);
    write8(MAG_OFFSET_X_MSB_ADDR, calibData[13]);
    write8(MAG_OFFSET_Y_LSB_ADDR, calibData[14]);
    write8(MAG_OFFSET_Y_MSB_ADDR, calibData[15]);
    write8(MAG_OFFSET_Z_LSB_ADDR, calibData[16]);
    write8(MAG_OFFSET_Z_MSB_ADDR, calibData[17]);
   
    write8(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
    write8(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
   
    write8(MAG_RADIUS_LSB_ADDR, calibData[20]);
    write8(MAG_RADIUS_MSB_ADDR, calibData[21]);
   
    setMode(lastMode);
}

/**************************************************************************/
/*!
 @brief  Writes to the sensor's offset registers from an offset struct
 */
/**************************************************************************/
void Adafruit_BNO055::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type)
{
    adafruit_bno055_opmode_t lastMode = _aMode;
    setMode(OPERATION_MODE_CONFIG);
    delay(25);
   
    write8(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
    write8(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
    write8(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
    write8(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
    write8(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
    write8(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
   
    write8(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
    write8(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
    write8(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
    write8(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
    write8(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
    write8(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
   
    write8(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
    write8(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
    write8(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
    write8(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
    write8(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
    write8(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
   
    write8(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
    write8(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
   
    write8(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
    write8(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
   
    setMode(lastMode);
}

bool Adafruit_BNO055::isFullyCalibrated(void)
{
    uint8_t system, gyro, accel, mag;
    getCalibration(&system, &gyro, &accel, &mag);
    if (system < 3 || gyro < 3 || accel < 3 || mag < 3)
        return false;
    return true;
}


/***************************************************************************
 PRIVATE FUNCTIONS
 ***************************************************************************/

/**************************************************************************/
/*!
 @brief  Writes an 8 bit value over I2C
 */
/**************************************************************************/
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value)
{
    if (_iBus == WIRE_BUS)
    {
        Wire.beginTransmission(_aAddress); //// MODIFIED RB
#if ARDUINO >= 100
        Wire.write((uint8_t)reg); /// TO BE MODIFIED? NO
        Wire.write((uint8_t)value); /// TO BE MODIFIED? NO
#else
        Wire.send(reg); /// TO BE MODIFIED? NO
        Wire.send(value); /// TO BE MODIFIED? NO
#endif
        Wire.endTransmission(); /// TO BE MODIFIED? NO
       
        /* ToDo: Check for error! */
        return true;
    }
    else
    {
        Wire1.beginTransmission(_aAddress); //// MODIFIED RB
#if ARDUINO >= 100
        Wire1.write((uint8_t)reg); /// TO BE MODIFIED? NO
        Wire1.write((uint8_t)value); /// TO BE MODIFIED? NO
#else
        Wire1.send(reg); /// TO BE MODIFIED? NO
        Wire1.send(value); /// TO BE MODIFIED? NO
#endif
        Wire1.endTransmission(); /// TO BE MODIFIED? NO
       
        /* 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;
   
    if (_iBus == WIRE_BUS)
    {
        Wire.beginTransmission(_aAddress); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
        Wire.write((uint8_t)reg); /// TO BE MODIFIED? NO
#else
        Wire.send(reg);
#endif
        Wire.endTransmission();
        Wire.requestFrom(_aAddress, (byte)1); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
        value = Wire.read(); /// TO BE MODIFIED? NO
#else
        value = Wire.receive(); //// MODIFIED RB receive -> readByte
        //value = Wire.readByte(); //// MODIFIED RB receive -> readByte
#endif
       
        return value;
    }
    else
    {
        Wire1.beginTransmission(_aAddress); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
        Wire1.write((uint8_t)reg); /// TO BE MODIFIED? NO
#else
        Wire1.send(reg);
#endif
        Wire1.endTransmission();
        Wire1.requestFrom(_aAddress, (byte)1); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
        value = Wire1.read(); /// TO BE MODIFIED? NO
#else
        value = Wire1.readByte(); //// MODIFIED RB receive -> readByte
#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)
{
    if (_iBus == WIRE_BUS)
    {
        Wire.beginTransmission(_aAddress); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
        Wire.write((uint8_t)reg); /// TO BE MODIFIED? NO
#else
        Wire.send(reg); /// TO BE MODIFIED? NO
#endif
        Wire.endTransmission();
        Wire.requestFrom(_aAddress, (byte)len); /// TO BE MODIFIED? NO
       
        for (uint8_t i = 0; i < len; i++)
        {
#if ARDUINO >= 100
            buffer[i] = Wire.read(); /// TO BE MODIFIED? NO
#else
            buffer[i] = Wire.readByte(); //// MODIFIE RB receive -> readByte
#endif
        }
       
        /* ToDo: Check for errors! */
        return true;
    }
    else
    {
        Wire1.beginTransmission(_aAddress); /// TO BE MODIFIED? NO
#if ARDUINO >= 100
        Wire1.write((uint8_t)reg); /// TO BE MODIFIED? NO
#else
        Wire1.send(reg); /// TO BE MODIFIED? NO
#endif
        Wire1.endTransmission();
        Wire1.requestFrom(_aAddress, (byte)len); /// TO BE MODIFIED? NO
       
        for (uint8_t i = 0; i < len; i++)
        {
#if ARDUINO >= 100
            buffer[i] = Wire1.read(); /// TO BE MODIFIED? NO
#else
            buffer[i] = Wire1.readByte(); //// MODIFIE RB receive -> readByte
#endif
        }
       
        /* ToDo: Check for errors! */
        return true;
    }
   
}

amundsen
 
Posts: 52
Joined: Thu Dec 15, 2011 3:24 am

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by henriqjt on Wed Aug 17, 2016 10:45 pm

good work amundsen!!

henriqjt
 
Posts: 5
Joined: Tue Oct 07, 2014 8:22 pm

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by hatdatsat on Mon Feb 12, 2018 12:15 pm

So I know the last post here has been a while, but i hope someone can help me.
Atm im trying to implement the code so still need to check if it will work.

But i still wanted to put my question out there, im running a teensy 3.2 and i have 2 BNO0555 boards that i want to connect to it.
So i have it pinned to 18,19 and 29,30 and i have been reading many posts about master and slave..... does this mean 1 BNO055 is the master and 1 the slave?
And if so what does that entail? does that mean you can only receive data from 1 at the time and you have to swap between them? Im looking for having them both send me data at the same time is this possible?

Sorry if this is a stupid question only started to work with teensy last week so kinda trying to get going quickly with it.

hatdatsat
 
Posts: 2
Joined: Mon Feb 12, 2018 11:12 am

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by amundsen on Mon Feb 12, 2018 1:13 pm

Hello,

The Teensy is usually the master (initiating the transmission) and both the BNOs would then be slaves.

You can receive data from both sensors simultaneously.

amundsen
 
Posts: 52
Joined: Thu Dec 15, 2011 3:24 am

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by hatdatsat on Fri Feb 16, 2018 11:37 am

So i figured out how it works.

Thanks for the awesome library!
I still had some trouble figuring it out so i made a little example for people like me (who are still new to teensy) hope it helps.
So in this example i'm using a teensy 3.2 and 2 BNO055 boards.
The first BNO has its pins on 18 and 19 and the other BNO is connected to the bottom 29 and 30.

Code: Select all | TOGGLE FULL SIZE
#include <i2c_t3.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055_t3.h>
#include <utility/imumaths.h>
#include <Encoder.h>

Adafruit_BNO055 bno  = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_ISR);
Adafruit_BNO055 bnoTwo = Adafruit_BNO055(WIRE1_BUS, 1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_29_30, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_ISR);

void setup() {
   
  Serial.begin(115200);
  if(!bno.begin()){   
    while(1);
    Serial.println("bno not starting up");
  }else{
    Serial.println("bno seems to be working!");
  }
 
  if(!bnoTwo.begin()){   
    while(1);
    Serial.println("bnoTwo not starting up");
  }else{
    Serial.println("bnoTwo seems to be working!");
  }

  delay(1000);
 
  //Use crystal for extra precision 
  bno.setExtCrystalUse(true);
  bnoTwo.setExtCrystalUse(true);

}


void loop(void) {
   
    //get dof data
    sensors_event_t event;
    bno.getEvent(&event);
    bnoTwo.getEvent(&event);
    imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
    imu::Vector<3> eulerTwo = bnoTwo.getVector(Adafruit_BNO055::VECTOR_EULER);
   
    Serial.print(kopValue); Serial.print(",");
    Serial.print(euler.x()); Serial.print(",");
    Serial.print(euler.y()); Serial.print(",");
    Serial.print(euler.z()); Serial.print(",");
    Serial.print(eulerTwo.x()); Serial.print(",");
    Serial.print(eulerTwo.y()); Serial.print(",");
    Serial.println(eulerTwo.z());
    }     
}

hatdatsat
 
Posts: 2
Joined: Mon Feb 12, 2018 11:12 am

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by amundsen on Sat Feb 17, 2018 9:08 pm

Just for information, I have never updated the lib to a more recent version of either i2c_t3 or BNO_055 library. I have stopped my project with the BNO-055.

amundsen
 
Posts: 52
Joined: Thu Dec 15, 2011 3:24 am

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by Gian_Maria on Tue May 22, 2018 2:02 pm

Did anybody tried to use the library with a teensy 3.6? I am trying to read an IMU BNO055 as reported in the previous comment without any kind of success.
Please let me know

Giamma

Gian_Maria
 
Posts: 2
Joined: Tue May 22, 2018 2:01 pm

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by amundsen on Tue May 22, 2018 3:48 pm

I have tested the library with Teensy 3.1 and 3.2 only.

amundsen
 
Posts: 52
Joined: Thu Dec 15, 2011 3:24 am

Re: BNO055 library adapted to i2c_t3 lib for Teensy

by Gian_Maria on Wed May 23, 2018 1:57 pm

Could you share some code for teensy3.2 in case of two/three BNO055 which actually run? Should I use pullup resistors around 1.5Kohm for sda1 scl1 (on teensy 3.2)?

Currently I am working with a teensy 3.6 and I would like to understand how to "update" the library.

Any help would be appreciated

Thank you very much

Giamma

Gian_Maria
 
Posts: 2
Joined: Tue May 22, 2018 2:01 pm

Please be positive and constructive with your questions and comments.