Statistics
| Branch: | Revision:

brix5 / firmware / demo / FeatherBase-BNO055_RGB_Haptics / FeatherBase-BNO055_RGB_Haptics.ino @ 18e991a4

History | View | Annotate | Download (45.6 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