adafruit_bno055 / examples / restore_offsets / restore_offsets.ino @ 1da09ad2
History | View | Annotate | Download (8.948 KB)
1 |
#include <Wire.h> |
---|---|
2 |
#include <Adafruit_Sensor.h> |
3 |
#include <Adafruit_BNO055.h> |
4 |
#include <utility/imumaths.h> |
5 |
#include <EEPROM.h> |
6 |
|
7 |
/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor), |
8 |
which provides a common 'type' for sensor data and some helper functions. |
9 |
|
10 |
To use this driver you will also need to download the Adafruit_Sensor |
11 |
library and include it in your libraries folder. |
12 |
|
13 |
You should also assign a unique ID to this sensor for use with |
14 |
the Adafruit Sensor API so that you can identify this particular |
15 |
sensor in any data logs, etc. To assign a unique ID, simply |
16 |
provide an appropriate value in the constructor below (12345 |
17 |
is used by default in this example). |
18 |
|
19 |
Connections |
20 |
=========== |
21 |
Connect SCL to analog 5 |
22 |
Connect SDA to analog 4 |
23 |
Connect VDD to 3-5V DC |
24 |
Connect GROUND to common ground |
25 |
|
26 |
History |
27 |
======= |
28 |
2015/MAR/03 - First release (KTOWN) |
29 |
2015/AUG/27 - Added calibration and system status helpers |
30 |
2015/NOV/13 - Added calibration save and restore |
31 |
*/ |
32 |
|
33 |
/* Set the delay between fresh samples */ |
34 |
#define BNO055_SAMPLERATE_DELAY_MS (100) |
35 |
|
36 |
Adafruit_BNO055 bno = Adafruit_BNO055(55); |
37 |
|
38 |
/**************************************************************************/ |
39 |
/* |
40 |
Displays some basic information on this sensor from the unified |
41 |
sensor API sensor_t type (see Adafruit_Sensor for more information) |
42 |
*/ |
43 |
/**************************************************************************/ |
44 |
void displaySensorDetails(void) |
45 |
{ |
46 |
sensor_t sensor; |
47 |
bno.getSensor(&sensor); |
48 |
Serial.println("------------------------------------"); |
49 |
Serial.print("Sensor: "); Serial.println(sensor.name); |
50 |
Serial.print("Driver Ver: "); Serial.println(sensor.version); |
51 |
Serial.print("Unique ID: "); Serial.println(sensor.sensor_id); |
52 |
Serial.print("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx"); |
53 |
Serial.print("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx"); |
54 |
Serial.print("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx"); |
55 |
Serial.println("------------------------------------"); |
56 |
Serial.println(""); |
57 |
delay(500); |
58 |
} |
59 |
|
60 |
/**************************************************************************/ |
61 |
/* |
62 |
Display some basic info about the sensor status |
63 |
*/ |
64 |
/**************************************************************************/ |
65 |
void displaySensorStatus(void) |
66 |
{ |
67 |
/* Get the system status values (mostly for debugging purposes) */ |
68 |
uint8_t system_status, self_test_results, system_error; |
69 |
system_status = self_test_results = system_error = 0; |
70 |
bno.getSystemStatus(&system_status, &self_test_results, &system_error); |
71 |
|
72 |
/* Display the results in the Serial Monitor */ |
73 |
Serial.println(""); |
74 |
Serial.print("System Status: 0x"); |
75 |
Serial.println(system_status, HEX); |
76 |
Serial.print("Self Test: 0x"); |
77 |
Serial.println(self_test_results, HEX); |
78 |
Serial.print("System Error: 0x"); |
79 |
Serial.println(system_error, HEX); |
80 |
Serial.println(""); |
81 |
delay(500); |
82 |
} |
83 |
|
84 |
/**************************************************************************/ |
85 |
/* |
86 |
Display sensor calibration status |
87 |
*/ |
88 |
/**************************************************************************/ |
89 |
void displayCalStatus(void) |
90 |
{ |
91 |
/* Get the four calibration values (0..3) */ |
92 |
/* Any sensor data reporting 0 should be ignored, */ |
93 |
/* 3 means 'fully calibrated" */ |
94 |
uint8_t system, gyro, accel, mag; |
95 |
system = gyro = accel = mag = 0; |
96 |
bno.getCalibration(&system, &gyro, &accel, &mag); |
97 |
|
98 |
/* The data should be ignored until the system calibration is > 0 */ |
99 |
Serial.print("\t"); |
100 |
if (!system) |
101 |
{ |
102 |
Serial.print("! "); |
103 |
} |
104 |
|
105 |
/* Display the individual values */ |
106 |
Serial.print("Sys:"); |
107 |
Serial.print(system, DEC); |
108 |
Serial.print(" G:"); |
109 |
Serial.print(gyro, DEC); |
110 |
Serial.print(" A:"); |
111 |
Serial.print(accel, DEC); |
112 |
Serial.print(" M:"); |
113 |
Serial.print(mag, DEC); |
114 |
} |
115 |
|
116 |
/**************************************************************************/ |
117 |
/* |
118 |
Display the raw calibration offset and radius data |
119 |
*/ |
120 |
/**************************************************************************/ |
121 |
void displaySensorOffsets(const adafruit_bno055_offsets_t &calibData) |
122 |
{ |
123 |
Serial.print("Accelerometer: "); |
124 |
Serial.print(calibData.accel_offset_x); Serial.print(" "); |
125 |
Serial.print(calibData.accel_offset_y); Serial.print(" "); |
126 |
Serial.print(calibData.accel_offset_z); Serial.print(" "); |
127 |
|
128 |
Serial.print("\nGyro: "); |
129 |
Serial.print(calibData.gyro_offset_x); Serial.print(" "); |
130 |
Serial.print(calibData.gyro_offset_y); Serial.print(" "); |
131 |
Serial.print(calibData.gyro_offset_z); Serial.print(" "); |
132 |
|
133 |
Serial.print("\nMag: "); |
134 |
Serial.print(calibData.mag_offset_x); Serial.print(" "); |
135 |
Serial.print(calibData.mag_offset_y); Serial.print(" "); |
136 |
Serial.print(calibData.mag_offset_z); Serial.print(" "); |
137 |
|
138 |
Serial.print("\nAccel Radius: "); |
139 |
Serial.print(calibData.accel_radius); |
140 |
|
141 |
Serial.print("\nMag Radius: "); |
142 |
Serial.print(calibData.mag_radius); |
143 |
} |
144 |
|
145 |
|
146 |
/**************************************************************************/ |
147 |
/* |
148 |
Arduino setup function (automatically called at startup) |
149 |
*/ |
150 |
/**************************************************************************/ |
151 |
void setup(void) |
152 |
{ |
153 |
Serial.begin(115200); |
154 |
delay(1000); |
155 |
Serial.println("Orientation Sensor Test"); Serial.println(""); |
156 |
|
157 |
/* Initialise the sensor */ |
158 |
if (!bno.begin()) |
159 |
{ |
160 |
/* There was a problem detecting the BNO055 ... check your connections */ |
161 |
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); |
162 |
while (1); |
163 |
} |
164 |
|
165 |
int eeAddress = 0; |
166 |
long bnoID; |
167 |
bool foundCalib = false; |
168 |
|
169 |
EEPROM.get(eeAddress, bnoID); |
170 |
|
171 |
adafruit_bno055_offsets_t calibrationData; |
172 |
sensor_t sensor; |
173 |
|
174 |
/* |
175 |
* Look for the sensor's unique ID at the beginning oF EEPROM. |
176 |
* This isn't foolproof, but it's better than nothing. |
177 |
*/ |
178 |
bno.getSensor(&sensor); |
179 |
if (bnoID != sensor.sensor_id) |
180 |
{ |
181 |
Serial.println("\nNo Calibration Data for this sensor exists in EEPROM"); |
182 |
delay(500); |
183 |
} |
184 |
else |
185 |
{ |
186 |
Serial.println("\nFound Calibration for this sensor in EEPROM."); |
187 |
eeAddress += sizeof(long); |
188 |
EEPROM.get(eeAddress, calibrationData); |
189 |
|
190 |
displaySensorOffsets(calibrationData); |
191 |
|
192 |
Serial.println("\n\nRestoring Calibration data to the BNO055..."); |
193 |
bno.setSensorOffsets(calibrationData); |
194 |
|
195 |
Serial.println("\n\nCalibration data loaded into BNO055"); |
196 |
foundCalib = true; |
197 |
} |
198 |
|
199 |
delay(1000); |
200 |
|
201 |
/* Display some basic information on this sensor */ |
202 |
displaySensorDetails(); |
203 |
|
204 |
/* Optional: Display current status */ |
205 |
displaySensorStatus(); |
206 |
|
207 |
bno.setExtCrystalUse(true); |
208 |
|
209 |
sensors_event_t event; |
210 |
bno.getEvent(&event); |
211 |
if (foundCalib){ |
212 |
Serial.println("Move sensor slightly to calibrate magnetometers"); |
213 |
while (!bno.isFullyCalibrated()) |
214 |
{ |
215 |
bno.getEvent(&event); |
216 |
delay(BNO055_SAMPLERATE_DELAY_MS); |
217 |
} |
218 |
} |
219 |
else |
220 |
{ |
221 |
Serial.println("Please Calibrate Sensor: "); |
222 |
while (!bno.isFullyCalibrated()) |
223 |
{ |
224 |
bno.getEvent(&event); |
225 |
|
226 |
Serial.print("X: "); |
227 |
Serial.print(event.orientation.x, 4); |
228 |
Serial.print("\tY: "); |
229 |
Serial.print(event.orientation.y, 4); |
230 |
Serial.print("\tZ: "); |
231 |
Serial.print(event.orientation.z, 4); |
232 |
|
233 |
/* Optional: Display calibration status */ |
234 |
displayCalStatus(); |
235 |
|
236 |
/* New line for the next sample */ |
237 |
Serial.println(""); |
238 |
|
239 |
/* Wait the specified delay before requesting new data */ |
240 |
delay(BNO055_SAMPLERATE_DELAY_MS); |
241 |
} |
242 |
} |
243 |
|
244 |
Serial.println("\nFully calibrated!"); |
245 |
Serial.println("--------------------------------"); |
246 |
Serial.println("Calibration Results: "); |
247 |
adafruit_bno055_offsets_t newCalib; |
248 |
bno.getSensorOffsets(newCalib); |
249 |
displaySensorOffsets(newCalib); |
250 |
|
251 |
Serial.println("\n\nStoring calibration data to EEPROM..."); |
252 |
|
253 |
eeAddress = 0; |
254 |
bno.getSensor(&sensor); |
255 |
bnoID = sensor.sensor_id; |
256 |
|
257 |
EEPROM.put(eeAddress, bnoID); |
258 |
|
259 |
eeAddress += sizeof(long); |
260 |
EEPROM.put(eeAddress, newCalib); |
261 |
Serial.println("Data stored to EEPROM."); |
262 |
|
263 |
Serial.println("\n--------------------------------\n"); |
264 |
delay(500); |
265 |
} |
266 |
|
267 |
void loop() { |
268 |
/* Get a new sensor event */ |
269 |
sensors_event_t event; |
270 |
bno.getEvent(&event); |
271 |
|
272 |
/* Display the floating point data */ |
273 |
Serial.print("X: "); |
274 |
Serial.print(event.orientation.x, 4); |
275 |
Serial.print("\tY: "); |
276 |
Serial.print(event.orientation.y, 4); |
277 |
Serial.print("\tZ: "); |
278 |
Serial.print(event.orientation.z, 4); |
279 |
|
280 |
/* Optional: Display calibration status */ |
281 |
displayCalStatus(); |
282 |
|
283 |
/* Optional: Display sensor status (debug only) */ |
284 |
//displaySensorStatus(); |
285 |
|
286 |
/* New line for the next sample */ |
287 |
Serial.println(""); |
288 |
|
289 |
/* Wait the specified delay before requesting new data */ |
290 |
delay(BNO055_SAMPLERATE_DELAY_MS); |
291 |
} |