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