brix5 / firmware / demo / FeatherBase-BNO055_RGB_Haptics / FeatherBase-BNO055_RGB_Haptics.ino @ 5ef3477f
History | View | Annotate | Download (45.631 KB)
1 | 7393c00e | Jan | /* vim:ts=2:sw=2:expandtab |
---|---|---|---|
2 | |||
3 | Mapping BNO055 Orientation to RGB WS2812 LEDs and DRV2605 Haptics |
||
4 | on FeatherBase |
||
5 | |||
6 | Based on BNO055_MS5637_t3 Basic Example Code by Kris Winer |
||
7 | |||
8 | Original Winer Comments follow: |
||
9 | |||
10 | date: October 19, 2014 |
||
11 | license: Beerware - Use this code however you'd like. If you |
||
12 | find it useful you can buy me a beer some time. |
||
13 | |||
14 | Demonstrates basic BNO055 functionality including parameterizing the register addresses, |
||
15 | initializing the sensor, communicating with pressure sensor MS5637, |
||
16 | getting properly scaled accelerometer, gyroscope, and magnetometer data out. |
||
17 | |||
18 | Added display functions to allow display to on breadboard monitor. |
||
19 | |||
20 | Addition of 9 DoF sensor fusion using open source Madgwick and Mahony filter algorithms. |
||
21 | Can compare results to hardware 9 DoF sensor fusion carried out on the BNO055. |
||
22 | Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1. |
||
23 | |||
24 | This sketch is intended specifically for the BNO055+MS5637 Add-On Shield for the Teensy 3.1. |
||
25 | It uses SDA/SCL on pins 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h. |
||
26 | |||
27 | The Add-on shield can also be used as a stand-alone breakout board for any Arduino, Teensy, or |
||
28 | other microcontroller by closing the solder jumpers on the back of the board. |
||
29 | |||
30 | The MS5637 is a simple but high resolution (24-bit) pressure sensor, which can be used in its high resolution |
||
31 | mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of |
||
32 | only 1 microAmp. The choice will depend on the application. |
||
33 | |||
34 | All sensors communicate via I2C at 400 Hz or higher. |
||
35 | SDA and SCL should have external pull-up resistors (to 3.3V). |
||
36 | 4K7 resistors are on the BNO055_MS5637 breakout board. |
||
37 | |||
38 | Hardware setup: |
||
39 | Breakout Board --------- Arduino/Teensy |
||
40 | 3V3 ---------------------- 3.3V |
||
41 | SDA -----------------------A4/17 |
||
42 | SCL -----------------------A5/16 |
||
43 | GND ---------------------- GND |
||
44 | |||
45 | Note: The BNO055_MS5637 breakout board is an I2C sensor and uses the Arduino Wire or Teensy i2c_t3.h library. |
||
46 | Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. |
||
47 | We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. |
||
48 | We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. |
||
49 | The Teensy has no internal pullups and we are using the Wire.begin function of the i2c_t3.h library |
||
50 | to select 400 Hz i2c speed. |
||
51 | */ |
||
52 | |||
53 | // Uncomment to use BNO055 on address 0x29 instead of 0x28 |
||
54 | //#define ADO 1 |
||
55 | |||
56 | 18e991a4 | Jan | // Which pin on the Arduino is connected to the WS2812 RGB LEDs? |
57 | // On ESP32, A3 is not output capable; |
||
58 | // quick fix is to hardware connect (e.g.) 21 to A3 and use 21 |
||
59 | #define LED_PIN A3 |
||
60 | //#define LED_PIN 21 |
||
61 | |||
62 | // use DRV2605 Haptics? |
||
63 | 7393c00e | Jan | #define USE_DRV |
64 | |||
65 | #if defined(__MK20DX128__) || defined(__MK20DX256__) |
||
66 | #include "i2c_t3.h" |
||
67 | #else |
||
68 | #include "Wire.h" |
||
69 | #endif |
||
70 | #include <Adafruit_NeoPixel.h> |
||
71 | #ifdef __AVR__ |
||
72 | #include <avr/power.h> // Required for 16 MHz Adafruit Trinket |
||
73 | #endif |
||
74 | |||
75 | #ifdef USE_DRV |
||
76 | #include <Adafruit_DRV2605.h> |
||
77 | Adafruit_DRV2605 drv; |
||
78 | #endif |
||
79 | |||
80 | // How many NeoPixels are attached to the Arduino? |
||
81 | #define LED_COUNT 2 |
||
82 | Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800); |
||
83 | |||
84 | //#include <Audio.h> |
||
85 | //AudioOutputI2S audioOutput; // audio shield: headphones & line-out |
||
86 | |||
87 | // Audio Code, Enable AudioOutputI2SQuad line to reproduce |
||
88 | // GUItool: begin automatically generated code |
||
89 | //AudioSynthWaveform waveform0; //xy=192,380 |
||
90 | //AudioSynthWaveform waveform1; //xy=192,420 |
||
91 | //AudioSynthWaveform waveform2; //xy=192,460 |
||
92 | //AudioSynthWaveform waveform3; //xy=192,500 |
||
93 | //AudioOutputI2SQuad i2s_quad1; //xy=385.8452453613281,443.1428527832031 |
||
94 | //AudioConnection patchCord0(waveform0, 0, i2s_quad1, 0); |
||
95 | //AudioConnection patchCord1(waveform1, 0, i2s_quad1, 1); |
||
96 | //AudioConnection patchCord2(waveform2, 0, i2s_quad1, 2); |
||
97 | //AudioConnection patchCord3(waveform3, 0, i2s_quad1, 3); |
||
98 | //AudioControlSGTL5000 sgtl5000_2; //xy=381.8453063964844,384.8571472167969 |
||
99 | //AudioControlSGTL5000 sgtl5000_1; //xy=382.2737731933594,345.14288330078125 |
||
100 | // GUItool: end automatically generated code |
||
101 | |||
102 | //#include <SPI.h> |
||
103 | |||
104 | // BNO055 Register Map |
||
105 | // http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_10_Release.pdf |
||
106 | // |
||
107 | // BNO055 Page 0 |
||
108 | #define BNO055_CHIP_ID 0x00 // should be 0xA0 |
||
109 | #define BNO055_ACC_ID 0x01 // should be 0xFB |
||
110 | #define BNO055_MAG_ID 0x02 // should be 0x32 |
||
111 | #define BNO055_GYRO_ID 0x03 // should be 0x0F |
||
112 | #define BNO055_SW_REV_ID_LSB 0x04 |
||
113 | #define BNO055_SW_REV_ID_MSB 0x05 |
||
114 | #define BNO055_BL_REV_ID 0x06 |
||
115 | #define BNO055_PAGE_ID 0x07 |
||
116 | #define BNO055_ACC_DATA_X_LSB 0x08 |
||
117 | #define BNO055_ACC_DATA_X_MSB 0x09 |
||
118 | #define BNO055_ACC_DATA_Y_LSB 0x0A |
||
119 | #define BNO055_ACC_DATA_Y_MSB 0x0B |
||
120 | #define BNO055_ACC_DATA_Z_LSB 0x0C |
||
121 | #define BNO055_ACC_DATA_Z_MSB 0x0D |
||
122 | #define BNO055_MAG_DATA_X_LSB 0x0E |
||
123 | #define BNO055_MAG_DATA_X_MSB 0x0F |
||
124 | #define BNO055_MAG_DATA_Y_LSB 0x10 |
||
125 | #define BNO055_MAG_DATA_Y_MSB 0x11 |
||
126 | #define BNO055_MAG_DATA_Z_LSB 0x12 |
||
127 | #define BNO055_MAG_DATA_Z_MSB 0x13 |
||
128 | #define BNO055_GYR_DATA_X_LSB 0x14 |
||
129 | #define BNO055_GYR_DATA_X_MSB 0x15 |
||
130 | #define BNO055_GYR_DATA_Y_LSB 0x16 |
||
131 | #define BNO055_GYR_DATA_Y_MSB 0x17 |
||
132 | #define BNO055_GYR_DATA_Z_LSB 0x18 |
||
133 | #define BNO055_GYR_DATA_Z_MSB 0x19 |
||
134 | #define BNO055_EUL_HEADING_LSB 0x1A |
||
135 | #define BNO055_EUL_HEADING_MSB 0x1B |
||
136 | #define BNO055_EUL_ROLL_LSB 0x1C |
||
137 | #define BNO055_EUL_ROLL_MSB 0x1D |
||
138 | #define BNO055_EUL_PITCH_LSB 0x1E |
||
139 | #define BNO055_EUL_PITCH_MSB 0x1F |
||
140 | #define BNO055_QUA_DATA_W_LSB 0x20 |
||
141 | #define BNO055_QUA_DATA_W_MSB 0x21 |
||
142 | #define BNO055_QUA_DATA_X_LSB 0x22 |
||
143 | #define BNO055_QUA_DATA_X_MSB 0x23 |
||
144 | #define BNO055_QUA_DATA_Y_LSB 0x24 |
||
145 | #define BNO055_QUA_DATA_Y_MSB 0x25 |
||
146 | #define BNO055_QUA_DATA_Z_LSB 0x26 |
||
147 | #define BNO055_QUA_DATA_Z_MSB 0x27 |
||
148 | #define BNO055_LIA_DATA_X_LSB 0x28 |
||
149 | #define BNO055_LIA_DATA_X_MSB 0x29 |
||
150 | #define BNO055_LIA_DATA_Y_LSB 0x2A |
||
151 | #define BNO055_LIA_DATA_Y_MSB 0x2B |
||
152 | #define BNO055_LIA_DATA_Z_LSB 0x2C |
||
153 | #define BNO055_LIA_DATA_Z_MSB 0x2D |
||
154 | #define BNO055_GRV_DATA_X_LSB 0x2E |
||
155 | #define BNO055_GRV_DATA_X_MSB 0x2F |
||
156 | #define BNO055_GRV_DATA_Y_LSB 0x30 |
||
157 | #define BNO055_GRV_DATA_Y_MSB 0x31 |
||
158 | #define BNO055_GRV_DATA_Z_LSB 0x32 |
||
159 | #define BNO055_GRV_DATA_Z_MSB 0x33 |
||
160 | #define BNO055_TEMP 0x34 |
||
161 | #define BNO055_CALIB_STAT 0x35 |
||
162 | #define BNO055_ST_RESULT 0x36 |
||
163 | #define BNO055_INT_STATUS 0x37 |
||
164 | #define BNO055_SYS_CLK_STATUS 0x38 |
||
165 | #define BNO055_SYS_STATUS 0x39 |
||
166 | #define BNO055_SYS_ERR 0x3A |
||
167 | #define BNO055_UNIT_SEL 0x3B |
||
168 | #define BNO055_OPR_MODE 0x3D |
||
169 | #define BNO055_PWR_MODE 0x3E |
||
170 | #define BNO055_SYS_TRIGGER 0x3F |
||
171 | #define BNO055_TEMP_SOURCE 0x40 |
||
172 | #define BNO055_AXIS_MAP_CONFIG 0x41 |
||
173 | #define BNO055_AXIS_MAP_SIGN 0x42 |
||
174 | #define BNO055_ACC_OFFSET_X_LSB 0x55 |
||
175 | #define BNO055_ACC_OFFSET_X_MSB 0x56 |
||
176 | #define BNO055_ACC_OFFSET_Y_LSB 0x57 |
||
177 | #define BNO055_ACC_OFFSET_Y_MSB 0x58 |
||
178 | #define BNO055_ACC_OFFSET_Z_LSB 0x59 |
||
179 | #define BNO055_ACC_OFFSET_Z_MSB 0x5A |
||
180 | #define BNO055_MAG_OFFSET_X_LSB 0x5B |
||
181 | #define BNO055_MAG_OFFSET_X_MSB 0x5C |
||
182 | #define BNO055_MAG_OFFSET_Y_LSB 0x5D |
||
183 | #define BNO055_MAG_OFFSET_Y_MSB 0x5E |
||
184 | #define BNO055_MAG_OFFSET_Z_LSB 0x5F |
||
185 | #define BNO055_MAG_OFFSET_Z_MSB 0x60 |
||
186 | #define BNO055_GYR_OFFSET_X_LSB 0x61 |
||
187 | #define BNO055_GYR_OFFSET_X_MSB 0x62 |
||
188 | #define BNO055_GYR_OFFSET_Y_LSB 0x63 |
||
189 | #define BNO055_GYR_OFFSET_Y_MSB 0x64 |
||
190 | #define BNO055_GYR_OFFSET_Z_LSB 0x65 |
||
191 | #define BNO055_GYR_OFFSET_Z_MSB 0x66 |
||
192 | #define BNO055_ACC_RADIUS_LSB 0x67 |
||
193 | #define BNO055_ACC_RADIUS_MSB 0x68 |
||
194 | #define BNO055_MAG_RADIUS_LSB 0x69 |
||
195 | #define BNO055_MAG_RADIUS_MSB 0x6A |
||
196 | // |
||
197 | // BNO055 Page 1 |
||
198 | #define BNO055_PAGE_ID 0x07 |
||
199 | #define BNO055_ACC_CONFIG 0x08 |
||
200 | #define BNO055_MAG_CONFIG 0x09 |
||
201 | #define BNO055_GYRO_CONFIG_0 0x0A |
||
202 | #define BNO055_GYRO_CONFIG_1 0x0B |
||
203 | #define BNO055_ACC_SLEEP_CONFIG 0x0C |
||
204 | #define BNO055_GYR_SLEEP_CONFIG 0x0D |
||
205 | #define BNO055_INT_MSK 0x0F |
||
206 | #define BNO055_INT_EN 0x10 |
||
207 | #define BNO055_ACC_AM_THRES 0x11 |
||
208 | #define BNO055_ACC_INT_SETTINGS 0x12 |
||
209 | #define BNO055_ACC_HG_DURATION 0x13 |
||
210 | #define BNO055_ACC_HG_THRESH 0x14 |
||
211 | #define BNO055_ACC_NM_THRESH 0x15 |
||
212 | #define BNO055_ACC_NM_SET 0x16 |
||
213 | #define BNO055_GYR_INT_SETTINGS 0x17 |
||
214 | #define BNO055_GYR_HR_X_SET 0x18 |
||
215 | #define BNO055_GYR_DUR_X 0x19 |
||
216 | #define BNO055_GYR_HR_Y_SET 0x1A |
||
217 | #define BNO055_GYR_DUR_Y 0x1B |
||
218 | #define BNO055_GYR_HR_Z_SET 0x1C |
||
219 | #define BNO055_GYR_DUR_Z 0x1D |
||
220 | #define BNO055_GYR_AM_THRESH 0x1E |
||
221 | #define BNO055_GYR_AM_SET 0x1F |
||
222 | |||
223 | // Using the BNO055_MS5637 breakout board/Teensy 3.1 Add-On Shield, ADO is set to 1 by default |
||
224 | #if ADO |
||
225 | #define BNO055_ADDRESS 0x29 // Device address of BNO055 when ADO = 1 |
||
226 | #define MS5637_ADDRESS 0x76 // Address of MS5637 altimeter |
||
227 | #else |
||
228 | #define BNO055_ADDRESS 0x28 // Device address of BNO055 when ADO = 0 |
||
229 | #define MS5637_ADDRESS 0x76 // Address of MS5637 altimeter |
||
230 | #endif |
||
231 | |||
232 | #define SerialDebug true // set to true to get Serial output for debugging |
||
233 | |||
234 | // Set initial input parameters |
||
235 | enum Ascale { // ACC Full Scale |
||
236 | AFS_2G = 0, |
||
237 | AFS_4G, |
||
238 | AFS_8G, |
||
239 | AFS_18G |
||
240 | }; |
||
241 | |||
242 | enum Abw { // ACC Bandwidth |
||
243 | ABW_7_81Hz = 0, |
||
244 | ABW_15_63Hz, |
||
245 | ABW_31_25Hz, |
||
246 | ABW_62_5Hz, |
||
247 | ABW_125Hz, |
||
248 | ABW_250Hz, |
||
249 | ABW_500Hz, |
||
250 | ABW_1000Hz, //0x07 |
||
251 | }; |
||
252 | |||
253 | enum APwrMode { // ACC Pwr Mode |
||
254 | NormalA = 0, |
||
255 | SuspendA, |
||
256 | LowPower1A, |
||
257 | StandbyA, |
||
258 | LowPower2A, |
||
259 | DeepSuspendA |
||
260 | }; |
||
261 | |||
262 | enum Gscale { // gyro full scale |
||
263 | GFS_2000DPS = 0, |
||
264 | GFS_1000DPS, |
||
265 | GFS_500DPS, |
||
266 | GFS_250DPS, |
||
267 | GFS_125DPS // 0x04 |
||
268 | }; |
||
269 | |||
270 | enum GPwrMode { // GYR Pwr Mode |
||
271 | NormalG = 0, |
||
272 | FastPowerUpG, |
||
273 | DeepSuspendedG, |
||
274 | SuspendG, |
||
275 | AdvancedPowerSaveG |
||
276 | }; |
||
277 | |||
278 | enum Gbw { // gyro bandwidth |
||
279 | GBW_523Hz = 0, |
||
280 | GBW_230Hz, |
||
281 | GBW_116Hz, |
||
282 | GBW_47Hz, |
||
283 | GBW_23Hz, |
||
284 | GBW_12Hz, |
||
285 | GBW_64Hz, |
||
286 | GBW_32Hz |
||
287 | }; |
||
288 | |||
289 | enum OPRMode { // BNO-55 operation modes |
||
290 | CONFIGMODE = 0x00, |
||
291 | // Sensor Mode |
||
292 | ACCONLY, |
||
293 | MAGONLY, |
||
294 | GYROONLY, |
||
295 | ACCMAG, |
||
296 | ACCGYRO, |
||
297 | MAGGYRO, |
||
298 | AMG, // 0x07 |
||
299 | // Fusion Mode |
||
300 | IMU, |
||
301 | COMPASS, |
||
302 | M4G, |
||
303 | NDOF_FMC_OFF, |
||
304 | NDOF // 0x0C |
||
305 | }; |
||
306 | |||
307 | enum PWRMode { |
||
308 | Normalpwr = 0, |
||
309 | Lowpower, |
||
310 | Suspendpwr |
||
311 | }; |
||
312 | |||
313 | enum Modr { // magnetometer output data rate |
||
314 | MODR_2Hz = 0, |
||
315 | MODR_6Hz, |
||
316 | MODR_8Hz, |
||
317 | MODR_10Hz, |
||
318 | MODR_15Hz, |
||
319 | MODR_20Hz, |
||
320 | MODR_25Hz, |
||
321 | MODR_30Hz |
||
322 | }; |
||
323 | |||
324 | enum MOpMode { // MAG Op Mode |
||
325 | LowPower = 0, |
||
326 | Regular, |
||
327 | EnhancedRegular, |
||
328 | HighAccuracy |
||
329 | }; |
||
330 | |||
331 | enum MPwrMode { // MAG power mode |
||
332 | Normal = 0, |
||
333 | Sleep, |
||
334 | Suspend, |
||
335 | ForceMode |
||
336 | }; |
||
337 | |||
338 | #define ADC_256 0x00 // define pressure and temperature conversion rates |
||
339 | #define ADC_512 0x02 |
||
340 | #define ADC_1024 0x04 |
||
341 | #define ADC_2048 0x06 |
||
342 | #define ADC_4096 0x08 |
||
343 | #define ADC_8192 0x0A |
||
344 | #define ADC_D1 0x40 |
||
345 | #define ADC_D2 0x50 |
||
346 | |||
347 | // Specify sensor configuration |
||
348 | uint8_t OSR = ADC_8192; // set pressure amd temperature oversample rate |
||
349 | // |
||
350 | uint8_t GPwrMode = Normal; // Gyro power mode |
||
351 | uint8_t Gscale = GFS_250DPS; // Gyro full scale |
||
352 | //uint8_t Godr = GODR_250Hz; // Gyro sample rate |
||
353 | uint8_t Gbw = GBW_23Hz; // Gyro bandwidth |
||
354 | // |
||
355 | uint8_t Ascale = AFS_2G; // Accel full scale |
||
356 | //uint8_t Aodr = AODR_250Hz; // Accel sample rate |
||
357 | uint8_t APwrMode = Normal; // Accel power mode |
||
358 | uint8_t Abw = ABW_31_25Hz; // Accel bandwidth, accel sample rate divided by ABW_divx |
||
359 | // |
||
360 | //uint8_t Mscale = MFS_4Gauss; // Select magnetometer full-scale resolution |
||
361 | uint8_t MOpMode = HighAccuracy; // Select magnetometer perfomance mode |
||
362 | uint8_t MPwrMode = Normal; // Select magnetometer power mode |
||
363 | uint8_t Modr = MODR_10Hz; // Select magnetometer ODR when in BNO055 bypass mode |
||
364 | |||
365 | uint8_t PWRMode = Normal ; // Select BNO055 power mode |
||
366 | uint8_t OPRMode = NDOF; // specify operation mode for sensors |
||
367 | uint8_t status; // BNO055 data status register |
||
368 | float aRes, gRes, mRes; // scale resolutions per LSB for the sensors |
||
369 | |||
370 | // Pin definitions |
||
371 | int intPin = 13; // These can be changed, 2 and 3 are the Arduinos ext int pins |
||
372 | int myLed = 9; |
||
373 | |||
374 | uint16_t Pcal[8]; // calibration constants from MS5637 PROM registers |
||
375 | unsigned char nCRC; // calculated check sum to ensure PROM integrity |
||
376 | uint32_t D1 = 0, D2 = 0; // raw MS5637 pressure and temperature data |
||
377 | double dT, OFFSET, SENS, OFFSET2, SENS2; // First order and second order corrections for raw S5637 temperature and pressure data |
||
378 | int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output |
||
379 | int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output |
||
380 | int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output |
||
381 | int16_t quatCount[4]; // Stores the 16-bit signed quaternion output |
||
382 | int16_t EulCount[3]; // Stores the 16-bit signed Euler angle output |
||
383 | int16_t LIACount[3]; // Stores the 16-bit signed linear acceleration output |
||
384 | int16_t GRVCount[3]; // Stores the 16-bit signed gravity vector output |
||
385 | float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, and magnetometer |
||
386 | int16_t tempGCount, tempMCount; // temperature raw count output of mag and gyro |
||
387 | float Gtemperature, Mtemperature; // Stores the BNO055 gyro and LIS3MDL mag internal chip temperatures in degrees Celsius |
||
388 | double Temperature, Pressure; // stores MS5637 pressures sensor pressure and temperature |
||
389 | |||
390 | // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) |
||
391 | float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) |
||
392 | float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) |
||
393 | // There is a tradeoff in the beta parameter between accuracy and response speed. |
||
394 | // In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. |
||
395 | // However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. |
||
396 | // Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! |
||
397 | // By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec |
||
398 | // I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; |
||
399 | // the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. |
||
400 | // In any case, this is the free parameter in the Madgwick filtering and fusion scheme. |
||
401 | float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta |
||
402 | float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value |
||
403 | #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral |
||
404 | #define Ki 0.0f |
||
405 | |||
406 | uint32_t delt_t = 0, count = 0, sumCount = 0; // used to control display output rate |
||
407 | float pitch, yaw, roll; |
||
408 | float Pitch, Yaw, Roll; |
||
409 | float LIAx, LIAy, LIAz, GRVx, GRVy, GRVz; |
||
410 | float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes |
||
411 | uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval |
||
412 | uint32_t Now = 0; // used to calculate integration interval |
||
413 | |||
414 | float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values |
||
415 | float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion |
||
416 | float quat[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion |
||
417 | float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method |
||
418 | |||
419 | void setup() |
||
420 | { |
||
421 | // Wire.begin(); |
||
422 | // TWBR = 12; // 400 kbit/sec I2C speed for Pro Mini |
||
423 | // Setup for Master mode, pins 16/17, external pullups, 400kHz for Teensy 3.1 |
||
424 | #if defined(__MK20DX128__) || defined(__MK20DX256__) |
||
425 | Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000); |
||
426 | #else |
||
427 | Wire.begin(); |
||
428 | #endif |
||
429 | |||
430 | strip.begin(); // INITIALIZE NeoPixel strip object (REQUIRED) |
||
431 | strip.show(); // Turn OFF all pixels ASAP |
||
432 | strip.setBrightness(100); // Set BRIGHTNESS to about 1/5 (max = 255) |
||
433 | |||
434 | #ifdef USE_DRV |
||
435 | 18e991a4 | Jan | pinMode(A1, OUTPUT); |
436 | digitalWrite(A1, HIGH); |
||
437 | |||
438 | 7393c00e | Jan | drv.selectLibrary(6); |
439 | drv.useLRA(); |
||
440 | drv.setMode(DRV2605_MODE_REALTIME); |
||
441 | #endif |
||
442 | |||
443 | delay(1000); |
||
444 | Serial.begin(115200); |
||
445 | |||
446 | // Set up the interrupt pin, its set as active high, push-pull |
||
447 | pinMode(intPin, INPUT); |
||
448 | pinMode(myLed, OUTPUT); |
||
449 | digitalWrite(myLed, HIGH); |
||
450 | |||
451 | /* |
||
452 | // scan for i2c devices |
||
453 | byte error, address; |
||
454 | int nDevices; |
||
455 | |||
456 | Serial.println("Scanning..."); |
||
457 | |||
458 | nDevices = 0; |
||
459 | for(address = 1; address < 127; address++ ) |
||
460 | { |
||
461 | // The i2c_scanner uses the return value of |
||
462 | // the Write.endTransmisstion to see if |
||
463 | // a device did acknowledge to the address. |
||
464 | Wire.beginTransmission(address); |
||
465 | error = Wire.endTransmission(); |
||
466 | |||
467 | if (error == 0) |
||
468 | { |
||
469 | Serial.print("I2C device found at address 0x"); |
||
470 | if (address<16) |
||
471 | Serial.print("0"); |
||
472 | Serial.print(address,HEX); |
||
473 | Serial.println(" !"); |
||
474 | |||
475 | nDevices++; |
||
476 | } |
||
477 | else if (error==4) |
||
478 | { |
||
479 | Serial.print("Unknow error at address 0x"); |
||
480 | if (address<16) |
||
481 | Serial.print("0"); |
||
482 | Serial.println(address,HEX); |
||
483 | } |
||
484 | } |
||
485 | if (nDevices == 0) |
||
486 | Serial.println("No I2C devices found\n"); |
||
487 | else |
||
488 | Serial.println("done\n"); |
||
489 | |||
490 | */ |
||
491 | |||
492 | // Read the WHO_AM_I register, this is a good test of communication |
||
493 | Serial.println("BNO055 9-axis motion sensor..."); |
||
494 | byte c = readByte(BNO055_ADDRESS, BNO055_CHIP_ID); // Read WHO_AM_I register for BNO055 |
||
495 | Serial.print("BNO055 Address = 0x"); Serial.println(BNO055_ADDRESS, HEX); |
||
496 | Serial.print("BNO055 WHO_AM_I = 0x"); Serial.println(BNO055_CHIP_ID, HEX); |
||
497 | Serial.print("BNO055 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.println(" I should be 0xA0"); |
||
498 | |||
499 | delay(500); |
||
500 | |||
501 | // Read the WHO_AM_I register of the accelerometer, this is a good test of communication |
||
502 | byte d = readByte(BNO055_ADDRESS, BNO055_ACC_ID); // Read WHO_AM_I register for accelerometer |
||
503 | Serial.print("BNO055 ACC "); Serial.print("I AM "); Serial.print(d, HEX); Serial.println(" I should be 0xFB"); |
||
504 | |||
505 | delay(500); |
||
506 | |||
507 | // Read the WHO_AM_I register of the magnetometer, this is a good test of communication |
||
508 | byte e = readByte(BNO055_ADDRESS, BNO055_MAG_ID); // Read WHO_AM_I register for magnetometer |
||
509 | Serial.print("BNO055 MAG "); Serial.print("I AM "); Serial.print(e, HEX); Serial.println(" I should be 0x32"); |
||
510 | |||
511 | delay(500); |
||
512 | |||
513 | // Read the WHO_AM_I register of the gyroscope, this is a good test of communication |
||
514 | byte f = readByte(BNO055_ADDRESS, BNO055_GYRO_ID); // Read WHO_AM_I register for LIS3MDL |
||
515 | Serial.print("BNO055 GYRO "); Serial.print("I AM "); Serial.print(f, HEX); Serial.println(" I should be 0x0F"); |
||
516 | |||
517 | delay(500); |
||
518 | |||
519 | if (c == 0xA0) // BNO055 WHO_AM_I should always be 0xA0 |
||
520 | { |
||
521 | Serial.println("BNO055 is online..."); |
||
522 | |||
523 | // Check software revision ID |
||
524 | byte swlsb = readByte(BNO055_ADDRESS, BNO055_SW_REV_ID_LSB); |
||
525 | byte swmsb = readByte(BNO055_ADDRESS, BNO055_SW_REV_ID_MSB); |
||
526 | Serial.print("BNO055 SW Revision ID: "); Serial.print(swmsb, HEX); Serial.print("."); Serial.println(swlsb, HEX); |
||
527 | Serial.println("Should be 03.04"); |
||
528 | |||
529 | // Check bootloader version |
||
530 | byte blid = readByte(BNO055_ADDRESS, BNO055_BL_REV_ID); |
||
531 | Serial.print("BNO055 bootloader Version: "); Serial.println(blid); |
||
532 | |||
533 | // Check self-test results |
||
534 | byte selftest = readByte(BNO055_ADDRESS, BNO055_ST_RESULT); |
||
535 | |||
536 | if (selftest & 0x01) { |
||
537 | Serial.println("accelerometer passed selftest"); |
||
538 | } else { |
||
539 | Serial.println("accelerometer failed selftest"); |
||
540 | } |
||
541 | if (selftest & 0x02) { |
||
542 | Serial.println("magnetometer passed selftest"); |
||
543 | } else { |
||
544 | Serial.println("magnetometer failed selftest"); |
||
545 | } |
||
546 | if (selftest & 0x04) { |
||
547 | Serial.println("gyroscope passed selftest"); |
||
548 | } else { |
||
549 | Serial.println("gyroscope failed selftest"); |
||
550 | } |
||
551 | if (selftest & 0x08) { |
||
552 | Serial.println("MCU passed selftest"); |
||
553 | } else { |
||
554 | Serial.println("MCU failed selftest"); |
||
555 | } |
||
556 | |||
557 | delay(500); |
||
558 | |||
559 | //delay(500); |
||
560 | |||
561 | //accelgyroCalBNO055(accelBias, gyroBias); |
||
562 | |||
563 | // Serial.println("Average accelerometer bias (mg) = "); Serial.println(accelBias[0]); Serial.println(accelBias[1]); Serial.println(accelBias[2]); |
||
564 | // Serial.println("Average gyro bias (dps) = "); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]); |
||
565 | |||
566 | //delay(500); |
||
567 | |||
568 | //magCalBNO055(magBias); |
||
569 | |||
570 | //Serial.println("Average magnetometer bias (mG) = "); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]); |
||
571 | |||
572 | // delay(500); |
||
573 | |||
574 | // Check calibration status of the sensors |
||
575 | uint8_t calstat = readByte(BNO055_ADDRESS, BNO055_CALIB_STAT); |
||
576 | Serial.println("Not calibrated = 0, fully calibrated = 3"); |
||
577 | Serial.print("System calibration status "); Serial.println( (0xC0 & calstat) >> 6); |
||
578 | Serial.print("Gyro calibration status "); Serial.println( (0x30 & calstat) >> 4); |
||
579 | Serial.print("Accel calibration status "); Serial.println( (0x0C & calstat) >> 2); |
||
580 | Serial.print("Mag calibration status "); Serial.println( (0x03 & calstat) >> 0); |
||
581 | |||
582 | initBNO055(); // Initialize the BNO055 |
||
583 | Serial.println("BNO055 initialized for sensor mode...."); // Initialize BNO055 for sensor read |
||
584 | |||
585 | } |
||
586 | else |
||
587 | { |
||
588 | Serial.print("Could not connect to BNO055: 0x"); |
||
589 | Serial.println(c, HEX); |
||
590 | while (1) ; // Loop forever if communication doesn't happen |
||
591 | } |
||
592 | } |
||
593 | |||
594 | void loop() |
||
595 | { |
||
596 | |||
597 | readAccelData(accelCount); // Read the x/y/z adc values |
||
598 | // Now we'll calculate the accleration value into actual mg's |
||
599 | ax = (float)accelCount[0] - accelBias[0]; // subtract off calculated accel bias |
||
600 | ay = (float)accelCount[1] - accelBias[1]; |
||
601 | az = (float)accelCount[2] - accelBias[2]; |
||
602 | |||
603 | readGyroData(gyroCount); // Read the x/y/z adc values |
||
604 | // Calculate the gyro value into actual degrees per second |
||
605 | gx = (float)gyroCount[0] / 16. - gyroBias[0]; // subtract off calculated gyro bias |
||
606 | gy = (float)gyroCount[1] / 16. - gyroBias[1]; |
||
607 | gz = (float)gyroCount[2] / 16. - gyroBias[2]; |
||
608 | |||
609 | readMagData(magCount); // Read the x/y/z adc values |
||
610 | // Calculate the magnetometer values in milliGauss |
||
611 | mx = (float)magCount[0] / 1.6 - magBias[0]; // get actual magnetometer value in mGauss |
||
612 | my = (float)magCount[1] / 1.6 - magBias[1]; |
||
613 | mz = (float)magCount[2] / 1.6 - magBias[2]; |
||
614 | |||
615 | readQuatData(quatCount); // Read the x/y/z adc values |
||
616 | // Calculate the quaternion values |
||
617 | quat[0] = (float)(quatCount[0]) / 16384.; |
||
618 | quat[1] = (float)(quatCount[1]) / 16384.; |
||
619 | quat[2] = (float)(quatCount[2]) / 16384.; |
||
620 | quat[3] = (float)(quatCount[3]) / 16384.; |
||
621 | |||
622 | readEulData(EulCount); // Read the x/y/z adc values |
||
623 | // Calculate the Euler angles values in degrees |
||
624 | Yaw = (float)EulCount[0] / 16.; |
||
625 | Roll = (float)EulCount[1] / 16.; |
||
626 | Pitch = (float)EulCount[2] / 16.; |
||
627 | |||
628 | readLIAData(LIACount); // Read the x/y/z adc values |
||
629 | // Calculate the linear acceleration (sans gravity) values in mg |
||
630 | LIAx = (float)LIACount[0]; |
||
631 | LIAy = (float)LIACount[1]; |
||
632 | LIAz = (float)LIACount[2]; |
||
633 | |||
634 | readGRVData(GRVCount); // Read the x/y/z adc values |
||
635 | // Calculate the linear acceleration (sans gravity) values in mg |
||
636 | GRVx = (float)GRVCount[0]; |
||
637 | GRVy = (float)GRVCount[1]; |
||
638 | GRVz = (float)GRVCount[2]; |
||
639 | |||
640 | Now = micros(); |
||
641 | deltat = ((Now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update |
||
642 | lastUpdate = Now; |
||
643 | |||
644 | sum += deltat; // sum for averaging filter update rate |
||
645 | sumCount++; |
||
646 | |||
647 | // Sensors x, y, and z-axes for the three sensor: accel, gyro, and magnetometer are all aligned, so |
||
648 | // no allowance for any orientation mismatch in feeding the output to the quaternion filter is required. |
||
649 | // For the BNO055, the sensor forward is along the x-axis just like |
||
650 | // in the LSM9DS0 and MPU9250 sensors. This rotation can be modified to allow any convenient orientation convention. |
||
651 | // This is ok by aircraft orientation standards! |
||
652 | // Pass gyro rate as rad/s |
||
653 | // MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz); |
||
654 | // MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz); |
||
655 | |||
656 | // Serial print and/or display at 0.5 s rate independent of data rates |
||
657 | delt_t = millis() - count; |
||
658 | if (delt_t > 100) { // update LCD once per half-second independent of read rate |
||
659 | |||
660 | // check BNO-055 error status at 2 Hz rate |
||
661 | uint8_t errstat = readByte(BNO055_ADDRESS, BNO055_CALIB_STAT); |
||
662 | if (errstat == 0x01) { |
||
663 | uint8_t syserr = readByte(BNO055_ADDRESS, BNO055_SYS_ERR); |
||
664 | if (syserr == 0x01) Serial.println("Peripheral initialization error"); |
||
665 | if (syserr == 0x02) Serial.println("System initialization error"); |
||
666 | if (syserr == 0x03) Serial.println("Self test result failed"); |
||
667 | if (syserr == 0x04) Serial.println("Register map value out of range"); |
||
668 | if (syserr == 0x05) Serial.println("Register map address out of range"); |
||
669 | if (syserr == 0x06) Serial.println("Register map write error"); |
||
670 | if (syserr == 0x07) Serial.println("BNO low power mode no available for selected operation mode"); |
||
671 | if (syserr == 0x08) Serial.println("Accelerometer power mode not available"); |
||
672 | if (syserr == 0x09) Serial.println("Fusion algorithm configuration error"); |
||
673 | if (syserr == 0x0A) Serial.println("Sensor configuration error"); |
||
674 | } |
||
675 | |||
676 | // Serial.print("ax = "); Serial.print((int)ax); |
||
677 | // Serial.print(" ay = "); Serial.print((int)ay); |
||
678 | // Serial.print(" az = "); Serial.print((int)az); Serial.println(" mg"); |
||
679 | // Serial.print("gx = "); Serial.print( gx, 2); |
||
680 | // Serial.print(" gy = "); Serial.print( gy, 2); |
||
681 | // Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s"); |
||
682 | // Serial.print("mx = "); Serial.print( (int)mx ); |
||
683 | // Serial.print(" my = "); Serial.print( (int)my ); |
||
684 | // Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG"); |
||
685 | // |
||
686 | // Serial.print("qx = "); Serial.print(q[0]); |
||
687 | // Serial.print(" qy = "); Serial.print(q[1]); |
||
688 | // Serial.print(" qz = "); Serial.print(q[2]); |
||
689 | // Serial.print(" qw = "); Serial.println(q[3]); |
||
690 | // Serial.print("quatw = "); Serial.print(quat[0]); |
||
691 | // Serial.print(" quatx = "); Serial.print(quat[1]); |
||
692 | // Serial.print(" quaty = "); Serial.print(quat[2]); |
||
693 | // Serial.print(" quatz = "); Serial.println(quat[3]); |
||
694 | |||
695 | tempGCount = readGyroTempData(); // Read the gyro adc values |
||
696 | Gtemperature = (float) tempGCount; // Gyro chip temperature in degrees Centigrade |
||
697 | // Print gyro die temperature in degrees Centigrade |
||
698 | // Serial.print("Gyro temperature is "); Serial.print(Gtemperature, 1); Serial.println(" degrees C"); // Print T values to tenths of a degree C |
||
699 | |||
700 | // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. |
||
701 | // In this coordinate system, the positive z-axis is down toward Earth. |
||
702 | // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. |
||
703 | // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. |
||
704 | // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. |
||
705 | // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. |
||
706 | // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be |
||
707 | // applied in the correct order which for this configuration is yaw, pitch, and then roll. |
||
708 | // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. |
||
709 | yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); |
||
710 | pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
||
711 | roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); |
||
712 | pitch *= 180.0f / PI; |
||
713 | yaw *= 180.0f / PI; |
||
714 | // yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04 |
||
715 | roll *= 180.0f / PI; |
||
716 | |||
717 | // Serial.print("Software Yaw, Pitch, Roll: "); |
||
718 | // Serial.print(yaw, 2); |
||
719 | // Serial.print(", "); |
||
720 | // Serial.print(pitch, 2); |
||
721 | // Serial.print(", "); |
||
722 | // Serial.println(roll, 2); |
||
723 | // |
||
724 | // Serial.print("Hardware Yaw, Pitch, Roll: "); |
||
725 | // Serial.print(Yaw, 2); |
||
726 | // Serial.print(", "); |
||
727 | // Serial.print(Pitch, 2); |
||
728 | // Serial.print(", "); |
||
729 | // Serial.println(Roll, 2); |
||
730 | // |
||
731 | // Serial.print("Hardware x, y, z linear acceleration: "); |
||
732 | // Serial.print(LIAx, 2); |
||
733 | // Serial.print(", "); |
||
734 | // Serial.print(LIAy, 2); |
||
735 | // Serial.print(", "); |
||
736 | // Serial.println(LIAz, 2); |
||
737 | |||
738 | Serial.print("Hardware x, y, z gravity vector: "); |
||
739 | Serial.print(GRVx, 2); |
||
740 | Serial.print(", "); |
||
741 | Serial.print(GRVy, 2); |
||
742 | Serial.print(", "); |
||
743 | Serial.println(GRVz, 2); |
||
744 | |||
745 | Serial.print("rate = "); Serial.print((float)sumCount / sum, 2); Serial.println(" Hz"); |
||
746 | uint8_t r = map (GRVx, -1000, 1000, 0, 255); |
||
747 | uint8_t g = map (GRVy, -1000, 1000, 0, 255); |
||
748 | uint8_t b = map (GRVz, -1000, 1000, 0, 255); |
||
749 | Serial.print("LED Data: "); |
||
750 | Serial.print(r); |
||
751 | Serial.print(", "); |
||
752 | Serial.print(g); |
||
753 | Serial.print(", "); |
||
754 | Serial.println(b); |
||
755 | strip.setPixelColor(0, r, g, b); // Set pixel 'c' to value 'color' |
||
756 | strip.setPixelColor(1, 255-r, 255-g, 255-b); // Set pixel 'c' to value 'color' |
||
757 | strip.show(); // Turn OFF all pixels ASAP |
||
758 | |||
759 | digitalWrite(myLed, !digitalRead(myLed)); |
||
760 | count = millis(); |
||
761 | sumCount = 0; |
||
762 | sum = 0; |
||
763 | |||
764 | #ifdef USE_DRV |
||
765 | drv.setRealtimeValue(map(GRVx, -1000, 1000, 0, 127)); |
||
766 | #endif |
||
767 | } |
||
768 | |||
769 | } |
||
770 | |||
771 | //=================================================================================================================== |
||
772 | //====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data |
||
773 | //=================================================================================================================== |
||
774 | |||
775 | void readAccelData(int16_t * destination) |
||
776 | { |
||
777 | uint8_t rawData[6]; // x/y/z accel register data stored here |
||
778 | readBytes(BNO055_ADDRESS, BNO055_ACC_DATA_X_LSB, 6, &rawData[0]); // Read the six raw data registers into data array |
||
779 | destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
||
780 | destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
||
781 | destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
||
782 | } |
||
783 | |||
784 | void readGyroData(int16_t * destination) |
||
785 | { |
||
786 | uint8_t rawData[6]; // x/y/z gyro register data stored here |
||
787 | readBytes(BNO055_ADDRESS, BNO055_GYR_DATA_X_LSB, 6, &rawData[0]); // Read the six raw data registers sequentially into data array |
||
788 | destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
||
789 | destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
||
790 | destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
||
791 | } |
||
792 | |||
793 | int8_t readGyroTempData() |
||
794 | { |
||
795 | return readByte(BNO055_ADDRESS, BNO055_TEMP); // Read the two raw data registers sequentially into data array |
||
796 | } |
||
797 | |||
798 | void readMagData(int16_t * destination) |
||
799 | { |
||
800 | uint8_t rawData[6]; // x/y/z gyro register data stored here |
||
801 | readBytes(BNO055_ADDRESS, BNO055_MAG_DATA_X_LSB, 6, &rawData[0]); // Read the six raw data registers sequentially into data array |
||
802 | destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
||
803 | destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
||
804 | destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
||
805 | } |
||
806 | |||
807 | void readQuatData(int16_t * destination) |
||
808 | { |
||
809 | uint8_t rawData[8]; // x/y/z gyro register data stored here |
||
810 | readBytes(BNO055_ADDRESS, BNO055_QUA_DATA_W_LSB, 8, &rawData[0]); // Read the six raw data registers sequentially into data array |
||
811 | destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
||
812 | destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
||
813 | destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
||
814 | destination[3] = ((int16_t)rawData[7] << 8) | rawData[6] ; |
||
815 | } |
||
816 | |||
817 | void readEulData(int16_t * destination) |
||
818 | { |
||
819 | uint8_t rawData[6]; // x/y/z gyro register data stored here |
||
820 | readBytes(BNO055_ADDRESS, BNO055_EUL_HEADING_LSB, 6, &rawData[0]); // Read the six raw data registers sequentially into data array |
||
821 | destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
||
822 | destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
||
823 | destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
||
824 | } |
||
825 | |||
826 | void readLIAData(int16_t * destination) |
||
827 | { |
||
828 | uint8_t rawData[6]; // x/y/z gyro register data stored here |
||
829 | readBytes(BNO055_ADDRESS, BNO055_LIA_DATA_X_LSB, 6, &rawData[0]); // Read the six raw data registers sequentially into data array |
||
830 | destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
||
831 | destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
||
832 | destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
||
833 | } |
||
834 | |||
835 | void readGRVData(int16_t * destination) |
||
836 | { |
||
837 | uint8_t rawData[6]; // x/y/z gyro register data stored here |
||
838 | readBytes(BNO055_ADDRESS, BNO055_GRV_DATA_X_LSB, 6, &rawData[0]); // Read the six raw data registers sequentially into data array |
||
839 | destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value |
||
840 | destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; |
||
841 | destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; |
||
842 | } |
||
843 | |||
844 | void initBNO055() { |
||
845 | // Select page 1 to configure sensors |
||
846 | writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x01); |
||
847 | // Configure ACC |
||
848 | writeByte(BNO055_ADDRESS, BNO055_ACC_CONFIG, APwrMode << 5 | Abw << 3 | Ascale ); |
||
849 | // Configure GYR |
||
850 | writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_0, Gbw << 3 | Gscale ); |
||
851 | writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_1, GPwrMode); |
||
852 | // Configure MAG |
||
853 | writeByte(BNO055_ADDRESS, BNO055_MAG_CONFIG, MPwrMode << 4 | MOpMode << 2 | Modr ); |
||
854 | |||
855 | // Select page 0 to read sensors |
||
856 | writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00); |
||
857 | |||
858 | // Select BNO055 gyro temperature source |
||
859 | writeByte(BNO055_ADDRESS, BNO055_TEMP_SOURCE, 0x01 ); |
||
860 | |||
861 | // Select BNO055 sensor units (temperature in degrees C, rate in dps, accel in mg) |
||
862 | writeByte(BNO055_ADDRESS, BNO055_UNIT_SEL, 0x01 ); |
||
863 | |||
864 | // Select BNO055 system power mode |
||
865 | writeByte(BNO055_ADDRESS, BNO055_PWR_MODE, PWRMode ); |
||
866 | |||
867 | // Select BNO055 system operation mode |
||
868 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode ); |
||
869 | } |
||
870 | |||
871 | void accelgyroCalBNO055(float * dest1, float * dest2) |
||
872 | { |
||
873 | uint8_t data[6]; // data array to hold accelerometer and gyro x, y, z, data |
||
874 | uint16_t ii = 0, sample_count = 0; |
||
875 | int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; |
||
876 | |||
877 | Serial.println("Accel/Gyro Calibration: Put device on a level surface and keep motionless! Wait......"); |
||
878 | delay(1000); |
||
879 | |||
880 | // Select page 0 to read sensors |
||
881 | writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00); |
||
882 | // Select BNO055 system operation mode as NDOF for calibration |
||
883 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE ); |
||
884 | delay(25); |
||
885 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, NDOF ); |
||
886 | |||
887 | // In NDF fusion mode, accel full scale is at +/- 4g, ODR is 62.5 Hz |
||
888 | sample_count = 256; |
||
889 | for (ii = 0; ii < sample_count; ii++) { |
||
890 | int16_t accel_temp[3] = {0, 0, 0}; |
||
891 | readBytes(BNO055_ADDRESS, BNO055_ACC_DATA_X_LSB, 6, &data[0]); // Read the six raw data registers into data array |
||
892 | accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO |
||
893 | accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ; |
||
894 | accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ; |
||
895 | accel_bias[0] += (int32_t) accel_temp[0]; |
||
896 | accel_bias[1] += (int32_t) accel_temp[1]; |
||
897 | accel_bias[2] += (int32_t) accel_temp[2]; |
||
898 | delay(20); // at 62.5 Hz ODR, new accel data is available every 16 ms |
||
899 | } |
||
900 | accel_bias[0] /= (int32_t) sample_count; // get average accel bias in mg |
||
901 | accel_bias[1] /= (int32_t) sample_count; |
||
902 | accel_bias[2] /= (int32_t) sample_count; |
||
903 | |||
904 | if (accel_bias[2] > 0L) { |
||
905 | accel_bias[2] -= (int32_t) 1000; // Remove gravity from the z-axis accelerometer bias calculation |
||
906 | } |
||
907 | else { |
||
908 | accel_bias[2] += (int32_t) 1000; |
||
909 | } |
||
910 | |||
911 | dest1[0] = (float) accel_bias[0]; // save accel biases in mg for use in main program |
||
912 | dest1[1] = (float) accel_bias[1]; // accel data is 1 LSB/mg |
||
913 | dest1[2] = (float) accel_bias[2]; |
||
914 | |||
915 | // In NDF fusion mode, gyro full scale is at +/- 2000 dps, ODR is 32 Hz |
||
916 | for (ii = 0; ii < sample_count; ii++) { |
||
917 | int16_t gyro_temp[3] = {0, 0, 0}; |
||
918 | readBytes(BNO055_ADDRESS, BNO055_GYR_DATA_X_LSB, 6, &data[0]); // Read the six raw data registers into data array |
||
919 | gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO |
||
920 | gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ; |
||
921 | gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ; |
||
922 | gyro_bias[0] += (int32_t) gyro_temp[0]; |
||
923 | gyro_bias[1] += (int32_t) gyro_temp[1]; |
||
924 | gyro_bias[2] += (int32_t) gyro_temp[2]; |
||
925 | delay(35); // at 32 Hz ODR, new gyro data available every 31 ms |
||
926 | } |
||
927 | gyro_bias[0] /= (int32_t) sample_count; // get average gyro bias in counts |
||
928 | gyro_bias[1] /= (int32_t) sample_count; |
||
929 | gyro_bias[2] /= (int32_t) sample_count; |
||
930 | |||
931 | dest2[0] = (float) gyro_bias[0] / 16.; // save gyro biases in dps for use in main program |
||
932 | dest2[1] = (float) gyro_bias[1] / 16.; // gyro data is 16 LSB/dps |
||
933 | dest2[2] = (float) gyro_bias[2] / 16.; |
||
934 | |||
935 | // Return to config mode to write accelerometer biases in offset register |
||
936 | // This offset register is only used while in fusion mode when accelerometer full-scale is +/- 4g |
||
937 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE ); |
||
938 | delay(25); |
||
939 | |||
940 | //write biases to accelerometer offset registers ad 16 LSB/dps |
||
941 | writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_LSB, (int16_t)accel_bias[0] & 0xFF); |
||
942 | writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_MSB, ((int16_t)accel_bias[0] >> 8) & 0xFF); |
||
943 | writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_LSB, (int16_t)accel_bias[1] & 0xFF); |
||
944 | writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_MSB, ((int16_t)accel_bias[1] >> 8) & 0xFF); |
||
945 | writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_LSB, (int16_t)accel_bias[2] & 0xFF); |
||
946 | writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_MSB, ((int16_t)accel_bias[2] >> 8) & 0xFF); |
||
947 | |||
948 | // Check that offsets were properly written to offset registers |
||
949 | // Serial.println("Average accelerometer bias = "); |
||
950 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_LSB))); |
||
951 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_LSB))); |
||
952 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_LSB))); |
||
953 | |||
954 | //write biases to gyro offset registers |
||
955 | writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_LSB, (int16_t)gyro_bias[0] & 0xFF); |
||
956 | writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_MSB, ((int16_t)gyro_bias[0] >> 8) & 0xFF); |
||
957 | writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_LSB, (int16_t)gyro_bias[1] & 0xFF); |
||
958 | writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_MSB, ((int16_t)gyro_bias[1] >> 8) & 0xFF); |
||
959 | writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_LSB, (int16_t)gyro_bias[2] & 0xFF); |
||
960 | writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_MSB, ((int16_t)gyro_bias[2] >> 8) & 0xFF); |
||
961 | |||
962 | // Select BNO055 system operation mode |
||
963 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode ); |
||
964 | |||
965 | // Check that offsets were properly written to offset registers |
||
966 | // Serial.println("Average gyro bias = "); |
||
967 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_LSB))); |
||
968 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_LSB))); |
||
969 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_LSB))); |
||
970 | |||
971 | Serial.println("Accel/Gyro Calibration done!"); |
||
972 | } |
||
973 | |||
974 | void magCalBNO055(float * dest1) |
||
975 | { |
||
976 | uint8_t data[6]; // data array to hold accelerometer and gyro x, y, z, data |
||
977 | uint16_t ii = 0, sample_count = 0; |
||
978 | int32_t mag_bias[3] = {0, 0, 0}; |
||
979 | int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0}; |
||
980 | |||
981 | Serial.println("Mag Calibration: Wave device in a figure eight until done!"); |
||
982 | delay(4000); |
||
983 | |||
984 | // Select page 0 to read sensors |
||
985 | writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00); |
||
986 | // Select BNO055 system operation mode as NDOF for calibration |
||
987 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE ); |
||
988 | delay(25); |
||
989 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, NDOF ); |
||
990 | |||
991 | // In NDF fusion mode, mag data is in 16 LSB/microTesla, ODR is 20 Hz in forced mode |
||
992 | sample_count = 256; |
||
993 | for (ii = 0; ii < sample_count; ii++) { |
||
994 | int16_t mag_temp[3] = {0, 0, 0}; |
||
995 | readBytes(BNO055_ADDRESS, BNO055_MAG_DATA_X_LSB, 6, &data[0]); // Read the six raw data registers into data array |
||
996 | mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO |
||
997 | mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ; |
||
998 | mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ; |
||
999 | for (int jj = 0; jj < 3; jj++) { |
||
1000 | if (mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; |
||
1001 | if (mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; |
||
1002 | } |
||
1003 | delay(55); // at 20 Hz ODR, new mag data is available every 50 ms |
||
1004 | } |
||
1005 | |||
1006 | // Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]); |
||
1007 | // Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]); |
||
1008 | // Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]); |
||
1009 | |||
1010 | mag_bias[0] = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts |
||
1011 | mag_bias[1] = (mag_max[1] + mag_min[1]) / 2; // get average y mag bias in counts |
||
1012 | mag_bias[2] = (mag_max[2] + mag_min[2]) / 2; // get average z mag bias in counts |
||
1013 | |||
1014 | dest1[0] = (float) mag_bias[0] / 1.6; // save mag biases in mG for use in main program |
||
1015 | dest1[1] = (float) mag_bias[1] / 1.6; // mag data is 1.6 LSB/mg |
||
1016 | dest1[2] = (float) mag_bias[2] / 1.6; |
||
1017 | |||
1018 | // Return to config mode to write mag biases in offset register |
||
1019 | // This offset register is only used while in fusion mode when magnetometer sensitivity is 16 LSB/microTesla |
||
1020 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE ); |
||
1021 | delay(25); |
||
1022 | |||
1023 | //write biases to accelerometer offset registers as 16 LSB/microTesla |
||
1024 | writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_LSB, (int16_t)mag_bias[0] & 0xFF); |
||
1025 | writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_MSB, ((int16_t)mag_bias[0] >> 8) & 0xFF); |
||
1026 | writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_LSB, (int16_t)mag_bias[1] & 0xFF); |
||
1027 | writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_MSB, ((int16_t)mag_bias[1] >> 8) & 0xFF); |
||
1028 | writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_LSB, (int16_t)mag_bias[2] & 0xFF); |
||
1029 | writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_MSB, ((int16_t)mag_bias[2] >> 8) & 0xFF); |
||
1030 | |||
1031 | // Check that offsets were properly written to offset registers |
||
1032 | // Serial.println("Average magnetometer bias = "); |
||
1033 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_LSB))); |
||
1034 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_LSB))); |
||
1035 | // Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_LSB))); |
||
1036 | // Select BNO055 system operation mode |
||
1037 | writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode ); |
||
1038 | |||
1039 | Serial.println("Mag Calibration done!"); |
||
1040 | } |
||
1041 | |||
1042 | // I2C communication with the MS5637 is a little different from that with the BNO055 and most other sensors |
||
1043 | // For the MS5637, we write commands, and the MS5637 sends data in response, rather than directly reading |
||
1044 | // MS5637 registers |
||
1045 | |||
1046 | // I2C read/write functions for the BNO055 sensor |
||
1047 | |||
1048 | void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) |
||
1049 | { |
||
1050 | Wire.beginTransmission(address); // Initialize the Tx buffer |
||
1051 | Wire.write(subAddress); // Put slave register address in Tx buffer |
||
1052 | Wire.write(data); // Put data in Tx buffer |
||
1053 | Wire.endTransmission(); // Send the Tx buffer |
||
1054 | } |
||
1055 | |||
1056 | uint8_t readByte(uint8_t address, uint8_t subAddress) |
||
1057 | { |
||
1058 | uint8_t data; // `data` will store the register data |
||
1059 | Wire.beginTransmission(address); // Initialize the Tx buffer |
||
1060 | Wire.write(subAddress); // Put slave register address in Tx buffer |
||
1061 | // Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive |
||
1062 | Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive |
||
1063 | // Wire.requestFrom(address, 1); // Read one byte from slave register address |
||
1064 | Wire.requestFrom(address, (size_t) 1); // Read one byte from slave register address |
||
1065 | data = Wire.read(); // Fill Rx buffer with result |
||
1066 | return data; // Return data read from slave register |
||
1067 | } |
||
1068 | |||
1069 | void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) |
||
1070 | { |
||
1071 | Wire.beginTransmission(address); // Initialize the Tx buffer |
||
1072 | Wire.write(subAddress); // Put slave register address in Tx buffer |
||
1073 | // Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive |
||
1074 | Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive |
||
1075 | uint8_t i = 0; |
||
1076 | // Wire.requestFrom(address, count); // Read bytes from slave register address |
||
1077 | Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address |
||
1078 | while (Wire.available()) { |
||
1079 | dest[i++] = Wire.read(); |
||
1080 | } // Put read results in the Rx buffer |
||
1081 | } |