adafruit_bno055 / examples / restore_offsets / restore_offsets.ino @ 55604844
History | View | Annotate | Download (8.948 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 | Adafruit_BNO055 bno = Adafruit_BNO055(55); |
||
37 | |||
38 | /**************************************************************************/ |
||
39 | /* |
||
40 | 463eabf7 | Wetmelon | Displays some basic information on this sensor from the unified |
41 | sensor API sensor_t type (see Adafruit_Sensor for more information) |
||
42 | */ |
||
43 | 312a5b9e | Wetmelon | /**************************************************************************/ |
44 | void displaySensorDetails(void) |
||
45 | { |
||
46 | 463eabf7 | Wetmelon | 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 | 312a5b9e | Wetmelon | } |
59 | |||
60 | /**************************************************************************/ |
||
61 | /* |
||
62 | 463eabf7 | Wetmelon | Display some basic info about the sensor status |
63 | */ |
||
64 | 312a5b9e | Wetmelon | /**************************************************************************/ |
65 | void displaySensorStatus(void) |
||
66 | { |
||
67 | 463eabf7 | Wetmelon | /* 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 | 312a5b9e | Wetmelon | } |
83 | |||
84 | /**************************************************************************/ |
||
85 | /* |
||
86 | 463eabf7 | Wetmelon | Display sensor calibration status |
87 | */ |
||
88 | 312a5b9e | Wetmelon | /**************************************************************************/ |
89 | void displayCalStatus(void) |
||
90 | { |
||
91 | 463eabf7 | Wetmelon | /* 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 | 312a5b9e | Wetmelon | } |
115 | |||
116 | /**************************************************************************/ |
||
117 | /* |
||
118 | 463eabf7 | Wetmelon | Display the raw calibration offset and radius data |
119 | */ |
||
120 | 312a5b9e | Wetmelon | /**************************************************************************/ |
121 | void displaySensorOffsets(const adafruit_bno055_offsets_t &calibData) |
||
122 | { |
||
123 | 463eabf7 | Wetmelon | 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 | 312a5b9e | Wetmelon | } |
144 | |||
145 | |||
146 | /**************************************************************************/ |
||
147 | /* |
||
148 | 463eabf7 | Wetmelon | Arduino setup function (automatically called at startup) |
149 | */ |
||
150 | 312a5b9e | Wetmelon | /**************************************************************************/ |
151 | void setup(void) |
||
152 | { |
||
153 | 463eabf7 | Wetmelon | 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 | e0b5db7e | Wetmelon | foundCalib = true; |
197 | 463eabf7 | Wetmelon | } |
198 | 312a5b9e | Wetmelon | |
199 | 463eabf7 | Wetmelon | delay(1000); |
200 | 312a5b9e | Wetmelon | |
201 | 463eabf7 | Wetmelon | /* Display some basic information on this sensor */ |
202 | displaySensorDetails(); |
||
203 | 312a5b9e | Wetmelon | |
204 | 463eabf7 | Wetmelon | /* Optional: Display current status */ |
205 | displaySensorStatus(); |
||
206 | 312a5b9e | Wetmelon | |
207 | 463eabf7 | Wetmelon | bno.setExtCrystalUse(true); |
208 | 312a5b9e | Wetmelon | |
209 | 463eabf7 | Wetmelon | sensors_event_t event; |
210 | bno.getEvent(&event); |
||
211 | if (foundCalib){ |
||
212 | e0b5db7e | Wetmelon | 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 | 463eabf7 | Wetmelon | Serial.println("Please Calibrate Sensor: "); |
222 | e0b5db7e | Wetmelon | while (!bno.isFullyCalibrated()) |
223 | { |
||
224 | bno.getEvent(&event); |
||
225 | 463eabf7 | Wetmelon | |
226 | e0b5db7e | Wetmelon | 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 | 463eabf7 | Wetmelon | |
233 | e0b5db7e | Wetmelon | /* Optional: Display calibration status */ |
234 | displayCalStatus(); |
||
235 | 463eabf7 | Wetmelon | |
236 | e0b5db7e | Wetmelon | /* New line for the next sample */ |
237 | Serial.println(""); |
||
238 | 463eabf7 | Wetmelon | |
239 | e0b5db7e | Wetmelon | /* Wait the specified delay before requesting new data */ |
240 | delay(BNO055_SAMPLERATE_DELAY_MS); |
||
241 | } |
||
242 | } |
||
243 | 312a5b9e | Wetmelon | |
244 | 463eabf7 | Wetmelon | 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 | e0b5db7e | Wetmelon | Serial.println("\n\nStoring calibration data to EEPROM..."); |
252 | 312a5b9e | Wetmelon | |
253 | 463eabf7 | Wetmelon | eeAddress = 0; |
254 | bno.getSensor(&sensor); |
||
255 | bnoID = sensor.sensor_id; |
||
256 | |||
257 | EEPROM.put(eeAddress, bnoID); |
||
258 | 312a5b9e | Wetmelon | |
259 | 463eabf7 | Wetmelon | eeAddress += sizeof(long); |
260 | EEPROM.put(eeAddress, newCalib); |
||
261 | Serial.println("Data stored to EEPROM."); |
||
262 | 312a5b9e | Wetmelon | |
263 | 463eabf7 | Wetmelon | Serial.println("\n--------------------------------\n"); |
264 | delay(500); |
||
265 | 312a5b9e | Wetmelon | } |
266 | |||
267 | void loop() { |
||
268 | 463eabf7 | Wetmelon | /* Get a new sensor event */ |
269 | sensors_event_t event; |
||
270 | bno.getEvent(&event); |
||
271 | 312a5b9e | Wetmelon | |
272 | 463eabf7 | Wetmelon | /* 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 | 312a5b9e | Wetmelon | |
280 | 463eabf7 | Wetmelon | /* Optional: Display calibration status */ |
281 | displayCalStatus(); |
||
282 | 312a5b9e | Wetmelon | |
283 | 463eabf7 | Wetmelon | /* Optional: Display sensor status (debug only) */ |
284 | //displaySensorStatus(); |
||
285 | 312a5b9e | Wetmelon | |
286 | 463eabf7 | Wetmelon | /* New line for the next sample */ |
287 | Serial.println(""); |
||
288 | 312a5b9e | Wetmelon | |
289 | 463eabf7 | Wetmelon | /* Wait the specified delay before requesting new data */ |
290 | delay(BNO055_SAMPLERATE_DELAY_MS); |
||
291 | 312a5b9e | Wetmelon | } |