Repository: kriswiner/ESP32
Branch: master
Commit: a7973d4dca78
Files: 76
Total size: 602.6 KB

Directory structure:
gitextract_wmdjks10/

├── Bosch/
│   ├── BME280_VEML6040_LTSleep.ino
│   ├── BNO055_MS5637.ino
│   └── readme.md
├── CCS811_BME280_ESP32C3Mini_WebServer/
│   ├── BME280.cpp
│   ├── BME280.h
│   ├── CCS811.cpp
│   ├── CCS811.h
│   ├── CCS811_BME280_ESP32C3Mini_WebServer.ino
│   ├── I2CDev.cpp
│   ├── I2CDev.h
│   └── Readme.md
├── CCS811_BME280_ESP32C3Mini_deepSleep/
│   ├── BME280.cpp
│   ├── BME280.h
│   ├── CCS811.cpp
│   ├── CCS811.h
│   ├── CCS811_BME280_ESP32C3Mini_deepSleep.ino
│   ├── I2CDev.cpp
│   ├── I2CDev.h
│   └── Readme.md
├── EM7180/
│   ├── EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/
│   │   ├── EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32.ino
│   │   ├── I2Cdev.cpp
│   │   ├── I2Cdev.h
│   │   ├── LIS2MDL.cpp
│   │   ├── LIS2MDL.h
│   │   ├── LPS22HB.cpp
│   │   ├── LPS22HB.h
│   │   ├── LSM6DSM.cpp
│   │   ├── LSM6DSM.h
│   │   ├── USFS.cpp
│   │   └── USFS.h
│   └── Readme.md
├── ESP32C3MiniEnvSensor/
│   ├── ESP32C3Mini_EnvSensor.v02b/
│   │   ├── APDS9253.cpp
│   │   ├── APDS9253.h
│   │   ├── ESP32C3Mini_EnvSensor.v02b.ino
│   │   ├── HDC2010.cpp
│   │   ├── HDC2010.h
│   │   ├── I2Cdev.cpp
│   │   ├── I2Cdev.h
│   │   ├── LPS22HB.cpp
│   │   ├── LPS22HB.h
│   │   ├── SPIFlash.cpp
│   │   └── SPIFlash.h
│   ├── Readme.md
│   ├── SPIFlash_ESP32C3Mini/
│   │   └── SPIFlash_ESP32C3Mini.ino
│   └── readSPIFlash_ESP32C3Mini_EnvSensor.v02b/
│       ├── SPIFlash.cpp
│       ├── SPIFlash.h
│       └── readSPIFlash_ESP32C3Mini_EnvSensor.v02b.ino
├── MPU9250_MS5637/
│   ├── MPU9250_MS5637_AHRS.ino
│   ├── MS5637.ino
│   ├── quaternionFilters.ino
│   └── readme.md
├── MPU9250_MS5637_AHRS_UDP/
│   ├── MPU9250_MS5637_AHRS_UDP.ino
│   ├── quaternionFilters.ino
│   └── readme.md
├── PWM/
│   ├── ledcWrite_demo_ESP32.ino
│   └── readme.md
├── README.md
├── SPI/
│   ├── Readme.md
│   └── SPIFlash_ESP32.ino
├── VISHAY/
│   ├── VEML6040.ino
│   └── readme.md
├── VL53L0x/
│   ├── VL53L0X_ESP32.ino
│   └── readme.md
└── extras/
    └── FreeIMU_cube_UDP_ES32/
        ├── AverageCompassVals.pde
        ├── FreeIMU_cube_UDP_ES32.pde
        ├── Kalman.pde
        ├── MathUtils.java
        ├── MotionDetect.pde
        ├── ODO.pde
        ├── Quaternion.java
        ├── SMA.pde
        ├── StopWatch.pde
        ├── Unused.pde
        ├── code/
        │   └── RXTXcomm.jar
        └── data/
            ├── ArialMT-10.vlw
            └── CourierNew36.vlw

================================================
FILE CONTENTS
================================================

================================================
FILE: Bosch/BME280_VEML6040_LTSleep.ino
================================================
/* BME280 and VEML6040 Basic Example Code using ESP32
 by: Kris Winer
 date: December 14, 2016
 license: Beerware - Use this code however you'd like. If you 
 find it useful you can buy me a beer some time.
 
 This sketch uses SDA/SCL on pins 0/2, respectively, and it uses the ESP32.
 
 The BME280 is a simple but high resolution pressure/humidity/temperature 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.

 VEML6040 color sensor senses red, green, blue, and white light and incorporates photodiodes, amplifiers, 
 and analog / digital circuits into a single chip using CMOS process. With the   color   sensor   applied,   
 the   brightness,   and   color temperature of backlight can be adjusted base on ambient light  source  
 that  makes  panel  looks  more  comfortable  for  end   user’s   eyes.   VEML6040’s   adoption   of   FiltronTM
 technology  achieves  the  closest  ambient  light  spectral  sensitivity to real human eye responses.

 VEML6040  provides  excellent  temperature  compensation  capability  for  keeping  the  output  stable  
 under  changing  temperature.   VEML6040’s   function   are   easily   operated   via the simple command format 
 of I2C (SMBus compatible) interface  protocol.  VEML6040’s  operating  voltage  ranges  from   2.5   V   to   
 3.6   V.   VEML6040   is   packaged   in   a   lead  (Pb)-free  4  pin  OPLGA  package  which  offers  the  best market-proven reliability.

 
 SDA and SCL should have 4K7 pull-up resistors (to 3.3V).
 
 Hardware setup:
 SDA ----------------------- 21
 SCL ----------------------- 22
 
  */
#include "Wire.h"   
#include <SPI.h>
#include <WiFi.h>
#include <WiFiClient.h>
//#include <WebServer.h>
//#include <ESP32mDNS.h>
//#include <ArduinoOTA.h>

/*#include "gpio.h"
extern "C" {
#include "user_interface.h"
  bool wifi_set_sleep_type(sleep_type_t);
  sleep_type_t wifi_get_sleep_type(void);
}
*/
// Must immediately declare functions to avoid "Not declared in this scope" errors
void      I2Cscan();
void      writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
uint8_t   readByte(uint8_t address, uint8_t subAddress);
void      readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
int32_t  readBME280Temperature();
int32_t  readBME280Pressure();
int32_t  readBME280Humidity();
void      BME280Init();
uint32_t  BME280_compensate_P(int32_t adc_P);
int32_t   BME280_compensate_T(int32_t adc_T);
uint32_t  BME280_compensate_H(int32_t adc_H);
uint16_t  getRGBWdata(uint16_t * destination);
void      enableVEML6040();
//void      initWifi();

//ADC_MODE(ADC_VCC); // to use getVcc, don't use if using battery voltage monotor

// BME280 registers
#define BME280_HUM_LSB    0xFE
#define BME280_HUM_MSB    0xFD
#define BME280_TEMP_XLSB  0xFC
#define BME280_TEMP_LSB   0xFB
#define BME280_TEMP_MSB   0xFA
#define BME280_PRESS_XLSB 0xF9
#define BME280_PRESS_LSB  0xF8
#define BME280_PRESS_MSB  0xF7
#define BME280_CONFIG     0xF5
#define BME280_CTRL_MEAS  0xF4
#define BME280_STATUS     0xF3
#define BME280_CTRL_HUM   0xF2
#define BME280_RESET      0xE0
#define BME280_ID         0xD0  // should be 0x60
#define BME280_CALIB00    0x88
#define BME280_CALIB26    0xE1

////////////////////////////
// VEML6040 Command Codes //
////////////////////////////
#define  VEML6040_CONF            0x00 // command codes
#define  VEML6040_R_DATA          0x08  
#define  VEML6040_G_DATA          0x09 
#define  VEML6040_B_DATA          0x0A
#define  VEML6040_W_DATA          0x0B

#define VEML6040_ADDRESS         0x10
#define BME280_ADDRESS           0x76   // Address of BMP280 altimeter when ADO = 0

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

enum Posr {
  P_OSR_00 = 0,  // no op
  P_OSR_01,
  P_OSR_02,
  P_OSR_04,
  P_OSR_08,
  P_OSR_16
};

enum Hosr {
  H_OSR_00 = 0,  // no op
  H_OSR_01,
  H_OSR_02,
  H_OSR_04,
  H_OSR_08,
  H_OSR_16
};

enum Tosr {
  T_OSR_00 = 0,  // no op
  T_OSR_01,
  T_OSR_02,
  T_OSR_04,
  T_OSR_08,
  T_OSR_16
};

enum IIRFilter {
  full = 0,  // bandwidth at full sample rate
  BW0_223ODR,
  BW0_092ODR,
  BW0_042ODR,
  BW0_021ODR // bandwidth at 0.021 x sample rate
};

enum Mode {
  BME280Sleep = 0,
  forced,
  forced2,
  normal
};

enum SBy {
  t_00_5ms = 0,
  t_62_5ms,
  t_125ms,
  t_250ms,
  t_500ms,
  t_1000ms,
  t_10ms,
  t_20ms,
};

enum IT {
  IT_40 = 0, //   40 ms
  IT_80,     //   80 ms
  IT_160,    //  160 ms
  IT_320,    //  320 ms
  IT_640,    //  640 ms
  IT_1280   // 1280 ms
};

// Specify BME280 configuration
uint8_t Posr = P_OSR_16, Hosr = H_OSR_16, Tosr = T_OSR_02, Mode = normal, IIRFilter = BW0_042ODR, SBy = t_62_5ms;     // set pressure amd temperature output data rate
// t_fine carries fine temperature as global value for BME280
int32_t t_fine;

// Specify VEML6070 Integration time
uint8_t IT = IT_160;
uint8_t ITime = 160;  // milliseconds
uint16_t RGBWData[4] = {0, 0, 0, 0};
float GSensitivity = 0.25168/((float) (IT + 1)); // ambient light sensitivity increases with integration time
float redLight, greenLight, blueLight, ambientLight;

float Temperature, Pressure, Humidity; // stores BME280 pressures sensor pressure and temperature
float VBAT;  // battery voltage from ESP8285 ADC read
int32_t rawPress, rawTemp, rawHumidity;   // pressure and temperature raw count output for BME280

// BME280 compensation parameters
uint8_t  dig_H1, dig_H3, dig_H6;
uint16_t dig_T1, dig_P1, dig_H4, dig_H5;
int16_t  dig_T2, dig_T3, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9, dig_H2;

float   temperature_C, temperature_F, pressure, humidity, altitude; // Scaled output of the BME280

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

/*ESP8266WebServer server(80);
 
String webString="";     // String to display
 
void handle_root() {
  server.send(200, "text/plain", "Hello from the environmental monitoring station ESP8285!");
  delay(100);
}

  String createHTML(float var1, float var2, float var3, float var4, float var5, float var6, float var7, float var8, float var9, float var10) {
   webString = "<html><head><meta http-equiv=\"Refresh\" content=\"10\"></head><body><UL>"

  +String("<LI>Temperature = ")+String(var1)+String(" C</LI>") 
  +String("<LI>Temperature = ")+String(var2)+String(" F</LI>") 
  +String("<LI>Pressure = ")+String(var3)+String(" milliBar</LI>") 
  +String("<LI>Altitude = ")+String(var4)+String(" feet</LI>") 
  +String("<LI>Humidity = ")+String(var5)+String(" %RH</LI>") 
  +String("<LI>Red Light = ")+String(var6)+String(" microWatts/sq. cm</LI>") 
  +String("<LI>Green Light = ")+String(var7)+String(" microWatts/sq. cm</LI>") 
  +String("<LI>Blue Light = ")+String(var8)+String(" microWatts/sq. cm</LI>") 
  +String("<LI>Ambient Light = ")+String(var9)+String(" lux</LI>") 
  +String("<LI>Battery Voltage = ")+String(var10)+String(" V</LI>") 
     
  +"</UL></body></html>" ;
   return webString;
  }
*/

void setup()
{

  Serial.begin(115200);
  delay(4000);

  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH);
  
  Wire.begin(21, 22, 400000); // SDA (21), SCL (22) on ESP32, 400 kHz rate

  I2Cscan(); // should detect BME280 at 0x76
  
  // Read the WHO_AM_I register of the BME280 this is a good test of communication
  byte f = readByte(BME280_ADDRESS, BME280_ID);  // Read WHO_AM_I register for BME280
  Serial.print("BME280 "); 
  Serial.print("I AM "); 
  Serial.print(f, HEX); 
  Serial.print(" I should be "); 
  Serial.println(0x60, HEX);
  Serial.println(" ");
  
 // delay(1000); 

  if(f == 0x60) {
   
  writeByte(BME280_ADDRESS, BME280_RESET, 0xB6); // reset BME280 before initilization
  delay(100);

  BME280Init(); // Initialize BME280 altimeter
  Serial.println("Calibration coeficients:");
  Serial.print("dig_T1 ="); 
  Serial.println(dig_T1);
  Serial.print("dig_T2 ="); 
  Serial.println(dig_T2);
  Serial.print("dig_T3 ="); 
  Serial.println(dig_T3);
  Serial.print("dig_P1 ="); 
  Serial.println(dig_P1);
  Serial.print("dig_P2 ="); 
  Serial.println(dig_P2);
  Serial.print("dig_P3 ="); 
  Serial.println(dig_P3);
  Serial.print("dig_P4 ="); 
  Serial.println(dig_P4);
  Serial.print("dig_P5 ="); 
  Serial.println(dig_P5);
  Serial.print("dig_P6 ="); 
  Serial.println(dig_P6);
  Serial.print("dig_P7 ="); 
  Serial.println(dig_P7);
  Serial.print("dig_P8 ="); 
  Serial.println(dig_P8);
  Serial.print("dig_P9 ="); 
  Serial.println(dig_P9);
  Serial.print("dig_H1 ="); 
  Serial.println(dig_H1);
  Serial.print("dig_H2 ="); 
  Serial.println(dig_H2);
  Serial.print("dig_H3 ="); 
  Serial.println(dig_H3);
  Serial.print("dig_H4 ="); 
  Serial.println(dig_H4);
  Serial.print("dig_H5 ="); 
  Serial.println(dig_H5);
  Serial.print("dig_H6 ="); 
  Serial.println(dig_H6);
 }
  else Serial.println(" BME280 not functioning!");

  enableVEML6040(); // initalize sensor
  delay(150);  

  // Get some information abut the ESP8285

  uint32_t freeheap = ESP.getFreeHeap();
  Serial.print("Free Heap Size = "); Serial.println(freeheap);
 // uint32_t chipID = ESP.getChipId();
 // Serial.print("ESP32 chip ID = "); Serial.println(chipID);
//  uint32_t flashChipID = ESP.getFlashChipId();
// Serial.print("ESP8285 flash chip ID = "); Serial.println(flashChipID);
  uint32_t flashChipSize = ESP.getFlashChipSize();
  Serial.print("ESP32 flash chip size = "); Serial.print(flashChipSize); Serial.println(" bytes");
  uint32_t flashChipSpeed = ESP.getFlashChipSpeed();
  Serial.print("ESP32 flash chip speed = "); Serial.print(flashChipSpeed); Serial.println(" Hz");
//  uint32_t getVcc = ESP.getVcc();
//  Serial.print("ESP8285 supply voltage = "); Serial.print(getVcc); Serial.println(" volts");

 /*    initWifi();

     Serial.println("Light sleep enabled");
     wifi_set_sleep_type(LIGHT_SLEEP_T); // Enable light sleep mode to save power
  
     server.on("/ESP8285Data", [](){

    // BME280 Data
    rawTemp =   readBME280Temperature();
    temperature_C = (float) BME280_compensate_T(rawTemp)/100.0; // temperature in Centigrade
    temperature_F = 9.*temperature_C/5. + 32.;
    rawPress =  readBME280Pressure();
    pressure = (float) BME280_compensate_P(rawPress)/25600.0; // Pressure in millibar
    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));
    rawHumidity =  readBME280Humidity();
    humidity = (float) BME280_compensate_H(rawHumidity)/1024.0; // Humidity in %rH

    // VEML6040 Data
    getRGBWdata(RGBWData);
    redLight = (float)RGBWData[0]/96.0f;
    greenLight = (float)RGBWData[1]/74.0f;
    blueLight = (float)RGBWData[2]/56.0f;
    ambientLight = (float)RGBWData[1]*GSensitivity;

    // Battery Voltage
    VBAT = (1300.0/300.0) * (float)(analogRead(A0)) / 1024.0; // LiPo battery voltage in volts
 //   VBAT = (1100.0/100.0) * (float)(analogRead(A0)) / 1024.0; // 9V battery voltage in volts
    
    createHTML(temperature_C, temperature_F, pressure, altitude, humidity, redLight, greenLight, blueLight, ambientLight, VBAT);
    server.send(200, "text/html", webString);               // send to someone's browser when asked
    });
  
  server.begin();
  Serial.println("HTTP server started");

  Serial.println("LABEL,Timestamp,Pressure(mbar),Temperature(F),Humidity(rH%)");
  */
}
 

void loop()
{  
    rawTemp =   readBME280Temperature();
    temperature_C = (float) BME280_compensate_T(rawTemp)/100.0f;
    rawPress =  readBME280Pressure();
    pressure = (float) BME280_compensate_P(rawPress)/25600.0f; // Pressure in mbar
    rawHumidity =   readBME280Humidity();
    humidity = (float) BME280_compensate_H(rawHumidity)/1024.0f;
 
      Serial.println("BME280:");
      Serial.print("Altimeter temperature = "); 
      Serial.print( temperature_C, 2); 
      Serial.println(" C"); // temperature in degrees Celsius
      Serial.print("Altimeter temperature = "); 
      Serial.print(9.0f*temperature_C/5.0f + 32.0f, 2); 
      Serial.println(" F"); // temperature in degrees Fahrenheit
      Serial.print("Altimeter pressure = "); 
      Serial.print(pressure, 2); Serial.println(" mbar");// pressure in millibar

      Serial.print("DATA,");Serial.print("TIMER,"); 
      Serial.print(pressure); Serial.print(",");
      Serial.print(9.0f*temperature_C/5.0f + 32.0f, 2); Serial.print(",");
      Serial.print(humidity); Serial.println();

      altitude = 145366.45f*(1.0f - pow((pressure/1013.25f), 0.190284f));
      Serial.print("Altitude = "); 
      Serial.print(altitude, 2); 
      Serial.println(" feet");
      Serial.print("Altimeter humidity = "); 
      Serial.print(humidity, 1);  
      Serial.println(" %RH");// pressure in millibar
      Serial.println(" ");

      getRGBWdata(RGBWData);
      Serial.print("Red raw counts = ");   Serial.println(RGBWData[0]);
      Serial.print("Green raw counts = "); Serial.println(RGBWData[1]);
      Serial.print("Blue raw counts = ");  Serial.println(RGBWData[2]);
      Serial.print("White raw counts = "); Serial.println(RGBWData[3]);
      Serial.print("Inferred IR raw counts = "); Serial.println(RGBWData[3] - RGBWData[0] - RGBWData[1] - RGBWData[2]);
      Serial.println("  ");
 
      Serial.print("Red   light power density = "); Serial.print((float)RGBWData[0]/96.0f, 2); Serial.println(" microWatt/cm^2");
      Serial.print("Green light power density = "); Serial.print((float)RGBWData[1]/74.0f, 2); Serial.println(" microWatt/cm^2");
      Serial.print("Blue  light power density = "); Serial.print((float)RGBWData[2]/56.0f, 2); Serial.println(" microWatt/cm^2");
      Serial.println("  ");

      Serial.print("Ambient light intensity = "); Serial.print((float)RGBWData[1]*GSensitivity, 2); Serial.println(" lux");
      Serial.println("  ");

  // Empirical estimation of the correlated color temperature CCT:
  // see https://www.vishay.com/docs/84331/designingveml6040.pdf
      float temp = ( (float) (RGBWData[0] - RGBWData[2])/(float) RGBWData[1] );
      float CCT = 4278.6f*pow(temp, -1.2455f) + 0.5f;

      Serial.print("Correlated Color Temperature = "); Serial.print(CCT, 2); Serial.println(" Kelvin");
      Serial.println("  ");

      float VBAT = (100.0f/120.0f) * float(analogRead(34)) / 1024.0f;  // LiPo battery
      Serial.print("Battery Voltage = "); Serial.print(VBAT, 2); Serial.println(" V");    
  
      digitalWrite(myLed, HIGH); // blink blue led
      delay(100);
      digitalWrite(myLed, LOW);   

//      server.handleClient(); // serve data to web server site 
 
      delay(1000); // wait 1 seconds before refreshing data
}

//===================================================================================================================
//====== Set of useful function to access acceleration, gyroscope, magnetometer, and temperature data
//===================================================================================================================
/*void initWifi() {
  const char* ssid     = "NETGEAR16";
  const char* password = "braveroad553";
 // Connect to WiFi network
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  Serial.print("\n\r \n\rWorking to connect");

 // Wait for connection
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.println("ESP8285 Environmental Data Server");
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  // print the received signal strength:
  long rssi = WiFi.RSSI();
  Serial.print("signal strength (RSSI):");
  Serial.print(rssi);
  Serial.println(" dBm");
  }
*/
int32_t readBME280Temperature()
{
  uint8_t rawData[3];  // 20-bit pressure register data stored here
  readBytes(BME280_ADDRESS, BME280_TEMP_MSB, 3, &rawData[0]);  
  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);
}

int32_t readBME280Pressure()
{
  uint8_t rawData[3];  // 20-bit pressure register data stored here
  readBytes(BME280_ADDRESS, BME280_PRESS_MSB, 3, &rawData[0]);  
  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);
}

int32_t readBME280Humidity()
{
  uint8_t rawData[2];  // 16-bit humidity register data stored here
  readBytes(BME280_ADDRESS, BME280_HUM_MSB, 2, &rawData[0]);  
   return (uint32_t) (((uint32_t) rawData[0] << 24 | (uint32_t) rawData[1] << 16) ) >> 16;
}


void BME280Init()
{
  // Configure the BME280
  // Set H oversampling rate
  writeByte(BME280_ADDRESS, BME280_CTRL_HUM, 0x07 & Hosr);
  // Set T and P oversampling rates and sensor mode
  writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);
  // Set standby time interval in normal mode and bandwidth
  writeByte(BME280_ADDRESS, BME280_CONFIG, SBy << 5 | IIRFilter << 2);
  // Read and store calibration data
  uint8_t calib[26];
  readBytes(BME280_ADDRESS, BME280_CALIB00, 26, &calib[0]);
  dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);
  dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);
  dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);
  dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);
  dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);
  dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);
  dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);
  dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);
  dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);
  dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);
  dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);
  dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);
  dig_H1 = calib[25];
  readBytes(BME280_ADDRESS, BME280_CALIB26, 7, &calib[0]);
  dig_H2 = ( int16_t)((( int16_t) calib[1] << 8) | calib[0]);
  dig_H3 = calib[2];
  dig_H4 = ( int16_t)(((( int16_t) calib[3] << 8) | (0x0F & calib[4]) << 4) >> 4);
  dig_H5 = ( int16_t)(((( int16_t) calib[5] << 8) | (0xF0 & calib[4]) ) >> 4 );
  dig_H6 = calib[6];
}

// Returns temperature in DegC, resolution is 0.01 DegC. Output value of
// “5123” equals 51.23 DegC.
int32_t BME280_compensate_T(int32_t adc_T)
{
  int32_t var1, var2, T;
  var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
  var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1))) >> 12) * ((int32_t)dig_T3)) >> 14;
  t_fine = var1 + var2;
  T = (t_fine * 5 + 128) >> 8;
  return T;
}

// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8
//fractional bits).
//Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
uint32_t BME280_compensate_P(int32_t adc_P)
{
  long long var1, var2, p;
  var1 = ((long long)t_fine) - 128000;
  var2 = var1 * var1 * (long long)dig_P6;
  var2 = var2 + ((var1*(long long)dig_P5)<<17);
  var2 = var2 + (((long long)dig_P4)<<35);
  var1 = ((var1 * var1 * (long long)dig_P3)>>8) + ((var1 * (long long)dig_P2)<<12);
  var1 = (((((long long)1)<<47)+var1))*((long long)dig_P1)>>33;
  if(var1 == 0)
  {
    return 0;
    // avoid exception caused by division by zero
  }
  p = 1048576 - adc_P;
  p = (((p<<31) - var2)*3125)/var1;
  var1 = (((long long)dig_P9) * (p>>13) * (p>>13)) >> 25;
  var2 = (((long long)dig_P8) * p)>> 19;
  p = ((p + var1 + var2) >> 8) + (((long long)dig_P7)<<4);
  return (uint32_t)p;
}

// Returns humidity in %RH as unsigned 32 bit integer in Q22.10 format (22integer and 10fractional bits).
// Output value of “47445”represents 47445/1024= 46.333%RH
uint32_t BME280_compensate_H(int32_t adc_H)
{
int32_t var;

var = (t_fine - ((int32_t)76800));
var = (((((adc_H << 14) - (((int32_t)dig_H4) << 20) - (((int32_t)dig_H5) * var)) +
((int32_t)16384)) >> 15) * (((((((var * ((int32_t)dig_H6)) >> 10) * (((var *
((int32_t)dig_H3)) >> 11) + ((int32_t)32768))) >> 10) + ((int32_t)2097152)) * ((int32_t)dig_H2) + 8192) >> 14));
var = (var - (((((var >> 15) * (var >> 15)) >> 7) * ((int32_t)dig_H1)) >> 4));
var = (var < 0 ? 0 : var); 
var = (var > 419430400 ? 419430400 : var);
return(uint32_t)(var >> 12);
}

uint16_t getRGBWdata(uint16_t * destination)
{
    for (int j = 0; j < 4; j++)
    {
    uint8_t rawData[2] = {0, 0};
    Wire.beginTransmission(VEML6040_ADDRESS);
    Wire.write(VEML6040_R_DATA + j);        // Command code for reading rgbw data channels in sequence
    Wire.endTransmission(false);  // Send the Tx buffer, but send a restart to keep connection alive

    Wire.requestFrom(VEML6040_ADDRESS, 2);  // Read two bytes from slave register address 
    uint8_t i = 0;
    while (Wire.available()) 
    {
        rawData[i++] = Wire.read();       // Put read results in the Rx buffer
    }     
    Wire.endTransmission();
    destination[j] = ((uint16_t) rawData[1] << 8) | rawData[0];
    }
 
}

void enableVEML6040()
{
  Wire.beginTransmission(VEML6040_ADDRESS);
  Wire.write(VEML6040_CONF); // Command code for configuration register
  Wire.write(IT << 4); // Bit 3 must be 0, bit 0 is 0 for run and 1 for shutdown, LS Byte
  Wire.write(0x00); // MS Byte
  Wire.endTransmission();
}
 
// simple function to scan for I2C devices on the bus
void I2Cscan() 
{
    // 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);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");
}


// I2C read/write functions for the BMP280 sensors

  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
	Wire.beginTransmission(address);  // Initialize the Tx buffer
	Wire.write(subAddress);           // Put slave register address in Tx buffer
	Wire.write(data);                 // Put data in Tx buffer
	Wire.endTransmission();           // Send the Tx buffer
}

  uint8_t readByte(uint8_t address, uint8_t subAddress)
{
	uint8_t data; // `data` will store the register data	 
	Wire.beginTransmission(address);         // Initialize the Tx buffer
	Wire.write(subAddress);	                 // Put slave register address in Tx buffer
	Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
	Wire.requestFrom(address, 1);  // Read one byte from slave register address 
	data = Wire.read();                      // Fill Rx buffer with result
	return data;                             // Return data read from slave register
}

  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
	Wire.beginTransmission(address);   // Initialize the Tx buffer
	Wire.write(subAddress);            // Put slave register address in Tx buffer
	Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
	uint8_t i = 0;
  Wire.requestFrom(address, count);  // Read bytes from slave register address 
	while (Wire.available()) {
        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer
}


================================================
FILE: Bosch/BNO055_MS5637.ino
================================================
/* BNO055_MS5637_ESP32 Basic Example Code
 by: Kris Winer
 date: December 14, 2016
 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. 
 
 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 ESP32.
 
 This sketch is intended specifically for the BNO055+MS5637 Add-On Shield.
 It uses SDA/SCL on pins 21/22, respectively, and it uses the Wire library.
 
 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 kHz 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 --------- ESP32
 3V3 ---------------------- 3.3V
 SDA -----------------------21
 SCL -----------------------22
 GND ---------------------- GND
 
 */
#include <Wire.h>   

// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet
// http://www.meas-spec.com/product/pressure/MS5637-02BA03.aspx
//
#define MS5637_RESET      0x1E
#define MS5637_CONVERT_D1 0x40
#define MS5637_CONVERT_D2 0x50
#define MS5637_ADC_READ   0x00

// 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 
#define ADO 1
#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 = NormalG;    // 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 = NormalA;    // 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 = Regular;    // 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 = Normalpwr;    // 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 = 14;  // These can be changed, 2 and 3 are the Arduinos ext int pins
int myLed = 5;

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, TT2, 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(21, 22, 400000); // (SDA, SCL) (21, 22) are default on ESP32, 400 kHz I2C bus speed
  delay(5000);
  Serial.begin(115200);
  
  // Set up the interrupt pin, its set as active high, push-pull
  pinMode(intPin, INPUT);
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH);

  I2Cscan(); // check for I2C devices on the bus8
  
  // Read the WHO_AM_I register, this is a good test of communication
  Serial.println("BNO055 9-axis motion sensor...");
  byte c = readByte(BNO055_ADDRESS, BNO055_CHIP_ID);  // Read WHO_AM_I register for BNO055
  Serial.print("BNO055 Address = 0x"); Serial.println(BNO055_ADDRESS, HEX);
  Serial.print("BNO055 WHO_AM_I = 0x"); Serial.println(BNO055_CHIP_ID, HEX);
  Serial.print("BNO055 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.println(" I should be 0xA0");  

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

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

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

  delay(1000); 

  if (c == 0xA0) // BNO055 WHO_AM_I should always be 0xA0
  {  
    Serial.println("BNO055 is online...");
    
    // Check software revision ID
    byte swlsb = readByte(BNO055_ADDRESS, BNO055_SW_REV_ID_LSB);
    byte swmsb = readByte(BNO055_ADDRESS, BNO055_SW_REV_ID_MSB);
    Serial.print("BNO055 SW Revision ID: "); Serial.print(swmsb, HEX); Serial.print("."); Serial.println(swlsb, HEX); 
    Serial.println("Should be 03.04");
    
    // Check bootloader version
    byte blid = readByte(BNO055_ADDRESS, BNO055_BL_REV_ID);
    Serial.print("BNO055 bootloader Version: "); Serial.println(blid); 
    
    // Check self-test results
    byte selftest = readByte(BNO055_ADDRESS, BNO055_ST_RESULT);
    
    if(selftest & 0x01) {
      Serial.println("accelerometer passed selftest"); 
    } else {
      Serial.println("accelerometer failed selftest"); 
    }
    if(selftest & 0x02) {
      Serial.println("magnetometer passed selftest"); 
    } else {
      Serial.println("magnetometer failed selftest"); 
    }  
    if(selftest & 0x04) {
      Serial.println("gyroscope passed selftest"); 
    } else {
      Serial.println("gyroscope failed selftest"); 
    }      
    if(selftest & 0x08) {
      Serial.println("MCU passed selftest"); 
    } else {
      Serial.println("MCU failed selftest"); 
    }
      
    delay(1000);
  
  // Reset the MS5637 pressure sensor
  MS5637Reset();
  delay(100);
  Serial.println("MS5637 pressure sensor reset...");
  // Read PROM data from MS5637 pressure sensor
  MS5637PromRead(Pcal);
  Serial.println("PROM data read:");
  Serial.print("C0 = "); Serial.println(Pcal[0]);
  unsigned char refCRC = Pcal[0] >> 12;
  Serial.print("C1 = "); Serial.println(Pcal[1]);
  Serial.print("C2 = "); Serial.println(Pcal[2]);
  Serial.print("C3 = "); Serial.println(Pcal[3]);
  Serial.print("C4 = "); Serial.println(Pcal[4]);
  Serial.print("C5 = "); Serial.println(Pcal[5]);
  Serial.print("C6 = "); Serial.println(Pcal[6]);
  
  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data
  Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC);  
  
  delay(1000);  
 
  accelgyroCalBNO055(accelBias, gyroBias);
  
  Serial.println("Average accelerometer bias (mg) = "); Serial.println(accelBias[0]); Serial.println(accelBias[1]); Serial.println(accelBias[2]);
  Serial.println("Average gyro bias (dps) = "); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);
  
  delay(1000); 
  
  magCalBNO055(magBias);
  
  Serial.println("Average magnetometer bias (mG) = "); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]);
 
  delay(1000); 
  
  // Check calibration status of the sensors
  uint8_t calstat = readByte(BNO055_ADDRESS, BNO055_CALIB_STAT);
  Serial.println("Not calibrated = 0, fully calibrated = 3");
  Serial.print("System calibration status "); Serial.println( (0xC0 & calstat) >> 6);
  Serial.print("Gyro   calibration status "); Serial.println( (0x30 & calstat) >> 4);
  Serial.print("Accel  calibration status "); Serial.println( (0x0C & calstat) >> 2);
  Serial.print("Mag    calibration status "); Serial.println( (0x03 & calstat) >> 0);
  
  initBNO055(); // Initialize the BNO055
  Serial.println("BNO055 initialized for sensor mode...."); // Initialize BNO055 for sensor read 
 
  }
  else
  {
    Serial.print("Could not connect to BNO055: 0x");
    Serial.println(c, HEX);
    while(1) ; // Loop forever if communication doesn't happen
  }
}

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

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

    readMagData(magCount);  // Read the x/y/z adc values   
    // Calculate the magnetometer values in milliGauss
    mx = (float)magCount[0]/1.6; // - magBias[0];  // get actual magnetometer value in mGauss 
    my = (float)magCount[1]/1.6; // - magBias[1];  
    mz = (float)magCount[2]/1.6; // - magBias[2];   
    
    readQuatData(quatCount);  // Read the x/y/z adc values   
    // Calculate the quaternion values  
    quat[0] = (float)(quatCount[0])/16384.;    
    quat[1] = (float)(quatCount[1])/16384.;  
    quat[2] = (float)(quatCount[2])/16384.;   
    quat[3] = (float)(quatCount[3])/16384.;   
    
    readEulData(EulCount);  // Read the x/y/z adc values   
    // Calculate the Euler angles values in degrees
    Yaw = (float)EulCount[0]/16.;  
    Roll = (float)EulCount[1]/16.;  
    Pitch = (float)EulCount[2]/16.;   
 
    readLIAData(LIACount);  // Read the x/y/z adc values   
    // Calculate the linear acceleration (sans gravity) values in mg
    LIAx = (float)LIACount[0];  
    LIAy = (float)LIACount[1];  
    LIAz = (float)LIACount[2];   

    readGRVData(GRVCount);  // Read the x/y/z adc values   
    // Calculate the linear acceleration (sans gravity) values in mg
    GRVx = (float)GRVCount[0];  
    GRVy = (float)GRVCount[1];  
    GRVz = (float)GRVCount[2];   
    
  
  Now = micros();
  deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
  lastUpdate = Now;
  
  sum += deltat; // sum for averaging filter update rate
  sumCount++;
  
  // Sensors x, y, and z-axes  for the three sensor: accel, gyro, and magnetometer are all aligned, so
  // no allowance for any orientation mismatch in feeding the output to the quaternion filter is required.
  // For the BNO055, the sensor forward is along the x-axis just like
  // in the LSM9DS0 and MPU9250 sensors. This rotation can be modified to allow any convenient orientation convention.
  // This is ok by aircraft orientation standards!  
  // Pass gyro rate as rad/s
  MadgwickQuaternionUpdate(ax, -ay, -az, gx*PI/180.0f, -gy*PI/180.0f, -gz*PI/180.0f,  -my,  -mx, -mz);
//  MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);
    
    // Serial print and/or display at 0.5 s rate independent of data rates
    delt_t = millis() - count;
    if (delt_t > 1000) { // update LCD once per half-second independent of read rate
    
  if (SerialDebug) {
  // check BNO-055 error status at 2 Hz rate
    uint8_t sysstat = readByte(BNO055_ADDRESS, BNO055_SYS_STATUS); // check system status
    Serial.print("System Status = 0x"); Serial.println(sysstat, HEX);
    if(sysstat == 0x05) Serial.println("Sensor fusion algorithm running");
    if(sysstat == 0x06) Serial.println("Sensor fusion not algorithm running");
    
    if(sysstat == 0x01) {
      uint8_t syserr = readByte(BNO055_ADDRESS, BNO055_SYS_ERR);
      if(syserr == 0x01) Serial.println("Peripheral initialization error");
      if(syserr == 0x02) Serial.println("System initialization error");
      if(syserr == 0x03) Serial.println("Self test result failed");
      if(syserr == 0x04) Serial.println("Register map value out of range");
      if(syserr == 0x05) Serial.println("Register map address out of range");
      if(syserr == 0x06) Serial.println("Register map write error");
      if(syserr == 0x07) Serial.println("BNO low power mode no available for selected operation mode");
      if(syserr == 0x08) Serial.println("Accelerometer power mode not available");
      if(syserr == 0x09) Serial.println("Fusion algorithm configuration error");
      if(syserr == 0x0A) Serial.println("Sensor configuration error");    
    }  
  }

    if(SerialDebug) {
    Serial.print("ax = "); Serial.print((int)ax);  
    Serial.print(" ay = "); Serial.print((int)ay); 
    Serial.print(" az = "); Serial.print((int)az); Serial.println(" mg");
    Serial.print("gx = "); Serial.print( gx, 2); 
    Serial.print(" gy = "); Serial.print( gy, 2); 
    Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
    Serial.print("mx = "); Serial.print( (int)mx ); 
    Serial.print(" my = "); Serial.print( (int)my ); 
    Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
    
    Serial.print("qx = "); Serial.print(q[0]);
    Serial.print(" qy = "); Serial.print(q[1]); 
    Serial.print(" qz = "); Serial.print(q[2]); 
    Serial.print(" qw = "); Serial.println(q[3]); 
    Serial.print("quatw = "); Serial.print(quat[0]);
    Serial.print(" quatx = "); Serial.print(quat[1]); 
    Serial.print(" quaty = "); Serial.print(quat[2]); 
    Serial.print(" quatz = "); Serial.println(quat[3]); 
    } 
    
    if(SerialDebug) {
      tempGCount = readGyroTempData();  // Read the gyro adc values
    Gtemperature = (float) tempGCount; // Gyro chip temperature in degrees Centigrade
   // Print gyro die temperature in degrees Centigrade      
    Serial.print("Gyro temperature is ");  Serial.print(Gtemperature, 1);  Serial.println(" degrees C"); // Print T values to tenths of a degree C
    }
    
    D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value
    D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value
    
    dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference
    OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);
    SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);
 
    Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade
//
// Second order corrections
    if(Temperature > 20) 
    {
      TT2 = 5*dT*dT/pow(2, 38); // correction for high temperatures
      OFFSET2 = 0;
      SENS2 = 0;
    }
    if(Temperature < 20)                   // correction for low temperature
    {
      TT2      = 3*dT*dT/pow(2, 33); 
      OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
      SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
    } 
    if(Temperature < -15)                      // correction for very low temperature
    {
      OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);
      SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);
    }
 // End of second order corrections
 //
     Temperature = Temperature - TT2/100;
     OFFSET = OFFSET - OFFSET2;
     SENS = SENS - SENS2;
 
     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or Pa/100
  
    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));
   
    if(SerialDebug) {
    Serial.print("Digital temperature value = "); Serial.print( (float)Temperature, 2); Serial.println(" C"); // temperature in degrees Celsius
    Serial.print("Digital temperature value = "); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(" F"); // temperature in degrees Fahrenheit
    Serial.print("Digital pressure value = "); Serial.print((float) Pressure, 2);  Serial.println(" mbar");// pressure in millibar
    Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet");
    }
    
  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
  // In this coordinate system, the positive z-axis is down toward Earth. 
  // 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.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
  // applied in the correct order which for this configuration is yaw, pitch, and then roll.
  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
    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]);   
    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    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]);
    pitch *= 180.0f / PI;
    yaw   *= 180.0f / PI; 
 //   yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    roll  *= 180.0f / PI;
     
    if(SerialDebug) {
    Serial.print("Software Yaw, Pitch, Roll: ");
    Serial.print(yaw, 2);
    Serial.print(", ");
    Serial.print(pitch, 2);
    Serial.print(", ");
    Serial.println(roll, 2);
    
    Serial.print("Hardware Yaw, Pitch, Roll: ");
    Serial.print(Yaw, 2);
    Serial.print(", ");
    Serial.print(Pitch, 2);
    Serial.print(", ");
    Serial.println(Roll, 2);
 
    Serial.print("Hardware x, y, z linear acceleration: ");
    Serial.print(LIAx, 2);
    Serial.print(", ");
    Serial.print(LIAy, 2);
    Serial.print(", ");
    Serial.println(LIAz, 2);

    Serial.print("Hardware x, y, z gravity vector: ");
    Serial.print(GRVx, 2);
    Serial.print(", ");
    Serial.print(GRVy, 2);
    Serial.print(", ");
    Serial.println(GRVz, 2);
 
    
    Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
    }

     Serial.print(millis()/1000);Serial.print(",");
     Serial.print(yaw, 2); Serial.print(",");Serial.print(pitch, 2); Serial.print(",");Serial.print(roll, 2); Serial.print(",");
     Serial.print(Yaw, 2); Serial.print(",");Serial.print(Pitch, 2); Serial.print(",");Serial.print(Roll, 2); Serial.println(",");
  
    digitalWrite(myLed, !digitalRead(myLed));
    count = millis(); 
    sumCount = 0;
    sum = 0;    
    }

}

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

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


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

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

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

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

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

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

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

void initBNO055() {
   // Select BNO055 config mode
   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
   delay(25);
   // Select page 1 to configure sensors
   writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x01);
   // Configure ACC
   writeByte(BNO055_ADDRESS, BNO055_ACC_CONFIG, APwrMode << 5 | Abw << 2 | Ascale );
   // Configure GYR
   writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_0, Gbw << 3 | Gscale );
   writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_1, GPwrMode);
   // Configure MAG
   writeByte(BNO055_ADDRESS, BNO055_MAG_CONFIG, MPwrMode << 5 | MOpMode << 3 | Modr );
   
   // Select page 0 to read sensors
   writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);

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

   // Select BNO055 sensor units (temperature in degrees C, rate in dps, accel in mg)
   writeByte(BNO055_ADDRESS, BNO055_UNIT_SEL, 0x01 );
   
   // Select BNO055 system power mode
   writeByte(BNO055_ADDRESS, BNO055_PWR_MODE, PWRMode );
 
   // Select BNO055 system operation mode
   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );
   delay(25);
   
}

void accelgyroCalBNO055(float * dest1, float * dest2) 
{
  uint8_t data[6]; // data array to hold accelerometer and gyro x, y, z, data
  uint16_t ii = 0, sample_count = 0;
  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
 
  Serial.println("Accel/Gyro Calibration: Put device on a level surface and keep motionless! Wait......");
  delay(4000);
  
  // Select page 0 to read sensors
   writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);
   // Select BNO055 system operation mode as AMG for calibration
   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
   delay(25);
   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, AMG);
   
 // In NDF fusion mode, accel full scale is at +/- 4g, ODR is 62.5 Hz, set it the same here
   writeByte(BNO055_ADDRESS, BNO055_ACC_CONFIG, APwrMode << 5 | Abw << 2 | AFS_4G );
   sample_count = 256;
   for(ii = 0; ii < sample_count; ii++) {
    int16_t accel_temp[3] = {0, 0, 0};
    readBytes(BNO055_ADDRESS, BNO055_ACC_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array
    accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO
    accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
    accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
    accel_bias[0]  += (int32_t) accel_temp[0];
    accel_bias[1]  += (int32_t) accel_temp[1];
    accel_bias[2]  += (int32_t) accel_temp[2];
    delay(20);  // at 62.5 Hz ODR, new accel data is available every 16 ms
   }
    accel_bias[0]  /= (int32_t) sample_count;  // get average accel bias in mg
    accel_bias[1]  /= (int32_t) sample_count;
    accel_bias[2]  /= (int32_t) sample_count;
    
  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) 1000;}  // Remove gravity from the z-axis accelerometer bias calculation
  else {accel_bias[2] += (int32_t) 1000;}

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

 // In NDF fusion mode, gyro full scale is at +/- 2000 dps, ODR is 32 Hz
   writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_0, Gbw << 3 | GFS_2000DPS );
   writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_1, GPwrMode);
   for(ii = 0; ii < sample_count; ii++) {
    int16_t gyro_temp[3] = {0, 0, 0};
    readBytes(BNO055_ADDRESS, BNO055_GYR_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array
    gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ;  // Form signed 16-bit integer for each sample in FIFO
    gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
    gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
    gyro_bias[0]  += (int32_t) gyro_temp[0];
    gyro_bias[1]  += (int32_t) gyro_temp[1];
    gyro_bias[2]  += (int32_t) gyro_temp[2];
    delay(35);  // at 32 Hz ODR, new gyro data available every 31 ms
   }
    gyro_bias[0]  /= (int32_t) sample_count;  // get average gyro bias in counts
    gyro_bias[1]  /= (int32_t) sample_count;
    gyro_bias[2]  /= (int32_t) sample_count;
 
    dest2[0] = (float) gyro_bias[0]/16.;  // save gyro biases in dps for use in main program
    dest2[1] = (float) gyro_bias[1]/16.;  // gyro data is 16 LSB/dps
    dest2[2] = (float) gyro_bias[2]/16.;          

  // Return to config mode to write accelerometer biases in offset register
  // This offset register is only used while in fusion mode when accelerometer full-scale is +/- 4g
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
  delay(25);
  
  //write biases to accelerometer offset registers ad 16 LSB/dps
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_LSB, (int16_t)accel_bias[0] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_MSB, ((int16_t)accel_bias[0] >> 8) & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_LSB, (int16_t)accel_bias[1] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_MSB, ((int16_t)accel_bias[1] >> 8) & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_LSB, (int16_t)accel_bias[2] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_MSB, ((int16_t)accel_bias[2] >> 8) & 0xFF);
  
  // Check that offsets were properly written to offset registers
//  Serial.println("Average accelerometer bias = "); 
//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_LSB))); 
//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_LSB))); 
//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_LSB)));

   //write biases to gyro offset registers
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_LSB, (int16_t)gyro_bias[0] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_MSB, ((int16_t)gyro_bias[0] >> 8) & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_LSB, (int16_t)gyro_bias[1] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_MSB, ((int16_t)gyro_bias[1] >> 8) & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_LSB, (int16_t)gyro_bias[2] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_MSB, ((int16_t)gyro_bias[2] >> 8) & 0xFF);
  
  // Select BNO055 system operation mode
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );

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

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

void magCalBNO055(float * dest1) 
{
  uint8_t data[6]; // data array to hold mag x, y, z, data
  uint16_t ii = 0, sample_count = 0;
  int32_t mag_bias[3] = {0, 0, 0};
  int16_t mag_max[3] = {0x8000, 0x8000, 0x8000}, mag_min[3] = {0x7FFF, 0x7FFF, 0x7FFF};
 
  Serial.println("Mag Calibration: Wave device in a figure eight until done!");
  delay(4000);
  
  // Select page 0 to read sensors
   writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);
   // Select BNO055 system operation mode as AMG for calibration
   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
   delay(25);
   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, AMG );

 // In NDF fusion mode, mag data is in 16 LSB/microTesla, ODR is 20 Hz in forced mode
   sample_count = 256;
   for(ii = 0; ii < sample_count; ii++) {
    int16_t mag_temp[3] = {0, 0, 0};
    readBytes(BNO055_ADDRESS, BNO055_MAG_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array
    mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ;   // Form signed 16-bit integer for each sample in FIFO
    mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;
    mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;
    for (int jj = 0; jj < 3; jj++) {
      if (ii == 0) {
        mag_max[jj] = mag_temp[jj]; // Offsets may be large enough that mag_temp[i] may not be bipolar! 
        mag_min[jj] = mag_temp[jj]; // This prevents max or min being pinned to 0 if the values are unipolar...
      } else {
        if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
        if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
      }
    }
    delay(105);  // at 10 Hz ODR, new mag data is available every 100 ms
   }

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

    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
    
    dest1[0] = (float) mag_bias[0] / 1.6;  // save mag biases in mG for use in main program
    dest1[1] = (float) mag_bias[1] / 1.6;  // mag data is 1.6 LSB/mg
    dest1[2] = (float) mag_bias[2] / 1.6;          

  // Return to config mode to write mag biases in offset register
  // This offset register is only used while in fusion mode when magnetometer sensitivity is 16 LSB/microTesla
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );
  delay(25);
  
  //write biases to magnetometer offset registers as 16 LSB/microTesla
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_LSB, (int16_t)mag_bias[0] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_MSB, ((int16_t)mag_bias[0] >> 8) & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_LSB, (int16_t)mag_bias[1] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_MSB, ((int16_t)mag_bias[1] >> 8) & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_LSB, (int16_t)mag_bias[2] & 0xFF);
  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_MSB, ((int16_t)mag_bias[2] >> 8) & 0xFF);
 
  // Check that offsets were properly written to offset registers
//  Serial.println("Average magnetometer bias = "); 
//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_LSB))); 
//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_LSB))); 
//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_LSB)));
  // Select BNO055 system operation mode
  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );

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


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

        void MS5637Reset()
        {
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
	Wire.write(MS5637_RESET);                // Put reset command in Tx buffer
	Wire.endTransmission();                  // Send the Tx buffer
        }
        
        void MS5637PromRead(uint16_t * destination)
        {
        uint8_t data[2] = {0,0};
        for (uint8_t ii = 0; ii < 7; ii++) {
          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer
          Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive
//          Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive
	  uint8_t i = 0;
          Wire.requestFrom(MS5637_ADDRESS, 2);   // Read two bytes from slave PROM address 
	  while (Wire.available()) {
          data[i++] = Wire.read(); }               // Put read results in the Rx buffer
          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program
        }
        }

        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read
        {
        uint8_t data[3] = {0,0,0};
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(CMD | OSR);                  // Put pressure conversion command in Tx buffer
        Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive
//        Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive
        
        switch (OSR)
        {
          case ADC_256: delay(1); break;  // delay for conversion to complete
          case ADC_512: delay(3); break;
          case ADC_1024: delay(4); break;
          case ADC_2048: delay(6); break;
          case ADC_4096: delay(10); break;
          case ADC_8192: delay(20); break;
        }
       
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(0x00);                        // Put ADC read command in Tx buffer
//        Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive
        Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive
	uint8_t i = 0;
        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address 
	while (Wire.available()) {
        data[i++] = Wire.read(); }               // Put read results in the Rx buffer
        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program
        }



unsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents
{
  int cnt;
  unsigned int n_rem = 0;
  unsigned char n_bit;
  
  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation
  n_prom[7] = 0;
  for(cnt = 0; cnt < 16; cnt++)
  {
    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);
    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);
    for(n_bit = 8; n_bit > 0; n_bit--)
    {
        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;
        else                  n_rem = (n_rem<<1);
    }
  }
  n_rem = ((n_rem>>12) & 0x000F);
  return (n_rem ^ 0x00);
}

// I2C scan function

void I2Cscan()
{
// 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);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");
    
}

// I2C read/write functions for the BNO055 sensor

  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
	Wire.beginTransmission(address);  // Initialize the Tx buffer
	Wire.write(subAddress);           // Put slave register address in Tx buffer
	Wire.write(data);                 // Put data in Tx buffer
	Wire.endTransmission();           // Send the Tx buffer
}

  uint8_t readByte(uint8_t address, uint8_t subAddress)
{
	uint8_t data; // `data` will store the register data	 
	Wire.beginTransmission(address);         // Initialize the Tx buffer
	Wire.write(subAddress);	                 // Put slave register address in Tx buffer
	Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
	Wire.requestFrom(address, 1);            // Read one byte from slave register address 
	data = Wire.read();                      // Fill Rx buffer with result
	return data;                             // Return data read from slave register
}

  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
	Wire.beginTransmission(address);   // Initialize the Tx buffer
	Wire.write(subAddress);            // Put slave register address in Tx buffer
	Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
	uint8_t i = 0;
  Wire.requestFrom(address, count);  // Read bytes from slave register address 
	while (Wire.available()) {
        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer
}


================================================
FILE: Bosch/readme.md
================================================
Collection of sketches for Bosch sensors. I use [this](https://www.tindie.com/products/onehorse/bno-055-9-axis-motion-sensor-with-hardware-fusion/) for the BNO055 breakout board.

![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-08-06T00:24:08.896Z-BNO055.top.jpg.855x570_q85_pad_rcrop.jpg)


================================================
FILE: CCS811_BME280_ESP32C3Mini_WebServer/BME280.cpp
================================================
/* 06/16/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 This sketch uses SDA/SCL on pins 42/43 (back pads), respectively, and it uses the Dragonfly STM32L476RE Breakout Board.
 The BME280 is a simple but high resolution pressure/humidity/temperature 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.
 
 Library may be used freely and without limit with attribution.
 
*/
 
#include "BME280.h"
#include "I2Cdev.h"

#define USBSerial Serial

BME280::BME280(I2Cdev* i2c_bus)
{
  _i2c_bus = i2c_bus;
}


uint8_t BME280::getChipID()
{
  uint8_t c = _i2c_bus->readByte(BME280_ADDRESS, BME280_ID);
  return c;
}

void BME280::resetBME280()
{
  _i2c_bus->writeByte(BME280_ADDRESS, BME280_RESET, 0xB6);
}


int32_t BME280::readBME280Temperature()
{
  uint8_t rawData[3];  // 20-bit temperature register data stored here
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_TEMP_MSB, 3, &rawData[0]);  
  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);
}


int32_t BME280::readBME280Pressure()
{
  uint8_t rawData[3];  // 20-bit pressure register data stored here
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_PRESS_MSB, 3, &rawData[0]);  
  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);
}


int32_t BME280::BME280::readBME280Humidity()
{
  uint8_t rawData[2];  // 16-bit humiditye register data stored here
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_HUM_MSB, 2, &rawData[0]);  
  return (uint32_t) (((uint32_t) rawData[0] << 24 | (uint32_t) rawData[1] << 16) ) >> 16;
}


void BME280::BME280forced()
{
    uint8_t temp = _i2c_bus->readByte(BME280_ADDRESS, BME280_CTRL_MEAS);
    _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, temp | Forced);
 
    while( (_i2c_bus->readByte(BME280_ADDRESS, BME280_STATUS)) & 0x10) { } // wait for conversion byte to clear
}


void BME280::BME280Init(uint8_t Posr, uint8_t Hosr, uint8_t Tosr, uint8_t Mode, uint8_t IIRFilter, uint8_t SBy)
{
  // Configure the BME280
  // Set H oversampling rate
  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_HUM, 0x07 & Hosr);
  // Set T and P oversampling rates and sensor mode
  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);
  // Set standby time interval in normal mode and bandwidth
  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CONFIG, SBy << 5 | IIRFilter << 2);
  
  uint8_t calib[26];
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_CALIB00, 26, &calib[0]);
  _dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);
  _dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);
  _dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);
  _dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);
  _dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);
  _dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);
  _dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);
  _dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);
  _dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);
  _dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);
  _dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);
  _dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);
  _dig_H1 = calib[25];
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_CALIB26, 7, &calib[0]);
  _dig_H2 = ( int16_t)((( int16_t) calib[1] << 8) | calib[0]);
  _dig_H3 = calib[2];
  _dig_H4 = ( int16_t)(((( int16_t) calib[3] << 8) | (0x0F & calib[4]) << 4) >> 4);
  _dig_H5 = ( int16_t)(((( int16_t) calib[5] << 8) | (0xF0 & calib[4]) ) >> 4 );
  _dig_H6 = calib[6];
  Serial.println("Calibration coeficients:");
  Serial.print("_dig_T1 ="); 
  Serial.println(_dig_T1);
  Serial.print("_dig_T2 ="); 
  Serial.println(_dig_T2);
  Serial.print("_dig_T3 ="); 
  Serial.println(_dig_T3);
  Serial.print("_dig_P1 ="); 
  Serial.println(_dig_P1);
  Serial.print("_dig_P2 ="); 
  Serial.println(_dig_P2);
  Serial.print("_dig_P3 ="); 
  Serial.println(_dig_P3);
  Serial.print("_dig_P4 ="); 
  Serial.println(_dig_P4);
  Serial.print("_dig_P5 ="); 
  Serial.println(_dig_P5);
  Serial.print("_dig_P6 ="); 
  Serial.println(_dig_P6);
  Serial.print("_dig_P7 ="); 
  Serial.println(_dig_P7);
  Serial.print("_dig_P8 ="); 
  Serial.println(_dig_P8);
  Serial.print("_dig_P9 ="); 
  Serial.println(_dig_P9);
  Serial.print("_dig_H1 ="); 
  Serial.println(_dig_H1);
  Serial.print("_dig_H2 ="); 
  Serial.println(_dig_H2);
  Serial.print("_dig_H3 ="); 
  Serial.println(_dig_H3);
  Serial.print("_dig_H4 ="); 
  Serial.println(_dig_H4);
  Serial.print("_dig_H5 ="); 
  Serial.println(_dig_H5);
  Serial.print("_dig_H6 ="); 
  Serial.println(_dig_H6);
}


// Returns temperature in DegC, resolution is 0.01 DegC. Output value of
// “5123” equals 51.23 DegC.
int32_t BME280::BME280_compensate_T(int32_t adc_T)
{
  int32_t var1, var2, T;
  var1 = ((((adc_T >> 3) - ((int32_t)_dig_T1 << 1))) * ((int32_t)_dig_T2)) >> 11;
  var2 = (((((adc_T >> 4) - ((int32_t)_dig_T1)) * ((adc_T >> 4) - ((int32_t)_dig_T1))) >> 12) * ((int32_t)_dig_T3)) >> 14;
  _t_fine = var1 + var2;
  T = (_t_fine * 5 + 128) >> 8;
  return T;
}


// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8
//fractional bits).
//Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
uint32_t BME280::BME280_compensate_P(int32_t adc_P)
{
  long long var1, var2, p;
  var1 = ((long long)_t_fine) - 128000;
  var2 = var1 * var1 * (long long)_dig_P6;
  var2 = var2 + ((var1*(long long)_dig_P5)<<17);
  var2 = var2 + (((long long)_dig_P4)<<35);
  var1 = ((var1 * var1 * (long long)_dig_P3)>>8) + ((var1 * (long long)_dig_P2)<<12);
  var1 = (((((long long)1)<<47)+var1))*((long long)_dig_P1)>>33;
  if(var1 == 0)
  {
    return 0;
    // avoid exception caused by division by zero
  }
  p = 1048576 - adc_P;
  p = (((p<<31) - var2)*3125)/var1;
  var1 = (((long long)_dig_P9) * (p>>13) * (p>>13)) >> 25;
  var2 = (((long long)_dig_P8) * p)>> 19;
  p = ((p + var1 + var2) >> 8) + (((long long)_dig_P7)<<4);
  return (uint32_t)p;
}


// Returns humidity in %RH as unsigned 32 bit integer in Q22.10 format (22integer and 10fractional bits).
// Output value of “47445”represents 47445/1024= 46.333%RH
uint32_t BME280::BME280_compensate_H(int32_t adc_H)
{
int32_t var;

var = (_t_fine - ((int32_t)76800));
var = (((((adc_H << 14) - (((int32_t)_dig_H4) << 20) - (((int32_t)_dig_H5) * var)) +
((int32_t)16384)) >> 15) * (((((((var * ((int32_t)_dig_H6)) >> 10) * (((var *
((int32_t)_dig_H3)) >> 11) + ((int32_t)32768))) >> 10) + ((int32_t)2097152)) * ((int32_t)_dig_H2) + 8192) >> 14));
var = (var - (((((var >> 15) * (var >> 15)) >> 7) * ((int32_t)_dig_H1)) >> 4));
var = (var < 0 ? 0 : var); 
var = (var > 419430400 ? 419430400 : var);
return(uint32_t)(var >> 12);
}


================================================
FILE: CCS811_BME280_ESP32C3Mini_WebServer/BME280.h
================================================
/* 06/16/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 This sketch uses SDA/SCL on pins 42/43 (back pads), respectively, and it uses the Dragonfly STM32L476RE Breakout Board.
 The BME280 is a simple but high resolution pressure/humidity/temperature 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.
 
 Library may be used freely and without limit with attribution.
 
*/
  
#ifndef BME280_h
#define BME280_h

#include "Arduino.h"
#include "I2CDev.h"
#include <Wire.h>

/* BME280 registers
*  http://www.mouser.com/ds/2/783/BST-BME280_DS001-11-844833.pdf
*/
#define BME280_HUM_LSB    0xFE
#define BME280_HUM_MSB    0xFD
#define BME280_TEMP_XLSB  0xFC
#define BME280_TEMP_LSB   0xFB
#define BME280_TEMP_MSB   0xFA
#define BME280_PRESS_XLSB 0xF9
#define BME280_PRESS_LSB  0xF8
#define BME280_PRESS_MSB  0xF7
#define BME280_CONFIG     0xF5
#define BME280_CTRL_MEAS  0xF4
#define BME280_STATUS     0xF3
#define BME280_CTRL_HUM   0xF2
#define BME280_RESET      0xE0
#define BME280_ID         0xD0  // should be 0x60
#define BME280_CALIB00    0x88
#define BME280_CALIB26    0xE1

#define BME280_ADDRESS           0x76   // Address of BMP280 altimeter when ADO = 0


#define  P_OSR_01 0x01
#define  P_OSR_02 0x02
#define  P_OSR_04 0x03
#define  P_OSR_08 0x04
#define  P_OSR_16 0x05

#define  H_OSR_01 0x01
#define  H_OSR_02 0x02
#define  H_OSR_04 0x03
#define  H_OSR_08 0x04
#define  H_OSR_16 0x05

#define  T_OSR_01 0x01
#define  T_OSR_02 0x02
#define  T_OSR_04 0x03
#define  T_OSR_08 0x04
#define  T_OSR_16 0x05

#define  full       0x00
#define  BW0_223ODR 0x01
#define  BW0_092ODR 0x02
#define  BW0_042ODR 0x03
#define  BW0_021ODR 0x04  

#define  BME280Sleep 0x00
#define  Forced      0x01
#define  Forced2     0x02
#define  Normal      0x03

#define  t_00_5ms 0x00
#define  t_62_5ms 0x01
#define  t_125ms  0x02
#define  t_250ms  0x03
#define  t_500ms  0x04
#define  t_1000ms 0x05
#define  t_10ms   0x06
#define  t_20ms   0x07 


class BME280
{
  public: 
  BME280(I2Cdev* i2c_bus);
  uint8_t getChipID();
  void resetBME280();
  int32_t readBME280Temperature();
  int32_t readBME280Pressure();
  int32_t readBME280Humidity();
  void BME280forced();
  void BME280Init(uint8_t Posr, uint8_t Hosr, uint8_t Tosr, uint8_t Mode, uint8_t IIRFilter, uint8_t SBy);
  int32_t  BME280_compensate_T(int32_t adc_T);
  uint32_t BME280_compensate_P(int32_t adc_P);
  uint32_t BME280_compensate_H(int32_t adc_H);
  
  private:
  uint8_t  _dig_H1, _dig_H3, _dig_H6;
  uint16_t _dig_T1, _dig_P1, _dig_H4, _dig_H5;
  int16_t  _dig_T2, _dig_T3, _dig_P2, _dig_P3, _dig_P4, _dig_P5, _dig_P6, _dig_P7, _dig_P8, _dig_P9, _dig_H2;
  int32_t  _t_fine;
  I2Cdev* _i2c_bus;
  };

#endif


================================================
FILE: CCS811_BME280_ESP32C3Mini_WebServer/CCS811.cpp
================================================
/* 06/16/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 *  The AMS CCS811 is an air quality sensor that provides equivalent CO2 and volatile organic measurements from direct
 *  I2C register reads as well as current and voltage (effective resistance of the sensing element). Gas sensors, including 
 *  this MEMs gas sensor in the CCS811 measure resistance of a substrate that changes when exposed to inert gasses and 
 *  volatile organic compounds. Changed in concentration vary exponentially with the changes in resistance. The CCS811
 *  has an embedded ASIC calibrated against most common indoor pollutants that returns a good estimate of
 *  equivalent CO2 concentration in parts per million (400 - 8192 range) and volatile organic componds in parts per billion (0 - 1187).
 *  The sensor is quite sensitive to breath and other human emissions.
 *  
 *  Library may be used freely and without limit with attribution.
 *  
 */
#include "CCS811.h"
#include "I2Cdev.h"

#define USBSerial Serial

CCS811::CCS811(I2Cdev* i2c_bus)
{
  _i2c_bus = i2c_bus;   
}


uint8_t CCS811::getChipID()
{
 byte e = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ID);  // Read WHO_AM_I register for CCS8110
 return e;
}


void CCS811::checkCCS811Status() 
{
   // Check CCS811 status
  uint8_t status = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_STATUS);
  Serial.print("status = 0X"); Serial.println(status, HEX);
  if(status & 0x80) {Serial.println("Firmware is in application mode. CCS811 is ready!");}
  else { Serial.println("Firmware is in boot mode!");}
  
  if(status & 0x10) {Serial.println("Valid application firmware loaded!");}
  else { Serial.println("No application firmware is loaded!");}

  if(status & 0x08) {Serial.println("New data available!");}
  else { Serial.println("No new data available!");}

  if(status & 0x01) {Serial.println("Error detected!");
        uint8_t error = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ERROR_ID);
        if(error & 0x01) Serial.println("CCS811 received invalid I2C write request!");
        if(error & 0x02) Serial.println("CCS811 received invalid I2C read request!");
        if(error & 0x04) Serial.println("CCS811 received unsupported mode request!");
        if(error & 0x08) Serial.println("Sensor resistance measurement at maximum range!");
        if(error & 0x10) Serial.println("Heater current is not in range!");
        if(error & 0x20) Serial.println("Heater voltage is not being applied correctly!");
  }
  else { Serial.println("No error detected!");
  }
  
  Serial.println(" ");
  
  }


  void CCS811::CCS811init(uint8_t AQRate)
  {
    // initialize CCS811 and check version and status
  byte HWVersion = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_HW_VERSION);
  Serial.print("CCS811 Hardware Version = 0x"); Serial.println(HWVersion, HEX); 

  uint8_t FWBootVersion[2] = {0, 0}, FWAppVersion[2] = {0,0};
  _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_FW_BOOT_VERSION, 2, &FWBootVersion[0]);
  Serial.println("CCS811 Firmware Boot Version: "); 
  Serial.print("Major = "); Serial.println((FWBootVersion[0] & 0xF0) >> 4); 
  Serial.print("Minor = "); Serial.println(FWBootVersion[0] & 0x04); 
  Serial.print("Trivial = "); Serial.println(FWBootVersion[1]); 
  
  _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_FW_APP_VERSION, 2, &FWAppVersion[0]);
  Serial.println("CCS811 Firmware App Version: "); 
  Serial.print("Major = "); Serial.println((FWAppVersion[0] & 0xF0) >> 4); 
  Serial.print("Minor = "); Serial.println(FWAppVersion[0] & 0x04); 
  Serial.print("Trivial = "); Serial.println(FWAppVersion[1]); 

  // Check CCS811 status
  checkCCS811Status();
  _i2c_bus->writeReg(CCS811_ADDRESS, CCS811_APP_START);
  delay(100);
  checkCCS811Status();

  // set CCS811 measurement mode
  _i2c_bus->writeByte(CCS811_ADDRESS, CCS811_MEAS_MODE, AQRate << 4 | 0x08); // pulsed heating mode, enable interrupt
  uint8_t measmode = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_MEAS_MODE);
  Serial.print("Confirm measurement mode = 0x"); Serial.println(measmode, HEX);
  }


  void CCS811::compensateCCS811(int32_t compHumidity, int32_t compTemp)
  {
      // Update CCS811 humidity and temperature compensation
      uint8_t temp[5] = {0, 0, 0, 0, 0};
      temp[0] = CCS811_ENV_DATA;
      temp[1] = ((compHumidity % 1024) / 100) > 7 ? (compHumidity/1024 + 1)<<1 : (compHumidity/1024)<<1;
      temp[2] = 0;
      if(((compHumidity % 1024) / 100) > 2 && (((compHumidity % 1024) / 100) < 8))
      {
       temp[1] |= 1;
      }

      compTemp += 2500;
      temp[3] = ((compTemp % 100) / 100) > 7 ? (compTemp/100 + 1)<<1 : (compTemp/100)<<1;
      temp[4] = 0;
      if(((compTemp % 100) / 100) > 2 && (((compTemp % 100) / 100) < 8))
      {
       temp[3] |= 1;
      }

//      Wire.transfer(CCS811_ADDRESS, &temp[0], 5, NULL, 0);
     _i2c_bus->writeBytes(CCS811_ADDRESS, CCS811_ENV_DATA, 4, &temp[1]);
  }

   void CCS811::readCCS811Data(uint8_t * destination)
   {
      uint8_t rawData[8] = {0, 0, 0, 0, 0, 0, 0, 0};
      uint8_t status = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_STATUS);
      
      if(status & 0x01) { // check for errors
        uint8_t error = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ERROR_ID);
        if(error & 0x01) Serial.println("CCS811 received invalid I2C write request!");
        if(error & 0x02) Serial.println("CCS811 received invalid I2C read request!");
        if(error & 0x04) Serial.println("CCS811 received unsupported mode request!");
        if(error & 0x08) Serial.println("Sensor resistance measurement at maximum range!");
        if(error & 0x10) Serial.println("Heater current is not in range!");
        if(error & 0x20) Serial.println("Heater voltage is not being applied correctly!");
      }

      _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_ALG_RESULT_DATA, 8, &rawData[0]);

   for(int ii = 0; ii < 8; ii++)
   {
    destination[ii] = rawData[ii];
   }
   
   }


================================================
FILE: CCS811_BME280_ESP32C3Mini_WebServer/CCS811.h
================================================
#ifndef CCS811_h
#define CCS811_h

#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"

/* CCS811 Registers
http://www.mouser.com/ds/2/588/CCS811_DS000459_3-00-1098798.pdf
*/
#define CCS811_STATUS             0x00
#define CCS811_MEAS_MODE          0x01
#define CCS811_ALG_RESULT_DATA    0x02
#define CCS811_RAW_DATA           0x03
#define CCS811_ENV_DATA           0x05
#define CCS811_NTC                0x06
#define CCS811_THRESHOLDS         0x10
#define CCS811_BASELINE           0x11
#define CCS811_HW_ID              0x20  // WHO_AM_I should be 0x81
#define CCS811_ID                 0x20  // WHO_AM_I should be 0x1X
#define CCS811_HW_VERSION         0x21  
#define CCS811_FW_BOOT_VERSION    0x23
#define CCS811_FW_APP_VERSION     0x24
#define CCS811_ERROR_ID           0xE0
#define CCS811_APP_START          0xF4
#define CCS811_SW_RESET           0xFF

#define CCS811_ADDRESS            0x5A   // Address of the CCS811 Air Quality Sensor

#define  dt_idle  0x00
#define  dt_1sec  0x01
#define  dt_10sec 0x02
#define  dt_60sec 0x03

class CCS811
{
  public: 
  CCS811(I2Cdev* i2c_bus);
  void checkCCS811Status();
  void CCS811init(uint8_t AQRate);
  uint8_t getChipID();
  void compensateCCS811(int32_t compHumidity, int32_t compTemp);
  void readCCS811Data(uint8_t * destination);
  
  private:
  I2Cdev* _i2c_bus;
};


#endif


================================================
FILE: CCS811_BME280_ESP32C3Mini_WebServer/CCS811_BME280_ESP32C3Mini_WebServer.ino
================================================
/* 06/16/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 *  The AMS CCS811 is an air quality sensor that provides equivalent CO2 and volatile organic measurements from direct
 *  I2C register reads as well as current and voltage (effective resistance of the sensing element). Gas sensors, including 
 *  this MEMs gas sensor in the CCS811 measure resistance of a substrate that changes when exposed to inert gasses and 
 *  volatile organic compounds. Changed in concentration vary exponentially with the changes in resistance. The CCS811
 *  has an embedded ASIC calibrated against most common indoor pollutants that returns a good estimate of
 *  equivalent CO2 concentration in parts per million (400 - 8192 range) and volatile organic componds in parts per billion (0 - 1187).
 *  The sensor is quite sensitive to breath and other human emissions.
 *  
 *  This sketch uses default SDA/SCL pins on the Ladybug development board.
 *  The BME280 is a simple but high resolution pressure/humidity/temperature 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.
 
    Library may be used freely and without limit with attribution.
 
  */
#include <Arduino.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include <WebServer.h>
#include <ESPmDNS.h>
#include <driver/adc.h>
#include "USB.h"
#include "BME280.h"
#include "CCS811.h"
#include "I2Cdev.h"

#define Serial USBSerial

const char* ssid = "XXXXXXXXX";
const char* password = "XXXXXXXXXXXX";

WebServer server(80);

String webString="";     // String to display

void handleRoot() {
  server.send(200, "text/plain", "Hello from the environmental monitroing station ESP32!");
  delay(100);
}

  String createHTML(float var1, float var2, float var3, float var4, float var5, float var6, float var7, float var8) {
   webString = "<html><head><meta http-equiv=\"Refresh\" content=\"5\"></head><body><UL>"

  +String("<LI>Temperature = ")+String(var1)+String(" C</LI>") 
  +String("<LI>Temperature = ")+String(var2)+String(" F</LI>") 
  +String("<LI>Pressure = ")+String(var3)+String(" milliBar</LI>") 
  +String("<LI>Altitude = ")+String(var4)+String(" feet</LI>") 
  +String("<LI>Humidity = ")+String(var5)+String(" %RH</LI>") 
  +String("<LI>eCO2 = ")+String(var6)+String(" ppm</LI>") 
  +String("<LI>TVOC = ")+String(var7)+String(" ppb</LI>") 
  +String("<LI>Battery Voltage = ")+String(var8)+String(" V</LI>") 
     
  +"</UL></body></html>" ;
   return webString;
  }
  
void handleNotFound() {
  String message = "File Not Found\n\n";
  message += "URI: ";
  message += server.uri();
  message += "\nMethod: ";
  message += (server.method() == HTTP_GET) ? "GET" : "POST";
  message += "\nArguments: ";
  message += server.args();
  message += "\n";
  for (uint8_t i = 0; i < server.args(); i++) {
    message += " " + server.argName(i) + ": " + server.arg(i) + "\n";
  }
  server.send(404, "text/plain", message);
}

/* create an ESP32 hardware timer */
hw_timer_t * timer = NULL;
volatile bool alarmFlag = false;

void IRAM_ATTR onTimer(){
  alarmFlag = true;
}

#define I2C_BUS          Wire               // Define the I2C bus (Wire instance) you wish to use

I2Cdev                   i2c_0(&I2C_BUS);   // Instantiate the I2Cdev object and point to the desired I2C bus

#define SerialDebug true  // set to true to get Serial output for debugging
#define       myLed    2 // green led
const uint8_t myBat =  1;

float VBat = 0.0f;
uint32_t chipId = 0;

// BME280 definitions
/* Specify BME280 configuration
 *  Choices are:
 P_OSR_01, P_OSR_02, P_OSR_04, P_OSR_08, P_OSR_16 // pressure oversampling
 H_OSR_01, H_OSR_02, H_OSR_04, H_OSR_08, H_OSR_16 // humidity oversampling
 T_OSR_01, T_OSR_02, T_OSR_04, T_OSR_08, T_OSR_16 // temperature oversampling
 full, BW0_223ODR,BW0_092ODR, BW0_042ODR, BW0_021ODR // bandwidth at 0.021 x sample rate
 BME280Sleep, forced,, forced2, normal //operation modes
 t_00_5ms = 0, t_62_5ms, t_125ms, t_250ms, t_500ms, t_1000ms, t_10ms, t_20ms // determines sample rate
 */
uint8_t Posr = P_OSR_01, Hosr = H_OSR_01, Tosr = T_OSR_01, Mode = BME280Sleep, IIRFilter = full, SBy = t_1000ms;     // set pressure amd temperature output data rate

float Temperature, Pressure, Humidity;              // stores BME280 pressures sensor pressure and temperature
int32_t rawPress, rawTemp, rawHumidity, compTemp;   // pressure and temperature raw count output for BME280
uint32_t compHumidity, compPress;                   // variables to hold raw BME280 humidity value

float temperature_C, temperature_F, pressure, humidity, altitude; // Scaled output of the BME280

BME280 BME280(&i2c_0); // instantiate BME280 class


// CCS811 definitions
#define CCS811_intPin  4
#define CCS811_wakePin 7

/* Specify CCS811 sensor parameters
 *  Choices are   dt_idle , dt_1sec, dt_10sec, dt_60sec
 */
uint8_t AQRate = dt_10sec;  // set the sample rate
uint8_t rawData[8] = {0, 0, 0, 0, 0, 0, 0, 0};  // array to hold the raw data
uint16_t eCO2 = 0, TVOC = 0;
uint8_t Current = 0;
float Voltage = 0.0f;

volatile bool newCCS811Data  = true; // boolean flag for interrupt

CCS811 CCS811(&i2c_0); // instantiate CCS811 class


void setup()
{
  Serial.begin(115200);
  delay(4000);
  Serial.println("Serial enabled!");

  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, LOW); // start with led on, active LOW

  for(int i=0; i<17; i=i+8) {
    chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xFF) << i;
  }

  Serial.printf("ESP32 Chip model = %s Rev %d\n", ESP.getChipModel(), ESP.getChipRevision());
  Serial.printf("This chip has %d cores\n", ESP.getChipCores());
  Serial.print("Chip ID: "); Serial.println(chipId);

  adc1_config_width(ADC_WIDTH_BIT_12);
  adc1_config_channel_atten(ADC1_CHANNEL_1, ADC_ATTEN_DB_11);

  pinMode(myBat, INPUT); // battery voltage monitor

  pinMode(CCS811_intPin, INPUT_PULLUP); // active LOW

  I2C_BUS.begin(0, 3);               // Set master mode, default on SDA on pin 0/SCL  on pin 3
  I2C_BUS.setClock(400000);          // I2C frequency at 400 kHz
  delay(1000);

  pinMode(CCS811_wakePin, OUTPUT);
  //Enable the CCS811 for I2C scan
  digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
  
  Serial.println("Scan for I2C devices:");
  i2c_0.I2Cscan();                   // should detect BME280 at 0x76 and CCS811 at 0x5A
  delay(1000);

  //Disable the CCS811 for I2C scan
  digitalWrite(CCS811_wakePin, HIGH); // set LOW to enable the CCS811 air quality sensor
  delay(1000);

  // Read the WHO_AM_I register of the BME280 this is a good test of communication
  byte BME280ChipID = BME280.getChipID();  // Read WHO_AM_I register for BME280
  Serial.print("BME280 "); Serial.print("I AM "); Serial.print(BME280ChipID, HEX); Serial.print(" I should be "); Serial.println(0x60, HEX);
  Serial.println(" ");
  delay(1000); 

  // Read the WHO_AM_I register of the CCS811 this is a good test of communication
  digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
  byte CCS811ChipID = CCS811.getChipID();
  digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor
  Serial.print("CCS811 "); Serial.print("I AM "); Serial.print(CCS811ChipID, HEX); Serial.print(" I should be "); Serial.println(0x81, HEX);
  Serial.println(" ");
  delay(1000); 
  
  if(BME280ChipID == 0x60 && CCS811ChipID == 0x81 ) {

   Serial.println("BME280+CCS811 are online..."); Serial.println(" ");
   digitalWrite(myLed, LOW);

   BME280.resetBME280();                                                        // reset BME280 before initilization
   delay(100);
   BME280.BME280Init(Posr, Hosr, Tosr, Mode, IIRFilter, SBy);                   // Initialize BME280 altimeter
   BME280.BME280forced();                                                       // get initial data sample, then go back to sleep

   // initialize CCS811 and check version and status
   digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
   CCS811.CCS811init(AQRate);
   digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor

   }
   else {
   if(BME280ChipID != 0x60) Serial.println(" BME280 not functioning!");    
   if(CCS811ChipID != 0x81) Serial.println(" CCS811 not functioning!");  
   while(1) { }; // no point proceeding, so wait here forever...
   }

  // Set up ESP32 timer
  /* Use 1st timer of 4 */
  /* 1 tick take 1/(80MHZ/80) = 1us so we set divider 80 and count up */
  timer = timerBegin(0, 80, true);

  /* Attach onTimer function to our timer */
  timerAttachInterrupt(timer, &onTimer, true);

  /* Set alarm to call onTimer function every second 1 tick is 1us
  => 1 second is 1000000us */
  /* Repeat the alarm (third parameter) */
  timerAlarmWrite(timer, 10000000, true); // update every ten seconds

  /* Start an alarm */
  timerAlarmEnable(timer);
  Serial.println("start timer");

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);
  Serial.println("");

  // Wait for connection
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());

  if (MDNS.begin("esp32")) {
    Serial.println("MDNS responder started");
  }

  server.on("/", handleRoot);

  server.onNotFound(handleNotFound);

  server.on("/ESP32Data", [](){ // define web server data

    // BME280 Data
    rawTemp =   BME280.readBME280Temperature();
    temperature_C = (float) BME280.BME280_compensate_T(rawTemp)/100.0f; // temperature in Centigrade
    temperature_F = 9.0f*temperature_C/5.0f + 32.0f;
    rawPress =  BME280.readBME280Pressure();
    pressure = (float) BME280.BME280_compensate_P(rawPress)/25600.0f; // Pressure in millibar
    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));
    rawHumidity =  BME280.readBME280Humidity();
    humidity = (float) BME280.BME280_compensate_H(rawHumidity)/1024.0f; // Humidity in %RH

    // CCS811 Data
    digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
    CCS811.readCCS811Data(rawData);
    CCS811.compensateCCS811(compHumidity, compTemp); // compensate CCS811 using BME280 humidity and temperature
    digitalWrite(CCS811_wakePin, HIGH); // set LOW to enable the CCS811 air quality sensor 

    eCO2 = (uint16_t) ((uint16_t) rawData[0] << 8 | rawData[1]);
    TVOC = (uint16_t) ((uint16_t) rawData[2] << 8 | rawData[3]);
    Current = (rawData[6] & 0xFC) >> 2;
    Voltage = (float) ((uint16_t) ((((uint16_t)rawData[6] & 0x02) << 8) | rawData[7])) * (1.65f/1023.0f); 

    // Battery Voltage
    VBat = 2.0f * 2.60f * 1.15f * ((float) adc1_get_raw((adc1_channel_t)1)) / 4095.0f;
    
    createHTML(temperature_C, temperature_F, pressure, altitude, humidity, eCO2, TVOC, VBat);
    server.send(200, "text/html", webString);               // send to someone's browser when asked
  });

  server.begin();
  Serial.println("HTTP server started");
  
  attachInterrupt(CCS811_intPin,  myinthandler, FALLING); // enable CCS811 interrupt 
}

void loop()
{  
    // CCS811 data
    // If intPin goes LOW, all data registers have new data
    if(newCCS811Data == true) {  // On interrupt, read data
    newCCS811Data = false;  // reset newData flag
     
    digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
    CCS811.readCCS811Data(rawData);
    CCS811.compensateCCS811(compHumidity, compTemp); // compensate CCS811 using BME280 humidity and temperature
    digitalWrite(CCS811_wakePin, HIGH); // set LOW to enable the CCS811 air quality sensor 

    eCO2 = (uint16_t) ((uint16_t) rawData[0] << 8 | rawData[1]);
    TVOC = (uint16_t) ((uint16_t) rawData[2] << 8 | rawData[3]);
    Current = (rawData[6] & 0xFC) >> 2;
    Voltage = (float) ((uint16_t) ((((uint16_t)rawData[6] & 0x02) << 8) | rawData[7])) * (1.65f/1023.0f); 
    
    Serial.println("CCS811:");
    Serial.print("Eq CO2 in ppm = "); Serial.println(eCO2);
    Serial.print("TVOC in ppb = "); Serial.println(TVOC);
    Serial.print("Sensor current (uA) = "); Serial.println(Current);
    Serial.print("Sensor voltage (V) = "); Serial.println(Voltage, 2);  
    Serial.println(" ");
  }
            
   /*RTC Timer*/
   if (alarmFlag) { // update serial output whenever there is a timer alarm
      alarmFlag = false;

    /* BME280 sensor data */
    BME280.BME280forced();  // get one data sample, then go back to sleep

    rawTemp =  BME280.readBME280Temperature();
    compTemp = BME280.BME280_compensate_T(rawTemp);
    temperature_C = (float) compTemp/100.0f;
    temperature_F = 9.0f*temperature_C/5.0f + 32.0f;
     
    rawPress =  BME280.readBME280Pressure();
    pressure = (float) BME280.BME280_compensate_P(rawPress)/25600.f; // Pressure in mbar
    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));   
   
    rawHumidity =  BME280.readBME280Humidity();
    compHumidity = BME280.BME280_compensate_H(rawHumidity);
    humidity = (float)compHumidity/1024.0f; // Humidity in %RH
 
    Serial.println("BME280:");
    Serial.print("Altimeter temperature = "); 
    Serial.print( temperature_C, 2); 
    Serial.println(" C"); // temperature in degrees Celsius
    Serial.print("Altimeter temperature = "); 
    Serial.print(temperature_F, 2); 
    Serial.println(" F"); // temperature in degrees Fahrenheit
    Serial.print("Altimeter pressure = "); 
    Serial.print(pressure, 2);  
    Serial.println(" mbar");// pressure in millibar
    Serial.print("Altitude = "); 
    Serial.print(altitude, 2); 
    Serial.println(" feet");
    Serial.print("Altimeter humidity = "); 
    Serial.print(humidity, 1);  
    Serial.println(" %RH");// pressure in millibar
    Serial.println(" ");

    uint16_t Counts = adc1_get_raw((adc1_channel_t)1);
    /* ADC at attenuation 11 should rad from 0 to 2.6 V nominally. The resistor divider
     *  is 1/2, and a calibration factor of 1.15 is applied to bring measurements into 
     *  agreement with multimeter */
    VBat = 2.0f * 2.60f * 1.15f * ((float) Counts) / 4095.0f;
    Serial.print("Battery voltage = "); Serial.print(VBat, 2); Serial.println(" V");

    server.handleClient(); // update web server page
      
    digitalWrite(myLed, LOW); delay(10); digitalWrite(myLed, HIGH); // blink led at end of loop
    }  
    
 yield(); //allow the cpu to switch to other tasks
//     ESP32.sleep();    // time out in deep sleep mode to save power
}

//===================================================================================================================
//====== Set of useful functions
//===================================================================================================================

void myinthandler()
{
  newCCS811Data = true;
}


void alarmMatch()
{
  alarmFlag = true;
}


================================================
FILE: CCS811_BME280_ESP32C3Mini_WebServer/I2CDev.cpp
================================================
/*
 * Copyright (c) 2018 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#include "Arduino.h"
#include "I2Cdev.h"

#define USBSerial Serial

I2Cdev::I2Cdev(TwoWire* i2c_bus)                                                                                                             // Class constructor
{
  _i2c_bus = i2c_bus;
}

I2Cdev::~I2Cdev()                                                                                                                            // Class destructor
{
}

/**
* @fn: readByte(uint8_t address, uint8_t subAddress)
*
* @brief: Read one byte from an I2C device
* 
* @params: I2C slave device address, Register subAddress
* @returns: unsigned short read
*/
uint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)
{
  uint8_t data = 0;                             // `data` will store the register data   
  _i2c_bus->beginTransmission(address);         // Initialize the Tx buffer
  _i2c_bus->write(subAddress);                  // Put slave register address in Tx buffer
  _i2c_bus->endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
  _i2c_bus->requestFrom(address, 1);            // Read one byte from slave register address  
  data = _i2c_bus->read();                      // Fill Rx buffer with result
  return data;                                  // Return data read from slave register
  
}


/**
* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
*
* @brief: Read multiple bytes from an I2C device
* 
* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data
* @returns: void
*/
void I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
  _i2c_bus->beginTransmission(address);   // Initialize the Tx buffer
  _i2c_bus->write(subAddress);            // Put slave register address in Tx buffer
  _i2c_bus->endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
  uint8_t i = 0;
  _i2c_bus->requestFrom(address, count);  // Read bytes from slave register address 
  while (_i2c_bus->available()) {
        dest[i++] = _i2c_bus->read(); }   // Put read results in the Rx buffer
}


/**
* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write one byte to an I2C device
* 
* @params: I2C slave device address, Register subAddress, data to be written
* @returns: void
*/
void I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
{
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  _i2c_bus->write(regAddr);              // Put slave register address in Tx buffer
  _i2c_bus->write(data);                 // Put data in Tx buffer
  _i2c_bus->endTransmission();           // Send the Tx buffer
}

/**
* @fn: writeReg(uint8_t devAddr, uint8_t regAddr)
*
* @brief: Write register to an I2C device
* 
* @params: I2C slave device address, Register subAddress, data to be written
* @returns: void
*/
void I2Cdev::writeReg(uint8_t devAddr, uint8_t regAddr)
{
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  _i2c_bus->write(regAddr);              // Put slave register address in Tx buffer
  _i2c_bus->endTransmission();           // Send the Tx buffer
}


/**
* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write multiple bytes to an I2C device
* 
* @params: I2C slave device address, Register subAddress, byte count, data array to be written
* @returns: void
*/
void I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)
{
  uint8_t temp[1 + count];
  
  temp[0] = regAddr;
  for (uint8_t ii = 0; ii < count; ii++)
  { 
    temp[ii + 1] = dest[ii];
  }
  
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  
  for (uint8_t jj = 0; jj < count + 1; jj++)
  {
  _i2c_bus->write(temp[jj]);            // Put data in Tx buffer
  }
  
  _i2c_bus->endTransmission();          // Send the Tx buffer
}



/**
* @fn:I2Cscan()
* @brief: Scan the I2C bus for active I2C slave devices
* 
* @params: void
* @returns: void
*/
void I2Cdev::I2Cscan() 
{
  // 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 Wire.endTransmisstion to see if a device did acknowledge to the address.
    _i2c_bus->beginTransmission(address);
    error = _i2c_bus->endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
      Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");
      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknow error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("I2C scan complete\n");
}


================================================
FILE: CCS811_BME280_ESP32C3Mini_WebServer/I2CDev.h
================================================
/*
 * Copyright (c) 2018 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#ifndef _I2CDEV_H_
#define _I2CDEV_H_

#include <Wire.h>

class I2Cdev {
    public:
                                        I2Cdev(TwoWire*);
                                        ~I2Cdev();                                                                                                                     // Class destructor for durable instances
         uint8_t                        readByte(uint8_t address, uint8_t subAddress);
         void                           readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
         void                           writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
         void                           writeReg(uint8_t devAddr, uint8_t regAddr);
         void                           writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);
         void                           I2Cscan();
    private:
         TwoWire*                       _i2c_bus;                                                                                                                      // Class constructor argument
};

#endif //_I2CDEV_H_


================================================
FILE: CCS811_BME280_ESP32C3Mini_WebServer/Readme.md
================================================
Environmental Monitoring sketch

Demonstration of a simple webserver pushing data from BME280 (pressure, temperature, humidity) and CCS811 (eCO2 and TVOC) sensors to a local web server via wifi.

The ESP32 is an ESP32 C3 Mini module on a custom development board using the native USB for programming with the Arduino IDE and USB serial output.

![image](https://user-images.githubusercontent.com/6698410/155865582-daab5d08-0a00-4984-9684-b989d95954c0.jpg)

![ESP32Data](https://user-images.githubusercontent.com/6698410/155866112-22a74cbd-d34c-4b75-ac39-dd21a73026d8.jpg)

The web server update is based on a simple timer (here every 10 seconds). I heven't gotten deep sleep to work yet since the ExternalWakeUp example won't compile. But the idea is to manage the power usage of the ESP32 by keeping it in deep sleep mode either on a timer or using the CCS811 (with 60 second sample period) to wake up the ESP32 on data ready interrupt.

The [ESP32 C3 Mini](https://www.digikey.com/en/products/detail/espressif-systems/ESP32-C3-MINI-1-H4/14548936?utm_adgroup=&utm_source=bing&utm_medium=cpc&utm_campaign=Shopping_DK%2BSupplier_Other&utm_term=&utm_content=&utm_id=bi_cmp-384720322_adg-1301822093609990_ad-81363949567673_pla-4584963495352066_dev-c_ext-_prd-14548936) is an inexpensive, fairly small, easy-to-use module that embeds wifi and BLE, and has just enough GPIOs available to be useful for environmental end nodes. Being able to program it with native USB (no CP2102 or FTDI tranceiver!) is a real plus.

The development board [design](https://oshpark.com/shared_projects/wibSiWQn) is open source and available at the OSH Park shared space.


================================================
FILE: CCS811_BME280_ESP32C3Mini_deepSleep/BME280.cpp
================================================
/* 06/16/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 This sketch uses SDA/SCL on pins 42/43 (back pads), respectively, and it uses the Dragonfly STM32L476RE Breakout Board.
 The BME280 is a simple but high resolution pressure/humidity/temperature 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.
 
 Library may be used freely and without limit with attribution.
 
*/
 
#include "BME280.h"
#include "I2Cdev.h"

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

BME280::BME280(I2Cdev* i2c_bus)
{
  _i2c_bus = i2c_bus;
}


uint8_t BME280::getChipID()
{
  uint8_t c = _i2c_bus->readByte(BME280_ADDRESS, BME280_ID);
  return c;
}

void BME280::resetBME280()
{
  _i2c_bus->writeByte(BME280_ADDRESS, BME280_RESET, 0xB6);
}


int32_t BME280::readBME280Temperature()
{
  uint8_t rawData[3];  // 20-bit temperature register data stored here
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_TEMP_MSB, 3, &rawData[0]);  
  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);
}


int32_t BME280::readBME280Pressure()
{
  uint8_t rawData[3];  // 20-bit pressure register data stored here
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_PRESS_MSB, 3, &rawData[0]);  
  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);
}


int32_t BME280::BME280::readBME280Humidity()
{
  uint8_t rawData[2];  // 16-bit humiditye register data stored here
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_HUM_MSB, 2, &rawData[0]);  
  return (uint32_t) (((uint32_t) rawData[0] << 24 | (uint32_t) rawData[1] << 16) ) >> 16;
}


void BME280::BME280forced()
{
    uint8_t temp = _i2c_bus->readByte(BME280_ADDRESS, BME280_CTRL_MEAS);
    _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, temp | Forced);
 
    while( (_i2c_bus->readByte(BME280_ADDRESS, BME280_STATUS)) & 0x10) { } // wait for conversion byte to clear
}


void BME280::BME280Init(uint8_t Posr, uint8_t Hosr, uint8_t Tosr, uint8_t Mode, uint8_t IIRFilter, uint8_t SBy)
{
  // Configure the BME280
  // Set H oversampling rate
  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_HUM, 0x07 & Hosr);
  // Set T and P oversampling rates and sensor mode
  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);
  // Set standby time interval in normal mode and bandwidth
  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CONFIG, SBy << 5 | IIRFilter << 2);
  
  uint8_t calib[26];
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_CALIB00, 26, &calib[0]);
  _dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);
  _dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);
  _dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);
  _dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);
  _dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);
  _dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);
  _dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);
  _dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);
  _dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);
  _dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);
  _dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);
  _dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);
  _dig_H1 = calib[25];
  _i2c_bus->readBytes(BME280_ADDRESS, BME280_CALIB26, 7, &calib[0]);
  _dig_H2 = ( int16_t)((( int16_t) calib[1] << 8) | calib[0]);
  _dig_H3 = calib[2];
  _dig_H4 = ( int16_t)(((( int16_t) calib[3] << 8) | (0x0F & calib[4]) << 4) >> 4);
  _dig_H5 = ( int16_t)(((( int16_t) calib[5] << 8) | (0xF0 & calib[4]) ) >> 4 );
  _dig_H6 = calib[6];
  
 if(SerialDebug)  {
  USBSerial.println("Calibration coeficients:");
  USBSerial.print("_dig_T1 ="); 
  USBSerial.println(_dig_T1);
  USBSerial.print("_dig_T2 ="); 
  USBSerial.println(_dig_T2);
  USBSerial.print("_dig_T3 ="); 
  USBSerial.println(_dig_T3);
  USBSerial.print("_dig_P1 ="); 
  USBSerial.println(_dig_P1);
  USBSerial.print("_dig_P2 ="); 
  USBSerial.println(_dig_P2);
  USBSerial.print("_dig_P3 ="); 
  USBSerial.println(_dig_P3);
  USBSerial.print("_dig_P4 ="); 
  USBSerial.println(_dig_P4);
  USBSerial.print("_dig_P5 ="); 
  USBSerial.println(_dig_P5);
  USBSerial.print("_dig_P6 ="); 
  USBSerial.println(_dig_P6);
  USBSerial.print("_dig_P7 ="); 
  USBSerial.println(_dig_P7);
  USBSerial.print("_dig_P8 ="); 
  USBSerial.println(_dig_P8);
  USBSerial.print("_dig_P9 ="); 
  USBSerial.println(_dig_P9);
  USBSerial.print("_dig_H1 ="); 
  USBSerial.println(_dig_H1);
  USBSerial.print("_dig_H2 ="); 
  USBSerial.println(_dig_H2);
  USBSerial.print("_dig_H3 ="); 
  USBSerial.println(_dig_H3);
  USBSerial.print("_dig_H4 ="); 
  USBSerial.println(_dig_H4);
  USBSerial.print("_dig_H5 ="); 
  USBSerial.println(_dig_H5);
  USBSerial.print("_dig_H6 ="); 
  USBSerial.println(_dig_H6);
 }
}


// Returns temperature in DegC, resolution is 0.01 DegC. Output value of
// “5123” equals 51.23 DegC.
int32_t BME280::BME280_compensate_T(int32_t adc_T)
{
  int32_t var1, var2, T;
  var1 = ((((adc_T >> 3) - ((int32_t)_dig_T1 << 1))) * ((int32_t)_dig_T2)) >> 11;
  var2 = (((((adc_T >> 4) - ((int32_t)_dig_T1)) * ((adc_T >> 4) - ((int32_t)_dig_T1))) >> 12) * ((int32_t)_dig_T3)) >> 14;
  _t_fine = var1 + var2;
  T = (_t_fine * 5 + 128) >> 8;
  return T;
}


// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8
//fractional bits).
//Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
uint32_t BME280::BME280_compensate_P(int32_t adc_P)
{
  long long var1, var2, p;
  var1 = ((long long)_t_fine) - 128000;
  var2 = var1 * var1 * (long long)_dig_P6;
  var2 = var2 + ((var1*(long long)_dig_P5)<<17);
  var2 = var2 + (((long long)_dig_P4)<<35);
  var1 = ((var1 * var1 * (long long)_dig_P3)>>8) + ((var1 * (long long)_dig_P2)<<12);
  var1 = (((((long long)1)<<47)+var1))*((long long)_dig_P1)>>33;
  if(var1 == 0)
  {
    return 0;
    // avoid exception caused by division by zero
  }
  p = 1048576 - adc_P;
  p = (((p<<31) - var2)*3125)/var1;
  var1 = (((long long)_dig_P9) * (p>>13) * (p>>13)) >> 25;
  var2 = (((long long)_dig_P8) * p)>> 19;
  p = ((p + var1 + var2) >> 8) + (((long long)_dig_P7)<<4);
  return (uint32_t)p;
}


// Returns humidity in %RH as unsigned 32 bit integer in Q22.10 format (22integer and 10fractional bits).
// Output value of “47445”represents 47445/1024= 46.333%RH
uint32_t BME280::BME280_compensate_H(int32_t adc_H)
{
int32_t var;

var = (_t_fine - ((int32_t)76800));
var = (((((adc_H << 14) - (((int32_t)_dig_H4) << 20) - (((int32_t)_dig_H5) * var)) +
((int32_t)16384)) >> 15) * (((((((var * ((int32_t)_dig_H6)) >> 10) * (((var *
((int32_t)_dig_H3)) >> 11) + ((int32_t)32768))) >> 10) + ((int32_t)2097152)) * ((int32_t)_dig_H2) + 8192) >> 14));
var = (var - (((((var >> 15) * (var >> 15)) >> 7) * ((int32_t)_dig_H1)) >> 4));
var = (var < 0 ? 0 : var); 
var = (var > 419430400 ? 419430400 : var);
return(uint32_t)(var >> 12);
}


================================================
FILE: CCS811_BME280_ESP32C3Mini_deepSleep/BME280.h
================================================
/* 06/16/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 This sketch uses SDA/SCL on pins 42/43 (back pads), respectively, and it uses the Dragonfly STM32L476RE Breakout Board.
 The BME280 is a simple but high resolution pressure/humidity/temperature 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.
 
 Library may be used freely and without limit with attribution.
 
*/
  
#ifndef BME280_h
#define BME280_h

#include "Arduino.h"
#include "I2CDev.h"
#include <Wire.h>

/* BME280 registers
*  http://www.mouser.com/ds/2/783/BST-BME280_DS001-11-844833.pdf
*/
#define BME280_HUM_LSB    0xFE
#define BME280_HUM_MSB    0xFD
#define BME280_TEMP_XLSB  0xFC
#define BME280_TEMP_LSB   0xFB
#define BME280_TEMP_MSB   0xFA
#define BME280_PRESS_XLSB 0xF9
#define BME280_PRESS_LSB  0xF8
#define BME280_PRESS_MSB  0xF7
#define BME280_CONFIG     0xF5
#define BME280_CTRL_MEAS  0xF4
#define BME280_STATUS     0xF3
#define BME280_CTRL_HUM   0xF2
#define BME280_RESET      0xE0
#define BME280_ID         0xD0  // should be 0x60
#define BME280_CALIB00    0x88
#define BME280_CALIB26    0xE1

#define BME280_ADDRESS           0x76   // Address of BMP280 altimeter when ADO = 0


#define  P_OSR_01 0x01
#define  P_OSR_02 0x02
#define  P_OSR_04 0x03
#define  P_OSR_08 0x04
#define  P_OSR_16 0x05

#define  H_OSR_01 0x01
#define  H_OSR_02 0x02
#define  H_OSR_04 0x03
#define  H_OSR_08 0x04
#define  H_OSR_16 0x05

#define  T_OSR_01 0x01
#define  T_OSR_02 0x02
#define  T_OSR_04 0x03
#define  T_OSR_08 0x04
#define  T_OSR_16 0x05

#define  full       0x00
#define  BW0_223ODR 0x01
#define  BW0_092ODR 0x02
#define  BW0_042ODR 0x03
#define  BW0_021ODR 0x04  

#define  BME280Sleep 0x00
#define  Forced      0x01
#define  Forced2     0x02
#define  Normal      0x03

#define  t_00_5ms 0x00
#define  t_62_5ms 0x01
#define  t_125ms  0x02
#define  t_250ms  0x03
#define  t_500ms  0x04
#define  t_1000ms 0x05
#define  t_10ms   0x06
#define  t_20ms   0x07 


class BME280
{
  public: 
  BME280(I2Cdev* i2c_bus);
  uint8_t getChipID();
  void resetBME280();
  int32_t readBME280Temperature();
  int32_t readBME280Pressure();
  int32_t readBME280Humidity();
  void BME280forced();
  void BME280Init(uint8_t Posr, uint8_t Hosr, uint8_t Tosr, uint8_t Mode, uint8_t IIRFilter, uint8_t SBy);
  int32_t  BME280_compensate_T(int32_t adc_T);
  uint32_t BME280_compensate_P(int32_t adc_P);
  uint32_t BME280_compensate_H(int32_t adc_H);
  
  private:
  uint8_t  _dig_H1, _dig_H3, _dig_H6;
  uint16_t _dig_T1, _dig_P1, _dig_H4, _dig_H5;
  int16_t  _dig_T2, _dig_T3, _dig_P2, _dig_P3, _dig_P4, _dig_P5, _dig_P6, _dig_P7, _dig_P8, _dig_P9, _dig_H2;
  int32_t  _t_fine;
  I2Cdev* _i2c_bus;
  };

#endif


================================================
FILE: CCS811_BME280_ESP32C3Mini_deepSleep/CCS811.cpp
================================================
/* 06/16/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 *  The AMS CCS811 is an air quality sensor that provides equivalent CO2 and volatile organic measurements from direct
 *  I2C register reads as well as current and voltage (effective resistance of the sensing element). Gas sensors, including 
 *  this MEMs gas sensor in the CCS811 measure resistance of a substrate that changes when exposed to inert gasses and 
 *  volatile organic compounds. Changed in concentration vary exponentially with the changes in resistance. The CCS811
 *  has an embedded ASIC calibrated against most common indoor pollutants that returns a good estimate of
 *  equivalent CO2 concentration in parts per million (400 - 8192 range) and volatile organic compounds in parts per billion (0 - 1187).
 *  The sensor is quite sensitive to breath and other human emissions.
 *  
 *  Library may be used freely and without limit with attribution.
 *  
 */
#include "CCS811.h"
#include "I2Cdev.h"

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

CCS811::CCS811(I2Cdev* i2c_bus)
{
  _i2c_bus = i2c_bus;   
}


uint8_t CCS811::getChipID()
{
 byte e = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ID);  // Read WHO_AM_I register for CCS8110
 return e;
}


void CCS811::checkCCS811Status() 
{
   // Check CCS811 status
  uint8_t status = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_STATUS);
  if(SerialDebug) {USBSerial.print("status = 0X"); USBSerial.println(status, HEX);}
  if(status & 0x80 && SerialDebug) {USBSerial.println("Firmware is in application mode. CCS811 is ready!");}
  else { 
    if(SerialDebug) {USBSerial.println("Firmware is in boot mode!");}
   }
  
  if(status & 0x10 && SerialDebug) {USBSerial.println("Valid application firmware loaded!");}
  else { 
    if(SerialDebug) {USBSerial.println("No application firmware is loaded!");}
  }
  if(status & 0x08 && SerialDebug) {
   USBSerial.println("New data available!");
  }
  else { 
    if(SerialDebug) {USBSerial.println("No new data available!");}
  }

  if(status & 0x01 && SerialDebug) {USBSerial.println("Error detected!");
        uint8_t error = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ERROR_ID);
        if(error & 0x01 && SerialDebug) USBSerial.println("CCS811 received invalid I2C write request!");
        if(error & 0x02 && SerialDebug) USBSerial.println("CCS811 received invalid I2C read request!");
        if(error & 0x04 && SerialDebug) USBSerial.println("CCS811 received unsupported mode request!");
        if(error & 0x08 && SerialDebug) USBSerial.println("Sensor resistance measurement at maximum range!");
        if(error & 0x10 && SerialDebug) USBSerial.println("Heater current is not in range!");
        if(error & 0x20 && SerialDebug) USBSerial.println("Heater voltage is not being applied correctly!");
  }
  else { 
    if(SerialDebug) {USBSerial.println("No error detected!");}
  }
  
  if(SerialDebug) USBSerial.println(" ");
  
  }


  void CCS811::CCS811init(uint8_t AQRate)
  {
    // initialize CCS811 and check version and status
  byte HWVersion = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_HW_VERSION);
    if(SerialDebug) {USBSerial.print("CCS811 Hardware Version = 0x"); USBSerial.println(HWVersion, HEX); }

  uint8_t FWBootVersion[2] = {0, 0}, FWAppVersion[2] = {0,0};
  _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_FW_BOOT_VERSION, 2, &FWBootVersion[0]);
  if(SerialDebug) {  
    USBSerial.println("CCS811 Firmware Boot Version: "); 
    USBSerial.print("Major = "); USBSerial.println((FWBootVersion[0] & 0xF0) >> 4); 
    USBSerial.print("Minor = "); USBSerial.println(FWBootVersion[0] & 0x04); 
    USBSerial.print("Trivial = "); USBSerial.println(FWBootVersion[1]); 
  }
  _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_FW_APP_VERSION, 2, &FWAppVersion[0]);
  if(SerialDebug) {
    USBSerial.println("CCS811 Firmware App Version: "); 
    USBSerial.print("Major = "); USBSerial.println((FWAppVersion[0] & 0xF0) >> 4); 
    USBSerial.print("Minor = "); USBSerial.println(FWAppVersion[0] & 0x04); 
    USBSerial.print("Trivial = "); USBSerial.println(FWAppVersion[1]); 
  }

  // Check CCS811 status
  checkCCS811Status();
  _i2c_bus->writeReg(CCS811_ADDRESS, CCS811_APP_START);
  delay(100);
  checkCCS811Status();

  // set CCS811 measurement mode
  _i2c_bus->writeByte(CCS811_ADDRESS, CCS811_MEAS_MODE, AQRate << 4 | 0x08); // pulsed heating mode, enable interrupt
  uint8_t measmode = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_MEAS_MODE);
  if(SerialDebug) {USBSerial.print("Confirm measurement mode = 0x"); USBSerial.println(measmode, HEX);}
  }


  void CCS811::compensateCCS811(int32_t compHumidity, int32_t compTemp)
  {
      // Update CCS811 humidity and temperature compensation
      uint8_t temp[5] = {0, 0, 0, 0, 0};
      temp[0] = CCS811_ENV_DATA;
      temp[1] = ((compHumidity % 1024) / 100) > 7 ? (compHumidity/1024 + 1)<<1 : (compHumidity/1024)<<1;
      temp[2] = 0;
      if(((compHumidity % 1024) / 100) > 2 && (((compHumidity % 1024) / 100) < 8))
      {
       temp[1] |= 1;
      }

      compTemp += 2500;
      temp[3] = ((compTemp % 100) / 100) > 7 ? (compTemp/100 + 1)<<1 : (compTemp/100)<<1;
      temp[4] = 0;
      if(((compTemp % 100) / 100) > 2 && (((compTemp % 100) / 100) < 8))
      {
       temp[3] |= 1;
      }

//      Wire.transfer(CCS811_ADDRESS, &temp[0], 5, NULL, 0);
     _i2c_bus->writeBytes(CCS811_ADDRESS, CCS811_ENV_DATA, 4, &temp[1]);
  }

   void CCS811::readCCS811Data(uint8_t * destination)
   {
      uint8_t rawData[8] = {0, 0, 0, 0, 0, 0, 0, 0};
      uint8_t status = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_STATUS);
      
      if(status & 0x01) { // check for errors
        uint8_t error = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ERROR_ID);
        if(error & 0x01 && SerialDebug) USBSerial.println("CCS811 received invalid I2C write request!");
        if(error & 0x02 && SerialDebug) USBSerial.println("CCS811 received invalid I2C read request!");
        if(error & 0x04 && SerialDebug) USBSerial.println("CCS811 received unsupported mode request!");
        if(error & 0x08 && SerialDebug) USBSerial.println("Sensor resistance measurement at maximum range!");
        if(error & 0x10 && SerialDebug) USBSerial.println("Heater current is not in range!");
        if(error & 0x20 && SerialDebug) USBSerial.println("Heater voltage is not being applied correctly!");
      }

      _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_ALG_RESULT_DATA, 8, &rawData[0]);

   for(int ii = 0; ii < 8; ii++)
   {
    destination[ii] = rawData[ii];
   }
   
   }


================================================
FILE: CCS811_BME280_ESP32C3Mini_deepSleep/CCS811.h
================================================
#ifndef CCS811_h
#define CCS811_h

#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"

/* CCS811 Registers
http://www.mouser.com/ds/2/588/CCS811_DS000459_3-00-1098798.pdf
*/
#define CCS811_STATUS             0x00
#define CCS811_MEAS_MODE          0x01
#define CCS811_ALG_RESULT_DATA    0x02
#define CCS811_RAW_DATA           0x03
#define CCS811_ENV_DATA           0x05
#define CCS811_NTC                0x06
#define CCS811_THRESHOLDS         0x10
#define CCS811_BASELINE           0x11
#define CCS811_HW_ID              0x20  // WHO_AM_I should be 0x81
#define CCS811_ID                 0x20  // WHO_AM_I should be 0x1X
#define CCS811_HW_VERSION         0x21  
#define CCS811_FW_BOOT_VERSION    0x23
#define CCS811_FW_APP_VERSION     0x24
#define CCS811_ERROR_ID           0xE0
#define CCS811_APP_START          0xF4
#define CCS811_SW_RESET           0xFF

#define CCS811_ADDRESS            0x5A   // Address of the CCS811 Air Quality Sensor

#define  dt_idle  0x00
#define  dt_1sec  0x01
#define  dt_10sec 0x02
#define  dt_60sec 0x03

class CCS811
{
  public: 
  CCS811(I2Cdev* i2c_bus);
  void checkCCS811Status();
  void CCS811init(uint8_t AQRate);
  uint8_t getChipID();
  void compensateCCS811(int32_t compHumidity, int32_t compTemp);
  void readCCS811Data(uint8_t * destination);
  
  private:
  I2Cdev* _i2c_bus;
};


#endif


================================================
FILE: CCS811_BME280_ESP32C3Mini_deepSleep/CCS811_BME280_ESP32C3Mini_deepSleep.ino
================================================
/* 06/16/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 *  The AMS CCS811 is an air quality sensor that provides equivalent CO2 and volatile organic measurements from direct
 *  I2C register reads as well as current and voltage (effective resistance of the sensing element). Gas sensors, including 
 *  this MEMs gas sensor in the CCS811 measure resistance of a substrate that changes when exposed to inert gasses and 
 *  volatile organic compounds. Changed in concentration vary exponentially with the changes in resistance. The CCS811
 *  has an embedded ASIC calibrated against most common indoor pollutants that returns a good estimate of
 *  equivalent CO2 concentration in parts per million (400 - 8192 range) and volatile organic componds in parts per billion (0 - 1187).
 *  The sensor is quite sensitive to breath and other human emissions.
 *  
  *  The BME280 is a simple but high resolution pressure/humidity/temperature 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.
 
    Library may be used freely and without limit with attribution.
 
  */
#include <Arduino.h>
#include <driver/adc.h>
#include "BME280.h"
#include "CCS811.h"
#include "I2Cdev.h"

#define uS_TO_S_FACTOR 1000000ULL  /* Conversion factor for micro seconds to seconds */
#define TIME_TO_SLEEP  600        /* Time ESP32 will go to sleep (in seconds) */

RTC_DATA_ATTR int bootCount = 0;

void print_wakeup_reason(){
  esp_sleep_wakeup_cause_t wakeup_reason;

  wakeup_reason = esp_sleep_get_wakeup_cause();

  switch(wakeup_reason)
  {
//      case ESP_SLEEP_WAKEUP_EXT0 : USBSerial.println("Wakeup caused by external signal using RTC_IO"); break;
//      case ESP_SLEEP_WAKEUP_EXT1 : USBSerial.println("Wakeup caused by external signal using RTC_CNTL"); break;
//      case ESP_SLEEP_WAKEUP_TIMER : USBSerial.println("Wakeup caused by timer"); break;
//      case ESP_SLEEP_WAKEUP_TOUCHPAD : USBSerial.println("Wakeup caused by touchpad"); break;
//      case ESP_SLEEP_WAKEUP_ULP : USBSerial.println("Wakeup caused by ULP program"); break;
//      default : USBSerial.printf("Wakeup was not caused by deep sleep: %d\n",wakeup_reason); break;
  }
}

#define I2C_BUS          Wire               // Define the I2C bus (Wire instance) you wish to use

I2Cdev                   i2c_0(&I2C_BUS);   // Instantiate the I2Cdev object and point to the desired I2C bus

#define SerialDebug false  // set to true to get Serial output for debugging
const uint8_t myLed =  2;  // green led
const uint8_t myBat =  1;  // battery monitor

float VBat = 0.0f;
uint32_t chipId = 0;

// BME280 definitions
/* Specify BME280 configuration
 *  Choices are:
 P_OSR_01, P_OSR_02, P_OSR_04, P_OSR_08, P_OSR_16 // pressure oversampling
 H_OSR_01, H_OSR_02, H_OSR_04, H_OSR_08, H_OSR_16 // humidity oversampling
 T_OSR_01, T_OSR_02, T_OSR_04, T_OSR_08, T_OSR_16 // temperature oversampling
 full, BW0_223ODR,BW0_092ODR, BW0_042ODR, BW0_021ODR // bandwidth at 0.021 x sample rate
 BME280Sleep, forced,, forced2, normal //operation modes
 t_00_5ms = 0, t_62_5ms, t_125ms, t_250ms, t_500ms, t_1000ms, t_10ms, t_20ms // determines sample rate
 */
uint8_t Posr = P_OSR_01, Hosr = H_OSR_01, Tosr = T_OSR_01, Mode = BME280Sleep, IIRFilter = full, SBy = t_1000ms;     // set pressure amd temperature output data rate

float Temperature, Pressure, Humidity;              // stores BME280 pressures sensor pressure and temperature
int32_t rawPress, rawTemp, rawHumidity, compTemp;   // pressure and temperature raw count output for BME280
uint32_t compHumidity, compPress;                   // variables to hold raw BME280 humidity value

float temperature_C, temperature_F, pressure, humidity, altitude; // Scaled output of the BME280

BME280 BME280(&i2c_0); // instantiate BME280 class


// CCS811 definitions
#define CCS811_intPin  4
#define CCS811_wakePin 7

/* Specify CCS811 sensor parameters
 *  Choices are   dt_idle , dt_1sec, dt_10sec, dt_60sec
 */
//uint8_t AQRate = dt_60sec;  // set the sample rate
uint8_t AQRate = dt_idle;  // set the sample rate
uint8_t rawData[8] = {0, 0, 0, 0, 0, 0, 0, 0};  // array to hold the raw data
uint16_t eCO2 = 0, TVOC = 0;
uint8_t Current = 0;
float Voltage = 0.0f;

volatile bool newCCS811Data  = true; // boolean flag for interrupt

CCS811 CCS811(&i2c_0); // instantiate CCS811 class


void setup()
{
  if(SerialDebug) USBSerial.begin(115200);
  if(SerialDebug) USBSerial.println("Serial enabled!");

  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH); // start with led off, active LOW
  pinMode(myBat, INPUT); // battery voltage monitor
  pinMode(CCS811_intPin, INPUT); // active LOW

  //Increment boot number and print it every reboot
  ++bootCount;
   if(SerialDebug) USBSerial.println("Boot number: " + String(bootCount));

  //Print the wakeup reason for ESP32
  print_wakeup_reason();

  if(bootCount == 1) { /*... Only need to do this part once on startup! */
    for(int i=0; i<17; i=i+8) {
      chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xFF) << i;
    }

   if(SerialDebug) USBSerial.printf("ESP32 Chip model = %s Rev %d\n", ESP.getChipModel(), ESP.getChipRevision());
   if(SerialDebug) USBSerial.printf("This chip has %d cores\n", ESP.getChipCores());
   if(SerialDebug){USBSerial.print("Chip ID: "); USBSerial.println(chipId);}
  } /*... Only need to do this part once on startup! */

  adc1_config_width(ADC_WIDTH_BIT_12);
  adc1_config_channel_atten(ADC1_CHANNEL_1, ADC_ATTEN_DB_11);

  I2C_BUS.begin(0, 3);               // Set master mode, default on SDA on pin 0/SCL  on pin 3
  I2C_BUS.setClock(400000);          // I2C frequency at 400 kHz

  pinMode(CCS811_wakePin, OUTPUT);
  digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor

  if(bootCount == 1) { /*... Only need to do this part once on startup! */
  //Enable the CCS811 for I2C scan
  digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
  
  if(SerialDebug) USBSerial.println("Scan for I2C devices:");
  i2c_0.I2Cscan();                   // should detect BME280 at 0x76 and CCS811 at 0x5A

  //Disable the CCS811 for I2C scan
  digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor
  delay(1);

  // Read the WHO_AM_I register of the BME280 this is a good test of communication
  byte BME280ChipID = BME280.getChipID();  // Read WHO_AM_I register for BME280
  if(SerialDebug){ 
    USBSerial.print("BME280 "); USBSerial.print("I AM "); USBSerial.print(BME280ChipID, HEX); USBSerial.print(" I should be "); USBSerial.println(0x60, HEX);
    USBSerial.println(" ");
  }

  // Read the WHO_AM_I register of the CCS811 this is a good test of communication
  digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
  byte CCS811ChipID = CCS811.getChipID();
  digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor
  if(SerialDebug) {
    USBSerial.print("CCS811 "); USBSerial.print("I AM "); USBSerial.print(CCS811ChipID, HEX); USBSerial.print(" I should be "); USBSerial.println(0x81, HEX);
    USBSerial.println(" ");
  }
  
  if(BME280ChipID == 0x60 && CCS811ChipID == 0x81 ) {

   if(SerialDebug) {USBSerial.println("BME280+CCS811 are online..."); USBSerial.println(" ");}
   digitalWrite(myLed, LOW);

   BME280.resetBME280();                                                        // reset BME280 before initilization
   delay(10);
   BME280.BME280Init(Posr, Hosr, Tosr, Mode, IIRFilter, SBy);                   // Initialize BME280 altimeter
   BME280.BME280forced();                                                       // get initial data sample, then go back to sleep

   // initialize CCS811 and check version and status
   digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
   CCS811.CCS811init(AQRate);
   digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor

   }
   else {
   if(BME280ChipID != 0x60 && SerialDebug) USBSerial.println(" BME280 not functioning!");    
   if(CCS811ChipID != 0x81 && SerialDebug) USBSerial.println(" CCS811 not functioning!");  
   while(1) { }; // no point proceeding, so wait here forever...
   }
  } /*... Only need to do this part once on startup! */
  

    /*   Put here anything you want to do before going to sleep */
    digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor
    CCS811.readCCS811Data(rawData);
    CCS811.compensateCCS811(compHumidity, compTemp); // compensate CCS811 using BME280 humidity and temperature
    digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor 

    eCO2 = (uint16_t) ((uint16_t) rawData[0] << 8 | rawData[1]);
    TVOC = (uint16_t) ((uint16_t) rawData[2] << 8 | rawData[3]);
    Current = (rawData[6] & 0xFC) >> 2;
    Voltage = (float) ((uint16_t) ((((uint16_t)rawData[6] & 0x02) << 8) | rawData[7])) * (1.65f/1023.0f); 
    
     if(SerialDebug){
      USBSerial.println("CCS811:");
      USBSerial.print("Eq CO2 in ppm = "); USBSerial.println(eCO2);
      USBSerial.print("TVOC in ppb = "); USBSerial.println(TVOC);
      USBSerial.print("Sensor current (uA) = "); USBSerial.println(Current);
      USBSerial.print("Sensor voltage (V) = "); USBSerial.println(Voltage, 2);  
      USBSerial.println(" ");
     }

    /* BME280 sensor data */
    BME280.BME280Init(Posr, Hosr, Tosr, Mode, IIRFilter, SBy);                   // Initialize BME280 altimeter
    BME280.BME280forced();  // get one data sample, then go back to sleep

    rawTemp =  BME280.readBME280Temperature();
    compTemp = BME280.BME280_compensate_T(rawTemp);
    temperature_C = (float) compTemp/100.0f;
    temperature_F = 9.0f*temperature_C/5.0f + 32.0f;
     
    rawPress =  BME280.readBME280Pressure();
    pressure = (float) BME280.BME280_compensate_P(rawPress)/25600.f; // Pressure in mbar
    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));   
   
    rawHumidity =  BME280.readBME280Humidity();
    compHumidity = BME280.BME280_compensate_H(rawHumidity);
    humidity = (float)compHumidity/1024.0f; // Humidity in %RH

    if(SerialDebug) {
    USBSerial.println("BME280:");
    USBSerial.print("Altimeter temperature = "); 
    USBSerial.print( temperature_C, 2); 
    USBSerial.println(" C"); // temperature in degrees Celsius
    USBSerial.print("Altimeter temperature = "); 
    USBSerial.print(temperature_F, 2); 
    USBSerial.println(" F"); // temperature in degrees Fahrenheit
    USBSerial.print("Altimeter pressure = "); 
    USBSerial.print(pressure, 2);  
    USBSerial.println(" mbar");// pressure in millibar
    USBSerial.print("Altitude = "); 
    USBSerial.print(altitude, 2); 
    USBSerial.println(" feet");
    USBSerial.print("Altimeter humidity = "); 
    USBSerial.print(humidity, 1);  
    USBSerial.println(" %RH");// pressure in millibar
    USBSerial.println(" ");
    }
    /* ADC at attenuation 11 should read from 0 to 2.6 V nominally. The resistor divider
     *  is 1/2, and a calibration factor of 1.15 is applied to bring measurements into 
     *  agreement with multimeter */
    VBat = 2.0f * 2.60f * 1.15f * ((float) adc1_get_raw((adc1_channel_t)1)) / 4095.0f;
    if(SerialDebug) {USBSerial.print("Battery voltage = "); USBSerial.print(VBat, 2); USBSerial.println(" V");}
      
    digitalWrite(myLed, LOW); delay(1); digitalWrite(myLed, HIGH); // blink led at end of loop

    esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * uS_TO_S_FACTOR);
    if(SerialDebug) USBSerial.println("Setup ESP32 to sleep for every " + String(TIME_TO_SLEEP) + " Seconds");
  
    //Go to sleep now
    if(SerialDebug) USBSerial.println("Going to sleep now");
    if(SerialDebug) USBSerial.flush(); 
    esp_deep_sleep_start();
    if(SerialDebug) USBSerial.println("This will never be printed");
}

void loop()
{
}


================================================
FILE: CCS811_BME280_ESP32C3Mini_deepSleep/I2CDev.cpp
================================================
/*
 * Copyright (c) 2018 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#include "Arduino.h"
#include "I2Cdev.h"

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

I2Cdev::I2Cdev(TwoWire* i2c_bus)                                                                                                             // Class constructor
{
  _i2c_bus = i2c_bus;
}

I2Cdev::~I2Cdev()                                                                                                                            // Class destructor
{
}

/**
* @fn: readByte(uint8_t address, uint8_t subAddress)
*
* @brief: Read one byte from an I2C device
* 
* @params: I2C slave device address, Register subAddress
* @returns: unsigned short read
*/
uint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)
{
  uint8_t data = 0;                             // `data` will store the register data   
  _i2c_bus->beginTransmission(address);         // Initialize the Tx buffer
  _i2c_bus->write(subAddress);                  // Put slave register address in Tx buffer
  _i2c_bus->endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
  _i2c_bus->requestFrom(address, 1);            // Read one byte from slave register address  
  data = _i2c_bus->read();                      // Fill Rx buffer with result
  return data;                                  // Return data read from slave register
  
}


/**
* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
*
* @brief: Read multiple bytes from an I2C device
* 
* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data
* @returns: void
*/
void I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
  _i2c_bus->beginTransmission(address);   // Initialize the Tx buffer
  _i2c_bus->write(subAddress);            // Put slave register address in Tx buffer
  _i2c_bus->endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
  uint8_t i = 0;
  _i2c_bus->requestFrom(address, count);  // Read bytes from slave register address 
  while (_i2c_bus->available()) {
        dest[i++] = _i2c_bus->read(); }   // Put read results in the Rx buffer
}


/**
* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write one byte to an I2C device
* 
* @params: I2C slave device address, Register subAddress, data to be written
* @returns: void
*/
void I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
{
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  _i2c_bus->write(regAddr);              // Put slave register address in Tx buffer
  _i2c_bus->write(data);                 // Put data in Tx buffer
  _i2c_bus->endTransmission();           // Send the Tx buffer
}

/**
* @fn: writeReg(uint8_t devAddr, uint8_t regAddr)
*
* @brief: Write register to an I2C device
* 
* @params: I2C slave device address, Register subAddress, data to be written
* @returns: void
*/
void I2Cdev::writeReg(uint8_t devAddr, uint8_t regAddr)
{
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  _i2c_bus->write(regAddr);              // Put slave register address in Tx buffer
  _i2c_bus->endTransmission();           // Send the Tx buffer
}


/**
* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write multiple bytes to an I2C device
* 
* @params: I2C slave device address, Register subAddress, byte count, data array to be written
* @returns: void
*/
void I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)
{
  uint8_t temp[1 + count];
  
  temp[0] = regAddr;
  for (uint8_t ii = 0; ii < count; ii++)
  { 
    temp[ii + 1] = dest[ii];
  }
  
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  
  for (uint8_t jj = 0; jj < count + 1; jj++)
  {
  _i2c_bus->write(temp[jj]);            // Put data in Tx buffer
  }
  
  _i2c_bus->endTransmission();          // Send the Tx buffer
}



/**
* @fn:I2Cscan()
* @brief: Scan the I2C bus for active I2C slave devices
* 
* @params: void
* @returns: void
*/
void I2Cdev::I2Cscan() 
{
  // Scan for i2c devices
  byte error, address;
  int nDevices;

  if(SerialDebug) USBSerial.println("Scanning...");

  nDevices = 0;
  for(address = 1; address < 127; address++ ) 
  {
    // The i2c_scanner uses the return value of the Wire.endTransmission to see if a device did acknowledge to the address.
    _i2c_bus->beginTransmission(address);
    error = _i2c_bus->endTransmission();

    if (error == 0 && SerialDebug)
    {
      USBSerial.print("I2C device found at address 0x");
      if (address<16) 
      USBSerial.print("0");
      USBSerial.print(address,HEX);
      USBSerial.println("  !");
      nDevices++;
    }
    else if (error==4 && SerialDebug) 
    {
      USBSerial.print("Unknown error at address 0x");
      if (address<16) 
        USBSerial.print("0");
        USBSerial.println(address,HEX);
    }    
  }
  if (nDevices == 0 && SerialDebug)
    USBSerial.println("No I2C devices found\n");
  else
    if(SerialDebug) USBSerial.println("I2C scan complete\n");
}


================================================
FILE: CCS811_BME280_ESP32C3Mini_deepSleep/I2CDev.h
================================================
/*
 * Copyright (c) 2018 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#ifndef _I2CDEV_H_
#define _I2CDEV_H_

#include <Wire.h>

class I2Cdev {
    public:
                                        I2Cdev(TwoWire*);
                                        ~I2Cdev();                                                                                                                     // Class destructor for durable instances
         uint8_t                        readByte(uint8_t address, uint8_t subAddress);
         void                           readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
         void                           writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
         void                           writeReg(uint8_t devAddr, uint8_t regAddr);
         void                           writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);
         void                           I2Cscan();
    private:
         TwoWire*                       _i2c_bus;                                                                                                                      // Class constructor argument
};

#endif //_I2CDEV_H_


================================================
FILE: CCS811_BME280_ESP32C3Mini_deepSleep/Readme.md
================================================
Sketch showing how to use the ESP32C3Mini1 as an environmental data logger. 

I am using a custom ESP32C3Mini1 development board with native USB for programming (no CP2102).

The CCS811 air quality sensor and BME280 humidity/pressure/temperature sensors are on a breakout board and are configured in setup once, 
their data is read and then the ESP32 goes into deep sleep for some time (10 minutes in the sketch). Upon wakeup, the sensor configuration is skipped if the bootcount variable is greater than 1 and the data is read and the ESP32C3 goes into deep sleep, etc. The cycle repeats.

![image](https://user-images.githubusercontent.com/6698410/155865582-daab5d08-0a00-4984-9684-b989d95954c0.jpg)

The idea is to log the data on the native SPIFFS (up to 3 MByte available) and send data updates via either BLE or ESPNow. These bits will be added when I assemble the next version which has the sensors (in this case APDS9253 ambient light sensor, HDC2010 humidity sensor, and LPS22HB barometer) directly on the ESP32C3Mini1 development board.

I measured deep sleep current with no sensors of 8.9 uA. However, this is with a simple 1 MOhm+1 MOhm voltage divider, so at least 2 uA come from this. In the new verson I added a dual FET to eliminate this current draw. Another 300 nA comes from the MCP1812 LDO. The data sheet specs 5 uA as typical, so 8.9 - ~2 - 0.3 is ~6.6 uA is in the ball park. The average current when the ESP32C3Mini is awake is ~18 mA for ~0.25 seconds; this is without BLE or ESPNow. So I expect running at a 10 minute data update duty cycle will use ~15 uA. Probably a bit more with BLE or ESPNow. In any case, I expect to be able to run this kind of environmental data logger for at least six months on a small (~100 mAH) LiPo battery.  

One oddity is that if I enable USBSerial output by setting SerialDebug = true in every file where USBSerial is called then the sketch works well, sending serial output to the serial monitor. However, upon wake from deep sleep the program stalls unless the serial monitor is closed then opened again. This happens even when running from battery with no USB connected. So in order to run this sketch as a data logger, one has to set SerialDebug to false everywhere then it works just fine.


================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32.ino
================================================
#include "LSM6DSM.h"
#include "LIS2MDL.h"
#include "LPS22HB.h"
#include "USFS.h"

#define I2C_BUS          Wire                           // Define the I2C bus (Wire instance) you wish to use

I2Cdev                   i2c_0(&I2C_BUS);               // Instantiate the I2Cdev object and point to the desired I2C bus

bool SerialDebug = true;  // set to true to get Serial output for debugging
bool passThru  = false;

#define myLed 5
#define pinGND 12
#define pin3V3 13

void EM7180intHandler();
void myinthandler1();
void myinthandler2();
void myinthandler3();

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
float pi = 3.141592653589793238462643383279502884f;
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)
float beta = sqrtf(3.0f / 4.0f) * GyroMeasError;   // compute beta
float zeta = sqrtf(3.0f / 4.0f) * GyroMeasDrift;   // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
uint32_t delt_t = 0;                      // used to control display output rate
uint32_t sumCount = 0;                    // used to control display output rate
float pitch, yaw, roll, Yaw, Pitch, Roll;
float a12, a22, a31, a32, a33;            // rotation matrix coefficients for Euler angles and gravity components
float A112, A22, A31, A32, A33;            // rotation matrix coefficients for Hardware Euler angles and gravity components
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 lin_ax, lin_ay, lin_az;             // linear acceleration (acceleration with gravity component subtracted)
float lin_Ax, lin_Ay, lin_Az;             // Hardware linear acceleration (acceleration with gravity component subtracted)
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
float Q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // hardware quaternion data register
float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method

//LSM6DSM definitions
#define LSM6DSM_intPin1 10  // interrupt1 pin definitions, significant motion
#define LSM6DSM_intPin2 9   // interrupt2 pin definitions, data ready

/* Specify sensor parameters (sample rate is twice the bandwidth)
 * choices are:
      AFS_2G, AFS_4G, AFS_8G, AFS_16G  
      GFS_245DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS 
      AODR_12_5Hz, AODR_26Hz, AODR_52Hz, AODR_104Hz, AODR_208Hz, AODR_416Hz, AODR_833Hz, AODR_1660Hz, AODR_3330Hz, AODR_6660Hz
      GODR_12_5Hz, GODR_26Hz, GODR_52Hz, GODR_104Hz, GODR_208Hz, GODR_416Hz, GODR_833Hz, GODR_1660Hz, GODR_3330Hz, GODR_6660Hz
*/ 
uint8_t Ascale = AFS_2G, Gscale = GFS_245DPS, AODR = AODR_208Hz, GODR = GODR_416Hz;

float aRes, gRes;              // scale resolutions per LSB for the accel and gyro sensor2
float accelBias[3] = {-0.00499, 0.01540, 0.02902}, gyroBias[3] = {-0.50, 0.14, 0.28}; // offset biases for the accel and gyro
int16_t LSM6DSMData[7];        // Stores the 16-bit signed sensor output
float   Gtemperature;           // Stores the real internal gyro temperature in degrees Celsius
float ax, ay, az, gx, gy, gz;  // variables to hold latest accel/gyro data values 

bool newLSM6DSMData = false;
bool newLSM6DSMTap  = false;

LSM6DSM LSM6DSM(LSM6DSM_intPin1, LSM6DSM_intPin2, &i2c_0); // instantiate LSM6DSM class


//LIS2MDL definitions
#define LIS2MDL_intPin  8 // interrupt for magnetometer data ready

/* Specify sensor parameters (sample rate is twice the bandwidth)
 * choices are: MODR_10Hz, MOIDR_20Hz, MODR_50 Hz and MODR_100Hz
*/ 
uint8_t MODR = MODR_100Hz;

float mRes = 0.0015f;            // mag sensitivity
float magBias[3] = {0,0,0}, magScale[3]  = {0,0,0}; // Bias corrections for magnetometer
int16_t LIS2MDLData[4];          // Stores the 16-bit signed sensor output
float Mtemperature;              // Stores the real internal chip temperature in degrees Celsius
float mx, my, mz;                // variables to hold latest mag data values 
uint8_t LIS2MDLstatus;

bool newLIS2MDLData = false;

LIS2MDL LIS2MDL(LIS2MDL_intPin, &i2c_0); // instantiate LIS2MDL class


// LPS22H definitions
uint8_t LPS22H_intPin = 5;

/* Specify sensor parameters (sample rate is twice the bandwidth) 
   Choices are P_1Hz, P_10Hz P_25 Hz, P_50Hz, and P_75Hz
 */
uint8_t PODR = P_25Hz;     // set pressure amd temperature output data rate
uint8_t LPS22Hstatus;
float temperature, pressure, altitude;

bool newLPS22HData = false;

LPS22H LPS22H(LPS22H_intPin, &i2c_0);


const uint8_t USFS_intPin = 27;
bool newEM7180Data = false;
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 tempCount, rawPressure, rawTemperature;            // temperature raw count output
float   Temperature, Pressure, Altitude; //  temperature in degrees Celsius, pressure in mbar
float Ax, Ay, Az, Gx, Gy, Gz, Mx, My, Mz; // variables to hold latest sensor data values


/* Choose EM7180, LSM6DSM, LIS2MDL sample rates and bandwidths
   Choices are:
   accBW, gyroBW 0x00 = 250 Hz, 0x01 = 184 Hz, 0x02 = 92 Hz, 0x03 = 41 Hz, 0x04 = 20 Hz, 0x05 = 10 Hz, 0x06 = 5 Hz, 0x07 = no filter (3600 Hz)
   QRtDiv 0x00, 0x01, 0x02, etc quat rate = gyroRt/(1 + QRtDiv)
   magRt 8 Hz = 0x08 or 100 Hz 0x64
   accRt, gyroRt 1000, 500, 250, 200, 125, 100, 50 Hz enter by choosing desired rate
   and dividing by 10, so 200 Hz would be 200/10 = 20 = 0x14
   sample rate of barometer is baroRt/2 so for 25 Hz enter 50 = 0x32
   LSM6DSM accel/gyro rates 0f 833 Hz set Rt variables to 0x53
*/
uint8_t accBW = 0x03, gyroBW = 0x03, QRtDiv = 0x03, magRt = 0x64, accRt = 0x53, gyroRt = 0x53, baroRt = 0x32;
/*
   Choose sensor full ranges
   Choices are 2, 4, 8, 16 g for accFS, 250, 500, 1000, and 2000 dps for gyro FS and 1000 uT for magFS expressed as HEX values
*/
uint16_t accFS = 0x02, gyroFS = 0x7D0, magFS = 0x3E8;

USFS USFS(USFS_intPin, passThru, &i2c_0);


void setup() {  
  Serial.begin(115200);
  delay(4000);

  // Configure led
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH); // start with led on

   pinMode(pinGND, OUTPUT);
  digitalWrite(pinGND, LOW);

  pinMode(pin3V3, OUTPUT);
  digitalWrite(pin3V3, HIGH);

  pinMode(USFS_intPin, INPUT);
  
  Wire.begin(16, 15, 400000); //(SDA, SCL) (21,22) are default on ESP32, 400 kHz I2C clock
  delay(1000);
 
  i2c_0.I2Cscan(); // which I2C device are on the bus?

  if(!passThru)
  {
  // Initialize the USFS
  USFS.getChipID();        // check ROM/RAM version of EM7180
  USFS.loadfwfromEEPROM(); // load EM7180 firmware from EEPROM
  USFS.initEM7180(accBW, gyroBW, accFS, gyroFS, magFS, QRtDiv, magRt, accRt, gyroRt, baroRt); // set MPU and MS5637 sensor parameters
  } // end of "if(!passThru)" handling

  if(passThru)
  {
  // Read the LSM6DSM Chip ID register, this is a good test of communication
  Serial.println("LSM6DSM accel/gyro...");
  byte c = LSM6DSM.getChipID();  // Read CHIP_ID register for LSM6DSM
  Serial.print("LSM6DSM "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x6A, HEX);
  Serial.println(" ");
  delay(1000); 

  // Read the LIS2MDL Chip ID register, this is a good test of communication
  Serial.println("LIS2MDL mag...");
  byte d = LIS2MDL.getChipID();  // Read CHIP_ID register for LSM6DSM
  Serial.print("LIS2MDL "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x40, HEX);
  Serial.println(" ");
  delay(1000); 

  Serial.println("LPS22HB barometer...");
  uint8_t e = LPS22H.getChipID();
  Serial.print("LPS25H "); Serial.print("I AM "); Serial.print(e, HEX); Serial.print(" I should be "); Serial.println(0xB1, HEX);
  delay(1000); 
  

  if(c == 0x6A && d == 0x40 && e == 0xB1) // check if all I2C sensors have acknowledged
  {
   Serial.println("LSM6DSM and LIS2MDL and LPS22HB are online..."); Serial.println(" ");
   
   digitalWrite(myLed, LOW);

   LSM6DSM.reset();  // software reset LSM6DSM to default registers

   // get sensor resolutions, only need to do this once
   aRes = LSM6DSM.getAres(Ascale);
   gRes = LSM6DSM.getGres(Gscale);

   LSM6DSM.init(Ascale, Gscale, AODR, GODR);

   LSM6DSM.selfTest();

   LSM6DSM.offsetBias(gyroBias, accelBias);
   Serial.println("accel biases (mg)"); Serial.println(1000.0f * accelBias[0]); Serial.println(1000.0f * accelBias[1]); Serial.println(1000.0f * accelBias[2]);
   Serial.println("gyro biases (dps)"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);
   delay(1000); 

   LIS2MDL.reset(); // software reset LIS2MDL to default registers

   mRes = 0.0015f;  // fixed sensitivity and full scale (+/- 49.152 Gauss); 
   
   LIS2MDL.init(MODR);

   LIS2MDL.selfTest();

   LIS2MDL.offsetBias(magBias, magScale);
   Serial.println("mag biases (mG)"); Serial.println(1000.0f * magBias[0]); Serial.println(1000.0f * magBias[1]); Serial.println(1000.0f * magBias[2]); 
   Serial.println("mag scale (mG)"); Serial.println(magScale[0]); Serial.println(magScale[1]); Serial.println(magScale[2]); 
   delay(2000); // add delay to see results before serial spew of data

   LPS22H.Init(PODR);  // Initialize LPS22H altimeter
   delay(1000);

   digitalWrite(myLed, HIGH);
   
  }
  else 
  {
  if(c != 0x6A) Serial.println(" LSM6DSM not functioning!");
  if(d != 0x40) Serial.println(" LIS2MDL not functioning!");    
  if(e != 0xB1) Serial.println(" LPS22HB not functioning!");   

  while(1){};
  }
  }  // end of "if(passThru)" handling


  if(!passThru)
  {
    attachInterrupt(USFS_intPin, EM7180intHandler, RISING);  // define interrupt for INT pin output of EM7180
 
    USFS.checkEM7180Status();
  }

  if(passThru)
  {
  attachInterrupt(LSM6DSM_intPin2, myinthandler1, RISING);  // define interrupt for intPin2 output of LSM6DSM
  attachInterrupt(LIS2MDL_intPin , myinthandler2, RISING);  // define interrupt for intPin  output of LIS2MDL
  attachInterrupt(LPS22H_intPin  , myinthandler3, RISING);  // define interrupt for intPin  output of LPS22HB

  LIS2MDLstatus = LIS2MDL.status();  // read status register to clear interrupt before main loop
  }

  digitalWrite(myLed, LOW); // turn led off when successfully through setup

}

/* End of setup */

void loop() {

   if(passThru)
   {
   // If intPin goes high, either all data registers have new data
   if(newLSM6DSMData == true) {   // On interrupt, read data
      newLSM6DSMData = false;     // reset newData flag

     LSM6DSM.readData(LSM6DSMData); // INT2 cleared on any read
   
   // Now we'll calculate the accleration value into actual g's
     ax = (float)LSM6DSMData[4]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
     ay = (float)LSM6DSMData[5]*aRes - accelBias[1];   
     az = (float)LSM6DSMData[6]*aRes - accelBias[2];  

   // Calculate the gyro value into actual degrees per second
     gx = (float)LSM6DSMData[1]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
     gy = (float)LSM6DSMData[2]*gRes - gyroBias[1];  
     gz = (float)LSM6DSMData[3]*gRes - gyroBias[2]; 

    for(uint8_t i = 0; i < 10; i++) { // iterate a fixed number of times per data read cycle
    Now = micros();
    deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
    lastUpdate = Now;

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

    USFS.MadgwickQuaternionUpdate(-ax, ay, az, gx*pi/180.0f, -gy*pi/180.0f, -gz*pi/180.0f,  mx,  my, -mz);
    }
    
   }

    // If intPin goes high, either all data registers have new data
   if(newLIS2MDLData == true) {   // On interrupt, read data
      newLIS2MDLData = false;     // reset newData flag

     LIS2MDLstatus = LIS2MDL.status();
     
     if(LIS2MDLstatus & 0x08) // if all axes have new data ready
     {
      LIS2MDL.readData(LIS2MDLData);  
   
   // Now we'll calculate the accleration value into actual G's
     mx = (float)LIS2MDLData[0]*mRes - magBias[0];  // get actual G value 
     my = (float)LIS2MDLData[1]*mRes - magBias[1];   
     mz = (float)LIS2MDLData[2]*mRes - magBias[2]; 
     mx *= magScale[0];
     my *= magScale[1];
     mz *= magScale[2];  
     }
   }
   }  // end of "if(passThru)" handling

   if(!passThru)
   {
      /*EM7180*/
  // If intpin goes high, all data registers have new data
  if (newEM7180Data == true) { // On interrupt, read data
    newEM7180Data = false;  // reset newData flag

    // Check event status register, way to chech data ready by polling rather than interrupt
    uint8_t eventStatus = USFS.checkEM7180Status(); // reading clears the register

    // Check for errors
    if (eventStatus & 0x02) { // error detected, what is it?

      uint8_t errorStatus = USFS.checkEM7180Errors();
      if (errorStatus != 0x00) { // is there an error?
        Serial.print(" EM7180 sensor status = "); Serial.println(errorStatus);
        if (errorStatus == 0x11) Serial.print("Magnetometer failure!");
        if (errorStatus == 0x12) Serial.print("Accelerometer failure!");
        if (errorStatus == 0x14) Serial.print("Gyro failure!");
        if (errorStatus == 0x21) Serial.print("Magnetometer initialization failure!");
        if (errorStatus == 0x22) Serial.print("Accelerometer initialization failure!");
        if (errorStatus == 0x24) Serial.print("Gyro initialization failure!");
        if (errorStatus == 0x30) Serial.print("Math error!");
        if (errorStatus == 0x80) Serial.print("Invalid sample rate!");
      }

      // Handle errors ToDo

    }

    // if no errors, see if new data is ready
    if (eventStatus & 0x10) { // new acceleration data available
      USFS.readSENtralAccelData(accelCount);

      // Now we'll calculate the accleration value into actual g's
      Ax = (float)accelCount[0] * 0.000488f; // get actual g value
      Ay = (float)accelCount[1] * 0.000488f;
      Az = (float)accelCount[2] * 0.000488f;
    }

    if (eventStatus & 0x20) { // new gyro data available
      USFS.readSENtralGyroData(gyroCount);

      // Now we'll calculate the gyro value into actual dps's
      Gx = (float)gyroCount[0] * 0.153f; // get actual dps value
      Gy = (float)gyroCount[1] * 0.153f;
      Gz = (float)gyroCount[2] * 0.153f;
    }

    if (eventStatus & 0x08) { // new mag data available
      USFS.readSENtralMagData(magCount);

      // Now we'll calculate the mag value into actual G's
      Mx = (float)magCount[0] * 0.305176f; // get actual G value
      My = (float)magCount[1] * 0.305176f;
      Mz = (float)magCount[2] * 0.305176f;
    }

    if (eventStatus & 0x04) { // new quaternion data available
      USFS.readSENtralQuatData(Q);
    }

    // get MS5637 pressure
    if (eventStatus & 0x40) { // new baro data available
      rawPressure = USFS.readSENtralBaroData();
      Pressure = (float)rawPressure * 0.01f + 1013.25f; // pressure in mBar

      // get MS5637 temperature
      rawTemperature = USFS.readSENtralTempData();
      Temperature = (float) rawTemperature * 0.01f; // temperature in degrees C
    }
  } 
   } // end of "if(!passThru)" handling
   
   // end sensor interrupt handling
    
   if(passThru)
   {
    if(SerialDebug) {
    Serial.print("ax = "); Serial.print((int)1000*ax);  
    Serial.print(" ay = "); Serial.print((int)1000*ay); 
    Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
    Serial.print("gx = "); Serial.print( gx, 2); 
    Serial.print(" gy = "); Serial.print( gy, 2); 
    Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
    Serial.print("mx = "); Serial.print((int)1000*mx);  
    Serial.print(" my = "); Serial.print((int)1000*my); 
    Serial.print(" mz = "); Serial.print((int)1000*mz); Serial.println(" mG");
    
    Serial.print("q0 = "); Serial.print(q[0]);
    Serial.print(" qx = "); Serial.print(q[1]); 
    Serial.print(" qy = "); Serial.print(q[2]); 
    Serial.print(" qz = "); Serial.println(q[3]); 
    }

    // get pressure and temperature from the LPS22HB
    LPS22Hstatus = LPS22H.status();

    if(LPS22Hstatus & 0x01) { // if new pressure data available
    pressure = (float) LPS22H.readAltimeterPressure()/4096.0f;
    temperature = (float) LPS22H.readAltimeterTemperature()/100.0f; 
    
    altitude = 145366.45f*(1.0f - pow((pressure/1013.25f), 0.190284f)); 

      if(SerialDebug) {
      Serial.print("Altimeter temperature = "); Serial.print( temperature, 2); Serial.println(" C"); // temperature in degrees Celsius  
      Serial.print("Altimeter temperature = "); Serial.print(9.0f*temperature/5.0f + 32.0f, 2); Serial.println(" F"); // temperature in degrees Fahrenheit
      Serial.print("Altimeter pressure = "); Serial.print(pressure, 2);  Serial.println(" mbar");// pressure in millibar
      Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet");
      }
    }

    Gtemperature = ((float) LSM6DSMData[0]) / 256.0f + 25.0f; // Gyro chip temperature in degrees Centigrade
    // Print temperature in degrees Centigrade      
    if(SerialDebug) {
      Serial.print("Gyro temperature is ");  Serial.print(Gtemperature, 1);  Serial.println(" degrees C"); // Print T values to tenths of s degree C
    }

    LIS2MDLData[3] = LIS2MDL.readTemperature();
    Mtemperature = ((float) LIS2MDLData[3]) / 8.0f + 25.0f; // Mag chip temperature in degrees Centigrade
    // Print temperature in degrees Centigrade      
    if(SerialDebug) {
      Serial.print("Mag temperature is ");  Serial.print(Mtemperature, 1);  Serial.println(" degrees C"); // Print T values to tenths of s degree C
    }

    a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);
    a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
    a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);
    a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);
    a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    pitch = -asinf(a32);
    roll  = atan2f(a31, a33);
    yaw   = atan2f(a12, a22);
    pitch *= 180.0f / pi;
    yaw   *= 180.0f / pi; 
    yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360
    roll  *= 180.0f / pi;
    lin_ax = ax + a31;
    lin_ay = ay + a32;
    lin_az = az - a33;

    if(SerialDebug) {
    Serial.print("Yaw, Pitch, Roll: ");
    Serial.print(yaw, 2);
    Serial.print(", ");
    Serial.print(pitch, 2);
    Serial.print(", ");
    Serial.println(roll, 2);

    Serial.print("Grav_x, Grav_y, Grav_z: ");
    Serial.print(-a31*1000.0f, 2);
    Serial.print(", ");
    Serial.print(-a32*1000.0f, 2);
    Serial.print(", ");
    Serial.print(a33*1000.0f, 2);  Serial.println(" mg");
    Serial.print("Lin_ax, Lin_ay, Lin_az: ");
    Serial.print(lin_ax*1000.0f, 2);
    Serial.print(", ");
    Serial.print(lin_ay*1000.0f, 2);
    Serial.print(", ");
    Serial.print(lin_az*1000.0f, 2);  Serial.println(" mg");
    
    Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
    }

//     Serial.print(millis()/1000);Serial.print(",");
//     Serial.print(yaw, 2); Serial.print(","); Serial.print(pitch, 2); Serial.print(","); Serial.print(roll, 2); Serial.print(","); Serial.println(Pressure, 2);

    sumCount = 0;
    sum = 0;      

    }  // end of "if(passThru)" handling

    if(!passThru)
    {

      if (SerialDebug) {
      Serial.print("Ax = "); Serial.print((int)1000 * Ax);
      Serial.print(" Ay = "); Serial.print((int)1000 * Ay);
      Serial.print(" Az = "); Serial.print((int)1000 * Az); Serial.println(" mg");
      Serial.print("Gx = "); Serial.print( Gx, 2);
      Serial.print(" Gy = "); Serial.print( Gy, 2);
      Serial.print(" Gz = "); Serial.print( Gz, 2); Serial.println(" deg/s");
      Serial.print("Mx = "); Serial.print( (int)Mx);
      Serial.print(" My = "); Serial.print( (int)My);
      Serial.print(" Mz = "); Serial.print( (int)Mz); Serial.println(" mG");

      Serial.println("Hardware quaternions:");
      Serial.print("Q0 = "); Serial.print(Q[0]);
      Serial.print(" Qx = "); Serial.print(Q[1]);
      Serial.print(" Qy = "); Serial.print(Q[2]);
      Serial.print(" Qz = "); Serial.println(Q[3]);
    }

    //Hardware AHRS:
    A112 =   2.0f * (Q[1] * Q[2] + Q[0] * Q[3]);
    A22 =   Q[0] * Q[0] + Q[1] * Q[1] - Q[2] * Q[2] - Q[3] * Q[3];
    A31 =   2.0f * (Q[0] * Q[1] + Q[2] * Q[3]);
    A32 =   2.0f * (Q[1] * Q[3] - Q[0] * Q[2]);
    A33 =   Q[0] * Q[0] - Q[1] * Q[1] - Q[2] * Q[2] + Q[3] * Q[3];
    Pitch = -asinf(A32);
    Roll  = atan2f(A31, A33);
    Yaw   = atan2f(A112, A22);
    Pitch *= 180.0f / pi;
    Yaw   *= 180.0f / pi;
    Yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    if (Yaw < 0) Yaw   += 360.0f ; // Ensure yaw stays between 0 and 360
    Roll  *= 180.0f / pi;
    lin_Ax = Ax + A31;
    lin_Ay = Ay + A32;
    lin_Az = Az - A33;

    if (SerialDebug) {
      Serial.print("Hardware Yaw, pitch, Roll: ");
      Serial.print(Yaw, 2);
      Serial.print(", ");
      Serial.print(Pitch, 2);
      Serial.print(", ");
      Serial.println(Roll, 2);

      Serial.print("Hardware Grav_x, Grav_y, Grav_z: ");
      Serial.print(-A31 * 1000, 2);
      Serial.print(", ");
      Serial.print(-A32 * 1000, 2);
      Serial.print(", ");
      Serial.print(A33 * 1000, 2);  Serial.println(" mg");
      Serial.print("Hardware Lin_ax, Lin_ay, Lin_az: ");
      Serial.print(lin_Ax * 1000, 2);
      Serial.print(", ");
      Serial.print(lin_Ay * 1000, 2);
      Serial.print(", ");
      Serial.print(lin_Az * 1000, 2);  Serial.println(" mg");

      Serial.println("MS5637:");
      Serial.print("Altimeter temperature = ");
      Serial.print(Temperature, 2);
      Serial.println(" C"); // temperature in degrees Celsius
      Serial.print("Altimeter temperature = ");
      Serial.print(9.0f * Temperature / 5.0f + 32.0f, 2);
      Serial.println(" F"); // temperature in degrees Fahrenheit
      Serial.print("Altimeter pressure = ");
      Serial.print(Pressure, 2);
      Serial.println(" mbar");// pressure in millibar
      Altitude = 145366.45f * (1.0f - powf(((Pressure) / 1013.25f), 0.190284f));
      Serial.print("Altitude = ");
      Serial.print(Altitude, 2);
      Serial.println(" feet");
      Serial.println(" ");
    }
    
    } // end of "if(!passThru)" handling
   
      digitalWrite(myLed, HIGH); delay(1); digitalWrite(myLed, LOW);  // flash led for 10 milliseconds
      delay(500);
 
}  //end of loop

/*  End of main loop */


void myinthandler1()
{
  newLSM6DSMData = true;
}

void myinthandler2()
{
  newLIS2MDLData = true;
}

void myinthandler3()
{
  newLPS22HData = true;
}

void EM7180intHandler()
{
  newEM7180Data = true;
}






================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/I2Cdev.cpp
================================================
/*
 * Copyright (c) 2018 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#include "Arduino.h"
#include "I2Cdev.h"

I2Cdev::I2Cdev(TwoWire* i2c_bus)                                                                                                             // Class constructor
{
  _i2c_bus = i2c_bus;
}

I2Cdev::~I2Cdev()                                                                                                                            // Class destructor
{
}

/**
* @fn: readByte(uint8_t address, uint8_t subAddress)
*
* @brief: Read one byte from an I2C device
* 
* @params: I2C slave device address, Register subAddress
* @returns: unsigned short read
*/
uint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)
{
  uint8_t data = 0;                             // `data` will store the register data   
  _i2c_bus->beginTransmission(address);         // Initialize the Tx buffer
  _i2c_bus->write(subAddress);                  // Put slave register address in Tx buffer
  _i2c_bus->endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
  _i2c_bus->requestFrom(address, 1);            // Read one byte from slave register address  
  data = _i2c_bus->read();                      // Fill Rx buffer with result
  return data;                                  // Return data read from slave register
  
}


/**
* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
*
* @brief: Read multiple bytes from an I2C device
* 
* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data
* @returns: void
*/
void I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
  _i2c_bus->beginTransmission(address);   // Initialize the Tx buffer
  _i2c_bus->write(subAddress);            // Put slave register address in Tx buffer
  _i2c_bus->endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
  uint8_t i = 0;
  _i2c_bus->requestFrom(address, count);  // Read bytes from slave register address 
  while (_i2c_bus->available()) {
        dest[i++] = _i2c_bus->read(); }   // Put read results in the Rx buffer
}


/**
* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write one byte to an I2C device
* 
* @params: I2C slave device address, Register subAddress, data to be written
* @returns: void
*/
void I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
{
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  _i2c_bus->write(regAddr);           // Put slave register address in Tx buffer
  _i2c_bus->write(data);                 // Put data in Tx buffer
  _i2c_bus->endTransmission();           // Send the Tx buffer
}


/**
* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write multiple bytes to an I2C device
* 
* @params: I2C slave device address, Register subAddress, byte count, data array to be written
* @returns: void
*/
void I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)
{
  uint8_t temp[1 + count];
  
  temp[0] = regAddr;
  for (uint8_t ii = 0; ii < count; ii++)
  { 
    temp[ii + 1] = dest[ii];
  }
  
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  
  for (uint8_t jj = 0; jj < count + 1; jj++)
  {
  _i2c_bus->write(temp[jj]);            // Put data in Tx buffer
  }
  
  _i2c_bus->endTransmission();           // Send the Tx buffer
}



/**
* @fn:I2Cscan()
* @brief: Scan the I2C bus for active I2C slave devices
* 
* @params: void
* @returns: void
*/
void I2Cdev::I2Cscan() 
{
  // 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 Wire.endTransmisstion to see if a device did acknowledge to the address.
    _i2c_bus->beginTransmission(address);
    error = _i2c_bus->endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
      Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");
      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("I2C scan complete\n");
}



================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/I2Cdev.h
================================================
/*
 * Copyright (c) 2018 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#ifndef _I2CDEV_H_
#define _I2CDEV_H_

#include <Wire.h>

class I2Cdev {
    public:
                                        I2Cdev(TwoWire*);
                                        ~I2Cdev();                                                                                                                     // Class destructor for durable instances
         uint8_t                        readByte(uint8_t address, uint8_t subAddress);
         void                           readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
         void                           writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
         void                           writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);
         void                           I2Cscan();
         
    private:
         TwoWire*                       _i2c_bus;                                                                                                                      // Class constructor argument
};

#endif //_I2CDEV_H_


================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LIS2MDL.cpp
================================================
/* 09/23/2017 Copyright Tlera Corporation

    Created by Kris Winer

  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
  The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution.

  Library may be used freely and without limit with attribution.

*/

#include "LIS2MDL.h"

LIS2MDL::LIS2MDL(uint8_t intPin, I2Cdev* i2c_bus)
{
  _intPin = intPin;
  _i2c_bus = i2c_bus;
}


uint8_t LIS2MDL::getChipID()
{
  uint8_t c = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
  return c;
}


void LIS2MDL::reset()
{
  // reset device
  uint8_t temp = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
  _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
  delay(1);
  _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
  delay(100); // Wait for all registers to reset 
}

void LIS2MDL::init(uint8_t MODR)
{
 // enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
 _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR<<2);  

 // enable low pass filter (bit 0 == 1), set to ODR/4
 _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);  

 // enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)
 _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);  
}


uint8_t LIS2MDL::status()
{
  // Read the status register of the altimeter  
  uint8_t temp = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG);   
  return temp;
}


void LIS2MDL::readData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z mag register data stored here
  _i2c_bus->readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8, &rawData[0]);  // Read the 6 raw data registers into data array

  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 
}


int16_t LIS2MDL::readTemperature()
{
  uint8_t rawData[2];  // x/y/z mag register data stored here
  _i2c_bus->readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2, &rawData[0]);  // Read the 8 raw data registers into data array
  int16_t temp = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
  return temp;
}


void LIS2MDL::offsetBias(float * dest1, float * dest2)
{
  int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
  int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
  float _mRes = 0.0015f;
  
  Serial.println("Calculate mag offset bias: move all around to sample the complete response surface!");
  delay(4000);

  for (int ii = 0; ii < 4000; ii++)
  {
    readData(mag_temp);
       for (int jj = 0; jj < 3; jj++) {
      if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
      if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
    }
    delay(12);
  }

  _mRes = 0.0015f; // fixed sensitivity
    // Get hard iron correction
    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
    
    dest1[0] = (float) mag_bias[0] * _mRes;  // save mag biases in G for main program
    dest1[1] = (float) mag_bias[1] * _mRes;   
    dest1[2] = (float) mag_bias[2] * _mRes;  
       
    // Get soft iron correction estimate
    mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts
    mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts
    mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts

    float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
    avg_rad /= 3.0f;

    dest2[0] = avg_rad/((float)mag_scale[0]);
    dest2[1] = avg_rad/((float)mag_scale[1]);
    dest2[2] = avg_rad/((float)mag_scale[2]);
  
   Serial.println("Mag Calibration done!");
}

void LIS2MDL::selfTest()
{
  int16_t temp[3] = {0, 0, 0};
  float magTest[3] = {0., 0., 0.};
  float magNom[3] = {0., 0., 0.};
  int32_t sum[3] = {0, 0, 0};
  float _mRes = 0.0015f;
    
  // first, get average response with self test disabled
  for (int ii = 0; ii < 50; ii++)
  {
    readData(temp);
    sum[0] += temp[0];
    sum[1] += temp[1];
    sum[2] += temp[2];
    delay(50);
  }
  
  magNom[0] = (float) sum[0] / 50.0f;
  magNom[1] = (float) sum[1] / 50.0f;
  magNom[2] = (float) sum[2] / 50.0f;
  
  uint8_t c = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
  _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
  delay(100); // let mag respond
  
  sum[0] = 0;
  sum[1] = 0;
  sum[2] = 0;
  for (int ii = 0; ii < 50; ii++)
  {
    readData(temp);
    sum[0] += temp[0];
    sum[1] += temp[1];
    sum[2] += temp[2];
    delay(50);
  }
  
  magTest[0] = (float) sum[0] / 50.0f;
  magTest[1] = (float) sum[1] / 50.0f;
  magTest[2] = (float) sum[2] / 50.0f;
  
  _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
  delay(100); // let mag respond

  Serial.println("Mag Self Test:");
  Serial.print("Mx results:"); Serial.print(  (magTest[0] - magNom[0]) * _mRes * 1000.0); Serial.println(" mG");
  Serial.print("My results:"); Serial.println((magTest[0] - magNom[0]) * _mRes * 1000.0);
  Serial.print("Mz results:"); Serial.println((magTest[1] - magNom[1]) * _mRes * 1000.0);
  Serial.println("Should be between 15 and 500 mG");
  delay(2000);  // give some time to read the screen
}



================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LIS2MDL.h
================================================
/* 09/23/2017 Copyright Tlera Corporation

    Created by Kris Winer

  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
  The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution.

  Library may be used freely and without limit with attribution.

*/

#ifndef LIS2MDL_h
#define LIS2MDL_h

#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"

//Register map for LIS2MDL'
// http://www.st.com/content/ccc/resource/technical/document/datasheet/group3/29/13/d1/e0/9a/4d/4f/30/DM00395193/files/DM00395193.pdf/jcr:content/translations/en.DM00395193.pdf
#define LIS2MDL_OFFSET_X_REG_L        0x45
#define LIS2MDL_OFFSET_X_REG_H        0x46
#define LIS2MDL_OFFSET_Y_REG_L        0x47
#define LIS2MDL_OFFSET_Y_REG_H        0x48
#define LIS2MDL_OFFSET_Z_REG_L        0x49
#define LIS2MDL_OFFSET_Z_REG_H        0x4A
#define LIS2MDL_WHO_AM_I              0x4F
#define LIS2MDL_CFG_REG_A             0x60
#define LIS2MDL_CFG_REG_B             0x61
#define LIS2MDL_CFG_REG_C             0x62
#define LIS2MDL_INT_CTRL_REG          0x63
#define LIS2MDL_INT_SOURCE_REG        0x64
#define LIS2MDL_INT_THS_L_REG         0x65
#define LIS2MDL_INT_THS_H_REG         0x66
#define LIS2MDL_STATUS_REG            0x67
#define LIS2MDL_OUTX_L_REG            0x68
#define LIS2MDL_OUTX_H_REG            0x69
#define LIS2MDL_OUTY_L_REG            0x6A
#define LIS2MDL_OUTY_H_REG            0x6B
#define LIS2MDL_OUTZ_L_REG            0x6C
#define LIS2MDL_OUTZ_H_REG            0x6D
#define LIS2MDL_TEMP_OUT_L_REG        0x6E
#define LIS2MDL_TEMP_OUT_H_REG        0x6F

#define LIS2MDL_ADDRESS               0x1E

#define MODR_10Hz   0x00
#define MODR_20Hz   0x01
#define MODR_50Hz   0x02
#define MODR_100Hz  0x03


class LIS2MDL
{
  public:
  LIS2MDL(uint8_t intPin,   I2Cdev* i2c_bus);
  uint8_t getChipID();
  void init(uint8_t MODR);
  void offsetBias(float * dest1, float * dest2);
  void reset();
  void selfTest();
  uint8_t status();
  void readData(int16_t * destination);
  int16_t readTemperature();
  void I2Cscan();
  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
  uint8_t readByte(uint8_t address, uint8_t subAddress);
  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
  private:
  uint8_t _intPin;
  float _mRes;
  I2Cdev* _i2c_bus;
};

#endif


================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LPS22HB.cpp
================================================
/* 09/23/2017 Copyright Tlera Corporation

    Created by Kris Winer

  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
  The LPS22HB is a low power barometerr.

  Library may be used freely and without limit with attribution.

*/

#include "LPS22HB.h"
#include "Wire.h"

LPS22H::LPS22H(uint8_t intPin, I2Cdev* i2c_bus)
{
  _intPin = intPin;
  _i2c_bus = i2c_bus; 
}

uint8_t LPS22H::getChipID()
{
  // Read the WHO_AM_I register of the altimeter this is a good test of communication
  uint8_t temp = _i2c_bus->readByte(LPS22H_ADDRESS, LPS22H_WHOAMI);  // Read WHO_AM_I register for LPS22H
  return temp;
}

uint8_t LPS22H::status()
{
  // Read the status register of the altimeter  
  uint8_t temp = _i2c_bus->readByte(LPS22H_ADDRESS, LPS22H_STATUS);   
  return temp;
}

int32_t LPS22H::readAltimeterPressure()
{
    uint8_t rawData[3];  // 24-bit pressure register data stored here
    _i2c_bus->readBytes(LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), 3, &rawData[0]); // bit 7 must be one to read multiple bytes
    return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8 | rawData[0]);
}

int16_t LPS22H::readAltimeterTemperature()
{
    uint8_t rawData[2];  // 16-bit pressure register data stored here
    _i2c_bus->readBytes(LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), 2, &rawData[0]); // bit 7 must be one to read multiple bytes
    return (int16_t)((int16_t) rawData[1] << 8 | rawData[0]);
}


void LPS22H::Init(uint8_t PODR)
{
  // set sample rate by setting bits 6:4 
  // enable low-pass filter by setting bit 3 to one
  // bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20
  // make sure data not updated during read by setting block data udate (bit 1) to 1
    _i2c_bus->writeByte(LPS22H_ADDRESS, LPS22H_CTRL_REG1, PODR << 4 | 0x08 | 0x02);  
    _i2c_bus->writeByte(LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04);  // enable data ready as interrupt source
}




================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LPS22HB.h
================================================
/* 09/23/2017 Copyright Tlera Corporation

    Created by Kris Winer

  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
  The LPS22HB is a low power barometerr.

  Library may be used freely and without limit with attribution.

*/

#ifndef LPS22HB_h
#define LPS22HB_h

#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"

// See LPS22H "MEMS pressure sensor: 260-1260 hPa absolute digital output barometer" Data Sheet
// http://www.st.com/content/ccc/resource/technical/document/datasheet/bf/c1/4f/23/61/17/44/8a/DM00140895.pdf/files/DM00140895.pdf/jcr:content/translations/en.DM00140895.pdf
#define LPS22H_INTERRUPT_CFG 0x0B
#define LPS22H_THS_P_L       0x0C
#define LPS22H_THS_P_H       0x0D
#define LPS22H_WHOAMI        0x0F // should return 0xB1
#define LPS22H_CTRL_REG1     0x10
#define LPS22H_CTRL_REG2     0x11
#define LPS22H_CTRL_REG3     0x12
#define LPS22H_FIFO_CTRL     0x14
#define LPS22H_REF_P_XL      0x15
#define LPS22H_REF_P_L       0x16
#define LPS22H_REF_P_H       0x17
#define LPS22H_RPDS_L        0x18
#define LPS22H_RPDS_H        0x19
#define LPS22H_RES_CONF      0x1A
#define LPS22H_INT_SOURCE    0x25
#define LPS22H_FIFO_STATUS   0x26
#define LPS22H_STATUS        0x27
#define LPS22H_PRESS_OUT_XL  0x28
#define LPS22H_PRESS_OUT_L   0x29
#define LPS22H_PRESS_OUT_H   0x2A
#define LPS22H_TEMP_OUT_L    0x2B
#define LPS22H_TEMP_OUT_H    0x2C
#define LPS22H_LPFP_RES      0x33

#define LPS22H_ADDRESS 0x5C   // Address of altimeter

// Altimeter output data rate
#define    P_1shot  0x00;
#define    P_1Hz    0x01;
#define    P_10Hz   0x02;
#define    P_25Hz   0x03;  // 25 Hz output data rate
#define    P_50Hz   0x04;
#define    P_75Hz   0x05;

class LPS22H
{
  public: 
  LPS22H(uint8_t intPin, I2Cdev* i2c_bus);
  void Init(uint8_t PODR);
  uint8_t getChipID();
  uint8_t status();
  int32_t readAltimeterPressure();
  int16_t readAltimeterTemperature();
  void I2Cscan();
  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
  uint8_t readByte(uint8_t address, uint8_t subAddress);
  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
  private:
  uint8_t   _intPin;
  I2Cdev* _i2c_bus;
};

#endif


================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LSM6DSM.cpp
================================================
/* 09/23/2017 Copyright Tlera Corporation

    Created by Kris Winer

  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
  The LSM6DSM is a sensor hub with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution.

  Library may be used freely and without limit with attribution.

*/

#include "LSM6DSM.h"

LSM6DSM::LSM6DSM(uint8_t intPin1, uint8_t intPin2, I2Cdev* i2c_bus)
{
  _intPin1 = intPin1;
  _intPin2 = intPin2;  
  _i2c_bus = i2c_bus; 
}


uint8_t LSM6DSM::getChipID()
{
  uint8_t c = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);
  return c;
}

float LSM6DSM::getAres(uint8_t Ascale) {
  switch (Ascale)
  {
  // Possible accelerometer scales (and their register bit settings) are:
  // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11). 
        // Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:
    case AFS_2G:
         _aRes = 2.0f/32768.0f;
         return _aRes;
         break;
    case AFS_4G:
         _aRes = 4.0f/32768.0f;
         return _aRes;
         break;
    case AFS_8G:
         _aRes = 8.0f/32768.0f;
         return _aRes;
         break;
    case AFS_16G:
         _aRes = 16.0f/32768.0f;
         return _aRes;
         break;
  }
}

float LSM6DSM::getGres(uint8_t Gscale) {
  switch (Gscale)
  {
  // Possible gyro scales (and their register bit settings) are:
  // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11). 
    case GFS_245DPS:
          _gRes = 245.0f/32768.0f;
          return _gRes;
          break;
    case GFS_500DPS:
          _gRes = 500.0f/32768.0f;
          return _gRes;
          break;
    case GFS_1000DPS:
         _gRes = 1000.0f/32768.0f;
         return _gRes;
         break;
    case GFS_2000DPS:
          _gRes = 2000.0f/32768.0f;
         return _gRes;
         break;
  }
}


void LSM6DSM::reset()
{
  // reset device
  uint8_t temp = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM
  delay(100); // Wait for all registers to reset 
}


void LSM6DSM::init(uint8_t Ascale, uint8_t Gscale, uint8_t AODR, uint8_t GODR)
{
  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL, AODR << 4 | Ascale << 2);
  
  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G, GODR << 4 | Gscale << 2);
 
  uint8_t temp = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);
  // enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)
  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04); 
  // by default, interrupts active HIGH, push pull, little endian data 
  // (can be changed by writing to bits 5, 4, and 1, resp to above register)

   // enable accel LP2 (bit 7 = 1), set LP2 tp ODR/9 (bit 6 = 1), enable input_composite (bit 3) for low noise
   _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08 );

   // interrupt handling
    _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read
    _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40);      // enable significant motion interrupts on INT1
    _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03);      // enable accel/gyro data ready interrupts on INT2  
}


void LSM6DSM::selfTest()
{
  int16_t temp[7] = {0, 0, 0, 0, 0, 0, 0};
  int16_t accelPTest[3] = {0, 0, 0}, accelNTest[3] = {0, 0, 0}, gyroPTest[3] = {0, 0, 0}, gyroNTest[3] = {0, 0, 0};
  int16_t accelNom[3] = {0, 0, 0}, gyroNom[3] = {0, 0, 0};

  readData(temp);
  accelNom[0] = temp[4];
  accelNom[1] = temp[5];
  accelNom[2] = temp[6];
  gyroNom[0]  = temp[1];
  gyroNom[1]  = temp[2];
  gyroNom[2]  = temp[3];
  
  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test
  delay(100); // let accel respond
  readData(temp);
  accelPTest[0] = temp[4];
  accelPTest[1] = temp[5];
  accelPTest[2] = temp[6];

  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test
  delay(100); // let accel respond
  readData(temp);
  accelNTest[0] = temp[4];
  accelNTest[1] = temp[5];
  accelNTest[2] = temp[6];

  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test
  delay(100); // let gyro respond
  readData(temp);
  gyroPTest[0] = temp[1];
  gyroPTest[1] = temp[2];
  gyroPTest[2] = temp[3];

  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test
  delay(100); // let gyro respond
  readData(temp);
  gyroNTest[0] = temp[1];
  gyroNTest[1] = temp[2];
  gyroNTest[2] = temp[3];

  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode
  delay(100); // let accel and gyro respond

  Serial.println("Accel Self Test:");
  Serial.print("+Ax results:"); Serial.print(  (accelPTest[0] - accelNom[0]) * _aRes * 1000.0); Serial.println(" mg");
  Serial.print("-Ax results:"); Serial.println((accelNTest[0] - accelNom[0]) * _aRes * 1000.0);
  Serial.print("+Ay results:"); Serial.println((accelPTest[1] - accelNom[1]) * _aRes * 1000.0);
  Serial.print("-Ay results:"); Serial.println((accelNTest[1] - accelNom[1]) * _aRes * 1000.0);
  Serial.print("+Az results:"); Serial.println((accelPTest[2] - accelNom[2]) * _aRes * 1000.0);
  Serial.print("-Az results:"); Serial.println((accelNTest[2] - accelNom[2]) * _aRes * 1000.0);
  Serial.println("Should be between 90 and 1700 mg");

  Serial.println("Gyro Self Test:");
  Serial.print("+Gx results:"); Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); Serial.println(" dps");
  Serial.print("-Gx results:"); Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes);
  Serial.print("+Gy results:"); Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes);
  Serial.print("-Gy results:"); Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes);
  Serial.print("+Gz results:"); Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes);
  Serial.print("-Gz results:"); Serial.println((gyroNTest[2] - gyroNom[2]) * _gRes);
  Serial.println("Should be between 20 and 80 dps");
  delay(2000);

 
}


void LSM6DSM::offsetBias(float * dest1, float * dest2)
{
  int16_t temp[7] = {0, 0, 0, 0, 0, 0, 0};
  int32_t sum[7] = {0, 0, 0, 0, 0, 0, 0};
    
  Serial.println("Calculate accel and gyro offset biases: keep sensor flat and motionless!");
  delay(4000);

  for (int ii = 0; ii < 128; ii++)
  {
    readData(temp);
    sum[1] += temp[1];
    sum[2] += temp[2];
    sum[3] += temp[3];
    sum[4] += temp[4];
    sum[5] += temp[5];
    sum[6] += temp[6];
    delay(50);
  }

  dest1[0] = sum[1]*_gRes/128.0f;
  dest1[1] = sum[2]*_gRes/128.0f;
  dest1[2] = sum[3]*_gRes/128.0f;
  dest2[0] = sum[4]*_aRes/128.0f;
  dest2[1] = sum[5]*_aRes/128.0f;
  dest2[2] = sum[6]*_aRes/128.0f;

  if(dest2[0] > 0.8f)  {dest2[0] -= 1.0f;}  // Remove gravity from the x-axis accelerometer bias calculation
  if(dest2[0] < -0.8f) {dest2[0] += 1.0f;}  // Remove gravity from the x-axis accelerometer bias calculation
  if(dest2[1] > 0.8f)  {dest2[1] -= 1.0f;}  // Remove gravity from the y-axis accelerometer bias calculation
  if(dest2[1] < -0.8f) {dest2[1] += 1.0f;}  // Remove gravity from the y-axis accelerometer bias calculation
  if(dest2[2] > 0.8f)  {dest2[2] -= 1.0f;}  // Remove gravity from the z-axis accelerometer bias calculation
  if(dest2[2] < -0.8f) {dest2[2] += 1.0f;}  // Remove gravity from the z-axis accelerometer bias calculation

}


void LSM6DSM::readData(int16_t * destination)
{
  uint8_t rawData[14];  // x/y/z accel register data stored here
  _i2c_bus->readBytes(LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, 14, &rawData[0]);  // Read the 14 raw data registers into data array
  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  
  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 
  destination[3] = ((int16_t)rawData[7] << 8) | rawData[6] ;   
  destination[4] = ((int16_t)rawData[9] << 8) | rawData[8] ;  
  destination[5] = ((int16_t)rawData[11] << 8) | rawData[10] ;  
  destination[6] = ((int16_t)rawData[13] << 8) | rawData[12] ; 
}


================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LSM6DSM.h
================================================
/* 09/23/2017 Copyright Tlera Corporation

    Created by Kris Winer

  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.
  The LSM6DSM is a sensor hub with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution.

  Library may be used freely and without limit with attribution.

*/

#ifndef LSM6DSM_h
#define LSM6DSM_h

#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"

/* LSM6DSM registers
  http://www.st.com/content/ccc/resource/technical/document/datasheet/76/27/cf/88/c5/03/42/6b/DM00218116.pdf/files/DM00218116.pdf/jcr:content/translations/en.DM00218116.pdf
*/
#define LSM6DSM_FUNC_CFG_ACCESS           0x01
#define LSM6DSM_SENSOR_SYNC_TIME_FRAME    0x04
#define LSM6DSM_SENSOR_SYNC_RES_RATIO     0x05
#define LSM6DSM_FIFO_CTRL1                0x06
#define LSM6DSM_FIFO_CTRL2                0x07
#define LSM6DSM_FIFO_CTRL3                0x08
#define LSM6DSM_FIFO_CTRL4                0x09
#define LSM6DSM_FIFO_CTRL5                0x0A
#define LSM6DSM_DRDY_PULSE_CFG            0x0B
#define LSM6DSM_INT1_CTRL                 0x0D
#define LSM6DSM_INT2_CTRL                 0x0E
#define LSM6DSM_WHO_AM_I                  0x0F  // should be 0x6A
#define LSM6DSM_CTRL1_XL                  0x10
#define LSM6DSM_CTRL2_G                   0x11
#define LSM6DSM_CTRL3_C                   0x12
#define LSM6DSM_CTRL4_C                   0x13
#define LSM6DSM_CTRL5_C                   0x14
#define LSM6DSM_CTRL6_C                   0x15
#define LSM6DSM_CTRL7_G                   0x16
#define LSM6DSM_CTRL8_XL                  0x17
#define LSM6DSM_CTRL9_XL                  0x18
#define LSM6DSM_CTRL10_C                  0x19
#define LSM6DSM_MASTER_CONFIG             0x1A
#define LSM6DSM_WAKE_UP_SRC               0x1B
#define LSM6DSM_TAP_SRC                   0x1C
#define LSM6DSM_D6D_SRC                   0x1D
#define LSM6DSM_STATUS_REG                0x1E
#define LSM6DSM_OUT_TEMP_L                0x20
#define LSM6DSM_OUT_TEMP_H                0x21
#define LSM6DSM_OUTX_L_G                  0x22
#define LSM6DSM_OUTX_H_G                  0x23
#define LSM6DSM_OUTY_L_G                  0x24
#define LSM6DSM_OUTY_H_G                  0x25
#define LSM6DSM_OUTZ_L_G                  0x26
#define LSM6DSM_OUTZ_H_G                  0x27
#define LSM6DSM_OUTX_L_XL                 0x28
#define LSM6DSM_OUTX_H_XL                 0x29
#define LSM6DSM_OUTY_L_XL                 0x2A
#define LSM6DSM_OUTY_H_XL                 0x2B
#define LSM6DSM_OUTZ_L_XL                 0x2C
#define LSM6DSM_OUTZ_H_XL                 0x2D
#define LSM6DSM_SENSORHUB1_REG            0x2E
#define LSM6DSM_SENSORHUB2_REG            0x2F
#define LSM6DSM_SENSORHUB3_REG            0x30
#define LSM6DSM_SENSORHUB4_REG            0x31
#define LSM6DSM_SENSORHUB5_REG            0x32
#define LSM6DSM_SENSORHUB6_REG            0x33
#define LSM6DSM_SENSORHUB7_REG            0x34
#define LSM6DSM_SENSORHUB8_REG            0x35
#define LSM6DSM_SENSORHUB9_REG            0x36
#define LSM6DSM_SENSORHUB10_REG           0x37
#define LSM6DSM_SENSORHUB11_REG           0x38
#define LSM6DSM_SENSORHUB12_REG           0x39
#define LSM6DSM_FIFO_STATUS1              0x3A
#define LSM6DSM_FIFO_STATUS2              0x3B
#define LSM6DSM_FIFO_STATUS3              0x3C
#define LSM6DSM_FIFO_STATUS4              0x3D
#define LSM6DSM_FIFO_DATA_OUT_L           0x3E
#define LSM6DSM_FIFO_DATA_OUT_H           0x3F
#define LSM6DSM_TIMESTAMP0_REG            0x40
#define LSM6DSM_TIMESTAMP1_REG            0x41
#define LSM6DSM_TIMESTAMP2_REG            0x42
#define LSM6DSM_STEP_TIMESTAMP_L          0x49
#define LSM6DSM_STEP_TIMESTAMP_H          0x4A
#define LSM6DSM_STEP_COUNTER_L            0x4B
#define LSM6DSM_STEP_COUNTER_H            0x4C
#define LSM6DSM_SENSORHUB13_REG           0x4D
#define LSM6DSM_SENSORHUB14_REG           0x4E
#define LSM6DSM_SENSORHUB15_REG           0x4F
#define LSM6DSM_SENSORHUB16_REG           0x50
#define LSM6DSM_SENSORHUB17_REG           0x51
#define LSM6DSM_SENSORHUB18_REG           0x52
#define LSM6DSM_FUNC_SRC1                 0x53
#define LSM6DSM_FUNC_SRC2                 0x54
#define LSM6DSM_WRIST_TILT_IA             0x55
#define LSM6DSM_TAP_CFG                   0x58
#define LSM6DSM_TAP_THS_6D                0x59
#define LSM6DSM_INT_DUR2                  0x5A
#define LSM6DSM_WAKE_UP_THS               0x5B
#define LSM6DSM_WAKE_UP_DUR               0x5C
#define LSM6DSM_FREE_FALL                 0x5D
#define LSM6DSM_MD1_CFG                   0x5E
#define LSM6DSM_MD2_CFG                   0x5F
#define LSM6DSM_MASTER_MODE_CODE          0x60
#define LSM6DSM_SENS_SYNC_SPI_ERROR_CODE  0x61
#define LSM6DSM_OUT_MAG_RAW_X_L           0x66
#define LSM6DSM_OUT_MAG_RAW_X_H           0x67
#define LSM6DSM_OUT_MAG_RAW_Y_L           0x68
#define LSM6DSM_OUT_MAG_RAW_Y_H           0x69
#define LSM6DSM_OUT_MAG_RAW_Z_L           0x6A
#define LSM6DSM_OUT_MAG_RAW_Z_H           0x6B
#define LSM6DSM_INT_OIS                   0x6F
#define LSM6DSM_CTRL1_OIS                 0x70
#define LSM6DSM_CTRL2_OIS                 0x71
#define LSM6DSM_CTRL3_OIS                 0x72
#define LSM6DSM_X_OFS_USR                 0x73
#define LSM6DSM_Y_OFS_USR                 0x74
#define LSM6DSM_Z_OFS_USR                 0x75

#define LSM6DSM_ADDRESS           0x6A   // Address of LSM6DSM accel/gyro when ADO = 0


#define AFS_2G  0x00
#define AFS_4G  0x02
#define AFS_8G  0x03
#define AFS_16G 0x01

#define GFS_245DPS  0x00
#define GFS_500DPS  0x01
#define GFS_1000DPS 0x02
#define GFS_2000DPS 0x03

#define AODR_12_5Hz  0x01  // same for accel and gyro in normal mode
#define AODR_26Hz    0x02
#define AODR_52Hz    0x03
#define AODR_104Hz   0x04
#define AODR_208Hz   0x05
#define AODR_416Hz   0x06
#define AODR_833Hz   0x07
#define AODR_1660Hz  0x08
#define AODR_3330Hz  0x09
#define AODR_6660Hz  0x0A

#define GODR_12_5Hz  0x01   
#define GODR_26Hz    0x02
#define GODR_52Hz    0x03
#define GODR_104Hz   0x04
#define GODR_208Hz   0x05
#define GODR_416Hz   0x06
#define GODR_833Hz   0x07
#define GODR_1660Hz  0x08
#define GODR_3330Hz  0x09
#define GODR_6660Hz  0x0A


class LSM6DSM
{
  public:
  LSM6DSM(uint8_t intPin1, uint8_t intPin2, I2Cdev* i2c_bus);
  float getAres(uint8_t Ascale);
  float getGres(uint8_t Gscale);
  uint8_t getChipID();
  void init(uint8_t Ascale, uint8_t Gscale, uint8_t AODR, uint8_t GODR);
  void offsetBias(float * dest1, float * dest2);
  void reset();
  void selfTest();
  void readData(int16_t * destination);
  void I2Cscan();
  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
  uint8_t readByte(uint8_t address, uint8_t subAddress);
  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
  private:
  uint8_t _intPin1;
  uint8_t _intPin2;
  float _aRes, _gRes;
  I2Cdev* _i2c_bus;
};

#endif


================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/USFS.cpp
================================================
/* 06/29/2017 Copyright Tlera Corporation
 *  
 *  Created by Kris Winer
 *  
 *  
 *  Library may be used freely and without limit with attribution.
 *  
 */
#include "USFS.h"

USFS::USFS(uint8_t intPin, bool passThru, I2Cdev* i2c_bus)
{
  _intPin = intPin;
  _passThru = passThru;
  _i2c_bus = i2c_bus;    
}

void USFS::getChipID()
{
  // Read SENtral device information
  uint16_t ROM1 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ROMVersion1);
  uint16_t ROM2 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ROMVersion2);
  Serial.print("EM7180 ROM Version: 0x"); Serial.print(ROM1, HEX); Serial.println(ROM2, HEX); Serial.println("Should be: 0xE609");
  uint16_t RAM1 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RAMVersion1);
  uint16_t RAM2 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RAMVersion2);
  Serial.print("EM7180 RAM Version: 0x"); Serial.print(RAM1); Serial.println(RAM2);
  uint8_t PID = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ProductID);
  Serial.print("EM7180 ProductID: 0x"); Serial.print(PID, HEX); Serial.println(" Should be: 0x80");
  uint8_t RID = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RevisionID);
  Serial.print("EM7180 RevisionID: 0x"); Serial.print(RID, HEX); Serial.println(" Should be: 0x02");
}

 void USFS::loadfwfromEEPROM()
 {
  // Check which sensors can be detected by the EM7180
  uint8_t featureflag = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_FeatureFlags);
    if(featureflag & 0x01)  Serial.println("A barometer is installed");
    if(featureflag & 0x02)  Serial.println("A humidity sensor is installed");
    if(featureflag & 0x04)  Serial.println("A temperature sensor is installed");
    if(featureflag & 0x08)  Serial.println("A custom sensor is installed");
    if(featureflag & 0x10)  Serial.println("A second custom sensor is installed");
    if(featureflag & 0x20)  Serial.println("A third custom sensor is installed");
  
  delay(1000); // give some time to read the screen

  // Check SENtral status, make sure EEPROM upload of firmware was accomplished
  byte STAT = (_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)  Serial.println("EEPROM detected on the sensor bus!");
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)  Serial.println("EEPROM uploaded config file!");
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)  Serial.println("EEPROM CRC incorrect!");
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)  Serial.println("EM7180 in initialized state!");
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)  Serial.println("No EEPROM detected!");
  int count = 0;
  while(!STAT) {
    _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);
    delay(500);  
    count++;  
    STAT = (_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)  Serial.println("EEPROM detected on the sensor bus!");
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)  Serial.println("EEPROM uploaded config file!");
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)  Serial.println("EEPROM CRC incorrect!");
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)  Serial.println("EM7180 in initialized state!");
    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)  Serial.println("No EEPROM detected!");
    if(count > 10) break;
  }
  
   if(!(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04))  Serial.println("EEPROM upload successful!");
 }

  uint8_t USFS::checkEM7180Status(){
  // Check event status register, way to check data ready by polling rather than interrupt
  uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt
  return c;
  }

  uint8_t USFS::checkEM7180Errors(){
  uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ErrorRegister); // check error register
  return c;
  }
  
 void  USFS::initEM7180(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, uint16_t magFS, uint8_t QRtDiv, uint8_t magRt, uint8_t accRt, uint8_t gyroRt, uint8_t baroRt)
 {
  uint16_t EM7180_mag_fs, EM7180_acc_fs, EM7180_gyro_fs; // EM7180 sensor full scale ranges
  uint8_t param[4];      

  // Enter EM7180 initialized state
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers
  
  //Setup LPF bandwidth (BEFORE setting ODR's)
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW);   // accBW = 3 = 41Hz
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz
 // Set accel/gyro/mag desired ODR rates
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_QRateDivisor, QRtDiv); // quat rate = gyroRt/(1 QRTDiv)
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_MagRate, magRt); // 0x64 = 100 Hz
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AccelRate, accRt); // 200/10 Hz, 0x14 = 200 Hz
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_GyroRate, gyroRt); // 200/10 Hz, 0x14 = 200 Hz
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_BaroRate, 0x80 | baroRt);  // set enable bit and set Baro rate to 25 Hz, rate = baroRt/2, 0x32 = 25 Hz

  // Configure operating mode
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data
  // Enable interrupt to host upon certain events
  // choose host interrupts when any sensor updated (0x40), new gyro data (0x20), new accel data (0x10),
  // new mag data (0x08), quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01)
  _i2c_bus-> writeByte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07);
  // Enable EM7180 run mode
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode
  delay(100);

 // EM7180 parameter adjustments
  Serial.println("Beginning Parameter Adjustments");
  
  // Read sensor default FS values from parameter space
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
  byte param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  while(!(param_xfer==0x4A)) {
    param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  }
  param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);
  param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);
  param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);
  param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);
  EM7180_mag_fs = ((int16_t)(param[1]<<8) | param[0]);
  EM7180_acc_fs = ((int16_t)(param[3]<<8) | param[2]);
  Serial.print("Magnetometer Default Full Scale Range: +/-"); Serial.print(EM7180_mag_fs); Serial.println("uT");
  Serial.print("Accelerometer Default Full Scale Range: +/-"); Serial.print(EM7180_acc_fs); Serial.println("g");
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read  parameter 75
  param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  while(!(param_xfer==0x4B)) {
    param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  }
  param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);
  param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);
  param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);
  param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);
  EM7180_gyro_fs = ((int16_t)(param[1]<<8) | param[0]);
  Serial.print("Gyroscope Default Full Scale Range: +/-"); Serial.print(EM7180_gyro_fs); Serial.println("dps");
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm
  
  //Disable stillness mode for balancing robot application
  EM7180_set_integer_param (0x49, 0x00);
  
  //Write desired sensor full scale ranges to the EM7180
  EM7180_set_mag_acc_FS (magFS, accFS); // 1000 uT == 0x3E8, 8 g == 0x08
  EM7180_set_gyro_FS (gyroFS); // 2000 dps == 0x7D0
  
  // Read sensor new FS values from parameter space
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read  parameter 74
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process
  param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  while(!(param_xfer==0x4A)) {
    param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  }
  param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);
  param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);
  param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);
  param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);
  EM7180_mag_fs = ((int16_t)(param[1]<<8) | param[0]);
  EM7180_acc_fs = ((int16_t)(param[3]<<8) | param[2]);
  Serial.print("Magnetometer New Full Scale Range: +/-"); Serial.print(EM7180_mag_fs); Serial.println("uT");
  Serial.print("Accelerometer New Full Scale Range: +/-"); Serial.print(EM7180_acc_fs); Serial.println("g");
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read  parameter 75
  param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  while(!(param_xfer==0x4B)) {
    param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  }
  param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);
  param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);
  param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);
  param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);
  EM7180_gyro_fs = ((int16_t)(param[1]<<8) | param[0]);
  Serial.print("Gyroscope New Full Scale Range: +/-"); Serial.print(EM7180_gyro_fs); Serial.println("dps");
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm

  
// Read EM7180 status
uint8_t runStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RunStatus);
if(runStatus & 0x01) Serial.println(" EM7180 run status = normal mode");
uint8_t algoStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
if(algoStatus & 0x01) Serial.println(" EM7180 standby status");
if(algoStatus & 0x02) Serial.println(" EM7180 algorithm slow");
if(algoStatus & 0x04) Serial.println(" EM7180 in stillness mode");
if(algoStatus & 0x08) Serial.println(" EM7180 mag calibration completed");
if(algoStatus & 0x10) Serial.println(" EM7180 magnetic anomaly detected");
if(algoStatus & 0x20) Serial.println(" EM7180 unreliable sensor data");
uint8_t passthruStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_PassThruStatus);
if(passthruStatus & 0x01) Serial.print(" EM7180 in passthru mode!");
uint8_t eventStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_EventStatus);
if(eventStatus & 0x01) Serial.println(" EM7180 CPU reset");
if(eventStatus & 0x02) Serial.println(" EM7180 Error");
if(eventStatus & 0x04) Serial.println(" EM7180 new quaternion result");
if(eventStatus & 0x08) Serial.println(" EM7180 new mag result");
if(eventStatus & 0x10) Serial.println(" EM7180 new accel result");
if(eventStatus & 0x20) Serial.println(" EM7180 new gyro result"); 
  
  delay(1000); // give some time to read the screen
  
  // Check sensor status
  uint8_t sensorStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SensorStatus);
  Serial.print(" EM7180 sensor status = "); Serial.println(sensorStatus);
  if(sensorStatus & 0x01) Serial.print("Magnetometer not acknowledging!");
  if(sensorStatus & 0x02) Serial.print("Accelerometer not acknowledging!");
  if(sensorStatus & 0x04) Serial.print("Gyro not acknowledging!");
  if(sensorStatus & 0x10) Serial.print("Magnetometer ID not recognized!");
  if(sensorStatus & 0x20) Serial.print("Accelerometer ID not recognized!");
  if(sensorStatus & 0x40) Serial.print("Gyro ID not recognized!");
  
  Serial.print("Actual MagRate = "); Serial.print(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualMagRate)); Serial.println(" Hz"); 
  Serial.print("Actual AccelRate = "); Serial.print(10*(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualAccelRate))); Serial.println(" Hz"); 
  Serial.print("Actual GyroRate = "); Serial.print(10*(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualGyroRate))); Serial.println(" Hz"); 
  Serial.print("Actual BaroRate = "); Serial.print(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualBaroRate)); Serial.println(" Hz"); 
}

float USFS::uint32_reg_to_float (uint8_t *buf)
{
  union {
    uint32_t ui32;
    float f;
  } u;

  u.ui32 =     (((uint32_t)buf[0]) +
               (((uint32_t)buf[1]) <<  8) +
               (((uint32_t)buf[2]) << 16) +
               (((uint32_t)buf[3]) << 24));
  return u.f;
}

float USFS::int32_reg_to_float (uint8_t *buf)
{
  union {
    int32_t i32;
    float f;
  } u;

  u.i32 =      (((int32_t)buf[0]) +
               (((int32_t)buf[1]) <<  8) +
               (((int32_t)buf[2]) << 16) +
               (((int32_t)buf[3]) << 24));
  return u.f;
}

void USFS::float_to_bytes (float param_val, uint8_t *buf) {
  union {
    float f;
    uint8_t comp[sizeof(float)];
  } u;
  u.f = param_val;
  for (uint8_t i=0; i < sizeof(float); i++) {
    buf[i] = u.comp[i];
  }
  //Convert to LITTLE ENDIAN
  for (uint8_t i=0; i < sizeof(float); i++) {
    buf[i] = buf[(sizeof(float)-1) - i];
  }
}

void USFS::EM7180_set_gyro_FS (uint16_t gyro_fs) {
  uint8_t bytes[4], STAT;
  bytes[0] = gyro_fs & (0xFF);
  bytes[1] = (gyro_fs >> 8) & (0xFF);
  bytes[2] = 0x00;
  bytes[3] = 0x00;
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Gyro LSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Gyro MSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Unused
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Unused
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCB); //Parameter 75; 0xCB is 75 decimal with the MSB set high to indicate a paramter write processs
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
  while(!(STAT==0xCB)) {
  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  }
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}

void USFS::EM7180_set_mag_acc_FS (uint16_t mag_fs, uint16_t acc_fs) {
  uint8_t bytes[4], STAT;
  bytes[0] = mag_fs & (0xFF);
  bytes[1] = (mag_fs >> 8) & (0xFF);
  bytes[2] = acc_fs & (0xFF);
  bytes[3] = (acc_fs >> 8) & (0xFF);
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Mag LSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Mag MSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Acc LSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Acc MSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0xCA); //Parameter 74; 0xCA is 74 decimal with the MSB set high to indicate a paramter write processs
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
  while(!(STAT==0xCA)) {
    STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  }
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}

void USFS::EM7180_set_integer_param (uint8_t param, uint32_t param_val) {
  uint8_t bytes[4], STAT;
  bytes[0] = param_val & (0xFF);
  bytes[1] = (param_val >> 8) & (0xFF);
  bytes[2] = (param_val >> 16) & (0xFF);
  bytes[3] = (param_val >> 24) & (0xFF);
  param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, param);
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
  while(!(STAT==param)) {
    STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  }
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}

void USFS::EM7180_set_float_param (uint8_t param, float param_val) {
  uint8_t bytes[4], STAT;
  float_to_bytes (param_val, &bytes[0]);
  param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, param);
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure
  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte
  while(!(STAT==param)) {
    STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);
  }
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm
}


void USFS::readSENtralQuatData(float * destination)
{
  uint8_t rawData[16];  // x/y/z quaternion register data stored here
  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_QX, 16, &rawData[0]); // Read the sixteen raw data registers into data array
  destination[1] = uint32_reg_to_float (&rawData[0]);
  destination[2] = uint32_reg_to_float (&rawData[4]);
  destination[3] = uint32_reg_to_float (&rawData[8]);
  destination[0] = uint32_reg_to_float (&rawData[12]);   // SENtral stores quats as qx, qy, qz, q0!

}

void USFS::readSENtralAccelData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z accel register data stored here
  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_AX, 6, &rawData[0]);       // Read the six raw data registers into data array
  destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);  
  destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]); 
}

void USFS::readSENtralGyroData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z gyro register data stored here
  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_GX, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
  destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);   // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);  
  destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]); 
}

void USFS::readSENtralMagData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z gyro register data stored here
  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_MX, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
  destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);   // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);  
  destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]); 
}

int16_t USFS::readSENtralBaroData()
{
  uint8_t rawData[2];  // x/y/z gyro register data stored here
  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_Baro, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array
  return  (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);   // Turn the MSB and LSB into a signed 16-bit value
}

int16_t USFS::readSENtralTempData()
{
  uint8_t rawData[2];  // x/y/z gyro register data stored here
  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_Temp, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array
  return  (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);   // Turn the MSB and LSB into a signed 16-bit value
}


void USFS::SENtralPassThroughMode()
{
  // First put SENtral in standby mode
  uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_AlgorithmControl);
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, c | 0x01);
//  c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);
//  Serial.print("c = "); Serial.println(c);
// Verify standby status
// if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) {
   Serial.println("SENtral in standby mode"); 
  // Place SENtral in pass-through mode
  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01); 
  if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01) {
    Serial.println("SENtral in pass-through mode");
  }
  else {
    Serial.println("ERROR! SENtral not in pass-through mode!");
  }
 }
 
 

// I2C communication with the M24512DFM EEPROM is a little different from I2C communication with the usual motion sensor
// since the address is defined by two bytes

       void USFS::M24512DFMwriteByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t  data)
       {
       Wire.beginTransmission(device_address);   // Initialize the Tx buffer
       Wire.write(data_address1);                // Put slave register address in Tx buffer
       Wire.write(data_address2);                // Put slave register address in Tx buffer
       Wire.write(data);                         // Put data in Tx buffer
       Wire.endTransmission();                   // Send the Tx buffer
       }


       void USFS::M24512DFMwriteBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest)
       {
       if(count > 128) 
       {
        count = 128;
        Serial.print("Page count cannot be more than 128 bytes!");
        }       
        Wire.beginTransmission(device_address);   // Initialize the Tx buffer
        Wire.write(data_address1);                // Put slave register address in Tx buffer
        Wire.write(data_address2);                // Put slave register address in Tx buffer
        for(uint8_t i=0; i < count; i++) {
           Wire.write(dest[i]);                      // Put data in Tx buffer
           }
        Wire.endTransmission();                   // Send the Tx buffer
        }


        uint8_t USFS::M24512DFMreadByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2)
        {
        uint8_t data; // `data` will store the register data   
        Wire.beginTransmission(device_address);         // Initialize the Tx buffer
        Wire.write(data_address1);                // Put slave register address in Tx buffer
        Wire.write(data_address2);                // Put slave register address in Tx buffer
        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
        Wire.requestFrom(device_address, 1);  // Read one byte from slave register address 
        data = Wire.read();                      // Fill Rx buffer with result
        return data;                             // Return data read from slave register
        }

        void USFS::M24512DFMreadBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest)
        {  
        uint8_t temp[2] = {data_address1, data_address2};
        Wire.beginTransmission(device_address);   // Initialize the Tx buffer
        Wire.write(data_address1);                     // Put slave register address in Tx buffer
        Wire.write(data_address2);                     // Put slave register address in Tx buffer
        Wire.endTransmission(false);              // Send the Tx buffer, but send a restart to keep connection alive
        uint8_t i = 0;
        Wire.requestFrom(device_address, count);       // Read bytes from slave register address 
        while (Wire.available()) {
        dest[i++] = Wire.read(); }                // Put read results in the Rx buffer
        }






  // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
void USFS::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
        {
            float q1 = _q[0], q2 = _q[1], q3 = _q[2], q4 = _q[3];   // short name local variable for readability
            float norm;
            float hx, hy, _2bx, _2bz;
            float s1, s2, s3, s4;
            float qDot1, qDot2, qDot3, qDot4;

            // Auxiliary variables to avoid repeated arithmetic
            float _2q1mx;
            float _2q1my;
            float _2q1mz;
            float _2q2mx;
            float _4bx;
            float _4bz;
            float _2q1 = 2.0f * q1;
            float _2q2 = 2.0f * q2;
            float _2q3 = 2.0f * q3;
            float _2q4 = 2.0f * q4;
            float _2q1q3 = 2.0f * q1 * q3;
            float _2q3q4 = 2.0f * q3 * q4;
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = sqrt(ax * ax + ay * ay + az * az);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f/norm;
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrt(mx * mx + my * my + mz * mz);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f/norm;
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            _2q1mx = 2.0f * q1 * mx;
            _2q1my = 2.0f * q1 * my;
            _2q1mz = 2.0f * q1 * mz;
            _2q2mx = 2.0f * q2 * mx;
            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            _2bx = sqrt(hx * hx + hy * hy);
            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
            _4bx = 2.0f * _2bx;
            _4bz = 2.0f * _2bz;

            // Gradient decent algorithm corrective step
            s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
            norm = 1.0f/norm;
            s1 *= norm;
            s2 *= norm;
            s3 *= norm;
            s4 *= norm;

            // Compute rate of change of quaternion
            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - _beta * s1;
            qDot2 = 0.5f * ( q1 * gx + q3 * gz - q4 * gy) - _beta * s2;
            qDot3 = 0.5f * ( q1 * gy - q2 * gz + q4 * gx) - _beta * s3;
            qDot4 = 0.5f * ( q1 * gz + q2 * gy - q3 * gx) - _beta * s4;

            // Integrate to yield quaternion
            q1 += qDot1 * _deltat;
            q2 += qDot2 * _deltat;
            q3 += qDot3 * _deltat;
            q4 += qDot4 * _deltat;
            norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
            norm = 1.0f/norm;
            _q[0] = q1 * norm;
            _q[1] = q2 * norm;
            _q[2] = q3 * norm;
            _q[3] = q4 * norm;

        }
  
  
  
 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
 // measured ones. 
            void USFS::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
        {
            float q1 = _q[0], q2 = _q[1], q3 = _q[2], q4 = _q[3];   // short name local variable for readability
            float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method
            float norm;
            float hx, hy, bx, bz;
            float vx, vy, vz, wx, wy, wz;
            float ex, ey, ez;
            float pa, pb, pc;

            // Auxiliary variables to avoid repeated arithmetic
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;   

            // Normalise accelerometer measurement
            norm = sqrt(ax * ax + ay * ay + az * az);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f / norm;        // use reciprocal for division
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrt(mx * mx + my * my + mz * mz);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f / norm;        // use reciprocal for division
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
            hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
            bx = sqrt((hx * hx) + (hy * hy));
            bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);

            // Estimated direction of gravity and magnetic field
            vx = 2.0f * (q2q4 - q1q3);
            vy = 2.0f * (q1q2 + q3q4);
            vz = q1q1 - q2q2 - q3q3 + q4q4;
            wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
            wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
            wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  

            // Error is cross product between estimated direction and measured direction of gravity
            ex = (ay * vz - az * vy) + (my * wz - mz * wy);
            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
            if (_Ki > 0.0f)
            {
                eInt[0] += ex;      // accumulate integral error
                eInt[1] += ey;
                eInt[2] += ez;
            }
            else
            {
                eInt[0] = 0.0f;     // prevent integral wind up
                eInt[1] = 0.0f;
                eInt[2] = 0.0f;
            }

            // Apply feedback terms
            gx = gx + _Kp * ex + _Ki * eInt[0];
            gy = gy + _Kp * ey + _Ki * eInt[1];
            gz = gz + _Kp * ez + _Ki * eInt[2];

            // Integrate rate of change of quaternion
            pa = q2;
            pb = q3;
            pc = q4;
            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * _deltat);
            q2 = pa + (q1 * gx + pb * gz - pc * gy) *  (0.5f * _deltat);
            q3 = pb + (q1 * gy - pa * gz + pc * gx) *  (0.5f * _deltat);
            q4 = pc + (q1 * gz + pa * gy - pb * gx) *  (0.5f * _deltat);

            // Normalise quaternion
            norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
            norm = 1.0f / norm;
            _q[0] = q1 * norm;
            _q[1] = q2 * norm;
            _q[2] = q3 * norm;
            _q[3] = q4 * norm;
 
        }


================================================
FILE: EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/USFS.h
================================================
#ifndef USFS_h
#define USFSh_h

#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"   

// EM7180 SENtral register map
// see http://www.emdeveloper.com/downloads/7180/EMSentral_EM7180_Register_Map_v1_3.pdf
//
#define EM7180_QX                 0x00  // this is a 32-bit normalized floating point number read from registers 0x00-03
#define EM7180_QY                 0x04  // this is a 32-bit normalized floating point number read from registers 0x04-07
#define EM7180_QZ                 0x08  // this is a 32-bit normalized floating point number read from registers 0x08-0B
#define EM7180_QW                 0x0C  // this is a 32-bit normalized floating point number read from registers 0x0C-0F
#define EM7180_QTIME              0x10  // this is a 16-bit unsigned integer read from registers 0x10-11
#define EM7180_MX                 0x12  // int16_t from registers 0x12-13
#define EM7180_MY                 0x14  // int16_t from registers 0x14-15
#define EM7180_MZ                 0x16  // int16_t from registers 0x16-17
#define EM7180_MTIME              0x18  // uint16_t from registers 0x18-19
#define EM7180_AX                 0x1A  // int16_t from registers 0x1A-1B
#define EM7180_AY                 0x1C  // int16_t from registers 0x1C-1D
#define EM7180_AZ                 0x1E  // int16_t from registers 0x1E-1F
#define EM7180_ATIME              0x20  // uint16_t from registers 0x20-21
#define EM7180_GX                 0x22  // int16_t from registers 0x22-23
#define EM7180_GY                 0x24  // int16_t from registers 0x24-25
#define EM7180_GZ                 0x26  // int16_t from registers 0x26-27
#define EM7180_GTIME              0x28  // uint16_t from registers 0x28-29
#define EM7180_Baro               0x2A  // start of two-byte MS5637 pressure data, 16-bit signed interger
#define EM7180_BaroTIME           0x2C  // start of two-byte MS5637 pressure timestamp, 16-bit unsigned
#define EM7180_Temp               0x2E  // start of two-byte MS5637 temperature data, 16-bit signed interger
#define EM7180_TempTIME           0x30  // start of two-byte MS5637 temperature timestamp, 16-bit unsigned
#define EM7180_QRateDivisor       0x32  // uint8_t 
#define EM7180_EnableEvents       0x33
#define EM7180_HostControl        0x34
#define EM7180_EventStatus        0x35
#define EM7180_SensorStatus       0x36
#define EM7180_SentralStatus      0x37
#define EM7180_AlgorithmStatus    0x38
#define EM7180_FeatureFlags       0x39
#define EM7180_ParamAcknowledge   0x3A
#define EM7180_SavedParamByte0    0x3B
#define EM7180_SavedParamByte1    0x3C
#define EM7180_SavedParamByte2    0x3D
#define EM7180_SavedParamByte3    0x3E
#define EM7180_ActualMagRate      0x45
#define EM7180_ActualAccelRate    0x46
#define EM7180_ActualGyroRate     0x47
#define EM7180_ActualBaroRate     0x48
#define EM7180_ActualTempRate     0x49
#define EM7180_ErrorRegister      0x50
#define EM7180_AlgorithmControl   0x54
#define EM7180_MagRate            0x55
#define EM7180_AccelRate          0x56
#define EM7180_GyroRate           0x57
#define EM7180_BaroRate           0x58
#define EM7180_TempRate           0x59
#define EM7180_LoadParamByte0     0x60
#define EM7180_LoadParamByte1     0x61
#define EM7180_LoadParamByte2     0x62
#define EM7180_LoadParamByte3     0x63
#define EM7180_ParamRequest       0x64
#define EM7180_ROMVersion1        0x70
#define EM7180_ROMVersion2        0x71
#define EM7180_RAMVersion1        0x72
#define EM7180_RAMVersion2        0x73
#define EM7180_ProductID          0x90
#define EM7180_RevisionID         0x91
#define EM7180_RunStatus          0x92
#define EM7180_UploadAddress      0x94 // uint16_t registers 0x94 (MSB)-5(LSB)
#define EM7180_UploadData         0x96  
#define EM7180_CRCHost            0x97  // uint32_t from registers 0x97-9A
#define EM7180_ResetRequest       0x9B   
#define EM7180_PassThruStatus     0x9E   
#define EM7180_PassThruControl    0xA0
#define EM7180_ACC_LPF_BW         0x5B  //Register GP36
#define EM7180_GYRO_LPF_BW        0x5C  //Register GP37
#define EM7180_BARO_LPF_BW        0x5D  //Register GP38

#define EM7180_ADDRESS           0x28   // Address of the EM7180 SENtral sensor hub
#define M24512DFM_DATA_ADDRESS   0x50   // Address of the 500 page M24512DFM EEPROM data buffer, 1024 bits (128 8-bit bytes) per page
#define M24512DFM_IDPAGE_ADDRESS 0x58   // Address of the single M24512DFM lockable EEPROM ID page
#define MPU9250_ADDRESS          0x68   // Device address when ADO = 0
#define AK8963_ADDRESS           0x0C   //  Address of magnetometer
#define MS5637_ADDRESS           0x76   // Address of altimeter

class USFS
{
  public: 
  USFS(uint8_t intPin, bool passThru, I2Cdev* i2c_bus);
  float uint32_reg_to_float (uint8_t *buf);
  float int32_reg_to_float (uint8_t *buf);
  void float_to_bytes (float param_val, uint8_t *buf);
  void EM7180_set_gyro_FS (uint16_t gyro_fs);
  void EM7180_set_mag_acc_FS (uint16_t mag_fs, uint16_t acc_fs);
  void EM7180_set_integer_param (uint8_t param, uint32_t param_val);
  void EM7180_set_float_param (uint8_t param, float param_val);
  void readSENtralQuatData(float * destination);
  void readSENtralAccelData(int16_t * destination);
  void readSENtralGyroData(int16_t * destination);
  void readSENtralMagData(int16_t * destination);
  void initEM7180(uint8_t accBW, uint8_t gyroBW, uint16_t accFS, uint16_t gyroFS, uint16_t magFS, uint8_t QRtDiv, uint8_t magRt, uint8_t accRt, uint8_t gyroRt, uint8_t baroRt);
  int16_t readSENtralBaroData();
  int16_t readSENtralTempData();
  void SENtralPassThroughMode();
  void M24512DFMwriteByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t  data);
  void M24512DFMwriteBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest);
  uint8_t M24512DFMreadByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2);
  void M24512DFMreadBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest);
  void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
  void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
  void getChipID();
  void loadfwfromEEPROM();
  uint8_t checkEM7180Status();
  uint8_t checkEM7180Errors();
  private:
  uint8_t _intPin;
  bool _passThru;
  float _q[4];
  float _beta;
  float _deltat;
  float _Kp;
  float _Ki;
  I2Cdev* _i2c_bus;
};

#endif


================================================
FILE: EM7180/Readme.md
================================================

Sketch for the [Ultimate Sensor Fusion Solution](https://www.tindie.com/products/onehorse/ultimate-sensor-fusion-solution-lsm6dsm-lis2md/) running on an [ESP32 Development Board](https://www.tindie.com/products/onehorse/smallest-esp32-development-board/).


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/APDS9253.cpp
================================================
/******************************************************************************
 *
 * Copyright (c) 2021 Tlera Corporation  All rights reserved.
 *
 * This library is open-source and freely available for all to use with attribution.
 * 
 * All rights reserved.
 *****************************************************************************
 */
#include "APDS9253.h"
#include "I2Cdev.h"


APDS9253::APDS9253(I2Cdev* i2c_bus)
{
  _i2c_bus = i2c_bus;
}

// set bit 4 to 1 for software reset
void APDS9253::reset()
{
   uint8_t temp = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_MAIN_CTRL);
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, temp | 0x10); 
}

 
uint8_t APDS9253::getChipID()
{
   uint8_t temp = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_PART_ID);
   return temp;
}


void APDS9253::init(uint8_t RGBmode, uint8_t LS_res, uint8_t LS_rate, uint8_t LS_gain)
{
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, RGBmode << 2); 
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_MEAS_RATE, (LS_res << 4) | LS_rate); 
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_GAIN, LS_gain); 

   // configure the interrupt
   // enable interrupt on ALS (green) threshold
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_INT_CFG, 0x14); 
   // set persistence to 2 consecutive ALS values below threshold to trigger
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_INT_PST, 0x10); 

   // set lower threshold
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_THRES_LOW_0, 0x0A); // low threshold 10 counts
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_THRES_LOW_1, 0x00); 
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_THRES_LOW_2, 0x00); 
}


void APDS9253::enable()
{
   uint8_t temp = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_MAIN_CTRL);
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, temp & ~(0x02) ); // clear LS_EN bit
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, temp | 0x02 );    // set LS_EN bit
}


void APDS9253::disable()
{
   uint8_t temp = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_MAIN_CTRL);
   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, temp & ~(0x02) ); // clear LS_EN bit
}


void APDS9253::getRGBiRdata(uint32_t * destination)
{
  uint8_t rawData[3] = {0, 0, 0};
     _i2c_bus->readBytes(APDS9253_ADDR, APDS9253_LS_DATA_IR_0, 3, &rawData[0]);  
     destination[3] = (((uint32_t) (rawData[3] & 0x0F)) << 16)  |  (((uint32_t) rawData[1]) << 8)  | ((uint32_t) rawData[0]);    // ir
     _i2c_bus->readBytes(APDS9253_ADDR, APDS9253_LS_DATA_BLUE_0, 3, &rawData[0]);  
     destination[2] = (((uint32_t) (rawData[3] & 0x0F)) << 16)  |  (((uint32_t) rawData[1]) << 8)  | ((uint32_t) rawData[0]);    // blue
     _i2c_bus->readBytes(APDS9253_ADDR, APDS9253_LS_DATA_GREEN_0, 3, &rawData[0]);  
     destination[1] = (((uint32_t) (rawData[3] & 0x0F)) << 16)  |  (((uint32_t) rawData[1]) << 8)  | ((uint32_t) rawData[0]);    // green
     _i2c_bus->readBytes(APDS9253_ADDR, APDS9253_LS_DATA_RED_0, 3, &rawData[0]);  
     destination[0] = (((uint32_t) (rawData[3] & 0x0F)) << 16)  |  (((uint32_t) rawData[1]) << 8)  | ((uint32_t) rawData[0]);    // red
}


uint8_t APDS9253::getStatus()
{
    uint8_t Status = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_MAIN_STATUS);
    return Status;
}


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/APDS9253.h
================================================
/******************************************************************************
 *
 * Copyright (c) 2021 Tlera Corporation  All rights reserved.
 *
 * This library is open-source and freely available for all to use with attribution.
 * 
 * All rights reserved.
 *****************************************************************************
 */

#ifndef APDS9253_h
#define APDS9253_h

#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"

// Register Map
#define APDS9253_MAIN_CTRL       (0x00)
#define APDS9253_LS_MEAS_RATE    (0x04)
#define APDS9253_LS_GAIN         (0x05)
#define APDS9253_PART_ID         (0x06)
#define APDS9253_MAIN_STATUS     (0x07)
#define APDS9253_LS_DATA_IR_0    (0x0A)
#define APDS9253_LS_DATA_IR_1    (0x0B)
#define APDS9253_LS_DATA_IR_2    (0x0C)
#define APDS9253_LS_DATA_GREEN_0 (0x0D)
#define APDS9253_LS_DATA_GREEN_1 (0x0E)
#define APDS9253_LS_DATA_GREEN_2 (0x0F)
#define APDS9253_LS_DATA_BLUE_0  (0x10)
#define APDS9253_LS_DATA_BLUE_1  (0x11)
#define APDS9253_LS_DATA_BLUE_2  (0x12)
#define APDS9253_LS_DATA_RED_0   (0x13)
#define APDS9253_LS_DATA_RED_1   (0x14)
#define APDS9253_LS_DATA_RED_2   (0x15)
#define APDS9253_INT_CFG         (0x19)
#define APDS9253_INT_PST         (0x1A)
#define APDS9253_LS_THRES_UP_0   (0x21)
#define APDS9253_LS_THRES_UP_1   (0x22)
#define APDS9253_LS_THRES_UP_2   (0x23)
#define APDS9253_LS_THRES_LOW_0  (0x24)
#define APDS9253_LS_THRES_LOW_1  (0x25)
#define APDS9253_LS_THRES_LOW_2  (0x26)
#define APDS9253_LS_THRES_VAR    (0x27)
#define APDS9253_DK_CNT_STOR     (0x29)

#define APDS9253_ADDR      0x52  // I2C address

#define ALSandIR 0x00
#define RGBiR    0x01

//LS_res
#define res20bit 0x00 // 400 ms
#define res19bit 0x01 // 200 ms
#define res18bit 0x02 // 100 ms default
#define res17bit 0x03 // 50 ms
#define res16bit 0x04 // 25 ms
//#define res13bit 0x05 // 3.125 ms

//LS_rate
#define rate40Hz 0x00 // 25 ms
#define rate20Hz 0x01
#define rate10Hz 0x02 // 100 ms default
#define rate5Hz 0x03
#define rate2_5Hz 0x04
#define rate1Hz 0x05
#define rate0_5Hz 0x06 // 2000 ms

// LS_gain
#define gain1 0x00
#define gain3 0x01
#define gain6 0x02
#define gain9 0x03
#define gain18 0x04


/* general accel methods */
class APDS9253{
 public:
    APDS9253(I2Cdev* i2c_bus);
    void reset();
    uint8_t getStatus();
    uint8_t getChipID();
    void init(uint8_t RGBmode, uint8_t LS_res, uint8_t LS_rate, uint8_t LS_gain);
    void enable();
    void disable();
    void getRGBiRdata(uint32_t * destination);
 private:
    I2Cdev* _i2c_bus;
};
#endif


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/ESP32C3Mini_EnvSensor.v02b.ino
================================================
/* ESP32 C3 Mini Environmental Sensor
 
   This example code is in the public domain.
*/
#include "Arduino.h"
#include "LPS22HB.h"
#include "HDC2010.h"
#include "APDS9253.h"
#include "SPIFlash.h"
#include "WiFi.h"
#include "time.h"
#include <driver/adc.h> // for battery voltage reading

#define Serial USBSerial
#define SerialDebug true  // set to true to get Serial output for debugging, set to false to run on battery only

extern "C" void phy_bbpll_en_usb(bool en);

const char* ssid       = "YourNetworkName";
const char* password   = "YourNetworkPassword";

const char* ntpServer = "pool.ntp.org";
const long  gmtOffset_sec = -8*3600; // PST for California
const int   daylightOffset_sec = 3600;

struct tm timeinfo; // local time variables

/* create an ESP32 hardware timer */
hw_timer_t * timer = NULL;
volatile bool alarmFlag = false;

void IRAM_ATTR onTimer(){
  alarmFlag = true;
}

#define I2C_BUS    Wire              // Define the I2C bus (Wire instance) you wish to use

I2Cdev             i2c_0(&I2C_BUS);   // Instantiate the I2Cdev object and point to the desired I2C bus

// ESP32 C3 Mini pin configuration
const uint8_t myLed   =    2;  // blue led active LOW
// define battery voltage monitor pins
const uint8_t myBat   =    1;
const uint8_t myBatEn =   10;


// Battery voltage monitor definitions
float VBat = 0.0f;
uint32_t chipId = 0;
uint16_t ADCCounts = 0;

// Configure LPS22HB barometer
/* Specify sensor parameters (sample rate is twice the bandwidth) 
   Choices are P_1Hz, P_10Hz P_25 Hz, P_50Hz, and P_75Hz
 */
uint8_t PODR = P_1Hz;     // set pressure amd temperature output data rate
int32_t rawPressure = 0;
float LPSTemperature, LPSPressure, LPSAltitude;
uint8_t LPS22HBstatus, LPSintSource;

LPS22HB LPS22HB(&i2c_0);  // Instantiate LPS22HB barometer


// Configure HCD2010 humidity/temperature sensor
// Choices are:
// freq = ForceMode, Freq_120s, Freq_60s, Freq_10s, Freq_5s, Freq_1s, Freq_0_5s, Freq_0_2s
// tres = TRES_14bit, TRES_11bit, TRES_9bit
// hres = HRES_14bit, HRES_11bit, HRES_9bit
uint8_t freq = ForceMode, tres = TRES_14bit, hres = HRES_14bit;

float HDCTemperature = 0.0f, HDCHumidity = 0.0f;
uint16_t rawHDCTemperature = 0, rawHDCHumidity = 0;
uint8_t HDCStatus;


HDC2010 HDC2010(&i2c_0); // Instantiate HDC2010 Humidity sensor


// APDS9253 Configuration
uint8_t RGB_mode = RGBiR; // Choice is ALSandIR (green and IR channels only) or RGBiR for all four channels
//rate has to be slower than ADC settle time defines by resolution
// Bright sunlight is maximum ~25 klux so choose gain of 6x and minimum resolution (16 bits)
// that allows ~24 klux maximum to be measured;  a 1 Hz rate costs ~114 uA * 25/1000 ~ 3 uA
uint8_t LS_res = res16bit; // Choices are res20bit (400 ms), res19bit, res18bit (100 ms, default), res17bit, res16bit (25 ms).
uint8_t LS_rate = rate0_5Hz; // Choices are rate40Hz (25 ms), rate20Hz, rate10Hz (100 ms, default), rate5Hz, rate2_5Hz (400 ms), rate1Hz, rate0_5Hz
uint8_t LS_gain = gain6; // Choices are gain1, gain3 (default), gain6, gain9, gain18
uint32_t RGBiRData[4] = {0, 0, 0, 0}; // red, green, blue, ir counts
float ambientLight = 0; // ambient (green) light intensity in lux

float ALSluxTable[25]={         // lux per count for ALS depends on gain and resolution chosen
0.136, 0.273, 0.548, 1.099, 2.193,
0.045, 0.090, 0.180, 0.359, 0.722,
0.022, 0.045, 0.090, 0.179, 0.360,
0.015, 0.030, 0.059, 0.119, 0.239,
0.007, 0.015, 0.029, 0.059, 0.117
};

// Assume all channels have the same lux per LSB scaling
float luxScale = ALSluxTable[LS_gain * 5 + LS_res];

APDS9253 APDS9253(&i2c_0);  // Instantiate APDS9253 light sensor


// SPI flash configuration
const uint8_t CSPIN  = 4;
uint8_t flash_id[3] = {0, 0, 0};
uint16_t page_number = 0;     // set the page number for flash page write
uint8_t  sector_number = 0;   // set the sector number for sector write
uint8_t  flashPage[256];      // array to hold the data for flash page write

SPIFlash SPIFlash(CSPIN); // instantiate SPI flash class


void setup() 
{ 
  if(SerialDebug) phy_bbpll_en_usb(true); //this brings the USB serial-jtag back to life. 

  if(SerialDebug) Serial.begin(115200);
  if(SerialDebug) delay(4000);

  //connect to WiFi
  if(SerialDebug) Serial.printf("Connecting to %s ", ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      if(SerialDebug) Serial.print(".");
  }
  if(SerialDebug) Serial.println(" CONNECTED");
  
  //init and get the time
  configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
  getLocalTime(&timeinfo);
  if(SerialDebug)Serial.print("The current date/time in Danville, California is ");
  if(SerialDebug) Serial.println(&timeinfo, "%A, %B %d %Y %H:%M:%S");

  //disconnect WiFi as it's no longer needed
  WiFi.disconnect(true);
  WiFi.mode(WIFI_OFF);
  
  // Configure led pin
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, LOW);  // start with led on since active LOW

  // Check ESP32 ID
  for(int i=0; i<17; i=i+8) {
    chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xFF) << i;
  }

  if(SerialDebug) { 
  Serial.printf("ESP32 Chip model = %s Rev %d\n", ESP.getChipModel(), ESP.getChipRevision());
  Serial.printf("This chip has %d cores\n", ESP.getChipCores());
  Serial.print("Chip ID: "); Serial.println(chipId); Serial.println(" ");
  }

  // Configure ADC for battery voltage monitor
  adc1_config_width(ADC_WIDTH_BIT_12);
  adc1_config_channel_atten(ADC1_CHANNEL_1, ADC_ATTEN_DB_11); // ADC 1/Channel 1 is pin 1 -- myBat
  pinMode(myBat, INPUT);
  pinMode(myBatEn, OUTPUT);
  digitalWrite(myBatEn, HIGH);   
  ADCCounts = adc1_get_raw((adc1_channel_t)1);
  digitalWrite(myBatEn, LOW);   
  /* ADC at attenuation 11 should rad from 0 to 2.6 V nominally. The resistor divider
   *  is 1/2, and a calibration factor of 1.14 is applied to bring measurements into 
   *  agreement with multimeter */
  VBat = 2.0f * 2.60f * 1.14f * ((float) ADCCounts) / 4095.0f;
  if(SerialDebug) { 
    Serial.print("Battery voltage = "); Serial.print(VBat, 2); Serial.println(" V");
    Serial.print("Battery ADC Counts = "); Serial.print(ADCCounts); Serial.println(" Counts"); Serial.println(" ");
  }

   /* initialize wire bus */
  I2C_BUS.begin(0, 3);                // Set master mode, I2C_BUS.begin(SDA, SCL);
  I2C_BUS.setClock(400000);           // I2C frequency at 400 kHz
  delay(100);

  I2CscanDevices();                    // should detect all I2C devices on the bus

  if(SerialDebug) Serial.println("LPS22HB barometer...");
  uint8_t LPS22HB_chipID = LPS22HB.getChipID();
  if(SerialDebug) {
      Serial.print("LPS22HB "); Serial.print("I AM "); Serial.print(LPS22HB_chipID, HEX); Serial.print(" I should be "); Serial.println(0xB1, HEX);
      Serial.println(" ");
  }
  delay(100); 

  if(SerialDebug) Serial.println("HDC2010 humidity sensor...");
  uint16_t HDC2010_devID = HDC2010.getDevID(HDC2010_0_ADDRESS);
  if(SerialDebug) {
    Serial.print("DeviceID = 0x0"); Serial.print(HDC2010_devID, HEX); Serial.println(". Should be 0x07D0"); 
    Serial.println(" ");
  }

  uint16_t HDC2010_manuID = HDC2010.getManuID(HDC2010_0_ADDRESS);
  if(SerialDebug){ 
    Serial.print("Manufacturer's ID = 0x"); Serial.print(HDC2010_manuID, HEX); Serial.println(". Should be 0x5449");
    Serial.println(" ");
  }
  delay(100);

  // Read the APDS9253 Part ID register, this is a good test of communication
  if(SerialDebug)Serial.println("APDS9253 RGBiR Light Sensor...");
  byte APDS9253_ID = APDS9253.getChipID();  // Read PART_ID register for APDS9253
  if(SerialDebug) {
      Serial.print("APDS9253 "); Serial.print("chipID = 0x"); Serial.print(APDS9253_ID, HEX); Serial.print(", Should be 0x"); Serial.println(0xC2, HEX);
      Serial.println(" ");
  }
  delay(100);   

  if(LPS22HB_chipID == 0xB1 && HDC2010_devID == 0x07D0 && APDS9253_ID == 0xC2) // check if all I2C sensors with WHO_AM_I have acknowledged
  {
   if(SerialDebug) {Serial.println("LPS22HB, HDC2010, and APDS9253 are all online..."); Serial.println(" ");}

   LPS22HB.reset();
   LPS22HB.Init(PODR);  // Initialize LPS22HB barometer
   delay(100);
  
   HDC2010.reset(HDC2010_0_ADDRESS);

   // Configure HCD2010 for auto measurement mode if freq not ForceMode
   // else measurement performed only once each time init is called
   HDC2010.init(HDC2010_0_ADDRESS, hres, tres, freq); 

   if(SerialDebug) {Serial.print("Lux per count = "); Serial.println(luxScale, 3);}

   APDS9253.reset();
   delay(10);
   APDS9253.init(RGB_mode, LS_res, LS_rate, LS_gain);
   APDS9253.disable();

   digitalWrite(myLed, HIGH);  // when sensors successfully configured, turn off led
  }
   else 
  {
   if(LPS22HB_chipID != 0xB1 && SerialDebug)  Serial.println(" LPS22HB2 not functioning!");   
   if(HDC2010_devID != 0x07D0 && SerialDebug) Serial.println(" HDC20102 not functioning!");
   if(APDS9253_ID != 0xC2 && SerialDebug)     Serial.println(" APDS9253 not functioning!");
  }

  // configure SPI flash
  pinMode(CSPIN, OUTPUT);
  digitalWrite(CSPIN, HIGH);

 // check SPI Flash ID
  SPIFlash.init(6, 7, 5, 4);               // SPI.begin(SCK, MISO, MOSI, SS)
  SPIFlash.powerUp();                      // MX25R6435FZAI defaults to power down state
  
  SPIFlash.getChipID(flash_id);                    // Verify SPI flash communication
  if(flash_id[0] == 0xC2 && flash_id[1] == 0x28 && flash_id[2] == 0x17 && SerialDebug) {
  Serial.println(" ");  
  Serial.println("Found Macronix MX25R6435FZAI with Chip ID = 0xC2, 0x28, 0x17!");
  Serial.println(" ");  
  }
  else {
  if(SerialDebug) {
    Serial.println(" ");  
    Serial.println("no or unknown SPI flash!");
    Serial.println(" "); 
  } 
  }

  SPIFlash.powerDown();                    // power down SPI flash

  // Set up ESP32 timer
  /* Use 1st timer of 4 */
  /* 1 tick take 1/(80MHZ/80) = 1 us so we set divider 80 and count up */
  timer = timerBegin(0, 80, true);

  /* Attach onTimer function to our timer */
  timerAttachInterrupt(timer, &onTimer, false);

  /* Set alarm to call onTimer function every second 1 tick is 1us
  => 1 second is 1000000us */
  /* Repeat the alarm (third parameter) */
  timerAlarmWrite(timer, 300000000, true); // set time interval to five minute

  /* Start an alarm */
  timerAlarmEnable(timer);
  if(SerialDebug) Serial.println("start timer");
  
} // end of setup


void loop() 
{
  /*ESP32 timer*/
  if (alarmFlag) { // update RTC output at the alarm
      alarmFlag = false;

   /* APDS9253 Data Handling */
   APDS9253.enable(); // enable APDS9253 sensor
   while( !(APDS9253.getStatus() & 0x08) ) {}; // wait for data ready
   APDS9253.getRGBiRdata(RGBiRData); // read light sensor data
   APDS9253.disable(); // disable APDS9253 sensor
 
   if(SerialDebug) {
    Serial.print("Red raw counts = ");   Serial.println(RGBiRData[0]);
    Serial.print("Green raw counts = "); Serial.println(RGBiRData[1]);
    Serial.print("Blue raw counts = ");  Serial.println(RGBiRData[2]);
    Serial.print("IR raw counts = ");    Serial.println(RGBiRData[3]);
    Serial.println("  ");

    ambientLight = ((float) RGBiRData[1])*luxScale;
    Serial.print("Red intensity = ");   Serial.print(((float) RGBiRData[0])*luxScale); Serial.println(" lux");
    Serial.print("Green intensity = "); Serial.print(((float) RGBiRData[1])*luxScale); Serial.println(" lux");
    Serial.print("Blue intensity = ");  Serial.print(((float) RGBiRData[2])*luxScale); Serial.println(" lux");
    Serial.print("IR intensity = ");    Serial.print(((float) RGBiRData[3])*luxScale); Serial.println(" lux");
    Serial.println("  ");
   }
   
   
   /* HDC2010 data handling */
   HDC2010.forcedMode(HDC2010_0_ADDRESS);
   while( !(HDC2010.getIntStatus(HDC2010_0_ADDRESS) & 0x80) ) {}; // wait for HDC2010 data ready bit set
    
   rawHDCTemperature = HDC2010.getRawTemperature(HDC2010_0_ADDRESS); 
   HDCTemperature = ((float) rawHDCTemperature) * (165.0f/65536.0f) - 40.0f; // float degrees C, absolute accuracy +/- 0.2 C typical
   rawHDCHumidity    = HDC2010.getRawHumidity(HDC2010_0_ADDRESS);          
   HDCHumidity    = ((float) rawHDCHumidity) * (100.0f/65536.0f);   // float %rel humidity
 
   if(SerialDebug) {
     Serial.print("HDC2010 Temperature is "); Serial.print(HDCTemperature, 2); Serial.println(" degrees C");
     Serial.print("HDC2010 Humidity is "); Serial.print(HDCHumidity, 2); Serial.println(" %RH");
     Serial.println(" "); 
   }   /* end of HDC2010 data handling*/


   /* LPS22HB data handling */
//   LPS22HB.oneShot(); // baro data pretty jittery in one-shot mode, so run at 1 Hz instead...
   while( !(LPS22HB.status() & 0x03) ) {}; // wait for pressure and temperature data ready

   rawPressure = LPS22HB.readAltimeterPressure();
   LPSPressure = (float) rawPressure/4096.0f;
   LPSTemperature = (float) LPS22HB.readAltimeterTemperature()/100.0f; // absolute accuracy +/- 1.5 C
   LPSAltitude = 145366.45f*(1.0f - pow((LPSPressure/1013.25f), 0.190284f)); 

   if(SerialDebug) {
     Serial.print("Baro temperature = "); Serial.print(LPSTemperature, 2); Serial.print(" C"); // temperature in degrees Celsius  
     Serial.println(" ");
     Serial.print("Baro pressure = "); Serial.print(LPSPressure, 2);  Serial.print(" mbar");// pressure in millibar
     Serial.println(" ");     
     Serial.print("Altitude = "); Serial.print(LPSAltitude, 2); Serial.println(" ft");
     Serial.println(" ");
   }  /* end of LPS22HB interrupt handling */

     
   // get battery voltage
   digitalWrite(myBatEn, HIGH);   
   ADCCounts = adc1_get_raw((adc1_channel_t)1);
   digitalWrite(myBatEn, LOW);   
   /* ADC at attenuation 11 should rad from 0 to 2.6 V nominally. The resistor divider
    *  is 1/2, and a calibration factor of 1.14 is applied to bring measurements into 
    *  agreement with multimeter */
   VBat = 2.0f * 2.60f * 1.14f * ((float) ADCCounts) / 4095.0f;
   if(SerialDebug) { 
    Serial.print("Battery voltage = "); Serial.print(VBat, 2); Serial.println(" V");
    Serial.print("Battery ADC Counts = "); Serial.print(ADCCounts); Serial.println(" Counts"); Serial.println(" ");
   }


   getLocalTime(&timeinfo);
   if(SerialDebug) Serial.print("The current date/time in Danville, California is ");
   if(SerialDebug) Serial.println(&timeinfo, "%A, %B %d %Y %H:%M:%S"); Serial.println(" ");

   // Capture time variables for logging https://www.cplusplus.com/reference/ctime/strftime/
   char timeYear[3];
   strftime(timeYear,3, "%y", &timeinfo);
   uint8_t Year = (uint8_t)atoi(timeYear);
   char timeMonth[3];
   strftime(timeMonth,3, "%m", &timeinfo);
   uint8_t Month = (uint8_t)atoi(timeMonth);
   char timeDay[3];
   strftime(timeDay,3, "%d", &timeinfo);
   uint8_t Day = (uint8_t)atoi(timeDay);
   char timeHour[3];
   strftime(timeHour,3, "%H", &timeinfo);
   uint8_t Hour = (uint8_t)atoi(timeHour); 
   char timeMinute[3];
   strftime(timeMinute,3, "%M", &timeinfo);
   uint8_t Minute = (uint8_t)atoi(timeMinute); 
   char timeSecond[3];
   strftime(timeSecond,3, "%S", &timeinfo);
   uint8_t Second = (uint8_t)atoi(timeSecond);


    /* Log some data to the QSPI flash */
    // Highest page number is 0x7FFF = 32767 for 64 Mbit flash
    // store some data to the SPI flash
    uint8_t bps = 23; // bytes per sector such that 256 bytes per page= sectors per page x bps = 11 x 23 <= 256
      if(sector_number < 11 && page_number < 0x7FFF) {
      
      flashPage[sector_number*bps + 0]  = (rawHDCTemperature & 0xFF00) >> 8;   // raw HDC2010 Temperature
      flashPage[sector_number*bps + 1]  = (rawHDCTemperature & 0x00FF); 
      flashPage[sector_number*bps + 2]  = (rawHDCHumidity & 0xFF00) >> 8;      // raw HDC2010 Humidity
      flashPage[sector_number*bps + 3]  = (rawHDCHumidity & 0x00FF);
      
      flashPage[sector_number*bps + 4]  = (rawPressure & 0x00FF0000) >> 16;    // LPS22HB raw Pressure
      flashPage[sector_number*bps + 5]  = (rawPressure & 0x0000FF00) >> 8; 
      flashPage[sector_number*bps + 6]  = (rawPressure & 0x000000FF);
      
      flashPage[sector_number*bps + 7]  = (RGBiRData[0] & 0xFF00) >> 8;        // APDS9253 RGBiR data, 16 bit or less 
      flashPage[sector_number*bps + 8]  = (RGBiRData[0] & 0x00FF);
      flashPage[sector_number*bps + 9]  = (RGBiRData[1] & 0xFF00) >> 8;
      flashPage[sector_number*bps + 10] = (RGBiRData[1] & 0x00FF);
      flashPage[sector_number*bps + 11] = (RGBiRData[2] & 0xFF00) >> 8;
      flashPage[sector_number*bps + 12] = (RGBiRData[2] & 0x00FF);
      flashPage[sector_number*bps + 13] = (RGBiRData[3] & 0xFF00) >> 8;
      flashPage[sector_number*bps + 14] = (RGBiRData[3] & 0x00FF);
      
      flashPage[sector_number*bps + 15] = (ADCCounts & 0xFF00) >> 8;           // raw VBAT from ADC
      flashPage[sector_number*bps + 16] =  ADCCounts & 0x00FF;

      flashPage[sector_number*bps + 17] =  Second;                            // Time and date
      flashPage[sector_number*bps + 18] =  Minute;
      flashPage[sector_number*bps + 19] =  Hour;
      flashPage[sector_number*bps + 20] =  Day;
      flashPage[sector_number*bps + 21] =  Month;
      flashPage[sector_number*bps + 22] =  Year;

      sector_number++;
      }
       
      if(sector_number == 11 && page_number < 0x7FFF)
      {
          SPIFlash.powerUp();
          SPIFlash.flash_page_program(flashPage, page_number);
          if(SerialDebug) {Serial.print("***Wrote flash page: "); Serial.println(page_number);}
          digitalWrite(myLed, LOW); delay(1); digitalWrite(myLed, HIGH); // indicate when flash page is written
          sector_number = 0;
          page_number++;
          SPIFlash.powerDown();  // Put SPI flash into power down mode
      }  
      else if(page_number >= 0x7FFF) 
      {
       if(SerialDebug){Serial.println("Reached last page of SPI flash!"); Serial.println("Data logging stopped!");}
      }
  
     digitalWrite(myLed, LOW); delay(1); digitalWrite(myLed, HIGH);
   } // end of time alarm section

     delay(1);    // wait for alarm
     
} // end of main loop


/* Useful functions */

void alarmMatch()
{
  alarmFlag = true;
}


void  I2CscanDevices() 
{
  // Scan for i2c devices
  uint8_t error, address, nDevices;

  Serial.println("Scanning for I2C Devices ...");

  nDevices = 0;
  for(address = 1; address < 127; address++ ) 
  {
    // The i2c_scanner uses the return value of the Wire.endTransmisstion to see if a device did acknowledge to the address.
    error = i2c_0.pollAddress(address);

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
      Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");
      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
        Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("I2C scan complete\n");
}


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/HDC2010.cpp
================================================
/*
 * Copyright (c) 2021 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#include "HDC2010.h"

  HDC2010::HDC2010(I2Cdev* i2c_bus){
  _i2c_bus = i2c_bus;
  }


  void HDC2010::reset(uint8_t HDC2010_ADDRESS) {
  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_INT_STATUS, 0x80);  // soft reset device
  delay(1);
  }


  uint16_t HDC2010::getDevID(uint8_t HDC2010_ADDRESS)
  {
  uint8_t rawData[2] = {0, 0};
  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_DEV_ID_L); // read Dev ID LSByte
  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_DEV_ID_H); // read Dev ID HSByte
  uint16_t devID = ( (uint16_t) rawData[1] << 8) | rawData[0];
  return devID;
  }


  uint16_t HDC2010::getManuID(uint8_t HDC2010_ADDRESS)
  {
  uint8_t rawData[2] = {0, 0};
  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_MANU_ID_L); // read Dev ID LSByte
  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_MANU_ID_H); // read Dev ID HSByte
  uint16_t manuID = ( (uint16_t) rawData[1] << 8) | rawData[0];
  return manuID;
  }


  uint8_t HDC2010::getIntStatus(uint8_t HDC2010_ADDRESS)
  {
  uint8_t c  = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_INT_STATUS); // read int status register
  return c;
  }
  

  void HDC2010::init(uint8_t HDC2010_ADDRESS, uint8_t hres, uint8_t tres, uint8_t freq)
  {
  // set sample frequency (bits 6 - 4), enable interrupt (bit 2 = 1), active HIGH (bit 1 = 1)
  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_CONFIG1, freq << 4 | 0x04 | 0x02);  
  // set temperature resolution (bits 7:6), set humidity resolution (bits 5:4), measure both H and T
  // start measurements by writing 1 to bit 0
  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_CONFIG2, tres << 6 | hres << 4 | 0x01);  
  // enable data ready interrupt (bit 7), can enable interrupt on thresholds here too
  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_INT_EN, 0x80); 
  }


  float HDC2010::getTemperature(uint8_t HDC2010_ADDRESS)
  {
  uint8_t rawData[2] = {0, 0};
  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_TEMP_L); // read Temp LSByte
  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_TEMP_H); // read Temp HSByte
  uint16_t temp = (uint16_t) ( ((uint16_t) rawData[1] << 8 ) | rawData[0]);
  float out = ((float) temp) * (165.0f/65536.0f) - 40.0f;
  return out;
  }


  float HDC2010::getHumidity(uint8_t HDC2010_ADDRESS)
  {
  uint8_t rawData[2] = {0, 0};
  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_HUM_L); // read Temp LSByte
  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_HUM_H); // read Temp HSByte
  uint16_t temp = (uint16_t) ( ((uint16_t) rawData[1] << 8 ) | rawData[0]);
  float out = ((float) temp) * (100.0f/65536.0f);
  return out;
  }

  uint16_t HDC2010::getRawTemperature(uint8_t HDC2010_ADDRESS)
  {
  uint8_t rawData[2] = {0, 0};
  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_TEMP_L); // read Temp LSByte
  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_TEMP_H); // read Temp HSByte
  uint16_t temp = (uint16_t) ( ((uint16_t) rawData[1] << 8 ) | rawData[0]);
  return temp;
  }


  uint16_t HDC2010::getRawHumidity(uint8_t HDC2010_ADDRESS)
  {
  uint8_t rawData[2] = {0, 0};
  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_HUM_L); // read Temp LSByte
  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_HUM_H); // read Temp HSByte
  uint16_t temp = (uint16_t) ( ((uint16_t) rawData[1] << 8 ) | rawData[0]);
  return temp;
  }


  void HDC2010::forcedMode(uint8_t HDC2010_ADDRESS)
  {
  uint8_t temp = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_CONFIG2);
  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_CONFIG2, temp | 0x01);   //start measurement, self clearing when done
  }


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/HDC2010.h
================================================
/*
 * Copyright (c) 2021 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */
  
#ifndef HDC2010_h
#define HDC2010_h

#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"

// http://www.ti.com/lit/ds/symlink/hdc2010.pdf
// HDC2010 Registers
#define HDC2010_TEMP_L              0x00
#define HDC2010_TEMP_H              0x01 
#define HDC2010_HUM_L               0x02
#define HDC2010_HUM_H               0x03
#define HDC2010_INT_STATUS          0x04
#define HDC2010_TEMP_MAX            0x05
#define HDC2010_HUM_MAX             0x06
#define HDC2010_INT_EN              0x07
#define HDC2010_TEMP_OFFSET         0x08
#define HDC2010_HUM_OFFSET          0x09
#define HDC2010_TEMP_THR_L          0x0A
#define HDC2010_TEMP_THR_H          0x0B
#define HDC2010_RH_THR_L            0x0C
#define HDC2010_RH_THR_H            0x0D
#define HDC2010_CONFIG1             0x0E
#define HDC2010_CONFIG2             0x0F
#define HDC2010_MANU_ID_L           0xFC
#define HDC2010_MANU_ID_H           0xFD
#define HDC2010_DEV_ID_L            0xFE
#define HDC2010_DEV_ID_H            0xFF

// I2C address  
#define HDC2010_0_ADDRESS 0x40 // SA0 = 0
#define HDC2010_1_ADDRESS 0x41 // SA0 = 1

// Measurement Frequency (inverse sample rate)
#define ForceMode    0x00
#define Freq_120s    0x01
#define Freq_60s     0x02
#define Freq_10s     0x03
#define Freq_5s      0x04
#define Freq_1s      0x05 // 1 Hz
#define Freq_0_5s    0x06 // 2 Hz
#define Freq_0_2s    0x07 // 5 Hz

// Temperature resolution
#define TRES_14bit    0x00
#define TRES_11bit    0x01
#define TRES_9bit     0x02

// Humidity resolution
#define HRES_14bit    0x00
#define HRES_11bit    0x01
#define HRES_9bit     0x02
 


class HDC2010
{
  public: 
  HDC2010(I2Cdev* i2c_bus);
  void reset(uint8_t HDC2010_ADDRESS);
  uint16_t getDevID(uint8_t HDC2010_ADDRESS);
  uint16_t getManuID(uint8_t HDC2010_ADDRESS);
  void init(uint8_t HDC2010_ADDRESS, uint8_t hres, uint8_t tres, uint8_t freq);  
  uint8_t getIntStatus(uint8_t HDC2010_ADDRESS);
  void heaterOn(uint8_t HDC2010_ADDRESS);
  void heaterOff(uint8_t HDC2010_ADDRESS);
  float getTemperature(uint8_t HDC2010_ADDRESS);
  float getHumidity(uint8_t HDC2010_ADDRESS);
  uint16_t getRawTemperature(uint8_t HDC2010_ADDRESS);
  uint16_t getRawHumidity(uint8_t HDC2010_ADDRESS);
  void forcedMode(uint8_t HDC2010_ADDRESS);
  
  private:
  // Register read variables
  I2Cdev* _i2c_bus;
  };

#endif


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/I2Cdev.cpp
================================================
/*
 * Copyright (c) 2018 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#include "Arduino.h"
#include "I2Cdev.h"


I2Cdev::I2Cdev(TwoWire* i2c_bus)                                                                                                             // Class constructor
{
  _i2c_bus = i2c_bus;
}

I2Cdev::~I2Cdev()                                                                                                                            // Class destructor
{
}

/**
* @fn: readByte(uint8_t address, uint8_t subAddress)
*
* @brief: Read one byte from an I2C device
* 
* @params: I2C slave device address, Register subAddress
* @returns: unsigned short read
*/
uint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)
{
  uint8_t data = 0;                             // `data` will store the register data   
  _i2c_bus->beginTransmission(address);         // Initialize the Tx buffer
  _i2c_bus->write(subAddress);                  // Put slave register address in Tx buffer
  _i2c_bus->endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
  _i2c_bus->requestFrom(address, 1);            // Read one byte from slave register address  
  data = _i2c_bus->read();                      // Fill Rx buffer with result
  return data;                                  // Return data read from slave register
  
}


/**
* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
*
* @brief: Read multiple bytes from an I2C device
* 
* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data
* @returns: void
*/
void I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
  _i2c_bus->beginTransmission(address);   // Initialize the Tx buffer
  _i2c_bus->write(subAddress);            // Put slave register address in Tx buffer
  _i2c_bus->endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
  uint8_t i = 0;
  _i2c_bus->requestFrom(address, count);  // Read bytes from slave register address 
  while (_i2c_bus->available()) {
        dest[i++] = _i2c_bus->read(); }   // Put read results in the Rx buffer
}


/**
* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write one byte to an I2C device
* 
* @params: I2C slave device address, Register subAddress, data to be written
* @returns: void
*/
void I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
{
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  _i2c_bus->write(regAddr);           // Put slave register address in Tx buffer
  _i2c_bus->write(data);                 // Put data in Tx buffer
  _i2c_bus->endTransmission();           // Send the Tx buffer
}


/**
* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)
*
* @brief: Write multiple bytes to an I2C device
* 
* @params: I2C slave device address, Register subAddress, byte count, data array to be written
* @returns: void
*/
void I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)
{
  uint8_t temp[1 + count];
  
  temp[0] = regAddr;
  for (uint8_t ii = 0; ii < count; ii++)
  { 
    temp[ii + 1] = dest[ii];
  }
  
  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer
  
  for (uint8_t jj = 0; jj < count + 1; jj++)
  {
  _i2c_bus->write(temp[jj]);            // Put data in Tx buffer
  }
  
  _i2c_bus->endTransmission();           // Send the Tx buffer
}



uint8_t I2Cdev::pollAddress(uint8_t address)
{
    _i2c_bus->beginTransmission(address);
    uint8_t error = _i2c_bus->endTransmission();
    return error;
}


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/I2Cdev.h
================================================
/*
 * Copyright (c) 2018 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#ifndef _I2CDEV_H_
#define _I2CDEV_H_

#include <Wire.h>

class I2Cdev {
    public:
                                        I2Cdev(TwoWire*);
                                        ~I2Cdev();                                                                                                                     // Class destructor for durable instances
         uint8_t                        readByte(uint8_t address, uint8_t subAddress);
         void                           readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
         void                           writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
         void                           writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);
         uint8_t                        pollAddress(uint8_t address);
    private:
         TwoWire*                       _i2c_bus;                                                                                                                      // Class constructor argument
};

#endif //_I2CDEV_H_


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/LPS22HB.cpp
================================================
/*
 * Copyright (c) 2021 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#include "LPS22HB.h"
#include "I2Cdev.h"

LPS22HB::LPS22HB(I2Cdev* i2c_bus)
{
  _i2c_bus = i2c_bus;
}

uint8_t LPS22HB::getChipID()
{
  // Read the WHO_AM_I register of the altimeter this is a good test of communication
  uint8_t temp = _i2c_bus->readByte(LPS22HB_ADDRESS, LPS22HB_WHOAMI);  // Read WHO_AM_I register for LPS22HB
  return temp;
}


void LPS22HB::reset()
{
 _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_CTRL_REG2, 0x04);  // Reset LPS22HB
}


uint8_t LPS22HB::status()
{
  // Read the status register of the altimeter  
  uint8_t temp = _i2c_bus->readByte(LPS22HB_ADDRESS, LPS22HB_STATUS);   
  return temp;
}


uint8_t LPS22HB::intSource()
{
  // Read the status register of the altimeter  
  uint8_t temp = _i2c_bus->readByte(LPS22HB_ADDRESS, LPS22HB_INT_SOURCE);   
  return temp;
}


void LPS22HB::Init(uint8_t PODR)
{
  // Before device is powered up via CTRL_REG1 setting of PODR, 
  // set low current (~3 uA @ 1 Hz) or low noise mode (default) (~12 uA @ 1 Hz)
  /* uncomment next two lines for low current mode */
    uint8_t temp = _i2c_bus->readByte(LPS22HB_ADDRESS, LPS22HB_RES_CONF);   
   _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_RES_CONF, temp | 0x01);  // write 1 to bit 0 to enable low current mode
    
  // set sample rate by setting bits 6:4 
  // enable low-pass filter by setting bit 3 to one
  // bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20
  // make sure data not updated during read by setting block data update (bit 1) to 1
    _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_CTRL_REG1, PODR << 4 | 0x08 | 0x02); 
    _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_CTRL_REG2, 0x10); // enable auto increment

   // interrupt configuration
    _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_CTRL_REG3, 0x04);  // enable data ready interrupt
}    


int32_t LPS22HB::readAltimeterPressure()
{
    uint8_t rawData[3];  // 24-bit pressure register data stored here
    _i2c_bus->readBytes(LPS22HB_ADDRESS, (LPS22HB_PRESS_OUT_XL), 3, &rawData[0]);  
    return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8 | rawData[0]);
}


int16_t LPS22HB::readAltimeterTemperature()
{
    uint8_t rawData[2];  // 16-bit pressure register data stored here
    _i2c_bus->readBytes(LPS22HB_ADDRESS, (LPS22HB_TEMP_OUT_L), 2, &rawData[0]);  
    return (int16_t)((int16_t) rawData[1] << 8 | rawData[0]);
}


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/LPS22HB.h
================================================
/*
 * Copyright (c) 2021 Tlera Corp.  All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal with the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimers.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimers in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of Tlera Corp, nor the names of its contributors
 *     may be used to endorse or promote products derived from this Software
 *     without specific prior written permission.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
 * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * WITH THE SOFTWARE.
 */

#ifndef LPS22HB_h
#define LPS22HB_h

#include "Arduino.h"
#include <Wire.h>
#include "I2Cdev.h"

// See LPS22HB "MEMS pressure sensor: 260-1260 hPa absolute digital output barometer" Data Sheet
//file:///C:/Users/kris/Documents/Arduino/libraries/LPS22HB.pdf
#define LPS22HB_INTERRUPT_CFG 0x0B
#define LPS22HB_THS_P_L       0x0C
#define LPS22HB_THS_P_H       0x0D
#define LPS22HB_WHOAMI        0x0F // should return 0xB1
#define LPS22HB_CTRL_REG1     0x10
#define LPS22HB_CTRL_REG2     0x11
#define LPS22HB_CTRL_REG3     0x12
#define LPS22HB_FIFO_CTRL     0x14
#define LPS22HB_REF_P_XL      0x15
#define LPS22HB_REF_P_L       0x16
#define LPS22HB_REF_P_H       0x17
#define LPS22HB_RPDS_L        0x18
#define LPS22HB_RPDS_H        0x19
#define LPS22HB_RES_CONF      0x1A
#define LPS22HB_INT_SOURCE    0x25
#define LPS22HB_FIFO_STATUS   0x26
#define LPS22HB_STATUS        0x27
#define LPS22HB_PRESS_OUT_XL  0x28
#define LPS22HB_PRESS_OUT_L   0x29
#define LPS22HB_PRESS_OUT_H   0x2A
#define LPS22HB_TEMP_OUT_L    0x2B
#define LPS22HB_TEMP_OUT_H    0x2C
#define LPS22HB_LPFP_RES      0x33

#define LPS22HB_ADDRESS 0x5C   // Address of altimeter

// Altimeter output data rate
#define    P_1shot  0x00;
#define    P_1Hz    0x01;
#define    P_10Hz   0x02;
#define    P_25Hz   0x03;  // 25 Hz output data rate
#define    P_50Hz   0x04;
#define    P_75Hz   0x05;

class LPS22HB
{
  public: 
  LPS22HB(I2Cdev* i2c_bus);
  void Init(uint8_t PODR);
  uint8_t getChipID();
  uint8_t status();
  uint8_t intSource();
  void reset();
  int32_t readAltimeterPressure();
  int16_t readAltimeterTemperature();
  private:
  I2Cdev* _i2c_bus;
};

#endif


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/SPIFlash.cpp
================================================
/* SPIFlash.cpp
Sketch by Kris Winer December 16. 2016

License: Use this sketch any way you choose; if you like it, buy me a beer sometime

Purpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4
Dragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.

Sketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency
and maximum speed.

Sketch based on the work of Pete (El Supremo) as follows:
 * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca
 *
 * Development of this audio library was funded by PJRC.COM, LLC by sales of
 * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop
 * open source software by purchasing Teensy or other PJRC products.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 */

#include "SPIFlash.h"
#include "SPI.h"


SPIFlash::SPIFlash(uint8_t CSPIN)
{
  _csPin = CSPIN; 
}


void SPIFlash::init(const uint8_t SCK, const uint8_t MISO, const uint8_t MOSI, const uint8_t SS)
{
  SPI.begin(SCK, MISO, MOSI, SS);
  delay(20);
}


void SPIFlash::getChipID(uint8_t * dest)
{ 
//  uint16_t id[3];
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(0x9F);
  dest[0] = SPI.transfer(0);
  dest[1] = SPI.transfer(0);
  dest[2] = SPI.transfer(0);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}

void SPIFlash::powerDown()
{
  write_pause();
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_POWER_DOWN);
  digitalWrite(_csPin, HIGH);
  delayMicroseconds(10);
  SPI.endTransaction();
}


void SPIFlash::powerUp()
{
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  delayMicroseconds(1);
  SPI.transfer(CMD_RELEASE_POWER_DOWN);
  digitalWrite(_csPin, HIGH);
  delayMicroseconds(35);
  SPI.endTransaction();
}


void SPIFlash::write_pause(void)
{
  if(flash_wait_for_write) {
    while(flash_read_status() & STAT_WIP){
      delayMicroseconds(10);
    }
    flash_wait_for_write = 0;
  }
}


//=====================================
// convert a page number to a 24-bit address
int SPIFlash::page_to_address(int pn)
{
  return(pn << 8);
}

//=====================================
// convert a 24-bit address to a page number
int SPIFlash::address_to_page(int addr)
{
  return(addr >> 8);
}

//=====================================
void SPIFlash::flash_read_id(unsigned char *idt)
{
  write_pause();
  //set control register 
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_READ_ID);
  for(uint16_t i = 0; i < 20; i++) {
    *idt++ = SPI.transfer(0x00);
  }
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}

//=====================================
unsigned char SPIFlash::flash_read_status(void)
{
  unsigned char c;

// This can't do a write_pause
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);  
  SPI.transfer(CMD_READ_STATUS_REG);
  c = SPI.transfer(0x00);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  return(c);
}

//=====================================

void SPIFlash::flash_hard_reset(void)
{
  // Make sure that the device is not busy before
  // doing the hard reset sequence
  // At the moment this does NOT check the
  // SUSpend status bit in Status Register 2
  // but the library does not support suspend
  // mode yet anyway
  write_pause();
  
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_RESET_DEVICE );
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  delayMicroseconds(50);
  // Wait for the hard reset to finish
  // Don't use flash_wait_for_write here
  while(flash_read_status() & STAT_WIP);
  // The spec says "the device will take
  // approximately tRST=30 microseconds
  // to reset"
}

//=====================================
void SPIFlash::flash_chip_erase(boolean wait)
{
  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_CHIP_ERASE);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  flash_wait_for_write = 1;
  if(wait)write_pause();
}

//=====================================
// Tse Typ=0.6sec Max=3sec
// measured 549.024ms
// Erase the sector which contains the specified
// page number.
// The smallest unit of memory which can be erased
// is the 4kB sector (which is 16 pages)
void SPIFlash::flash_erase_pages_sector(int pn)
{
  int address;

  write_pause(); 
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_SECTOR_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xff);
  SPI.transfer((address >> 8) & 0xff);
  SPI.transfer(address & 0xff);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();  
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
// Erase the 32kb block which contains the specified
// page number.
void SPIFlash::flash_erase_pages_block32k(int pn)
{
  int address;

  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_BLOCK32K_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
// Erase the 64kb block which contains the specified
// page number.
void SPIFlash::flash_erase_pages_block64k(int pn)
{
  int address;
  
  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_BLOCK64K_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
void SPIFlash::flash_page_program(unsigned char *wp,int pn)
{
  int address;

  write_pause(); 
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_PAGE_PROGRAM);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // Now write 256 bytes to the page
  for(uint16_t i = 0; i < 256; i++) {
  SPI.transfer(*wp++);
  }
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
void SPIFlash::flash_read_pages(unsigned char *p,int pn,const int n_pages)
{
  int address;
  unsigned char *rp = p;
  
  write_pause();
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_READ_DATA);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // Now read the page's data bytes
  for(uint16_t i = 0; i < n_pages * 256; i++) {
    *rp++ = SPI.transfer(0);
  }
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}

//=====================================
// Read specified number of pages starting with pn
void SPIFlash::flash_fast_read_pages(unsigned char *p,int pn,const int n_pages)
{
  int address;
  unsigned char *rp = p;
  
  write_pause();
// The chip doesn't run at the higher clock speed until
// after the command and address have been sent
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_READ_HIGH_SPEED);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // send dummy byte
  SPI.transfer(0);
  // Now read the number of pages required
  for(uint16_t i = 0; i < n_pages * 256; i++) {
    *rp++ = SPI.transfer(0);
  }
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}


================================================
FILE: ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/SPIFlash.h
================================================
/* SPIFlash.h
Sketch by Kris Winer December 16. 2016

License: Use this sketch any way you choose; if you like it, buy me a beer sometime

Purpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4
Dragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.

Sketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency
and maximum speed.

Sketch based on the work of Pete (El Supremo) as follows:
 * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca
 *
 * Development of this audio library was funded by PJRC.COM, LLC by sales of
 * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop
 * open source software by purchasing Teensy or other PJRC products.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 */

#ifndef SPIFlash_h
#define SPIFlash_h

#include "Arduino.h"
#include "SPI.h"

#define STAT_WIP 1
#define STAT_WEL 2

#define CMD_WRITE_STATUS_REG   0x01
#define CMD_PAGE_PROGRAM       0x02
#define CMD_READ_DATA          0x03
#define CMD_WRITE_DISABLE      0x04 
#define CMD_READ_STATUS_REG    0x05
#define CMD_WRITE_ENABLE       0x06
#define CMD_READ_HIGH_SPEED    0x0B 
#define CMD_SECTOR_ERASE       0x20 
#define CMD_BLOCK32K_ERASE     0x52 
#define CMD_RESET_DEVICE       0xF0 
#define CMD_READ_ID            0x9F
#define CMD_RELEASE_POWER_DOWN 0xAB 
#define CMD_POWER_DOWN         0xB9 
#define CMD_CHIP_ERASE         0xC7
#define CMD_BLOCK64K_ERASE     0xD8 

#define clkSpeed 10000000  // 10 MHz


class SPIFlash
{
  public: 
  SPIFlash(uint8_t CSPIN);
  void init(const uint8_t SCK, const uint8_t MISO, const uint8_t MOSI, const uint8_t SS);
  void getChipID(uint8_t * dest);
  void powerDown();
  void powerUp();
  void write_pause(void);
  int page_to_address(int pn);
  int address_to_page(int addr);
  void flash_read_id(unsigned char *idt);
  unsigned char flash_read_status(void);
  void flash_hard_reset(void);
  void flash_chip_erase(boolean wait);
  void flash_erase_pages_sector(int pn);
  void flash_erase_pages_block32k(int pn);
  void flash_erase_pages_block64k(int pn);
  void flash_page_program(unsigned char *wp,int pn);
  void flash_read_pages(unsigned char *p,int pn,const int n_pages);
  void flash_fast_read_pages(unsigned char *p,int pn,const int n_pages);
  private:
  uint8_t   _csPin;
  unsigned char flash_wait_for_write = 0;

};

#endif


================================================
FILE: ESP32C3MiniEnvSensor/Readme.md
================================================
**ESP32C3Mini-hosted environmental data logger**

The idea is to use the remarkably [cheap](https://www.digikey.com/en/products/detail/espressif-systems/ESP32-C3-MINI-1-N4/13877574) ESP32C3Mini module (ESP32-C3-Mini-1-N4) as host for a small (25.6 x 20.5 mm) environmental data logger that can run for months on a small battery. 

The set of I2C sensors includes the APDS9253 RGBiR light sensor, HDC2010 humidity and temperature sensor, and LPS22HB barometer.

Rather than use the internal ~3 MByte SPIFFS, which uses a lot of power, I selected an external, ultra-low-power (100 nA in deep power down mode) 8 MByte MX25R6435FZAI SPI NOR flash memory to log the data. 

In addition to four-color lux, humidity, temperature, and pressure measurements, I added a resistor divider and dual FET to sample the battery voltage via an ESP32C3 ADC. The board also has user/boot and reset buttons, and an led for indication. 

The board main power rail is 3V3 from an MCP1812 LDO whose Iq is just 300 nA. I took pains to minimize the power usage which is dominated by the ESP32Mini module itself, being 5.6 uA in deep sleep mode. Everything else adds a small amount (~few uA, TBD) to this.

I am using Wifi to connect to the NTP server to initially sync the time peripheral then disconnecting Wifi. The idea is to log human readable time (sec, min, hour, day, month, year) along with the sensor data so over long periods of logging the sensor data can be correlated to real time. One could drop year and second to reduce the memory burden especially over short logging sessions since logging at duty cycles where sensor variations meaningfully reflect environmental changes, say every ten minutes, means the seconds are always the same. Short sessions means the year is unlikely to change.

The ESP32C3Mini cannot be woken from deep sleep by GPIO, i.e., by sensor interrupt, for example, which is a serious deficiency. So everything has to be based on timers and the only low power tool for the module is timed deep sleep. If the module could be woken by GPIO then recording the time (including seconds) of an event would make sense. For timed deep sleep only, minute resolution is probably good enough.

So far I am logging 23 bytes of data for each logging event. So I can log 11 events (253 bytes) before I write a 256-byte page to flash. This is efficient enough for me but YMMV.

I have two utility sketches in addition to the main logging sketch. SPIFlash tests the flash and ends up erasing it. This is useful for initial assembly tests as well as erasing the flash for the next logging session. The readSPIFlash sketch reads the  bytes stored on the flash during a data logging session and reconstructs the properly-scaled and formatted data and prints it to the serial monitor as comma-delimited lines (one data log per line) for subsequent plotting in a spreadsheet like Excel or OpenOffice (see below for an example).

So far I have the basic sketch working to configure the sensors and flash, and then peridically read sensor data, store it in an page array and then write a full page to external flash. In each case but the baro, the sensors/flash is kept in its lowest power state until needed. In the case of the sensors, this means once every five or ten minutes. For the flash, this means once every 55 or 110 minutes. I am running the baro continuously at 1 Hz since my attempts to use the forced mode have resulted in poor data quality from the sensor. I think it has to run a few cycles to settle so forced or one-shot mode for this sensor isn't a good option. I might replace it with the more accurate [ILPS22QS](https://github.com/kriswiner/ILPS22QS) baro which does work in one-shot mode in a subsequent redesign.

It took a while to get this all working properly because of some of the quirks of the ESP32C3Mini. One big issue was the USB serial (I am using the native USB not a USB-to-Serial transceiver). Turns out with WiFi this is automatically disconnected and a boolean flag has to be set to make sure this is turned on again. The other difficulty I had was selecting the SPI Flash clock speed. I settled on 10 MHz, which produced the most reliable results. It seemed to work at 20 and 40 MHz but in testing I had intermittent failures to record some or all of the data, and once the data was recorded but the time/date was mangled. The data never wrote to the flash at 80 MHz, the speed at which the internal flash usually operates. So the MX25R6435 external SPI flash or the ESP32C3 SPI peripheral or both might be a little flaky. Could also be pilot error. However I have been using the MX25R6435FZAI in STM32L0-hosted asset tracking applications running at 50 MHz SPI clock speed with no trouble for years. I will continue testing to assess logging reliability in real-world logging applications.

I don't expect perfection in a $2 module, but so far the ESP32C3Mini has worked well enough to keep me interested in continuing with the project.

Lastly, some data from an overnight logging test run:

![ESP32C3MiniEnvLogTest3](https://user-images.githubusercontent.com/6698410/166608157-96e9a205-15b8-46f6-a29f-296c916ab96c.jpg)

You can see the temperature spike from the initial Wifi time sync drop rapidly at the beginning. You can also see sunset at ~7:55 pm. The 105 mAH freshly-charged LiPo only lasted 7.3 hours before conking out, so an average current usage of 14 mA at 80 MHz clock speed and no deep sleep. Implementing the latter is the next task...

![ESP32C3Mini top](https://user-images.githubusercontent.com/6698410/166591280-3111662b-efe1-49bb-904c-abd950bf572f.jpg)
![ESP32C3Mini_bottom](https://user-images.githubusercontent.com/6698410/166591298-9c89f85a-87d2-4b78-b5d7-5e32c969c563.jpg)

Project pcb EAGLE CAD design files available in the OSH Park shared space [here.](https://oshpark.com/shared_projects/6YSyYfg9)


================================================
FILE: ESP32C3MiniEnvSensor/SPIFlash_ESP32C3Mini/SPIFlash_ESP32C3Mini.ino
================================================
/* SPIFlash_ESP32C3Mini SPIFlash Test
Sketch by Kris Winer December 16. 2016

License: Use this sketch any way you choose; if you like it, buy me a beer sometime

Purpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4
Dragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.

Sketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency
and maximum speed.

Sketch based on the work of Pete (El Supremo) as follows:
 * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca
 *
 * Development of this audio library was funded by PJRC.COM, LLC by sales of
 * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop
 * open source software by purchasing Teensy or other PJRC products.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 */

#include <SPI.h>

#define Serial USBSerial

// Highest page number is 0xFFFF = 65535 for 128 Mbit flash
// Highest page number is 0x0EFF =  4095 for   8 Mbit flash
int page_number = 0x0EFF;
unsigned char w_page[256];
unsigned char r_page[256];

static const int spiClk = 10000000; // 10 MHz, integer multiples of 80

#define CSPIN  4   

#define STAT_WIP 1
#define STAT_WEL 2

#define CMD_WRITE_STATUS_REG   0x01
#define CMD_PAGE_PROGRAM       0x02
#define CMD_READ_DATA          0x03
#define CMD_WRITE_DISABLE      0x04//not tested
#define CMD_READ_STATUS_REG    0x05
#define CMD_WRITE_ENABLE       0x06
#define CMD_READ_HIGH_SPEED    0x0B//not tested
#define CMD_SECTOR_ERASE       0x20//not tested
#define CMD_BLOCK32K_ERASE     0x52//not tested
#define CMD_RESET_DEVICE       0xF0//<<-different from winbond
#define CMD_READ_ID            0x9F
#define CMD_RELEASE_POWER_DOWN 0xAB//not tested
#define CMD_POWER_DOWN         0xB9//not tested
#define CMD_CHIP_ERASE         0xC7
#define CMD_BLOCK64K_ERASE     0xD8//not tested

unsigned char flash_wait_for_write = 0;

void setup(void)
{
  pinMode(CSPIN, OUTPUT);
  digitalWrite(CSPIN, HIGH);

  SPI.begin(6, 7, 5, 4); // sck, miso, mosi, ss (ss can be any GPIO)
  
  unsigned char id_tab[32];
  unsigned long t_start;
  
  Serial.begin(115200);
  delay(4000);
  Serial.println("Serial enabled!");

  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_RELEASE_POWER_DOWN);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  delay(100);

  Serial.print("ID bytes: ");
  uint16_t id[3];
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(0x9F);
  id[0] = SPI.transfer(0);
  id[1] = SPI.transfer(0);
  id[2] = SPI.transfer(0);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  Serial.print(id[0], HEX); Serial.print(" "); Serial.print(id[1], HEX);  Serial.print(" ");  Serial.println(id[2], HEX); 

  Serial.println("Winbond  W25Q80BLUX1G   Chip ID = 0xEF, 0x40, 0x14, 0x0");
  Serial.println("Macronix MX25L12835FZNI Chip ID = 0xC2, 0x20, 0x18, 0xC2");
  Serial.println("Spansion S25FL127S      Chip ID = 0x01, 0x20, 0x18, 0x4D");
  Serial.println(" ");
  
/* Initialize the array to 0,1,2,3 etc.*/
  for(uint16_t i = 0; i < 256; i++) {
    w_page[i] = i;
  }

  delay(100);
  
/* Write the page to page_number - this page MUST be in the erased state*/
  Serial.print("Write page:  0x"); Serial.println(page_number, HEX);  
  t_start = micros();
  flash_page_program(w_page, page_number);
  t_start = micros() - t_start;
  Serial.print("time (us) = "); Serial.println(t_start);

  delay(100);
  
/* Read back page_number and print its contents which should be 0,1,2,3...*/
  Serial.print("Read Page 0x"); Serial.println(page_number, HEX);
  t_start = micros();
  flash_read_pages(r_page, page_number,1);
  t_start = micros() - t_start;
  Serial.print("time (us) = "); Serial.println(t_start);

  delay(100);
  
  for(uint16_t i = 0; i < 256; i++) {
    Serial.print(" 0x"); Serial.print(r_page[i], HEX);
	if (i % 16==0) Serial.println();
  }
  Serial.println("");

  delay(100);
  
/* Erase the sector which includes page_number*/
  t_start = millis();
  flash_chip_erase(true);
  t_start = millis() - t_start;
  Serial.print("time (ms) = "); Serial.println(t_start);

  delay(100);
  
/* Now read back the page. It should now be all 255.*/
  Serial.print( "Read Page 0x"); Serial.println(page_number, HEX);
  t_start = micros();
  flash_read_pages(r_page, page_number,1);
  t_start = micros() - t_start;
  
  delay(100);
  
  Serial.print("time (us) = "); Serial.println(t_start);
  for(uint16_t i = 0; i < 256; i++) {
    Serial.print(" 0x"); Serial.print(r_page[i], HEX);
	if (i % 16==0) Serial.println();
  }
  Serial.println("");
}

void loop(void)
{
  yield();
}

/*********************************************************************************************/
// Useful functions
/*********************************************************************************************/
void write_pause(void)
{
  if(flash_wait_for_write) {
    while(flash_read_status() & STAT_WIP){
      delay(1);
    }
    flash_wait_for_write = 0;
  }
}

//=====================================
// convert a page number to a 24-bit address
int page_to_address(int pn)
{
  return(pn << 8);
}

//=====================================
// convert a 24-bit address to a page number
int address_to_page(int addr)
{
  return(addr >> 8);
}

//=====================================
void flash_read_id(unsigned char *idt)
{
  write_pause();
  //set control register 
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_READ_ID);
  for(uint16_t i = 0; i < 20; i++) {
    *idt++ = SPI.transfer(0x00);
  }
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
}

//=====================================
unsigned char flash_read_status(void)
{
  unsigned char c;

// This can't do a write_pause
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);  
  SPI.transfer(CMD_READ_STATUS_REG);
  c = SPI.transfer(0x00);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  return(c);
}

//=====================================

void flash_hard_reset(void)
{
  // Make sure that the device is not busy before
  // doing the hard reset sequence
  // At the moment this does NOT check the
  // SUSpend status bit in Status Register 2
  // but the library does not support suspend
  // mode yet anyway
  write_pause();
  
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_RESET_DEVICE );
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  delayMicroseconds(50);
  // Wait for the hard reset to finish
  // Don't use flash_wait_for_write here
  while(flash_read_status() & STAT_WIP);
  // The spec says "the device will take
  // approximately tRST=30 microseconds
  // to reset"
}

//=====================================
void flash_chip_erase(boolean wait)
{
  uint8_t status = 0x01;
  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
    
  while(status & 0x01) { // check that WEP bit is 0 before erasing
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  status = SPI.transfer(CMD_READ_STATUS_REG);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  }

  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_CHIP_ERASE);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
    
  flash_wait_for_write = 1;
  if(wait)write_pause();
}

//=====================================
// Tse Typ=0.6sec Max=3sec
// measured 549.024ms
// Erase the sector which contains the specified
// page number.
// The smallest unit of memory which can be erased
// is the 4kB sector (which is 16 pages)
void flash_erase_pages_sector(int pn)
{
  int address;

  write_pause(); 
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_SECTOR_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xff);
  SPI.transfer((address >> 8) & 0xff);
  SPI.transfer(address & 0xff);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();  
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
// Erase the 32kb block which contains the specified
// page number.
void flash_erase_pages_block32k(int pn)
{
  int address;

  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_BLOCK32K_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
// Erase the 64kb block which contains the specified
// page number.
void flash_erase_pages_block64k(int pn)
{
  int address;
  
  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_BLOCK64K_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
void flash_page_program(unsigned char *wp,int pn)
{
  int address;

  write_pause(); 
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_PAGE_PROGRAM);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // Now write 256 bytes to the page
  for(uint16_t i = 0; i < 256; i++) {
  SPI.transfer(*wp++);
  }
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
void flash_read_pages(unsigned char *p,int pn,const int n_pages)
{
  int address;
  unsigned char *rp = p;
  
  write_pause();
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_READ_DATA);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // Now read the page's data bytes
  for(uint16_t i = 0; i < n_pages * 256; i++) {
    *rp++ = SPI.transfer(0);
  }
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
}

//=====================================
// Read specified number of pages starting with pn
void flash_fast_read_pages(unsigned char *p,int pn,const int n_pages)
{
  int address;
  unsigned char *rp = p;
  
  write_pause();
// The chip doesn't run at the higher clock speed until
// after the command and address have been sent
  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_READ_HIGH_SPEED);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // send dummy byte
  SPI.transfer(0);
  // Now read the number of pages required
  for(uint16_t i = 0; i < n_pages * 256; i++) {
    *rp++ = SPI.transfer(0);
  }
  digitalWrite(CSPIN, HIGH);
  SPI.endTransaction();
}


================================================
FILE: ESP32C3MiniEnvSensor/readSPIFlash_ESP32C3Mini_EnvSensor.v02b/SPIFlash.cpp
================================================
/* SPIFlash.cpp
Sketch by Kris Winer December 16. 2016

License: Use this sketch any way you choose; if you like it, buy me a beer sometime

Purpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4
Dragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.

Sketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency
and maximum speed.

Sketch based on the work of Pete (El Supremo) as follows:
 * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca
 *
 * Development of this audio library was funded by PJRC.COM, LLC by sales of
 * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop
 * open source software by purchasing Teensy or other PJRC products.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 */

#include "SPIFlash.h"
#include "SPI.h"


SPIFlash::SPIFlash(uint8_t CSPIN)
{
  _csPin = CSPIN; 
}


void SPIFlash::init(const uint8_t SCK, const uint8_t MISO, const uint8_t MOSI, const uint8_t SS)
{
  SPI.begin(SCK, MISO, MOSI, SS);
  delay(20);
}


void SPIFlash::getChipID(uint8_t * dest)
{ 
//  uint16_t id[3];
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(0x9F);
  dest[0] = SPI.transfer(0);
  dest[1] = SPI.transfer(0);
  dest[2] = SPI.transfer(0);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}

void SPIFlash::powerDown()
{
  write_pause();
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_POWER_DOWN);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  delayMicroseconds(10);
}


void SPIFlash::powerUp()
{
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_RELEASE_POWER_DOWN);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}


void SPIFlash::write_pause(void)
{
  if(flash_wait_for_write) {
    while(flash_read_status() & STAT_WIP){
      delay(1);
    }
    flash_wait_for_write = 0;
  }
}


//=====================================
// convert a page number to a 24-bit address
int SPIFlash::page_to_address(int pn)
{
  return(pn << 8);
}

//=====================================
// convert a 24-bit address to a page number
int SPIFlash::address_to_page(int addr)
{
  return(addr >> 8);
}

//=====================================
void SPIFlash::flash_read_id(unsigned char *idt)
{
  write_pause();
  //set control register 
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_READ_ID);
  for(uint16_t i = 0; i < 20; i++) {
    *idt++ = SPI.transfer(0x00);
  }
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}

//=====================================
unsigned char SPIFlash::flash_read_status(void)
{
  unsigned char c;

// This can't do a write_pause
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);  
  SPI.transfer(CMD_READ_STATUS_REG);
  c = SPI.transfer(0x00);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  return(c);
}

//=====================================

void SPIFlash::flash_hard_reset(void)
{
  // Make sure that the device is not busy before
  // doing the hard reset sequence
  // At the moment this does NOT check the
  // SUSpend status bit in Status Register 2
  // but the library does not support suspend
  // mode yet anyway
  write_pause();
  
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_RESET_DEVICE );
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  delayMicroseconds(50);
  // Wait for the hard reset to finish
  // Don't use flash_wait_for_write here
  while(flash_read_status() & STAT_WIP);
  // The spec says "the device will take
  // approximately tRST=30 microseconds
  // to reset"
}

//=====================================
void SPIFlash::flash_chip_erase(boolean wait)
{
  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_CHIP_ERASE);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  flash_wait_for_write = 1;
  if(wait)write_pause();
}

//=====================================
// Tse Typ=0.6sec Max=3sec
// measured 549.024ms
// Erase the sector which contains the specified
// page number.
// The smallest unit of memory which can be erased
// is the 4kB sector (which is 16 pages)
void SPIFlash::flash_erase_pages_sector(int pn)
{
  int address;

  write_pause(); 
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_SECTOR_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xff);
  SPI.transfer((address >> 8) & 0xff);
  SPI.transfer(address & 0xff);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();  
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
// Erase the 32kb block which contains the specified
// page number.
void SPIFlash::flash_erase_pages_block32k(int pn)
{
  int address;

  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_BLOCK32K_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
// Erase the 64kb block which contains the specified
// page number.
void SPIFlash::flash_erase_pages_block64k(int pn)
{
  int address;
  
  write_pause();
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_BLOCK64K_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
void SPIFlash::flash_page_program(unsigned char *wp,int pn)
{
  int address;

  write_pause(); 
  // Send Write Enable command
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_PAGE_PROGRAM);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // Now write 256 bytes to the page
  for(uint16_t i = 0; i < 256; i++) {
  SPI.transfer(*wp++);
  }
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
void SPIFlash::flash_read_pages(unsigned char *p,int pn,const int n_pages)
{
  int address;
  unsigned char *rp = p;
  
  write_pause();
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_READ_DATA);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // Now read the page's data bytes
  for(uint16_t i = 0; i < n_pages * 256; i++) {
    *rp++ = SPI.transfer(0);
  }
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}

//=====================================
// Read specified number of pages starting with pn
void SPIFlash::flash_fast_read_pages(unsigned char *p,int pn,const int n_pages)
{
  int address;
  unsigned char *rp = p;
  
  write_pause();
// The chip doesn't run at the higher clock speed until
// after the command and address have been sent
  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));
  digitalWrite(_csPin, LOW);
  SPI.transfer(CMD_READ_HIGH_SPEED);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // send dummy byte
  SPI.transfer(0);
  // Now read the number of pages required
  for(uint16_t i = 0; i < n_pages * 256; i++) {
    *rp++ = SPI.transfer(0);
  }
  digitalWrite(_csPin, HIGH);
  SPI.endTransaction();
}


================================================
FILE: ESP32C3MiniEnvSensor/readSPIFlash_ESP32C3Mini_EnvSensor.v02b/SPIFlash.h
================================================
/* SPIFlash.h
Sketch by Kris Winer December 16. 2016

License: Use this sketch any way you choose; if you like it, buy me a beer sometime

Purpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4
Dragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.

Sketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency
and maximum speed.

Sketch based on the work of Pete (El Supremo) as follows:
 * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca
 *
 * Development of this audio library was funded by PJRC.COM, LLC by sales of
 * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop
 * open source software by purchasing Teensy or other PJRC products.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 */

#ifndef SPIFlash_h
#define SPIFlash_h

#include "Arduino.h"
#include "SPI.h"

#define STAT_WIP 1
#define STAT_WEL 2

#define CMD_WRITE_STATUS_REG   0x01
#define CMD_PAGE_PROGRAM       0x02
#define CMD_READ_DATA          0x03
#define CMD_WRITE_DISABLE      0x04 
#define CMD_READ_STATUS_REG    0x05
#define CMD_WRITE_ENABLE       0x06
#define CMD_READ_HIGH_SPEED    0x0B 
#define CMD_SECTOR_ERASE       0x20 
#define CMD_BLOCK32K_ERASE     0x52 
#define CMD_RESET_DEVICE       0xF0 
#define CMD_READ_ID            0x9F
#define CMD_RELEASE_POWER_DOWN 0xAB 
#define CMD_POWER_DOWN         0xB9 
#define CMD_CHIP_ERASE         0xC7
#define CMD_BLOCK64K_ERASE     0xD8 

#define clkSpeed 10000000  // 10 MHz

class SPIFlash
{
  public: 
  SPIFlash(uint8_t CSPIN);
  void init(const uint8_t SCK, const uint8_t MISO, const uint8_t MOSI, const uint8_t SS);
  void getChipID(uint8_t * dest);
  void powerDown();
  void powerUp();
  void write_pause(void);
  int page_to_address(int pn);
  int address_to_page(int addr);
  void flash_read_id(unsigned char *idt);
  unsigned char flash_read_status(void);
  void flash_hard_reset(void);
  void flash_chip_erase(boolean wait);
  void flash_erase_pages_sector(int pn);
  void flash_erase_pages_block32k(int pn);
  void flash_erase_pages_block64k(int pn);
  void flash_page_program(unsigned char *wp,int pn);
  void flash_read_pages(unsigned char *p,int pn,const int n_pages);
  void flash_fast_read_pages(unsigned char *p,int pn,const int n_pages);
  private:
  uint8_t   _csPin;
  unsigned char flash_wait_for_write = 0;

};

#endif


================================================
FILE: ESP32C3MiniEnvSensor/readSPIFlash_ESP32C3Mini_EnvSensor.v02b/readSPIFlash_ESP32C3Mini_EnvSensor.v02b.ino
================================================
/* readSPIFlash_ESP32C3Mini_EnvSensor.v02b
 *  
2021 Copyright Tlera Corporation 

April 30, 2022

Sketch to read the QSPI Flash on the ESP32C3Mini Environmental Sensor v.02b, reconstruct the sensor data, and output CMS-compatible data for plotting.
 */

#include "SPIFlash.h"
#include "SPI.h"

#define Serial USBSerial

float HDCTemperature = 0.0f, HDCHumidity = 0.0f;
uint16_t rawHDCTemperature = 0, rawHDCHumidity = 0;
int32_t rawPress = 0;
float pressure, altitude; // Scaled output of the LPS22HB
uint8_t Second, Minute, Hour, Day, Month, Year;
float VBat = 0.0f;
uint16_t ADCCounts = 0;

uint8_t LS_gain = 0x02;  
uint8_t LS_res  = 0x04;  
uint32_t RGBiRData[4] = {0, 0, 0, 0}; // red, green, blue, ir counts
float ambientLight = 0; // ambient (green) light intensity in lux

float ALSluxTable[25]={         // lux per count for ALS depends on gain and resolution chosen
0.136, 0.273, 0.548, 1.099, 2.193,
0.045, 0.090, 0.180, 0.359, 0.722,
0.022, 0.045, 0.090, 0.179, 0.360,
0.015, 0.030, 0.059, 0.119, 0.239,
0.007, 0.015, 0.029, 0.059, 0.117
};

// Assume all channels have the same lux per LSB scaling
float luxScale = ALSluxTable[LS_gain * 5 + LS_res];

// SPI flash configuration
//  Highest page number is 0x7FFF = 32767 for 64 Mbit flash
uint16_t max_page_number = 0x7FFF;
const uint8_t CSPIN  = 4;
uint8_t flash_id[3] = {0, 0, 0};
uint16_t page_number = 0;     // set the page number for flash page write
uint8_t  sector_number = 0;   // set the sector number for sector write
uint8_t  flashPage[256];      // array to hold the data for flash page write
uint8_t  bps = 23;            // bytes per sector such that 256 bytes per page= sectors per page x bps = 11 x 23

SPIFlash SPIFlash(CSPIN); // instantiate SPI flash class
 

void setup(void)
{ 
  Serial.begin(115200);
  while (!Serial) { }
  Serial.println("Serial enabled!");

  // configure SPI flash
  pinMode(CSPIN, OUTPUT);
  digitalWrite(CSPIN, HIGH);

 // check SPI Flash ID
  SPIFlash.init(6, 7, 5, 4);               // SPI.begin(SCK, MISO, MOSI, SS)
  SPIFlash.powerUp();                      // MX25R6435FZAI defaults to power down state
  
  SPIFlash.getChipID(flash_id);                    // Verify SPI flash communication
  if(flash_id[0] == 0xC2 && flash_id[1] == 0x28 && flash_id[2] == 0x17) {
  Serial.println(" ");  
  Serial.println("Found Macronix MX25R6435FZAI with Chip ID = 0xC2, 0x28, 0x17!");
  Serial.println(" ");  
  }
  else {
  Serial.println(" ");  
  Serial.println("no or unknown SPI flash!");
  Serial.println(" ");  
  }
  delay(4000); // give some time to read the screen
  
  // read the SPI flash
  for(page_number = 0; page_number < 10; page_number++)  { // change the page number limit to correspond to number of pages logged

//  Serial.print("Read Page 0x"); Serial.println(page_number, HEX);
   SPIFlash.flash_read_pages(flashPage, page_number, 1);
      
   for(sector_number = 0; sector_number < 11; sector_number++) {
    
    rawHDCTemperature =  ((uint16_t) flashPage[sector_number*bps + 0] << 8) | flashPage[sector_number*bps + 1];
    rawHDCHumidity =     ((uint16_t) flashPage[sector_number*bps + 2] << 8) | flashPage[sector_number*bps + 3];
    rawPress =           ((int32_t)  flashPage[sector_number*bps + 4] << 16) |  ((int32_t)flashPage[sector_number*bps + 5] << 8) | flashPage[sector_number*bps + 6];

    Second =  flashPage[sector_number*bps + 17];
    Minute =  flashPage[sector_number*bps + 18];
    Hour =    flashPage[sector_number*bps + 19];
    Day =     flashPage[sector_number*bps + 20];
    Month =   flashPage[sector_number*bps + 21];
    Year =    flashPage[sector_number*bps + 22];

    HDCTemperature = ((float) rawHDCTemperature) * (165.0f/65536.0f) - 40.0f; // float degrees C, absolute accuracy +/- 0.2 C typical
    HDCHumidity    = ((float) rawHDCHumidity) * (100.0f/65536.0f);   // float %rel humidity
      
    pressure = (float) rawPress/4096.0f; // Pressure in mbar
    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));   
   

    RGBiRData[0] = ((uint16_t) flashPage[sector_number*bps +  7] << 8) |  flashPage[sector_number*bps + 8];
    RGBiRData[1] = ((uint16_t) flashPage[sector_number*bps +  9] << 8) |  flashPage[sector_number*bps + 10];
    RGBiRData[2] = ((uint16_t) flashPage[sector_number*bps + 11] << 8) |  flashPage[sector_number*bps + 12];
    RGBiRData[3] = ((uint16_t) flashPage[sector_number*bps + 13] << 8) |  flashPage[sector_number*bps + 14];

    ADCCounts = ((uint16_t) flashPage[sector_number*bps + 15] << 8) |  flashPage[sector_number*bps + 16];
    VBat = 2.0f * 2.60f * 1.14f * ((float) ADCCounts) / 4095.0f;

    // Output for spreadsheet analysis
    if(Month < 10) {Serial.print("0"); Serial.print(Month);} else Serial.print(Month);
    Serial.print("/");Serial.print(Day); Serial.print("/");Serial.print(2000 + Year); Serial.print(" ");
    if(Hour < 10) {Serial.print("0"); Serial.print(Hour);} else Serial.print(Hour);
    Serial.print(":"); 
    if(Minute < 10) {Serial.print("0"); Serial.print(Minute);} else Serial.print(Minute); 
    Serial.print(":"); 
    if(Second < 10) {Serial.print("0"); Serial.print(Second);} else Serial.print(Second); Serial.print(",");      
    Serial.print(pressure, 2); Serial.print(","); Serial.print(HDCTemperature, 2); Serial.print(",");  
    Serial.print(HDCHumidity, 1); Serial.print(","); 
    Serial.print(((float)RGBiRData[0])*luxScale, 2); Serial.print(","); // R
    Serial.print(((float)RGBiRData[1])*luxScale, 2); Serial.print(","); // G
    Serial.print(((float)RGBiRData[2])*luxScale, 2); Serial.print(","); // B
    Serial.print(((float)RGBiRData[3])*luxScale, 2); Serial.print(","); // IR
    Serial.println(VBat, 2);  // Bat voltage  
    }
  
  }


}

void loop(void)
{
}


================================================
FILE: MPU9250_MS5637/MPU9250_MS5637_AHRS.ino
================================================
/* MPU9250_MS5637_ESP32 Basic Example Code
 by: Kris Winer
 date: December 14, 2016
 license: Beerware - Use this code however you'd like. If you 
 find it useful you can buy me a beer some time.
 
 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 
 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. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
 
 This sketch is intended specifically for the MPU9250+MS5637 Add-on shield.
 It uses SDA/SCL on pins 21/22, respectively, and it uses the Wire library.
 The MS5637 is a simple but high resolution 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.
 
 SDA and SCL should have external pull-up resistors (to 3.3V).
 4K7 resistors are on the MPU9250+MS5637 breakout board.
 
 Hardware setup:
 MPU9250 Breakout --------- ESP32
 VDD ---------------------- 3.3V
 SDA ----------------------- 21
 SCL ----------------------- 22
 GND ---------------------- GND
 
 */
#include "Wire.h"   

// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet
#define MS5637_RESET      0x1E
#define MS5637_CONVERT_D1 0x40
#define MS5637_CONVERT_D2 0x50
#define MS5637_ADC_READ   0x00

// See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in 
// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
//
//Magnetometer Registers
#define AK8963_ADDRESS   0x0C
#define WHO_AM_I_AK8963  0x00 // should return 0x48
#define INFO             0x01
#define AK8963_ST1       0x02  // data ready status bit 0
#define AK8963_XOUT_L	   0x03  // data
#define AK8963_XOUT_H	   0x04
#define AK8963_YOUT_L	   0x05
#define AK8963_YOUT_H	   0x06
#define AK8963_ZOUT_L	   0x07
#define AK8963_ZOUT_H	   0x08
#define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC      0x0C  // Self test control
#define AK8963_I2CDIS    0x0F  // I2C disable
#define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value

#define SELF_TEST_X_GYRO 0x00                  
#define SELF_TEST_Y_GYRO 0x01                                                                          
#define SELF_TEST_Z_GYRO 0x02

/*#define X_FINE_GAIN      0x03 // [7:0] fine gain
#define Y_FINE_GAIN      0x04
#define Z_FINE_GAIN      0x05
#define XA_OFFSET_H      0x06 // User-defined trim values for accelerometer
#define XA_OFFSET_L_TC   0x07
#define YA_OFFSET_H      0x08
#define YA_OFFSET_L_TC   0x09
#define ZA_OFFSET_H      0x0A
#define ZA_OFFSET_L_TC   0x0B */

#define SELF_TEST_X_ACCEL 0x0D
#define SELF_TEST_Y_ACCEL 0x0E    
#define SELF_TEST_Z_ACCEL 0x0F

#define SELF_TEST_A      0x10

#define XG_OFFSET_H      0x13  // User-defined trim values for gyroscope
#define XG_OFFSET_L      0x14
#define YG_OFFSET_H      0x15
#define YG_OFFSET_L      0x16
#define ZG_OFFSET_H      0x17
#define ZG_OFFSET_L      0x18
#define SMPLRT_DIV       0x19
#define CONFIG           0x1A
#define GYRO_CONFIG      0x1B
#define ACCEL_CONFIG     0x1C
#define ACCEL_CONFIG2    0x1D
#define LP_ACCEL_ODR     0x1E   
#define WOM_THR          0x1F   

#define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
#define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]
#define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms

#define FIFO_EN          0x23
#define I2C_MST_CTRL     0x24   
#define I2C_SLV0_ADDR    0x25
#define I2C_SLV0_REG     0x26
#define I2C_SLV0_CTRL    0x27
#define I2C_SLV1_ADDR    0x28
#define I2C_SLV1_REG     0x29
#define I2C_SLV1_CTRL    0x2A
#define I2C_SLV2_ADDR    0x2B
#define I2C_SLV2_REG     0x2C
#define I2C_SLV2_CTRL    0x2D
#define I2C_SLV3_ADDR    0x2E
#define I2C_SLV3_REG     0x2F
#define I2C_SLV3_CTRL    0x30
#define I2C_SLV4_ADDR    0x31
#define I2C_SLV4_REG     0x32
#define I2C_SLV4_DO      0x33
#define I2C_SLV4_CTRL    0x34
#define I2C_SLV4_DI      0x35
#define I2C_MST_STATUS   0x36
#define INT_PIN_CFG      0x37
#define INT_ENABLE       0x38
#define DMP_INT_STATUS   0x39  // Check DMP interrupt
#define INT_STATUS       0x3A
#define ACCEL_XOUT_H     0x3B
#define ACCEL_XOUT_L     0x3C
#define ACCEL_YOUT_H     0x3D
#define ACCEL_YOUT_L     0x3E
#define ACCEL_ZOUT_H     0x3F
#define ACCEL_ZOUT_L     0x40
#define TEMP_OUT_H       0x41
#define TEMP_OUT_L       0x42
#define GYRO_XOUT_H      0x43
#define GYRO_XOUT_L      0x44
#define GYRO_YOUT_H      0x45
#define GYRO_YOUT_L      0x46
#define GYRO_ZOUT_H      0x47
#define GYRO_ZOUT_L      0x48
#define EXT_SENS_DATA_00 0x49
#define EXT_SENS_DATA_01 0x4A
#define EXT_SENS_DATA_02 0x4B
#define EXT_SENS_DATA_03 0x4C
#define EXT_SENS_DATA_04 0x4D
#define EXT_SENS_DATA_05 0x4E
#define EXT_SENS_DATA_06 0x4F
#define EXT_SENS_DATA_07 0x50
#define EXT_SENS_DATA_08 0x51
#define EXT_SENS_DATA_09 0x52
#define EXT_SENS_DATA_10 0x53
#define EXT_SENS_DATA_11 0x54
#define EXT_SENS_DATA_12 0x55
#define EXT_SENS_DATA_13 0x56
#define EXT_SENS_DATA_14 0x57
#define EXT_SENS_DATA_15 0x58
#define EXT_SENS_DATA_16 0x59
#define EXT_SENS_DATA_17 0x5A
#define EXT_SENS_DATA_18 0x5B
#define EXT_SENS_DATA_19 0x5C
#define EXT_SENS_DATA_20 0x5D
#define EXT_SENS_DATA_21 0x5E
#define EXT_SENS_DATA_22 0x5F
#define EXT_SENS_DATA_23 0x60
#define MOT_DETECT_STATUS 0x61
#define I2C_SLV0_DO      0x63
#define I2C_SLV1_DO      0x64
#define I2C_SLV2_DO      0x65
#define I2C_SLV3_DO      0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET  0x68
#define MOT_DETECT_CTRL  0x69
#define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
#define PWR_MGMT_1       0x6B // Device defaults to the SLEEP mode
#define PWR_MGMT_2       0x6C
#define DMP_BANK         0x6D  // Activates a specific bank in the DMP
#define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank
#define DMP_REG          0x6F  // Register in DMP from which to read or to which to write
#define DMP_REG_1        0x70
#define DMP_REG_2        0x71 
#define FIFO_COUNTH      0x72
#define FIFO_COUNTL      0x73
#define FIFO_R_W         0x74
#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
#define XA_OFFSET_H      0x77
#define XA_OFFSET_L      0x78
#define YA_OFFSET_H      0x7A
#define YA_OFFSET_L      0x7B
#define ZA_OFFSET_H      0x7D
#define ZA_OFFSET_L      0x7E

// Using the MPU9250_MS5637 Add-On shield, ADO is set to 0 
// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
#define ADO 0
#if ADO
#define MPU9250_ADDRESS 0x69  // Device address when ADO = 1
#define AK8963_ADDRESS 0x0C   //  Address of magnetometer
#define MS5637_ADDRESS 0x76   // Address of altimeter
#else
#define MPU9250_ADDRESS 0x68  // Device address when ADO = 0
#define AK8963_ADDRESS 0x0C   //  Address of magnetometer
#define MS5637_ADDRESS 0x76   // Address of altimeter
#endif  

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

// Set initial input parameters
enum Ascale {
  AFS_2G = 0,
  AFS_4G,
  AFS_8G,
  AFS_16G
};

enum Gscale {
  GFS_250DPS = 0,
  GFS_500DPS,
  GFS_1000DPS,
  GFS_2000DPS
};

enum Mscale {
  MFS_14BITS = 0, // 0.6 mG per LSB
  MFS_16BITS      // 0.15 mG per LSB
};

#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 full scale
uint8_t OSR = ADC_8192;     // set pressure amd temperature oversample rate
uint8_t Gscale = GFS_250DPS;
uint8_t Ascale = AFS_2G;
uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution
uint8_t Mmode = 0x06;        // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
float aRes, gRes, mRes;      // scale resolutions per LSB for the sensors

// Pin definitions
int intPin = 14;  // can be any pin
bool newData = false;
bool newMagData = false;

int myLed = 5;

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, TT2, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data

int16_t MPU9250Data[7]; // used to read all 14 bytes at once from the MPU9250 accel/gyro
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
float magCalibration[3] = {0, 0, 0};  // Factory mag calibration and mag bias
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}, magScale[3]  = {0, 0, 0};      // Bias corrections for gyro and accelerometer
int16_t tempCount;            // temperature raw count output
float   temperature;          // Stores the MPU9250 gyro internal chip temperature in degrees Celsius
double Temperature, Pressure; // stores MS5637 pressures sensor pressure and temperature
float SelfTest[6];            // holds results of gyro and accelerometer self test

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
float pi = 3.141592653589793238462643383279502884f;
float GyroMeasError = PI * (4.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 a12, a22, a31, a32, a33;            // rotation matrix coefficients for Euler angles and gravity components
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 lin_ax, lin_ay, lin_az;             // linear acceleration (acceleration with gravity component subtracted)
float q[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()
{
  Serial.begin(115200);
  delay(4000);
  
  Wire.begin(21, 22, 400000); //(SDA, SCL) (21,22) are default on ESP32, 400 kHz I2C clock
  delay(1000);
  
  // Set up the interrupt pin, its set as active high, push-pull
  pinMode(intPin, INPUT);
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, LOW);
  
  I2Cscan();// look for I2C devices on the bus
    
  // Read the WHO_AM_I register, this is a good test of communication
  Serial.println("MPU9250 9-axis motion sensor...");
  uint8_t c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
  Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX);

  delay(1000); 

  if (c == 0x71) // WHO_AM_I should always be 0x71
  {  
    Serial.println("MPU9250 is online...");
    
    MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
    Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value");
    Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value");
    Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value");
    Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value");
    Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value");
    Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value");
    delay(1000);
    
   // get sensor resolutions, only need to do this once
   getAres();
   getGres();
   getMres();
    
   Serial.println(" Calibrate gyro and accel");
   accelgyrocalMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
   Serial.println("accel biases (mg)"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); Serial.println(1000.*accelBias[2]);
   Serial.println("gyro biases (dps)"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);

  delay(1000);  
   
  initMPU9250(); 
  Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
  
  // Read the WHO_AM_I register of the magnetometer, this is a good test of communication
  byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);  // Read WHO_AM_I register for AK8963
  Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX);
 
  delay(1000); 
  
  // Get magnetometer calibration from AK8963 ROM
  initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer
  
  magcalMPU9250(magBias, magScale);
  Serial.println("AK8963 mag biases (mG)"); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]); 
  Serial.println("AK8963 mag scale (mG)"); Serial.println(magScale[0]); Serial.println(magScale[1]); Serial.println(magScale[2]); 
  delay(2000); // add delay to see results before serial spew of data
   
  if(SerialDebug) {
//  Serial.println("Calibration values: ");
  Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2);
  Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2);
  Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2);
  }

  delay(1000);  
  
  // Reset the MS5637 pressure sensor
  MS5637Reset();
  delay(100);
  Serial.println("MS5637 pressure sensor reset...");
  // Read PROM data from MS5637 pressure sensor
  MS5637PromRead(Pcal);
  Serial.println("PROM dta read:");
  Serial.print("C0 = "); Serial.println(Pcal[0]);
  unsigned char refCRC = Pcal[0] >> 12;
  Serial.print("C1 = "); Serial.println(Pcal[1]);
  Serial.print("C2 = "); Serial.println(Pcal[2]);
  Serial.print("C3 = "); Serial.println(Pcal[3]);
  Serial.print("C4 = "); Serial.println(Pcal[4]);
  Serial.print("C5 = "); Serial.println(Pcal[5]);
  Serial.print("C6 = "); Serial.println(Pcal[6]);
  
  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data
  Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC);  
 
  delay(1000);  

  attachInterrupt(intPin, myinthandler, RISING);  // define interrupt for INT pin output of MPU9250

  }
  else
  {
    Serial.print("Could not connect to MPU9250: 0x");
    Serial.println(c, HEX);
    while(1) ; // Loop forever if communication doesn't happen
  }
  
}

void loop()
{  
   // If intPin goes high, all data registers have new data
   if(newData == true) {  // On interrupt, read data
     newData = false;  // reset newData flag
     readMPU9250Data(MPU9250Data); // INT cleared on any read
   
    // Now we'll calculate the accleration value into actual g's
     ax = (float)MPU9250Data[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
     ay = (float)MPU9250Data[1]*aRes - accelBias[1];   
     az = (float)MPU9250Data[2]*aRes - accelBias[2];  

    // Calculate the gyro value into actual degrees per second
     gx = (float)MPU9250Data[4]*gRes;  // get actual gyro value, this depends on scale being set
     gy = (float)MPU9250Data[5]*gRes;  
     gz = (float)MPU9250Data[6]*gRes; 
  
    newMagData = (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01);
    if(newMagData == true) { // wait for magnetometer data ready bit to be set
      readMagData(magCount);  // Read the x/y/z adc values
  
    // Calculate the magnetometer values in milliGauss
    // Include factory calibration per data sheet and user environmental corrections
      mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0];  // get actual magnetometer value, this depends on scale being set
      my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];  
      mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];  
      mx *= magScale[0];
      my *= magScale[1];
      mz *= magScale[2]; 
    }
    
    for(uint8_t i = 0; i < 10; i++) { // iterate a fixed number of times per data read cycle
    Now = micros();
    deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
    lastUpdate = Now;

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

    MadgwickQuaternionUpdate(-ax, ay, az, gx*pi/180.0f, -gy*pi/180.0f, -gz*pi/180.0f,  my,  -mx, mz);
    }
 } 

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

    if(SerialDebug) {
    Serial.print("ax = "); Serial.print((int)1000*ax);  
    Serial.print(" ay = "); Serial.print((int)1000*ay); 
    Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
    Serial.print("gx = "); Serial.print( gx, 2); 
    Serial.print(" gy = "); Serial.print( gy, 2); 
    Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
    Serial.print("mx = "); Serial.print( (int)mx ); 
    Serial.print(" my = "); Serial.print( (int)my ); 
    Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
    
    Serial.print("q0 = "); Serial.print(q[0]);
    Serial.print(" qx = "); Serial.print(q[1]); 
    Serial.print(" qy = "); Serial.print(q[2]); 
    Serial.print(" qz = "); Serial.println(q[3]); 
    }               
    tempCount = readTempData();  // Read the gyro adc values
    temperature = ((float) tempCount) / 333.87f + 21.0f; // Gyro chip temperature in degrees Centigrade
   // Print temperature in degrees Centigrade      
    Serial.print("Gyro temperature is ");  Serial.print(temperature, 1);  Serial.println(" degrees C"); // Print T values to tenths of s degree C
 
    D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value
    D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value
    dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference
    OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);
    SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);
 
    Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade
//
// Second order corrections
    if(Temperature > 20) 
    {
      TT2 = 5*dT*dT/pow(2, 38); // correction for high temperatures
      OFFSET2 = 0;
      SENS2 = 0;
    }
    if(Temperature < 20)                   // correction for low temperature
    {
      TT2      = 3*dT*dT/pow(2, 33); 
      OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
      SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
    } 
    if(Temperature < -15)                      // correction for very low temperature
    {
      OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);
      SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);
    }
 // End of second order corrections
 //
     Temperature = Temperature - T2/100;
     OFFSET = OFFSET - OFFSET2;
     SENS = SENS - SENS2;
 
     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or kPa

    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));
   
    if(SerialDebug) {
    Serial.print("Digital temperature value = "); Serial.print( (float)Temperature, 2); Serial.println(" C"); // temperature in degrees Celsius
    Serial.print("Digital temperature value = "); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(" F"); // temperature in degrees Fahrenheit
    Serial.print("Digital pressure value = "); Serial.print((float) Pressure, 2);  Serial.println(" mbar");// pressure in millibar
    Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet");
    }
    
   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
  // In this coordinate system, the positive z-axis is down toward Earth. 
  // 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.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
  // applied in the correct order which for this configuration is yaw, pitch, and then roll.
  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
    //Software AHRS:
 //   yaw   = atan2f(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]);   
 //   pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
 //   roll  = atan2f(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]);
 //   pitch *= 180.0f / PI;
 //   yaw   *= 180.0f / PI; 
 //   yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
 //   if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360
 //   roll  *= 180.0f / PI;
    a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);
    a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
    a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);
    a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);
    a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    pitch = -asin(a32);
    roll  = atan2(a31, a33);
    yaw   = atan2(a12, a22);
    pitch *= 180.0f / pi;
    yaw   *= 180.0f / pi; 
    yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360
    roll  *= 180.0f / pi;
    lin_ax = ax + a31;
    lin_ay = ay + a32;
    lin_az = az - a33;
    if(SerialDebug) {
    Serial.print("Yaw, Pitch, Roll: ");
    Serial.print(yaw, 2);
    Serial.print(", ");
    Serial.print(pitch, 2);
    Serial.print(", ");
    Serial.println(roll, 2);

    Serial.print("Grav_x, Grav_y, Grav_z: ");
    Serial.print(-a31*1000.0f, 2);
    Serial.print(", ");
    Serial.print(-a32*1000.0f, 2);
    Serial.print(", ");
    Serial.print(a33*1000.0f, 2);  Serial.println(" mg");
    Serial.print("Lin_ax, Lin_ay, Lin_az: ");
    Serial.print(lin_ax*1000.0f, 2);
    Serial.print(", ");
    Serial.print(lin_ay*1000.0f, 2);
    Serial.print(", ");
    Serial.print(lin_az*1000.0f, 2);  Serial.println(" mg");

    Serial.print("sumCount = "); Serial.println(sumCount);
    Serial.print("sum = "); Serial.println(sum);
    
    Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
    }
 
    digitalWrite(myLed, !digitalRead(myLed));
    count = millis(); 
    sumCount = 0;
    sum = 0;    
    }
 
}

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

void myinthandler()
{
  newData = true;
}

void getMres() {
  switch (Mscale)
  {
 	// Possible magnetometer scales (and their register bit settings) are:
	// 14 bit resolution (0) and 16 bit resolution (1)
    case MFS_14BITS:
          mRes = 10.*4912./8190.; // Proper scale to return milliGauss
          break;
    case MFS_16BITS:
          mRes = 10.*4912./32760.0; // Proper scale to return milliGauss
          break;
  }
}

void getGres() {
  switch (Gscale)
  {
 	// Possible gyro scales (and their register bit settings) are:
	// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11). 
        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
    case GFS_250DPS:
          gRes = 250.0/32768.0;
          break;
    case GFS_500DPS:
          gRes = 500.0/32768.0;
          break;
    case GFS_1000DPS:
          gRes = 1000.0/32768.0;
          break;
    case GFS_2000DPS:
          gRes = 2000.0/32768.0;
          break;
  }
}

void getAres() {
  switch (Ascale)
  {
 	// Possible accelerometer scales (and their register bit settings) are:
	// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11). 
        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
    case AFS_2G:
          aRes = 2.0/32768.0;
          break;
    case AFS_4G:
          aRes = 4.0/32768.0;
          break;
    case AFS_8G:
          aRes = 8.0/32768.0;
          break;
    case AFS_16G:
          aRes = 16.0/32768.0;
          break;
  }
}

void readMPU9250Data(int16_t * destination)
{
  uint8_t rawData[14];  // x/y/z accel register data stored here
  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, &rawData[0]);  // Read the 14 raw data registers into data array
  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;  
  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; 
  destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;   
  destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;  
  destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;  
  destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ; 
}

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


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

void readMagData(int16_t * destination)
{
  uint8_t rawData[7];  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
  readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array
  uint8_t c = rawData[6]; // End data read by reading ST2 register
    if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
    destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value
    destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  // Data stored as little Endian
    destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 
   }
}

int16_t readTempData()
{
  uint8_t rawData[2];  // x/y/z gyro register data stored here
  readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array 
  return ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a 16-bit value
}
       
void initAK8963(float * destination)
{
  // First extract the factory calibration for each magnetometer axis
  uint8_t rawData[3];  // x/y/z gyro calibration data stored here
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  
  delay(10);
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
  delay(10);
  readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);  // Read the x-, y-, and z-axis calibration values
  destination[0] =  (float)(rawData[0] - 128)/256. + 1.;   // Return x-axis sensitivity adjustment values, etc.
  destination[1] =  (float)(rawData[1] - 128)/256. + 1.;  
  destination[2] =  (float)(rawData[2] - 128)/256. + 1.; 
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  
  delay(10);
  // Configure the magnetometer for continuous read and highest resolution
  // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
  // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
  writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
  delay(10);
}


void initMPU9250()
{  
 // wake up device
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 
  delay(100); // Wait for all registers to reset 

 // get stable time source
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  // Auto select clock source to be PLL gyroscope reference if ready else
  delay(200); 
  
 // Configure Gyro and Thermometer
 // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; 
 // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
 // be higher than 1 / 0.0059 = 170 Hz
 // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
 // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
  writeByte(MPU9250_ADDRESS, CONFIG, 0x03);  

 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);  // Use a 200 Hz rate; a rate consistent with the filter update rate 
                                    // determined inset in CONFIG above
 
 // Set gyroscope full scale range
 // Range selects FS_SEL and GFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
  uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
 // c = c & ~0xE0; // Clear self-test bits [7:5] 
  c = c & ~0x03; // Clear Fchoice bits [1:0] 
  c = c & ~0x18; // Clear GFS bits [4:3]
  c = c | Gscale << 3; // Set full scale range for the gyro
 // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
  
 // Set accelerometer full-scale range configuration
  c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
 // c = c & ~0xE0; // Clear self-test bits [7:5] 
  c = c & ~0x18;  // Clear AFS bits [4:3]
  c = c | Ascale << 3; // Set full scale range for the accelerometer 
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value

 // Set accelerometer sample rate configuration
 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
  c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
  c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])  
  c = c | 0x03;  // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
  
 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, 
 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting

  // Configure Interrupts and Bypass Enable
  // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
  // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips 
  // can join the I2C bus and all can be controlled by the Arduino as master
//   writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);    
   writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x12);  // INT is 50 microsecond pulse and any read to clear  
   writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);  // Enable data ready (bit 0) interrupt
   delay(100);
}


// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void accelgyrocalMPU9250(float * dest1, float * dest2)
{  
  uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
  uint16_t ii, packet_count, fifo_count;
  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
  
 // reset device
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
  delay(100);
   
 // get stable time source; Auto select clock source to be PLL gyroscope reference if ready 
 // else use the internal oscillator, bits 2:0 = 001
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  
  writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
  delay(200);                                    

// Configure device for bias calculation
  writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts
  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);      // Disable FIFO
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);   // Turn on internal clock source
  writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C);    // Reset FIFO and DMP
  delay(15);
  
// Configure MPU6050 gyro and accelerometer for bias calculation
  writeByte(MPU9250_ADDRESS, CONFIG, 0x01);      // Set low-pass filter to 188 Hz
  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);  // Set sample rate to 1 kHz
  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
 
  uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
  uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g

// Configure FIFO to capture accelerometer and gyro data for bias calculation
  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  
  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in MPU-9150)
  delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes

// At end of sample accumulation, turn off FIFO sensor read
  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO
  readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
  fifo_count = ((uint16_t)data[0] << 8) | data[1];
  packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
  
  for (ii = 0; ii < packet_count; ii++) {
    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
    readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
    accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ;  // Form signed 16-bit integer for each sample in FIFO
    accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;
    accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;    
    gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;
    gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;
    gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
    
    accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    accel_bias[1] += (int32_t) accel_temp[1];
    accel_bias[2] += (int32_t) accel_temp[2];
    gyro_bias[0]  += (int32_t) gyro_temp[0];
    gyro_bias[1]  += (int32_t) gyro_temp[1];
    gyro_bias[2]  += (int32_t) gyro_temp[2];
            
}
    accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
    accel_bias[1] /= (int32_t) packet_count;
    accel_bias[2] /= (int32_t) packet_count;
    gyro_bias[0]  /= (int32_t) packet_count;
    gyro_bias[1]  /= (int32_t) packet_count;
    gyro_bias[2]  /= (int32_t) packet_count;
    
  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
  else {accel_bias[2] += (int32_t) accelsensitivity;}
   
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
  data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
  data[1] = (-gyro_bias[0]/4)       & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
  data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;
  data[3] = (-gyro_bias[1]/4)       & 0xFF;
  data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;
  data[5] = (-gyro_bias[2]/4)       & 0xFF;
  
// Push gyro biases to hardware registers
  writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
  writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
  writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
  writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
  writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
  writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
  
// Output scaled gyro biases for display in the main program
  dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;  
  dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
  dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;

// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.

  int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
  readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
  accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
  accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
  accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  
  uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
  uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
  
  for(ii = 0; ii < 3; ii++) {
    if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
  }
  
  // Construct total accelerometer bias, including calculated average accelerometer bias from above
  accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
  accel_bias_reg[1] -= (accel_bias[1]/8);
  accel_bias_reg[2] -= (accel_bias[2]/8);
  
  data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
  data[1] = (accel_bias_reg[0])      & 0xFF;
  data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
  data[3] = (accel_bias_reg[1])      & 0xFF;
  data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
  data[5] = (accel_bias_reg[2])      & 0xFF;
  data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
 
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
/*  writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
  writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
  writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
  writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
  writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
  writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
*/
// Output scaled accelerometer biases for display in the main program
   dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 
   dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
   dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}


void magcalMPU9250(float * dest1, float * dest2) 
{
  uint16_t ii = 0, sample_count = 0;
  int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
  int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};

  Serial.println("Mag Calibration: Wave device in a figure eight until done!");
  delay(4000);
  
    // shoot for ~fifteen seconds of mag data
    if(Mmode == 0x02) sample_count = 128;  // at 8 Hz ODR, new mag data is available every 125 ms
    if(Mmode == 0x06) sample_count = 1500;  // at 100 Hz ODR, new mag data is available every 10 ms
   for(ii = 0; ii < sample_count; ii++) {
    readMagData(mag_temp);  // Read the mag data   
    for (int jj = 0; jj < 3; jj++) {
      if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
      if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
    }
    if(Mmode == 0x02) delay(135);  // at 8 Hz ODR, new mag data is available every 125 ms
    if(Mmode == 0x06) delay(12);  // at 100 Hz ODR, new mag data is available every 10 ms
    }

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

    // Get hard iron correction
    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
    
    dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0];  // save mag biases in G for main program
    dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];   
    dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];  
       
    // Get soft iron correction estimate
    mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts
    mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts
    mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts

    float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
    avg_rad /= 3.0;

    dest2[0] = avg_rad/((float)mag_scale[0]);
    dest2[1] = avg_rad/((float)mag_scale[1]);
    dest2[2] = avg_rad/((float)mag_scale[2]);
  
   Serial.println("Mag Calibration done!");
}



// Accelerometer and gyroscope self test; check calibration wrt factory settings
void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
{
   uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
   uint8_t selfTest[6];
   int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
   float factoryTrim[6];
   uint8_t FS = 0;
   
  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);    // Set gyro sample rate to 1 kHz
  writeByte(MPU9250_ADDRESS, CONFIG, 0x02);        // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS);  // Set full scale range for the gyro to 250 dps
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g

  for( int ii = 0; ii < 200; ii++) {  // get average current values of gyro and acclerometer
  
  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);        // Read the six raw data registers into data array
  aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
  
    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);       // Read the six raw data registers sequentially into data array
  gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
  }
  
  for (int ii =0; ii < 3; ii++) {  // Get average of 200 values and store as average current readings
  aAvg[ii] /= 200;
  gAvg[ii] /= 200;
  }
  
// Configure the accelerometer for self-test
   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
   delay(25);  // Delay a while to let the device stabilize

  for( int ii = 0; ii < 200; ii++) {  // get average self-test values of gyro and acclerometer
  
  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array
  aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
  
    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
  gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
  }
  
  for (int ii =0; ii < 3; ii++) {  // Get average of 200 values and store as average self-test readings
  aSTAvg[ii] /= 200;
  gSTAvg[ii] /= 200;
  }   
  
 // Configure the gyro and accelerometer for normal operation
   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);  
   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0x00);  
   delay(25);  // Delay a while to let the device stabilize
   
   // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
   selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
   selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
   selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
   selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO);  // X-axis gyro self-test results
   selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO);  // Y-axis gyro self-test results
   selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO);  // Z-axis gyro self-test results

  // Retrieve factory self-test value from self-test code reads
   factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
   factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
   factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
   factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
   factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
   factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
 
 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
 // To get percent, must multiply by 100
   for (int i = 0; i < 3; i++) {
     destination[i]   = 100.0f*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.0f;   // Report percent differences
     destination[i+3] = 100.0f*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.0f; // Report percent differences
   }
   
}

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

        void MS5637Reset()
        {
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(MS5637_RESET);                // Put reset command in Tx buffer
        Wire.endTransmission();                  // Send the Tx buffer
        }
        
        void MS5637PromRead(uint16_t * destination)
        {
        uint8_t data[2] = {0,0};
        for (uint8_t ii = 0; ii < 7; ii++) {
          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer
          Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
          uint8_t i = 0;
          Wire.requestFrom(MS5637_ADDRESS, 2);     // Read two bytes from slave PROM address 
          while (Wire.available()) {
          data[i++] = Wire.read(); }               // Put read results in the Rx buffer
          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program
        }
        }

        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read
        {
        uint8_t data[3] = {0,0,0};
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(CMD | OSR);                   // Put pressure conversion command in Tx buffer
        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
        
        switch (OSR)
        {
          case ADC_256: delay(1); break;         // delay for conversion to complete
          case ADC_512: delay(3); break;
          case ADC_1024: delay(4); break;
          case ADC_2048: delay(6); break;
          case ADC_4096: delay(10); break;
          case ADC_8192: delay(20); break;
        }
       
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(0x00);                        // Put ADC read command in Tx buffer
        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
        uint8_t i = 0;
        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address 
        while (Wire.available()) {
        data[i++] = Wire.read(); }               // Put read results in the Rx buffer
        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program
        }



unsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents
{
  int cnt;
  unsigned int n_rem = 0;
  unsigned char n_bit;
  
  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation
  n_prom[7] = 0;
  for(cnt = 0; cnt < 16; cnt++)
  {
    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);
    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);
    for(n_bit = 8; n_bit > 0; n_bit--)
    {
        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;
        else                  n_rem = (n_rem<<1);
    }
  }
  n_rem = ((n_rem>>12) & 0x000F);
  return (n_rem ^ 0x00);
}



// simple function to scan for I2C devices on the bus
void I2Cscan() 
{
    // 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);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");
}


// I2C read/write functions for the MPU9250 sensors

  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
  Wire.beginTransmission(address);  // Initialize the Tx buffer
  Wire.write(subAddress);           // Put slave register address in Tx buffer
  Wire.write(data);                 // Put data in Tx buffer
  Wire.endTransmission();           // Send the Tx buffer
}

  uint8_t readByte(uint8_t address, uint8_t subAddress)
{
  uint8_t data = 0;                        // `data` will store the register data   
  Wire.beginTransmission(address);         // Initialize the Tx buffer
  Wire.write(subAddress);                  // Put slave register address in Tx buffer
  Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
  Wire.requestFrom(address, 1);            // Read two bytes from slave register address on MPU9250 
  data = Wire.read();                      // Fill Rx buffer with result
  return data;                             // Return data read from slave register
}

  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
  Wire.beginTransmission(address);   // Initialize the Tx buffer
  Wire.write(subAddress);            // Put slave register address in Tx buffer
  Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
  uint8_t i = 0;
  Wire.requestFrom(address, count);  // Read bytes from slave register address 
  while (Wire.available()) {
        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer
}


================================================
FILE: MPU9250_MS5637/MS5637.ino
================================================
#include "Wire.h"   

// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet
#define MS5637_RESET      0x1E
#define MS5637_CONVERT_D1 0x40
#define MS5637_CONVERT_D2 0x50
#define MS5637_ADC_READ   0x00

#define MS5637_ADDRESS 0x76   // Address of altimeter

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

#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 full scale
uint8_t OSR = ADC_8192;     // set pressure amd temperature oversample rate
  
// Pin definitions
int intPin = 14;  

int myLed = 5;

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, TT2, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data
    
int16_t tempCount;            // temperature raw count output
float   temperature;          // Stores the MPU9250 gyro internal chip temperature 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 pi = 3.141592653589793238462643383279502884f;

void setup()
{
  Serial.begin(115200);
  delay(4000);
  
  Wire.begin(21,22); // 21/22 are default on ESP32
  Wire.setClock(400000); // choose 400 kHz I2C rate
//  Wire.setClockStretchLimit(1000000);
  delay(1000);
  
  // Set up the interrupt pin, its set as active high, push-pull
  pinMode(intPin, INPUT);
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, LOW);
  
  I2Cscan();// look for I2C devices on the bus
    
  // Reset the MS5637 pressure sensor
  MS5637Reset();
  delay(100);
  Serial.println("MS5637 pressure sensor reset...");
  // Read PROM data from MS5637 pressure sensor
  MS5637PromRead(Pcal);
  Serial.println("PROM dta read:");
  Serial.print("C0 = "); Serial.println(Pcal[0]);
  unsigned char refCRC = Pcal[0] >> 12;
  Serial.print("C1 = "); Serial.println(Pcal[1]);
  Serial.print("C2 = "); Serial.println(Pcal[2]);
  Serial.print("C3 = "); Serial.println(Pcal[3]);
  Serial.print("C4 = "); Serial.println(Pcal[4]);
  Serial.print("C5 = "); Serial.println(Pcal[5]);
  Serial.print("C6 = "); Serial.println(Pcal[6]);
  
  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data
  Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC);  
}

void loop()
{  

    D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value
    D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value
    dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference
    OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);
    SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);
 
    Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade
//
// Second order corrections
    if(Temperature > 20) 
    {
      TT2 = 5*dT*dT/pow(2, 38); // correction for high temperatures
      OFFSET2 = 0;
      SENS2 = 0;
    }
    if(Temperature < 20)                   // correction for low temperature
    {
      TT2      = 3*dT*dT/pow(2, 33); 
      OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
      SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
    } 
    if(Temperature < -15)                      // correction for very low temperature
    {
      OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);
      SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);
    }
 // End of second order corrections
 //
     Temperature = Temperature - T2/100;
     OFFSET = OFFSET - OFFSET2;
     SENS = SENS - SENS2;
 
     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or kPa

    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));
   
    if(SerialDebug) {
    Serial.print("Digital temperature value = "); Serial.print( (float)Temperature, 2); Serial.println(" C"); // temperature in degrees Celsius
    Serial.print("Digital temperature value = "); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(" F"); // temperature in degrees Fahrenheit
    Serial.print("Digital pressure value = "); Serial.print((float) Pressure, 2);  Serial.println(" mbar");// pressure in millibar
    Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet");
    }   
 
   digitalWrite(myLed, !digitalRead(myLed));
   delay(500);
}

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

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

        void MS5637Reset()
        {
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(MS5637_RESET);                // Put reset command in Tx buffer
        Wire.endTransmission();                  // Send the Tx buffer
        }
        
        void MS5637PromRead(uint16_t * destination)
        {
        uint8_t data[2] = {0,0};
        for (uint8_t ii = 0; ii < 7; ii++) {
          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer
          Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
          uint8_t i = 0;
          Wire.requestFrom(MS5637_ADDRESS, 2);     // Read two bytes from slave PROM address 
          while (Wire.available()) {
          data[i++] = Wire.read(); }               // Put read results in the Rx buffer
          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program
        }
        }

        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read
        {
        uint8_t data[3] = {0,0,0};
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(CMD | OSR);                   // Put pressure conversion command in Tx buffer
        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
        
        switch (OSR)
        {
          case ADC_256: delay(1); break;         // delay for conversion to complete
          case ADC_512: delay(3); break;
          case ADC_1024: delay(4); break;
          case ADC_2048: delay(6); break;
          case ADC_4096: delay(10); break;
          case ADC_8192: delay(20); break;
        }
       
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(0x00);                        // Put ADC read command in Tx buffer
        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
        uint8_t i = 0;
        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address 
        while (Wire.available()) {
        data[i++] = Wire.read(); }               // Put read results in the Rx buffer
        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program
        }

unsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents
{
  int cnt;
  unsigned int n_rem = 0;
  unsigned char n_bit;
  
  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation
  n_prom[7] = 0;
  for(cnt = 0; cnt < 16; cnt++)
  {
    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);
    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);
    for(n_bit = 8; n_bit > 0; n_bit--)
    {
        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;
        else                  n_rem = (n_rem<<1);
    }
  }
  n_rem = ((n_rem>>12) & 0x000F);
  return (n_rem ^ 0x00);
}


// simple function to scan for I2C devices on the bus
void I2Cscan() 
{
    // 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);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");
}


================================================
FILE: MPU9250_MS5637/quaternionFilters.ino
================================================
// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
        void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
        {
            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
            float norm;
            float hx, hy, _2bx, _2bz;
            float s1, s2, s3, s4;
            float qDot1, qDot2, qDot3, qDot4;

            // Auxiliary variables to avoid repeated arithmetic
            float _2q1mx;
            float _2q1my;
            float _2q1mz;
            float _2q2mx;
            float _4bx;
            float _4bz;
            float _2q1 = 2.0f * q1;
            float _2q2 = 2.0f * q2;
            float _2q3 = 2.0f * q3;
            float _2q4 = 2.0f * q4;
            float _2q1q3 = 2.0f * q1 * q3;
            float _2q3q4 = 2.0f * q3 * q4;
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = sqrtf(ax * ax + ay * ay + az * az);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f/norm;
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrtf(mx * mx + my * my + mz * mz);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f/norm;
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            _2q1mx = 2.0f * q1 * mx;
            _2q1my = 2.0f * q1 * my;
            _2q1mz = 2.0f * q1 * mz;
            _2q2mx = 2.0f * q2 * mx;
            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            _2bx = sqrtf(hx * hx + hy * hy);
            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
            _4bx = 2.0f * _2bx;
            _4bz = 2.0f * _2bz;

            // Gradient decent algorithm corrective step
            s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
            norm = 1.0f/norm;
            s1 *= norm;
            s2 *= norm;
            s3 *= norm;
            s4 *= norm;

            // Compute rate of change of quaternion
            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

            // Integrate to yield quaternion
            q1 += qDot1 * deltat;
            q2 += qDot2 * deltat;
            q3 += qDot3 * deltat;
            q4 += qDot4 * deltat;
            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
            norm = 1.0f/norm;
            q[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;

        }
  
  
  
 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
 // measured ones. 
            void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
        {
            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
            float norm;
            float hx, hy, bx, bz;
            float vx, vy, vz, wx, wy, wz;
            float ex, ey, ez;
            float pa, pb, pc;

            // Auxiliary variables to avoid repeated arithmetic
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;   

            // Normalise accelerometer measurement
            norm = sqrtf(ax * ax + ay * ay + az * az);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f / norm;        // use reciprocal for division
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrtf(mx * mx + my * my + mz * mz);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f / norm;        // use reciprocal for division
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
            hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
            bx = sqrtf((hx * hx) + (hy * hy));
            bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);

            // Estimated direction of gravity and magnetic field
            vx = 2.0f * (q2q4 - q1q3);
            vy = 2.0f * (q1q2 + q3q4);
            vz = q1q1 - q2q2 - q3q3 + q4q4;
            wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
            wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
            wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  

            // Error is cross product between estimated direction and measured direction of gravity
            ex = (ay * vz - az * vy) + (my * wz - mz * wy);
            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
            if (Ki > 0.0f)
            {
                eInt[0] += ex;      // accumulate integral error
                eInt[1] += ey;
                eInt[2] += ez;
            }
            else
            {
                eInt[0] = 0.0f;     // prevent integral wind up
                eInt[1] = 0.0f;
                eInt[2] = 0.0f;
            }

            // Apply feedback terms
            gx = gx + Kp * ex + Ki * eInt[0];
            gy = gy + Kp * ey + Ki * eInt[1];
            gz = gz + Kp * ez + Ki * eInt[2];

            // Integrate rate of change of quaternion
            pa = q2;
            pb = q3;
            pc = q4;
            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
            q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
            q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
            q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);

            // Normalise quaternion
            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
            norm = 1.0f / norm;
            q[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;
 
}


================================================
FILE: MPU9250_MS5637/readme.md
================================================
Sketch for the MPU9250 9 DoF motion sensor including open-source sensor fusion using either the Madgwick or Mahony algorithms. The breakout board I use is [this](https://www.tindie.com/products/onehorse/mpu9250-teensy-3x-add-on-shields/) one which also has the excellent MS5637 pressure/temperature sensor. This can provide an additional estimate of altitude accurate to a foot or so.

![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-07-07T22:15:55.669Z-MPU9250MiniTop.2.jpg.855x570_q85_pad_rcrop.jpg)


================================================
FILE: MPU9250_MS5637_AHRS_UDP/MPU9250_MS5637_AHRS_UDP.ino
================================================
/* MPU9250_MS5637_ESP32 Basic Example Code
 by: Kris Winer
 date: December 14, 2016
 license: Beerware - Use this code however you'd like. If you 
 find it useful you can buy me a beer some time.
 
 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 
 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. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
 
 This sketch is intended specifically for the MPU9250+MS5637 Add-on shield.
 It uses SDA/SCL on pins 21/22, respectively, and it uses the Wire library.
 The MS5637 is a simple but high resolution 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.
 
 SDA and SCL should have external pull-up resistors (to 3.3V).
 4K7 resistors are on the MPU9250+MS5637 breakout board.
 
 Hardware setup:
 MPU9250 Breakout --------- ESP32
 VDD ---------------------- 3.3V
 SDA ----------------------- 21
 SCL ----------------------- 22
 GND ---------------------- GND
 
 */
#include "Wire.h"   

// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet
#define MS5637_RESET      0x1E
#define MS5637_CONVERT_D1 0x40
#define MS5637_CONVERT_D2 0x50
#define MS5637_ADC_READ   0x00

// See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in 
// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
//
//Magnetometer Registers
#define AK8963_ADDRESS   0x0C
#define WHO_AM_I_AK8963  0x00 // should return 0x48
#define INFO             0x01
#define AK8963_ST1       0x02  // data ready status bit 0
#define AK8963_XOUT_L	   0x03  // data
#define AK8963_XOUT_H	   0x04
#define AK8963_YOUT_L	   0x05
#define AK8963_YOUT_H	   0x06
#define AK8963_ZOUT_L	   0x07
#define AK8963_ZOUT_H	   0x08
#define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC      0x0C  // Self test control
#define AK8963_I2CDIS    0x0F  // I2C disable
#define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value

#define SELF_TEST_X_GYRO 0x00                  
#define SELF_TEST_Y_GYRO 0x01                                                                          
#define SELF_TEST_Z_GYRO 0x02

/*#define X_FINE_GAIN      0x03 // [7:0] fine gain
#define Y_FINE_GAIN      0x04
#define Z_FINE_GAIN      0x05
#define XA_OFFSET_H      0x06 // User-defined trim values for accelerometer
#define XA_OFFSET_L_TC   0x07
#define YA_OFFSET_H      0x08
#define YA_OFFSET_L_TC   0x09
#define ZA_OFFSET_H      0x0A
#define ZA_OFFSET_L_TC   0x0B */

#define SELF_TEST_X_ACCEL 0x0D
#define SELF_TEST_Y_ACCEL 0x0E    
#define SELF_TEST_Z_ACCEL 0x0F

#define SELF_TEST_A      0x10

#define XG_OFFSET_H      0x13  // User-defined trim values for gyroscope
#define XG_OFFSET_L      0x14
#define YG_OFFSET_H      0x15
#define YG_OFFSET_L      0x16
#define ZG_OFFSET_H      0x17
#define ZG_OFFSET_L      0x18
#define SMPLRT_DIV       0x19
#define CONFIG           0x1A
#define GYRO_CONFIG      0x1B
#define ACCEL_CONFIG     0x1C
#define ACCEL_CONFIG2    0x1D
#define LP_ACCEL_ODR     0x1E   
#define WOM_THR          0x1F   

#define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
#define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]
#define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms

#define FIFO_EN          0x23
#define I2C_MST_CTRL     0x24   
#define I2C_SLV0_ADDR    0x25
#define I2C_SLV0_REG     0x26
#define I2C_SLV0_CTRL    0x27
#define I2C_SLV1_ADDR    0x28
#define I2C_SLV1_REG     0x29
#define I2C_SLV1_CTRL    0x2A
#define I2C_SLV2_ADDR    0x2B
#define I2C_SLV2_REG     0x2C
#define I2C_SLV2_CTRL    0x2D
#define I2C_SLV3_ADDR    0x2E
#define I2C_SLV3_REG     0x2F
#define I2C_SLV3_CTRL    0x30
#define I2C_SLV4_ADDR    0x31
#define I2C_SLV4_REG     0x32
#define I2C_SLV4_DO      0x33
#define I2C_SLV4_CTRL    0x34
#define I2C_SLV4_DI      0x35
#define I2C_MST_STATUS   0x36
#define INT_PIN_CFG      0x37
#define INT_ENABLE       0x38
#define DMP_INT_STATUS   0x39  // Check DMP interrupt
#define INT_STATUS       0x3A
#define ACCEL_XOUT_H     0x3B
#define ACCEL_XOUT_L     0x3C
#define ACCEL_YOUT_H     0x3D
#define ACCEL_YOUT_L     0x3E
#define ACCEL_ZOUT_H     0x3F
#define ACCEL_ZOUT_L     0x40
#define TEMP_OUT_H       0x41
#define TEMP_OUT_L       0x42
#define GYRO_XOUT_H      0x43
#define GYRO_XOUT_L      0x44
#define GYRO_YOUT_H      0x45
#define GYRO_YOUT_L      0x46
#define GYRO_ZOUT_H      0x47
#define GYRO_ZOUT_L      0x48
#define EXT_SENS_DATA_00 0x49
#define EXT_SENS_DATA_01 0x4A
#define EXT_SENS_DATA_02 0x4B
#define EXT_SENS_DATA_03 0x4C
#define EXT_SENS_DATA_04 0x4D
#define EXT_SENS_DATA_05 0x4E
#define EXT_SENS_DATA_06 0x4F
#define EXT_SENS_DATA_07 0x50
#define EXT_SENS_DATA_08 0x51
#define EXT_SENS_DATA_09 0x52
#define EXT_SENS_DATA_10 0x53
#define EXT_SENS_DATA_11 0x54
#define EXT_SENS_DATA_12 0x55
#define EXT_SENS_DATA_13 0x56
#define EXT_SENS_DATA_14 0x57
#define EXT_SENS_DATA_15 0x58
#define EXT_SENS_DATA_16 0x59
#define EXT_SENS_DATA_17 0x5A
#define EXT_SENS_DATA_18 0x5B
#define EXT_SENS_DATA_19 0x5C
#define EXT_SENS_DATA_20 0x5D
#define EXT_SENS_DATA_21 0x5E
#define EXT_SENS_DATA_22 0x5F
#define EXT_SENS_DATA_23 0x60
#define MOT_DETECT_STATUS 0x61
#define I2C_SLV0_DO      0x63
#define I2C_SLV1_DO      0x64
#define I2C_SLV2_DO      0x65
#define I2C_SLV3_DO      0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET  0x68
#define MOT_DETECT_CTRL  0x69
#define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
#define PWR_MGMT_1       0x6B // Device defaults to the SLEEP mode
#define PWR_MGMT_2       0x6C
#define DMP_BANK         0x6D  // Activates a specific bank in the DMP
#define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank
#define DMP_REG          0x6F  // Register in DMP from which to read or to which to write
#define DMP_REG_1        0x70
#define DMP_REG_2        0x71 
#define FIFO_COUNTH      0x72
#define FIFO_COUNTL      0x73
#define FIFO_R_W         0x74
#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
#define XA_OFFSET_H      0x77
#define XA_OFFSET_L      0x78
#define YA_OFFSET_H      0x7A
#define YA_OFFSET_L      0x7B
#define ZA_OFFSET_H      0x7D
#define ZA_OFFSET_L      0x7E

// Using the MPU9250_MS5637 Add-On shield, ADO is set to 0 
// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
#define ADO 0
#if ADO
#define MPU9250_ADDRESS 0x69  // Device address when ADO = 1
#define AK8963_ADDRESS 0x0C   //  Address of magnetometer
#define MS5637_ADDRESS 0x76   // Address of altimeter
#else
#define MPU9250_ADDRESS 0x68  // Device address when ADO = 0
#define AK8963_ADDRESS 0x0C   //  Address of magnetometer
#define MS5637_ADDRESS 0x76   // Address of altimeter
#endif  

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

// Set initial input parameters
enum Ascale {
  AFS_2G = 0,
  AFS_4G,
  AFS_8G,
  AFS_16G
};

enum Gscale {
  GFS_250DPS = 0,
  GFS_500DPS,
  GFS_1000DPS,
  GFS_2000DPS
};

enum Mscale {
  MFS_14BITS = 0, // 0.6 mG per LSB
  MFS_16BITS      // 0.15 mG per LSB
};

#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 full scale
uint8_t OSR = ADC_8192;     // set pressure amd temperature oversample rate
uint8_t Gscale = GFS_250DPS;
uint8_t Ascale = AFS_2G;
uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution
uint8_t Mmode = 0x06;        // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
float aRes, gRes, mRes;      // scale resolutions per LSB for the sensors

// Pin definitions
int intPin = 14;  // can be any pin
bool newData = false;
bool newMagData = false;

int myLed = 5;

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, TT2, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data

int16_t MPU9250Data[7]; // used to read all 14 bytes at once from the MPU9250 accel/gyro
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
float magCalibration[3] = {0, 0, 0};  // Factory mag calibration and mag bias
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}, magScale[3]  = {0, 0, 0};      // Bias corrections for gyro and accelerometer
int16_t tempCount;            // temperature raw count output
float   temperature;          // Stores the MPU9250 gyro internal chip temperature in degrees Celsius
double Temperature, Pressure; // stores MS5637 pressures sensor pressure and temperature
float SelfTest[6];            // holds results of gyro and accelerometer self test

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
float pi = 3.141592653589793238462643383279502884f;
float GyroMeasError = PI * (4.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 a12, a22, a31, a32, a33;            // rotation matrix coefficients for Euler angles and gravity components
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 lin_ax, lin_ay, lin_az;             // linear acceleration (acceleration with gravity component subtracted)
float q[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


//#include <ESP8266WiFi.h>  //use for ESP8266
#include <WiFi.h>       // use for ESP32
#include <WiFiUdp.h>

#define sendInterval 100

const char* ssid = "network-name";
const char* password = "password";
WiFiUDP Udp;
static IPAddress remoteIp = IPAddress();
static uint16_t remotePort = 4210;

unsigned int localUdpPort = 4210;  // local port to listen on
char incomingPacket[256];  // buffer for incoming packets
String payload;

//The command from the PC
String cmd;
float val_array[21]; 

void setup()
{
  Serial.begin(115200);
  //Setup wifi and udp connection
    Serial.println();
    Serial.printf("Connecting to %s ", ssid);
    
    // delete old config
    WiFi.disconnect(true);
    
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED)
    {
        delay(500);
        Serial.print(".");
    }
    Serial.println(" connected");
    Udp.begin(localUdpPort);
    Serial.printf("Now listening at IP %s, UDP port %d\n", WiFi.localIP().toString().c_str(), localUdpPort);
    WiFi.localIP().toString().c_str(), localUdpPort;

 
  delay(4000);
  
  Wire.begin(21, 22, 400000); //(SDA, SCL) (21,22) are default on ESP32, 400 kHz I2C clock
  delay(1000);
  
  // Set up the interrupt pin, its set as active high, push-pull
  pinMode(intPin, INPUT);
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, LOW);
  
  I2Cscan();// look for I2C devices on the bus
    
  // Read the WHO_AM_I register, this is a good test of communication
  Serial.println("MPU9250 9-axis motion sensor...");
  uint8_t c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
  Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX);

  delay(1000); 

  if (c == 0x71) // WHO_AM_I should always be 0x71
  {  
    Serial.println("MPU9250 is online...");
    
    MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
    Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value");
    Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value");
    Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value");
    Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value");
    Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value");
    Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value");
    delay(1000);
    
   // get sensor resolutions, only need to do this once
   getAres();
   getGres();
   getMres();
    
   Serial.println(" Calibrate gyro and accel");
   accelgyrocalMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
   Serial.println("accel biases (mg)"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); Serial.println(1000.*accelBias[2]);
   Serial.println("gyro biases (dps)"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);

  delay(1000);  
   
  initMPU9250(); 
  Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
  
  // Read the WHO_AM_I register of the magnetometer, this is a good test of communication
  byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);  // Read WHO_AM_I register for AK8963
  Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX);
 
  delay(1000); 
  
  // Get magnetometer calibration from AK8963 ROM
  initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer
  
  magcalMPU9250(magBias, magScale);
  Serial.println("AK8963 mag biases (mG)"); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]); 
  Serial.println("AK8963 mag scale (mG)"); Serial.println(magScale[0]); Serial.println(magScale[1]); Serial.println(magScale[2]); 
  delay(2000); // add delay to see results before serial spew of data
   
  if(SerialDebug) {
//  Serial.println("Calibration values: ");
  Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2);
  Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2);
  Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2);
  }

  delay(1000);  
  
  // Reset the MS5637 pressure sensor
  MS5637Reset();
  delay(100);
  Serial.println("MS5637 pressure sensor reset...");
  // Read PROM data from MS5637 pressure sensor
  MS5637PromRead(Pcal);
  Serial.println("PROM dta read:");
  Serial.print("C0 = "); Serial.println(Pcal[0]);
  unsigned char refCRC = Pcal[0] >> 12;
  Serial.print("C1 = "); Serial.println(Pcal[1]);
  Serial.print("C2 = "); Serial.println(Pcal[2]);
  Serial.print("C3 = "); Serial.println(Pcal[3]);
  Serial.print("C4 = "); Serial.println(Pcal[4]);
  Serial.print("C5 = "); Serial.println(Pcal[5]);
  Serial.print("C6 = "); Serial.println(Pcal[6]);
  
  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data
  Serial.print("Checksum = "); Serial.print(nCRC); Serial.print(" , should be "); Serial.println(refCRC);  
 
  delay(1000);  

  attachInterrupt(intPin, myinthandler, RISING);  // define interrupt for INT pin output of MPU9250

  }
  else
  {
    Serial.print("Could not connect to MPU9250: 0x");
    Serial.println(c, HEX);
    while(1) ; // Loop forever if communication doesn't happen
  }
  
  //Setup wifi and udp connection
    Serial.println();
    Serial.printf("Connecting to %s ", ssid);
    
    // delete old config
    WiFi.disconnect(true);
    
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED)
    {
        delay(500);
        Serial.print(".");
    }
    Serial.println(" connected");
    Udp.begin(localUdpPort);
    Serial.printf("Now listening at IP %s, UDP port %d\n", WiFi.localIP().toString().c_str(), localUdpPort);
    WiFi.localIP().toString().c_str(), localUdpPort;

}

void loop()
{  
    int packetSize = Udp.parsePacket();
    if (packetSize)
    {
        // receive incoming UDP packets
        remoteIp = Udp.remoteIP();
        remotePort = Udp.remotePort();
        Serial.printf("Received %d bytes from %s, port %d\n", packetSize, remoteIp.toString().c_str(), remotePort);
        int len = Udp.read(incomingPacket, 255);
        incomingPacket[len] = 0;
        Serial.printf("UDP packet contents: %s\n", incomingPacket);
        
        cmd = incomingPacket[0];
        if(cmd=="s") {
			      Serial.print("Hit s to continue...");
            Serial.print('\n');
        }
        else if(cmd=="x"){
        WiFi.disconnect(true);
        Serial.println("Disconnected");
      }
    }
    
    if(!remotePort){
        //nobody have connected yet
        return;
    }
    

   // If intPin goes high, all data registers have new data
   if(newData == true) {  // On interrupt, read data
     newData = false;  // reset newData flag
     readMPU9250Data(MPU9250Data); // INT cleared on any read
   
    // Now we'll calculate the accleration value into actual g's
     ax = (float)MPU9250Data[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
     ay = (float)MPU9250Data[1]*aRes - accelBias[1];   
     az = (float)MPU9250Data[2]*aRes - accelBias[2];  

    // Calculate the gyro value into actual degrees per second
     gx = (float)MPU9250Data[4]*gRes;  // get actual gyro value, this depends on scale being set
     gy = (float)MPU9250Data[5]*gRes;  
     gz = (float)MPU9250Data[6]*gRes; 
  
    newMagData = (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01);
    if(newMagData == true) { // wait for magnetometer data ready bit to be set
      readMagData(magCount);  // Read the x/y/z adc values
  
    // Calculate the magnetometer values in milliGauss
    // Include factory calibration per data sheet and user environmental corrections
      mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0];  // get actual magnetometer value, this depends on scale being set
      my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];  
      mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];  
      mx *= magScale[0];
      my *= magScale[1];
      mz *= magScale[2]; 
    }
    
    for(uint8_t i = 0; i < 10; i++) { // iterate a fixed number of times per data read cycle
      Now = micros();
      deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
      lastUpdate = Now;

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

      MadgwickQuaternionUpdate(-ax, ay, az, gx*pi/180.0f, -gy*pi/180.0f, -gz*pi/180.0f,  my,  -mx, mz);
    }
   } 

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

      if(SerialDebug) {
        Serial.print("ax = "); Serial.print((int)1000*ax);  
        Serial.print(" ay = "); Serial.print((int)1000*ay); 
        Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
        Serial.print("gx = "); Serial.print( gx, 2); 
        Serial.print(" gy = "); Serial.print( gy, 2); 
        Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
        Serial.print("mx = "); Serial.print( (int)mx ); 
        Serial.print(" my = "); Serial.print( (int)my ); 
        Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
    
        Serial.print("q0 = "); Serial.print(q[0]);
        Serial.print(" qx = "); Serial.print(q[1]); 
        Serial.print(" qy = "); Serial.print(q[2]); 
        Serial.print(" qz = "); Serial.println(q[3]); 
      }
                     
      tempCount = readTempData();  // Read the gyro adc values
      temperature = ((float) tempCount) / 333.87f + 21.0f; // Gyro chip temperature in degrees Centigrade
      
      if(SerialDebug) {
      // Print temperature in degrees Centigrade      
        Serial.print("Gyro temperature is ");  Serial.print(temperature, 1);  Serial.println(" degrees C"); // Print T values to tenths of s degree C
      }
      
      D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value
      D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value
      dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference
      OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);
      SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);
 
      Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade

      //
      // Second order corrections
      if(Temperature > 20) 
      {
        TT2 = 5*dT*dT/pow(2, 38); // correction for high temperatures
        OFFSET2 = 0;
        SENS2 = 0;
      }
      if(Temperature < 20)                   // correction for low temperature
      {
        TT2      = 3*dT*dT/pow(2, 33); 
        OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
        SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;
      } 
      if(Temperature < -15)                      // correction for very low temperature
      {
        OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);
        SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);
      }
      // End of second order corrections
      //
    
     Temperature = Temperature - T2/100;
     OFFSET = OFFSET - OFFSET2;
     SENS = SENS - SENS2;
 
     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or kPa

    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));
   
    if(SerialDebug) {
      Serial.print("Digital temperature value = "); Serial.print( (float)Temperature, 2); Serial.println(" C"); // temperature in degrees Celsius
      Serial.print("Digital temperature value = "); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(" F"); // temperature in degrees Fahrenheit
      Serial.print("Digital pressure value = "); Serial.print((float) Pressure, 2);  Serial.println(" mbar");// pressure in millibar
      Serial.print("Altitude = "); Serial.print(altitude, 2); Serial.println(" feet");
    }
    
   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
   // In this coordinate system, the positive z-axis is down toward Earth. 
   // 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.
   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
   //Software AHRS:
   //   yaw   = atan2f(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]);   
   //   pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
   //   roll  = atan2f(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]);
   //   pitch *= 180.0f / PI;
   //   yaw   *= 180.0f / PI; 
   //   yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
   //   if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360
   //   roll  *= 180.0f / PI;
    a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);
    a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
    a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);
    a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);
    a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
    pitch = -asin(a32);
    roll  = atan2(a31, a33);
    yaw   = atan2(a12, a22);
    pitch *= 180.0f / pi;
    yaw   *= 180.0f / pi; 
    yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
    if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360
    roll  *= 180.0f / pi;
    lin_ax = ax + a31;
    lin_ay = ay + a32;
    lin_az = az - a33;
    
    if(SerialDebug) {
      Serial.print("Yaw, Pitch, Roll: ");
      Serial.print(yaw, 2);
      Serial.print(", ");
      Serial.print(pitch, 2);
      Serial.print(", ");
      Serial.println(roll, 2);

      Serial.print("Grav_x, Grav_y, Grav_z: ");
      Serial.print(-a31*1000.0f, 2);
      Serial.print(", ");
      Serial.print(-a32*1000.0f, 2);
      Serial.print(", ");
      Serial.print(a33*1000.0f, 2);  Serial.println(" mg");
      Serial.print("Lin_ax, Lin_ay, Lin_az: ");
      Serial.print(lin_ax*1000.0f, 2);
      Serial.print(", ");
      Serial.print(lin_ay*1000.0f, 2);
      Serial.print(", ");
      Serial.print(lin_az*1000.0f, 2);  Serial.println(" mg");

      Serial.print("sumCount = "); Serial.println(sumCount);
      Serial.print("sum = "); Serial.println(sum);
    
      Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
    }
	
      val_array[15] = (float)sumCount/sum;        
      val_array[7] = (gx * M_PI/180);
      val_array[8] = (gy * M_PI/180);
      val_array[9] = (gz * M_PI/180);
      val_array[4] = (ax);
      val_array[5] = (ay);
      val_array[6] = (az);
      val_array[10] = (mx);
      val_array[11] = (my);
      val_array[12] = (mz);
      val_array[0] = (q[0]);
      val_array[1] = (q[1]);
      val_array[2] = (q[2]);
      val_array[3] = (q[3]);
      val_array[16] = yaw;
      val_array[18] = 0;
      val_array[19] = 0;
      val_array[17] = altitude;
      val_array[13] = (9.*(float) Temperature/5. + 32., 2);
      val_array[14] = (float) Pressure;

      payload = "";
      serialPayloadFloatArr(val_array, 20);
      payload += "\r\n";
  
      int payloadLen = payload.length();
      byte message[payloadLen];  
      payload.getBytes(message,payloadLen);
    
      Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
      Udp.write(message,sizeof(message));
      Udp.endPacket();

      //delay(sendInterval);
	
      digitalWrite(myLed, !digitalRead(myLed));
      count = millis(); 
      sumCount = 0;
      sum = 0;    
    }
 
}

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

void myinthandler()
{
  newData = true;
}

void getMres() {
  switch (Mscale)
  {
 	// Possible magnetometer scales (and their register bit settings) are:
	// 14 bit resolution (0) and 16 bit resolution (1)
    case MFS_14BITS:
          mRes = 10.*4912./8190.; // Proper scale to return milliGauss
          break;
    case MFS_16BITS:
          mRes = 10.*4912./32760.0; // Proper scale to return milliGauss
          break;
  }
}

void getGres() {
  switch (Gscale)
  {
 	// Possible gyro scales (and their register bit settings) are:
	// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11). 
        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
    case GFS_250DPS:
          gRes = 250.0/32768.0;
          break;
    case GFS_500DPS:
          gRes = 500.0/32768.0;
          break;
    case GFS_1000DPS:
          gRes = 1000.0/32768.0;
          break;
    case GFS_2000DPS:
          gRes = 2000.0/32768.0;
          break;
  }
}

void getAres() {
  switch (Ascale)
  {
 	// Possible accelerometer scales (and their register bit settings) are:
	// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11). 
        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
    case AFS_2G:
          aRes = 2.0/32768.0;
          break;
    case AFS_4G:
          aRes = 4.0/32768.0;
          break;
    case AFS_8G:
          aRes = 8.0/32768.0;
          break;
    case AFS_16G:
          aRes = 16.0/32768.0;
          break;
  }
}

void readMPU9250Data(int16_t * destination)
{
  uint8_t rawData[14];  // x/y/z accel register data stored here
  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, &rawData[0]);  // Read the 14 raw data registers into data array
  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;  
  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; 
  destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;   
  destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;  
  destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;  
  destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ; 
}

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


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

void readMagData(int16_t * destination)
{
  uint8_t rawData[7];  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
  readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array
  uint8_t c = rawData[6]; // End data read by reading ST2 register
    if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
    destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value
    destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  // Data stored as little Endian
    destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; 
   }
}

int16_t readTempData()
{
  uint8_t rawData[2];  // x/y/z gyro register data stored here
  readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array 
  return ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a 16-bit value
}
       
void initAK8963(float * destination)
{
  // First extract the factory calibration for each magnetometer axis
  uint8_t rawData[3];  // x/y/z gyro calibration data stored here
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  
  delay(10);
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
  delay(10);
  readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);  // Read the x-, y-, and z-axis calibration values
  destination[0] =  (float)(rawData[0] - 128)/256. + 1.;   // Return x-axis sensitivity adjustment values, etc.
  destination[1] =  (float)(rawData[1] - 128)/256. + 1.;  
  destination[2] =  (float)(rawData[2] - 128)/256. + 1.; 
  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  
  delay(10);
  // Configure the magnetometer for continuous read and highest resolution
  // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
  // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
  writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
  delay(10);
}


void initMPU9250()
{  
 // wake up device
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 
  delay(100); // Wait for all registers to reset 

 // get stable time source
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  // Auto select clock source to be PLL gyroscope reference if ready else
  delay(200); 
  
 // Configure Gyro and Thermometer
 // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; 
 // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
 // be higher than 1 / 0.0059 = 170 Hz
 // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
 // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
  writeByte(MPU9250_ADDRESS, CONFIG, 0x03);  

 // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);  // Use a 200 Hz rate; a rate consistent with the filter update rate 
                                    // determined inset in CONFIG above
 
 // Set gyroscope full scale range
 // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
  uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
 // c = c & ~0xE0; // Clear self-test bits [7:5] 
  c = c & ~0x02; // Clear Fchoice bits [1:0] 
  c = c & ~0x18; // Clear AFS bits [4:3]
  c = c | Gscale << 3; // Set full scale range for the gyro
 // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
  
 // Set accelerometer full-scale range configuration
  c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
 // c = c & ~0xE0; // Clear self-test bits [7:5] 
  c = c & ~0x18;  // Clear AFS bits [4:3]
  c = c | Ascale << 3; // Set full scale range for the accelerometer 
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value

 // Set accelerometer sample rate configuration
 // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
 // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
  c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
  c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])  
  c = c | 0x03;  // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
  
 // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, 
 // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting

  // Configure Interrupts and Bypass Enable
  // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
  // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips 
  // can join the I2C bus and all can be controlled by the Arduino as master
//   writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);    
   writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x12);  // INT is 50 microsecond pulse and any read to clear  
   writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);  // Enable data ready (bit 0) interrupt
   delay(100);
}


// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void accelgyrocalMPU9250(float * dest1, float * dest2)
{  
  uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
  uint16_t ii, packet_count, fifo_count;
  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
  
 // reset device
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
  delay(100);
   
 // get stable time source; Auto select clock source to be PLL gyroscope reference if ready 
 // else use the internal oscillator, bits 2:0 = 001
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  
  writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
  delay(200);                                    

// Configure device for bias calculation
  writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts
  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);      // Disable FIFO
  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);   // Turn on internal clock source
  writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C);    // Reset FIFO and DMP
  delay(15);
  
// Configure MPU6050 gyro and accelerometer for bias calculation
  writeByte(MPU9250_ADDRESS, CONFIG, 0x01);      // Set low-pass filter to 188 Hz
  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);  // Set sample rate to 1 kHz
  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
 
  uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
  uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g

// Configure FIFO to capture accelerometer and gyro data for bias calculation
  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  
  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in MPU-9150)
  delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes

// At end of sample accumulation, turn off FIFO sensor read
  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO
  readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
  fifo_count = ((uint16_t)data[0] << 8) | data[1];
  packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
  
  for (ii = 0; ii < packet_count; ii++) {
    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
    readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
    accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ;  // Form signed 16-bit integer for each sample in FIFO
    accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;
    accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;    
    gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;
    gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;
    gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
    
    accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
    accel_bias[1] += (int32_t) accel_temp[1];
    accel_bias[2] += (int32_t) accel_temp[2];
    gyro_bias[0]  += (int32_t) gyro_temp[0];
    gyro_bias[1]  += (int32_t) gyro_temp[1];
    gyro_bias[2]  += (int32_t) gyro_temp[2];
            
}
    accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
    accel_bias[1] /= (int32_t) packet_count;
    accel_bias[2] /= (int32_t) packet_count;
    gyro_bias[0]  /= (int32_t) packet_count;
    gyro_bias[1]  /= (int32_t) packet_count;
    gyro_bias[2]  /= (int32_t) packet_count;
    
  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
  else {accel_bias[2] += (int32_t) accelsensitivity;}
   
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
  data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
  data[1] = (-gyro_bias[0]/4)       & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
  data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;
  data[3] = (-gyro_bias[1]/4)       & 0xFF;
  data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;
  data[5] = (-gyro_bias[2]/4)       & 0xFF;
  
// Push gyro biases to hardware registers
  writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
  writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
  writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
  writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
  writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
  writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
  
// Output scaled gyro biases for display in the main program
  dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;  
  dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
  dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;

// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.

  int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
  readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
  accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
  accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
  accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
  
  uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
  uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
  
  for(ii = 0; ii < 3; ii++) {
    if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
  }
  
  // Construct total accelerometer bias, including calculated average accelerometer bias from above
  accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
  accel_bias_reg[1] -= (accel_bias[1]/8);
  accel_bias_reg[2] -= (accel_bias[2]/8);
  
  data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
  data[1] = (accel_bias_reg[0])      & 0xFF;
  data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
  data[3] = (accel_bias_reg[1])      & 0xFF;
  data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
  data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
  data[5] = (accel_bias_reg[2])      & 0xFF;
  data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
 
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
/*  writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
  writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
  writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
  writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
  writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
  writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
*/
// Output scaled accelerometer biases for display in the main program
   dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 
   dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
   dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}


void magcalMPU9250(float * dest1, float * dest2) 
{
  uint16_t ii = 0, sample_count = 0;
  int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
  int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};

  Serial.println("Mag Calibration: Wave device in a figure eight until done!");
  delay(4000);
  
    // shoot for ~fifteen seconds of mag data
    if(Mmode == 0x02) sample_count = 128;  // at 8 Hz ODR, new mag data is available every 125 ms
    if(Mmode == 0x06) sample_count = 1500;  // at 100 Hz ODR, new mag data is available every 10 ms
   for(ii = 0; ii < sample_count; ii++) {
    readMagData(mag_temp);  // Read the mag data   
    for (int jj = 0; jj < 3; jj++) {
      if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
      if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
    }
    if(Mmode == 0x02) delay(135);  // at 8 Hz ODR, new mag data is available every 125 ms
    if(Mmode == 0x06) delay(12);  // at 100 Hz ODR, new mag data is available every 10 ms
    }

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

    // Get hard iron correction
    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
    
    dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0];  // save mag biases in G for main program
    dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];   
    dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];  
       
    // Get soft iron correction estimate
    mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts
    mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts
    mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts

    float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
    avg_rad /= 3.0;

    dest2[0] = avg_rad/((float)mag_scale[0]);
    dest2[1] = avg_rad/((float)mag_scale[1]);
    dest2[2] = avg_rad/((float)mag_scale[2]);
  
   Serial.println("Mag Calibration done!");
}



// Accelerometer and gyroscope self test; check calibration wrt factory settings
void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
{
   uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
   uint8_t selfTest[6];
   int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
   float factoryTrim[6];
   uint8_t FS = 0;
   
  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);    // Set gyro sample rate to 1 kHz
  writeByte(MPU9250_ADDRESS, CONFIG, 0x02);        // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS);  // Set full scale range for the gyro to 250 dps
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g

  for( int ii = 0; ii < 200; ii++) {  // get average current values of gyro and acclerometer
  
  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);        // Read the six raw data registers into data array
  aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
  
    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);       // Read the six raw data registers sequentially into data array
  gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
  }
  
  for (int ii =0; ii < 3; ii++) {  // Get average of 200 values and store as average current readings
  aAvg[ii] /= 200;
  gAvg[ii] /= 200;
  }
  
// Configure the accelerometer for self-test
   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
   delay(25);  // Delay a while to let the device stabilize

  for( int ii = 0; ii < 200; ii++) {  // get average self-test values of gyro and acclerometer
  
  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array
  aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
  
    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
  gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
  }
  
  for (int ii =0; ii < 3; ii++) {  // Get average of 200 values and store as average self-test readings
  aSTAvg[ii] /= 200;
  gSTAvg[ii] /= 200;
  }   
  
 // Configure the gyro and accelerometer for normal operation
   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);  
   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0x00);  
   delay(25);  // Delay a while to let the device stabilize
   
   // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
   selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
   selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
   selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
   selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO);  // X-axis gyro self-test results
   selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO);  // Y-axis gyro self-test results
   selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO);  // Z-axis gyro self-test results

  // Retrieve factory self-test value from self-test code reads
   factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
   factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
   factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
   factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
   factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
   factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
 
 // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
 // To get percent, must multiply by 100
   for (int i = 0; i < 3; i++) {
     destination[i]   = 100.0f*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.0f;   // Report percent differences
     destination[i+3] = 100.0f*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.0f; // Report percent differences
   }
   
}

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

        void MS5637Reset()
        {
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(MS5637_RESET);                // Put reset command in Tx buffer
        Wire.endTransmission();                  // Send the Tx buffer
        }
        
        void MS5637PromRead(uint16_t * destination)
        {
        uint8_t data[2] = {0,0};
        for (uint8_t ii = 0; ii < 7; ii++) {
          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer
          Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
          uint8_t i = 0;
          Wire.requestFrom(MS5637_ADDRESS, 2);     // Read two bytes from slave PROM address 
          while (Wire.available()) {
          data[i++] = Wire.read(); }               // Put read results in the Rx buffer
          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program
        }
        }

        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read
        {
        uint8_t data[3] = {0,0,0};
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(CMD | OSR);                   // Put pressure conversion command in Tx buffer
        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
        
        switch (OSR)
        {
          case ADC_256: delay(1); break;         // delay for conversion to complete
          case ADC_512: delay(3); break;
          case ADC_1024: delay(4); break;
          case ADC_2048: delay(6); break;
          case ADC_4096: delay(10); break;
          case ADC_8192: delay(20); break;
        }
       
        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer
        Wire.write(0x00);                        // Put ADC read command in Tx buffer
        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
        uint8_t i = 0;
        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address 
        while (Wire.available()) {
        data[i++] = Wire.read(); }               // Put read results in the Rx buffer
        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program
        }



unsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents
{
  int cnt;
  unsigned int n_rem = 0;
  unsigned char n_bit;
  
  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation
  n_prom[7] = 0;
  for(cnt = 0; cnt < 16; cnt++)
  {
    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);
    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);
    for(n_bit = 8; n_bit > 0; n_bit--)
    {
        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;
        else                  n_rem = (n_rem<<1);
    }
  }
  n_rem = ((n_rem>>12) & 0x000F);
  return (n_rem ^ 0x00);
}



// simple function to scan for I2C devices on the bus
void I2Cscan() 
{
    // 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);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");
}


// I2C read/write functions for the MPU9250 sensors

  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
  Wire.beginTransmission(address);  // Initialize the Tx buffer
  Wire.write(subAddress);           // Put slave register address in Tx buffer
  Wire.write(data);                 // Put data in Tx buffer
  Wire.endTransmission();           // Send the Tx buffer
}

  uint8_t readByte(uint8_t address, uint8_t subAddress)
{
  uint8_t data = 0;                        // `data` will store the register data   
  Wire.beginTransmission(address);         // Initialize the Tx buffer
  Wire.write(subAddress);                  // Put slave register address in Tx buffer
  Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
  Wire.requestFrom(address, 1);            // Read two bytes from slave register address on MPU9250 
  data = Wire.read();                      // Fill Rx buffer with result
  return data;                             // Return data read from slave register
}

  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{  
  Wire.beginTransmission(address);   // Initialize the Tx buffer
  Wire.write(subAddress);            // Put slave register address in Tx buffer
  Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
  uint8_t i = 0;
  Wire.requestFrom(address, count);  // Read bytes from slave register address 
  while (Wire.available()) {
        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer
}


void serialPayloadPrint(float f) {
    byte * b = (byte *) &f;
    for(int i=0; i<4; i++) {
        
        byte b1 = (b[i] >> 4) & 0x0f;
        byte b2 = (b[i] & 0x0f);
        
        char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
        char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
        
        payload += (c1);
        payload += (c2);
    }
}

void serialPayloadFloatArr(float * arr, int length) {
    for(int i=0; i<length; i++) {
        serialPayloadPrint(arr[i]);
        payload += ",";
    }
}


================================================
FILE: MPU9250_MS5637_AHRS_UDP/quaternionFilters.ino
================================================

// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
        void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
        {
            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
            float norm;
            float hx, hy, _2bx, _2bz;
            float s1, s2, s3, s4;
            float qDot1, qDot2, qDot3, qDot4;

            // Auxiliary variables to avoid repeated arithmetic
            float _2q1mx;
            float _2q1my;
            float _2q1mz;
            float _2q2mx;
            float _4bx;
            float _4bz;
            float _2q1 = 2.0f * q1;
            float _2q2 = 2.0f * q2;
            float _2q3 = 2.0f * q3;
            float _2q4 = 2.0f * q4;
            float _2q1q3 = 2.0f * q1 * q3;
            float _2q3q4 = 2.0f * q3 * q4;
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = sqrtf(ax * ax + ay * ay + az * az);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f/norm;
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrtf(mx * mx + my * my + mz * mz);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f/norm;
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            _2q1mx = 2.0f * q1 * mx;
            _2q1my = 2.0f * q1 * my;
            _2q1mz = 2.0f * q1 * mz;
            _2q2mx = 2.0f * q2 * mx;
            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            _2bx = sqrtf(hx * hx + hy * hy);
            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
            _4bx = 2.0f * _2bx;
            _4bz = 2.0f * _2bz;

            // Gradient decent algorithm corrective step
            s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
            norm = 1.0f/norm;
            s1 *= norm;
            s2 *= norm;
            s3 *= norm;
            s4 *= norm;

            // Compute rate of change of quaternion
            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

            // Integrate to yield quaternion
            q1 += qDot1 * deltat;
            q2 += qDot2 * deltat;
            q3 += qDot3 * deltat;
            q4 += qDot4 * deltat;
            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
            norm = 1.0f/norm;
            q[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;

        }
  
  
  
 // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
 // measured ones. 
            void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
        {
            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
            float norm;
            float hx, hy, bx, bz;
            float vx, vy, vz, wx, wy, wz;
            float ex, ey, ez;
            float pa, pb, pc;

            // Auxiliary variables to avoid repeated arithmetic
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;   

            // Normalise accelerometer measurement
            norm = sqrtf(ax * ax + ay * ay + az * az);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f / norm;        // use reciprocal for division
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrtf(mx * mx + my * my + mz * mz);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f / norm;        // use reciprocal for division
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
            hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
            bx = sqrtf((hx * hx) + (hy * hy));
            bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);

            // Estimated direction of gravity and magnetic field
            vx = 2.0f * (q2q4 - q1q3);
            vy = 2.0f * (q1q2 + q3q4);
            vz = q1q1 - q2q2 - q3q3 + q4q4;
            wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
            wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
            wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  

            // Error is cross product between estimated direction and measured direction of gravity
            ex = (ay * vz - az * vy) + (my * wz - mz * wy);
            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
            if (Ki > 0.0f)
            {
                eInt[0] += ex;      // accumulate integral error
                eInt[1] += ey;
                eInt[2] += ez;
            }
            else
            {
                eInt[0] = 0.0f;     // prevent integral wind up
                eInt[1] = 0.0f;
                eInt[2] = 0.0f;
            }

            // Apply feedback terms
            gx = gx + Kp * ex + Ki * eInt[0];
            gy = gy + Kp * ey + Ki * eInt[1];
            gz = gz + Kp * ez + Ki * eInt[2];

            // Integrate rate of change of quaternion
            pa = q2;
            pb = q3;
            pc = q4;
            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
            q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
            q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
            q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);

            // Normalise quaternion
            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
            norm = 1.0f / norm;
            q[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;
 
        }


================================================
FILE: MPU9250_MS5637_AHRS_UDP/readme.md
================================================
Sketch for the MPU9250 9 DoF motion sensor including open-source sensor fusion using either the Madgwick or Mahony algorithms. The breakout board I use is [this](https://www.tindie.com/products/onehorse/mpu9250-teensy-3x-add-on-shields/) one which also has the excellent MS5637 pressure/temperature sensor. This can provide an additional estimate of altitude accurate to a foot or so.

![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-07-07T22:15:55.669Z-MPU9250MiniTop.2.jpg.855x570_q85_pad_rcrop.jpg)


================================================
FILE: PWM/ledcWrite_demo_ESP32.ino
================================================
/*
  ledcWrite_demo_ESP32.ino
  Runs through the full 255 color spectrum for an rgb led 
  Demonstrate ledcWrite functionality for driving leds with PWM on ESP32
 
  This example code is in the public domain.
 */
 
// Set up the rgb led names
uint8_t ledR = 12;
uint8_t ledG = 14; // internally pulled up
uint8_t ledB = 13; 

uint8_t myLed = 5;  // on-board blue led (also internally pulled up)

uint8_t ledArray[3] = {1, 2, 3}; // three led channels

const boolean invert = false; // set true if common anode, false if common cathode

uint8_t color = 0;          // a value from 0 to 255 representing the hue
uint32_t R, G, B;           // the Red Green and Blue color components
uint8_t brightness = 255;  // 255 is maximum brightness, but can be changed

// the setup routine runs once when you press reset:
void setup() 
{             
  pinMode(myLed, OUTPUT); 
  digitalWrite(myLed, LOW);// Turn off on-board blue led
  
  ledcAttachPin(ledR, 1); // assign RGB led pins to channels
  ledcAttachPin(ledG, 2);
  ledcAttachPin(ledB, 3);
  
  // Initialize channels 
  // channels 0-15, resolution 1-16 bits, freq limits depend on resolution
  // ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits);
  ledcSetup(1, 12000, 8); // 12 kHz PWM, 8-bit resolution
  ledcSetup(2, 12000, 8);
  ledcSetup(3, 12000, 8);

 for(uint8_t i=0; i < 3; i++) {
  // ledcWrite(channel, dutycycle)
  // For 8-bit resolution duty cycle is 0 - 255
  ledcWrite(ledArray[i], 255);  // test high output of all leds in sequence
  delay(1000);
  ledcWrite(ledArray[i], 0);
 }
 
}

// void loop runs over and over again
void loop() 
{
 for (color = 0; color < 255; color++) { // Slew through the color spectrum

  hueToRGB(color, brightness);  // call function to convert hue to RGB

  // write the RGB values to the pins
  ledcWrite(1, R); // write red component to channel 1, etc.
  ledcWrite(2, G);   
  ledcWrite(3, B); 
 
  delay(100); // full cycle of rgb over 256 colors takes 26 seconds
 }
 
 digitalWrite(myLed, HIGH); delay(10); digitalWrite(myLed, LOW); // indicate end of cycle
}


// Courtesy http://www.instructables.com/id/How-to-Use-an-RGB-LED/?ALLSTEPS
// function to convert a color to its Red, Green, and Blue components.

void hueToRGB(uint8_t hue, uint8_t brightness)
{
    uint16_t scaledHue = (hue * 6);
    uint8_t segment = scaledHue / 256; // segment 0 to 5 around the
                                            // color wheel
    uint16_t segmentOffset =
      scaledHue - (segment * 256); // position within the segment

    uint8_t complement = 0;
    uint16_t prev = (brightness * ( 255 -  segmentOffset)) / 256;
    uint16_t next = (brightness *  segmentOffset) / 256;

    if(invert)
    {
      brightness = 255 - brightness;
      complement = 255;
      prev = 255 - prev;
      next = 255 - next;
    }

    switch(segment ) {
    case 0:      // red
        R = brightness;
        G = next;
        B = complement;
    break;
    case 1:     // yellow
        R = prev;
        G = brightness;
        B = complement;
    break;
    case 2:     // green
        R = complement;
        G = brightness;
        B = next;
    break;
    case 3:    // cyan
        R = complement;
        G = prev;
        B = brightness;
    break;
    case 4:    // blue
        R = next;
        G = complement;
        B = brightness;
    break;
   case 5:      // magenta
    default:
        R = brightness;
        G = complement;
        B = prev;
    break;
    }
}


================================================
FILE: PWM/readme.md
================================================
Sketch using the ledcWrite utility to sweep through 256 colors of an rgb led using pins 12, 13, and 14 with setup as follows:

![](https://cloud.githubusercontent.com/assets/6698410/21281621/25fb9a54-c3a3-11e6-9110-46ba3339c284.jpg)

Don't forget the current-limiting resistors, here a 330-Ohm resistor array does the job.


================================================
FILE: README.md
================================================
# ESP32 Arduino sketch repository

These are sketches I have found to run on my own 1.4" x 0.7" ESP32 development board pictured below.

![](https://cloud.githubusercontent.com/assets/6698410/21195535/0eff4500-c1e9-11e6-9fa1-deb1077887aa.jpg)

These are mostly for using I2C sensors with the ESP32 but I intend to make use of all of the peripherals of the ESP32 and I will post working sketches as I do so. The board is open source and the design can be found [here](https://www.oshpark.com/shared_projects/sHwYbfxM). Now you can buy this ESP32 Development Board on [Tindie](https://www.tindie.com/products/onehorse/esp32-development-board/).


================================================
FILE: SPI/Readme.md
================================================
I use [this](https://www.tindie.com/products/onehorse/spi-flash-memory-add-ons-for-teensy-3x/) for the SPI NOR flash add-on board.

![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2015-03-16T03:24:10.978Z-S25FL127.front.jpg.855x570_q85_pad_rcrop.jpg)

Output for the SPIFlash test program should look like this for a Spansion S25FL127 128 Mbit SPI NOR flash:

Serial enabled!

ID bytes: 1 20 18 4D

Winbond W25Q80BLUX1G    Chip ID = 0xEF, 0x40, 0x14, 0x0

Macronix MX25L12835FZNI Chip ID = 0xC2, 0x20, 0x18, 0xC2

Spansion S25FL127S      Chip ID = 0x01, 0x20, 0x18, 0x4D
 
Write page:  0xFFFF

time (us) = 4470

Read Page 0xFFFF

time (us) = 4813

 0x0
 0x1 0x2 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10
 0x11 0x12 0x13 0x14 0x15 0x16 0x17 0x18 0x19 0x1A 0x1B 0x1C 0x1D 0x1E 0x1F 0x20
 0x21 0x22 0x23 0x24 0x25 0x26 0x27 0x28 0x29 0x2A 0x2B 0x2C 0x2D 0x2E 0x2F 0x30
 0x31 0x32 0x33 0x34 0x35 0x36 0x37 0x38 0x39 0x3A 0x3B 0x3C 0x3D 0x3E 0x3F 0x40
 0x41 0x42 0x43 0x44 0x45 0x46 0x47 0x48 0x49 0x4A 0x4B 0x4C 0x4D 0x4E 0x4F 0x50
 0x51 0x52 0x53 0x54 0x55 0x56 0x57 0x58 0x59 0x5A 0x5B 0x5C 0x5D 0x5E 0x5F 0x60
 0x61 0x62 0x63 0x64 0x65 0x66 0x67 0x68 0x69 0x6A 0x6B 0x6C 0x6D 0x6E 0x6F 0x70
 0x71 0x72 0x73 0x74 0x75 0x76 0x77 0x78 0x79 0x7A 0x7B 0x7C 0x7D 0x7E 0x7F 0x80
 0x81 0x82 0x83 0x84 0x85 0x86 0x87 0x88 0x89 0x8A 0x8B 0x8C 0x8D 0x8E 0x8F 0x90
 0x91 0x92 0x93 0x94 0x95 0x96 0x97 0x98 0x99 0x9A 0x9B 0x9C 0x9D 0x9E 0x9F 0xA0
 0xA1 0xA2 0xA3 0xA4 0xA5 0xA6 0xA7 0xA8 0xA9 0xAA 0xAB 0xAC 0xAD 0xAE 0xAF 0xB0
 0xB1 0xB2 0xB3 0xB4 0xB5 0xB6 0xB7 0xB8 0xB9 0xBA 0xBB 0xBC 0xBD 0xBE 0xBF 0xC0
 0xC1 0xC2 0xC3 0xC4 0xC5 0xC6 0xC7 0xC8 0xC9 0xCA 0xCB 0xCC 0xCD 0xCE 0xCF 0xD0
 0xD1 0xD2 0xD3 0xD4 0xD5 0xD6 0xD7 0xD8 0xD9 0xDA 0xDB 0xDC 0xDD 0xDE 0xDF 0xE0
 0xE1 0xE2 0xE3 0xE4 0xE5 0xE6 0xE7 0xE8 0xE9 0xEA 0xEB 0xEC 0xED 0xEE 0xEF 0xF0
 0xF1 0xF2 0xF3 0xF4 0xF5 0xF6 0xF7 0xF8 0xF9 0xFA 0xFB 0xFC 0xFD 0xFE 0xFF
 
time (ms) = 42829

Read Page 0xFFFF

time (us) = 4414

 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF
 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF


================================================
FILE: SPI/SPIFlash_ESP32.ino
================================================
/* SPIFlash_ESP32.ino
Sketch by Kris Winer December 16, 2016

License: Use this sketch any way you choose; if you like it, buy me a beer sometime

Purpose: Checks function of a variety of SPI NOR flash memory chips hosted by the ESP32 development board.

Sketch based on the work of Pete (El Supremo) as follows:
 * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca
 *
 * Development of this audio library was funded by PJRC.COM, LLC by sales of
 * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop
 * open source software by purchasing Teensy or other PJRC products.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 */

#include <SPI.h>

// Highest page number is 0xffff=65535
int page_number = 0xFFFF;
unsigned char w_page[256];
unsigned char r_page[256];

#define CSPIN  5  // A1 for IU board

#define STAT_WIP 1
#define STAT_WEL 2

#define CMD_WRITE_STATUS_REG   0x01
#define CMD_PAGE_PROGRAM       0x02
#define CMD_READ_DATA          0x03
#define CMD_WRITE_DISABLE      0x04//not tested
#define CMD_READ_STATUS_REG    0x05
#define CMD_WRITE_ENABLE       0x06
#define CMD_READ_HIGH_SPEED    0x0B//not tested
#define CMD_SECTOR_ERASE       0x20//not tested
#define CMD_BLOCK32K_ERASE     0x52//not tested
#define CMD_RESET_DEVICE       0xF0//<<-different from winbond
#define CMD_READ_ID            0x9F
#define CMD_RELEASE_POWER_DOWN 0xAB//not tested
#define CMD_POWER_DOWN         0xB9//not tested
#define CMD_CHIP_ERASE         0xC7
#define CMD_BLOCK64K_ERASE     0xD8//not tested

unsigned char flash_wait_for_write = 0;

void setup(void)
{
  pinMode(CSPIN, OUTPUT);

  SPI.begin(18, 19, 23, 5); // sck, miso, mosi, ss (ss can be any GPIO)
  pinMode(CSPIN, OUTPUT);
  digitalWrite(CSPIN, HIGH);
  
  unsigned char id_tab[32];
  unsigned long t_start;
  
  Serial.begin(115200);
  delay(4000);
  Serial.println("Serial enabled!");

  Serial.print("ID bytes: ");
  uint8_t id[4];
  digitalWrite(CSPIN, LOW);
  SPI.transfer(0x9F);
  id[0] = SPI.transfer(0);
  id[1] = SPI.transfer(0);
  id[2] = SPI.transfer(0);
  id[3] = SPI.transfer(0);
  digitalWrite(CSPIN, HIGH);
  Serial.print(id[0], HEX); Serial.print(" "); Serial.print(id[1], HEX);  Serial.print(" ");  Serial.print(id[2], HEX); Serial.print(" ");  Serial.println(id[3], HEX);

  Serial.println("Winbond W25Q80BLUX1G    Chip ID = 0xEF, 0x40, 0x14, 0x0");
  Serial.println("Macronix MX25L12835FZNI Chip ID = 0xC2, 0x20, 0x18, 0xC2");
  Serial.println("Spansion S25FL127S      Chip ID = 0x01, 0x20, 0x18, 0x4D");
  Serial.println(" ");
  
/* Initialize the array to 0,1,2,3 etc.*/
  for(uint16_t i = 0; i < 256; i++) {
    w_page[i] = i;
  }
  
/* Write the page to page_number - this page MUST be in the erased state*/
  Serial.print("Write page:  0x"); Serial.println(page_number, HEX);  
  t_start = micros();
  flash_page_program(w_page, page_number);
  t_start = micros() - t_start;
  Serial.print("time (us) = "); Serial.println(t_start);

/* Read back page_number and print its contents which should be 0,1,2,3...*/
  Serial.print("Read Page 0x"); Serial.println(page_number, HEX);
  t_start = micros();
  flash_read_pages(r_page, page_number,1);
  t_start = micros() - t_start;
  Serial.print("time (us) = "); Serial.println(t_start);
  
  for(uint16_t i = 0; i < 256; i++) {
    Serial.print(" 0x"); Serial.print(r_page[i], HEX);
	if (i % 16==0) Serial.println();
  }
  Serial.println("");
  
/* Erase the sector which includes page_number*/
  t_start = millis();
  flash_chip_erase(true);
  t_start = millis() - t_start;
  Serial.print("time (ms) = "); Serial.println(t_start);

/* Now read back the page. It should now be all 255.*/
  Serial.print( "Read Page 0x"); Serial.println(page_number, HEX);
  t_start = micros();
  flash_read_pages(r_page, page_number,1);
  t_start = micros() - t_start;

  Serial.print("time (us) = "); Serial.println(t_start);
  for(uint16_t i = 0; i < 256; i++) {
    Serial.print(" 0x"); Serial.print(r_page[i], HEX);
	if (i % 16==0) Serial.println();
  }
  Serial.println("");
}

void loop(void)
{
}

/*********************************************************************************************/
// Useful functions
/*********************************************************************************************/
void write_pause(void)
{
  if(flash_wait_for_write) {
    while(flash_read_status() & STAT_WIP);
    flash_wait_for_write = 0;
  }
}

//=====================================
// convert a page number to a 24-bit address
int page_to_address(int pn)
{
  return(pn << 8);
}

//=====================================
// convert a 24-bit address to a page number
int address_to_page(int addr)
{
  return(addr >> 8);
}

//=====================================
void flash_read_id(unsigned char *idt)
{
  write_pause();
  //set control register 
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_READ_ID);
  for(uint16_t i = 0; i < 20; i++) {
    *idt++ = SPI.transfer(0x00);
  }
  digitalWrite(CSPIN, HIGH);
}

//=====================================
unsigned char flash_read_status(void)
{
  unsigned char c;

// This can't do a write_pause
  digitalWrite(CSPIN, LOW);  
  SPI.transfer(CMD_READ_STATUS_REG);
  c = SPI.transfer(0x00);
  digitalWrite(CSPIN, HIGH);
  return(c);
}

//=====================================

void flash_hard_reset(void)
{
  // Make sure that the device is not busy before
  // doing the hard reset sequence
  // At the moment this does NOT check the
  // SUSpend status bit in Status Register 2
  // but the library does not support suspend
  // mode yet anyway
  write_pause();
  
  // Send Write Enable command
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_RESET_DEVICE );
  digitalWrite(CSPIN, HIGH);
  delayMicroseconds(50);
  // Wait for the hard reset to finish
  // Don't use flash_wait_for_write here
  while(flash_read_status() & STAT_WIP);
  // The spec says "the device will take
  // approximately tRST=30 microseconds
  // to reset"
}

//=====================================
void flash_chip_erase(boolean wait)
{
  write_pause();
  // Send Write Enable command
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_CHIP_ERASE);
  digitalWrite(CSPIN, HIGH);
  flash_wait_for_write = 1;
  if(wait)write_pause();
}

//=====================================
// Tse Typ=0.6sec Max=3sec
// measured 549.024ms
// Erase the sector which contains the specified
// page number.
// The smallest unit of memory which can be erased
// is the 4kB sector (which is 16 pages)
void flash_erase_pages_sector(int pn)
{
  int address;

  write_pause(); 
  // Send Write Enable command
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_SECTOR_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xff);
  SPI.transfer((address >> 8) & 0xff);
  SPI.transfer(address & 0xff);
  digitalWrite(CSPIN, HIGH);
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
// Erase the 32kb block which contains the specified
// page number.
void flash_erase_pages_block32k(int pn)
{
  int address;

  write_pause();
  // Send Write Enable command
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_BLOCK32K_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  digitalWrite(CSPIN, HIGH);
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
// Erase the 64kb block which contains the specified
// page number.
void flash_erase_pages_block64k(int pn)
{
  int address;
  
  write_pause();
  // Send Write Enable command
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_BLOCK64K_ERASE);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  digitalWrite(CSPIN, HIGH);
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
void flash_page_program(unsigned char *wp,int pn)
{
  int address;

  write_pause(); 
  // Send Write Enable command
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_WRITE_ENABLE);
  digitalWrite(CSPIN, HIGH);
  
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_PAGE_PROGRAM);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // Now write 256 bytes to the page
  for(uint16_t i = 0; i < 256; i++) {
  SPI.transfer(*wp++);
  }
  digitalWrite(CSPIN, HIGH);
  // Indicate that next I/O must wait for this write to finish
  flash_wait_for_write = 1;
}

//=====================================
void flash_read_pages(unsigned char *p,int pn,const int n_pages)
{
  int address;
  unsigned char *rp = p;
  
  write_pause();
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_READ_DATA);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // Now read the page's data bytes
  for(uint16_t i = 0; i < n_pages * 256; i++) {
    *rp++ = SPI.transfer(0);
  }
  digitalWrite(CSPIN, HIGH);
}

//=====================================
// Read specified number of pages starting with pn
void flash_fast_read_pages(unsigned char *p,int pn,const int n_pages)
{
  int address;
  unsigned char *rp = p;
  
  write_pause();
// The chip doesn't run at the higher clock speed until
// after the command and address have been sent
  digitalWrite(CSPIN, LOW);
  SPI.transfer(CMD_READ_HIGH_SPEED);
  // Send the 3 byte address
  address = page_to_address(pn);
  SPI.transfer((address >> 16) & 0xFF);
  SPI.transfer((address >> 8) & 0xFF);
  SPI.transfer(address & 0xFF);
  // send dummy byte
  SPI.transfer(0);
  // Now read the number of pages required
  for(uint16_t i = 0; i < n_pages * 256; i++) {
    *rp++ = SPI.transfer(0);
  }
  digitalWrite(CSPIN, HIGH);
}


================================================
FILE: VISHAY/VEML6040.ino
================================================
/* VEML6040 Basic Example Code
 by: Kris Winer
 date: December 14, 2016
 license: Beerware - Use this code however you'd like. If you 
 find it useful you can buy me a beer some time.
 
 Demonstrate basic VEML6040 functionality including parameterizing the register addresses, initializing the sensor, 
 getting properly scaled rgbw intensity data out. Sketch runs on the 3.3 V ESP32.
 
 From the data sheet: (https://www.vishay.com/docs/84276/veml6040.pdf)
 
 VEML6040 color sensor senses red, green, blue, and white light and incorporates photodiodes, amplifiers, 
 and analog / digital circuits into a single chip using CMOS process. With the   color   sensor   applied,   
 the   brightness,   and   color temperature of backlight can be adjusted base on ambient light  source  
 that  makes  panel  looks  more  comfortable  for  end   user’s   eyes.   VEML6040’s   adoption   of   FiltronTM
 technology  achieves  the  closest  ambient  light  spectral  sensitivity to real human eye responses.

 VEML6040  provides  excellent  temperature  compensation  capability  for  keeping  the  output  stable  
 under  changing  temperature.   VEML6040’s   function   are   easily   operated   via the simple command format 
 of I2C (SMBus compatible) interface  protocol.  VEML6040’s  operating  voltage  ranges  from   2.5   V   to   
 3.6   V.   VEML6040   is   packaged   in   a   lead  (Pb)-free  4  pin  OPLGA  package  which  offers  the  best market-proven reliability.

 SDA and SCL  have external pull-up resistors (to 3.3V).
 2K2 resistors are on the VEML6040 breakout board.
 
 Hardware setup:
 VEML6040 Breakout ------ ESP32
 VDD ---------------------- 3.3V or any digital pin i.e., digitalWrite(HIGH)
 SDA ----------------------- pin 21
 SCL ----------------------- pin 22
 GND ---------------------- GND or any digital pin i.e., digitalWrite(LOW)
 
*/
  
#include <Wire.h>

////////////////////////////
// VEML6040 Command Codes //
////////////////////////////
#define  VEML6040_CONF	          0x00 // command codes
#define  VEML6040_R_DATA   		    0x08  
#define  VEML6040_G_DATA		      0x09 
#define  VEML6040_B_DATA		      0x0A
#define  VEML6040_W_DATA          0x0B

#define VEML6040_ADDRESS          0x10

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

// Pin definitions
int myLed  = 5;                      
uint16_t count = 0;

enum IT {
  IT_40 = 0, //   40 ms
  IT_80,     //   80 ms
  IT_160,    //  160 ms
  IT_320,    //  320 ms
  IT_640,    //  640 ms
  IT_1280   // 1280 ms
};

// Specify VEML6040 Integration time
uint8_t IT = IT_160;
uint8_t ITime = 160;  // milliseconds
uint16_t RGBWData[4] = {0, 0, 0, 0};
float GSensitivity = 0.25168/((float) (IT + 1)); // ambient light sensitivity increases with integration time


void setup()
{
  Wire.begin(21, 22, 400000); // (SDA, SCL) (21, 22) are default on ESP32, 400 kHz I2C bus speed
  delay(4000);
  Serial.begin(115200);
  
  // Set up the interrupt pin, its set as active high, push-pull
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH);

  I2Cscan();
  
  enableVEML6040(); // initalize sensor
 
  delay(150);
  
  digitalWrite(myLed, LOW);

}

void loop()
{  
  getRGBWdata(RGBWData);
  Serial.print("Red raw counts = ");   Serial.println(RGBWData[0]);
  Serial.print("Green raw counts = "); Serial.println(RGBWData[1]);
  Serial.print("Blue raw counts = ");  Serial.println(RGBWData[2]);
  Serial.print("White raw counts = "); Serial.println(RGBWData[3]);
  Serial.print("Inferred IR raw counts = "); Serial.println(RGBWData[3] - RGBWData[0] - RGBWData[1] - RGBWData[2]);
  Serial.println("  ");
 
  Serial.print("Red   light power density = "); Serial.print((float)RGBWData[0]/96.0f, 2); Serial.println(" microWatt/cm^2");
  Serial.print("Green light power density = "); Serial.print((float)RGBWData[1]/74.0f, 2); Serial.println(" microWatt/cm^2");
  Serial.print("Blue  light power density = "); Serial.print((float)RGBWData[2]/56.0f, 2); Serial.println(" microWatt/cm^2");
  Serial.println("  ");

  Serial.print("Ambient light intensity = "); Serial.print((float)RGBWData[1]*GSensitivity, 2); Serial.println(" lux");
  Serial.println("  ");

  // Empirical estimation of the correlated color temperature CCT:
  // see https://www.vishay.com/docs/84331/designingveml6040.pdf
  float temp = ( (float) (RGBWData[0] - RGBWData[2])/(float) RGBWData[1] );
  float CCT = 4278.6f*pow(temp, -1.2455) + 0.5f;

  Serial.print("Correlated Color Temperature = "); Serial.print(CCT, 2); Serial.println(" Kelvin");
  Serial.println("  ");


  digitalWrite(myLed, !digitalRead(myLed));
  delay(ITime+100);
}

//===================================================================================================================
//====== Set of useful function to access UV data
//===================================================================================================================

uint16_t getRGBWdata(uint16_t * destination)
{
    for (int j = 0; j < 4; j++)
    {
    uint8_t rawData[2] = {0, 0};
    Wire.beginTransmission(VEML6040_ADDRESS);
    Wire.write(VEML6040_R_DATA + j);        // Command code for reading rgbw data channels in sequence
    Wire.endTransmission(false);  // Send the Tx buffer, but send a restart to keep connection alive

    Wire.requestFrom(VEML6040_ADDRESS, 2);  // Read two bytes from slave register address 
    uint8_t i = 0;
    while (Wire.available()) 
    {
        rawData[i++] = Wire.read();       // Put read results in the Rx buffer
    }     
    Wire.endTransmission();
    destination[j] = ((uint16_t) rawData[1] << 8) | rawData[0];
    }
 
}

void enableVEML6040()
{
  Wire.beginTransmission(VEML6040_ADDRESS);
  Wire.write(VEML6040_CONF); // Command code for configuration register
  Wire.write(IT << 4); // Bit 3 must be 0, bit 0 is 0 for run and 1 for shutdown, LS Byte
  Wire.write(0x00); // MS Byte
  Wire.endTransmission();
}


// I2C scan function

void I2Cscan()
{
// 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);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");
    
}


================================================
FILE: VISHAY/readme.md
================================================
Sketch for the VEML6040 RGBW ambient light sensor. I use [this](https://www.tindie.com/products/onehorse/veml6040-rgbw-color-and-ambient-light-sensor/) as the breakout board.

![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-05-07T02:47:51.413Z-VEML6040.top1.jpg.855x570_q85_pad_rcrop.jpg)


================================================
FILE: VL53L0x/VL53L0X_ESP32.ino
================================================
/* This example shows how to use continuous mode to take
range measurements with the VL53L0X. It is based on
vl53l0x_ContinuousRanging_Example.c from the VL53L0X API.

Basic sketch from Pololu version of ST Microelectronic's API
see: https://github.com/pololu/vl53l0x-arduino

Further modified by Kris Winer for the ESP32 and interrupt data ready, etc.

The range readings are in units of mm. */

#include <Wire.h>
#include <VL53L0X.h>

VL53L0X sensor;

// Uncomment this line to use long range mode. This
// increases the sensitivity of the sensor and extends its
// potential range, but increases the likelihood of getting
// an inaccurate reading because of reflections from objects
// other than the intended target. It works best in dark
// conditions.

//#define LONG_RANGE


// Uncomment ONE of these two lines to get
// - higher speed at the cost of lower accuracy OR
// - higher accuracy at the cost of lower speed

//#define HIGH_SPEED
#define HIGH_ACCURACY

// Pin definitions
int myLed = 5;
int intPin = 14;

bool newData = false;

uint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate
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

void setup()
{
  Serial.begin(115200);
  delay(4000);
  
 Wire.begin(21, 22, 400000); // SDA (21), SCL (22) on ESP32, 400 kHz rate
 
// Set up the led indicator
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, LOW);
  pinMode(intPin, INPUT);

  I2Cscan();
  
  delay(1000);
  
  sensor.init();
  sensor.setTimeout(500);

  #if defined LONG_RANGE
  // lower the return signal rate limit (default is 0.25 MCPS)
  sensor.setSignalRateLimit(0.1);
  // increase laser pulse periods (defaults are 14 and 10 PCLKs)
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
  #endif

  #if defined HIGH_SPEED
  // reduce timing budget to 20 ms (default is about 33 ms)
  sensor.setMeasurementTimingBudget(20000);  // minimum timing budget 20 ms
  #elif defined HIGH_ACCURACY
  // increase timing budget to 200 ms
  sensor.setMeasurementTimingBudget(200000);
  #endif

  // Start continuous back-to-back mode (take readings as
  // fast as possible).  To use continuous timed mode
  // instead, provide a desired inter-measurement period in
  // ms (e.g. sensor.startContinuous(100)).
  sensor.startContinuous();

  attachInterrupt(intPin, myinthandler, FALLING);  // define interrupt for GPI01 pin output of VL53L0X

}

void loop()
{  
  if (newData) // wait for data ready interrupt
  {
     newData = false; // reset data ready flag
     Now = micros(); // capture interrupt time
     // calculate time between last interrupt and current one, convert to sample data rate, and print to serial monitor
     Serial.print("data rate = "); Serial.print(1000000./(Now - lastUpdate)); Serial.println(" Hz");

     Serial.print(sensor.readRangeContinuousMillimeters()); // prit range in mm to serial monitor
     if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }

     Serial.println();
  }
  lastUpdate = Now;
}

/****************************************************************************************************/
/* Useful functions*/
/****************************************************************************************************/
void myinthandler()
{
  newData = true; // set the new data ready flag to true on interrupt
}

// I2C scan function
void I2Cscan()
{
// 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);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknown error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");
    
}


================================================
FILE: VL53L0x/readme.md
================================================
Sketch to read the time-of-flight ranging sensor VL53L0X with 200 cm range and better than 1% accuracy. I use [this](https://www.tindie.com/products/onehorse/vl53l0x-time-of-flight-ranging-sensor/) VL53L0X breakout board for the measurements. Get the Pololu VL53L0X library [here](https://github.com/pololu/vl53l0x-arduino).

![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-06-16T03:54:45.593Z-VL53L0X.group2.jpg.2560x2560_q85.jpg)


================================================
FILE: extras/FreeIMU_cube_UDP_ES32/AverageCompassVals.pde
================================================
/*==============================================================================
 
 Copyright (c) 2010-2013 Christopher Baker <http://christopherbaker.net>
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 
 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.
 
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
 
 https://github.com/SAIC-ATS/Algorithms.git
  
 ==============================================================================*/


int historyLength = 20;

float[] history = new float[historyLength]; // keep our history values

float[] historyX = new float[historyLength];
float[] historyY = new float[historyLength];

float averageAngle = 0;
float yamartinoAverageAngle = 0;

float currentAngle = 0;

void calculateMathematicalAverageOfHistory() {
    float sum = 0;
    float sq_sum = 0;
    for(int i = 0; i < history.length; ++i) {
       sum += history[i];
       sq_sum += history[i] * history[i];
    }
    
    averageAngle = sum / history.length;

}

void calculateYamartinoAverageOfHistory() {

  float sumX = 0;
  float sumY = 0;
  
  for (int i = 0; i < history.length; i++) {
    sumX += historyX[i];
    sumY += historyY[i];
  }

  float meanX = sumX / history.length;
  float meanY = sumY / history.length;

  yamartinoAverageAngle = myAtan2(sumY, sumX);
}

void addItemsToHistoryBuffers(float input) {
  addToHistory(history,input);
  addToHistory(historyX,cos(input));
  addToHistory(historyY,sin(input));
}

void addToHistory(float[] buffer, float input) {
  // delete the oldest value from the history
  // add one value to the history (the input)
  // take the average of the history and return it;

  // shift the values to the left in the array
  for (int i = buffer.length - 1; i >= 0; i--) {
    if (i == 0) {
      buffer[0] = input;
    } 
    else {
      buffer[i] = buffer[i-1];
    }
  }
}


float myAtan2(float y, float x) {
  float t = atan2(y, x);
  return t > 0 ? t : 2 * PI + t;
}

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/FreeIMU_cube_UDP_ES32.pde
================================================
/**
Visualize a cube which will assumes the orientation described
in a quaternion coming from the serial port.

INSTRUCTIONS: 
This program has to be run when you have the FreeIMU_serial
program running on your Arduino and the Arduino connected to your PC.
Remember to set the serialPort variable below to point to the name the
Arduino serial port has in your system. You can get the port using the
Arduino IDE from Tools->Serial Port: the selected entry is what you have
to use as serialPort variable.


Copyright (C) 2011-2012 Fabio Varesano - http://www.varesano.net/
Modified for FreeIMU-Upades Library starting in 2013-2017, Michael J Smorto -
https://github.com/mjs513/FreeIMU-Updates

This program is free software: you can redistribute it and/or modify
it under the terms of the version 3 GNU General Public License as
published by the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

// import UDP library
import hypermedia.net.*;

UDP udp;  // define the UDP object
//UDP udpS;

    String ip       = "xxx.xxx.x.xxx";  // the remote IP address of device
    int port        = 4210;    // the destination port

int BaudRate=115200;



//Settup Stop Watch
StopWatchTimer sw = new StopWatchTimer();

// These are needed for the moving average calculation
float[] data = new float[32];
float total = 0, average = 0;
int p = 0, n = 0;

//LPF
float filterFactor = 0.05;

//Moving average Heading
float corr_heading;

//set motiondetect types
float accnorm,accnorm_var_test, motionDetect1;
int accnorm_test, omegax, omegay, omegaz, omega_test, motionDetect;

//Movingaverage filters for motion detection
//float accnorm_test_avg, accnorm_var,motion_detect_ma, accnorm_avg;

MovingAverage accnorm_test_avg = new MovingAverage(5);
MovingAverage accnorm_var = new MovingAverage(7);
MovingAverage motion_detect_ma = new MovingAverage(7);
MovingAverage accnorm_avg = new MovingAverage(5);

float [] q = new float [4];
float [] acc = new float [3];
float [] gyro = new float [3];
float [] magn = new float [3];
float [] ypr = new float [3];
float temp; float press; float altitude; 
float dt, heading;

// Altitude - Accel Complimentary filter setup
float[] dyn_acc = new float[3];
float fused_alt;

// GPS Variables
float hdop, lat, longt, cog, sog, gpsalt, gpschars;
float hdop_val, loc_val, gpsalt_val, sog_val, cog_val;

float S;
float A;

String seapresscmd = "99";
float STATIONALTFT = 36.0;
float sea_press = 1013.25;            //Input local sea level pressure
float declinationAngle = -13.1603;   //Flushing, NY magnetic declination in degrees
float SEA_PRESS  = 1013.25;          //default sea level pressure level in mb
float KNOWNALT   = 65.0;            //default known altitude, 
float INHG       = 0.02952998751;    //convert mb to in/Hg constant
float MB         = 33.8638815;       //convert in/Hg to mb constant
float FTMETERS   = 0.3048;
float METERS2FT  = 3.2808399;
float PI         = 3.14159;

float [] hq = null;
float [] Euler = new float [3]; // psi, theta, phi

int lf = 10; // 10 is '\n' in ASCII
byte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)

//---------------------------------------------------
float Sample_X;
float Sample_Y; 
float Sample_Z; 
long [] Sensor_Data = new long [8]; 
short countx; short county ;

float [] accelerationx = new float [2];
float [] accelerationy = new float [2];
float [] velocityx = new float [2];
float [] velocityy = new float [2];
float [] positionX= new float [2]; 
float [] positionY = new float [2]; 
float [] positionZ = new float [2]; 
long direction; 
float sstatex; float sstatey;

float motionDetect_transition, motionDetect_old;

int calib = 0;
int cube_odo = 0;
//-------------------------------------

PFont font;
final int VIEW_SIZE_X = 900, VIEW_SIZE_Y = 600;

final int burst = 32;
int count = 0;

void myDelay(int time) {
  try {
    Thread.sleep(time);
  } catch (InterruptedException e) { }
}

void setup() 
{
  size(900, 600, OPENGL);

  // create a new datagram connection on port 6000
  // and wait for incomming message
  udp = new UDP( this, port );
//  udpS = new UDP(this, portS );
  udp.log( true ); 		// <-- printout the connection activity
  //udpS.log( true );
  udp.listen( true );
//  udpS.listen(true );

  
  // The font must be located in the sketch's "data" directory to load successfully
  font = loadFont("CourierNew36.vlw"); 
  
}

float decodeFloat(String inString) {
  byte [] inData = new byte[4];
  
  if(inString.length() == 8) {
    inData[0] = (byte) unhex(inString.substring(0, 2));
    inData[1] = (byte) unhex(inString.substring(2, 4));
    inData[2] = (byte) unhex(inString.substring(4, 6));
    inData[3] = (byte) unhex(inString.substring(6, 8));
  }
      
  int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);
  return Float.intBitsToFloat(intbits);
}

////////////////////////////////////////////////////////////////////////
/**
 * To perform any action on datagram reception, you need to implement this 
 * handler in your code. This method will be automatically called by the UDP 
 * object each time he receive a nonnull message.
 * By default, this method have just one argument (the received message as 
 * byte[] array), but in addition, two arguments (representing in order the 
 * sender IP address and his port) can be set like below.
 */
// void receive( byte[] data ) { 			// <-- default handler
void receive( byte[] data) {	// <-- extended handler
  
  // get the "real" message =
  // forget the ";\n" at the end <-- !!! only for a communication with Pd !!!
  data = subset(data, 0, data.length-2);
  String message = new String( data );
  
  // print the result
  //println( "receive: \""+message+"\" from "+ip+" on port "+port );
  
    if (message != null && message.length() > 0) {
      String [] inputStringArr = split(message, ",");
      if(inputStringArr.length >= 18) { // q1,q2,q3,q4,\r\n so we have 5 elements
        q[0] = decodeFloat(inputStringArr[0]);
        q[1] = decodeFloat(inputStringArr[1]);
        q[2] = decodeFloat(inputStringArr[2]);
        q[3] = decodeFloat(inputStringArr[3]);
        acc[0] = decodeFloat(inputStringArr[4]);
        acc[1] = decodeFloat(inputStringArr[5]);
        acc[2] = decodeFloat(inputStringArr[6]);
        gyro[0] = decodeFloat(inputStringArr[7]);
        gyro[1] = decodeFloat(inputStringArr[8]);
        gyro[2] = decodeFloat(inputStringArr[9]);
        magn[0] = decodeFloat(inputStringArr[10]);
        magn[1] = decodeFloat(inputStringArr[11]);    
        magn[2] = decodeFloat(inputStringArr[12]);
        temp = decodeFloat(inputStringArr[13]);
        press = decodeFloat(inputStringArr[14]);
        //dt = (1./decodeFloat(inputStringArr[15]))/4;
        dt = (1./decodeFloat(inputStringArr[15]));
        heading = decodeFloat(inputStringArr[16]);
        //dt = tnew - told;
        //told = tnew;
        if(heading < -9990) {
            heading = 0;
        }
        altitude = decodeFloat(inputStringArr[17]);
        motionDetect1 = decodeFloat(inputStringArr[18]);
      }
    }
}

/** 
 * on key pressed event:
 * send the current key value over the network
 */
void keyPressed() {
    
    String message  = str( key );	// the message to send

    if(key == 's'){
       udp.send("s" + "\n", ip, port);
    } else if(key == 'g'){
      udp.send( "g" + "\n", ip, port ); 
    } else if(key == 'v'){
      udp.send( "v" + "\n", ip, port ); 
    } else if(key == '1'){
      udp.send( "1" + "\n", ip, port ); 
    } else if(key == '2'){
      udp.send( "2" + "\n", ip, port );
    } else if(key == 't'){
      udp.send( "t" + "\n", ip, port ); 
    } else if(key == 'f'){
      udp.send( "f" + ";\n", ip, port ); 
    } else if(key == 'x'){
      udp.send( "x" + "\n", ip, port ); 
    } else {
      println("NOT VALID COMMAND");
    }

  if(key == 'h') {
    println("pressed h");
    
    // set hq the home quaternion as the quatnion conjugate coming from the sensor fusion
    hq = quatConjugate(q);
    sw.start();
  }
  else if(key == 'n') {
    println("pressed n");
    hq = null;
  }
  else if(key == 'w') {
    println("pressed w"); 
    sw.start();
  }  
}

void buildBoxShape() {
  //box(60, 10, 40);
  noStroke();
  beginShape(QUADS);
  
  //Z+ (to the drawing area)
  fill(#00ff00);
  vertex(-30, -5, 20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);
  
  //Z-
  fill(#0000ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, 5, -20);
  vertex(-30, 5, -20);
  
  //X-
  fill(#ff0000);
  vertex(-30, -5, -20);
  vertex(-30, -5, 20);
  vertex(-30, 5, 20);
  vertex(-30, 5, -20);
  
  //X+
  fill(#ffff00);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(30, 5, 20);
  vertex(30, 5, -20);
  
  //Y-
  fill(#ff00ff);
  vertex(-30, -5, -20);
  vertex(30, -5, -20);
  vertex(30, -5, 20);
  vertex(-30, -5, 20);
  
  //Y+
  fill(#00ffff);
  vertex(-30, 5, -20);
  vertex(30, 5, -20);
  vertex(30, 5, 20);
  vertex(-30, 5, 20);
  
  endShape();
}


void drawCube() {  
  pushMatrix();
    translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);
    scale(5,5,5);
    
    // a demonstration of the following is at 
    // http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube
    rotateZ(-Euler[2]);
    rotateX(-Euler[1]);
    rotateY(-Euler[0]);
    
    buildBoxShape();
    
  popMatrix();
}


void draw() {
  background(#000000);
  fill(#ffffff);
  
  
  if(hq != null) { // use home quaternion
    quaternionToEuler(quatProd(hq, q), Euler);
	//println(quatProd(hq,q));println();
    text("Disable home position by pressing \"n\"", 20, VIEW_SIZE_Y - 30);
  }
  else {
    quaternionToEuler(q, Euler);
    text("Point FreeIMU's X axis to your monitor then press \"h\"", 20, VIEW_SIZE_Y - 30);
  }

  fused_alt = altitude + STATIONALTFT/METERS2FT;

  text("Temp: " + temp + "\n" + "Press: " + press + "\n" + "   Alt: " + nfp((fused_alt),3,2), 20, VIEW_SIZE_Y - 110);
  text("DeltaT: " + dt, 180, VIEW_SIZE_Y - 110);
  
  textFont(font, 20);
  textAlign(LEFT, TOP);
  text("Q:\n" + q[0] + "\n" + q[1] + "\n" + q[2] + "\n" + q[3], 20, 20);
  text("Euler Angles:\nYaw (psi)  : " + nfp(degrees(Euler[0]),3,2) + "\nPitch (theta): " + nfp(degrees(Euler[1]),3,2) + "\nRoll (phi)  : " + nfp(degrees(Euler[2]),3,2), 200, 20);

  //Compass averaging
  //currentAngle = myAtan2(mouseY-height/2, mouseX-width/2) + radians(myNoise); 
  addItemsToHistoryBuffers(radians(heading));
  calculateMathematicalAverageOfHistory();
  calculateYamartinoAverageOfHistory(); 
  
  //corr_heading = heading;
  corr_heading = degrees(yamartinoAverageAngle);
  
  text("Heading " + nfp(((corr_heading)),4,1),400,20); 

  text( "Elapsed Time: " + sw.hour() + ":" + sw.minute() + ":" + sw.second(), 500, 40);
  
  text("Acc:\n" + nfp(acc[0],1,6) + "\n" + nfp(acc[1],1,6) + "\n" + nfp(acc[2],1,6) + "\n", 20, 130);
  text("Gyro:\n" + nfp(gyro[0],1,6) + "\n" + nfp(gyro[1],1,6) + "\n" + nfp(gyro[2],1,6) + "\n", 20, 220);
  text("Magn:\n" + nfp(magn[0],1,6) + "\n" + nfp(magn[1],1,6) + "\n" + nfp(magn[2],1,6) + "\n", 20, 310);


  text(MotionDetect(),VIEW_SIZE_X-125,VIEW_SIZE_Y-125) ;
  if(MotionDetect() > 0 ){
    fill(#FF0000);
  } else {
    fill(#FFFFFF)
  ; }
  rect(VIEW_SIZE_X-100,VIEW_SIZE_Y-100,50,50);

  if(cube_odo == 0) { 
     drawCube();
    } else {
	position();
        text("px:  " + positionX[0] + "\n" + "py:  " + positionY[0], 200, 200);
   }
  //myPort.write("q" + 1);
}

// See Sebastian O.H. Madwick report 
// "An efficient orientation filter for inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation

void quaternionToEuler(float [] q, float [] euler) {
  euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
  euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
  euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
}

float [] quatProd(float [] a, float [] b) {
  float [] q = new float[4];
  
  q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
  q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
  q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
  q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
  
  return q;
}

// returns a quaternion from an axis angle representation
float [] quatAxisAngle(float [] axis, float angle) {
  float [] q = new float[4];
  
  float halfAngle = angle / 2.0;
  float sinHalfAngle = sin(halfAngle);
  q[0] = cos(halfAngle);
  q[1] = -axis[0] * sinHalfAngle;
  q[2] = -axis[1] * sinHalfAngle;
  q[3] = -axis[2] * sinHalfAngle;
  
  return q;
}

// return the quaternion conjugate of quat
float [] quatConjugate(float [] quat) {
  float [] conj = new float[4];
  
  conj[0] = quat[0];
  conj[1] = -quat[1];
  conj[2] = -quat[2];
  conj[3] = -quat[3];
  
  return conj;
}

void getYawPitchRollRad() {
  //float q[4]; // quaternion
  float gx, gy, gz; // estimated gravity direction
  
  gx = 2 * (q[1]*q[3] - q[0]*q[2]);
  gy = 2 * (q[0]*q[1] + q[2]*q[3]);
  gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
  
  ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
  ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
  ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
}

//=============================================================
void gravityCompensateDynAcc() {
  float[] g = new float[3];
  
  // get expected direction of gravity in the sensor frame
  g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
  g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  
  // compensate accelerometer readings with the expected direction of gravity
  dyn_acc[0] = acc[0] - g[0];
  dyn_acc[1] = acc[1] - g[1];
  dyn_acc[2] = acc[2] - g[2];
}
 

//=============================================================
// converted from Michael Shimniok Data Bus code
// http://mbed.org/users/shimniok/code/AVC_20110423/

float clamp360(float x) {
    while ((x) >= 360.0) (x) -= 360.0; 
    while ((x) < 0) (x) += 360.0; 
    return x;
}



//==============================================================
//
float HeadingAvgCorr(float newx, float oldx) {
    while ((newx + 180.0) < oldx) (newx) += 360.0;
    while ((newx - 180.0) > oldx) (newx) -= 360.0;
    while ((newx) == 360.0) (newx) = 0.0;
    return newx;
}

//==============================================================
//SMA filter
// Use the next value and calculate the 
// moving average 
public void AddNewValue(float value){
  total -= data[p];
  data[p] = value;
  total += value;
  p = ++p % data.length;
  if(n < data.length) n++;
  average = total / n;
} 

//=======================================
public float iround(float number, float decimal) {
  int ix;
  ix = round(number*pow(10, decimal));
  return float(ix)/pow(10, decimal);
} 

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/Kalman.pde
================================================
public class MyKalman {
//private float Q = 0.001;
//private float R = 0.1;
private float Q = 0.0000005;
private float R = 0.01;
private float P = 1, X = 0, K;
 
private void measurementUpdate(){
K = (P + Q) / (P + Q + R);
P = R * (P + Q) / (R + P + Q);
}
 
public float update(float measurement){
measurementUpdate();
float result = X + (measurement - X) * K;
X = result;
 
return result;
}
} 

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/MathUtils.java
================================================

/**
 *                     ProScene (version 1.1.1)      
 *    Copyright (c) 2010-2012 by National University of Colombia
 *                 @author Jean Pierre Charalambos      
 *           http://www.disi.unal.edu.co/grupos/remixlab/
 *                           
 * This java package provides classes to ease the creation of interactive 3D
 * scenes in Processing.
 * 
 * This source file is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation; either version 3 of the License, or (at your option)
 * any later version.
 * 
 * This code is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 * 
 * A copy of the GNU General Public License is available on the World Wide Web
 * at <http://www.gnu.org/copyleft/gpl.html>. You can also obtain it by
 * writing to the Free Software Foundation, 51 Franklin Street, Suite 500
 * Boston, MA 02110-1335, USA.
 */


import processing.core.*;

/**
 * Utility class that complements the PVector and PMatrix classes.
 */
public class MathUtils /**implements PConstants*/ {
	/**
	 * Utility function that simply projects {@code src} on the axis of direction
	 * {@code direction} that passes through the origin.
	 * <p>
	 * {@code direction} does not need to be normalized (but must be non null).
	 */
	public static PVector projectVectorOnAxis(PVector src, PVector direction) {
		float directionSquaredNorm = squaredNorm(direction);
		if (directionSquaredNorm < 1E-10f)
			throw new RuntimeException("Direction squared norm is nearly 0");

		float modulation = src.dot(direction) / directionSquaredNorm;
		return PVector.mult(direction, modulation);
	}

	/**
	 * Utility function that simply projects {@code src} on the plane whose normal
	 * is {@code normal} that passes through the origin.
	 * <p>
	 * {@code normal} does not need to be normalized (but must be non null).
	 */
	public static PVector projectVectorOnPlane(PVector src, PVector normal) {
		float normalSquaredNorm = squaredNorm(normal);
		if (normalSquaredNorm < 1E-10f)
			throw new RuntimeException("Normal squared norm is nearly 0");

		float modulation = src.dot(normal) / normalSquaredNorm;
		return PVector.sub(src, PVector.mult(normal, modulation));
	}

	/**
	 * Utility function that returns the squared norm of the PVector.
	 */
	public static float squaredNorm(PVector v) {
		return (v.x * v.x) + (v.y * v.y) + (v.z * v.z);
	}

	/**
	 * Utility function that returns a PVector orthogonal to {@code v}. Its
	 * {@code mag()} depends on the PVector, but is zero only for a {@code null}
	 * PVector. Note that the function that associates an {@code
	 * orthogonalVector()} to a PVector is not continuous.
	 */
	public static PVector orthogonalVector(PVector v) {
		// Find smallest component. Keep equal case for null values.
		if ((PApplet.abs(v.y) >= 0.9f * PApplet.abs(v.x))
				&& (PApplet.abs(v.z) >= 0.9f * PApplet.abs(v.x)))
			return new PVector(0.0f, -v.z, v.y);
		else if ((PApplet.abs(v.x) >= 0.9f * PApplet.abs(v.y))
				&& (PApplet.abs(v.z) >= 0.9f * PApplet.abs(v.y)))
			return new PVector(-v.z, 0.0f, v.x);
		else
			return new PVector(-v.y, v.x, 0.0f);
	}

	/**
	 * Utility function that returns the PMatrix3D representation of the 4x4
	 * {@code m} given in European format.
	 */
	public static final PMatrix3D fromMatrix(float[][] m) {
		return new PMatrix3D(m[0][0], m[0][1], m[0][2], m[0][3], m[1][0], m[1][1],
				m[1][2], m[1][3], m[2][0], m[2][1], m[2][2], m[2][3], m[3][0], m[3][1],
				m[3][2], m[3][3]);
	}

	/**
	 * Utility function that returns the PMatrix3D representation of the 4x4
	 * {@code matrix} given in OpenGL format.
	 */
	public static final PMatrix3D fromOpenGLMatrix(float[][] matrix) {
		return fromMatrix(transpose4x4Matrix(matrix));
	}

	/**
	 * Utility function that returns the PMatrix3D representation of the 16
	 * {@code array} given in European format.
	 */
	public static final PMatrix3D fromArray(float[] a) {
		return new PMatrix3D(a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8],
				a[9], a[10], a[11], a[12], a[13], a[14], a[15]);
	}

	/**
	 * Utility function that returns the PMatrix3D representation of the 16
	 * {@code array} given in OpenGL format.
	 */
	public static final PMatrix3D fromOpenGLArray(float[] array) {
		return fromArray(from4x4MatrixToArray(transpose4x4Matrix(fromArrayTo4x4Matrix(array))));
	}

	/**
	 * Utility function that returns the [4][4]float matrix representation
	 * (European format) of the given PMatrix3D.
	 */
	public static final float[][] toMatrix(PMatrix3D pM) {
		float[] array = new float[16];
		pM.get(array);
		return fromArrayTo4x4Matrix(array);
	}

	/**
	 * Utility function that returns the [16]array representation (OpenGL format)
	 * of the given PMatrix3D.
	 */
	public static final float[] toOpenGLArray(PMatrix3D pM) {
		return from4x4MatrixToArray(toOpenGLMatrix(pM));
	}

	/**
	 * Utility function that returns the [4][4]float matrix representation (OpenGL
	 * format) of the given PMatrix3D.
	 */
	public static final float[][] toOpenGLMatrix(PMatrix3D pM) {
		return transpose4x4Matrix(toMatrix(pM));
	}

	/**
	 * Utility function that returns the transpose of the given 3X3 matrix.
	 */
	public static final float[][] transpose3x3Matrix(float[][] m) {
		float[][] matrix = new float[4][4];
		for (int i = 0; i < 3; ++i)
			for (int j = 0; j < 3; ++j)
				matrix[i][j] = m[j][i];
		return matrix;
	}

	/**
	 * Utility function that returns the transpose of the given 4X4 matrix.
	 */
	public static final float[][] transpose4x4Matrix(float[][] m) {
		float[][] matrix = new float[4][4];
		for (int i = 0; i < 4; ++i)
			for (int j = 0; j < 4; ++j)
				matrix[i][j] = m[j][i];
		return matrix;
	}

	/**
	 * Utility function that returns the [4][4] float matrix version of the given
	 * {@code m} array.
	 */
	public static final float[][] fromArrayTo4x4Matrix(float[] m) {
		// m should be of size [16]
		float[][] mat = new float[4][4];
		for (int i = 0; i < 4; ++i)
			for (int j = 0; j < 4; ++j)
				mat[i][j] = m[i * 4 + j];
		return mat;
	}

	/**
	 * Utility function that returns the [16] float array version of the given
	 * {@code mat} matrix.
	 */
	public static final float[] from4x4MatrixToArray(float[][] mat) {
		float[] m = new float[16];
		for (int i = 0; i < 4; ++i)
			for (int j = 0; j < 4; ++j)
				m[i * 4 + j] = mat[i][j];
		return m;
	}

	/**
	 * Utility function that returns the 3x3 upper left sub-matrix of the given
	 * PMatrix3D.
	 */
	public static final float[][] get3x3UpperLeftMatrixFromPMatrix3D(PMatrix3D pM) {
		float[][] m = new float[3][3];
		m[0][0] = pM.m00;
		m[0][1] = pM.m01;
		m[0][2] = pM.m02;
		m[1][0] = pM.m10;
		m[1][1] = pM.m11;
		m[1][2] = pM.m12;
		m[2][0] = pM.m20;
		m[2][1] = pM.m21;
		m[2][2] = pM.m22;
		return m;
	}	
}

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/MotionDetect.pde
================================================
public float MotionDetect() {
  
    /*###################################################################
    
       accelerationsquared euclidean norm analysis
    
    ################################################################### */
    accnorm = (acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
    //if((accnorm >=0.96) && (accnorm <= 0.99)){
    if((accnorm >=0.94) && (accnorm <= 1.03)){  
        accnorm_test = 0;
    } else {
        accnorm_test = 1; }
    //take average of 5 to 10 points
    accnorm_avg.newNum(accnorm);
    accnorm_test_avg.newNum(accnorm_test);

    /* ####################################################################
    #
    #   squared norm analysis to determine suddenly changes signal
    #
    ##################################################################### */
    accnorm_var.newNum(sq(accnorm-accnorm_avg.getAvg()));
    if(accnorm_var.getAvg() < 0.0005) {
        accnorm_var_test = 0;
    }else {
        accnorm_var_test = 1; }

    /*####################################################################
    #
    #   angular rate analysis in order to disregard linear acceleration
    #
    ################################################################### */
    if ((gyro[0] >=-0.005) && (gyro[0] <= 0.005)) {
        omegax = 0;
    } else {
        omegax = 1; }
        
    if((gyro[1] >= -0.005) && (gyro[1] <= 0.005)) {
        omegay = 0;
    } else {
        omegay = 1; }
        
    if((gyro[2] >= -0.005) && (gyro[2] <= 0.005)) {
        omegaz = 0;
    } else {
        omegaz = 1; }
        
    if ((omegax+omegay+omegaz) > 0) {
        omega_test = 1;
    } else {
        omega_test = 0; }


    /* ####################################################################
    #
    # combined movement detector
    #
    #################################################################### */
    if ((accnorm_test_avg.getAvg() + omega_test + accnorm_var_test) > 0) {
        motionDetect = 1;
    } else {
        motionDetect = 0; }
    
    //####################################################################    
    
    motion_detect_ma.newNum(motionDetect);
    
    if(motion_detect_ma.getAvg() > 0.5) {
      return 1;
    } else {
      return 0; }
      
     //return omegaz;
}

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/ODO.pde
================================================

/**************************************************************************
//This function allows movement end detection. If a certain number of 
//acceleration samples are  equal to zero we can assume movement has stopped.
//Accumulated Error generated in the velocity calculations is eliminated by 
//resetting the velocity variables. This stops position increment and greatly
//eliminates position error.
//***************************************************************************/  

void movement_end_check()   
{

  if (accelerationx[1] == 0)   //we count the number of acceleration samples that equals zero 
{ countx++;}
  else { countx = 0;}

  if (countx >= 25)             //if this number exceeds 25, we can assume that velocity is zero
   {    velocityx[1] = 0;
        velocityx[0] = 0;
    }
   
   if (accelerationy[1] == 0)   //we do the same for the Y axis
   { county++;}
   else {  county = 0;}
  
  if (county >= 25)
   { 
     velocityy[1] = 0;
     velocityy[0] = 0;
    } 
}

//***********************************************************************
//***********************************************************************
//This function transforms acceleration to a proportional position by 
//integrating the acceleration data twice. It also adjusts sensibility by   
//multiplying the "positionX" and "positionY" variables. 
//This integration algorithm carries error, which is compensated in the 
//"movenemt end check" subroutine. Faster sampling frequency implies less 
//error but requires more memory. Keep in  mind that the same process is 
//applied to the X and Y axis. 
//******************************************************************************/  
 
void position()  
{
  int count2=0;
    
  do{
     //ADC GetAllAxis();
     gravity_compensate();
     
     accelerationx[0] = accelerationx[0] + Sample_X; //filtering routine for noise attenuation
     accelerationy[0] = accelerationy[0] + Sample_Y; //64 samples are averaged. The resulting 
     //average represents the acceleration of 
     //an instant
      count2++;                                       

    } while (count2 != 0x40);                   // 64 sums of the acceleration sample
  

  accelerationx[0]= accelerationx[0]/64;       // division by 64
  accelerationy[0]= accelerationy[0]/64;
  
  accelerationx[0] = accelerationx[0] - (int)sstatex;  //eliminating zero reference 
                                                       //offset of the acceleration data
  accelerationy[0] = accelerationy[0] - (int)sstatey;  //to obtain positive and negative 
                                                       //acceleration
  
  if ((accelerationx[0] <=0.010)&&(accelerationx[0] >= -0.010))  //Discrimination window applied 
          {accelerationx[0] = 0;}                        // to the X axis acceleration 
                                                         //variable
               
  if ((accelerationy[0] <=0.010) && (accelerationy[0] >= -0.010)) 
                {accelerationy[0] = 0;} 
  
  //first X integration:
  velocityx[0]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] - accelerationx[0] )/2);  

  //second X integration:
  positionX[0]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])/2);

  //first Y integration:       
  velocityy[0] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])/2);

  //second Y integration:       
  positionY[0] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])/2);  

  accelerationx[1] = accelerationx[0];   //The current acceleration value must be sent 
                                         //to the previous acceleration 
  accelerationy[1] = accelerationy[0];   //variable in order to introduce the new 
                                         //acceleration value.
    
  velocityx[1] = velocityx[0];         //Same done for the velocity variable
  velocityy[1] = velocityy[0];
  
  /* positionX[1] = positionX[1]*262144;       //The idea behind this shifting (multiplication) 
                                         //is a sensibility adjustment. <<18
  positionY[1] = positionY[1]*262144;      //Some applications require adjustments to a 
                                        //particular situation 
                                        //i.e. mouse application 
  */
  
  //data_transfer();
  
  /*  
  positionX[1] = positionX[1]/262144;      //once the variables are sent them must return to 
  positionY[1] = positionY[1]/262144;      //their original state
  */
  //text("px:  " + positionX[0] + "\n" + "py:  " + positionY[0], 200, 200);
  
  movement_end_check();
  
  /*  
  positionX[0] = positionX[1];          //actual position data must be sent to the  
  positionY[0] = positionY[1];     //previous position
  */  

  direction = 0;                        // data variable to direction variable reset


}    

//*****************************************************************************/

// compensate the accelerometer readings from gravity. 
// @param q the quaternion representing the orientation of a 9DOM MARG sensor array
// @param acc the readings coming from an accelerometer expressed in g
//
// Code snippet from Fabio Versano - blog on same topic
//
// @return a 3d vector representing dynamic acceleration expressed in g
void gravity_compensate(){
  float [] g = {0.0, 0.0, 0.0};
  
  // get expected direction of gravity
  g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
  g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  
  // compensate accelerometer readings with the expected direction of gravity
  //return [acc[0] - g[0], acc[1] - g[1], acc[2] - g[2]]
  Sample_X = acc[0] - g[0];
  Sample_Y = acc[1] - g[1];

 }

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/Quaternion.java
================================================

/**
 *                     ProScene (version 1.1.1)      
 *    Copyright (c) 2010-2012 by National University of Colombia
 *                 @author Jean Pierre Charalambos      
 *           http://www.disi.unal.edu.co/grupos/remixlab/
 *                           
 * This java package provides classes to ease the creation of interactive 3D
 * scenes in Processing.
 * 
 * This source file is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation; either version 3 of the License, or (at your option)
 * any later version.
 * 
 * This code is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 * 
 * A copy of the GNU General Public License is available on the World Wide Web
 * at <http://www.gnu.org/copyleft/gpl.html>. You can also obtain it by
 * writing to the Free Software Foundation, 51 Franklin Street, Suite 500
 * Boston, MA 02110-1335, USA.
 */


import processing.core.*;

/**
 * A 4 element unit quaternion represented by single precision floating point
 * x,y,z,w coordinates.
 * 
 */

public class Quaternion extends PApplet {
	/**
	 * The x coordinate, i.e., the x coordinate of the vector part of the
	 * Quaternion.
	 */
	public float x;

	/**
	 * The y coordinate, i.e., the y coordinate of the vector part of the
	 * Quaternion.
	 */
	public float y;

	/**
	 * The z coordinate, i.e., the z coordinate of the vector part of the
	 * Quaternion.
	 */
	public float z;

	/**
	 * The w coordinate which corresponds to the scalar part of the Quaternion.
	 */
	public float w;

	/**
	 * Constructs and initializes a Quaternion to (0.0,0.0,0.0,1.0), i.e., an
	 * identity rotation.
	 */
	public Quaternion () {
		this.x = 0;
		this.y = 0;
		this.z = 0;
		this.w = 1;
	}

	/**
	 * Default constructor for Quaternion(float x, float y, float z, float w,
	 * boolean normalize), with {@code normalize=true}.
	 * 
	 */
	public Quaternion (float x, float y, float z, float w) {
		this(x, y, z, w, true);
	}

	/**
	 * Constructs and initializes a Quaternion from the specified xyzw
	 * coordinates.
	 * 
	 * @param x
	 *          the x coordinate
	 * @param y
	 *          the y coordinate
	 * @param z
	 *          the z coordinate
	 * @param w
	 *          the w scalar component
	 * @param normalize
	 *          tells whether or not the constructed Quaternion should be
	 *          normalized.
	 */
	public Quaternion(float x, float y, float z, float w, boolean normalize) {
		if (normalize) {
			float mag = PApplet.sqrt(x * x + y * y + z * z + w * w);
			if (mag > 0.0f) {
				this.x = x / mag;
				this.y = y / mag;
				this.z = z / mag;
				this.w = w / mag;
			} else {
				this.x = 0f;
				this.y = 0f;
				this.z = 0f;
				this.w = 1f;
			}
		} else {
			this.x = x;
			this.y = y;
			this.z = z;
			this.w = w;
		}
	}

	/**
	 * Default constructor for Quaternion(float[] q, boolean normalize) with
	 * {@code normalize=true}.
	 * 
	 */
	public Quaternion(float[] q) {
		this(q, true);
	}

	/**
	 * Constructs and initializes a Quaternion from the array of length 4.
	 * 
	 * @param q
	 *          the array of length 4 containing xyzw in order
	 */
	public Quaternion(float[] q, boolean normalize) {
		if (normalize) {
			float mag = PApplet.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3]
					* q[3]);
			if (mag > 0.0f) {
				this.x = q[0] / mag;
				this.y = q[1] / mag;
				this.z = q[2] / mag;
				this.w = q[3] / mag;
			} else {
				this.x = 0;
				this.y = 0;
				this.z = 0;
				this.w = 1;
			}
		} else {
			this.x = q[0];
			this.y = q[1];
			this.z = q[2];
			this.w = q[3];
		}
	}

	/**
	 * Copy constructor.
	 * 
	 * @param q1
	 *          the Quaternion containing the initialization x y z w data
	 */
	public Quaternion(Quaternion q1) {
		set(q1);
	}

	/**
	 * Copy constructor. If {@code normalize} is {@code true} this Quaternion is
	 * {@link #normalize()}.
	 * 
	 * @param q1
	 *          the Quaternion containing the initialization x y z w data
	 */
	public Quaternion(Quaternion q1, boolean normalize) {
		set(q1, normalize);
	}

	/**
	 * Convenience function that simply calls {@code set(q1, true);}
	 * 
	 * @see #set(Quaternion, boolean)
	 */
	public void set(Quaternion q1) {
		set(q1, true);
	}

	/**
	 * Set this Quaternion from quaternion {@code q1}. If {@code normalize} is
	 * {@code true} this Quaternion is {@link #normalize()}.
	 */
	public void set(Quaternion q1, boolean normalize) {
		this.x = q1.x;
		this.y = q1.y;
		this.z = q1.z;
		this.w = q1.w;
		if (normalize)
			this.normalize();
	}

	/**
	 * Constructs and initializes a Quaternion from the specified rotation
	 * {@link #axis() axis} (non null) and {@link #angle() angle} (in radians).
	 * 
	 * @param axis
	 *          the PVector representing the axis
	 * @param angle
	 *          the angle in radians
	 * 
	 * @see #fromAxisAngle(PVector, float)
	 */
	public Quaternion(PVector axis, float angle) {
		fromAxisAngle(axis, angle);
	}

	/**
	 * Constructs a Quaternion that will rotate from the {@code from} direction to
	 * the {@code to} direction.
	 * 
	 * @param from
	 *          the first PVector
	 * @param to
	 *          the second PVector
	 * 
	 * @see #fromTo(PVector, PVector)
	 */
	public Quaternion(PVector from, PVector to) {
		fromTo(from, to);
	}

	/**
	 * Sets the value of this Quaternion to the conjugate of itself.
	 */
	public final void conjugate() {
		this.x = -this.x;
		this.y = -this.y;
		this.z = -this.z;
	}

	/**
	 * Sets the value of this Quaternion to the conjugate of Quaternion q1.
	 * 
	 * @param q1
	 *          the source vector
	 */
	public final void conjugate(Quaternion q1) {
		this.x = -q1.x;
		this.y = -q1.y;
		this.z = -q1.z;
		this.w = q1.w;
	}

	/**
	 * Negates all the coefficients of the Quaternion.
	 */
	public final void negate() {
		this.x = -this.x;
		this.y = -this.y;
		this.z = -this.z;
		this.w = -this.w;
	}

	/**
	 * Returns the "dot" product of this Quaternion and {@code b}:
	 * <p>
	 * {@code this.x * b.x + this.y * b.y + this.z * b.z + this.w * b.w}
	 * 
	 * @param b
	 *          the Quaternion
	 */
	public final float dotProduct(Quaternion b) {
		return this.x * b.x + this.y * b.y + this.z * b.z + this.w * b.w;
	}

	/**
	 * Returns the "dot" product of {@code a} and {@code b}:
	 * <p>
	 * {@code a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w}
	 * 
	 * @param a
	 *          the first Quaternion
	 * @param b
	 *          the second Quaternion
	 */
	public final static float dotProduct(Quaternion a, Quaternion b) {
		return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
	}

	/**
	 * Sets the value of this Quaternion to the Quaternion product of itself and
	 * {@code q1}, (i.e., {@code this = this * q1}).
	 * 
	 * @param q1
	 *          the other Quaternion
	 */
	public final void multiply(Quaternion q1) {
		float x, y, w;

		w = this.w * q1.w - this.x * q1.x - this.y * q1.y - this.z * q1.z;
		x = this.w * q1.x + q1.w * this.x + this.y * q1.z - this.z * q1.y;
		y = this.w * q1.y + q1.w * this.y - this.x * q1.z + this.z * q1.x;
		this.z = this.w * q1.z + q1.w * this.z + this.x * q1.y - this.y * q1.x;
		this.w = w;
		this.x = x;
		this.y = y;
	}

	/**
	 * Returns the Quaternion which is product of quaternions {@code q1} and
	 * {@code q2}.
	 * 
	 * @param q1
	 *          the first Quaternion
	 * @param q2
	 *          the second Quaternion
	 */
	public final static Quaternion multiply(Quaternion q1, Quaternion q2) {
		float x, y, z, w;
		w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
		x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
		y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
		z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
		return new Quaternion(x, y, z, w);
	}

	/**
	 * Returns the image of {@code v} by the rotation of this vector. Same as
	 * {@code this.rotate(v).}
	 * 
	 * @param v
	 *          the PVector
	 * 
	 * @see #rotate(PVector)
	 * @see #inverseRotate(PVector)
	 */
	public final PVector multiply(PVector v) {
		return this.rotate(v);
	}

	/**
	 * Returns the image of {@code v} by the rotation {@code q1}. Same as {@code
	 * q1.rotate(v).}
	 * 
	 * @param q1
	 *          the Quaternion
	 * 
	 * @param v
	 *          the PVector
	 * 
	 * @see #rotate(PVector)
	 * @see #inverseRotate(PVector)
	 */
	public static final PVector multiply(Quaternion q1, PVector v) {
		return q1.rotate(v);
	}

	/**
	 * Multiplies this Quaternion by the inverse of Quaternion {@code q1} and
	 * places the value into this Quaternion (i.e., {@code this = this * q^-1}).
	 * The value of the argument Quaternion is preserved.
	 * 
	 * @param q1
	 *          the other Quaternion
	 */
	public final void multiplyInverse(Quaternion q1) {
		Quaternion tempQuat = new Quaternion(q1);
		tempQuat.invert();
		this.multiply(tempQuat);
	}

	/**
	 * Returns the product of Quaternion {@code q1} by the inverse of Quaternion
	 * {@code q2} (i.e., {@code q1 * q2^-1}). The value of both argument
	 * quaternions is preserved.
	 * 
	 * @param q1
	 *          the first Quaternion
	 * @param q2
	 *          the second Quaternion
	 */
	public static final Quaternion multiplyInverse(Quaternion q1, Quaternion q2) {
		Quaternion tempQuat = new Quaternion(q2);
		tempQuat.invert();
		return Quaternion.multiply(q1, tempQuat);
	}

	/**
	 * Returns the inverse Quaternion (inverse rotation).
	 * <p>
	 * The result has a negated {@link #axis()} direction and the same
	 * {@link #angle()}.
	 * <p>
	 * A composition of a Quaternion and its {@link #inverse()} results in an
	 * identity function. Use {@link #invert()} to actually modify the Quaternion.
	 * 
	 * @see #invert()
	 */
	public final Quaternion inverse() {
		Quaternion tempQuat = new Quaternion(this);
		tempQuat.invert();
		return tempQuat;
	}

	/**
	 * Sets the value of this Quaternion to the inverse of itself.
	 * 
	 * @see #inverse()
	 */
	public final void invert() {
		float sqNorm = squaredNorm(this);
		this.w /= sqNorm;
		this.x /= -sqNorm;
		this.y /= -sqNorm;
		this.z /= -sqNorm;
	}

	/**
	 * Sets the value of this Quaternion to the Quaternion inverse of {@code q1}.
	 * 
	 * @param q1
	 *          the Quaternion to be inverted
	 */
	public final void invert(Quaternion q1) {
		float sqNorm = squaredNorm(q1);
		this.w = q1.w / sqNorm;
		this.x = -q1.x / sqNorm;
		this.y = -q1.y / sqNorm;
		this.z = -q1.z / sqNorm;
	}

	/**
	 * Normalizes the value of this Quaternion in place and return its {@code
	 * norm}.
	 */
	public final float normalize() {
		float norm = PApplet.sqrt(this.x * this.x + this.y * this.y + this.z
				* this.z + this.w * this.w);
		if (norm > 0.0f) {
			this.x /= norm;
			this.y /= norm;
			this.z /= norm;
			this.w /= norm;
		} else {
			this.x = (float) 0.0;
			this.y = (float) 0.0;
			this.z = (float) 0.0;
			this.w = (float) 1.0;
		}
		return norm;
	}

	/**
	 * Returns the image of {@code v} by the Quaternion rotation.
	 * 
	 * @param v
	 *          the PVector
	 */
	public final PVector rotate(PVector v) {
		float q00 = 2.0f * x * x;
		float q11 = 2.0f * y * y;
		float q22 = 2.0f * z * z;

		float q01 = 2.0f * x * y;
		float q02 = 2.0f * x * z;
		float q03 = 2.0f * x * w;

		float q12 = 2.0f * y * z;
		float q13 = 2.0f * y * w;

		float q23 = 2.0f * z * w;

		return new PVector((1.0f - q11 - q22) * v.x + (q01 - q23) * v.y
				+ (q02 + q13) * v.z, (q01 + q23) * v.x + (1.0f - q22 - q00) * v.y
				+ (q12 - q03) * v.z, (q02 - q13) * v.x + (q12 + q03) * v.y
				+ (1.0f - q11 - q00) * v.z);
	}

	/**
	 * Returns the image of {@code v} by the Quaternion {@link #inverse()}
	 * rotation.
	 * <p>
	 * {@link #rotate(PVector)} performs an inverse transformation.
	 * 
	 * @param v
	 *          the PVector
	 */
	public final PVector inverseRotate(PVector v) {
		Quaternion tempQuat = new Quaternion(x, y, z, w);
		tempQuat.invert();
		return tempQuat.rotate(v);
	}

	/**
	 * Sets the Quaternion as a rotation of {@link #axis() axis} and
	 * {@link #angle() angle} (in radians).
	 * <p>
	 * The {@code axis} does not need to be normalized. A null {@code axis} will
	 * result in an identity Quaternion.
	 * 
	 * @param axis
	 *          the PVector representing the axis
	 * @param angle
	 *          the angle in radians
	 */
	public void fromAxisAngle(PVector axis, float angle) {
		float norm = axis.mag();
		if (norm < 1E-8f) {
			// Null rotation
			this.x = 0.0f;
			this.y = 0.0f;
			this.z = 0.0f;
			this.w = 1.0f;
		} else {
			float sin_half_angle = PApplet.sin(angle / 2.0f);
			this.x = sin_half_angle * axis.x / norm;
			this.y = sin_half_angle * axis.y / norm;
			this.z = sin_half_angle * axis.z / norm;
			this.w = PApplet.cos(angle / 2.0f);
		}
	}

	/**
	 * Same as {@link #fromEulerAngles(PVector)}.
	 */
	public void fromTaitBryan(PVector angles) {
		fromEulerAngles(angles);
	}

	/**
	 * Same as {@link #fromEulerAngles(float, float, float)}.
	 */
	public void fromTaitBryan(float roll, float pitch, float yaw) {
		fromEulerAngles(roll, pitch, yaw);
	}

	/**
	 * Convenience function that simply calls {@code fromEulerAngles(angles.x,
	 * angles.y, angles.z)}.
	 * 
	 * @see #fromEulerAngles(float, float, float)
	 * @see #eulerAngles()
	 */
	public void fromEulerAngles(PVector angles) {
		fromEulerAngles(angles.x, angles.y, angles.z);
	}

	/**
	 * Converts Euler rotation angles {@code roll}, {@code pitch} and {@code yaw},
	 * respectively defined to the x, y and z axes, to this Quaternion. In the
	 * convention used here these angles represent a composition of extrinsic
	 * rotations (rotations about the reference frame axes), which is also known
	 * as {@link #taitBryanAngles()} (See
	 * http://en.wikipedia.org/wiki/Euler_angles and
	 * http://en.wikipedia.org/wiki/Tait-Bryan_angles). {@link #eulerAngles()}
	 * performs the inverse operation.
	 * <p>
	 * Each rotation angle is converted to an axis-angle pair, with the axis
	 * corresponding to one of the Euclidean axes. The axis-angle pairs are
	 * converted to quaternions and multiplied together. The order of the
	 * rotations is: y->z->x which follows the convention found here:
	 * http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm.
	 * 
	 * @see #eulerAngles()
	 */
	public void fromEulerAngles(float roll, float pitch, float yaw) {
		Quaternion qx = new Quaternion(new PVector(1, 0, 0), roll);
		Quaternion qy = new Quaternion(new PVector(0, 1, 0), pitch);
		Quaternion qz = new Quaternion(new PVector(0, 0, 1), yaw);
		set(qy);
		multiply(qz);
		multiply(qx);
	}

	/**
	 * Same as {@link #eulerAngles()}.
	 */
	public PVector taitBryanAngles() {
		return eulerAngles();
	}

	/**
	 * Converts this Quaternion to Euler rotation angles {@code roll}, {@code
	 * pitch} and {@code yaw} in radians.
	 * {@link #fromEulerAngles(float, float, float)} performs the inverse
	 * operation. The code was adapted from:
	 * http://www.euclideanspace.com/maths/geometry
	 * /rotations/conversions/quaternionToEuler/index.htm.
	 * <p>
	 * <b>Attention:</b> This method assumes that this Quaternion is normalized.
	 * 
	 * @return the PVector holding the roll (x coordinate of the vector), pitch (y
	 *         coordinate of the vector) and yaw angles (z coordinate of the
	 *         vector). <b>Note:</b> The order of the rotations that would produce
	 *         this Quaternion (i.e., as with {@code fromEulerAngles(roll, pitch,
	 *         yaw)}) is: y->z->x.
	 * 
	 * @see #fromEulerAngles(float, float, float)
	 */
	public PVector eulerAngles() {
		float roll, pitch, yaw;
		float test = x * y + z * w;
		if (test > 0.499) { // singularity at north pole
			pitch = 2 * PApplet.atan2(x, w);
			yaw = PApplet.PI / 2;
			roll = 0;
			return new PVector(roll, pitch, yaw);
		}
		if (test < -0.499) { // singularity at south pole
			pitch = -2 * PApplet.atan2(x, w);
			yaw = -PApplet.PI / 2;
			roll = 0;
			return new PVector(roll, pitch, yaw);
		}
		float sqx = x * x;
		float sqy = y * y;
		float sqz = z * z;
		pitch = PApplet.atan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz);
		yaw = PApplet.asin(2 * test);
		roll = PApplet.atan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz);
		return new PVector(roll, pitch, yaw);
	}

	/**
	 * public PVector eulerAngles() { //This quaternion does not need to be
	 * normalized. See:
	 * //http://www.euclideanspace.com/maths/geometry/rotations/conversions
	 * /quaternionToEuler/index.htm float roll, pitch, yaw; float sqw = w*w; float
	 * sqx = x*x; float sqy = y*y; float sqz = z*z; float unit = sqx + sqy + sqz +
	 * sqw; // if normalised is one, otherwise is correction factor float test =
	 * x*y + z*w; if (test > 0.499*unit) { // singularity at north pole pitch = 2
	 * * PApplet.atan2(x,w); yaw = PApplet.PI/2; roll = 0; return new
	 * PVector(roll, pitch, yaw); } if (test < -0.499*unit) { // singularity at
	 * south pole pitch = -2 * PApplet.atan2(x,w); yaw = - PApplet.PI/2; roll = 0;
	 * return new PVector(roll, pitch, yaw); } pitch = PApplet.atan2(2*y*w-2*x*z ,
	 * sqx - sqy - sqz + sqw); yaw = PApplet.asin(2*test/unit); roll =
	 * PApplet.atan2(2*x*w-2*y*z , -sqx + sqy - sqz + sqw); return new
	 * PVector(roll, pitch, yaw); } //
	 */

	/**
	 * Sets the Quaternion as a rotation from the {@code from} direction to the
	 * {@code to} direction.
	 * <p>
	 * <b>Attention:</b> this rotation is not uniquely defined. The selected axis
	 * is usually orthogonal to {@code from} and {@code to}, minimizing the
	 * rotation angle. This method is robust and can handle small or almost
	 * identical vectors.
	 * 
	 * @see #fromAxisAngle(PVector, float)
	 */
	public void fromTo(PVector from, PVector to) {
		float fromSqNorm = MathUtils.squaredNorm(from);
		float toSqNorm = MathUtils.squaredNorm(to);
		// Identity Quaternion when one vector is null
		if ((fromSqNorm < 1E-10f) || (toSqNorm < 1E-10f)) {
			this.x = this.y = this.z = 0.0f;
			this.w = 1.0f;
		} else {

			PVector axis = from.cross(to);

			float axisSqNorm = MathUtils.squaredNorm(axis);

			// Aligned vectors, pick any axis, not aligned with from or to
			if (axisSqNorm < 1E-10f)
				axis = MathUtils.orthogonalVector(from);

			float angle = PApplet.asin(PApplet.sqrt(axisSqNorm
					/ (fromSqNorm * toSqNorm)));

			if (from.dot(to) < 0.0)
				angle = PI - angle;

			fromAxisAngle(axis, angle);
		}
	}

	/**
	 * Set the Quaternion from a (supposedly correct) 3x3 rotation matrix.
	 * <p>
	 * The matrix is expressed in European format: its three columns are the
	 * images by the rotation of the three vectors of an orthogonal basis.
	 * <p>
	 * {@link #fromRotatedBasis(PVector, PVector, PVector)} sets a Quaternion from
	 * the three axis of a rotated frame. It actually fills the three columns of a
	 * matrix with these rotated basis vectors and calls this method.
	 * 
	 * @param m
	 *          the 3*3 matrix of float values
	 */
	public final void fromRotationMatrix(float m[][]) {
		// Compute one plus the trace of the matrix
		float onePlusTrace = 1.0f + m[0][0] + m[1][1] + m[2][2];

		if (onePlusTrace > 1E-5f) {
			// Direct computation
			float s = PApplet.sqrt(onePlusTrace) * 2.0f;
			this.x = (m[2][1] - m[1][2]) / s;
			this.y = (m[0][2] - m[2][0]) / s;
			this.z = (m[1][0] - m[0][1]) / s;
			this.w = 0.25f * s;
		} else {
			// Computation depends on major diagonal term
			if ((m[0][0] > m[1][1]) & (m[0][0] > m[2][2])) {
				float s = PApplet.sqrt(1.0f + m[0][0] - m[1][1] - m[2][2]) * 2.0f;
				this.x = 0.25f * s;
				this.y = (m[0][1] + m[1][0]) / s;
				this.z = (m[0][2] + m[2][0]) / s;
				this.w = (m[1][2] - m[2][1]) / s;
			} else if (m[1][1] > m[2][2]) {
				float s = PApplet.sqrt(1.0f + m[1][1] - m[0][0] - m[2][2]) * 2.0f;
				this.x = (m[0][1] + m[1][0]) / s;
				this.y = 0.25f * s;
				this.z = (m[1][2] + m[2][1]) / s;
				this.w = (m[0][2] - m[2][0]) / s;
			} else {
				float s = PApplet.sqrt(1.0f + m[2][2] - m[0][0] - m[1][1]) * 2.0f;
				this.x = (m[0][2] + m[2][0]) / s;
				this.y = (m[1][2] + m[2][1]) / s;
				this.z = 0.25f * s;
				this.w = (m[0][1] - m[1][0]) / s;
			}
		}
		normalize();
	}

	/**
	 * Set the Quaternion from a (supposedly correct) 3x3 rotation matrix given in
	 * the upper left 3x3 sub-matrix of the PMatrix3D.
	 * 
	 * @see #fromRotationMatrix(float[][])
	 */
	public final void fromMatrix(PMatrix3D pM) {
		fromRotationMatrix(MathUtils.get3x3UpperLeftMatrixFromPMatrix3D(pM));
	}

	/**
	 * Sets the Quaternion from the three rotated vectors of an orthogonal basis.
	 * <p>
	 * The three vectors do not have to be normalized but must be orthogonal and
	 * direct (i,e., {@code X^Y=k*Z, with k>0}).
	 * 
	 * @param X
	 *          the first PVector
	 * @param Y
	 *          the second PVector
	 * @param Z
	 *          the third PVector
	 * 
	 * @see #fromRotationMatrix(float[][])
	 * @see #Quaternion(PVector, PVector)
	 * 
	 */
	public final void fromRotatedBasis(PVector X, PVector Y, PVector Z) {
		float m[][] = new float[3][3];
		float normX = X.mag();
		float normY = Y.mag();
		float normZ = Z.mag();

		for (int i = 0; i < 3; ++i) {
			m[i][0] = (X.array())[i] / normX;
			m[i][1] = (Y.array())[i] / normY;
			m[i][2] = (Z.array())[i] / normZ;
		}

		fromRotationMatrix(m);
	}

	/**
	 * Returns the normalized axis direction of the rotation represented by the
	 * Quaternion.
	 * <p>
	 * The result is null for an identity Quaternion.
	 * 
	 * @see #angle()
	 */
	public final PVector axis() {
		PVector res = new PVector(this.x, this.y, this.z);
		float sinus = res.mag();
		if (sinus > 1E-8f)
			res.div(sinus);
		if (PApplet.acos(this.w) <= HALF_PI)
			return res;
		else {
			res.x = -res.x;
			res.y = -res.y;
			res.z = -res.z;
			return res;
		}
	}

	/**
	 * Returns the {@code angle} (in radians) of the rotation represented by the
	 * Quaternion.
	 * <p>
	 * This value is always in the range {@code [0-pi]}. Larger rotational angles
	 * are obtained by inverting the {@link #axis()} direction.
	 * 
	 * @see #axis()
	 */
	public final float angle() {
		float angle = 2.0f * PApplet.acos(this.w);
		return (angle <= PI) ? angle : 2.0f * PI - angle;
	}

	/**
	 * Returns the 3x3 rotation matrix associated with the Quaternion.
	 * <p>
	 * <b>Attention:</b> The method returns the European mathematical
	 * representation of the rotation matrix.
	 * 
	 * @see #inverseRotationMatrix()
	 * 
	 */
	public final float[][] rotationMatrix() {
		return MathUtils.get3x3UpperLeftMatrixFromPMatrix3D(matrix());
	}

	/**
	 * Returns the PMatrix3D (processing matrix) which represents the rotation
	 * matrix associated with the Quaternion.
	 * 
	 * @see #rotationMatrix()
	 */
	public final PMatrix3D matrix() {

		float q00 = 2.0f * this.x * this.x;
		float q11 = 2.0f * this.y * this.y;
		float q22 = 2.0f * this.z * this.z;

		float q01 = 2.0f * this.x * this.y;
		float q02 = 2.0f * this.x * this.z;
		float q03 = 2.0f * this.x * this.w;

		float q12 = 2.0f * this.y * this.z;
		float q13 = 2.0f * this.y * this.w;

		float q23 = 2.0f * this.z * this.w;

		float m00 = 1.0f - q11 - q22;
		float m01 = q01 - q23;
		float m02 = q02 + q13;

		float m10 = q01 + q23;
		float m11 = 1.0f - q22 - q00;
		float m12 = q12 - q03;

		float m20 = q02 - q13;
		float m21 = q12 + q03;
		float m22 = 1.0f - q11 - q00;

		float m30 = 0.0f;
		float m31 = 0.0f;
		float m32 = 0.0f;

		float m03 = 0.0f;
		float m13 = 0.0f;
		float m23 = 0.0f;
		float m33 = 1.0f;

		return new PMatrix3D(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22,
				m23, m30, m31, m32, m33);
	}

	/**
	 * Returns the associated inverse rotation processing PMatrix3D. This is
	 * simply {@link #matrix()} of the {@link #inverse()}.
	 * <p>
	 * <b>Attention:</b> The result is only valid until the next call to
	 * {@link #inverseMatrix()}. Use it immediately (as in {@code
	 * applyMatrix(q.inverseMatrix())}).
	 */
	public final PMatrix3D inverseMatrix() {
		Quaternion tempQuat = new Quaternion(x, y, z, w);
		tempQuat.invert();
		return tempQuat.matrix();
	}

	/**
	 * Returns the 3x3 inverse rotation matrix associated with the Quaternion.
	 * <p>
	 * <b>Attention:</b> This is the classical mathematical rotation matrix.
	 */
	public final float[][] inverseRotationMatrix() {
		return MathUtils.get3x3UpperLeftMatrixFromPMatrix3D(inverseMatrix());
	}

	/**
	 * Returns the logarithm of the Quaternion.
	 * 
	 * @see #exp()
	 */
	public final Quaternion log() {
		// Warning: this method should not normalize the Quaternion
		float len = PApplet.sqrt(this.x * this.x + this.y * this.y + this.z
				* this.z);

		if (len < 1E-6f)
			return new Quaternion(this.x, this.y, this.z, 0.0f, false);
		else {
			float coef = PApplet.acos(this.w) / len;
			return new Quaternion(this.x * coef, this.y * coef, this.z * coef, 0.0f,
					false);
		}
	}

	/**
	 * Returns the exponential of the Quaternion.
	 * 
	 * @see #log()
	 */
	public final Quaternion exp() {
		float theta = PApplet.sqrt(this.x * this.x + this.y * this.y + this.z
				* this.z);

		if (theta < 1E-6f)
			return new Quaternion(this.x, this.y, this.z, PApplet.cos(theta));
		else {
			float coef = PApplet.sin(theta) / theta;
			return new Quaternion(this.x * coef, this.y * coef, this.z * coef,
					PApplet.cos(theta));
		}
	}

	/**
	 * Returns a random unit Quaternion.
	 * <p>
	 * You can create a randomly directed unit vector using:
	 * <p>
	 * {@code PVector randomDir = new PVector(1.0f, 0.0f, 0.0f);} <br>
	 * {@code randomDir = Quaternion.multiply(Quaternion.randomQuaternion(),
	 * randomDir);}
	 */
	public final static Quaternion randomQuaternion() {
		float seed = (float) Math.random();
		float r1 = PApplet.sqrt(1.0f - seed);
		float r2 = PApplet.sqrt(seed);
		float t1 = 2.0f * PI * (float) Math.random();
		float t2 = 2.0f * PI * (float) Math.random();

		return new Quaternion(PApplet.sin(t1) * r1, PApplet.cos(t1) * r1, PApplet
				.sin(t2)
				* r2, PApplet.cos(t2) * r2);
	}

	/**
	 * Wrapper function that simply calls {@code slerp(a, b, t, true)}.
	 * <p>
	 * See {@link #slerp(Quaternion, Quaternion, float, boolean)} for details.
	 */
	public static final Quaternion slerp(Quaternion a, Quaternion b, float t) {
		return Quaternion.slerp(a, b, t, true);
	}

	/**
	 * Returns the slerp interpolation of quaternions {@code a} and {@code b}, at
	 * time {@code t}.
	 * <p>
	 * {@code t} should range in {@code [0,1]}. Result is a when {@code t=0 } and
	 * {@code b} when {@code t=1}.
	 * <p>
	 * When {@code allowFlip} is true (default) the slerp interpolation will
	 * always use the "shortest path" between the quaternions' orientations, by
	 * "flipping" the source Quaternion if needed (see {@link #negate()}).
	 * 
	 * @param a
	 *          the first Quaternion
	 * @param b
	 *          the second Quaternion
	 * @param t
	 *          the t interpolation parameter
	 * @param allowFlip
	 *          tells whether or not the interpolation allows axis flip
	 */
	public static final Quaternion slerp(Quaternion a, Quaternion b, float t,
			boolean allowFlip) {
		// Warning: this method should not normalize the Quaternion
		float cosAngle = Quaternion.dotProduct(a, b);

		float c1, c2;
		// Linear interpolation for close orientations
		if ((1.0 - PApplet.abs(cosAngle)) < 0.01) {
			c1 = 1.0f - t;
			c2 = t;
		} else {
			// Spherical interpolation
			float angle = PApplet.acos(PApplet.abs(cosAngle));
			float sinAngle = PApplet.sin(angle);
			c1 = PApplet.sin(angle * (1.0f - t)) / sinAngle;
			c2 = PApplet.sin(angle * t) / sinAngle;
		}

		// Use the shortest path
		if (allowFlip && (cosAngle < 0.0))
			c1 = -c1;

		return new Quaternion(c1 * a.x + c2 * b.x, c1 * a.y + c2 * b.y, c1 * a.z
				+ c2 * b.z, c1 * a.w + c2 * b.w, false);
	}

	/**
	 * Returns the slerp interpolation of the two quaternions {@code a} and
	 * {@code b}, at time {@code t}, using tangents {@code tgA} and {@code tgB}.
	 * <p>
	 * The resulting Quaternion is "between" {@code a} and {@code b} (result is
	 * {@code a} when {@code t=0} and {@code b} for {@code t=1}).
	 * <p>
	 * Use {@link #squadTangent(Quaternion, Quaternion, Quaternion)} to define the
	 * Quaternion tangents {@code tgA} and {@code tgB}.
	 * 
	 * @param a
	 *          the first Quaternion
	 * @param tgA
	 *          the first tangent Quaternion
	 * @param tgB
	 *          the second tangent Quaternion
	 * @param b
	 *          the second Quaternion
	 * @param t
	 *          the t interpolation parameter
	 */
	public static final Quaternion squad(Quaternion a, Quaternion tgA,
			Quaternion tgB, Quaternion b, float t) {
		Quaternion ab = Quaternion.slerp(a, b, t);
		Quaternion tg = Quaternion.slerp(tgA, tgB, t, false);
		return Quaternion.slerp(ab, tg, 2.0f * t * (1.0f - t), false);
	}

	/**
	 * Simply returns {@code log(a. inverse() * b)}.
	 * <p>
	 * Useful for {@link #squadTangent(Quaternion, Quaternion, Quaternion)}.
	 * 
	 * @param a
	 *          the first Quaternion
	 * @param b
	 *          the second Quaternion
	 */
	public static final Quaternion lnDif(Quaternion a, Quaternion b) {
		Quaternion dif = a.inverse();
		dif.multiply(b);

		dif.normalize();
		return dif.log();
	}

	/**
	 * Returns a tangent Quaternion for {@code center}, defined by {@code before}
	 * and {@code after} quaternions.
	 * 
	 * @param before
	 *          the first Quaternion
	 * @param center
	 *          the second Quaternion
	 * @param after
	 *          the third Quaternion
	 */
	public static final Quaternion squadTangent(Quaternion before,
			Quaternion center, Quaternion after) {
		Quaternion l1 = Quaternion.lnDif(center, before);
		Quaternion l2 = Quaternion.lnDif(center, after);
		Quaternion e = new Quaternion();

		e.x = -0.25f * (l1.x + l2.x);
		e.y = -0.25f * (l1.y + l2.y);
		e.z = -0.25f * (l1.z + l2.z);
		e.w = -0.25f * (l1.w + l2.w);

		return Quaternion.multiply(center, e.exp());
	}

	/**
	 * Utility function that returns the squared norm of the Quaternion.
	 */
	public static float squaredNorm(Quaternion q) {
		return (q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w);
	}
}

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/SMA.pde
================================================

import java.util.LinkedList;
import java.util.Queue;
public class MovingAverage {
    private final Queue<Float> window = new LinkedList<Float>();
    private final int period;
    private float sum;
 
    public MovingAverage(int period) {
        assert period > 0 : "Period must be a positive integer";
        this.period = period;
    }
 
    public void newNum(float num) {
        sum += num;
        window.add(num);
        if (window.size() > period) {
            sum -= window.remove();
        }
    }
 
    public float getAvg() {
        if (window.isEmpty()) return 0; // technically the average is undefined
        return sum / window.size();
    }
}

public class EMA {
    private float alpha;
    private float oldValue;
    public EMA(float alpha) {
        this.alpha = alpha;
    }

    public float getAvg(float value){
        //if(oldValue == null) {
        //    oldValue = value;
        //    return value;
        //}
        float newValue = oldValue + alpha * (value - oldValue);
        oldValue = newValue;
        return newValue;
    }
}

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/StopWatch.pde
================================================
// This class based on code found here: http://www.goldb.org/stopwatchjava.html
// http://forum.processing.org/one/topic/timer-in-processing.html

class StopWatchTimer {
  int startTime = 0, stopTime = 0;
  boolean running = false;  
   
    void start() {
        startTime = millis();
        running = true;
    }
    void stop() {
        stopTime = millis();
        running = false;
    }
    int getElapsedTime() {
        int elapsed;
        if (running) {
             elapsed = (millis() - startTime);
        }
        else {
            elapsed = (stopTime - startTime);
        }
        return elapsed;
    }
    int second() {
      return (getElapsedTime() / 1000) % 60;
    }
    int minute() {
      return (getElapsedTime() / (1000*60)) % 60;
    }
    int hour() {
      return (getElapsedTime() / (1000*60*60)) % 24;
    }
}

================================================
FILE: extras/FreeIMU_cube_UDP_ES32/Unused.pde
================================================
/*
//****************************************************************************
//The purpose of the calibration routine is to obtain the value of the 
//reference threshold. It consists on a 1024 samples average in no-movement 
//condition.
//*****************************************************************************

void Calibrate() 
{
  int count1;
  count1 = 0;
  do {  
  //ADC_GetAllAxis();
  gravity_compensate();
  sstatex = sstatex + Sample_X;             // Accumulate Samples
  sstatey = sstatey + Sample_Y;
  println(Sample_X);
  count1++;
  } while(count1 != 0x0400);                   // 1024 times

  sstatex=sstatex / 1024;                      // division between l024
  sstatey=sstatey / 1024;
  
}

*/

/* 
//**************************************************************************/
//**************************************************************************/ 
//This function obtains magnitude and direction 
//In   this   particular   protocol   direction   and   magnitude   are   sent   in   separate   variables. 
//Management can be done in many other different ways.
//**************************************************************************/  

/* void data_transfer() 
{
 
 float positionXbkp; 
 float positionYbkp;
 int delay;
 float [] posx_seg = new float [4];
 float [] posy_seg = new float [4]; 
 
 
  if (positionX[1] >= 0) {                          //This line compares the sign of the X direction data 
    direction = (direction | 0x10);                 //if its positive the most significant byte 
    posx_seg[0]= positionX[1] & 0x000000FF;         //is set to 1 else it is set to 8
    posx_seg[1]= (positionX[1]>>8) & 0x000000FF;    //the data is also managed in the 
                                                    // subsequent lines in order to 
    posx_seg[2]= (positionX[1]>>16) & 0x000000FF;   // be sent. The 32 bit variable must be 
    posx_seg[3]= (positionX[1]>>24) & 0x000000FF;   // split into 4 different 8 bit 
                                                    // variables in order to be sent via   
                                                    // the 8 bit SCI frame

      }

    else {direction = (direction | 0x80);
             positionXbkp = positionX[1]-1;
             positionXbkp = positionXbkp & 0xFFFFFFFF;
             posx_seg[0] = positionXbkp & 0x000000FF;
             posx_seg[1] = (positionXbkp >> 8) & 0x000000FF;
             posx_seg[2] = (positionXbkp >> 16) & 0x000000FF;
             posx_seg[3] = (positionXbkp >> 24) & 0x000000FF;
             } 
  
  if (positionY[1] >=0 ) {                            //  Same management than in the previous case 
             direction= (direction | 0x08);        // but with the Y data.
             posy_seg[0]= positionY[1] & 0x000000FF;
             posy_seg[1]= (positionY[1]>>8) & 0x000000FF;
             posy_seg[2]= (positionY[1]>>16) & 0x000000FF;
             posy_seg[3]= (positionY[1]>>24) & 0x000000FF;
             }
  
  else {direction= (direction | 0x01);
             positionYbkp = positionY[1]-1;
             positionYbkp = positionYbkp & 0xFFFFFFFF;
             posy_seg[0] = positionYbkp & 0x000000FF;
             posy_seg[1] = (positionYbkp >> 8) & 0x000000FF;
             posy_seg[2] = (positionYbkp >> 16) & 0x000000FF;
             posy_seg[3] = (positionYbkp >> 24) & 0x000000FF;
             } 

  delay =0x0100;
  
  Sensor_Data[0] = 0x03;
  Sensor_Data[1] = direction;
  Sensor_Data[2] = posx_seg[3];
  Sensor_Data[3] = posy_seg[3];
  Sensor_Data[4] = 0x01;
  Sensor_Data[7] = 0x01;
  Sensor_Data[6] = 9999;
  
  //while (--delay);                                   
  
  //SCITxMsg(Sensor_Data);         //  Data transferring function   
  //while (SCIC2 & 0x08);

} */


//************************************************************************
//************************************************************************
//This function returns data format to its original state. When obtaining 
//the magnitude and  direction of the position, an inverse two's complement 
//is made. This function makes the two's complement in order to return the 
//data to it original state.
//It is important to notice that the sensibility adjustment is greatly 
//impacted here, the amount of "ones" inserted in the mask must be equivalent
//to the "ones" lost in the shifting made in the previous function upon the 
//sensibility modification. 
//************************************************************************

/* void data_reintegration() 
{
  if (direction >= 10) 
   {
    positionX[1]= positionX[1] | 0xFFFFC000; }   // l8 "ones" inserted. Same size as the 
                  //amount of shifts 
    
    direction = direction & 0x01;
    if (direction == 1)   
      {  positionY[1]= positionY[1] | 0xFFFFC000;  }
 } */