BRIX5

/* vim:ts=2:sw=2:expandtab

  Mapping BNO055 Orientation to RGB WS2812 LEDs and DRV2605 Haptics

  on FeatherBase

  Based on BNO055_MS5637_t3 Basic Example Code by Kris Winer

  Original Winer Comments follow:

  date: October 19, 2014

  license: Beerware - Use this code however you'd like. If you

  find it useful you can buy me a beer some time.

  Demonstrates basic BNO055 functionality including parameterizing the register addresses,

  initializing the sensor, communicating with pressure sensor MS5637,

  getting properly scaled accelerometer, gyroscope, and magnetometer data out.

  Added display functions to allow display to on breadboard monitor.

  Addition of 9 DoF sensor fusion using open source Madgwick and Mahony filter algorithms.

  Can compare results to hardware 9 DoF sensor fusion carried out on the BNO055.

  Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.

  This sketch is intended specifically for the BNO055+MS5637 Add-On Shield for the Teensy 3.1.

  It uses SDA/SCL on pins 17/16, respectively, and it uses the Teensy 3.1-specific Wire library i2c_t3.h.

  The Add-on shield can also be used as a stand-alone breakout board for any Arduino, Teensy, or

  other microcontroller by closing the solder jumpers on the back of the board.

  The MS5637 is a simple but high resolution (24-bit) pressure sensor, which can be used in its high resolution

  mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of

  only 1 microAmp. The choice will depend on the application.

  All sensors communicate via I2C at 400 Hz or higher.

  SDA and SCL should have external pull-up resistors (to 3.3V).

  4K7 resistors are on the BNO055_MS5637 breakout board.

  Hardware setup:

  Breakout Board --------- Arduino/Teensy

  3V3 ---------------------- 3.3V

  SDA -----------------------A4/17

  SCL -----------------------A5/16

  GND ---------------------- GND

  Note: The BNO055_MS5637 breakout board is an I2C sensor and uses the Arduino Wire or Teensy i2c_t3.h library.

  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.

  We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.

  We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.

  The Teensy has no internal pullups and we are using the Wire.begin function of the i2c_t3.h library

  to select 400 Hz i2c speed.

*/

// Uncomment to use BNO055 on address 0x29 instead of 0x28

//#define ADO 1

// use DRV2605 Haptics

#define USE_DRV

#if defined(__MK20DX128__) || defined(__MK20DX256__)

#include "i2c_t3.h"

#else

#include "Wire.h"

#endif

#include <Adafruit_NeoPixel.h>

#ifdef __AVR__

#include <avr/power.h> // Required for 16 MHz Adafruit Trinket

#endif

#ifdef USE_DRV

#include <Adafruit_DRV2605.h>

Adafruit_DRV2605 drv;

#endif

// Which pin on the Arduino is connected to the NeoPixels?

// On a Trinket or Gemma we suggest changing this to 1:

#define LED_PIN    A1

// How many NeoPixels are attached to the Arduino?

#define LED_COUNT 2

Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

//#include <Audio.h>

//AudioOutputI2S      audioOutput;        // audio shield: headphones & line-out

// Audio Code, Enable AudioOutputI2SQuad line to reproduce

// GUItool: begin automatically generated code

//AudioSynthWaveform       waveform0; //xy=192,380

//AudioSynthWaveform       waveform1; //xy=192,420

//AudioSynthWaveform       waveform2; //xy=192,460

//AudioSynthWaveform       waveform3; //xy=192,500

//AudioOutputI2SQuad       i2s_quad1;      //xy=385.8452453613281,443.1428527832031

//AudioConnection          patchCord0(waveform0, 0, i2s_quad1, 0);

//AudioConnection          patchCord1(waveform1, 0, i2s_quad1, 1);

//AudioConnection          patchCord2(waveform2, 0, i2s_quad1, 2);

//AudioConnection          patchCord3(waveform3, 0, i2s_quad1, 3);

//AudioControlSGTL5000     sgtl5000_2;     //xy=381.8453063964844,384.8571472167969

//AudioControlSGTL5000     sgtl5000_1;     //xy=382.2737731933594,345.14288330078125

// GUItool: end automatically generated code

//#include <SPI.h>

// BNO055 Register Map

// http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_10_Release.pdf

//

// BNO055 Page 0

#define BNO055_CHIP_ID          0x00    // should be 0xA0              

#define BNO055_ACC_ID           0x01    // should be 0xFB              

#define BNO055_MAG_ID           0x02    // should be 0x32              

#define BNO055_GYRO_ID          0x03    // should be 0x0F              

#define BNO055_SW_REV_ID_LSB    0x04

#define BNO055_SW_REV_ID_MSB    0x05

#define BNO055_BL_REV_ID        0x06

#define BNO055_PAGE_ID          0x07

#define BNO055_ACC_DATA_X_LSB   0x08

#define BNO055_ACC_DATA_X_MSB   0x09

#define BNO055_ACC_DATA_Y_LSB   0x0A

#define BNO055_ACC_DATA_Y_MSB   0x0B

#define BNO055_ACC_DATA_Z_LSB   0x0C

#define BNO055_ACC_DATA_Z_MSB   0x0D

#define BNO055_MAG_DATA_X_LSB   0x0E

#define BNO055_MAG_DATA_X_MSB   0x0F

#define BNO055_MAG_DATA_Y_LSB   0x10

#define BNO055_MAG_DATA_Y_MSB   0x11

#define BNO055_MAG_DATA_Z_LSB   0x12

#define BNO055_MAG_DATA_Z_MSB   0x13

#define BNO055_GYR_DATA_X_LSB   0x14

#define BNO055_GYR_DATA_X_MSB   0x15

#define BNO055_GYR_DATA_Y_LSB   0x16

#define BNO055_GYR_DATA_Y_MSB   0x17

#define BNO055_GYR_DATA_Z_LSB   0x18

#define BNO055_GYR_DATA_Z_MSB   0x19

#define BNO055_EUL_HEADING_LSB  0x1A

#define BNO055_EUL_HEADING_MSB  0x1B

#define BNO055_EUL_ROLL_LSB     0x1C

#define BNO055_EUL_ROLL_MSB     0x1D

#define BNO055_EUL_PITCH_LSB    0x1E

#define BNO055_EUL_PITCH_MSB    0x1F

#define BNO055_QUA_DATA_W_LSB   0x20

#define BNO055_QUA_DATA_W_MSB   0x21

#define BNO055_QUA_DATA_X_LSB   0x22

#define BNO055_QUA_DATA_X_MSB   0x23

#define BNO055_QUA_DATA_Y_LSB   0x24

#define BNO055_QUA_DATA_Y_MSB   0x25

#define BNO055_QUA_DATA_Z_LSB   0x26

#define BNO055_QUA_DATA_Z_MSB   0x27

#define BNO055_LIA_DATA_X_LSB   0x28

#define BNO055_LIA_DATA_X_MSB   0x29

#define BNO055_LIA_DATA_Y_LSB   0x2A

#define BNO055_LIA_DATA_Y_MSB   0x2B

#define BNO055_LIA_DATA_Z_LSB   0x2C

#define BNO055_LIA_DATA_Z_MSB   0x2D

#define BNO055_GRV_DATA_X_LSB   0x2E

#define BNO055_GRV_DATA_X_MSB   0x2F

#define BNO055_GRV_DATA_Y_LSB   0x30

#define BNO055_GRV_DATA_Y_MSB   0x31

#define BNO055_GRV_DATA_Z_LSB   0x32

#define BNO055_GRV_DATA_Z_MSB   0x33

#define BNO055_TEMP             0x34

#define BNO055_CALIB_STAT       0x35

#define BNO055_ST_RESULT        0x36

#define BNO055_INT_STATUS       0x37

#define BNO055_SYS_CLK_STATUS   0x38

#define BNO055_SYS_STATUS       0x39

#define BNO055_SYS_ERR          0x3A

#define BNO055_UNIT_SEL         0x3B

#define BNO055_OPR_MODE         0x3D

#define BNO055_PWR_MODE         0x3E

#define BNO055_SYS_TRIGGER      0x3F

#define BNO055_TEMP_SOURCE      0x40

#define BNO055_AXIS_MAP_CONFIG  0x41

#define BNO055_AXIS_MAP_SIGN    0x42

#define BNO055_ACC_OFFSET_X_LSB 0x55

#define BNO055_ACC_OFFSET_X_MSB 0x56

#define BNO055_ACC_OFFSET_Y_LSB 0x57

#define BNO055_ACC_OFFSET_Y_MSB 0x58

#define BNO055_ACC_OFFSET_Z_LSB 0x59

#define BNO055_ACC_OFFSET_Z_MSB 0x5A

#define BNO055_MAG_OFFSET_X_LSB 0x5B

#define BNO055_MAG_OFFSET_X_MSB 0x5C

#define BNO055_MAG_OFFSET_Y_LSB 0x5D

#define BNO055_MAG_OFFSET_Y_MSB 0x5E

#define BNO055_MAG_OFFSET_Z_LSB 0x5F

#define BNO055_MAG_OFFSET_Z_MSB 0x60

#define BNO055_GYR_OFFSET_X_LSB 0x61

#define BNO055_GYR_OFFSET_X_MSB 0x62

#define BNO055_GYR_OFFSET_Y_LSB 0x63

#define BNO055_GYR_OFFSET_Y_MSB 0x64

#define BNO055_GYR_OFFSET_Z_LSB 0x65

#define BNO055_GYR_OFFSET_Z_MSB 0x66

#define BNO055_ACC_RADIUS_LSB   0x67

#define BNO055_ACC_RADIUS_MSB   0x68

#define BNO055_MAG_RADIUS_LSB   0x69

#define BNO055_MAG_RADIUS_MSB   0x6A

//

// BNO055 Page 1

#define BNO055_PAGE_ID          0x07

#define BNO055_ACC_CONFIG       0x08

#define BNO055_MAG_CONFIG       0x09

#define BNO055_GYRO_CONFIG_0    0x0A

#define BNO055_GYRO_CONFIG_1    0x0B

#define BNO055_ACC_SLEEP_CONFIG 0x0C

#define BNO055_GYR_SLEEP_CONFIG 0x0D

#define BNO055_INT_MSK          0x0F

#define BNO055_INT_EN           0x10

#define BNO055_ACC_AM_THRES     0x11

#define BNO055_ACC_INT_SETTINGS 0x12

#define BNO055_ACC_HG_DURATION  0x13

#define BNO055_ACC_HG_THRESH    0x14

#define BNO055_ACC_NM_THRESH    0x15

#define BNO055_ACC_NM_SET       0x16

#define BNO055_GYR_INT_SETTINGS 0x17

#define BNO055_GYR_HR_X_SET     0x18

#define BNO055_GYR_DUR_X        0x19

#define BNO055_GYR_HR_Y_SET     0x1A

#define BNO055_GYR_DUR_Y        0x1B

#define BNO055_GYR_HR_Z_SET     0x1C

#define BNO055_GYR_DUR_Z        0x1D

#define BNO055_GYR_AM_THRESH    0x1E

#define BNO055_GYR_AM_SET       0x1F

// Using the BNO055_MS5637 breakout board/Teensy 3.1 Add-On Shield, ADO is set to 1 by default

#if ADO

#define BNO055_ADDRESS 0x29   //  Device address of BNO055 when ADO = 1

#define MS5637_ADDRESS   0x76   //  Address of MS5637 altimeter

#else

#define BNO055_ADDRESS 0x28   //  Device address of BNO055 when ADO = 0

#define MS5637_ADDRESS   0x76   //  Address of MS5637 altimeter

#endif

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

// Set initial input parameters

enum Ascale {  // ACC Full Scale

  AFS_2G = 0,

  AFS_4G,

  AFS_8G,

  AFS_18G

};

enum Abw { // ACC Bandwidth

  ABW_7_81Hz = 0,

  ABW_15_63Hz,

  ABW_31_25Hz,

  ABW_62_5Hz,

  ABW_125Hz,

  ABW_250Hz,

  ABW_500Hz,

  ABW_1000Hz,    //0x07

};

enum APwrMode { // ACC Pwr Mode

  NormalA = 0,

  SuspendA,

  LowPower1A,

  StandbyA,

  LowPower2A,

  DeepSuspendA

};

enum Gscale {  // gyro full scale

  GFS_2000DPS = 0,

  GFS_1000DPS,

  GFS_500DPS,

  GFS_250DPS,

  GFS_125DPS    // 0x04

};

enum GPwrMode { // GYR Pwr Mode

  NormalG = 0,

  FastPowerUpG,

  DeepSuspendedG,

  SuspendG,

  AdvancedPowerSaveG

};

enum Gbw { // gyro bandwidth

  GBW_523Hz = 0,

  GBW_230Hz,

  GBW_116Hz,

  GBW_47Hz,

  GBW_23Hz,

  GBW_12Hz,

  GBW_64Hz,

  GBW_32Hz

};

enum OPRMode {  // BNO-55 operation modes

  CONFIGMODE = 0x00,

  // Sensor Mode

  ACCONLY,

  MAGONLY,

  GYROONLY,

  ACCMAG,

  ACCGYRO,

  MAGGYRO,

  AMG,            // 0x07

  // Fusion Mode

  IMU,

  COMPASS,

  M4G,

  NDOF_FMC_OFF,

  NDOF            // 0x0C

};

enum PWRMode {

  Normalpwr = 0,

  Lowpower,

  Suspendpwr

};

enum Modr {         // magnetometer output data rate

  MODR_2Hz = 0,

  MODR_6Hz,

  MODR_8Hz,

  MODR_10Hz,

  MODR_15Hz,

  MODR_20Hz,

  MODR_25Hz,

  MODR_30Hz

};

enum MOpMode { // MAG Op Mode

  LowPower = 0,

  Regular,

  EnhancedRegular,

  HighAccuracy

};

enum MPwrMode { // MAG power mode

  Normal = 0,

  Sleep,

  Suspend,

  ForceMode

};

#define ADC_256  0x00 // define pressure and temperature conversion rates

#define ADC_512  0x02

#define ADC_1024 0x04

#define ADC_2048 0x06

#define ADC_4096 0x08

#define ADC_8192 0x0A

#define ADC_D1   0x40

#define ADC_D2   0x50

// Specify sensor configuration

uint8_t OSR = ADC_8192;       // set pressure amd temperature oversample rate

//

uint8_t GPwrMode = Normal;    // Gyro power mode

uint8_t Gscale = GFS_250DPS;  // Gyro full scale

//uint8_t Godr = GODR_250Hz;    // Gyro sample rate

uint8_t Gbw = GBW_23Hz;       // Gyro bandwidth

//

uint8_t Ascale = AFS_2G;      // Accel full scale

//uint8_t Aodr = AODR_250Hz;    // Accel sample rate

uint8_t APwrMode = Normal;    // Accel power mode

uint8_t Abw = ABW_31_25Hz;    // Accel bandwidth, accel sample rate divided by ABW_divx

//

//uint8_t Mscale = MFS_4Gauss;  // Select magnetometer full-scale resolution

uint8_t MOpMode = HighAccuracy;    // Select magnetometer perfomance mode

uint8_t MPwrMode = Normal;    // Select magnetometer power mode

uint8_t Modr = MODR_10Hz;     // Select magnetometer ODR when in BNO055 bypass mode

uint8_t PWRMode = Normal ;    // Select BNO055 power mode

uint8_t OPRMode = NDOF;        // specify operation mode for sensors

uint8_t status;               // BNO055 data status register

float aRes, gRes, mRes;       // scale resolutions per LSB for the sensors

// Pin definitions

int intPin = 13;  // These can be changed, 2 and 3 are the Arduinos ext int pins

int myLed = 9;

uint16_t Pcal[8];         // calibration constants from MS5637 PROM registers

unsigned char nCRC;       // calculated check sum to ensure PROM integrity

uint32_t D1 = 0, D2 = 0;  // raw MS5637 pressure and temperature data

double dT, OFFSET, SENS, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data

int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output

int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output

int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output

int16_t quatCount[4];   // Stores the 16-bit signed quaternion output

int16_t EulCount[3];    // Stores the 16-bit signed Euler angle output

int16_t LIACount[3];    // Stores the 16-bit signed linear acceleration output

int16_t GRVCount[3];    // Stores the 16-bit signed gravity vector output

float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0};  // Bias corrections for gyro, accelerometer, and magnetometer

int16_t tempGCount, tempMCount;      // temperature raw count output of mag and gyro

float   Gtemperature, Mtemperature;  // Stores the BNO055 gyro and LIS3MDL mag internal chip temperatures in degrees Celsius

double Temperature, Pressure;        // stores MS5637 pressures sensor pressure and temperature

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)

float GyroMeasError = PI * (40.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 40 deg/s)

float GyroMeasDrift = PI * (0.0f  / 180.0f);   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)

// There is a tradeoff in the beta parameter between accuracy and response speed.

// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.

// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.

// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!

// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec

// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;

// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.

// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.

float beta = sqrt(3.0f / 4.0f) * GyroMeasError;   // compute beta

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

#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

#define Ki 0.0f

uint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate

float pitch, yaw, roll;

float Pitch, Yaw, Roll;

float LIAx, LIAy, LIAz, GRVx, GRVy, GRVz;

float deltat = 0.0f, sum = 0.0f;          // integration interval for both filter schemes

uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval

uint32_t Now = 0;                         // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values

float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion

float quat[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion

float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method

void setup()

{

  //  Wire.begin();

  //  TWBR = 12;  // 400 kbit/sec I2C speed for Pro Mini

  // Setup for Master mode, pins 16/17, external pullups, 400kHz for Teensy 3.1

#if defined(__MK20DX128__) || defined(__MK20DX256__)

  Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);

#else

  Wire.begin();

#endif

  strip.begin();           // INITIALIZE NeoPixel strip object (REQUIRED)

  strip.show();            // Turn OFF all pixels ASAP

  strip.setBrightness(100); // Set BRIGHTNESS to about 1/5 (max = 255)

#ifdef USE_DRV

  drv.selectLibrary(6);

  drv.useLRA();

  drv.setMode(DRV2605_MODE_REALTIME);

  pinMode(A3, OUTPUT);

  digitalWrite(A3, HIGH);

#endif

  delay(1000);

  Serial.begin(115200);

  // Set up the interrupt pin, its set as active high, push-pull

  pinMode(intPin, INPUT);

  pinMode(myLed, OUTPUT);

  digitalWrite(myLed, HIGH);

  /*

    // scan for i2c devices

    byte error, address;

    int nDevices;

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

    nDevices = 0;

    for(address = 1; address < 127; address++ )

    {

      // The i2c_scanner uses the return value of

      // the Write.endTransmisstion to see if

      // a device did acknowledge to the address.

      Wire.beginTransmission(address);