brix5 / firmware / demo / FeatherBase-BNO055_RGB_Haptics / FeatherBase-BNO055_RGB_Haptics.ino @ d83f65fc
History | View | Annotate | Download (45.631 KB)
1 |
/* 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 |
// 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 |
#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 |
pinMode(A1, OUTPUT); |
436 |
digitalWrite(A1, HIGH); |
437 |
|
438 |
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 |
} |
1082 |
|