STM32 I2C Library for BNO080 triaxial accelerometer gyroscope magnetometer

STM32 I2C Library for BNO080 triaxial accelerometer gyroscope magnetometer

Please read Liability Disclaimer and License Agreement CAREFULLY

Before going further please read BNO080 datasheet

BNO080.h

 

#ifndef __BNO_080_I2C_H
#define __BNO_080_I2C_H

#ifdef __cplusplus
extern "C" {
#endif

#include "i2c.h"
#define RESET_DELAY                                                10

#define Byte1                                                    1
#define Byte2                                                    2
#define Byte3                                                    3
#define Byte4                                                    4
#define BNO_W_ADDR                                                0x96
#define BNO_R_ADDR                                                0x97

// Report IDs on the Executable channel
// See Figure 1-27 in the BNO080 datasheet
#define EXECUTABLE_REPORTID_RESET                                0x1
#define STORED_PACKET_SIZE                                        128 
#define SHORT_PACKET_SIZE                                        (STORED_PACKET_SIZE - 4)
//Registers
#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

//All the ways we can configure or talk to the BNO080, figure 34, page 36 reference manual
//These are used for low level communication with the sensor, on channel 2
#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_VECTO            0x28
#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR    0x29

//Record IDs from figure 29, page 29 reference manual
//These are used to read the metadata for each sensor type
#define FRS_RECORDID_ACCELEROMETER                                0xE302
#define FRS_RECORDID_GYROSCOPE_CALIBRATED                        0xE306
#define FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED                    0xE309
#define FRS_RECORDID_ROTATION_VECTOR                            0xE30B

//Command IDs from section 6.4, page 42
//These are used to calibrate, initialize, set orientation, tare etc the sensor
#define COMMAND_ERRORS                                            1
#define COMMAND_COUNTER                                            2
#define COMMAND_TARE                                            3
#define COMMAND_INITIALIZE                                        4
#define COMMAND_DCD                                                6
#define COMMAND_ME_CALIBRATE                                    7
#define COMMAND_DCD_PERIOD_SAVE                                    9
#define COMMAND_OSCILLATOR                                        10
#define COMMAND_CLEAR_DCD                                        11

#define CALIBRATE_ACCEL                                            0
#define CALIBRATE_GYRO                                            1
#define CALIBRATE_MAG                                            2
#define CALIBRATE_PLANAR_ACCEL                                    3
#define CALIBRATE_ACCEL_GYRO_MAG                                4
#define CALIBRATE_STOP                                            5

    extern uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy;
    extern uint32_t BMO_timeStamp;
    uint8_t BNO_Init(void);
    uint8_t BNO_dataAvailable(void);
    void BNO_requestCalibrationStatus(void);
    void BNO_setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig);
    uint8_t BNO_isCalibrationComplete(void);
    void BNO0_saveCalibration(void);
    uint8_t BNO_calibrate(void);
#ifdef __cplusplus
}
#endif

#endif /* __BNO_080_I2C_H */

 

BNO080.c

 

#include <math.h>
#include <string.h>
#include <stdio.h>
#include "BNO_080_I2C.h"
#include "i2c.h"
#include "gpio.h"
#include "stm32f7xx_it.h"
#define BNO_DEBUG 0




//Global Variables
static uint8_t ret;
//Each packet has a header of 4 bytes
//static uint8_t shtpHeader[Byte4]; 
static uint8_t shtpData[STORED_PACKET_SIZE];
//There are 6 com channels. Each channel has its own seqnum
static uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0}; 
//Commands have a seqNum as well. These are inside command packet, the header uses its own seqNum per channel
static uint8_t commandSequenceNumber = 0;        
static uint16_t packetLength = 0;
//Byte R0 of ME Calibration Response
static uint8_t calibrationStatus;    
uint32_t BMO_timeStamp;

//These are the raw sensor values (without Q applied) pulled from the user requested Input Report
static uint16_t rawAccelX, rawAccelY, rawAccelZ, accelAccuracy;
static uint16_t rawLinAccelX, rawLinAccelY, rawLinAccelZ, accelLinAccuracy;
static uint16_t rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy;
static uint16_t rawMagX, rawMagY, rawMagZ, magAccuracy;
uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy;
static uint16_t rawFastGyroX, rawFastGyroY, rawFastGyroZ;
static uint16_t stepCount;
static uint8_t stabilityClassifier;
static uint8_t activityClassifier;
static uint8_t *_activityConfidences;                          //Array that store the confidences of the 9 possible activities
static uint16_t memsRawAccelX, memsRawAccelY, memsRawAccelZ; //Raw readings from MEMS sensor
static uint16_t memsRawGyroX, memsRawGyroY, memsRawGyroZ;    //Raw readings from MEMS sensor
static uint16_t memsRawMagX, memsRawMagY, memsRawMagZ;          //Raw readings from MEMS sensor

//These Q values are defined in the datasheet
//but can also be obtained by querying the meta data records
//See the read metadata example for more info
static const int16_t magnetometer_Q1         = 4;
static const int16_t rotationVector_Q1    = 14;
//Given a register value and a Q point, convert to float
//See https://en.wikipedia.org/wiki/Q_(number_format)
static double qToFloat(int16_t fixedPointValue, uint8_t qPoint){
    double qFloat = fixedPointValue;
    qFloat *= pow(2, qPoint * -1);
    return (qFloat);
}

//This function pulls the data from the command response report
//Unit responds with packet that contains the following:
//shtpData[0:3]: First, a 4 byte header
//shtpData[4]: The Report ID
//shtpData[5]: Sequence number (See 6.5.18.2)
//shtpData[6]: Command
//shtpData[7]: Command Sequence Number
//shtpData[8]: Response Sequence Number
//shtpData[9 + 0]: R0
//shtpData[9 + 1]: R1
//shtpData[9 + 2]: R2
//shtpData[9 + 3]: R3
//shtpData[9 + 4]: R4
//shtpData[9 + 5]: R5
//shtpData[9 + 6]: R6
//shtpData[9 + 7]: R7
//shtpData[9 + 8]: R8
static void parseCommandReport(void){
    if (shtpData[4] == SHTP_REPORT_COMMAND_RESPONSE){
        //The BNO080 responds with this report to command requests. It's up to use to remember which command we issued.
        uint8_t command = shtpData[6]; //This is the Command byte of the response
        if (command == COMMAND_ME_CALIBRATE){
            calibrationStatus = shtpData[9 + 0]; //R0 - Status (0 = success, non-zero = fail)
        }
    }
    //else    {
        //This sensor report ID is unhandled.
        //See reference manual to add additional feature reports as needed
    //}
    //TODO additional feature reports may be strung together. Parse them all.
}

//This function pulls the data from the input report
//The input reports vary in length so this function stores the various 16-bit values as globals

//Unit responds with packet that contains the following:
//shtpData[0:3]: First, a 4 byte header
//shtpData[4:8]: Then a 5 byte timestamp of microsecond clicks since reading was taken
//shtpData[9 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector)
//shtpData[9 + 1]: Sequence number (See 6.5.18.2)
//shtpData[9 + 2]: Status
//shtpData[7]: Delay
//shtpData[4:5]: i/accel x/gyro x/etc
//shtpData[6:7]: j/accel y/gyro y/etc
//shtpData[8:9]: k/accel z/gyro z/etc
//shtpData[10:11]: real/gyro temp/etc
//shtpData[12:13]: Accuracy estimate
static void parseInputReport(void) {
    //Calculate the number of data bytes in this packet
    int16_t dataLength = ((shtpData[1] << 8) + (shtpData[0])) & (~0x8000);
    //Ignore it for now. TODO catch this as an error and exit
    BMO_timeStamp = ((uint32_t)shtpData[8] << (8 * 3)) | ((uint32_t)shtpData[7] << (8 * 2)) | ((uint32_t)shtpData[6] << (8 * 1)) | ((uint32_t)shtpData[5] << (8 * 0));
        // The gyro-integrated input reports are sent via the special gyro channel and do no include the usual ID, sequence, and status fields
    if(shtpData[2] == CHANNEL_GYRO) {
        rawQuatI = (uint16_t)shtpData[5] << 8 | shtpData[4];
        rawQuatJ = (uint16_t)shtpData[7] << 8 | shtpData[6];
        rawQuatK = (uint16_t)shtpData[9] << 8 | shtpData[8];
        rawQuatReal = (uint16_t)shtpData[11] << 8 | shtpData[10];
        rawFastGyroX = (uint16_t)shtpData[13] << 8 | shtpData[12];
        rawFastGyroY = (uint16_t)shtpData[15] << 8 | shtpData[14];
        rawFastGyroZ = (uint16_t)shtpData[17] << 8 | shtpData[16];
        return;
    }
    uint8_t status = shtpData[9 + 2] & 0x03; //Get status bits
    uint16_t data1 = (uint16_t)shtpData[9 + 5] << 8 | shtpData[9 + 4];
    uint16_t data2 = (uint16_t)shtpData[9 + 7] << 8 | shtpData[9 + 6];
    uint16_t data3 = (uint16_t)shtpData[9 + 9] << 8 | shtpData[9 + 8];
    uint16_t data4 = 0;
    uint16_t data5 = 0; //We would need to change this to uin32_t to capture time stamp value on Raw Accel/Gyro/Mag reports
    if (dataLength - 5 > 9) {
        data4 = (uint16_t)shtpData[9 + 11] << 8 | shtpData[9 + 10];
    }
    if (dataLength - 5 > 11) {
        data5 = (uint16_t)shtpData[9 + 13] << 8 | shtpData[9 + 12];
    }
    //Store these generic values to their proper global variable
    if (shtpData[9] == SENSOR_REPORTID_ACCELEROMETER){
        accelAccuracy = status;
        rawAccelX = data1;
        rawAccelY = data2;
        rawAccelZ = data3;
    }    else if (shtpData[9] == SENSOR_REPORTID_LINEAR_ACCELERATION){
        accelLinAccuracy = status;
        rawLinAccelX = data1;
        rawLinAccelY = data2;
        rawLinAccelZ = data3;
    }    else if (shtpData[9] == SENSOR_REPORTID_GYROSCOPE)    {
        gyroAccuracy = status;
        rawGyroX = data1;
        rawGyroY = data2;
        rawGyroZ = data3;
    }    else if (shtpData[9] == SENSOR_REPORTID_MAGNETIC_FIELD)    {
        magAccuracy = status;
        rawMagX = data1;
        rawMagY = data2;
        rawMagZ = data3;
    }    else if (shtpData[9] == SENSOR_REPORTID_ROTATION_VECTOR ||
        shtpData[9] == SENSOR_REPORTID_GAME_ROTATION_VECTOR ||
        shtpData[9] == SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR ||
        shtpData[9] == SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR)    {
        quatAccuracy = status;
        rawQuatI = data1;
        rawQuatJ = data2;
        rawQuatK = data3;
        rawQuatReal = data4;
        //Only available on rotation vector and ar/vr stabilized rotation vector,
        // not game rot vector and not ar/vr stabilized rotation vector
        rawQuatRadianAccuracy = data5;
    }    else if (shtpData[9] == SENSOR_REPORTID_STEP_COUNTER)    {
        stepCount = data3; //Bytes 8/9
    }    else if (shtpData[9] == SENSOR_REPORTID_STABILITY_CLASSIFIER)    {
        stabilityClassifier = shtpData[9 + 4]; //Byte 4 only
    }    else if (shtpData[9] == SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER)    {
        activityClassifier = shtpData[9 + 5]; //Most likely state
        //Load activity classification confidences into the array
        for (uint8_t x = 0; x < 9; x++)                       //Hardcoded to max of 9. TODO - bring in array size
            _activityConfidences[x] = shtpData[9 + 6 + x]; //5 bytes of timestamp, byte 6 is first confidence byte
    }    else if (shtpData[9] == SENSOR_REPORTID_RAW_ACCELEROMETER)    {
        memsRawAccelX = data1;
        memsRawAccelY = data2;
        memsRawAccelZ = data3;
    }    else if (shtpData[9] == SENSOR_REPORTID_RAW_GYROSCOPE)    {
        memsRawGyroX = data1;
        memsRawGyroY = data2;
        memsRawGyroZ = data3;
    }    else if (shtpData[9] == SENSOR_REPORTID_RAW_MAGNETOMETER)    {
        memsRawMagX = data1;
        memsRawMagY = data2;
        memsRawMagZ = data3;
    }    else if (shtpData[9] == SHTP_REPORT_COMMAND_RESPONSE)    {
        printf("!\n");
        //The BNO080 responds with this report to command requests. It's up to use to remember which command we issued.
        uint8_t command = shtpData[9 + 2]; //This is the Command byte of the response
        if (command == COMMAND_ME_CALIBRATE)        {
            printf("ME Cal report found!\n");
            calibrationStatus = shtpData[9 + 5]; //R0 - Status (0 = success, non-zero = fail)
        }
    }    else    {
        //This sensor report ID is unhandled.
        //See reference manual to add additional feature reports as needed
    }
    //TODO additional feature reports may be strung together. Parse them all.
}
//Pretty prints the contents of the current shtp header and data packets
static void printPacket(){
#if BNO_DEBUG
    //Print the four byte header
    uint16_t packLen = ((shtpData[1] << 8) + (shtpData[0])) & (~0x8000);
    printf("Header:");
    for (uint8_t x = 0 ; x < 4 ; x++){
        printf(" ");
        if (shtpData[x] < 0x10) printf("0");
        printf("%hhx", shtpData[x]);
    }
    uint8_t printLength = packLen - 4;
    //Artificial limit. We don't want the phone book.
    if (printLength > 40) printLength = 40; 
    printf(" Body:");
    for (uint8_t x = 4 ; x < printLength ; x++) {
        printf(" ");
        if (shtpData[x] < 0x10) printf("0");
        printf("%hhx", shtpData[x]);
    }
    printf(", Length:");
    printf("%d", packLen);
    if(packLen & 1 << 15) {
        printf(" [Continued packet] ");
    }
    printf(", SeqNum: %hhu", shtpData[3]);
    printf(", Channel:");
    if (shtpData[2] == 0) printf("Command");
    else if (shtpData[2] == 1) printf("Executable");
    else if (shtpData[2] == 2) printf("Control");
    else if (shtpData[2] == 3) printf("Sensor-report");
    else if (shtpData[2] == 4) printf("Wake-report");
    else if (shtpData[2] == 5) printf("Gyro-vector");
    else printf("%hhu", shtpData[2]);
    printf("\n");
#endif
}

static uint8_t sendPacket(uint8_t channelNumber, uint8_t dataLength){
    //Add four bytes for the header
    uint8_t len = dataLength + 4;
    shtpData[0] =  (len & 0xFF);
    shtpData[1] =  (len >> 8) & 0xFF;
    shtpData[2] =  channelNumber;
    shtpData[3] =  sequenceNumber[channelNumber]++;
    #if BNO_DEBUG
        printf("Data to send= %d\n",packetLength);
        for (uint8_t i = 0; i < len; i++){
                printf("%hhx", shtpData[i]);
                printf(" ");
        }    
        printf("\r\n\r\n");
    #endif
    ret = HAL_I2C_Master_Transmit(&hi2c1, BNO_W_ADDR, shtpData, len, PORT_TIMEOUT);
    return ret;
}

//Check to see if there is any new data available
//Read the contents of the incoming packet into the shtpData array
static uint8_t receivePacket(void){
    uint32_t timer = HAL_GetTick() + PORT_TIMEOUT;
    //wait for the interrupt signal
    while(!BNO_Ready){
        if(HAL_GetTick() > timer) return 1;
    }
    //Ask for four bytes to find out how much data we need to read
    ret = HAL_I2C_Master_Receive(&hi2c1, BNO_R_ADDR, shtpData, Byte4, PORT_TIMEOUT);
    if(ret) {
        #if BNO_DEBUG
        printf("BNO I2C read header failed!\n");
        #endif
        return 2;
    }    
    if(shtpData[0] == 0xFF && shtpData[1] == 0xFF) {
        // invalid according to BNO080 datasheet section 1.4.1
        #if BNO_DEBUG
            printf("Recieved 0xFFFF packet length, protocol error!\n");
        #endif
        return 3;
    }
    //Calculate the number of data bytes in this packet
    packetLength = ((shtpData[1] << 8) + (shtpData[0])) & (~0x8000);
    if (packetLength == 0){
        //Packet is empty
        return 0; //All done
    }
    //Remove the header bytes from the data count
    packetLength -= Byte4; 
    //Read incoming data into the shtpData array
//    printf("getData(dataLength) = %d!\n", packetLength);
    //Read incoming data into the shtpData array
//    ret = HAL_I2C_Master_Receive(&hi2c1, BNO_R_ADDR, shtpData, packetLength, PORT_TIMEOUT);
    //Setup a series of chunked 32 byte reads
    while (packetLength > 0){
        uint8_t numberOfBytesToRead = packetLength;
        if (packetLength > SHORT_PACKET_SIZE)
            numberOfBytesToRead = SHORT_PACKET_SIZE;
        ret = HAL_I2C_Master_Receive(&hi2c1, BNO_R_ADDR, shtpData, (numberOfBytesToRead + 4), PORT_TIMEOUT);
        #if BNO_DEBUG
            printf("Recieved packet: --------%d--------\n", ret);
            printPacket(); // note: add 4 for the header length
        #endif
        packetLength -= numberOfBytesToRead;
    }    
    return ret; //We're done!
}

//Tell the sensor to do a command
//See 6.3.8 page 41, Command request
//The caller is expected to set P0 through P8 prior to calling
static void sendCommand(uint8_t command){
    shtpData[4] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
    shtpData[5] = commandSequenceNumber++; //Increments automatically each function call
    shtpData[6] = command; //Command
    //Caller must set these
    /*shtpData[7] = 0; //P0
    shtpData[8] = 0; //P1
    shtpData[9] = 0; //P2
    shtpData[10] = 0;
    shtpData[11] = 0;
    shtpData[12] = 0;
    shtpData[13] = 0;
    shtpData[14] = 0;
    shtpData[15] = 0;*/
    //Transmit packet on channel 2, 12 bytes
    sendPacket(CHANNEL_CONTROL, 12);
}

//Given a sensor's report ID, this tells the BNO080 to begin reporting the values
//Also sets the specific config word. Useful for personal activity classifier
void BNO_setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig){
    long microsBetweenReports = (long)timeBetweenReports * 1000L;
    shtpData[4] = SHTP_REPORT_SET_FEATURE_COMMAND;            //Set feature command. Reference page 55
    shtpData[5] = reportID;                                                            //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
    shtpData[6] = 0;                                                                        //Feature flags
    shtpData[7] = 0;                                                                        //Change sensitivity (LSB)
    shtpData[8] = 0;                                                                        //Change sensitivity (MSB)
    shtpData[9] = (microsBetweenReports >> 0) & 0xFF;        //Report interval (LSB) in microseconds. 0x7A120 = 500ms
    shtpData[10] = (microsBetweenReports >> 8) & 0xFF;    //Report interval
    shtpData[11] = (microsBetweenReports >> 16) & 0xFF; //Report interval
    shtpData[12] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
    shtpData[12] = 0;                                                                        //Batch Interval (LSB)
    shtpData[14] = 0;                                                                        //Batch Interval
    shtpData[15] = 0;                                                                        //Batch Interval
    shtpData[16] = 0;                                                                        //Batch Interval (MSB)
    shtpData[17] = (specificConfig >> 0) & 0xFF;                //Sensor-specific config (LSB)
    shtpData[18] = (specificConfig >> 8) & 0xFF;                //Sensor-specific config
    shtpData[19] = (specificConfig >> 16) & 0xFF;                //Sensor-specific config
    shtpData[20] = (specificConfig >> 24) & 0xFF;                //Sensor-specific config (MSB)
    //Transmit packet on channel 2, 17 bytes
    sendPacket(CHANNEL_CONTROL, 17);
}
//Updates the latest variables if possible
//Returns false if new readings are not available
uint8_t BNO_dataAvailable(void){
    //If we have an interrupt pin connection available, check if data is available.
    //If int pin is not set, then we'll rely on receivePacket() to timeout
    if(!BNO_Ready)
        return 0;
    if (!receivePacket()){
        switch(shtpData[2]){
            case CHANNEL_COMMAND:
                printf("CHANNEL_COMMAND\n");
                break;
            case CHANNEL_EXECUTABLE:
                printf("CHANNEL_EXECUTABLE\n");
                break;
            case CHANNEL_CONTROL:
                //printf("CHANNEL_CONTROL\n");
                parseCommandReport();
                //printf("parseCommandReport()\n");
                break;
            case CHANNEL_REPORTS:
                //printf("CHANNEL_REPORTS\n");
                if(shtpData[4] == SHTP_REPORT_BASE_TIMESTAMP){
                    parseInputReport();
                    //printf("parseInputReport() 0 \n");
                }
                break;
            case CHANNEL_WAKE_REPORTS:
                printf("CHANNEL_WAKE_REPORTS\n");
                break;
            case CHANNEL_GYRO:
                printf("CHANNEL_GYRO\n");
                parseInputReport();
                printf("parseInputReport() 1\n");
                break;
        }
        return 1;
    }
    return 0;
}


//This tells the BNO080 to begin calibrating
//See page 50 of reference manual and the 1000-4044 calibration doc
static void sendCalibrateCommand(uint8_t thingToCalibrate){
    /*shtpData[7] = 0; //P0 - Accel Cal Enable
    shtpData[8] = 0; //P1 - Gyro Cal Enable
    shtpData[9] = 0; //P2 - Mag Cal Enable
    shtpData[10] = 0; //P3 - Subcommand 0x00
    shtpData[11] = 0; //P4 - Planar Accel Cal Enable
    shtpData[12] = 0; //P5 - Reserved
    shtpData[13] = 0; //P6 - Reserved
    shtpData[14] = 0; //P7 - Reserved
    shtpData[15] = 0; //P8 - Reserved*/
    for (uint8_t x = 7; x < 16; x++) //Clear this section of the shtpData array
        shtpData[x] = 0;

    if (thingToCalibrate == CALIBRATE_ACCEL)
        shtpData[7] = 1;
    else if (thingToCalibrate == CALIBRATE_GYRO)
        shtpData[8] = 1;
    else if (thingToCalibrate == CALIBRATE_MAG)
        shtpData[9] = 1;
    else if (thingToCalibrate == CALIBRATE_PLANAR_ACCEL)
        shtpData[11] = 1;
    else if (thingToCalibrate == CALIBRATE_ACCEL_GYRO_MAG)
    {
        shtpData[7] = 1;
        shtpData[8] = 1;
        shtpData[9] = 1;
    }
    else if (thingToCalibrate == CALIBRATE_STOP)
        ; //Do nothing, bytes are set to zero
    //Make the internal calStatus variable non-zero (operation failed) so that user can test while we wait
    calibrationStatus = 1;
    //Using this shtpData packet, send a command
    sendCommand(COMMAND_ME_CALIBRATE);
}

//Request ME Calibration Status from BNO080
//See page 51 of reference manual
void BNO_requestCalibrationStatus(void){
/*shtpData[7] = 0; //P0 - Reserved
    shtpData[8] = 0; //P1 - Reserved
    shtpData[9] = 0; //P2 - Reserved
    shtpData[10] = 0; //P3 - 0x01 - Subcommand: Get ME Calibration
    shtpData[11] = 0; //P4 - Reserved
    shtpData[12] = 0; //P5 - Reserved
    shtpData[13] = 0; //P6 - Reserved
    shtpData[14] = 0; //P7 - Reserved
    shtpData[15] = 0; //P8 - Reserved*/
    //Clear this section of the shtpData array
    for (uint8_t x = 7; x < 16; x++) 
        shtpData[x] = 0;
    shtpData[10] = 0x01; //P3 - 0x01 - Subcommand: Get ME Calibration
    //Using this shtpData packet, send a command
    sendCommand(COMMAND_ME_CALIBRATE);
}

//This tells the BNO080 to save the Dynamic Calibration Data (DCD) to flash
//See page 49 of reference manual and the 1000-4044 calibration doc
void BNO0_saveCalibration(void){
/*shtpData[7] = 0; //P0 - Reserved
    shtpData[8] = 0; //P1 - Reserved
    shtpData[9] = 0; //P2 - Reserved
    shtpData[10] = 0; //P3 - Reserved
    shtpData[11] = 0; //P4 - Reserved
    shtpData[12] = 0; //P5 - Reserved
    shtpData[13] = 0; //P6 - Reserved
    shtpData[14] = 0; //P7 - Reserved
    shtpData[15] = 0; //P8 - Reserved*/
    for (uint8_t x = 7; x < 16; x++) //Clear this section of the shtpData array
        shtpData[x] = 0;
    //Using this shtpData packet, send a command
    sendCommand(COMMAND_DCD); //Save DCD command
}

//See page 51 of reference manual - ME Calibration Response
//Byte 5 is parsed during the readPacket and stored in calibrationStatus
uint8_t BNO_isCalibrationComplete(void){
    return calibrationStatus;
}

uint8_t BNO_calibrate(void){
    sendCalibrateCommand(CALIBRATE_ACCEL_GYRO_MAG);
    BNO_setFeatureCommand(SENSOR_REPORTID_GAME_ROTATION_VECTOR, 20, 0);
    BNO_setFeatureCommand(SENSOR_REPORTID_MAGNETIC_FIELD, 20, 0);
    while(1){
        if(BNO_dataAvailable()){
            double x = qToFloat(rawMagX, magnetometer_Q1);
            double y = qToFloat(rawMagY, magnetometer_Q1);
            double z = qToFloat(rawMagZ, magnetometer_Q1);
            //magAccuracy
            double quatI = qToFloat(rawQuatI, rotationVector_Q1);
            double quatJ = qToFloat(rawQuatJ, rotationVector_Q1);
            double quatK = qToFloat(rawQuatK, rotationVector_Q1);
            double quatReal = qToFloat(rawQuatReal, rotationVector_Q1);
            //quatAccuracy
            printf("x=%.2lf y=%.2lf z=%.2lf A=%d || QI=%.2lf QJ=%.2lf QK=%.2lf QR=%.2lf QA=%d\r\n", x, y, z, magAccuracy, quatI, quatJ, quatK, quatReal, quatAccuracy);
        }
        if((magAccuracy == 3) && (quatAccuracy == 3))
            break;
    }
    BNO0_saveCalibration();
    HAL_Delay(PORT_TIMEOUT);
    if(!BNO_isCalibrationComplete()){
        printf("Calibration data successfully stored\r\n");
        return 1;
    } else {
        printf("Calibration data failed to store. Please try again.\r\n");
        return 0;
    }
}

uint8_t BNO_Init(void){
    //reset BNO
    BNO_RST_On;
    //wait
    HAL_Delay(RESET_DELAY);
    //enable the interrupt
    HAL_NVIC_EnableIRQ(EXTI2_IRQn);
    BNO_RST_Off;

    //wait for the interrupt signal
    while(!BNO_Ready){}
    shtpData[4] = 1; //Reset
    //Attempt to start communication with sensor
    sendPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET); //Transmit packet on channel 1, 1 byte
    //Read all incoming data and flush it
    HAL_Delay(PORT_TIMEOUT);    
    while(receivePacket()){}
    //Check communication with device
    shtpData[4] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
    shtpData[5] = 0;                              //Reserved
    //Transmit packet on channel 2, 2 bytes
    sendPacket(CHANNEL_CONTROL, 2);
    //Now we wait for response
    while(receivePacket()){}
    
    if (shtpData[4] == SHTP_REPORT_PRODUCT_ID_RESPONSE){
        HAL_Delay(PORT_TIMEOUT);
        return 1;
    } else {
        return 0;
    }    
    
}

 

Comments powered by CComment

Who’s online

We have 28 guests and no members online