Hello, I'm using an Arduino Mega 2560 and a Sparkfun BNO080 IMU sensor. The wiring is good, and I used a logic converter to convert the Arduino's 5V I2C to 3.3V. I wanted to create a compact code that only handles vector rotation in AVR C. I'm trying to retrieve the quaternion values with shtp, but apparently the sensor isn't responding; it doesn't return the product ID.

The begin function always returns false, and then the sensor reads the wrong channel. I don't understand why. However, the I2C communication works; I did a scan, and it's the correct address. It's really with the SHTP communication that I'm having trouble, I was inspired by the Arduino SparkFun libraries.
Can someone help me please?

Here is my code:

#define F_CPU 16000000UL
#define F_SCL 400000UL // 400kHz
#include <avr/io.h>
#include <util/delay.h>
#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include "SerialEvdm.h" //for serial communication and serial port display

// === I2C ===

#define TWBR_VAL ((F_CPU/F_SCL - 16)/2)
#define BNO080_ADDRESS 0x4B
#define MAX_PACKET_SIZE 128
#define MAX_METADATA_SIZE 9
bool sendPacket(uint8_t channelNumber, uint8_t* data, uint8_t dataLength);
bool receivePacket();

void I2C_Init() {
    TWSR = 0;
    TWBR = (uint8_t)TWBR_VAL;
}

bool I2C_Start(uint8_t address, bool read) {
    TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
    while (!(TWCR & (1 << TWINT)));
    if ((TWSR & 0xF8) != 0x08 && (TWSR & 0xF8) != 0x10)
    return false;

    TWDR = (address << 1) | (read ? 1 : 0);
    TWCR = (1 << TWINT) | (1 << TWEN);
    while (!(TWCR & (1 << TWINT)));

    uint8_t status = TWSR & 0xF8;
    return (read ? status == 0x40 : status == 0x18);
}

void I2C_Stop() {
    TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
    while (TWCR & (1 << TWSTO));
}

bool I2C_WriteByte(uint8_t data) {
    TWDR = data;
    TWCR = (1 << TWINT) | (1 << TWEN);
    while (!(TWCR & (1 << TWINT)));
    return (TWSR & 0xF8) == 0x28;
}

uint8_t I2C_ReadByte(bool ack) {
    TWCR = (1 << TWINT) | (1 << TWEN) | (ack ? (1 << TWEA) : 0);
    while (!(TWCR & (1 << TWINT)));
    return TWDR;
}

bool I2C_Write(uint8_t address, uint8_t* data, uint8_t length) {
    if (!I2C_Start(address, false)) return false;
    for (uint8_t i = 0; i < length; i++) {
        if (!I2C_WriteByte(data[i])) {
            I2C_Stop();
            return false;
        }
    }
    I2C_Stop();
    return true;
}

bool I2C_Read(uint8_t address, uint8_t* data, uint8_t length) {
    if (!I2C_Start(address, true)) return false;
    for (uint8_t i = 0; i < length; i++) {
        data[i] = I2C_ReadByte(i < (length - 1));
    }
    I2C_Stop();
    return true;
}

// === BNO080 / SHTP ===
#define CHANNEL_COMMAND  0
#define CHANNEL_EXECUTABLE 1
#define CHANNEL_CONTROL 2
#define CHANNEL_REPORTS 3
#define CHANNEL_WAKE_REPORTS 4
#define CHANNEL_GYRO 5

#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

#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08

#define FRS_RECORDID_ROTATION_VECTOR 0xE30B

#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD

#define FRS_RECORDID_ROTATION_VECTOR 0xE30B

#define EXECUTABLE_RESET_COMPLETE 0x1

typedef struct {
    uint16_t packetLength;
    uint8_t channelNumber;
    uint8_t sequenceNumber;
} SHTP_Header;

uint8_t shtpData[128];
SHTP_Header shtpHeader;
uint8_t sequenceNumber[6] = {0};

float quatI = 0, quatJ = 0, quatK = 0, quatReal = 0;

float qToFloat(int16_t fixed, uint8_t q) {
    return fixed * powf(2, -q);
}

void softReset (void) {
   
    shtpData[0] = 1;
    sendPacket(CHANNEL_CONTROL, shtpData, 1);

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

bool begin() {
   
    // BNO080 reset
    softReset();
    _delay_ms(1000);
   
    I2C_Init();
    MyInitSerial();
    _delay_ms(1000);

    PrintString("Init...\n");

    if (!I2C_Start(BNO080_ADDRESS, false)) {
        PrintString("BNO080 not found!\n");
        while (1);
    }
   
    I2C_Stop();
    PrintString("BNO080 detected\n");

    // Clear junk packets
    for (uint8_t i = 0; i < 3; i++) {
        receivePacket();
        _delay_ms(100);
    }
   
    // Request product ID
    shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST;
    shtpData[1] = 0; // Reserved
    sendPacket(CHANNEL_CONTROL, shtpData, 2);
    // Wait for response
    if (receivePacket()) {
        if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) {
            // Display product info (optional, for debugging)
            PrintString("Product ID Response:\n");
            uint32_t SW_Part_Number = ((uint32_t)shtpData[7] << 24) | ((uint32_t)shtpData[6] << 16) |
            ((uint32_t)shtpData[5] << 8) | ((uint32_t)shtpData[4]);
            PrintString("SW Part Number: ");
            PrintHex((uint8_t*)&SW_Part_Number, sizeof(SW_Part_Number));
            return true;
        }
    }
    PrintString("not received\n");
    return false; // If the response was not received or is incorrect
}

bool sendPacket(uint8_t channelNumber, uint8_t* data, uint8_t dataLength) {
    uint8_t packet[dataLength + 4];
    packet[0] = (dataLength + 4) & 0xFF;
    packet[1] = ((dataLength + 4) >> 8) & 0xFF;
    packet[2] = channelNumber;
    packet[3] = sequenceNumber[channelNumber]++;

    memcpy(&packet[4], data, dataLength);
    return I2C_Write(BNO080_ADDRESS, packet, dataLength + 4);
}

bool receivePacket() {
    uint8_t header[4];
    if (!I2C_Read(BNO080_ADDRESS, header, 4)) return false;

    shtpHeader.packetLength = (header[1] << 8) | header[0];
    shtpHeader.channelNumber = header[2];
    shtpHeader.sequenceNumber = header[3];


    if (shtpHeader.packetLength <= 4) return false;


    if (!I2C_Read(BNO080_ADDRESS, shtpData, 1)) return false;

    uint8_t reportID = shtpData[0];

    uint8_t remainingLength = 0;
    if (reportID == 0x08 || reportID == 0x29)
    remainingLength = 21 - 1;
    else
    remainingLength = 23 - 1;

    if (!I2C_Read(BNO080_ADDRESS, &shtpData[1], remainingLength)) return false;

    return true;
}

void enableRotationVector(uint16_t interval_ms) {
    uint32_t interval_us = interval_ms * 1000UL;
    uint8_t command[17] = {
        SHTP_REPORT_SET_FEATURE_COMMAND,
        SENSOR_REPORTID_ROTATION_VECTOR,
        0x00, 0x00, 0x00,
        interval_us & 0xFF,
        (interval_us >> 8) & 0xFF,
        (interval_us >> 16) & 0xFF,
        (interval_us >> 24) & 0xFF,
        0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00
    };
    sendPacket(CHANNEL_CONTROL, command, sizeof(command));
}

void parseRotationVector() {
    if (shtpHeader.channelNumber == CHANNEL_REPORTS &&
    shtpData[0] == SENSOR_REPORTID_ROTATION_VECTOR) {

        quatI = qToFloat((int16_t)(shtpData[2] | (shtpData[3] << 8)), 14);
        quatJ = qToFloat((int16_t)(shtpData[4] | (shtpData[5] << 8)), 14);
        quatK = qToFloat((int16_t)(shtpData[6] | (shtpData[7] << 8)), 14);
        quatReal = qToFloat((int16_t)(shtpData[8] | (shtpData[9] << 8)), 14);
    }
}

void printQuaternion() {
    char buf[64];
    snprintf(buf, sizeof(buf), "Quat: i=%.2f j=%.2f k=%.2f real=%.2f\n", quatI, quatJ, quatK, quatReal);
    PrintString(buf);
}

int main(void) {
    int count = 0;
   
   
    begin();

    enableRotationVector(50); // 50ms = 20Hz

    while (count <10) {
        if (receivePacket()) {
            // Display Channel and Report ID
            char debugMsg[64];
            snprintf(debugMsg, sizeof(debugMsg), "Channel: %d, ReportID: 0x%02X\n", shtpHeader.channelNumber, shtpData[0]);
            PrintString(debugMsg); // Uses your PrintString function to display

            // Display raw data
            PrintString("Data: ");
            PrintHex(shtpData, shtpHeader.packetLength - 4); // Displays the received data in hex

            // If it's a quaternion report, parse it
            parseRotationVector();
            printQuaternion(); // Displays the quaternion values
            count++;
        }
        _delay_ms(10); // Small delay for more responsive reading
    }
}

This is what I get on the serial port now:

Init...

BNO080 detected

not received

Channel: 3, ReportID: 0x9C

Data: 9C 9C 80 03 2C 3C A5 C1 05 B5 FB 20 D8 8B 31 44 32 05 BE 44 99 C1 05 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 9A ED 3F FD 47 CF E3 D6 9F 14 35 EF FB 8A 3C 8D 5A 57 6B 3B D7 99 B0 4E

Quat: i=? j=? k=? real=?

Channel: 3, ReportID: 0x8A

Data: 8A 8A 80 03 2F B5 FB 1F D8 8A 31 44 32 05 BF 4C 8E C0 05 B5 FB 1F D8 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 9A ED 3F FD 47 CF

Quat: i=? j=? k=? real=?

Channel: 3, ReportID: 0x78

Data: 78 78 80 03 32 8B 31 44 32 05 C0 54 83 C0 05 B5 FB 1F D8 8A 31 44 32 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

Quat: i=? j=? k=? real=?

Channel: 3, ReportID: 0x66

Data: 66 66 80 03 35 05 C1 5C 77 C0 05 B4 FB 1F D8 8A 31 44 32 05 C2 64 6C 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

Quat: i=? j=? k=? real=?

Channel: 3, ReportID: 0x54

Data: 54 54 80 03 38 BF 05 B5 FB 1F D8 8B 31 44 32 05 C3 6C 61 BF 05 B4 FB 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

Quat: i=? j=? k=? real=?

Channel: 3, ReportID: 0x42

Data: 42 42 80 03 3B 20 D8 8B 31 44 32 05 C4 74 56 BF 05 B4 FB 20 D8 8B 31 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

Quat: i=? j=? k=? real=?

 

SHTP communication is complex protocol. I've used the BNO086 that fixed the com bugs in the earlier versions like the 80. It's a packet based message protocol that requires at bit of setup before you get good data. I used SPI and interrupt driven I/O for speed and device abstraction.

https://github.com/adafruit/Adafruit_CircuitPython_BNO08x/issues/7

• Install an external 1K resistor from SDA to 3.3V.
• Install an external 4.7K resistor from SCL to 3.3V.=

https://forum.allaboutcircuits.com/...tter-for-this-application.193257/post-1818575
https://forum.allaboutcircuits.com/...tter-for-this-application.193257/post-1823342

 

Hello, thank you for your reply.
However, I see the resistors integrated into the sensor for the I2C. I tried it on the Arduino IDE and I'm receiving the I2C data correctly.
However, in low-level C, when sending requests via SHTP, I notice that it ignores them. I saw that it's also possible to retrieve the data directly without making a request via the UART RVC.

Maybe I should try that?

Sorry for my bad English.

 

Hello, thank you for your reply.
However, I see the resistors integrated into the sensor for the I2C. I tried it on the Arduino IDE and I'm receiving the I2C data correctly.
However, in low-level C, when sending requests via SHTP, I notice that it ignores them. I saw that it's also possible to retrieve the data directly without making a request via the UART RVC.

Maybe I should try that?

Sorry for my bad English.

Where is your C code from?

 

Where is your C code from?

I took it from the SparkFun C++ libraries and tried to re-adapt it for low-level C.

https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library

 

Here is my C source for a SPI BNO086 driver. The timing is quirky on this IMU. I do a couple of chip resets and buffer writes to clear the command buffer before trying SH commands. I use interrupts to trigger received packet reads.
https://github.com/nsaspook/ll-test/blob/newboard/firmware/src/bno086.c
https://github.com/nsaspook/ll-test/blob/newboard/firmware/src/bno086.h

The project is here: https://github.com/nsaspook/ll-test/tree/newboard

Debug port boot file listing from the UART test-points on the board.

 

Thank you very much for your patience and help, in the end I decided to recover the quaternions in uart rvc in c. for my project where I just have to recover the Z axis to locate an autonomous robot on a table, it is enough and it worked directly. it avoids having to make shtp requests and configure the sensor because it is asynchronous

 

Thank you very much for your patience and help, in the end I decided to recover the quaternions in uart rvc in c. for my project where I just have to recover the Z axis to locate an autonomous robot on a table, it is enough and it worked directly. it avoids having to make shtp requests and configure the sensor because it is asynchronous

Great, the uart mode is much simpler. The SPI interface is, IMO much superior to I2C for these types of devices, faster, cleaner and easier to debug. The BNO series is good for systems that need complex movement and action interactions. It's overkill for just directional vectors. It took a bit of digging to get a decent C framework running on my hand held device project but you only do it once, I loved the challenge and I have several IMU alternatives for simple things.

https://forum.allaboutcircuits.com/...tter-for-this-application.193257/post-1823455
https://github.com/nsaspook/ll-test/blob/newboard/ll-tester_schem.pdf