adafruit_bno055 / examples / restore_offsets / restore_offsets.ino @ 3448c31d
History | View | Annotate | Download (9.236 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 |
// Check I2C device address and correct line below (by default address is 0x29 or 0x28) |
37 |
// id, address |
38 |
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28); |
39 |
|
40 |
/**************************************************************************/ |
41 |
/* |
42 |
Displays some basic information on this sensor from the unified |
43 |
sensor API sensor_t type (see Adafruit_Sensor for more information) |
44 |
*/ |
45 |
/**************************************************************************/ |
46 |
void displaySensorDetails(void) |
47 |
{ |
48 |
sensor_t sensor; |
49 |
bno.getSensor(&sensor); |
50 |
Serial.println("------------------------------------"); |
51 |
Serial.print("Sensor: "); Serial.println(sensor.name); |
52 |
Serial.print("Driver Ver: "); Serial.println(sensor.version); |
53 |
Serial.print("Unique ID: "); Serial.println(sensor.sensor_id); |
54 |
Serial.print("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx"); |
55 |
Serial.print("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx"); |
56 |
Serial.print("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx"); |
57 |
Serial.println("------------------------------------"); |
58 |
Serial.println(""); |
59 |
delay(500); |
60 |
} |
61 |
|
62 |
/**************************************************************************/ |
63 |
/* |
64 |
Display some basic info about the sensor status |
65 |
*/ |
66 |
/**************************************************************************/ |
67 |
void displaySensorStatus(void) |
68 |
{ |
69 |
/* Get the system status values (mostly for debugging purposes) */ |
70 |
uint8_t system_status, self_test_results, system_error; |
71 |
system_status = self_test_results = system_error = 0; |
72 |
bno.getSystemStatus(&system_status, &self_test_results, &system_error); |
73 |
|
74 |
/* Display the results in the Serial Monitor */ |
75 |
Serial.println(""); |
76 |
Serial.print("System Status: 0x"); |
77 |
Serial.println(system_status, HEX); |
78 |
Serial.print("Self Test: 0x"); |
79 |
Serial.println(self_test_results, HEX); |
80 |
Serial.print("System Error: 0x"); |
81 |
Serial.println(system_error, HEX); |
82 |
Serial.println(""); |
83 |
delay(500); |
84 |
} |
85 |
|
86 |
/**************************************************************************/ |
87 |
/* |
88 |
Display sensor calibration status |
89 |
*/ |
90 |
/**************************************************************************/ |
91 |
void displayCalStatus(void) |
92 |
{ |
93 |
/* Get the four calibration values (0..3) */ |
94 |
/* Any sensor data reporting 0 should be ignored, */ |
95 |
/* 3 means 'fully calibrated" */ |
96 |
uint8_t system, gyro, accel, mag; |
97 |
system = gyro = accel = mag = 0; |
98 |
bno.getCalibration(&system, &gyro, &accel, &mag); |
99 |
|
100 |
/* The data should be ignored until the system calibration is > 0 */ |
101 |
Serial.print("\t"); |
102 |
if (!system) |
103 |
{ |
104 |
Serial.print("! "); |
105 |
} |
106 |
|
107 |
/* Display the individual values */ |
108 |
Serial.print("Sys:"); |
109 |
Serial.print(system, DEC); |
110 |
Serial.print(" G:"); |
111 |
Serial.print(gyro, DEC); |
112 |
Serial.print(" A:"); |
113 |
Serial.print(accel, DEC); |
114 |
Serial.print(" M:"); |
115 |
Serial.print(mag, DEC); |
116 |
} |
117 |
|
118 |
/**************************************************************************/ |
119 |
/* |
120 |
Display the raw calibration offset and radius data |
121 |
*/ |
122 |
/**************************************************************************/ |
123 |
void displaySensorOffsets(const adafruit_bno055_offsets_t &calibData) |
124 |
{ |
125 |
Serial.print("Accelerometer: "); |
126 |
Serial.print(calibData.accel_offset_x); Serial.print(" "); |
127 |
Serial.print(calibData.accel_offset_y); Serial.print(" "); |
128 |
Serial.print(calibData.accel_offset_z); Serial.print(" "); |
129 |
|
130 |
Serial.print("\nGyro: "); |
131 |
Serial.print(calibData.gyro_offset_x); Serial.print(" "); |
132 |
Serial.print(calibData.gyro_offset_y); Serial.print(" "); |
133 |
Serial.print(calibData.gyro_offset_z); Serial.print(" "); |
134 |
|
135 |
Serial.print("\nMag: "); |
136 |
Serial.print(calibData.mag_offset_x); Serial.print(" "); |
137 |
Serial.print(calibData.mag_offset_y); Serial.print(" "); |
138 |
Serial.print(calibData.mag_offset_z); Serial.print(" "); |
139 |
|
140 |
Serial.print("\nAccel Radius: "); |
141 |
Serial.print(calibData.accel_radius); |
142 |
|
143 |
Serial.print("\nMag Radius: "); |
144 |
Serial.print(calibData.mag_radius); |
145 |
} |
146 |
|
147 |
|
148 |
/**************************************************************************/ |
149 |
/* |
150 |
Arduino setup function (automatically called at startup) |
151 |
*/ |
152 |
/**************************************************************************/ |
153 |
void setup(void) |
154 |
{ |
155 |
Serial.begin(115200); |
156 |
delay(1000); |
157 |
Serial.println("Orientation Sensor Test"); Serial.println(""); |
158 |
|
159 |
/* Initialise the sensor */ |
160 |
if (!bno.begin()) |
161 |
{ |
162 |
/* There was a problem detecting the BNO055 ... check your connections */ |
163 |
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); |
164 |
while (1); |
165 |
} |
166 |
|
167 |
int eeAddress = 0; |
168 |
long bnoID; |
169 |
bool foundCalib = false; |
170 |
|
171 |
EEPROM.get(eeAddress, bnoID); |
172 |
|
173 |
adafruit_bno055_offsets_t calibrationData; |
174 |
sensor_t sensor; |
175 |
|
176 |
/* |
177 |
* Look for the sensor's unique ID at the beginning oF EEPROM. |
178 |
* This isn't foolproof, but it's better than nothing. |
179 |
*/ |
180 |
bno.getSensor(&sensor); |
181 |
if (bnoID != sensor.sensor_id) |
182 |
{ |
183 |
Serial.println("\nNo Calibration Data for this sensor exists in EEPROM"); |
184 |
delay(500); |
185 |
} |
186 |
else |
187 |
{ |
188 |
Serial.println("\nFound Calibration for this sensor in EEPROM."); |
189 |
eeAddress += sizeof(long); |
190 |
EEPROM.get(eeAddress, calibrationData); |
191 |
|
192 |
displaySensorOffsets(calibrationData); |
193 |
|
194 |
Serial.println("\n\nRestoring Calibration data to the BNO055..."); |
195 |
bno.setSensorOffsets(calibrationData); |
196 |
|
197 |
Serial.println("\n\nCalibration data loaded into BNO055"); |
198 |
foundCalib = true; |
199 |
} |
200 |
|
201 |
delay(1000); |
202 |
|
203 |
/* Display some basic information on this sensor */ |
204 |
displaySensorDetails(); |
205 |
|
206 |
/* Optional: Display current status */ |
207 |
displaySensorStatus(); |
208 |
|
209 |
/* Crystal must be configured AFTER loading calibration data into BNO055. */ |
210 |
bno.setExtCrystalUse(true); |
211 |
|
212 |
sensors_event_t event; |
213 |
bno.getEvent(&event); |
214 |
/* always recal the mag as It goes out of calibration very often */ |
215 |
if (foundCalib){ |
216 |
Serial.println("Move sensor slightly to calibrate magnetometers"); |
217 |
while (!bno.isFullyCalibrated()) |
218 |
{ |
219 |
bno.getEvent(&event); |
220 |
delay(BNO055_SAMPLERATE_DELAY_MS); |
221 |
} |
222 |
} |
223 |
else |
224 |
{ |
225 |
Serial.println("Please Calibrate Sensor: "); |
226 |
while (!bno.isFullyCalibrated()) |
227 |
{ |
228 |
bno.getEvent(&event); |
229 |
|
230 |
Serial.print("X: "); |
231 |
Serial.print(event.orientation.x, 4); |
232 |
Serial.print("\tY: "); |
233 |
Serial.print(event.orientation.y, 4); |
234 |
Serial.print("\tZ: "); |
235 |
Serial.print(event.orientation.z, 4); |
236 |
|
237 |
/* Optional: Display calibration status */ |
238 |
displayCalStatus(); |
239 |
|
240 |
/* New line for the next sample */ |
241 |
Serial.println(""); |
242 |
|
243 |
/* Wait the specified delay before requesting new data */ |
244 |
delay(BNO055_SAMPLERATE_DELAY_MS); |
245 |
} |
246 |
} |
247 |
|
248 |
Serial.println("\nFully calibrated!"); |
249 |
Serial.println("--------------------------------"); |
250 |
Serial.println("Calibration Results: "); |
251 |
adafruit_bno055_offsets_t newCalib; |
252 |
bno.getSensorOffsets(newCalib); |
253 |
displaySensorOffsets(newCalib); |
254 |
|
255 |
Serial.println("\n\nStoring calibration data to EEPROM..."); |
256 |
|
257 |
eeAddress = 0; |
258 |
bno.getSensor(&sensor); |
259 |
bnoID = sensor.sensor_id; |
260 |
|
261 |
EEPROM.put(eeAddress, bnoID); |
262 |
|
263 |
eeAddress += sizeof(long); |
264 |
EEPROM.put(eeAddress, newCalib); |
265 |
Serial.println("Data stored to EEPROM."); |
266 |
|
267 |
Serial.println("\n--------------------------------\n"); |
268 |
delay(500); |
269 |
} |
270 |
|
271 |
void loop() { |
272 |
/* Get a new sensor event */ |
273 |
sensors_event_t event; |
274 |
bno.getEvent(&event); |
275 |
|
276 |
/* Display the floating point data */ |
277 |
Serial.print("X: "); |
278 |
Serial.print(event.orientation.x, 4); |
279 |
Serial.print("\tY: "); |
280 |
Serial.print(event.orientation.y, 4); |
281 |
Serial.print("\tZ: "); |
282 |
Serial.print(event.orientation.z, 4); |
283 |
|
284 |
/* Optional: Display calibration status */ |
285 |
displayCalStatus(); |
286 |
|
287 |
/* Optional: Display sensor status (debug only) */ |
288 |
//displaySensorStatus(); |
289 |
|
290 |
/* New line for the next sample */ |
291 |
Serial.println(""); |
292 |
|
293 |
/* Wait the specified delay before requesting new data */ |
294 |
delay(BNO055_SAMPLERATE_DELAY_MS); |
295 |
} |