Skip to content

Commit 371d5cd

Browse files
lewispg228lewispg228
committed
Started example - not done
-currently not working -seems to be stalling out at calibrateALL(), more speicifcally at the call to the sh2_startCal() function. Not sure what's going on yet.
1 parent d2a757f commit 371d5cd

File tree

3 files changed

+332
-27
lines changed

3 files changed

+332
-27
lines changed

‎examples/Example_19_Calibrate/Example_19_Calibrate.ino

Lines changed: 263 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,263 @@
1+
/*
2+
Using the BNO08x IMU
3+
4+
This example shows how to calibrate the sensor. See document 1000-4044.
5+
You move the sensor into a sequence of positions, then send it an "S" to save.
6+
7+
While Calibration is in progress, it will output the x/y/z/accuracy of the mag
8+
and the i/j/k/real parts of the game rotation vector.
9+
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
10+
11+
By: Nathan Seidle
12+
SparkFun Electronics
13+
Date: December 21st, 2017
14+
SparkFun code, firmware, and software is released under the MIT License.
15+
Please see LICENSE.md for further details.
16+
17+
Originally written by Nathan Seidle @ SparkFun Electronics, December 28th, 2017
18+
19+
Adjusted by Pete Lewis @ SparkFun Electronics, June 2023 to incorporate the
20+
CEVA Sensor Hub Driver, found here:
21+
https://github.com/ceva-dsp/sh2
22+
23+
Also, utilizing code from the Adafruit BNO08x Arduino Library by Bryan Siepert
24+
for Adafruit Industries. Found here:
25+
https://github.com/adafruit/Adafruit_BNO08x
26+
27+
Also, utilizing I2C and SPI read/write functions and code from the Adafruit
28+
BusIO library found here:
29+
https://github.com/adafruit/Adafruit_BusIO
30+
31+
Hardware Connections:
32+
IoT RedBoard --> BNO08x
33+
QWIIC --> QWIIC
34+
A4 --> INT
35+
A5 --> RST
36+
37+
BNO08x "mode" jumpers set for I2C (default):
38+
PSO: OPEN
39+
PS1: OPEN
40+
41+
Serial.print it out at 115200 baud to serial monitor.
42+
43+
Feel like supporting our work? Buy a board from SparkFun!
44+
https://www.sparkfun.com/products/22857
45+
*/
46+
47+
#include <Wire.h>
48+
49+
#include "SparkFun_BNO08x_Arduino_Library.h" // CTRL+Click here to get the library: http://librarymanager/All#SparkFun_BNO08x
50+
BNO08x myIMU;
51+
52+
// For the most reliable interaction with the SHTP bus, we need
53+
// to use hardware reset control, and to monitor the H_INT pin.
54+
// The H_INT pin will go low when its okay to talk on the SHTP bus.
55+
// Note, these can be other GPIO if you like.
56+
// Define as -1 to disable these features.
57+
#define BNO08X_INT A4
58+
//#define BNO08X_INT -1
59+
#define BNO08X_RST A5
60+
//#define BNO08X_RST -1
61+
62+
#define BNO08X_ADDR 0x4B // SparkFun BNO08x Breakout (Qwiic) defaults to 0x4B
63+
//#define BNO08X_ADDR 0x4A // Alternate address if ADR jumper is closed
64+
65+
// variables to store all our incoming values
66+
67+
// mags
68+
float mx;
69+
float my;
70+
float mz;
71+
byte magAccuracy;
72+
73+
// quats
74+
float quatI;
75+
float quatJ;
76+
float quatK;
77+
float quatReal;
78+
79+
unsigned long previousDebugMicros = 0;
80+
#define DEBUG_INTERVAL_MICROSECONDS 10000
81+
82+
void setup() {
83+
Serial.begin(115200);
84+
85+
while(!Serial) delay(10); // Wait for Serial to become available.
86+
// Necessary for boards with native USB (like the SAMD51 Thing+).
87+
// For a final version of a project that does not need serial debug (or a USB cable plugged in),
88+
// Comment out this while loop, or it will prevent the remaining code from running.
89+
90+
Serial.println();
91+
Serial.println("BNO08x Calibration Example");
92+
93+
Wire.begin();
94+
95+
//if (myIMU.begin() == false) { // Setup without INT/RST control (Not Recommended)
96+
if (myIMU.begin(BNO08X_ADDR, Wire, BNO08X_INT, BNO08X_RST) == false) {
97+
Serial.println("BNO08x not detected at default I2C address. Check your jumpers and the hookup guide. Freezing...");
98+
while (1)
99+
;
100+
}
101+
Serial.println("BNO08x found!");
102+
103+
// Wire.setClock(400000); //Increase I2C data rate to 400kHz
104+
105+
//Enable dynamic calibration for accel, gyro, and mag
106+
if (myIMU.calibrateAll() == true) {
107+
Serial.println(F("Calibration Command Sent Successfully"));
108+
} else {
109+
Serial.println("Could not send Calibration Command. Freezing...");
110+
while(1) delay(10);
111+
}
112+
113+
setReports();
114+
115+
Serial.println("Reading events");
116+
delay(100);
117+
}
118+
119+
// Here is where you define the sensor outputs you want to receive
120+
void setReports(void) {
121+
Serial.println("Setting desired reports");
122+
123+
if (myIMU.enableMagnetometer(1) == true) {
124+
Serial.println(F("Magnetometer enabled"));
125+
Serial.println(F("Output in form x, y, z, in uTesla"));
126+
} else {
127+
Serial.println("Could not enable magnetometer");
128+
}
129+
130+
if (myIMU.enableGameRotationVector(1) == true) {
131+
Serial.println(F("Game Rotation vector enabled"));
132+
Serial.println(F("Output in form i, j, k, real"));
133+
} else {
134+
Serial.println("Could not enable game rotation vector");
135+
}
136+
}
137+
138+
void loop() {
139+
delayMicroseconds(10);
140+
141+
if (myIMU.wasReset()) {
142+
Serial.print("sensor was reset ");
143+
setReports();
144+
}
145+
146+
// Has a new event come in on the Sensor Hub Bus?
147+
if (myIMU.getSensorEvent() == true) {
148+
// is the event a report of the magnetometer?
149+
if (myIMU.getSensorEventID() == SENSOR_REPORTID_MAGNETIC_FIELD) {
150+
mx = myIMU.getMagX();
151+
my = myIMU.getMagY();
152+
mz = myIMU.getMagZ();
153+
magAccuracy = myIMU.getMagAccuracy();
154+
}
155+
// is the event a report of the game rotation vector?
156+
else if (myIMU.getSensorEventID() == SENSOR_REPORTID_GAME_ROTATION_VECTOR) {
157+
quatI = myIMU.getGameQuatI();
158+
quatJ = myIMU.getGameQuatJ();
159+
quatK = myIMU.getGameQuatK();
160+
quatReal = myIMU.getGameQuatReal();
161+
}
162+
}
163+
164+
// Only print data to the terminal at a user defined interval
165+
// Each data type (accel or gyro or mag) is reported from the
166+
// BNO086 as separate messages.
167+
// To allow for all these separate messages to arrive, and thus
168+
// have updated data on all axis/types,
169+
// The report intervals for each datatype must be much faster
170+
// than our debug interval.
171+
172+
// time since last debug data printed to terminal
173+
unsigned long microsSinceLastSerialPrint = (micros() - previousDebugMicros);
174+
175+
// Only print data to the terminal at a user deficed interval
176+
if(microsSinceLastSerialPrint > DEBUG_INTERVAL_MICROSECONDS)
177+
{
178+
Serial.print(mx, 2);
179+
Serial.print("\t\t");
180+
Serial.print(my, 2);
181+
Serial.print("\t\t");
182+
Serial.print(mz, 2);
183+
Serial.print("\t\t");
184+
printAccuracyLevel(magAccuracy);
185+
Serial.print("\t\t");
186+
187+
Serial.print(quatI, 2);
188+
Serial.print("\t\t");
189+
Serial.print(quatJ, 2);
190+
Serial.print("\t\t");
191+
Serial.print(quatK, 2);
192+
Serial.print("\t\t");
193+
Serial.print(quatReal, 2);
194+
Serial.print("\t\t");
195+
196+
Serial.print(microsSinceLastSerialPrint);
197+
Serial.println();
198+
previousDebugMicros = micros();
199+
}
200+
201+
if(Serial.available())
202+
{
203+
byte incoming = Serial.read();
204+
205+
if(incoming == 's')
206+
{
207+
//Saves the current dynamic calibration data (DCD) to memory
208+
if (myIMU.saveCalibration() == true) {
209+
Serial.println(F("Calibration data was saved successfully"));
210+
} else {
211+
Serial.println("Save Calibration Failure");
212+
}
213+
214+
////////////////////////////////////
215+
// CODE FROM PREVIOUS BNO080 LIBRARY vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
216+
// BEGIN ///////////////////////////
217+
218+
// myIMU.requestCalibrationStatus(); //Sends command to get the latest calibration status
219+
220+
// //Wait for calibration response, timeout if no response
221+
// int counter = 100;
222+
// while(1)
223+
// {
224+
// if(--counter == 0) break;
225+
// if(myIMU.dataAvailable() == true)
226+
// {
227+
// //The IMU can report many different things. We must wait
228+
// //for the ME Calibration Response Status byte to go to zero
229+
// if(myIMU.calibrationComplete() == true)
230+
// {
231+
// Serial.println("Calibration data successfully stored");
232+
// delay(1000);
233+
// break;
234+
// }
235+
// }
236+
237+
// delay(1);
238+
// }
239+
// if(counter == 0)
240+
// {
241+
// Serial.println("Calibration data failed to store. Please try again.");
242+
// }
243+
244+
//myIMU.endCalibration(); //Turns off all calibration
245+
//In general, calibration should be left on at all times. The BNO080
246+
//auto-calibrates and auto-records cal data roughly every 5 minutes
247+
248+
////////////////////////////////////
249+
// CODE FROM PREVIOUS BNO080 LIBRARY ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
250+
// END /////////////////////////////
251+
252+
}
253+
}
254+
}
255+
256+
//Given a accuracy number, print what it means
257+
void printAccuracyLevel(byte accuracyNumber)
258+
{
259+
if(accuracyNumber == 0) Serial.print(F("Unreliable"));
260+
else if(accuracyNumber == 1) Serial.print(F("Low"));
261+
else if(accuracyNumber == 2) Serial.print(F("Medium"));
262+
else if(accuracyNumber == 3) Serial.print(F("High"));
263+
}

‎src/SparkFun_BNO08x_Arduino_Library.cpp

Lines changed: 62 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -328,6 +328,30 @@ uint8_t BNO08x::getQuatAccuracy()
328328
return _sensor_value->status;
329329
}
330330

331+
//Return the game rotation vector quaternion I
332+
float BNO08x::getGameQuatI()
333+
{
334+
return _sensor_value->un.gameRotationVector.i;
335+
}
336+
337+
//Return the game rotation vector quaternion J
338+
float BNO08x::getGameQuatJ()
339+
{
340+
return _sensor_value->un.gameRotationVector.j;
341+
}
342+
343+
//Return the game rotation vector quaternion K
344+
float BNO08x::getGameQuatK()
345+
{
346+
return _sensor_value->un.gameRotationVector.k;
347+
}
348+
349+
//Return the game rotation vector quaternion Real
350+
float BNO08x::getGameQuatReal()
351+
{
352+
return _sensor_value->un.gameRotationVector.real;
353+
}
354+
331355
//Gets the full acceleration
332356
//x,y,z output floats
333357
void BNO08x::getAccel(float &x, float &y, float &z, uint8_t &accuracy)
@@ -949,11 +973,34 @@ bool BNO08x::enableActivityClassifier(uint16_t timeBetweenReports, uint32_t acti
949973
// sendCalibrateCommand(CALIBRATE_PLANAR_ACCEL);
950974
// }
951975

952-
// //See 2.2 of the Calibration Procedure document 1000-4044
953-
// void BNO08x::calibrateAll()
954-
// {
955-
// sendCalibrateCommand(CALIBRATE_ACCEL_GYRO_MAG);
956-
// }
976+
//See 2.2 of the Calibration Procedure document 1000-4044
977+
bool BNO08x::calibrateAll()
978+
{
979+
int status;
980+
981+
Serial.print("Attempting sh2_setCalConfig...");
982+
//int status = sh2_setCalConfig(SH2_CAL_ACCEL);
983+
status = sh2_setCalConfig(SH2_CAL_ACCEL || SH2_CAL_GYRO || SH2_CAL_MAG);
984+
985+
if (status != SH2_OK) {
986+
return false;
987+
}
988+
//delay(100); // may not be necessary
989+
990+
Serial.println("complete.");
991+
992+
Serial.print("Attempting sh2_startCal...");
993+
994+
status = sh2_startCal(1000); // argument is report interval in uS
995+
996+
Serial.println("complete.");
997+
998+
if (status != SH2_OK) {
999+
return false;
1000+
}
1001+
1002+
return true;
1003+
}
9571004

9581005
// void BNO08x::endCalibration()
9591006
// {
@@ -1068,26 +1115,16 @@ bool BNO08x::clearTare()
10681115
// sendCommand(COMMAND_ME_CALIBRATE);
10691116
// }
10701117

1071-
// //This tells the BNO08x to save the Dynamic Calibration Data (DCD) to flash
1072-
// //See page 49 of reference manual and the 1000-4044 calibration doc
1073-
// void BNO08x::saveCalibration()
1074-
// {
1075-
// /*shtpData[3] = 0; //P0 - Reserved
1076-
// shtpData[4] = 0; //P1 - Reserved
1077-
// shtpData[5] = 0; //P2 - Reserved
1078-
// shtpData[6] = 0; //P3 - Reserved
1079-
// shtpData[7] = 0; //P4 - Reserved
1080-
// shtpData[8] = 0; //P5 - Reserved
1081-
// shtpData[9] = 0; //P6 - Reserved
1082-
// shtpData[10] = 0; //P7 - Reserved
1083-
// shtpData[11] = 0; //P8 - Reserved*/
1084-
1085-
// for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array
1086-
// shtpData[x] = 0;
1087-
1088-
// //Using this shtpData packet, send a command
1089-
// sendCommand(COMMAND_DCD); //Save DCD command
1090-
// }
1118+
//This tells the BNO08x to save the Dynamic Calibration Data (DCD) to flash
1119+
//See page 49 of reference manual and the 1000-4044 calibration doc
1120+
bool BNO08x::saveCalibration()
1121+
{
1122+
int status = sh2_saveDcdNow();
1123+
if (status != SH2_OK) {
1124+
return false;
1125+
}
1126+
return true;
1127+
}
10911128

10921129
/*! @brief Initializer for post i2c/spi init
10931130
* @param sensor_id Optional unique ID for the sensor set

‎src/SparkFun_BNO08x_Arduino_Library.h

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -198,6 +198,11 @@ class BNO08x
198198
float getQuatRadianAccuracy();
199199
uint8_t getQuatAccuracy();
200200

201+
float getGameQuatI();
202+
float getGameQuatJ();
203+
float getGameQuatK();
204+
float getGameQuatReal();
205+
201206
void getAccel(float &x, float &y, float &z, uint8_t &accuracy);
202207
float getAccelX();
203208
float getAccelY();
@@ -249,9 +254,9 @@ class BNO08x
249254
// void calibrateGyro();
250255
// void calibrateMagnetometer();
251256
// void calibratePlanarAccelerometer();
252-
// void calibrateAll();
257+
bool calibrateAll();
253258
// void endCalibration();
254-
// void saveCalibration();
259+
bool saveCalibration();
255260
// void requestCalibrationStatus(); //Sends command to get status
256261
// boolean calibrationComplete(); //Checks ME Cal response for byte 5, R0 - Status
257262

0 commit comments

Comments
 (0)