adafruit_bno055 / examples / restore_offsets / restore_offsets.ino @ 312a5b9e
History | View | Annotate | Download (7.662 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 | 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 | Serial.println("Orientation Sensor Test"); Serial.println(""); |
||
155 | |||
156 | /* Initialise the sensor */ |
||
157 | if (!bno.begin()) |
||
158 | { |
||
159 | /* There was a problem detecting the BNO055 ... check your connections */ |
||
160 | Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); |
||
161 | while (1); |
||
162 | } |
||
163 | |||
164 | int eeAddress = 0; |
||
165 | long bnoID; |
||
166 | bool foundCalib = false; |
||
167 | |||
168 | EEPROM.get(eeAddress, bnoID); |
||
169 | |||
170 | adafruit_bno055_offsets_t calibrationData; |
||
171 | sensor_t sensor; |
||
172 | |||
173 | /* |
||
174 | * Look for the sensor's unique ID at the beginning oF EEPROM. |
||
175 | * This isn't foolproof, but it's better than nothing. |
||
176 | */ |
||
177 | bno.getSensor(&sensor); |
||
178 | if (bnoID != sensor.sensor_id) |
||
179 | { |
||
180 | Serial.println("No Calibration Data for this sensor exists in EEPROM"); |
||
181 | } |
||
182 | else |
||
183 | { |
||
184 | foundCalib = true; |
||
185 | Serial.println("Found Calibration for this sensor in EEPROM."); |
||
186 | eeAddress += sizeof(long); |
||
187 | EEPROM.get(eeAddress, calibrationData); |
||
188 | |||
189 | displaySensorOffsets(calibrationData); |
||
190 | |||
191 | Serial.println("\n\nRestoring Calibration data to the BNO055..."); |
||
192 | bno.setSensorOffsets(calibrationData); |
||
193 | |||
194 | Serial.println("\n\nCalibration data loaded into BNO055"); |
||
195 | } |
||
196 | |||
197 | delay(1000); |
||
198 | |||
199 | /* Display some basic information on this sensor */ |
||
200 | displaySensorDetails(); |
||
201 | |||
202 | /* Optional: Display current status */ |
||
203 | displaySensorStatus(); |
||
204 | |||
205 | bno.setExtCrystalUse(true); |
||
206 | |||
207 | sensors_event_t event; |
||
208 | bno.getEvent(&event); |
||
209 | Serial.println("Move sensor slightly to calibrate magnetometers"); |
||
210 | while (!bno.isFullyCalibrated()) |
||
211 | { |
||
212 | bno.getEvent(&event); |
||
213 | delay(BNO055_SAMPLERATE_DELAY_MS); |
||
214 | } |
||
215 | |||
216 | Serial.println("\nFully calibrated!"); |
||
217 | Serial.println("--------------------------------"); |
||
218 | Serial.println("Calibration Results: "); |
||
219 | bno.getSensorOffsets(calibrationData); |
||
220 | displaySensorOffsets(calibrationData); |
||
221 | Serial.println("\n--------------------------------\n\n"); |
||
222 | |||
223 | //if (!foundCalib){ |
||
224 | Serial.println("\n\nStoring calibration data to EEPROM..."); |
||
225 | |||
226 | eeAddress = 0; |
||
227 | bno.getSensor(&sensor); |
||
228 | bnoID = sensor.sensor_id; |
||
229 | |||
230 | EEPROM.put(eeAddress, bnoID); |
||
231 | |||
232 | eeAddress += sizeof(long); |
||
233 | EEPROM.put(eeAddress, calibrationData); |
||
234 | Serial.println("Data stored to EEPROM.\n"); |
||
235 | //} |
||
236 | } |
||
237 | |||
238 | void loop() { |
||
239 | /* Get a new sensor event */ |
||
240 | sensors_event_t event; |
||
241 | bno.getEvent(&event); |
||
242 | |||
243 | /* Display the floating point data */ |
||
244 | Serial.print("X: "); |
||
245 | Serial.print(event.orientation.x, 4); |
||
246 | Serial.print("\tY: "); |
||
247 | Serial.print(event.orientation.y, 4); |
||
248 | Serial.print("\tZ: "); |
||
249 | Serial.print(event.orientation.z, 4); |
||
250 | |||
251 | /* Optional: Display calibration status */ |
||
252 | displayCalStatus(); |
||
253 | |||
254 | /* Optional: Display sensor status (debug only) */ |
||
255 | //displaySensorStatus(); |
||
256 | |||
257 | /* New line for the next sample */ |
||
258 | Serial.println(""); |
||
259 | |||
260 | /* Wait the specified delay before requesting nex data */ |
||
261 | delay(BNO055_SAMPLERATE_DELAY_MS); |
||
262 | } |