Revision 7393c00e

View differences:

firmware/demo/FeatherBase-BNO055_RGB_Haptics/FeatherBase-BNO055_RGB_Haptics.ino
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
// use DRV2605 Haptics
57
#define USE_DRV
58

  
59
#if defined(__MK20DX128__) || defined(__MK20DX256__)
60
#include "i2c_t3.h"
61
#else
62
#include "Wire.h"
63
#endif
64
#include <Adafruit_NeoPixel.h>
65
#ifdef __AVR__
66
#include <avr/power.h> // Required for 16 MHz Adafruit Trinket
67
#endif
68

  
69
#ifdef USE_DRV
70
#include <Adafruit_DRV2605.h>
71
Adafruit_DRV2605 drv;
72

  
73
#endif
74

  
75
// Which pin on the Arduino is connected to the NeoPixels?
76
// On a Trinket or Gemma we suggest changing this to 1:
77
#define LED_PIN    A1
78

  
79
// How many NeoPixels are attached to the Arduino?
80
#define LED_COUNT 2
81
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);
82

  
83
//#include <Audio.h>
84
//AudioOutputI2S      audioOutput;        // audio shield: headphones & line-out
85

  
86
// Audio Code, Enable AudioOutputI2SQuad line to reproduce
87
// GUItool: begin automatically generated code
88
//AudioSynthWaveform       waveform0; //xy=192,380
89
//AudioSynthWaveform       waveform1; //xy=192,420
90
//AudioSynthWaveform       waveform2; //xy=192,460
91
//AudioSynthWaveform       waveform3; //xy=192,500
92
//AudioOutputI2SQuad       i2s_quad1;      //xy=385.8452453613281,443.1428527832031
93
//AudioConnection          patchCord0(waveform0, 0, i2s_quad1, 0);
94
//AudioConnection          patchCord1(waveform1, 0, i2s_quad1, 1);
95
//AudioConnection          patchCord2(waveform2, 0, i2s_quad1, 2);
96
//AudioConnection          patchCord3(waveform3, 0, i2s_quad1, 3);
97
//AudioControlSGTL5000     sgtl5000_2;     //xy=381.8453063964844,384.8571472167969
98
//AudioControlSGTL5000     sgtl5000_1;     //xy=382.2737731933594,345.14288330078125
99
// GUItool: end automatically generated code
100

  
101
//#include <SPI.h>
102

  
103
// BNO055 Register Map
104
// http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_10_Release.pdf
105
//
106
// BNO055 Page 0
107
#define BNO055_CHIP_ID          0x00    // should be 0xA0              
108
#define BNO055_ACC_ID           0x01    // should be 0xFB              
109
#define BNO055_MAG_ID           0x02    // should be 0x32              
110
#define BNO055_GYRO_ID          0x03    // should be 0x0F              
111
#define BNO055_SW_REV_ID_LSB    0x04
112
#define BNO055_SW_REV_ID_MSB    0x05
113
#define BNO055_BL_REV_ID        0x06
114
#define BNO055_PAGE_ID          0x07
115
#define BNO055_ACC_DATA_X_LSB   0x08
116
#define BNO055_ACC_DATA_X_MSB   0x09
117
#define BNO055_ACC_DATA_Y_LSB   0x0A
118
#define BNO055_ACC_DATA_Y_MSB   0x0B
119
#define BNO055_ACC_DATA_Z_LSB   0x0C
120
#define BNO055_ACC_DATA_Z_MSB   0x0D
121
#define BNO055_MAG_DATA_X_LSB   0x0E
122
#define BNO055_MAG_DATA_X_MSB   0x0F
123
#define BNO055_MAG_DATA_Y_LSB   0x10
124
#define BNO055_MAG_DATA_Y_MSB   0x11
125
#define BNO055_MAG_DATA_Z_LSB   0x12
126
#define BNO055_MAG_DATA_Z_MSB   0x13
127
#define BNO055_GYR_DATA_X_LSB   0x14
128
#define BNO055_GYR_DATA_X_MSB   0x15
129
#define BNO055_GYR_DATA_Y_LSB   0x16
130
#define BNO055_GYR_DATA_Y_MSB   0x17
131
#define BNO055_GYR_DATA_Z_LSB   0x18
132
#define BNO055_GYR_DATA_Z_MSB   0x19
133
#define BNO055_EUL_HEADING_LSB  0x1A
134
#define BNO055_EUL_HEADING_MSB  0x1B
135
#define BNO055_EUL_ROLL_LSB     0x1C
136
#define BNO055_EUL_ROLL_MSB     0x1D
137
#define BNO055_EUL_PITCH_LSB    0x1E
138
#define BNO055_EUL_PITCH_MSB    0x1F
139
#define BNO055_QUA_DATA_W_LSB   0x20
140
#define BNO055_QUA_DATA_W_MSB   0x21
141
#define BNO055_QUA_DATA_X_LSB   0x22
142
#define BNO055_QUA_DATA_X_MSB   0x23
143
#define BNO055_QUA_DATA_Y_LSB   0x24
144
#define BNO055_QUA_DATA_Y_MSB   0x25
145
#define BNO055_QUA_DATA_Z_LSB   0x26
146
#define BNO055_QUA_DATA_Z_MSB   0x27
147
#define BNO055_LIA_DATA_X_LSB   0x28
148
#define BNO055_LIA_DATA_X_MSB   0x29
149
#define BNO055_LIA_DATA_Y_LSB   0x2A
150
#define BNO055_LIA_DATA_Y_MSB   0x2B
151
#define BNO055_LIA_DATA_Z_LSB   0x2C
152
#define BNO055_LIA_DATA_Z_MSB   0x2D
153
#define BNO055_GRV_DATA_X_LSB   0x2E
154
#define BNO055_GRV_DATA_X_MSB   0x2F
155
#define BNO055_GRV_DATA_Y_LSB   0x30
156
#define BNO055_GRV_DATA_Y_MSB   0x31
157
#define BNO055_GRV_DATA_Z_LSB   0x32
158
#define BNO055_GRV_DATA_Z_MSB   0x33
159
#define BNO055_TEMP             0x34
160
#define BNO055_CALIB_STAT       0x35
161
#define BNO055_ST_RESULT        0x36
162
#define BNO055_INT_STATUS       0x37
163
#define BNO055_SYS_CLK_STATUS   0x38
164
#define BNO055_SYS_STATUS       0x39
165
#define BNO055_SYS_ERR          0x3A
166
#define BNO055_UNIT_SEL         0x3B
167
#define BNO055_OPR_MODE         0x3D
168
#define BNO055_PWR_MODE         0x3E
169
#define BNO055_SYS_TRIGGER      0x3F
170
#define BNO055_TEMP_SOURCE      0x40
171
#define BNO055_AXIS_MAP_CONFIG  0x41
172
#define BNO055_AXIS_MAP_SIGN    0x42
173
#define BNO055_ACC_OFFSET_X_LSB 0x55
174
#define BNO055_ACC_OFFSET_X_MSB 0x56
175
#define BNO055_ACC_OFFSET_Y_LSB 0x57
176
#define BNO055_ACC_OFFSET_Y_MSB 0x58
177
#define BNO055_ACC_OFFSET_Z_LSB 0x59
178
#define BNO055_ACC_OFFSET_Z_MSB 0x5A
179
#define BNO055_MAG_OFFSET_X_LSB 0x5B
180
#define BNO055_MAG_OFFSET_X_MSB 0x5C
181
#define BNO055_MAG_OFFSET_Y_LSB 0x5D
182
#define BNO055_MAG_OFFSET_Y_MSB 0x5E
183
#define BNO055_MAG_OFFSET_Z_LSB 0x5F
184
#define BNO055_MAG_OFFSET_Z_MSB 0x60
185
#define BNO055_GYR_OFFSET_X_LSB 0x61
186
#define BNO055_GYR_OFFSET_X_MSB 0x62
187
#define BNO055_GYR_OFFSET_Y_LSB 0x63
188
#define BNO055_GYR_OFFSET_Y_MSB 0x64
189
#define BNO055_GYR_OFFSET_Z_LSB 0x65
190
#define BNO055_GYR_OFFSET_Z_MSB 0x66
191
#define BNO055_ACC_RADIUS_LSB   0x67
192
#define BNO055_ACC_RADIUS_MSB   0x68
193
#define BNO055_MAG_RADIUS_LSB   0x69
194
#define BNO055_MAG_RADIUS_MSB   0x6A
195
//
196
// BNO055 Page 1
197
#define BNO055_PAGE_ID          0x07
198
#define BNO055_ACC_CONFIG       0x08
199
#define BNO055_MAG_CONFIG       0x09
200
#define BNO055_GYRO_CONFIG_0    0x0A
201
#define BNO055_GYRO_CONFIG_1    0x0B
202
#define BNO055_ACC_SLEEP_CONFIG 0x0C
203
#define BNO055_GYR_SLEEP_CONFIG 0x0D
204
#define BNO055_INT_MSK          0x0F
205
#define BNO055_INT_EN           0x10
206
#define BNO055_ACC_AM_THRES     0x11
207
#define BNO055_ACC_INT_SETTINGS 0x12
208
#define BNO055_ACC_HG_DURATION  0x13
209
#define BNO055_ACC_HG_THRESH    0x14
210
#define BNO055_ACC_NM_THRESH    0x15
211
#define BNO055_ACC_NM_SET       0x16
212
#define BNO055_GYR_INT_SETTINGS 0x17
213
#define BNO055_GYR_HR_X_SET     0x18
214
#define BNO055_GYR_DUR_X        0x19
215
#define BNO055_GYR_HR_Y_SET     0x1A
216
#define BNO055_GYR_DUR_Y        0x1B
217
#define BNO055_GYR_HR_Z_SET     0x1C
218
#define BNO055_GYR_DUR_Z        0x1D
219
#define BNO055_GYR_AM_THRESH    0x1E
220
#define BNO055_GYR_AM_SET       0x1F
221

  
222
// Using the BNO055_MS5637 breakout board/Teensy 3.1 Add-On Shield, ADO is set to 1 by default
223
#if ADO
224
#define BNO055_ADDRESS 0x29   //  Device address of BNO055 when ADO = 1
225
#define MS5637_ADDRESS   0x76   //  Address of MS5637 altimeter
226
#else
227
#define BNO055_ADDRESS 0x28   //  Device address of BNO055 when ADO = 0
228
#define MS5637_ADDRESS   0x76   //  Address of MS5637 altimeter
229
#endif
230

  
231
#define SerialDebug true      // set to true to get Serial output for debugging
232

  
233
// Set initial input parameters
234
enum Ascale {  // ACC Full Scale
235
  AFS_2G = 0,
236
  AFS_4G,
237
  AFS_8G,
238
  AFS_18G
239
};
240

  
241
enum Abw { // ACC Bandwidth
242
  ABW_7_81Hz = 0,
243
  ABW_15_63Hz,
244
  ABW_31_25Hz,
245
  ABW_62_5Hz,
246
  ABW_125Hz,
247
  ABW_250Hz,
248
  ABW_500Hz,
249
  ABW_1000Hz,    //0x07
250
};
251

  
252
enum APwrMode { // ACC Pwr Mode
253
  NormalA = 0,
254
  SuspendA,
255
  LowPower1A,
256
  StandbyA,
257
  LowPower2A,
258
  DeepSuspendA
259
};
260

  
261
enum Gscale {  // gyro full scale
262
  GFS_2000DPS = 0,
263
  GFS_1000DPS,
264
  GFS_500DPS,
265
  GFS_250DPS,
266
  GFS_125DPS    // 0x04
267
};
268

  
269
enum GPwrMode { // GYR Pwr Mode
270
  NormalG = 0,
271
  FastPowerUpG,
272
  DeepSuspendedG,
273
  SuspendG,
274
  AdvancedPowerSaveG
275
};
276

  
277
enum Gbw { // gyro bandwidth
278
  GBW_523Hz = 0,
279
  GBW_230Hz,
280
  GBW_116Hz,
281
  GBW_47Hz,
282
  GBW_23Hz,
283
  GBW_12Hz,
284
  GBW_64Hz,
285
  GBW_32Hz
286
};
287

  
288
enum OPRMode {  // BNO-55 operation modes
289
  CONFIGMODE = 0x00,
290
  // Sensor Mode
291
  ACCONLY,
292
  MAGONLY,
293
  GYROONLY,
294
  ACCMAG,
295
  ACCGYRO,
296
  MAGGYRO,
297
  AMG,            // 0x07
298
  // Fusion Mode
299
  IMU,
300
  COMPASS,
301
  M4G,
302
  NDOF_FMC_OFF,
303
  NDOF            // 0x0C
304
};
305

  
306
enum PWRMode {
307
  Normalpwr = 0,
308
  Lowpower,
309
  Suspendpwr
310
};
311

  
312
enum Modr {         // magnetometer output data rate
313
  MODR_2Hz = 0,
314
  MODR_6Hz,
315
  MODR_8Hz,
316
  MODR_10Hz,
317
  MODR_15Hz,
318
  MODR_20Hz,
319
  MODR_25Hz,
320
  MODR_30Hz
321
};
322

  
323
enum MOpMode { // MAG Op Mode
324
  LowPower = 0,
325
  Regular,
326
  EnhancedRegular,
327
  HighAccuracy
328
};
329

  
330
enum MPwrMode { // MAG power mode
331
  Normal = 0,
332
  Sleep,
333
  Suspend,
334
  ForceMode
335
};
336

  
337
#define ADC_256  0x00 // define pressure and temperature conversion rates
338
#define ADC_512  0x02
339
#define ADC_1024 0x04
340
#define ADC_2048 0x06
341
#define ADC_4096 0x08
342
#define ADC_8192 0x0A
343
#define ADC_D1   0x40
344
#define ADC_D2   0x50
345

  
346
// Specify sensor configuration
347
uint8_t OSR = ADC_8192;       // set pressure amd temperature oversample rate
348
//
349
uint8_t GPwrMode = Normal;    // Gyro power mode
350
uint8_t Gscale = GFS_250DPS;  // Gyro full scale
351
//uint8_t Godr = GODR_250Hz;    // Gyro sample rate
352
uint8_t Gbw = GBW_23Hz;       // Gyro bandwidth
353
//
354
uint8_t Ascale = AFS_2G;      // Accel full scale
355
//uint8_t Aodr = AODR_250Hz;    // Accel sample rate
356
uint8_t APwrMode = Normal;    // Accel power mode
357
uint8_t Abw = ABW_31_25Hz;    // Accel bandwidth, accel sample rate divided by ABW_divx
358
//
359
//uint8_t Mscale = MFS_4Gauss;  // Select magnetometer full-scale resolution
360
uint8_t MOpMode = HighAccuracy;    // Select magnetometer perfomance mode
361
uint8_t MPwrMode = Normal;    // Select magnetometer power mode
362
uint8_t Modr = MODR_10Hz;     // Select magnetometer ODR when in BNO055 bypass mode
363

  
364
uint8_t PWRMode = Normal ;    // Select BNO055 power mode
365
uint8_t OPRMode = NDOF;        // specify operation mode for sensors
366
uint8_t status;               // BNO055 data status register
367
float aRes, gRes, mRes;       // scale resolutions per LSB for the sensors
368

  
369
// Pin definitions
370
int intPin = 13;  // These can be changed, 2 and 3 are the Arduinos ext int pins
371
int myLed = 9;
372

  
373
uint16_t Pcal[8];         // calibration constants from MS5637 PROM registers
374
unsigned char nCRC;       // calculated check sum to ensure PROM integrity
375
uint32_t D1 = 0, D2 = 0;  // raw MS5637 pressure and temperature data
376
double dT, OFFSET, SENS, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data
377
int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
378
int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
379
int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output
380
int16_t quatCount[4];   // Stores the 16-bit signed quaternion output
381
int16_t EulCount[3];    // Stores the 16-bit signed Euler angle output
382
int16_t LIACount[3];    // Stores the 16-bit signed linear acceleration output
383
int16_t GRVCount[3];    // Stores the 16-bit signed gravity vector output
384
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0};  // Bias corrections for gyro, accelerometer, and magnetometer
385
int16_t tempGCount, tempMCount;      // temperature raw count output of mag and gyro
386
float   Gtemperature, Mtemperature;  // Stores the BNO055 gyro and LIS3MDL mag internal chip temperatures in degrees Celsius
387
double Temperature, Pressure;        // stores MS5637 pressures sensor pressure and temperature
388

  
389
// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
390
float GyroMeasError = PI * (40.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 40 deg/s)
391
float GyroMeasDrift = PI * (0.0f  / 180.0f);   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
392
// There is a tradeoff in the beta parameter between accuracy and response speed.
393
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
394
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
395
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
396
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
397
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
398
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
399
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
400
float beta = sqrt(3.0f / 4.0f) * GyroMeasError;   // compute beta
401
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
402
#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
403
#define Ki 0.0f
404

  
405
uint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate
406
float pitch, yaw, roll;
407
float Pitch, Yaw, Roll;
408
float LIAx, LIAy, LIAz, GRVx, GRVy, GRVz;
409
float deltat = 0.0f, sum = 0.0f;          // integration interval for both filter schemes
410
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
411
uint32_t Now = 0;                         // used to calculate integration interval
412

  
413
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
414
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
415
float quat[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
416
float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method
417

  
418
void setup()
419
{
420
  //  Wire.begin();
421
  //  TWBR = 12;  // 400 kbit/sec I2C speed for Pro Mini
422
  // Setup for Master mode, pins 16/17, external pullups, 400kHz for Teensy 3.1
423
#if defined(__MK20DX128__) || defined(__MK20DX256__)
424
  Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);
425
#else
426
  Wire.begin();
427
#endif
428

  
429
  strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
430
  strip.show();            // Turn OFF all pixels ASAP
431
  strip.setBrightness(100); // Set BRIGHTNESS to about 1/5 (max = 255)
432

  
433
#ifdef USE_DRV
434
  drv.selectLibrary(6);
435
  drv.useLRA();
436
  drv.setMode(DRV2605_MODE_REALTIME);
437
  pinMode(A3, OUTPUT);
438
  digitalWrite(A3, HIGH);
439
#endif
440

  
441
  delay(1000);
442
  Serial.begin(115200);
443

  
444
  // Set up the interrupt pin, its set as active high, push-pull
445
  pinMode(intPin, INPUT);
446
  pinMode(myLed, OUTPUT);
447
  digitalWrite(myLed, HIGH);
448

  
449
  /*
450
    // scan for i2c devices
451
    byte error, address;
452
    int nDevices;
453

  
454
    Serial.println("Scanning...");
455

  
456
    nDevices = 0;
457
    for(address = 1; address < 127; address++ )
458
    {
459
      // The i2c_scanner uses the return value of
460
      // the Write.endTransmisstion to see if
461
      // a device did acknowledge to the address.
462
      Wire.beginTransmission(address);
463
      error = Wire.endTransmission();
464

  
465
      if (error == 0)
466
      {
467
        Serial.print("I2C device found at address 0x");
468
        if (address<16)
469
          Serial.print("0");
470
        Serial.print(address,HEX);
471
        Serial.println("  !");
472

  
473
        nDevices++;
474
      }
475
      else if (error==4)
476
      {
477
        Serial.print("Unknow error at address 0x");
478
        if (address<16)
479
          Serial.print("0");
480
        Serial.println(address,HEX);
481
      }
482
    }
483
    if (nDevices == 0)
484
      Serial.println("No I2C devices found\n");
485
    else
486
      Serial.println("done\n");
487

  
488
  */
489

  
490
  // Read the WHO_AM_I register, this is a good test of communication
491
  Serial.println("BNO055 9-axis motion sensor...");
492
  byte c = readByte(BNO055_ADDRESS, BNO055_CHIP_ID);  // Read WHO_AM_I register for BNO055
493
  Serial.print("BNO055 Address = 0x"); Serial.println(BNO055_ADDRESS, HEX);
494
  Serial.print("BNO055 WHO_AM_I = 0x"); Serial.println(BNO055_CHIP_ID, HEX);
495
  Serial.print("BNO055 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.println(" I should be 0xA0");
496

  
497
  delay(500);
498

  
499
  // Read the WHO_AM_I register of the accelerometer, this is a good test of communication
500
  byte d = readByte(BNO055_ADDRESS, BNO055_ACC_ID);  // Read WHO_AM_I register for accelerometer
501
  Serial.print("BNO055 ACC "); Serial.print("I AM "); Serial.print(d, HEX); Serial.println(" I should be 0xFB");
502

  
503
  delay(500);
504

  
505
  // Read the WHO_AM_I register of the magnetometer, this is a good test of communication
506
  byte e = readByte(BNO055_ADDRESS, BNO055_MAG_ID);  // Read WHO_AM_I register for magnetometer
507
  Serial.print("BNO055 MAG "); Serial.print("I AM "); Serial.print(e, HEX); Serial.println(" I should be 0x32");
508

  
509
  delay(500);
510

  
511
  // Read the WHO_AM_I register of the gyroscope, this is a good test of communication
512
  byte f = readByte(BNO055_ADDRESS, BNO055_GYRO_ID);  // Read WHO_AM_I register for LIS3MDL
513
  Serial.print("BNO055 GYRO "); Serial.print("I AM "); Serial.print(f, HEX); Serial.println(" I should be 0x0F");
514

  
515
  delay(500);
516

  
517
  if (c == 0xA0) // BNO055 WHO_AM_I should always be 0xA0
518
  {
519
    Serial.println("BNO055 is online...");
520

  
521
    // Check software revision ID
522
    byte swlsb = readByte(BNO055_ADDRESS, BNO055_SW_REV_ID_LSB);
523
    byte swmsb = readByte(BNO055_ADDRESS, BNO055_SW_REV_ID_MSB);
524
    Serial.print("BNO055 SW Revision ID: "); Serial.print(swmsb, HEX); Serial.print("."); Serial.println(swlsb, HEX);
525
    Serial.println("Should be 03.04");
526

  
527
    // Check bootloader version
528
    byte blid = readByte(BNO055_ADDRESS, BNO055_BL_REV_ID);
529
    Serial.print("BNO055 bootloader Version: "); Serial.println(blid);
530

  
531
    // Check self-test results
532
    byte selftest = readByte(BNO055_ADDRESS, BNO055_ST_RESULT);
533

  
534
    if (selftest & 0x01) {
535
      Serial.println("accelerometer passed selftest");
536
    } else {
537
      Serial.println("accelerometer failed selftest");
538
    }
539
    if (selftest & 0x02) {
540
      Serial.println("magnetometer passed selftest");
541
    } else {
542
      Serial.println("magnetometer failed selftest");
543
    }
544
    if (selftest & 0x04) {
545
      Serial.println("gyroscope passed selftest");
546
    } else {
547
      Serial.println("gyroscope failed selftest");
548
    }
549
    if (selftest & 0x08) {
550
      Serial.println("MCU passed selftest");
551
    } else {
552
      Serial.println("MCU failed selftest");
553
    }
554

  
555
    delay(500);
556

  
557
    //delay(500);
558

  
559
    //accelgyroCalBNO055(accelBias, gyroBias);
560

  
561
    // Serial.println("Average accelerometer bias (mg) = "); Serial.println(accelBias[0]); Serial.println(accelBias[1]); Serial.println(accelBias[2]);
562
    //  Serial.println("Average gyro bias (dps) = "); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);
563

  
564
    //delay(500);
565

  
566
    //magCalBNO055(magBias);
567

  
568
    //Serial.println("Average magnetometer bias (mG) = "); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]);
569

  
570
    // delay(500);
571

  
572
    // Check calibration status of the sensors
573
    uint8_t calstat = readByte(BNO055_ADDRESS, BNO055_CALIB_STAT);
574
    Serial.println("Not calibrated = 0, fully calibrated = 3");
575
    Serial.print("System calibration status "); Serial.println( (0xC0 & calstat) >> 6);
576
    Serial.print("Gyro   calibration status "); Serial.println( (0x30 & calstat) >> 4);
577
    Serial.print("Accel  calibration status "); Serial.println( (0x0C & calstat) >> 2);
578
    Serial.print("Mag    calibration status "); Serial.println( (0x03 & calstat) >> 0);
579

  
580
    initBNO055(); // Initialize the BNO055
581
    Serial.println("BNO055 initialized for sensor mode...."); // Initialize BNO055 for sensor read
582

  
583
  }
584
  else
585
  {
586
    Serial.print("Could not connect to BNO055: 0x");
587
    Serial.println(c, HEX);
588
    while (1) ; // Loop forever if communication doesn't happen
589
  }
590
}
591

  
592
void loop()
593
{
594

  
595
  readAccelData(accelCount);  // Read the x/y/z adc values
596
  // Now we'll calculate the accleration value into actual mg's
597
  ax = (float)accelCount[0] - accelBias[0];  // subtract off calculated accel bias
598
  ay = (float)accelCount[1] - accelBias[1];
599
  az = (float)accelCount[2] - accelBias[2];
600

  
601
  readGyroData(gyroCount);  // Read the x/y/z adc values
602
  // Calculate the gyro value into actual degrees per second
603
  gx = (float)gyroCount[0] / 16. - gyroBias[0]; // subtract off calculated gyro bias
604
  gy = (float)gyroCount[1] / 16. - gyroBias[1];
605
  gz = (float)gyroCount[2] / 16. - gyroBias[2];
606

  
607
  readMagData(magCount);  // Read the x/y/z adc values
608
  // Calculate the magnetometer values in milliGauss
609
  mx = (float)magCount[0] / 1.6 - magBias[0]; // get actual magnetometer value in mGauss
610
  my = (float)magCount[1] / 1.6 - magBias[1];
611
  mz = (float)magCount[2] / 1.6 - magBias[2];
612

  
613
  readQuatData(quatCount);  // Read the x/y/z adc values
614
  // Calculate the quaternion values
615
  quat[0] = (float)(quatCount[0]) / 16384.;
616
  quat[1] = (float)(quatCount[1]) / 16384.;
617
  quat[2] = (float)(quatCount[2]) / 16384.;
618
  quat[3] = (float)(quatCount[3]) / 16384.;
619

  
620
  readEulData(EulCount);  // Read the x/y/z adc values
621
  // Calculate the Euler angles values in degrees
622
  Yaw = (float)EulCount[0] / 16.;
623
  Roll = (float)EulCount[1] / 16.;
624
  Pitch = (float)EulCount[2] / 16.;
625

  
626
  readLIAData(LIACount);  // Read the x/y/z adc values
627
  // Calculate the linear acceleration (sans gravity) values in mg
628
  LIAx = (float)LIACount[0];
629
  LIAy = (float)LIACount[1];
630
  LIAz = (float)LIACount[2];
631

  
632
  readGRVData(GRVCount);  // Read the x/y/z adc values
633
  // Calculate the linear acceleration (sans gravity) values in mg
634
  GRVx = (float)GRVCount[0];
635
  GRVy = (float)GRVCount[1];
636
  GRVz = (float)GRVCount[2];
637

  
638
  Now = micros();
639
  deltat = ((Now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update
640
  lastUpdate = Now;
641

  
642
  sum += deltat; // sum for averaging filter update rate
643
  sumCount++;
644

  
645
  // Sensors x, y, and z-axes  for the three sensor: accel, gyro, and magnetometer are all aligned, so
646
  // no allowance for any orientation mismatch in feeding the output to the quaternion filter is required.
647
  // For the BNO055, the sensor forward is along the x-axis just like
648
  // in the LSM9DS0 and MPU9250 sensors. This rotation can be modified to allow any convenient orientation convention.
649
  // This is ok by aircraft orientation standards!
650
  // Pass gyro rate as rad/s
651
  // MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  mx,  my,  mz);
652
  //  MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
653

  
654
  // Serial print and/or display at 0.5 s rate independent of data rates
655
  delt_t = millis() - count;
656
  if (delt_t > 100) { // update LCD once per half-second independent of read rate
657

  
658
    // check BNO-055 error status at 2 Hz rate
659
    uint8_t errstat = readByte(BNO055_ADDRESS, BNO055_CALIB_STAT);
660
    if (errstat == 0x01) {
661
      uint8_t syserr = readByte(BNO055_ADDRESS, BNO055_SYS_ERR);
662
      if (syserr == 0x01) Serial.println("Peripheral initialization error");
663
      if (syserr == 0x02) Serial.println("System initialization error");
664
      if (syserr == 0x03) Serial.println("Self test result failed");
665
      if (syserr == 0x04) Serial.println("Register map value out of range");
666
      if (syserr == 0x05) Serial.println("Register map address out of range");
667
      if (syserr == 0x06) Serial.println("Register map write error");
668
      if (syserr == 0x07) Serial.println("BNO low power mode no available for selected operation mode");
669
      if (syserr == 0x08) Serial.println("Accelerometer power mode not available");
670
      if (syserr == 0x09) Serial.println("Fusion algorithm configuration error");
671
      if (syserr == 0x0A) Serial.println("Sensor configuration error");
672
    }
673

  
674
    //    Serial.print("ax = "); Serial.print((int)ax);
675
    //    Serial.print(" ay = "); Serial.print((int)ay);
676
    //    Serial.print(" az = "); Serial.print((int)az); Serial.println(" mg");
677
    //    Serial.print("gx = "); Serial.print( gx, 2);
678
    //    Serial.print(" gy = "); Serial.print( gy, 2);
679
    //    Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
680
    //    Serial.print("mx = "); Serial.print( (int)mx );
681
    //    Serial.print(" my = "); Serial.print( (int)my );
682
    //    Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
683
    //
684
    //    Serial.print("qx = "); Serial.print(q[0]);
685
    //    Serial.print(" qy = "); Serial.print(q[1]);
686
    //    Serial.print(" qz = "); Serial.print(q[2]);
687
    //    Serial.print(" qw = "); Serial.println(q[3]);
688
    //    Serial.print("quatw = "); Serial.print(quat[0]);
689
    //    Serial.print(" quatx = "); Serial.print(quat[1]);
690
    //    Serial.print(" quaty = "); Serial.print(quat[2]);
691
    //    Serial.print(" quatz = "); Serial.println(quat[3]);
692

  
693
    tempGCount = readGyroTempData();  // Read the gyro adc values
694
    Gtemperature = (float) tempGCount; // Gyro chip temperature in degrees Centigrade
695
    // Print gyro die temperature in degrees Centigrade
696
    //    Serial.print("Gyro temperature is ");  Serial.print(Gtemperature, 1);  Serial.println(" degrees C"); // Print T values to tenths of a degree C
697

  
698
    // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
699
    // In this coordinate system, the positive z-axis is down toward Earth.
700
    // 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.
701
    // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
702
    // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
703
    // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
704
    // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
705
    // applied in the correct order which for this configuration is yaw, pitch, and then roll.
706
    // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
707
    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]);
708
    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
709
    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]);
710
    pitch *= 180.0f / PI;
711
    yaw   *= 180.0f / PI;
712
    //   yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
713
    roll  *= 180.0f / PI;
714

  
715
    //    Serial.print("Software Yaw, Pitch, Roll: ");
716
    //    Serial.print(yaw, 2);
717
    //    Serial.print(", ");
718
    //    Serial.print(pitch, 2);
719
    //    Serial.print(", ");
720
    //    Serial.println(roll, 2);
721
    //
722
    //    Serial.print("Hardware Yaw, Pitch, Roll: ");
723
    //    Serial.print(Yaw, 2);
724
    //    Serial.print(", ");
725
    //    Serial.print(Pitch, 2);
726
    //    Serial.print(", ");
727
    //    Serial.println(Roll, 2);
728
    //
729
    //    Serial.print("Hardware x, y, z linear acceleration: ");
730
    //    Serial.print(LIAx, 2);
731
    //    Serial.print(", ");
732
    //    Serial.print(LIAy, 2);
733
    //    Serial.print(", ");
734
    //    Serial.println(LIAz, 2);
735

  
736
    Serial.print("Hardware x, y, z gravity vector: ");
737
    Serial.print(GRVx, 2);
738
    Serial.print(", ");
739
    Serial.print(GRVy, 2);
740
    Serial.print(", ");
741
    Serial.println(GRVz, 2);
742

  
743
    Serial.print("rate = "); Serial.print((float)sumCount / sum, 2); Serial.println(" Hz");
744
    uint8_t r = map (GRVx, -1000, 1000, 0, 255);
745
    uint8_t g = map (GRVy, -1000, 1000, 0, 255);
746
    uint8_t b = map (GRVz, -1000, 1000, 0, 255);
747
    Serial.print("LED Data: ");
748
    Serial.print(r);
749
    Serial.print(", ");
750
    Serial.print(g);
751
    Serial.print(", ");
752
    Serial.println(b);
753
    strip.setPixelColor(0, r, g, b); // Set pixel 'c' to value 'color'
754
    strip.setPixelColor(1, 255-r, 255-g, 255-b); // Set pixel 'c' to value 'color'
755
    strip.show();            // Turn OFF all pixels ASAP
756

  
757
    digitalWrite(myLed, !digitalRead(myLed));
758
    count = millis();
759
    sumCount = 0;
760
    sum = 0;
761

  
762
#ifdef USE_DRV
763
    drv.setRealtimeValue(map(GRVx, -1000, 1000, 0, 127));
764
#endif
765
  }
766

  
767
}
768

  
769
//===================================================================================================================
770
//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data
771
//===================================================================================================================
772

  
773
void readAccelData(int16_t * destination)
774
{
775
  uint8_t rawData[6];  // x/y/z accel register data stored here
776
  readBytes(BNO055_ADDRESS, BNO055_ACC_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers into data array
777
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;      // Turn the MSB and LSB into a signed 16-bit value
778
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
779
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
780
}
781

  
782
void readGyroData(int16_t * destination)
783
{
784
  uint8_t rawData[6];  // x/y/z gyro register data stored here
785
  readBytes(BNO055_ADDRESS, BNO055_GYR_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
786
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
787
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
788
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
789
}
790

  
791
int8_t readGyroTempData()
792
{
793
  return readByte(BNO055_ADDRESS, BNO055_TEMP);  // Read the two raw data registers sequentially into data array
794
}
795

  
796
void readMagData(int16_t * destination)
797
{
798
  uint8_t rawData[6];  // x/y/z gyro register data stored here
799
  readBytes(BNO055_ADDRESS, BNO055_MAG_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
800
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
801
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
802
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
803
}
804

  
805
void readQuatData(int16_t * destination)
806
{
807
  uint8_t rawData[8];  // x/y/z gyro register data stored here
808
  readBytes(BNO055_ADDRESS, BNO055_QUA_DATA_W_LSB, 8, &rawData[0]);  // Read the six raw data registers sequentially into data array
809
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
810
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
811
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
812
  destination[3] = ((int16_t)rawData[7] << 8) | rawData[6] ;
813
}
814

  
815
void readEulData(int16_t * destination)
816
{
817
  uint8_t rawData[6];  // x/y/z gyro register data stored here
818
  readBytes(BNO055_ADDRESS, BNO055_EUL_HEADING_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
819
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
820
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
821
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
822
}
823

  
824
void readLIAData(int16_t * destination)
825
{
826
  uint8_t rawData[6];  // x/y/z gyro register data stored here
827
  readBytes(BNO055_ADDRESS, BNO055_LIA_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
828
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
829
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
830
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
831
}
832

  
833
void readGRVData(int16_t * destination)
834
{
835
  uint8_t rawData[6];  // x/y/z gyro register data stored here
836
  readBytes(BNO055_ADDRESS, BNO055_GRV_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
837
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
838
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
839
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
840
}
841

  
842
void initBNO055() {
843
  // Select page 1 to configure sensors
844
  writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x01);
845
  // Configure ACC
846
  writeByte(BNO055_ADDRESS, BNO055_ACC_CONFIG, APwrMode << 5 | Abw << 3 | Ascale );
847
  // Configure GYR
848
  writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_0, Gbw << 3 | Gscale );
849
  writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_1, GPwrMode);
850
  // Configure MAG
851
  writeByte(BNO055_ADDRESS, BNO055_MAG_CONFIG, MPwrMode << 4 | MOpMode << 2 | Modr );
852

  
853
  // Select page 0 to read sensors
854
  writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);
855

  
856
  // Select BNO055 gyro temperature source
857
  writeByte(BNO055_ADDRESS, BNO055_TEMP_SOURCE, 0x01 );
858

  
859
  // Select BNO055 sensor units (temperature in degrees C, rate in dps, accel in mg)
860
  writeByte(BNO055_ADDRESS, BNO055_UNIT_SEL, 0x01 );
861

  
862
  // Select BNO055 system power mode
863
  writeByte(BNO055_ADDRESS, BNO055_PWR_MODE, PWRMode );
864

  
865
  // Select BNO055 system operation mode
866
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );
867
}
868

  
869
void accelgyroCalBNO055(float * dest1, float * dest2)
870
{
871
  uint8_t data[6]; // data array to hold accelerometer and gyro x, y, z, data
872
  uint16_t ii = 0, sample_count = 0;
873
  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
874

  
875
  Serial.println("Accel/Gyro Calibration: Put device on a level surface and keep motionless! Wait......");
876
  delay(1000);
877

  
878
  // Select page 0 to read sensors
879
  writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);
880
  // Select BNO055 system operation mode as NDOF for calibration
881
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
882
  delay(25);
883
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, NDOF );
884

  
885
  // In NDF fusion mode, accel full scale is at +/- 4g, ODR is 62.5 Hz
886
  sample_count = 256;
887
  for (ii = 0; ii < sample_count; ii++) {
888
    int16_t accel_temp[3] = {0, 0, 0};
889
    readBytes(BNO055_ADDRESS, BNO055_ACC_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array
890
    accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO
891
    accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
892
    accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
893
    accel_bias[0]  += (int32_t) accel_temp[0];
894
    accel_bias[1]  += (int32_t) accel_temp[1];
895
    accel_bias[2]  += (int32_t) accel_temp[2];
896
    delay(20);  // at 62.5 Hz ODR, new accel data is available every 16 ms
897
  }
898
  accel_bias[0]  /= (int32_t) sample_count;  // get average accel bias in mg
899
  accel_bias[1]  /= (int32_t) sample_count;
900
  accel_bias[2]  /= (int32_t) sample_count;
901

  
902
  if (accel_bias[2] > 0L) {
903
    accel_bias[2] -= (int32_t) 1000; // Remove gravity from the z-axis accelerometer bias calculation
904
  }
905
  else {
906
    accel_bias[2] += (int32_t) 1000;
907
  }
908

  
909
  dest1[0] = (float) accel_bias[0];  // save accel biases in mg for use in main program
910
  dest1[1] = (float) accel_bias[1];  // accel data is 1 LSB/mg
911
  dest1[2] = (float) accel_bias[2];
912

  
913
  // In NDF fusion mode, gyro full scale is at +/- 2000 dps, ODR is 32 Hz
914
  for (ii = 0; ii < sample_count; ii++) {
915
    int16_t gyro_temp[3] = {0, 0, 0};
916
    readBytes(BNO055_ADDRESS, BNO055_GYR_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array
917
    gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ;  // Form signed 16-bit integer for each sample in FIFO
918
    gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
919
    gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
920
    gyro_bias[0]  += (int32_t) gyro_temp[0];
921
    gyro_bias[1]  += (int32_t) gyro_temp[1];
922
    gyro_bias[2]  += (int32_t) gyro_temp[2];
923
    delay(35);  // at 32 Hz ODR, new gyro data available every 31 ms
924
  }
925
  gyro_bias[0]  /= (int32_t) sample_count;  // get average gyro bias in counts
926
  gyro_bias[1]  /= (int32_t) sample_count;
927
  gyro_bias[2]  /= (int32_t) sample_count;
928

  
929
  dest2[0] = (float) gyro_bias[0] / 16.; // save gyro biases in dps for use in main program
930
  dest2[1] = (float) gyro_bias[1] / 16.; // gyro data is 16 LSB/dps
931
  dest2[2] = (float) gyro_bias[2] / 16.;
932

  
933
  // Return to config mode to write accelerometer biases in offset register
934
  // This offset register is only used while in fusion mode when accelerometer full-scale is +/- 4g
935
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
936
  delay(25);
937

  
938
  //write biases to accelerometer offset registers ad 16 LSB/dps
939
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_LSB, (int16_t)accel_bias[0] & 0xFF);
940
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_MSB, ((int16_t)accel_bias[0] >> 8) & 0xFF);
941
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_LSB, (int16_t)accel_bias[1] & 0xFF);
942
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_MSB, ((int16_t)accel_bias[1] >> 8) & 0xFF);
943
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_LSB, (int16_t)accel_bias[2] & 0xFF);
944
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_MSB, ((int16_t)accel_bias[2] >> 8) & 0xFF);
945

  
946
  // Check that offsets were properly written to offset registers
947
  //  Serial.println("Average accelerometer bias = ");
948
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_LSB)));
949
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_LSB)));
950
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_LSB)));
951

  
952
  //write biases to gyro offset registers
953
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_LSB, (int16_t)gyro_bias[0] & 0xFF);
954
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_MSB, ((int16_t)gyro_bias[0] >> 8) & 0xFF);
955
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_LSB, (int16_t)gyro_bias[1] & 0xFF);
956
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_MSB, ((int16_t)gyro_bias[1] >> 8) & 0xFF);
957
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_LSB, (int16_t)gyro_bias[2] & 0xFF);
958
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_MSB, ((int16_t)gyro_bias[2] >> 8) & 0xFF);
959

  
960
  // Select BNO055 system operation mode
961
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );
962

  
963
  // Check that offsets were properly written to offset registers
964
  //  Serial.println("Average gyro bias = ");
965
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_LSB)));
966
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_LSB)));
967
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_LSB)));
968

  
969
  Serial.println("Accel/Gyro Calibration done!");
970
}
971

  
972
void magCalBNO055(float * dest1)
973
{
974
  uint8_t data[6]; // data array to hold accelerometer and gyro x, y, z, data
975
  uint16_t ii = 0, sample_count = 0;
976
  int32_t mag_bias[3] = {0, 0, 0};
977
  int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0};
978

  
979
  Serial.println("Mag Calibration: Wave device in a figure eight until done!");
980
  delay(4000);
981

  
982
  // Select page 0 to read sensors
983
  writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);
984
  // Select BNO055 system operation mode as NDOF for calibration
985
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
986
  delay(25);
987
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, NDOF );
988

  
989
  // In NDF fusion mode, mag data is in 16 LSB/microTesla, ODR is 20 Hz in forced mode
990
  sample_count = 256;
991
  for (ii = 0; ii < sample_count; ii++) {
992
    int16_t mag_temp[3] = {0, 0, 0};
993
    readBytes(BNO055_ADDRESS, BNO055_MAG_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array
994
    mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ;   // Form signed 16-bit integer for each sample in FIFO
995
    mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
996
    mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
997
    for (int jj = 0; jj < 3; jj++) {
998
      if (mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
999
      if (mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
1000
    }
1001
    delay(55);  // at 20 Hz ODR, new mag data is available every 50 ms
1002
  }
1003

  
1004
  //   Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]);
1005
  //   Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]);
1006
  //   Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);
1007

  
1008
  mag_bias[0]  = (mag_max[0] + mag_min[0]) / 2; // get average x mag bias in counts
1009
  mag_bias[1]  = (mag_max[1] + mag_min[1]) / 2; // get average y mag bias in counts
1010
  mag_bias[2]  = (mag_max[2] + mag_min[2]) / 2; // get average z mag bias in counts
1011

  
1012
  dest1[0] = (float) mag_bias[0] / 1.6;  // save mag biases in mG for use in main program
1013
  dest1[1] = (float) mag_bias[1] / 1.6;  // mag data is 1.6 LSB/mg
1014
  dest1[2] = (float) mag_bias[2] / 1.6;
1015

  
1016
  // Return to config mode to write mag biases in offset register
1017
  // This offset register is only used while in fusion mode when magnetometer sensitivity is 16 LSB/microTesla
1018
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
1019
  delay(25);
1020

  
1021
  //write biases to accelerometer offset registers as 16 LSB/microTesla
1022
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_LSB, (int16_t)mag_bias[0] & 0xFF);
1023
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_MSB, ((int16_t)mag_bias[0] >> 8) & 0xFF);
1024
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_LSB, (int16_t)mag_bias[1] & 0xFF);
1025
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_MSB, ((int16_t)mag_bias[1] >> 8) & 0xFF);
1026
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_LSB, (int16_t)mag_bias[2] & 0xFF);
1027
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_MSB, ((int16_t)mag_bias[2] >> 8) & 0xFF);
1028

  
1029
  // Check that offsets were properly written to offset registers
1030
  //  Serial.println("Average magnetometer bias = ");
1031
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_LSB)));
1032
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_LSB)));
1033
  //  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_LSB)));
1034
  // Select BNO055 system operation mode
1035
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );
1036

  
1037
  Serial.println("Mag Calibration done!");
1038
}
1039

  
1040
// I2C communication with the MS5637 is a little different from that with the BNO055 and most other sensors
1041
// For the MS5637, we write commands, and the MS5637 sends data in response, rather than directly reading
1042
// MS5637 registers
1043

  
1044
// I2C read/write functions for the BNO055 sensor
1045

  
1046
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
1047
{
1048
  Wire.beginTransmission(address);  // Initialize the Tx buffer
1049
  Wire.write(subAddress);           // Put slave register address in Tx buffer
1050
  Wire.write(data);                 // Put data in Tx buffer
1051
  Wire.endTransmission();           // Send the Tx buffer
1052
}
1053

  
1054
uint8_t readByte(uint8_t address, uint8_t subAddress)
1055
{
1056
  uint8_t data; // `data` will store the register data
1057
  Wire.beginTransmission(address);         // Initialize the Tx buffer
1058
  Wire.write(subAddress);                     // Put slave register address in Tx buffer
1059
  //    Wire.endTransmission(I2C_NOSTOP);        // Send the Tx buffer, but send a restart to keep connection alive
1060
  Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
1061
  //    Wire.requestFrom(address, 1);  // Read one byte from slave register address
1062
  Wire.requestFrom(address, (size_t) 1);   // Read one byte from slave register address
1063
  data = Wire.read();                      // Fill Rx buffer with result
1064
  return data;                             // Return data read from slave register
1065
}
1066

  
1067
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
1068
{
1069
  Wire.beginTransmission(address);   // Initialize the Tx buffer
1070
  Wire.write(subAddress);            // Put slave register address in Tx buffer
1071
  //    Wire.endTransmission(I2C_NOSTOP);  // Send the Tx buffer, but send a restart to keep connection alive
1072
  Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
1073
  uint8_t i = 0;
1074
  //        Wire.requestFrom(address, count);  // Read bytes from slave register address
1075
  Wire.requestFrom(address, (size_t) count);  // Read bytes from slave register address
1076
  while (Wire.available()) {
1077
    dest[i++] = Wire.read();
1078
  }         // Put read results in the Rx buffer
1079
}
1080

  
firmware/demo/FeatherBase-DRV2605-BasicEffects/FeatherBase-DRV2605-BasicEffects.ino
1
#include <Wire.h>
2
#include "Adafruit_DRV2605.h"
3

  
4
// Play all effects of Immersion Library on DRV2605(L)
5
// based on basic example of Adafruit DRV2605 library
6

  
7
// Comment following line for ERM support
8
#define USE_LRA 1
9

  
10
Adafruit_DRV2605 drv;
11

  
12
void setup() {
13
  Serial.begin(9600);
14
  Serial.println("DRV test");
15

  
16
  // Enable DRV2605
17
  pinMode(A1, OUTPUT);
18
  digitalWrite(A1, HIGH);
19
  
20
  drv.begin();
21
#ifdef USE_LRA
22
  drv.selectLibrary(6);
23
  drv.useLRA();
24
#elif
25
  drv.selectLibrary(1);
26
#endif
27

  
28
  // I2C trigger by sending 'go' command 
29
  // default, internal trigger when sending GO command
30
  drv.setMode(DRV2605_MODE_INTTRIG); 
31
}
32

  
33
uint8_t effect = 1;
34

  
35
void loop() {
36
  Serial.print("Effect #"); Serial.println(effect);
37

  
38
  // set the effect to play
39
  drv.setWaveform(0, effect);  // play effect 
40
  drv.setWaveform(1, 0);       // end waveform
41

  
42
  // play the effect!
43
  drv.go();
44

  
45
  // wait a bit
46
  delay(500);
47

  
48
  effect++;
49
  if (effect > 117) effect = 1;
50
}
firmware/demo/FeatherBase-RGB_LED_Test/FeatherBase-RGB_LED_Test.ino
1
// A basic everyday NeoPixel strip test program.
2

  
3
// NEOPIXEL BEST PRACTICES for most reliable operation:
4
// - Add 1000 uF CAPACITOR between NeoPixel strip's + and - connections.
5
// - MINIMIZE WIRING LENGTH between microcontroller board and first pixel.
6
// - NeoPixel strip's DATA-IN should pass through a 300-500 OHM RESISTOR.
7
// - AVOID connecting NeoPixels on a LIVE CIRCUIT. If you must, ALWAYS
8
//   connect GROUND (-) first, then +, then data.
9
// - When using a 3.3V microcontroller with a 5V-powered NeoPixel strip,
10
//   a LOGIC-LEVEL CONVERTER on the data line is STRONGLY RECOMMENDED.
11
// (Skipping these may work OK on your workbench but can fail in the field)
12

  
13
#include <Adafruit_NeoPixel.h>
14
#ifdef __AVR__
15
 #include <avr/power.h> // Required for 16 MHz Adafruit Trinket
16
#endif
17

  
18
// Which pin on the Arduino is connected to the NeoPixels?
19
// On ESP32, A3 is not output capable;
20
// quick fix is to hardware connect (e.g.) A1 to A3 and use A1
21
#define LED_PIN    A1
22

  
23
// How many NeoPixels are attached to the Arduino?
24
#define LED_COUNT 2
25

  
26
// Declare our NeoPixel strip object:
27
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);
28
// Argument 1 = Number of pixels in NeoPixel strip
29
// Argument 2 = Arduino pin number (most are valid)
30
// Argument 3 = Pixel type flags, add together as needed:
31
//   NEO_KHZ800  800 KHz bitstream (most NeoPixel products w/WS2812 LEDs)
32
//   NEO_KHZ400  400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers)
33
//   NEO_GRB     Pixels are wired for GRB bitstream (most NeoPixel products)
34
//   NEO_RGB     Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2)
35
//   NEO_RGBW    Pixels are wired for RGBW bitstream (NeoPixel RGBW products)
36

  
37

  
38
// setup() function -- runs once at startup --------------------------------
39

  
40
void setup() {
41
  // These lines are specifically to support the Adafruit Trinket 5V 16 MHz.
42
  // Any other board, you can remove this part (but no harm leaving it):
43
#if defined(__AVR_ATtiny85__) && (F_CPU == 16000000)
44
  clock_prescale_set(clock_div_1);
45
#endif
46
  // END of Trinket-specific code.
47

  
48
  strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)
49
  strip.show();            // Turn OFF all pixels ASAP
50
  strip.setBrightness(50); // Set BRIGHTNESS to about 1/5 (max = 255)
51
}
52

  
53

  
54
// loop() function -- runs repeatedly as long as board is on ---------------
55

  
56
void loop() {
57
  // Fill along the length of the strip in various colors...
58
  colorWipe(strip.Color(255,   0,   0), 50); // Red
59
  colorWipe(strip.Color(  0, 255,   0), 50); // Green
60
  colorWipe(strip.Color(  0,   0, 255), 50); // Blue
61

  
62
  // Do a theater marquee effect in various colors...
63
  theaterChase(strip.Color(127, 127, 127), 50); // White, half brightness
64
  theaterChase(strip.Color(127,   0,   0), 50); // Red, half brightness
65
  theaterChase(strip.Color(  0,   0, 127), 50); // Blue, half brightness
66

  
67
  rainbow(10);             // Flowing rainbow cycle along the whole strip
68
  theaterChaseRainbow(50); // Rainbow-enhanced theaterChase variant
69
}
70

  
71

  
72
// Some functions of our own for creating animated effects -----------------
73

  
74
// Fill strip pixels one after another with a color. Strip is NOT cleared
75
// first; anything there will be covered pixel by pixel. Pass in color
76
// (as a single 'packed' 32-bit value, which you can get by calling
77
// strip.Color(red, green, blue) as shown in the loop() function above),
78
// and a delay time (in milliseconds) between pixels.
79
void colorWipe(uint32_t color, int wait) {
80
  for(int i=0; i<strip.numPixels(); i++) { // For each pixel in strip...
81
    strip.setPixelColor(i, color);         //  Set pixel's color (in RAM)
82
    strip.show();                          //  Update strip to match
83
    delay(wait);                           //  Pause for a moment
84
  }
85
}
86

  
87
// Theater-marquee-style chasing lights. Pass in a color (32-bit value,
88
// a la strip.Color(r,g,b) as mentioned above), and a delay time (in ms)
89
// between frames.
90
void theaterChase(uint32_t color, int wait) {
91
  for(int a=0; a<10; a++) {  // Repeat 10 times...
92
    for(int b=0; b<3; b++) { //  'b' counts from 0 to 2...
93
      strip.clear();         //   Set all pixels in RAM to 0 (off)
94
      // 'c' counts up from 'b' to end of strip in steps of 3...
95
      for(int c=b; c<strip.numPixels(); c += 3) {
96
        strip.setPixelColor(c, color); // Set pixel 'c' to value 'color'
97
      }
98
      strip.show(); // Update strip with new contents
99
      delay(wait);  // Pause for a moment
100
    }
101
  }
102
}
103

  
104
// Rainbow cycle along whole strip. Pass delay time (in ms) between frames.
105
void rainbow(int wait) {
106
  // Hue of first pixel runs 5 complete loops through the color wheel.
107
  // Color wheel has a range of 65536 but it's OK if we roll over, so
108
  // just count from 0 to 5*65536. Adding 256 to firstPixelHue each time
109
  // means we'll make 5*65536/256 = 1280 passes through this outer loop:
110
  for(long firstPixelHue = 0; firstPixelHue < 5*65536; firstPixelHue += 256) {
111
    for(int i=0; i<strip.numPixels(); i++) { // For each pixel in strip...
112
      // Offset pixel hue by an amount to make one full revolution of the
113
      // color wheel (range of 65536) along the length of the strip
114
      // (strip.numPixels() steps):
115
      int pixelHue = firstPixelHue + (i * 65536L / strip.numPixels());
116
      // strip.ColorHSV() can take 1 or 3 arguments: a hue (0 to 65535) or
117
      // optionally add saturation and value (brightness) (each 0 to 255).
118
      // Here we're using just the single-argument hue variant. The result
119
      // is passed through strip.gamma32() to provide 'truer' colors
120
      // before assigning to each pixel:
121
      strip.setPixelColor(i, strip.gamma32(strip.ColorHSV(pixelHue)));
122
    }
123
    strip.show(); // Update strip with new contents
124
    delay(wait);  // Pause for a moment
125
  }
126
}
127

  
128
// Rainbow-enhanced theater marquee. Pass delay time (in ms) between frames.
129
void theaterChaseRainbow(int wait) {
130
  int firstPixelHue = 0;     // First pixel starts at red (hue 0)
131
  for(int a=0; a<30; a++) {  // Repeat 30 times...
132
    for(int b=0; b<3; b++) { //  'b' counts from 0 to 2...
133
      strip.clear();         //   Set all pixels in RAM to 0 (off)
134
      // 'c' counts up from 'b' to end of strip in increments of 3...
135
      for(int c=b; c<strip.numPixels(); c += 3) {
136
        // hue of pixel 'c' is offset by an amount to make one full
137
        // revolution of the color wheel (range 65536) along the length
138
        // of the strip (strip.numPixels() steps):
139
        int      hue   = firstPixelHue + c * 65536L / strip.numPixels();
140
        uint32_t color = strip.gamma32(strip.ColorHSV(hue)); // hue -> RGB
141
        strip.setPixelColor(c, color); // Set pixel 'c' to value 'color'
142
      }
143
      strip.show();                // Update strip with new contents
144
      delay(wait);                 // Pause for a moment
145
      firstPixelHue += 65536 / 90; // One cycle of color wheel over 90 frames
146
    }
147
  }
148
}

Also available in: Unified diff