adafruit_bno055 / examples / restore_offsets / restore_offsets.ino @ 5e9f001d
History | View | Annotate | Download (9.236 KB)
1 | 312a5b9e | Wetmelon | #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 | e5a77cdb | Gintaras | // 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 | 312a5b9e | Wetmelon | |
40 | /**************************************************************************/ |
||
41 | /* |
||
42 | 463eabf7 | Wetmelon | Displays some basic information on this sensor from the unified |
43 | sensor API sensor_t type (see Adafruit_Sensor for more information) |
||
44 | */ |
||
45 | 312a5b9e | Wetmelon | /**************************************************************************/ |
46 | void displaySensorDetails(void) |
||
47 | { |
||
48 | 463eabf7 | Wetmelon | 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 | 312a5b9e | Wetmelon | } |
61 | |||
62 | /**************************************************************************/ |
||
63 | /* |
||
64 | 463eabf7 | Wetmelon | Display some basic info about the sensor status |
65 | */ |
||
66 | 312a5b9e | Wetmelon | /**************************************************************************/ |
67 | void displaySensorStatus(void) |
||
68 | { |
||
69 | 463eabf7 | Wetmelon | /* 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 | 312a5b9e | Wetmelon | } |
85 | |||
86 | /**************************************************************************/ |
||
87 | /* |
||
88 | 463eabf7 | Wetmelon | Display sensor calibration status |
89 | */ |
||
90 | 312a5b9e | Wetmelon | /**************************************************************************/ |
91 | void displayCalStatus(void) |
||
92 | { |
||
93 | 463eabf7 | Wetmelon | /* 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 | 312a5b9e | Wetmelon | } |
117 | |||
118 | /**************************************************************************/ |
||
119 | /* |
||
120 | 463eabf7 | Wetmelon | Display the raw calibration offset and radius data |
121 | */ |
||
122 | 312a5b9e | Wetmelon | /**************************************************************************/ |
123 | void displaySensorOffsets(const adafruit_bno055_offsets_t &calibData) |
||
124 | { |
||
125 | 463eabf7 | Wetmelon | 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 | 312a5b9e | Wetmelon | } |
146 | |||
147 | |||
148 | /**************************************************************************/ |
||
149 | /* |
||
150 | 463eabf7 | Wetmelon | Arduino setup function (automatically called at startup) |
151 | */ |
||
152 | 312a5b9e | Wetmelon | /**************************************************************************/ |
153 | void setup(void) |
||
154 | { |
||
155 | 463eabf7 | Wetmelon | 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 | e0b5db7e | Wetmelon | foundCalib = true; |
199 | 463eabf7 | Wetmelon | } |
200 | 312a5b9e | Wetmelon | |
201 | 463eabf7 | Wetmelon | delay(1000); |
202 | 312a5b9e | Wetmelon | |
203 | 463eabf7 | Wetmelon | /* Display some basic information on this sensor */ |
204 | displaySensorDetails(); |
||
205 | 312a5b9e | Wetmelon | |
206 | 463eabf7 | Wetmelon | /* Optional: Display current status */ |
207 | displaySensorStatus(); |
||
208 | 312a5b9e | Wetmelon | |
209 | ecdc6229 | Jan Hoffmann | /* Crystal must be configured AFTER loading calibration data into BNO055. */ |
210 | 463eabf7 | Wetmelon | bno.setExtCrystalUse(true); |
211 | 312a5b9e | Wetmelon | |
212 | 463eabf7 | Wetmelon | sensors_event_t event; |
213 | bno.getEvent(&event); |
||
214 | ecdc6229 | Jan Hoffmann | /* always recal the mag as It goes out of calibration very often */ |
215 | 463eabf7 | Wetmelon | if (foundCalib){ |
216 | e0b5db7e | Wetmelon | 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 | 463eabf7 | Wetmelon | Serial.println("Please Calibrate Sensor: "); |
226 | e0b5db7e | Wetmelon | while (!bno.isFullyCalibrated()) |
227 | { |
||
228 | bno.getEvent(&event); |
||
229 | 463eabf7 | Wetmelon | |
230 | e0b5db7e | Wetmelon | 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 | 463eabf7 | Wetmelon | |
237 | e0b5db7e | Wetmelon | /* Optional: Display calibration status */ |
238 | displayCalStatus(); |
||
239 | 463eabf7 | Wetmelon | |
240 | e0b5db7e | Wetmelon | /* New line for the next sample */ |
241 | Serial.println(""); |
||
242 | 463eabf7 | Wetmelon | |
243 | e0b5db7e | Wetmelon | /* Wait the specified delay before requesting new data */ |
244 | delay(BNO055_SAMPLERATE_DELAY_MS); |
||
245 | } |
||
246 | } |
||
247 | 312a5b9e | Wetmelon | |
248 | 463eabf7 | Wetmelon | 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 | e0b5db7e | Wetmelon | Serial.println("\n\nStoring calibration data to EEPROM..."); |
256 | 312a5b9e | Wetmelon | |
257 | 463eabf7 | Wetmelon | eeAddress = 0; |
258 | bno.getSensor(&sensor); |
||
259 | bnoID = sensor.sensor_id; |
||
260 | |||
261 | EEPROM.put(eeAddress, bnoID); |
||
262 | 312a5b9e | Wetmelon | |
263 | 463eabf7 | Wetmelon | eeAddress += sizeof(long); |
264 | EEPROM.put(eeAddress, newCalib); |
||
265 | Serial.println("Data stored to EEPROM."); |
||
266 | 312a5b9e | Wetmelon | |
267 | 463eabf7 | Wetmelon | Serial.println("\n--------------------------------\n"); |
268 | delay(500); |
||
269 | 312a5b9e | Wetmelon | } |
270 | |||
271 | void loop() { |
||
272 | 463eabf7 | Wetmelon | /* Get a new sensor event */ |
273 | sensors_event_t event; |
||
274 | bno.getEvent(&event); |
||
275 | 312a5b9e | Wetmelon | |
276 | 463eabf7 | Wetmelon | /* 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 | 312a5b9e | Wetmelon | |
284 | 463eabf7 | Wetmelon | /* Optional: Display calibration status */ |
285 | displayCalStatus(); |
||
286 | 312a5b9e | Wetmelon | |
287 | 463eabf7 | Wetmelon | /* Optional: Display sensor status (debug only) */ |
288 | //displaySensorStatus(); |
||
289 | 312a5b9e | Wetmelon | |
290 | 463eabf7 | Wetmelon | /* New line for the next sample */ |
291 | Serial.println(""); |
||
292 | 312a5b9e | Wetmelon | |
293 | 463eabf7 | Wetmelon | /* Wait the specified delay before requesting new data */ |
294 | delay(BNO055_SAMPLERATE_DELAY_MS); |
||
295 | 312a5b9e | Wetmelon | } |