BNO086 I2C Communication problem with Teensy 4.1

EcodroneSRL

Member
Hello there!

We are trying to develop our own code to read data from a BNO086 IMU by CEVA, using I2C and Teensy 4.1. We read the datasheet and understood all the main points of the CEVA's SHTP and SH2 protocols. We are able to open the communication and to read the first advertising messages that BNO086 send to the host at startup. After this, we are going to write the command on CHANNEL 2 to enable the output of Rotation Vector data with a 10ms update time, and here starts the pain. After the write, we perform the reads when interrupt form BNO086 goes down. The data are delivered correctly, but between the Rotation Vector buffers, we receive a lot of messages from CHANNEL 0. We figured out that those messages are about the list of errors that BNO086 has experienced since the startup. What we discovered is that:

1) Running the SAME code on an ESP32 DEV-MODULE, no error packets are sent from BNO086
2) Running an analysis of the packets with Saleae Logic, we can see that the problem is that after the first write to enable the output, while we are reading the Rotation Vector packets, in the middle of the reading another unexpected write request happens, but we are pretty sure that this write request IS NOT inside our main code. So we think that maybe the library could have a sort of bug, because we can't figure out who is to make this request.
3) If we set the the output, get the errors and then we reprogram the teensy with the reset button without powering off BNO086, magically the errors vanish and all is fixed.

I'm going to attach both the codes for Teensy 4.1 and ESP32 and some screenshots and files from Logic analysis.

Logic Analyzer log generated with Logic 1.1.15
View attachment BNO086 Logic Analysis and Arduino Code.zip

Teensy 4.1 Code:
Code:
#include <Wire.h>
#include <math.h>

#define SERIAL_CMD false
uint8_t MAX_BUFF_LENGTH = 32;
uint8_t requestBytes = 4;

#define HEADER_LENGTH 4

#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD

//All the different sensors and features we can get reports from
//These are used when enabling a given sensor
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GRAVITY 0x06
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A
#define SENSOR_REPORTID_TAP_DETECTOR 0x10
#define SENSOR_REPORTID_STEP_COUNTER 0x11
#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
#define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14
#define SENSOR_REPORTID_RAW_GYROSCOPE 0x15
#define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16
#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
#define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28
//#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR*0x29

#define DEBUG_I2C true

const byte CHANNEL_COMMAND = 0;
const byte CHANNEL_EXECUTABLE = 1;
const byte CHANNEL_CONTROL = 2;
const byte CHANNEL_REPORTS = 3;
const byte CHANNEL_WAKE_REPORTS = 4;
const byte CHANNEL_GYRO = 5;

uint8_t deviceAddress = 0x4A;
uint8_t shtpDataRead[128];
uint8_t shtpDataWrite[128];
uint8_t shtpDataSend[128];
uint32_t numInc = 0;
uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0};

uint8_t syncCom = 0;
uint16_t remainingBytes = 0;
uint8_t byteToread  = 0;
uint8_t flagStartup = 0;

uint32_t timerToSend = 100; 
uint32_t timeToSend = 0; 

uint32_t timerToSendConf = 2000; 
uint32_t timeToSendConf = 0;

uint32_t timer_head_i2c = 70; 
uint32_t time_head_i2c = 0; 

int pinInterrupt = 2;

uint8_t packetContinue(uint8_t MSB)
{
  uint8_t temp_bit = (MSB >> 7);
  return temp_bit;
}

void setup() {
  // put your setup code here, to run once:
  pinMode(pinInterrupt, INPUT_PULLUP);
  Serial.begin(115200);
  //softReset();
  Wire.begin();
  Wire.setClock(100000);
  delay(250);
  Serial.println("END_SETUP");
}

void loop() 
{
if(SERIAL_CMD)
{
	if(Serial.available()>0)
	{
	  char cmd = Serial.read();
	  if(cmd == '1')
	  {
  		//softReset();
  		timeToSend = millis();
  		productIdReq();
  		flagStartup = 2;
  		Serial.println("productIdReq");
	  }
	  if(cmd == '2')
	  {
  		enableRotationVectorI2C(10);
  		flagStartup = 3;
  		Serial.println("enableRotationVector");
	  }
		if(cmd == '3')
	  {
  		softResetI2C();
  		Serial.println("softReset");
	  }
	}
}
else
{
  if (((millis() - timeToSend) > timerToSend) && (flagStartup == 1))
  {
    timeToSend = millis();
    productIdReq();
    flagStartup = 2;
    //Serial.println("ENABLE_ID");
  }
  if (((millis() - timeToSendConf) > timerToSendConf) && (flagStartup == 2))
  {
    timeToSendConf = millis();
    //softReset();
    enableRotationVectorI2C(10);
    flagStartup = 3;
    Serial.println("ENABLEEEEEEEEEEEEEEEEEEEEEEEEE");
  }
}

  if (interruptOn())
  {
    timeToSend = millis();
    if((micros() - time_head_i2c) > timer_head_i2c)
    {
      receivePackets();
      time_head_i2c = micros();
    }
  }
}

void printSyncCom()
{
  Serial.print("syncCom:");
  Serial.println(syncCom);
}

void receivePackets()
{
  Wire.requestFrom((uint8_t)deviceAddress, requestBytes);
  readI2C();
}

uint8_t readI2C()
{
  switch(syncCom)
  {
    case 0:
        byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        Serial.println(requestBytes);
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes = getRemainingBytes(shtpDataRead[1],shtpDataRead[0]);
        Serial.println(remainingBytes);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
        else
        {
          requestBytes = MAX_BUFF_LENGTH;
          syncCom = 1;
        }
      }

    break;
  
    case 1:
      byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes -= (MAX_BUFF_LENGTH - HEADER_LENGTH);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
      }

    break;

    case 2:
      byteToread = Wire.available();
      if (Wire.available() >= byteToread)
      {
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        syncCom = 0;
        printPacketI2C(requestBytes);
        requestBytes = HEADER_LENGTH;
        if(flagStartup == 0)
        {
          flagStartup = 1;
          if(SERIAL_CMD)
          {
            Serial.println("Options: send '1' to requestID msg; send '2' to enable Rotation Vector Output");
          }
        }
      }
      
    break;
  }
  return true;
}

uint16_t getRemainingBytes(uint8_t MSB, uint8_t LSB)
{
  uint16_t dataLengtha = (((uint16_t)MSB) << 8) | ((uint16_t)LSB);
  dataLengtha &= ~((uint16_t)1 << 15); //Clear the MSbit.
  return dataLengtha;
}

uint8_t interruptOn()
{
  if (digitalRead(pinInterrupt) == LOW)
  {
    return true;
  }
  else
  {
    return false;
  }
}

void printPacketI2C(uint8_t printBytes)
{
  if(DEBUG_I2C)
  {
    for (uint8_t j = 0; j < printBytes; j++)
    {
      Serial.print(shtpDataRead[j], HEX);
      Serial.print(",");
    }
    Serial.println();
  }
}

ESP32 Code:
Code:
#include <Wire.h>
#include <math.h>
#include "driver/gpio.h"

#define SERIAL_CMD true
uint8_t MAX_BUFF_LENGTH = 32;
uint8_t requestBytes = 4;

#define HEADER_LENGTH 4

#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD

//All the different sensors and features we can get reports from
//These are used when enabling a given sensor
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GRAVITY 0x06
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A
#define SENSOR_REPORTID_TAP_DETECTOR 0x10
#define SENSOR_REPORTID_STEP_COUNTER 0x11
#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
#define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14
#define SENSOR_REPORTID_RAW_GYROSCOPE 0x15
#define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16
#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
#define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28
//#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR*0x29

#define DEBUG_I2C true

const byte CHANNEL_COMMAND = 0;
const byte CHANNEL_EXECUTABLE = 1;
const byte CHANNEL_CONTROL = 2;
const byte CHANNEL_REPORTS = 3;
const byte CHANNEL_WAKE_REPORTS = 4;
const byte CHANNEL_GYRO = 5;

uint8_t deviceAddress = 0x4A;
uint8_t shtpDataRead[128];
uint8_t shtpDataWrite[128];
uint8_t shtpDataSend[128];
uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0};

int pin_SDA = 18;
int pin_SCL = 19;

uint8_t syncCom = 0;
uint16_t remainingBytes = 0;
uint8_t flagStartup = 0;

uint32_t timerToSend = 1000; 
uint32_t timeToSend = 0; 

uint32_t timerToSendConf = 2000; 
uint32_t timeToSendConf = 0;

uint32_t timer_head_i2c = 70; 
uint32_t time_head_i2c = 0;
 
int pinInterrupt = 16;

void setup() 
{
  // put your setup code here, to run once:
  pinMode(pinInterrupt, INPUT_PULLUP);
  Serial.begin(115200);
  Wire.begin(pin_SDA, pin_SCL);
  Wire.setClock(400000);
  delay(250);
  Serial.println("END SETUP");
  Serial.println("AFTER STARTUP ADVERTISE MSG READ:");
  Serial.println("- Send '1' to request ProductID msg");
  Serial.println("- Send '2' to enable Rotation Vector Output");
}

void loop() 
{
  if(SERIAL_CMD)
  {
    if(Serial.available()>0)
    {
      char cmd = Serial.read();
      if(cmd == '1')
      {
        timeToSend = millis();
        productIdReq();
        delay(100);
        flagStartup = 2;
        Serial.println("productIdReq");
      }
      if(cmd == '2')
      {
        timeToSendConf = millis();
        enableRotationVectorI2C(10);
        flagStartup = 3;
        Serial.println("enableRotationVector");
      }
        if(cmd == '3')
      {
        timeToSendConf = millis();
        softResetI2C();
        Serial.println("softReset");
      }
    }
  }
  else
  {
    if (((millis() - timeToSendConf) > timerToSendConf) && (flagStartup == 1))
    {
      timeToSend = millis();
      productIdReq();
      flagStartup = 2;
      Serial.println("productIdReq");
    }
    if (((millis() - timeToSend) > timerToSend) && (flagStartup == 2))
    {
      timeToSend = millis();
      enableRotationVectorI2C(10);
      flagStartup = 3;
      Serial.println("enableRotationVector");
    }
  }

   if (interruptOn())
  {
    timeToSend = millis();
    if((micros() - time_head_i2c) > timer_head_i2c)
    {
      receivePackets();
      time_head_i2c = micros();
    }
  }
}

void printSyncCom()
{
  Serial.print("syncCom:");
  Serial.println(syncCom);
}

void receivePackets()
{
  Wire.requestFrom((uint8_t)deviceAddress, requestBytes);
  readI2C();
}
uint8_t byteToread  = 0;
uint8_t readI2C()
{
  switch(syncCom)
  {
    case 0:
      byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        Serial.println(requestBytes);
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes = getRemainingBytes(shtpDataRead[1],shtpDataRead[0]);
        Serial.println(remainingBytes);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
        else
        {
          requestBytes = MAX_BUFF_LENGTH;
          syncCom = 1;
        }
      }

    break;
  
    case 1:
      byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes -= (MAX_BUFF_LENGTH - HEADER_LENGTH);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
      }

    break;

    case 2:
      byteToread = Wire.available();
      if (Wire.available() >= byteToread)
      {
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        syncCom = 0;
        printPacketI2C(requestBytes);
        requestBytes = HEADER_LENGTH;
        if(flagStartup == 0)
        {
          flagStartup = 1;
          Serial.println("flagStartup = 1");
        }
      }
      
    break;
  }
}

uint8_t packetContinue(uint8_t MSB)
{
  uint8_t temp_bit = (MSB >> 7);
  return temp_bit;
}

uint16_t getRemainingBytes(uint8_t MSB, uint8_t LSB)
{
  uint16_t dataLengtha = (((uint16_t)MSB) << 8) | ((uint16_t)LSB);
  dataLengtha &= ~((uint16_t)1 << 15); //Clear the MSbit.
  return dataLengtha;
}


uint8_t interruptOn()
{
  if (digitalRead(pinInterrupt) == LOW)
  {
    return true;
  }
  else
  {
    return false;
  }
}

void setFeatureCommandI2C(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
{
  long microsBetweenReports = (long)timeBetweenReports * 1000L;

  shtpDataWrite[0] = SHTP_REPORT_SET_FEATURE_COMMAND;  //Set feature command. Reference page 55
  shtpDataWrite[1] = reportID;                 //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
  shtpDataWrite[2] = 0;                  //Feature flags
  shtpDataWrite[3] = 0;                  //Change sensitivity (LSB)
  shtpDataWrite[4] = 0;                  //Change sensitivity (MSB)
  shtpDataWrite[5] = (microsBetweenReports >> 0) & 0xFF;  //Report interval (LSB) in microseconds. 0x7A120 = 500ms
  shtpDataWrite[6] = (microsBetweenReports >> 8) & 0xFF;  //Report interval
  shtpDataWrite[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
  shtpDataWrite[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
  shtpDataWrite[9] = 0;                  //Batch Interval (LSB)
  shtpDataWrite[10] = 0;                   //Batch Interval
  shtpDataWrite[11] = 0;                   //Batch Interval
  shtpDataWrite[12] = 0;                   //Batch Interval (MSB)
  shtpDataWrite[13] = (specificConfig >> 0) & 0xFF;    //Sensor-specific config (LSB)
  shtpDataWrite[14] = (specificConfig >> 8) & 0xFF;    //Sensor-specific config
  shtpDataWrite[15] = (specificConfig >> 16) & 0xFF;    //Sensor-specific config
  shtpDataWrite[16] = (specificConfig >> 24) & 0xFF;    //Sensor-specific config (MSB)

  //Transmit packet on channel 2, 17 bytes
  sendPacketI2C(CHANNEL_CONTROL, 17);
}

uint8_t sendPacketI2C(uint8_t channelNumber, uint8_t dataLength)
{
  uint8_t packetLength = dataLength + 4; //Add four bytes for the header

  Wire.beginTransmission(deviceAddress);

  shtpDataSend[0] = (packetLength & 0xFF);
  shtpDataSend[1] =(packetLength >> 8);
  shtpDataSend[2] =(channelNumber);
  shtpDataSend[3] =(sequenceNumber[channelNumber]); //Send the sequence number, increments with each packet sent, different counter for each channel

  for (uint8_t i = 0; i < (dataLength); i++)
  {
      shtpDataSend[i+4]= shtpDataWrite[i];
  }

  for (uint8_t i = 0; i < (dataLength + 4); i++)
  {
      Wire.write(shtpDataSend[i]);
  }
  
  sequenceNumber[channelNumber]++;
  Wire.endTransmission();
  return true;

}
void productIdReq()
{
  shtpDataWrite[0] = SHTP_REPORT_PRODUCT_ID_REQUEST;  //Set feature command. Reference page 55
  shtpDataWrite[1] = 0;  
  sendPacketI2C(CHANNEL_CONTROL,2);
}

void softResetI2C()
{
  shtpDataWrite[0] = 1;
  sendPacketI2C(CHANNEL_EXECUTABLE,1);
}

void enableRotationVectorI2C(uint16_t timeBetweenReports)
{
  setFeatureCommandI2C(SENSOR_REPORTID_ROTATION_VECTOR, timeBetweenReports, 0);
}

void enableAccelerometerI2C(uint16_t timeBetweenReports)
{
  setFeatureCommandI2C(SENSOR_REPORTID_LINEAR_ACCELERATION, timeBetweenReports, 0);
}

void printPacketI2C(uint8_t printBytes)
{
  if(DEBUG_I2C)
  {
    for (uint8_t j = 0; j < printBytes; j++)
    {
      Serial.print(shtpDataRead[j], HEX);
      Serial.print(",");
    }
    Serial.println();
  }
}

Thank you very much in advance for your time and support.

Andrea Moreschi, Ecodrone SRL
 
Last edited:
Was poking around but could not find an associated breakout board. But I did notice from the hillcrest website that said the 086 is backward compatible with the 080 including existing software. You might try using the sparkfun bno080 library just as an experiment to see if you get the same behavior. I also believe you should be running at 100mhz on the i2c bus.

It might also be a timing issue. I know when I was originally experimenting with the t4.1 and the bno080 I added some delays after the reads. But Paul put some fixes into the i2c lib to fix the issue. But it sounds like you are running into that now.
 
Thank you for your answer,

Our read and write methods are basically the same as the Sparkfun library. We tried that library immediately to see if the chip worked fine with the test board that we designed, and we tried the Adafruit one too.
We read also the previous and long thread about BNO080 but we didn't find any solution to this problem. We are pretty sure that the problem with the chip is in the writing procedure, but we don't know if it is an hardware, library or sketch problem. The fact that on ESP32 the sketch runs perfectly is suspicious and suggests that something could be wrong in the I2C library.
A BNO080/85 breakout board would be useful to test our methods and to undestand if something changed with the 086 version of the chip, but unfortunately they're out of stock everywhere...

Ecodrone
 
Thank you for your answer,

Our read and write methods are basically the same as the Sparkfun library. We tried that library immediately to see if the chip worked fine with the test board that we designed, and we tried the Adafruit one too.
We read also the previous and long thread about BNO080 but we didn't find any solution to this problem. We are pretty sure that the problem with the chip is in the writing procedure, but we don't know if it is an hardware, library or sketch problem. The fact that on ESP32 the sketch runs perfectly is suspicious and suggests that something could be wrong in the I2C library.
A BNO080/85 breakout board would be useful to test our methods and to undestand if something changed with the 086 version of the chip, but unfortunately they're out of stock everywhere...

Ecodrone

Good to know. Its hard to debug this type of issue without having hardware to test with as I am sure you know. But a guess is a timing issue, remember Teensy 4.1 is running at 600mhz. In going through the BNO080 thread noticed that the issue was primarily with i2c clock stretching.

You also might want to check this post out: https://forum.pjrc.com/threads/5826...-3-6-gt-gt-4-0?p=225042&viewfull=1#post225042 on where I had added delays. Also noted in that thread it seemed to work better at a something other that 600mhz - you might want to try 528mhz or lower.

PS As soon as i find my bno080 I can give your sketch a try and see what happens.
 
@EcodroneSRL

you are missing 3 functions from your example sketch:
softreseti2
enableRotationVectorI2C
and
productIdReq
 
Found my Sparkfun bno080 breakout and using the sketch in the zip was causing a crash on both the micro mod and t41. got rid of the crash on the micromod by changing slightly how you do interrupt handling to make it look like in the bno080 example but still crashed:
Code:
END SETUP
AFTER STARTUP ADVERTISE MSG READ:
- Send '1' to request ProductID msg
- Send '2' to enable Rotation Vector Output
4
14,1,0,0,
276
14,81,0,1,0,1,4,0,0,0,0,80,6,31,2E,30,2E,30,0,2,2,0,1,3,2,FF,7F,4,2,0,1,5,
14,81,0,1,0,1,4,0,0,0,0,80,6,31,2E,30,2E,30,0,2,2,0,1,3,2,FF,7F,4,2,0,1,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
276
F8,80,0,2,2,FF,7F,8,5,53,48,54,50,0,6,1,0,9,8,63,6F,6E,74,72,6F,6C,0,1,4,1,0,0,0,8,B,65,78,65,63,75,74,61,62,6C,65,0,6,1,1,9,7,64,65,76,69,63,65,0,1,4,2,0,0,0,8,A,73,65,6E,73,6F,72,68,75,62,0,6,1,2,9,8,63,6F,6E,74,72,6F,6C,0,6,1,3,9,C,69,6E,70,75,74,4E,6F,72,6D,61,6C,0,7,1,4,9,A,69,6E,70,75,74,57,61,6B,65,0,6,1,5,9,C,69,6E,70,75,74,47,79,72,6F,52,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
F8,80,0,2,2,FF,7F,8,5,53,48,54,50,0,6,1,0,9,8,63,6F,6E,74,72,6F,6C,0,1,4,1,0,0,0,8,B,65,78,65,63,75,74,61,62,6C,65,0,6,1,1,9,7,64,65,76,69,63,65,0,1,4,2,0,0,0,8,A,73,65,6E,73,6F,72,68,75,62,0,6,1,2,9,8,63,6F,6E,74,72,6F,
248
F8,10,F5,4,F3,10,F1,10,FB,5,FA,5,FC,11,EF,2,1,A,2,A,3,A,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,6F,72,6D,61,6C,0,7,1,4,9,A,69,6E,70,75,74,57,61,6B,65,0,6,1,5,9,C,69,6E,70,75,74,47,79,72,6F,52,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
F8,10,F5,4,F3,10,F1,10,FB,5,FA,5,FC,11,EF,2,1,A,2,A,3,A,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,
4344
F8,10,F5,4,F3,10,F1,10,FB,5,FA,5,FC,11,EF,2,1,A,2,A,3,A,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,FF,6F,72,6D,61,6C,0,7,1,4,9,A,69,6E,70,75,74,57,61,6B,65,0,6,1,5,9,C,69,6E,70,75,74,47,79,72,6F,52,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
CrashReport:
  A problem occurred at (system time) 0:0:0
  Code was executing from address 0x25E
  CFSR: 82
	(DACCVIOL) Data Access Violation
	(MMARVALID) Accessed Address: 0x0 (nullptr)
	  Check code at 0x25E - very likely a bug!
	  Run "addr2line -e sketch_dec18b.ino.elf 0x25E" for filename & line number.
  Temperature inside the chip was 38.24 °C
  Startup CPU clock speed is 528MHz
  Reboot was caused by auto reboot after fault or bad interrupt detected
  Breadcrumb #4 was 2888764994 (0xAC2F0E42)
  Breadcrumb #5 was 1661726339 (0x630BEE83)
  Breadcrumb #6 was 45893979 (0x2BC495B)
but looks like I did get some data. Doing a trace on the error:
Code:
addr2line -e sketch_dec18b.ino.elf 0x25E
addr2line: Dwarf Error: Can't find .debug_ranges section.

Never came across that error.
 
What IDE are you using? If the beta versions of Teensyduino. Addr2line at least for me did not work.
So I use the generated list file in the build directory
 
Hello again @mjs513 and @KurtE! Thank you again for your time. Sorry for the missing functions on the Teensy code, here is the complete version, but as you noticed it was already available in the zip file :eek: :

Code:
#include <Wire.h>
#include <math.h>

#define SERIAL_CMD true
uint8_t MAX_BUFF_LENGTH = 32;
uint8_t requestBytes = 4;

#define HEADER_LENGTH 4

#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD

//All the different sensors and features we can get reports from
//These are used when enabling a given sensor
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GRAVITY 0x06
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A
#define SENSOR_REPORTID_TAP_DETECTOR 0x10
#define SENSOR_REPORTID_STEP_COUNTER 0x11
#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
#define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14
#define SENSOR_REPORTID_RAW_GYROSCOPE 0x15
#define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16
#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
#define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28
//#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR*0x29

#define DEBUG_I2C true

const byte CHANNEL_COMMAND = 0;
const byte CHANNEL_EXECUTABLE = 1;
const byte CHANNEL_CONTROL = 2;
const byte CHANNEL_REPORTS = 3;
const byte CHANNEL_WAKE_REPORTS = 4;
const byte CHANNEL_GYRO = 5;

uint8_t deviceAddress = 0x4A;
uint8_t shtpDataRead[128];
uint8_t shtpDataWrite[128];
uint8_t shtpDataSend[128];
uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0};

uint8_t syncCom = 0;
uint16_t remainingBytes = 0;
uint8_t flagStartup = 0;

uint32_t timerToSend = 1000; 
uint32_t timeToSend = 0; 

uint32_t timerToSendConf = 2000; 
uint32_t timeToSendConf = 0;

uint32_t timer_head_i2c = 70; 
uint32_t time_head_i2c = 0; 
int pinInterrupt = 2;

void setup() 
{
  // put your setup code here, to run once:
  pinMode(pinInterrupt, INPUT_PULLUP);
  Serial.begin(115200);
  //softReset();
  Wire.begin();
  Wire.setClock(400000);
  delay(250);
  Serial.println("END SETUP");
  Serial.println("AFTER STARTUP ADVERTISE MSG READ:");
  Serial.println("- Send '1' to request ProductID msg");
  Serial.println("- Send '2' to enable Rotation Vector Output");
}

void loop() 
{
  if(SERIAL_CMD)
  {
    if(Serial.available()>0)
    {
      char cmd = Serial.read();
      if(cmd == '1')
      {
        timeToSend = millis();
        productIdReq();
        delay(100);
        flagStartup = 2;
        Serial.println("productIdReq");
      }
      if(cmd == '2')
      {
        timeToSendConf = millis();
        enableRotationVectorI2C(10);
        flagStartup = 3;
        Serial.println("enableRotationVector");
      }
        if(cmd == '3')
      {
        timeToSendConf = millis();
        softResetI2C();
        Serial.println("softReset");
      }
    }
  }
  else
  {
    if (((millis() - timeToSendConf) > timerToSendConf) && (flagStartup == 1))
    {
      timeToSend = millis();
      productIdReq();
      flagStartup = 2;
      Serial.println("productIdReq");
    }
    if (((millis() - timeToSend) > timerToSend) && (flagStartup == 2))
    {
      timeToSend = millis();
      enableRotationVectorI2C(10);
      flagStartup = 3;
      Serial.println("enableRotationVector");
    }
  }

   if (interruptOn())
  {
    timeToSend = millis();
    if((micros() - time_head_i2c) > timer_head_i2c)
    {
      receivePackets();
      time_head_i2c = micros();
    }
  }
}

void printSyncCom()
{
  Serial.print("syncCom:");
  Serial.println(syncCom);
}

void receivePackets()
{
  Wire.requestFrom((uint8_t)deviceAddress, requestBytes);
  readI2C();
}
uint8_t byteToread  = 0;
uint8_t readI2C()
{
  switch(syncCom)
  {
    case 0:
      byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        Serial.println(requestBytes);
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes = getRemainingBytes(shtpDataRead[1],shtpDataRead[0]);
        Serial.println(remainingBytes);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
        else
        {
          requestBytes = MAX_BUFF_LENGTH;
          syncCom = 1;
        }
      }

    break;
  
    case 1:
      byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes -= (MAX_BUFF_LENGTH - HEADER_LENGTH);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
      }

    break;

    case 2:
      byteToread = Wire.available();
      if (Wire.available() >= byteToread)
      {
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        syncCom = 0;
        printPacketI2C(requestBytes);
        requestBytes = HEADER_LENGTH;
        if(flagStartup == 0)
        {
          flagStartup = 1;
          Serial.println("flagStartup = 1");
        }
      }
      
    break;
  }
}

uint8_t packetContinue(uint8_t MSB)
{
  uint8_t temp_bit = (MSB >> 7);
  return temp_bit;
}

uint16_t getRemainingBytes(uint8_t MSB, uint8_t LSB)
{
  uint16_t dataLengtha = (((uint16_t)MSB) << 8) | ((uint16_t)LSB);
  dataLengtha &= ~((uint16_t)1 << 15); //Clear the MSbit.
  return dataLengtha;
}


uint8_t interruptOn()
{
  if (digitalRead(pinInterrupt) == LOW)
  {
    return true;
  }
  else
  {
    return false;
  }
}

void setFeatureCommandI2C(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
{
  long microsBetweenReports = (long)timeBetweenReports * 1000L;

  shtpDataWrite[0] = SHTP_REPORT_SET_FEATURE_COMMAND;  //Set feature command. Reference page 55
  shtpDataWrite[1] = reportID;                 //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
  shtpDataWrite[2] = 0;                  //Feature flags
  shtpDataWrite[3] = 0;                  //Change sensitivity (LSB)
  shtpDataWrite[4] = 0;                  //Change sensitivity (MSB)
  shtpDataWrite[5] = (microsBetweenReports >> 0) & 0xFF;  //Report interval (LSB) in microseconds. 0x7A120 = 500ms
  shtpDataWrite[6] = (microsBetweenReports >> 8) & 0xFF;  //Report interval
  shtpDataWrite[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
  shtpDataWrite[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
  shtpDataWrite[9] = 0;                  //Batch Interval (LSB)
  shtpDataWrite[10] = 0;                   //Batch Interval
  shtpDataWrite[11] = 0;                   //Batch Interval
  shtpDataWrite[12] = 0;                   //Batch Interval (MSB)
  shtpDataWrite[13] = (specificConfig >> 0) & 0xFF;    //Sensor-specific config (LSB)
  shtpDataWrite[14] = (specificConfig >> 8) & 0xFF;    //Sensor-specific config
  shtpDataWrite[15] = (specificConfig >> 16) & 0xFF;    //Sensor-specific config
  shtpDataWrite[16] = (specificConfig >> 24) & 0xFF;    //Sensor-specific config (MSB)

  //Transmit packet on channel 2, 17 bytes
  sendPacketI2C(CHANNEL_CONTROL, 17);
}

uint8_t sendPacketI2C(uint8_t channelNumber, uint8_t dataLength)
{
  uint8_t packetLength = dataLength + 4; //Add four bytes for the header

  Wire.beginTransmission(deviceAddress);

  shtpDataSend[0] = (packetLength & 0xFF);
  shtpDataSend[1] =(packetLength >> 8);
  shtpDataSend[2] =(channelNumber);
  shtpDataSend[3] =(sequenceNumber[channelNumber]); //Send the sequence number, increments with each packet sent, different counter for each channel

  for (uint8_t i = 0; i < (dataLength); i++)
  {
      shtpDataSend[i+4]= shtpDataWrite[i];
  }

  for (uint8_t i = 0; i < (dataLength + 4); i++)
  {
      Wire.write(shtpDataSend[i]);
  }
  
  sequenceNumber[channelNumber]++;
  Wire.endTransmission();
  return true;

}
void productIdReq()
{
  shtpDataWrite[0] = SHTP_REPORT_PRODUCT_ID_REQUEST;  //Set feature command. Reference page 55
  shtpDataWrite[1] = 0;  
  sendPacketI2C(CHANNEL_CONTROL,2);
}

void softResetI2C()
{
  shtpDataWrite[0] = 1;
  sendPacketI2C(CHANNEL_EXECUTABLE,1);
}

void enableRotationVectorI2C(uint16_t timeBetweenReports)
{
  setFeatureCommandI2C(SENSOR_REPORTID_ROTATION_VECTOR, timeBetweenReports, 0);
}

void enableAccelerometerI2C(uint16_t timeBetweenReports)
{
  setFeatureCommandI2C(SENSOR_REPORTID_LINEAR_ACCELERATION, timeBetweenReports, 0);
}

void printPacketI2C(uint8_t printBytes)
{
  if(DEBUG_I2C)
  {
    for (uint8_t j = 0; j < printBytes; j++)
    {
      Serial.print(shtpDataRead[j], HEX);
      Serial.print(",");
    }
    Serial.println();
  }
}

About the clock stretching, we didn't notice any way to manage or enable it in the Wire library. Is there something you suggest?
The code compiles and runs both on Arduino IDE 1.8.19 and 2.0.3. Teensyduino version is 1.57. We never bumped into that crash errors before. To reproduce the exact test case we suggest to unplug the Teensy and the BNO from the power after the code upload, open the Serial monitor and replug all again together. The expected output (we added some comments lines //) is:

Code:
END SETUP
AFTER STARTUP ADVERTISE MSG READ:
- Send '1' to request ProductID msg
- Send '2' to enable Rotation Vector Output
4
16,1,0,0,
278
16,81,0,1,0,1,4,0,0,0,0,80,6,31,2E,30,2E,30,0,2,2,0,1,3,2,FF,7F,4,2,0,1,5, //START of advertise message
FA,80,0,2,2,FF,7F,8,5,53,48,54,50,0,6,1,0,9,8,63,6F,6E,74,72,6F,6C,0,1,4,1,0,0,
DE,80,0,3,0,8,B,65,78,65,63,75,74,61,62,6C,65,0,6,1,1,9,7,64,65,76,69,63,65,0,1,4,
C2,80,0,4,2,0,0,0,8,A,73,65,6E,73,6F,72,68,75,62,0,6,1,2,9,8,63,6F,6E,74,72,6F,6C,
A6,80,0,5,0,6,1,3,9,C,69,6E,70,75,74,4E,6F,72,6D,61,6C,0,7,1,4,9,A,69,6E,70,75,74,
8A,80,0,6,57,61,6B,65,0,6,1,5,9,C,69,6E,70,75,74,47,79,72,6F,52,76,0,80,6,31,2E,31,2E,
6E,80,0,7,30,0,81,66,F8,10,F5,4,F3,10,F1,10,FB,5,FA,5,FC,11,EF,2,1,A,2,A,3,A,4,A,
52,80,0,8,5,E,6,A,7,10,8,C,9,E,A,8,B,8,C,6,D,6,E,6,F,10,10,5,11,C,12,6,
36,80,0,9,13,6,14,10,15,10,16,10,17,0,18,8,19,6,1A,0,1B,0,1C,6,1D,0,1E,10,1F,0,20,0,
1A,80,0,A,21,0,22,0,23,0,24,0,25,0,26,0,27,0,28,E,29,C,2A,E,2B,6, //END of advertise message
flagStartup = 1
4
5,0,1,0,
5
5,80,1,1,1,
4
14,0,2,0,
20
14,80,2,1,F1,0,84,0,0,0,1,0,0,0,0,0,0,0,0,0, //ANOTHER STARTUP MSG
4
15,0,2,2,
21
15,80,2,3,FC,5,0,0,0,10,27,0,0,0,0,0,0,0,0,0,0, //ANOTHER STARTUP MSG
enableRotationVector // WE PRESS '2' to enable Rotation Vector output data
4
17,0,3,0,
23
17,80,3,1,FB,36,0,0,0,5,0,0,0,13,2C,A4,D1,76,1,80,1,44,32, //FIRST CORRECT OUTPUT (0x17 = 23 bytes)
4
8,0,0,B,
8
8,80,0,C,1,E,E,E, //FIRST ERROR LIST MSG, FROM NOW THIS MSG IS SENT CONTINUOUSLY ON CH 0
4
9,0,0,D,
9
9,80,0,E,1,E,E,E,E, //SECOND ERROR LIST MSG
4
17,0,3,2,
23
17,80,3,3,FB,31,0,0,0,5,1,0,0,13,2C,A4,D1,76,1,80,1,44,32, //SECOND CORRECT OUTPUT (0x17 = 23 bytes)
4
B,0,0,F,
11
B,80,0,10,1,E,E,E,E,E,E, //THIRD ERROR LIST MSG
4
C,0,0,11,
12
C,80,0,12,1,E,E,E,E,E,E,E, //FOURTH ERROR LIST MSG
4
17,0,3,4,
23
17,80,3,5,FB,28,0,0,0,5,2,0,0,13,2C,A5,D1,75,1,80,1,44,32, //THIRD CORRECT OUTPUT (0x17 = 23 bytes)

Etc....

Thank you very much!

Ecodrone
 
In addition, I post a screen of the detail where in our opinion lies the error, or that Teensy periodically sends unwanted write requests (containing the buffer of the last request actually executed in the sketch) during the periodic reading of the data. In our opinion, this is a misbehavior due to the Wire library or Teensy itself.

BNO086 Logic.jpg

The I2C protocol is designed to receive a response to a request for a specific register. Unexpected writes come immediately after a read, so we suspect that there is an algorithm in the Wire library that repeats the write in case the response read is not what one expects.
 
Last edited:
Ok, found a workaround and the problem is clear!

When I write the request to enable the output of the Rotation Vector, if I add a beginTransmission to a different address and then make an endTransmission, the errors disappear.

Code:
uint8_t sendPacketI2C(uint8_t channelNumber, uint8_t dataLength)
{
  uint8_t packetLength = dataLength + 4; //Add four bytes for the header

  Wire.beginTransmission(deviceAddress);

  shtpDataSend[0] = (packetLength & 0xFF);
  shtpDataSend[1] =(packetLength >> 8);
  shtpDataSend[2] =(channelNumber);
  shtpDataSend[3] =(sequenceNumber[channelNumber]); //Send the sequence number, increments with each packet sent, different counter for each channel

  for (uint8_t i = 0; i < (dataLength); i++)
  {
      shtpDataSend[i+4]= shtpDataWrite[i];
  }

  for (uint8_t i = 0; i < (dataLength + 4); i++)
  {
      Wire.write(shtpDataSend[i]);
  }
  
  sequenceNumber[channelNumber]++;
  Wire.endTransmission();
  memset(shtpDataSend,0,sizeof(shtpDataSend));

  Wire.beginTransmission(0x4B); //WRITING TO A FAKE ADDRESS WORKAROUND
  Wire.endTransmission();
  return true;
}

So our suspicions are becoming more and more founded, namely that the Wire library makes unwanted writes by itself, probably to repeat the last message written for which it did not receive the response it expected. By doing any write to a different address on the bus, something internal to Wire is reset and the unwanted write request is no longer read by the BNO but probably repeated all the same.

Probably within the Wire library, if after writing the same address is not read in the response, this continues to write the last message after the first reading. It is a problem to be solved for particular protocols on I2C such as SHTP of BNO08x devices.

To use this protocol with I2C, which is blocking both in reading and in writing, it would be ideal to solve the problem of the rewritten message and insert functions that support DMA, and therefore non-blocking (asking Roll, Pitch, Yaw, AccXYZ, GyrXYZ, MagXYZ at 100Hz the I2C bus is full and consequently the loop is blocked).
 
Last edited:
@EcodroneSRL

Been playing with your sketch using a BNO080 and no matter what I tried to make you sketch work it doesn't. A couple of things seem to happen - 1) interrupts never fire on the BNO080, 2) I won't get any errors intially when I request product id but as I send more requests it always ends with a crashreport that looks like this:
Code:
END SETUP
AFTER STARTUP ADVERTISE MSG READ:
- Send '1' to request ProductID msg
- Send '2' to enable Rotation Vector Output
sendPacket(I2C): endTransmission successful
productIdReq
sendPacket(I2C): endTransmission successful
enableRotationVector
sendPacket(I2C): endTransmission successful
productIdReq
sendPacket(I2C): endTransmission successful
productIdReq
4
14,1,0,0,
276
I2C timeout
====

CrashReport:
  A problem occurred at (system time) 7:8:16
  Code was executing from address 0x2112
  CFSR: 8600
	(PRECISERR) Data bus error(address in BFAR)
	(BFARVALID) Accessed Address: 0x20078000
  Temperature inside the chip was 41.40 °C
  Startup CPU clock speed is 528MHz
  Reboot was caused by auto reboot after fault or bad interrupt detected

 *** Press any key to continue ***

On the other hand if I use the Sparkfun interrupt example:
Code:
BNO080 Read example with Interrupt handler, getReading and multiple access
LinearAccelerometer enabled, Output in form x, y, z, accuracy, in m/s^2
Rotation vector, Output in form i, j, k, real, accuracy
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
acc :0.00,0.00,-0.04,-0.04,High
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
acc :0.00,-0.01,0.00,0.00,High
quat:0.11,0.08,-0.77,0.62,Unreliable
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
acc :-0.01,-0.01,0.00,0.00,High
Doing other things
Doing other things
Doing other things
Doing other things
Doing other things
acc :-0.01,-0.01,-0.03,-0.03,High
quat:0.11,0.08,-0.77,0.62,Unreliable

So without a BNO086 can't really say for sure what the issue is.

Could be an issue with registers in my case - didnt check if they are the same, couple be the way you are doing your timing.
 
Our read and write methods are basically the same as the Sparkfun library. We tried that library immediately to see if the chip worked fine with the test board that we designed, and we tried the Adafruit one too.

Going to ask one more question. I am assuing that the Sparkfun library worked. If it did and I cant get your version working with the BNO080 then maybe something else going on.
 
Going to ask one more question. I am assuing that the Sparkfun library worked. If it did and I cant get your version working with the BNO080 then maybe something else going on.

We had a lot of trouble with Sparkfun library too, we've been able to see some output datas when we tried the library with your modifications that we found in the other big thread, but the data rate was really slow because of the same error messages filling the bus. The Adafruit library instead works very well.
 
We had a lot of trouble with Sparkfun library too, we've been able to see some output datas when we tried the library with your modifications that we found in the other big thread, but the data rate was really slow because of the same error messages filling the bus. The Adafruit library instead works very well.

Ok think I got it working now. Started with a simple extraction of info using a hybrid of your sketch and sparkfuns'
View attachment BNO080_86_initialization_test-221219a.zip

this seemed to work and gave me:
Code:
sendPacket(I2C): endTransmission successful
receivePacket (I2C): dataLength is: 276
I2C timeout
receivePacket (I2C): dataLength is: 276
receivePacket (I2C): dataLength is: 0
receivePacket (I2C): dataLength is: 20
receivePacket (I2C): dataLength is: 5
receivePacket (I2C): dataLength is: 0
sendPacket(I2C): endTransmission successful
receivePacket (I2C): dataLength is: 20
SW Version Major: 0x3 SW Version Minor: 0x2 SW Part Number: 0x98A498 SW Build Number: 0x172 SW Version Patch: 0x7
END SETUP
AFTER STARTUP ADVERTISE MSG READ:
- Send '1' to request ProductID msg
- Send '2' to enable Rotation Vector Output
issue was the softreset function and added the waitForI2C function.

Once I got that working and finding where the issue was I went back to your posted sketch and made some changes (note my device address is 4b and interrupt pin is 3 in the sketch):
View attachment sketch_dec19b-001.zip

and now looks like its working:
Code:
END SETUP
AFTER STARTUP ADVERTISE MSG READ:
- Send '1' to request ProductID msg
- Send '2' to enable Rotation Vector Output
4
34,0,2,4,
52
34,80,2,5,F8,0,1,2,96,A4,98,0,E6,0,0,0,4,0,0,0,F8,0,4,4,36,A3,98,0,95,1,0,0,
18,80,2,6,2,0,0,0,F8,0,4,2,E3,A2,98,0,D9,1,0,0,7,0,0,0,
flagStartup = 1
enableRotationVector
4
15,0,2,7,
21
15,80,2,8,FC,5,0,0,0,10,27,0,0,0,0,0,0,0,0,0,0,
4
17,0,3,0,
23
17,80,3,1,FB,33,0,0,0,5,0,0,0,B9,2,C8,1,66,CE,50,28,44,32,
4
17,0,3,2,
23
17,80,3,3,FB,17,0,0,0,5,1,0,0,B9,2,C8,1,66,CE,50,28,44,32,
4
17,0,3,4,
23
17,80,3,5,FB,15,0,0,0,5,2,0,0,B9,2,C8,1,66,CE,50,28,44,32,
4
17,0,3,6,
23
17,80,3,7,FB,17,0,0,0,5,3,0,0,B9,2,C8,1,66,CE,50,28,44,32,
4
17,0,3,8,
23
17,80,3,9,FB,19,0,0,0,5,4,0,0,BA,2,C8,1,66,CE,50,28,44,32,
4
17,0,3,A,
23
17,80,3,B,FB,17,0,0,0,5,5,0,0,BA,2,C8,1,66,CE,50,28,44,32,
4
17,0,3,C,
23
17,80,3,D,FB,15,0,0,0,5,6,0,0,BA,2,C8,1,66,CE,50,28,44,32,
4
17,0,3,E,
23
17,80,3,F,FB,15,0,0,0,5,7,0,0,BA,2,C8,1,66,CE,50,28,44,32,
4
17,0,3,10,
23
17,80,3,11,FB,17,0,0,0,5,8,0,0,BA,2,C7,1,66,CE,50,28,44,32,
4
17,0,3,12,
23
17,80,3,13,FB,17,0,0,0,5,9,0,0,BA,2,C7,1,66,CE,50,28,44,32,
4
17,0,3,14,
23
17,80,3,15,FB,15,0,0,0,5,A,0,0,BA,2,C7,1,66,CE,50,28,44,32,
4
17,0,3,16,
23
17,80,3,17,FB,17,0,0,0,5,B,0,0,BA,2,C7,1,66,CE,50,28,44,32,
4
17,0,3,18,
23
17,80,3,19,FB,17,0,0,0,5,C,0,0,BA,2,C7,1,66,CE,50,28,44,32,
4
17,0,3,1A,
23
17,80,3,1B,FB,17,0,0,0,5,D,0,0,BA,2,C7,1,66,CE,50,28,44,32,
4
17,0,3,1C,
you might give it a try and see if it works for you. remember to change the address and pin.
 
@EcodroneSRL
At least now I have you mods mostly working without crashing using a BNO080. [EDITIED]
 
Last edited:
Here is an updated sketch:
Code:
#include <Wire.h>
#include <math.h>

#define SERIAL_CMD true
uint8_t MAX_BUFF_LENGTH = 32;
uint8_t requestBytes = 4;

#define HEADER_LENGTH 4

#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD

//All the different sensors and features we can get reports from
//These are used when enabling a given sensor
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GRAVITY 0x06
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A
#define SENSOR_REPORTID_TAP_DETECTOR 0x10
#define SENSOR_REPORTID_STEP_COUNTER 0x11
#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
#define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14
#define SENSOR_REPORTID_RAW_GYROSCOPE 0x15
#define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16
#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
#define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28
//#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR*0x29

#define DEBUG_I2C true

const byte CHANNEL_COMMAND = 0;
const byte CHANNEL_EXECUTABLE = 1;
const byte CHANNEL_CONTROL = 2;
const byte CHANNEL_REPORTS = 3;
const byte CHANNEL_WAKE_REPORTS = 4;
const byte CHANNEL_GYRO = 5;

uint8_t deviceAddress = 0x4B;
uint8_t shtpDataRead[128];
uint8_t shtpDataWrite[128];
uint8_t shtpDataSend[128];
uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0};

uint8_t syncCom = 0;
uint16_t remainingBytes = 0;
uint8_t flagStartup = 0;

uint32_t timerToSend = 1000; 
uint32_t timeToSend = 0; 

uint32_t timerToSendConf = 2000; 
uint32_t timeToSendConf = 0;

uint32_t timer_head_i2c = 70; 
uint32_t time_head_i2c = 0; 
int pinInterrupt = 3;

void setup() 
{
  // put your setup code here, to run once:
  pinMode(pinInterrupt, INPUT_PULLUP);
  Serial.begin(115200);
  //softReset();
  Wire.begin();
  Wire.setClock(400000);
  delay(250);
  Serial.println("END SETUP");
  Serial.println("AFTER STARTUP ADVERTISE MSG READ:");
  Serial.println("- Send '1' to request ProductID msg");
  Serial.println("- Send '2' to enable Rotation Vector Output");
}

void loop() 
{
  if(SERIAL_CMD)
  {
    if(Serial.available()>0)
    {
      char cmd = Serial.read();
      if(cmd == '1')
      {
        timeToSend = millis();
        productIdReq();
        delay(100);
        flagStartup = 2;
        Serial.println("productIdReq");
      }
      if(cmd == '2')
      {
        timeToSendConf = millis();
        enableRotationVectorI2C(10);
        flagStartup = 3;
        Serial.println("enableRotationVector");
      }
        if(cmd == '3')
      {
        timeToSendConf = millis();
        softResetI2C();
        Serial.println("softReset");
      }
    }
  }
  else
  {
    if (((millis() - timeToSendConf) > timerToSendConf) && (flagStartup == 1))
    {
      timeToSend = millis();
      productIdReq();
      flagStartup = 2;
      Serial.println("productIdReq");
    }
    if (((millis() - timeToSend) > timerToSend) && (flagStartup == 2))
    {
      timeToSend = millis();
      enableRotationVectorI2C(10);
      flagStartup = 3;
      Serial.println("enableRotationVector");
    }
  }

   if (interruptOn())
  {
    timeToSend = millis();
    if((micros() - time_head_i2c) > timer_head_i2c)
    {
      receivePackets();
      time_head_i2c = micros();
    }
  }
}

void printSyncCom()
{
  Serial.print("syncCom:");
  Serial.println(syncCom);
}

bool receivePackets()
{
  Wire.requestFrom((uint8_t)deviceAddress, requestBytes);
  if (waitForI2C() == false)
    return (false); //Error
  return readI2C();
}
uint8_t byteToread  = 0;
uint8_t readI2C()
{
  switch(syncCom)
  {
    case 0:
      byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        Serial.println(requestBytes);
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes = getRemainingBytes(shtpDataRead[1],shtpDataRead[0]);
        Serial.println(remainingBytes);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
        else
        {
          requestBytes = MAX_BUFF_LENGTH;
          syncCom = 1;
        }
      }
    if( syncCom == 0) true;
    break;
  
    case 1:
      byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes -= (MAX_BUFF_LENGTH - HEADER_LENGTH);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
      }
     if( syncCom == 1) true;
    break;

    case 2:
      byteToread = Wire.available();
      if (Wire.available() >= byteToread)
      {
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        syncCom = 0;
        printPacketI2C(requestBytes);
        requestBytes = HEADER_LENGTH;
        if(flagStartup == 0)
        {
          flagStartup = 1;
          Serial.println("flagStartup = 1");
        }
      }
      return true;
    break;
  }
  return false;
}

uint8_t packetContinue(uint8_t MSB)
{
  uint8_t temp_bit = (MSB >> 7);
  return temp_bit;
}

uint16_t getRemainingBytes(uint8_t MSB, uint8_t LSB)
{
  uint16_t dataLengtha = (((uint16_t)MSB) << 8) | ((uint16_t)LSB);
  dataLengtha &= ~((uint16_t)1 << 15); //Clear the MSbit.
  return dataLengtha;
}


uint8_t interruptOn()
{
  if (digitalRead(pinInterrupt) == LOW)
  {
    return true;
  }
  else
  {
    return false;
  }
}

void setFeatureCommandI2C(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
{
  long microsBetweenReports = (long)timeBetweenReports * 1000L;

  shtpDataWrite[0] = SHTP_REPORT_SET_FEATURE_COMMAND;  //Set feature command. Reference page 55
  shtpDataWrite[1] = reportID;                 //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
  shtpDataWrite[2] = 0;                  //Feature flags
  shtpDataWrite[3] = 0;                  //Change sensitivity (LSB)
  shtpDataWrite[4] = 0;                  //Change sensitivity (MSB)
  shtpDataWrite[5] = (microsBetweenReports >> 0) & 0xFF;  //Report interval (LSB) in microseconds. 0x7A120 = 500ms
  shtpDataWrite[6] = (microsBetweenReports >> 8) & 0xFF;  //Report interval
  shtpDataWrite[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
  shtpDataWrite[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
  shtpDataWrite[9] = 0;                  //Batch Interval (LSB)
  shtpDataWrite[10] = 0;                   //Batch Interval
  shtpDataWrite[11] = 0;                   //Batch Interval
  shtpDataWrite[12] = 0;                   //Batch Interval (MSB)
  shtpDataWrite[13] = (specificConfig >> 0) & 0xFF;    //Sensor-specific config (LSB)
  shtpDataWrite[14] = (specificConfig >> 8) & 0xFF;    //Sensor-specific config
  shtpDataWrite[15] = (specificConfig >> 16) & 0xFF;    //Sensor-specific config
  shtpDataWrite[16] = (specificConfig >> 24) & 0xFF;    //Sensor-specific config (MSB)

  //Transmit packet on channel 2, 17 bytes
  sendPacketI2C(CHANNEL_CONTROL, 17);
}

uint8_t sendPacketI2C(uint8_t channelNumber, uint8_t dataLength)
{
  uint8_t packetLength = dataLength + 4; //Add four bytes for the header

  Wire.beginTransmission(deviceAddress);

  shtpDataSend[0] = (packetLength & 0xFF);
  shtpDataSend[1] =(packetLength >> 8);
  shtpDataSend[2] =(channelNumber);
  shtpDataSend[3] =(sequenceNumber[channelNumber]); //Send the sequence number, increments with each packet sent, different counter for each channel

  for (uint8_t i = 0; i < (dataLength); i++)
  {
      shtpDataSend[i+4]= shtpDataWrite[i];
  }

  for (uint8_t i = 0; i < (dataLength + 4); i++)
  {
      Wire.write(shtpDataSend[i]);
  }
  
  sequenceNumber[channelNumber]++;
  Wire.endTransmission();
  return true;

}
void productIdReq()
{
  shtpDataWrite[0] = SHTP_REPORT_PRODUCT_ID_REQUEST;  //Set feature command. Reference page 55
  shtpDataWrite[1] = 0;  
  sendPacketI2C(CHANNEL_CONTROL,2);
}

void softResetI2C()
{
  shtpDataWrite[0] = 1;
  sendPacketI2C(CHANNEL_EXECUTABLE,1);
  
  //Read all incoming data and flush it
  delay(50);
  while (receivePackets() == true)
    ; //delay(1);
  delay(50);
  while (receivePackets() == true)
    ; //delay(1);
}

void enableRotationVectorI2C(uint16_t timeBetweenReports)
{
  setFeatureCommandI2C(SENSOR_REPORTID_ROTATION_VECTOR, timeBetweenReports, 0);
}

void enableAccelerometerI2C(uint16_t timeBetweenReports)
{
  setFeatureCommandI2C(SENSOR_REPORTID_LINEAR_ACCELERATION, timeBetweenReports, 0);
}

void printPacketI2C(uint8_t printBytes)
{
  if(DEBUG_I2C)
  {
    for (uint8_t j = 0; j < printBytes; j++)
    {
      Serial.print(shtpDataRead[j], HEX);
      Serial.print(",");
    }
    Serial.println();
  }
}

boolean waitForI2C()
{
  for (uint8_t counter = 0; counter < 100; counter++) //Don't got more than 255
  {
    if (Wire.available() > 0)
      return (true);
    delay(1);
  }

  //if (_printDebug == true)
    Serial.println(F("I2C timeout"));
  return (false);
}
 
I am trying the original sketch, which faults on Startup...

Especially if it is running or faulted before. One reason is the Interrupt pin is low at startup as the BNO probably still has data to be processed.

Screenshot.jpg
Note: I added a few debug messages (ones with readI2C

Code:
END SETUP
AFTER STARTUP ADVERTISE MSG READ:
- Send '1' to request ProductID msg
- Send '2' to enable Rotation Vector Output
readI2C: 0  avail: 4
4
DC,80,0,4,
220
readI2C: 1  avail: 32
DC,80,0,5,0,8,B,65,78,65,63,75,74,61,62,6C,65,0,6,1,1,9,7,64,65,76,69,63,65,0,1,4,
CrashReport:
  A problem occurred at (system time) 8:40:10
  Code was executing from address 0x1C8
  CFSR: 82
	(DACCVIOL) Data Access Violation
	(MMARVALID) Accessed Address: 0x1 (nullptr)
	  Check code at 0x1C8 - very likely a bug!
	  Run "addr2line -e mysketch.ino.elf 0x1C8" for filename & line number.
  Temperature inside the chip was 44.27 °C
  Startup CPU clock speed is 600MHz
  Reboot was caused by auto reboot after fault or bad interrupt detected

Code:
        shtpDataRead[k] = Wire.read();
     1ae:	f802 1b01 	strb.w	r1, [r2], #1
        for (uint8_t k = 0; k < byteToread; k++) {
     1b2:	4282      	cmp	r2, r0
     1b4:	d1f3      	bne.n	19e <readI2C()+0xee>
     1b6:	f1bc 0f00 	cmp.w	ip, #0
     1ba:	d001      	beq.n	1c0 <readI2C()+0x110>
     1bc:	f884 30a2 	strb.w	r3, [r4, #162]	; 0xa2
		return endTransmission(1);
     1c0:	2101      	movs	r1, #1
     1c2:	4833      	ldr	r0, [pc, #204]	; (290 <readI2C()+0x1e0>)
     1c4:	f000 fb74 	bl	8b0 <TwoWire::endTransmission(unsigned char)>
        printPacketI2C(requestBytes);
   [COLOR="#FF0000"]  1c8:	7830      	ldrb	r0, [r6, #0][/COLOR]
     1ca:	f7ff ff4d 	bl	68 <printPacketI2C(unsigned char)>
  dataLengtha &= ~((uint16_t)1 << 15);  //Clear the MSbit.
     1ce:	8839      	ldrh	r1, [r7, #0]
        remainingBytes = getRemainingBytes(shtpDataRead[1], shtpDataRead[0]);
     1d0:	4c36      	ldr	r4, [pc, #216]	; (2ac <readI2C()+0x1fc>)

Anytime that I played with devices like this, I typically tried to do a full hardware reset (pull reset pin low) or if not setup, at least do the software reset.
So will try next to change:
Code:
  //softReset();
  Wire.begin();
  Wire.setClock(400000);
  delay(250);
To:
Code:
  Wire.begin();
  Wire.setClock(400000);
  softReset();
  delay(250);
And see if it makes a difference
 
After the code upload, you have to unplug the teensy, reopen the console, and then replug the teensy from usb (when we unplug the teensy also the bno is powered off together). When you replug the USB you can also sample with Saleae. That's the procedure to produce the test case. But that crash error is totally new for us honestly, I can't say what it is...

@mjs513 Thank you for the updated sketch, I'm going to try it tomorrow and let you know!:eek:

@KurtE yes we tried every combination with softReset(). Tomorrow morning I will give it another try to it, placing the softReset() in the order you suggested. Thank you very much for your support, I'm going to update you if we discover something new!

P.s. did you give a try to the ESP32 version with BNO080? That's working perfectly with BNO086!
 
@KurtE
The Sparkfun softreset in addition to sending the reset command reads whats left and flushes it:
Code:
	//Read all incoming data and flush it
	delay(50);
	while (receivePacket() == true)
		; //delay(1);
	delay(50);
	while (receivePacket() == true)
		; //delay(1);

Switching to pin 3 I don't get that startup crash.
 
Yes - I merged your changes into mine... So logically the same, except I have debug messages and DigitalWrites...

And Sketch has been running for an hour or so now.

Code:
#include <Wire.h>
#include <math.h>

#define SERIAL_CMD true
uint8_t MAX_BUFF_LENGTH = 32;
uint8_t requestBytes = 4;

#define HEADER_LENGTH 4

#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD

//All the different sensors and features we can get reports from
//These are used when enabling a given sensor
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GRAVITY 0x06
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A
#define SENSOR_REPORTID_TAP_DETECTOR 0x10
#define SENSOR_REPORTID_STEP_COUNTER 0x11
#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
#define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14
#define SENSOR_REPORTID_RAW_GYROSCOPE 0x15
#define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16
#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
#define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28
//#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR*0x29

#define DEBUG_I2C true

const byte CHANNEL_COMMAND = 0;
const byte CHANNEL_EXECUTABLE = 1;
const byte CHANNEL_CONTROL = 2;
const byte CHANNEL_REPORTS = 3;
const byte CHANNEL_WAKE_REPORTS = 4;
const byte CHANNEL_GYRO = 5;

uint8_t deviceAddress = 0x4B;
uint8_t shtpDataRead[128];
uint8_t shtpDataWrite[128];
uint8_t shtpDataSend[128];
uint8_t sequenceNumber[6] = { 0, 0, 0, 0, 0, 0 };

uint8_t syncCom = 0;
uint16_t remainingBytes = 0;
uint8_t flagStartup = 0;

uint32_t timerToSend = 1000;
uint32_t timeToSend = 0;

uint32_t timerToSendConf = 2000;
uint32_t timeToSendConf = 0;

uint32_t timer_head_i2c = 70;
uint32_t time_head_i2c = 0;
int pinInterrupt = 2;

void setup() {
  // put your setup code here, to run once:
  while(!Serial && millis() < 3000) ;
  if (CrashReport) {
    Serial.print(CrashReport);
    while (Serial.read() == -1);
    while (Serial.read() != -1);
  }
  pinMode(pinInterrupt, INPUT_PULLUP);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);
  softResetI2C();
  delay(250);
  Serial.println("END SETUP");
  Serial.println("AFTER STARTUP ADVERTISE MSG READ:");
  Serial.println("- Send '1' to request ProductID msg");
  Serial.println("- Send '2' to enable Rotation Vector Output");
}

void loop() {
  if (SERIAL_CMD) {
    if (Serial.available() > 0) {
      char cmd = Serial.read();
      if (cmd == '1') {
        timeToSend = millis();
        productIdReq();
        delay(100);
        flagStartup = 2;
        Serial.println("productIdReq");
      }
      if (cmd == '2') {
        timeToSendConf = millis();
        enableRotationVectorI2C(10);
        flagStartup = 3;
        Serial.println("enableRotationVector");
      }
      if (cmd == '3') {
        timeToSendConf = millis();
        softResetI2C();
        Serial.println("softReset");
      }
    }
  } else {
    if (((millis() - timeToSendConf) > timerToSendConf) && (flagStartup == 1)) {
      timeToSend = millis();
      productIdReq();
      flagStartup = 2;
      Serial.println("productIdReq");
    }
    if (((millis() - timeToSend) > timerToSend) && (flagStartup == 2)) {
      timeToSend = millis();
      enableRotationVectorI2C(10);
      flagStartup = 3;
      Serial.println("enableRotationVector");
    }
  }

  if (interruptOn()) {
    timeToSend = millis();
    if ((micros() - time_head_i2c) > timer_head_i2c) {
      receivePackets();
      time_head_i2c = micros();
    }
  }
}

void printSyncCom() {
  Serial.print("syncCom:");
  Serial.println(syncCom);
}

bool receivePackets() {
  Wire.requestFrom((uint8_t)deviceAddress, requestBytes);
  if (waitForI2C() == false)
    return (false);  //Error
  return readI2C();
}
uint8_t byteToread = 0;
uint8_t readI2C() {
  Serial.printf("readI2C: %u ", syncCom); Serial.flush();
  switch (syncCom) {
    case 0:
      byteToread = Wire.available();
      Serial.printf(" avail: %u\n", byteToread); Serial.flush();
      if (byteToread >= requestBytes) {
        Serial.println(requestBytes);
        for (uint8_t k = 0; k < byteToread; k++) {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes = getRemainingBytes(shtpDataRead[1], shtpDataRead[0]);
        Serial.println(remainingBytes);
        if (remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH)) {
          requestBytes = remainingBytes;
          syncCom = 2;
        } else {
          requestBytes = MAX_BUFF_LENGTH;
          syncCom = 1;
        }
      }

      break;

    case 1:
      byteToread = Wire.available();
      Serial.printf(" avail: %u\n", byteToread); Serial.flush();
      if (byteToread >= requestBytes) {
        for (uint8_t k = 0; k < byteToread; k++) {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        printPacketI2C(requestBytes);
        remainingBytes -= (MAX_BUFF_LENGTH - HEADER_LENGTH);
        if (remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH)) {
          requestBytes = remainingBytes;
          syncCom = 2;
        }
      }

      break;

    case 2:
      byteToread = Wire.available();
      Serial.printf(" avail: %u\n", byteToread); Serial.flush();
      if (Wire.available() >= byteToread) {
        for (uint8_t k = 0; k < byteToread; k++) {
          shtpDataRead[k] = Wire.read();
        }
        Wire.endTransmission();
        syncCom = 0;
        printPacketI2C(requestBytes);
        requestBytes = HEADER_LENGTH;
        if (flagStartup == 0) {
          flagStartup = 1;
          Serial.println("flagStartup = 1");
        }
      }
      return true;
      break;
  }
  return false;
}

uint8_t packetContinue(uint8_t MSB) {
  uint8_t temp_bit = (MSB >> 7);
  return temp_bit;
}

uint16_t getRemainingBytes(uint8_t MSB, uint8_t LSB) {
  uint16_t dataLengtha = (((uint16_t)MSB) << 8) | ((uint16_t)LSB);
  dataLengtha &= ~((uint16_t)1 << 15);  //Clear the MSbit.
  return dataLengtha;
}


uint8_t interruptOn() {
  if (digitalRead(pinInterrupt) == LOW) {
    return true;
  } else {
    return false;
  }
}

void setFeatureCommandI2C(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig) {
  long microsBetweenReports = (long)timeBetweenReports * 1000L;

  shtpDataWrite[0] = SHTP_REPORT_SET_FEATURE_COMMAND;      //Set feature command. Reference page 55
  shtpDataWrite[1] = reportID;                             //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
  shtpDataWrite[2] = 0;                                    //Feature flags
  shtpDataWrite[3] = 0;                                    //Change sensitivity (LSB)
  shtpDataWrite[4] = 0;                                    //Change sensitivity (MSB)
  shtpDataWrite[5] = (microsBetweenReports >> 0) & 0xFF;   //Report interval (LSB) in microseconds. 0x7A120 = 500ms
  shtpDataWrite[6] = (microsBetweenReports >> 8) & 0xFF;   //Report interval
  shtpDataWrite[7] = (microsBetweenReports >> 16) & 0xFF;  //Report interval
  shtpDataWrite[8] = (microsBetweenReports >> 24) & 0xFF;  //Report interval (MSB)
  shtpDataWrite[9] = 0;                                    //Batch Interval (LSB)
  shtpDataWrite[10] = 0;                                   //Batch Interval
  shtpDataWrite[11] = 0;                                   //Batch Interval
  shtpDataWrite[12] = 0;                                   //Batch Interval (MSB)
  shtpDataWrite[13] = (specificConfig >> 0) & 0xFF;        //Sensor-specific config (LSB)
  shtpDataWrite[14] = (specificConfig >> 8) & 0xFF;        //Sensor-specific config
  shtpDataWrite[15] = (specificConfig >> 16) & 0xFF;       //Sensor-specific config
  shtpDataWrite[16] = (specificConfig >> 24) & 0xFF;       //Sensor-specific config (MSB)

  //Transmit packet on channel 2, 17 bytes
  sendPacketI2C(CHANNEL_CONTROL, 17);
}

uint8_t sendPacketI2C(uint8_t channelNumber, uint8_t dataLength) {
  digitalWriteFast(4, HIGH);
  uint8_t packetLength = dataLength + 4;  //Add four bytes for the header

  Wire.beginTransmission(deviceAddress);

  shtpDataSend[0] = (packetLength & 0xFF);
  shtpDataSend[1] = (packetLength >> 8);
  shtpDataSend[2] = (channelNumber);
  shtpDataSend[3] = (sequenceNumber[channelNumber]);  //Send the sequence number, increments with each packet sent, different counter for each channel

  for (uint8_t i = 0; i < (dataLength); i++) {
    shtpDataSend[i + 4] = shtpDataWrite[i];
  }

  for (uint8_t i = 0; i < (dataLength + 4); i++) {
    Wire.write(shtpDataSend[i]);
  }

  sequenceNumber[channelNumber]++;
  Wire.endTransmission();
  digitalWriteFast(4, LOW);
  return true;
}
void productIdReq() {
  shtpDataWrite[0] = SHTP_REPORT_PRODUCT_ID_REQUEST;  //Set feature command. Reference page 55
  shtpDataWrite[1] = 0;
  sendPacketI2C(CHANNEL_CONTROL, 2);
}

void softResetI2C() {
  shtpDataWrite[0] = 1;
  sendPacketI2C(CHANNEL_EXECUTABLE, 1);

  //Read all incoming data and flush it
  delay(50);
  while (receivePackets() == true)
    ;  //delay(1);
  delay(50);
  while (receivePackets() == true)
    ;  //delay(1);
}

void enableRotationVectorI2C(uint16_t timeBetweenReports) {
  setFeatureCommandI2C(SENSOR_REPORTID_ROTATION_VECTOR, timeBetweenReports, 0);
}

void enableAccelerometerI2C(uint16_t timeBetweenReports) {
  setFeatureCommandI2C(SENSOR_REPORTID_LINEAR_ACCELERATION, timeBetweenReports, 0);
}

void printPacketI2C(uint8_t printBytes) {
  if (DEBUG_I2C) {
    Serial.printf("printPacketI2C(%u)\n", printBytes); Serial.flush();
    for (uint8_t j = 0; j < printBytes; j++) {
      Serial.print(shtpDataRead[j], HEX);
      Serial.print(",");
    }
    Serial.println();
  }
}

boolean waitForI2C() {
  for (uint8_t counter = 0; counter < 100; counter++)  //Don't got more than 255
  {
    if (Wire.available() > 0)
      return (true);
    delay(1);
  }

  //if (_printDebug == true)
  Serial.println(F("I2C timeout"));
  return (false);
}
 
Are you experiencing the error messages between data readings or with this startup procedure is all fine and you get the data packets only? Thank you!
 
Are you experiencing the error messages between data readings or with this startup procedure is all fine and you get the data packets only? Thank you!

With the sketch I posted in post #17 I am seeing what appears to be a counter between reports as I showed in post #15:
Code:
[COLOR="#FF0000"]17,0,3,6,[/COLOR]
23
17,80,3,7,FB,15,0,0,0,5,8B,0,0,87,2,DB,1,15,D0,4F,2A,44,32,
[COLOR="#FF0000"]4
17,0,3,8,[/COLOR]
23
17,80,3,9,FB,12,0,0,0,5,8C,0,0,87,2,DB,1,15,D0,4F,2A,44,32,
[COLOR="#FF0000"]4
17,0,3,A,[/COLOR]
23
17,80,3,B,FB,14,0,0,0,5,8D,0,0,87,2,DB,1,15,D0,4F,2A,44,32,
[COLOR="#FF0000"]4
17,0,3,C[/COLOR]
23
17,80,3,D,FB,12,0,0,0,5,8E,0,0,87,2,DB,1,15,D0,4F,2A,44,32,
which I believe matches your correct message format in post #8.

I have not tried @KurtE's sketch in post #21 but will.
 
Are you experiencing the error messages between data readings or with this startup procedure is all fine and you get the data packets only? Thank you!

With @KurtE's sketch on a BNO080 once I request the report I see that data as shown in previous post.
 
Code:
uint8_t readI2C()
{
  switch(syncCom)
  {
    case 0:
      byteToread = Wire.available();
      if (byteToread >= requestBytes)
      {
        Serial.println(requestBytes);
        for (uint8_t k = 0; k < byteToread; k++)
        {
          shtpDataRead[k] = Wire.read();
        }
       [COLOR="#FF0000"] Wire.endTransmission();[/COLOR]
        printPacketI2C(requestBytes);
        remainingBytes = getRemainingBytes(shtpDataRead[1],shtpDataRead[0]);
        Serial.println(remainingBytes);
        if(remainingBytes < (MAX_BUFF_LENGTH - HEADER_LENGTH))
        {
          requestBytes = remainingBytes;
          syncCom = 2;
        }

Aren't these calls to endTransmission superfluous? The read is terminated by nacking the last read. The library then puts a stop on the bus.
A call to endTransmission does send something, looking at the code in the library:

Code:
uint8_t TwoWire::endTransmission(uint8_t sendStop)
{
  // transmit buffer (blocking)
[B]  int8_t ret = twi_writeTo(txAddress, txBuffer, txBufferLength, 1, sendStop);[/B]
  // reset tx buffer iterator vars
  txBufferIndex = 0;
  txBufferLength = 0;
  // indicate that we are done transmitting
  transmitting = 0;
  return ret;
}
 
Back
Top