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:
ESP32 Code:
Thank you very much in advance for your time and support.
Andrea Moreschi, Ecodrone SRL
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:
:
