[
  {
    "path": "Bosch/BME280_VEML6040_LTSleep.ino",
    "content": "/* BME280 and VEML6040 Basic Example Code using ESP32\n by: Kris Winer\n date: December 14, 2016\n license: Beerware - Use this code however you'd like. If you \n find it useful you can buy me a beer some time.\n \n This sketch uses SDA/SCL on pins 0/2, respectively, and it uses the ESP32.\n \n The BME280 is a simple but high resolution pressure/humidity/temperature sensor, which can be used in its high resolution\n mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\n only 1 microAmp. The choice will depend on the application.\n\n VEML6040 color sensor senses red, green, blue, and white light and incorporates photodiodes, amplifiers, \n and analog / digital circuits into a single chip using CMOS process. With the   color   sensor   applied,   \n the   brightness,   and   color temperature of backlight can be adjusted base on ambient light  source  \n that  makes  panel  looks  more  comfortable  for  end   user’s   eyes.   VEML6040’s   adoption   of   FiltronTM\n technology  achieves  the  closest  ambient  light  spectral  sensitivity to real human eye responses.\n\n VEML6040  provides  excellent  temperature  compensation  capability  for  keeping  the  output  stable  \n under  changing  temperature.   VEML6040’s   function   are   easily   operated   via the simple command format \n of I2C (SMBus compatible) interface  protocol.  VEML6040’s  operating  voltage  ranges  from   2.5   V   to   \n 3.6   V.   VEML6040   is   packaged   in   a   lead  (Pb)-free  4  pin  OPLGA  package  which  offers  the  best market-proven reliability.\n\n \n SDA and SCL should have 4K7 pull-up resistors (to 3.3V).\n \n Hardware setup:\n SDA ----------------------- 21\n SCL ----------------------- 22\n \n  */\n#include \"Wire.h\"   \n#include <SPI.h>\n#include <WiFi.h>\n#include <WiFiClient.h>\n//#include <WebServer.h>\n//#include <ESP32mDNS.h>\n//#include <ArduinoOTA.h>\n\n/*#include \"gpio.h\"\nextern \"C\" {\n#include \"user_interface.h\"\n  bool wifi_set_sleep_type(sleep_type_t);\n  sleep_type_t wifi_get_sleep_type(void);\n}\n*/\n// Must immediately declare functions to avoid \"Not declared in this scope\" errors\nvoid      I2Cscan();\nvoid      writeByte(uint8_t address, uint8_t subAddress, uint8_t data);\nuint8_t   readByte(uint8_t address, uint8_t subAddress);\nvoid      readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);\nint32_t  readBME280Temperature();\nint32_t  readBME280Pressure();\nint32_t  readBME280Humidity();\nvoid      BME280Init();\nuint32_t  BME280_compensate_P(int32_t adc_P);\nint32_t   BME280_compensate_T(int32_t adc_T);\nuint32_t  BME280_compensate_H(int32_t adc_H);\nuint16_t  getRGBWdata(uint16_t * destination);\nvoid      enableVEML6040();\n//void      initWifi();\n\n//ADC_MODE(ADC_VCC); // to use getVcc, don't use if using battery voltage monotor\n\n// BME280 registers\n#define BME280_HUM_LSB    0xFE\n#define BME280_HUM_MSB    0xFD\n#define BME280_TEMP_XLSB  0xFC\n#define BME280_TEMP_LSB   0xFB\n#define BME280_TEMP_MSB   0xFA\n#define BME280_PRESS_XLSB 0xF9\n#define BME280_PRESS_LSB  0xF8\n#define BME280_PRESS_MSB  0xF7\n#define BME280_CONFIG     0xF5\n#define BME280_CTRL_MEAS  0xF4\n#define BME280_STATUS     0xF3\n#define BME280_CTRL_HUM   0xF2\n#define BME280_RESET      0xE0\n#define BME280_ID         0xD0  // should be 0x60\n#define BME280_CALIB00    0x88\n#define BME280_CALIB26    0xE1\n\n////////////////////////////\n// VEML6040 Command Codes //\n////////////////////////////\n#define  VEML6040_CONF            0x00 // command codes\n#define  VEML6040_R_DATA          0x08  \n#define  VEML6040_G_DATA          0x09 \n#define  VEML6040_B_DATA          0x0A\n#define  VEML6040_W_DATA          0x0B\n\n#define VEML6040_ADDRESS         0x10\n#define BME280_ADDRESS           0x76   // Address of BMP280 altimeter when ADO = 0\n\n#define SerialDebug true  // set to true to get Serial output for debugging\n#define myLed 5\n\nenum Posr {\n  P_OSR_00 = 0,  // no op\n  P_OSR_01,\n  P_OSR_02,\n  P_OSR_04,\n  P_OSR_08,\n  P_OSR_16\n};\n\nenum Hosr {\n  H_OSR_00 = 0,  // no op\n  H_OSR_01,\n  H_OSR_02,\n  H_OSR_04,\n  H_OSR_08,\n  H_OSR_16\n};\n\nenum Tosr {\n  T_OSR_00 = 0,  // no op\n  T_OSR_01,\n  T_OSR_02,\n  T_OSR_04,\n  T_OSR_08,\n  T_OSR_16\n};\n\nenum IIRFilter {\n  full = 0,  // bandwidth at full sample rate\n  BW0_223ODR,\n  BW0_092ODR,\n  BW0_042ODR,\n  BW0_021ODR // bandwidth at 0.021 x sample rate\n};\n\nenum Mode {\n  BME280Sleep = 0,\n  forced,\n  forced2,\n  normal\n};\n\nenum SBy {\n  t_00_5ms = 0,\n  t_62_5ms,\n  t_125ms,\n  t_250ms,\n  t_500ms,\n  t_1000ms,\n  t_10ms,\n  t_20ms,\n};\n\nenum IT {\n  IT_40 = 0, //   40 ms\n  IT_80,     //   80 ms\n  IT_160,    //  160 ms\n  IT_320,    //  320 ms\n  IT_640,    //  640 ms\n  IT_1280   // 1280 ms\n};\n\n// Specify BME280 configuration\nuint8_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\n// t_fine carries fine temperature as global value for BME280\nint32_t t_fine;\n\n// Specify VEML6070 Integration time\nuint8_t IT = IT_160;\nuint8_t ITime = 160;  // milliseconds\nuint16_t RGBWData[4] = {0, 0, 0, 0};\nfloat GSensitivity = 0.25168/((float) (IT + 1)); // ambient light sensitivity increases with integration time\nfloat redLight, greenLight, blueLight, ambientLight;\n\nfloat Temperature, Pressure, Humidity; // stores BME280 pressures sensor pressure and temperature\nfloat VBAT;  // battery voltage from ESP8285 ADC read\nint32_t rawPress, rawTemp, rawHumidity;   // pressure and temperature raw count output for BME280\n\n// BME280 compensation parameters\nuint8_t  dig_H1, dig_H3, dig_H6;\nuint16_t dig_T1, dig_P1, dig_H4, dig_H5;\nint16_t  dig_T2, dig_T3, dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9, dig_H2;\n\nfloat   temperature_C, temperature_F, pressure, humidity, altitude; // Scaled output of the BME280\n\nuint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate\n\n/*ESP8266WebServer server(80);\n \nString webString=\"\";     // String to display\n \nvoid handle_root() {\n  server.send(200, \"text/plain\", \"Hello from the environmental monitoring station ESP8285!\");\n  delay(100);\n}\n\n  String createHTML(float var1, float var2, float var3, float var4, float var5, float var6, float var7, float var8, float var9, float var10) {\n   webString = \"<html><head><meta http-equiv=\\\"Refresh\\\" content=\\\"10\\\"></head><body><UL>\"\n\n  +String(\"<LI>Temperature = \")+String(var1)+String(\" C</LI>\") \n  +String(\"<LI>Temperature = \")+String(var2)+String(\" F</LI>\") \n  +String(\"<LI>Pressure = \")+String(var3)+String(\" milliBar</LI>\") \n  +String(\"<LI>Altitude = \")+String(var4)+String(\" feet</LI>\") \n  +String(\"<LI>Humidity = \")+String(var5)+String(\" %RH</LI>\") \n  +String(\"<LI>Red Light = \")+String(var6)+String(\" microWatts/sq. cm</LI>\") \n  +String(\"<LI>Green Light = \")+String(var7)+String(\" microWatts/sq. cm</LI>\") \n  +String(\"<LI>Blue Light = \")+String(var8)+String(\" microWatts/sq. cm</LI>\") \n  +String(\"<LI>Ambient Light = \")+String(var9)+String(\" lux</LI>\") \n  +String(\"<LI>Battery Voltage = \")+String(var10)+String(\" V</LI>\") \n     \n  +\"</UL></body></html>\" ;\n   return webString;\n  }\n*/\n\nvoid setup()\n{\n\n  Serial.begin(115200);\n  delay(4000);\n\n  pinMode(myLed, OUTPUT);\n  digitalWrite(myLed, HIGH);\n  \n  Wire.begin(21, 22, 400000); // SDA (21), SCL (22) on ESP32, 400 kHz rate\n\n  I2Cscan(); // should detect BME280 at 0x76\n  \n  // Read the WHO_AM_I register of the BME280 this is a good test of communication\n  byte f = readByte(BME280_ADDRESS, BME280_ID);  // Read WHO_AM_I register for BME280\n  Serial.print(\"BME280 \"); \n  Serial.print(\"I AM \"); \n  Serial.print(f, HEX); \n  Serial.print(\" I should be \"); \n  Serial.println(0x60, HEX);\n  Serial.println(\" \");\n  \n // delay(1000); \n\n  if(f == 0x60) {\n   \n  writeByte(BME280_ADDRESS, BME280_RESET, 0xB6); // reset BME280 before initilization\n  delay(100);\n\n  BME280Init(); // Initialize BME280 altimeter\n  Serial.println(\"Calibration coeficients:\");\n  Serial.print(\"dig_T1 =\"); \n  Serial.println(dig_T1);\n  Serial.print(\"dig_T2 =\"); \n  Serial.println(dig_T2);\n  Serial.print(\"dig_T3 =\"); \n  Serial.println(dig_T3);\n  Serial.print(\"dig_P1 =\"); \n  Serial.println(dig_P1);\n  Serial.print(\"dig_P2 =\"); \n  Serial.println(dig_P2);\n  Serial.print(\"dig_P3 =\"); \n  Serial.println(dig_P3);\n  Serial.print(\"dig_P4 =\"); \n  Serial.println(dig_P4);\n  Serial.print(\"dig_P5 =\"); \n  Serial.println(dig_P5);\n  Serial.print(\"dig_P6 =\"); \n  Serial.println(dig_P6);\n  Serial.print(\"dig_P7 =\"); \n  Serial.println(dig_P7);\n  Serial.print(\"dig_P8 =\"); \n  Serial.println(dig_P8);\n  Serial.print(\"dig_P9 =\"); \n  Serial.println(dig_P9);\n  Serial.print(\"dig_H1 =\"); \n  Serial.println(dig_H1);\n  Serial.print(\"dig_H2 =\"); \n  Serial.println(dig_H2);\n  Serial.print(\"dig_H3 =\"); \n  Serial.println(dig_H3);\n  Serial.print(\"dig_H4 =\"); \n  Serial.println(dig_H4);\n  Serial.print(\"dig_H5 =\"); \n  Serial.println(dig_H5);\n  Serial.print(\"dig_H6 =\"); \n  Serial.println(dig_H6);\n }\n  else Serial.println(\" BME280 not functioning!\");\n\n  enableVEML6040(); // initalize sensor\n  delay(150);  \n\n  // Get some information abut the ESP8285\n\n  uint32_t freeheap = ESP.getFreeHeap();\n  Serial.print(\"Free Heap Size = \"); Serial.println(freeheap);\n // uint32_t chipID = ESP.getChipId();\n // Serial.print(\"ESP32 chip ID = \"); Serial.println(chipID);\n//  uint32_t flashChipID = ESP.getFlashChipId();\n// Serial.print(\"ESP8285 flash chip ID = \"); Serial.println(flashChipID);\n  uint32_t flashChipSize = ESP.getFlashChipSize();\n  Serial.print(\"ESP32 flash chip size = \"); Serial.print(flashChipSize); Serial.println(\" bytes\");\n  uint32_t flashChipSpeed = ESP.getFlashChipSpeed();\n  Serial.print(\"ESP32 flash chip speed = \"); Serial.print(flashChipSpeed); Serial.println(\" Hz\");\n//  uint32_t getVcc = ESP.getVcc();\n//  Serial.print(\"ESP8285 supply voltage = \"); Serial.print(getVcc); Serial.println(\" volts\");\n\n /*    initWifi();\n\n     Serial.println(\"Light sleep enabled\");\n     wifi_set_sleep_type(LIGHT_SLEEP_T); // Enable light sleep mode to save power\n  \n     server.on(\"/ESP8285Data\", [](){\n\n    // BME280 Data\n    rawTemp =   readBME280Temperature();\n    temperature_C = (float) BME280_compensate_T(rawTemp)/100.0; // temperature in Centigrade\n    temperature_F = 9.*temperature_C/5. + 32.;\n    rawPress =  readBME280Pressure();\n    pressure = (float) BME280_compensate_P(rawPress)/25600.0; // Pressure in millibar\n    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));\n    rawHumidity =  readBME280Humidity();\n    humidity = (float) BME280_compensate_H(rawHumidity)/1024.0; // Humidity in %rH\n\n    // VEML6040 Data\n    getRGBWdata(RGBWData);\n    redLight = (float)RGBWData[0]/96.0f;\n    greenLight = (float)RGBWData[1]/74.0f;\n    blueLight = (float)RGBWData[2]/56.0f;\n    ambientLight = (float)RGBWData[1]*GSensitivity;\n\n    // Battery Voltage\n    VBAT = (1300.0/300.0) * (float)(analogRead(A0)) / 1024.0; // LiPo battery voltage in volts\n //   VBAT = (1100.0/100.0) * (float)(analogRead(A0)) / 1024.0; // 9V battery voltage in volts\n    \n    createHTML(temperature_C, temperature_F, pressure, altitude, humidity, redLight, greenLight, blueLight, ambientLight, VBAT);\n    server.send(200, \"text/html\", webString);               // send to someone's browser when asked\n    });\n  \n  server.begin();\n  Serial.println(\"HTTP server started\");\n\n  Serial.println(\"LABEL,Timestamp,Pressure(mbar),Temperature(F),Humidity(rH%)\");\n  */\n}\n \n\nvoid loop()\n{  \n    rawTemp =   readBME280Temperature();\n    temperature_C = (float) BME280_compensate_T(rawTemp)/100.0f;\n    rawPress =  readBME280Pressure();\n    pressure = (float) BME280_compensate_P(rawPress)/25600.0f; // Pressure in mbar\n    rawHumidity =   readBME280Humidity();\n    humidity = (float) BME280_compensate_H(rawHumidity)/1024.0f;\n \n      Serial.println(\"BME280:\");\n      Serial.print(\"Altimeter temperature = \"); \n      Serial.print( temperature_C, 2); \n      Serial.println(\" C\"); // temperature in degrees Celsius\n      Serial.print(\"Altimeter temperature = \"); \n      Serial.print(9.0f*temperature_C/5.0f + 32.0f, 2); \n      Serial.println(\" F\"); // temperature in degrees Fahrenheit\n      Serial.print(\"Altimeter pressure = \"); \n      Serial.print(pressure, 2); Serial.println(\" mbar\");// pressure in millibar\n\n      Serial.print(\"DATA,\");Serial.print(\"TIMER,\"); \n      Serial.print(pressure); Serial.print(\",\");\n      Serial.print(9.0f*temperature_C/5.0f + 32.0f, 2); Serial.print(\",\");\n      Serial.print(humidity); Serial.println();\n\n      altitude = 145366.45f*(1.0f - pow((pressure/1013.25f), 0.190284f));\n      Serial.print(\"Altitude = \"); \n      Serial.print(altitude, 2); \n      Serial.println(\" feet\");\n      Serial.print(\"Altimeter humidity = \"); \n      Serial.print(humidity, 1);  \n      Serial.println(\" %RH\");// pressure in millibar\n      Serial.println(\" \");\n\n      getRGBWdata(RGBWData);\n      Serial.print(\"Red raw counts = \");   Serial.println(RGBWData[0]);\n      Serial.print(\"Green raw counts = \"); Serial.println(RGBWData[1]);\n      Serial.print(\"Blue raw counts = \");  Serial.println(RGBWData[2]);\n      Serial.print(\"White raw counts = \"); Serial.println(RGBWData[3]);\n      Serial.print(\"Inferred IR raw counts = \"); Serial.println(RGBWData[3] - RGBWData[0] - RGBWData[1] - RGBWData[2]);\n      Serial.println(\"  \");\n \n      Serial.print(\"Red   light power density = \"); Serial.print((float)RGBWData[0]/96.0f, 2); Serial.println(\" microWatt/cm^2\");\n      Serial.print(\"Green light power density = \"); Serial.print((float)RGBWData[1]/74.0f, 2); Serial.println(\" microWatt/cm^2\");\n      Serial.print(\"Blue  light power density = \"); Serial.print((float)RGBWData[2]/56.0f, 2); Serial.println(\" microWatt/cm^2\");\n      Serial.println(\"  \");\n\n      Serial.print(\"Ambient light intensity = \"); Serial.print((float)RGBWData[1]*GSensitivity, 2); Serial.println(\" lux\");\n      Serial.println(\"  \");\n\n  // Empirical estimation of the correlated color temperature CCT:\n  // see https://www.vishay.com/docs/84331/designingveml6040.pdf\n      float temp = ( (float) (RGBWData[0] - RGBWData[2])/(float) RGBWData[1] );\n      float CCT = 4278.6f*pow(temp, -1.2455f) + 0.5f;\n\n      Serial.print(\"Correlated Color Temperature = \"); Serial.print(CCT, 2); Serial.println(\" Kelvin\");\n      Serial.println(\"  \");\n\n      float VBAT = (100.0f/120.0f) * float(analogRead(34)) / 1024.0f;  // LiPo battery\n      Serial.print(\"Battery Voltage = \"); Serial.print(VBAT, 2); Serial.println(\" V\");    \n  \n      digitalWrite(myLed, HIGH); // blink blue led\n      delay(100);\n      digitalWrite(myLed, LOW);   \n\n//      server.handleClient(); // serve data to web server site \n \n      delay(1000); // wait 1 seconds before refreshing data\n}\n\n//===================================================================================================================\n//====== Set of useful function to access acceleration, gyroscope, magnetometer, and temperature data\n//===================================================================================================================\n/*void initWifi() {\n  const char* ssid     = \"NETGEAR16\";\n  const char* password = \"braveroad553\";\n // Connect to WiFi network\n  WiFi.mode(WIFI_STA);\n  WiFi.begin(ssid, password);\n  Serial.print(\"\\n\\r \\n\\rWorking to connect\");\n\n // Wait for connection\n  while (WiFi.status() != WL_CONNECTED) {\n    delay(500);\n    Serial.print(\".\");\n  }\n  Serial.println(\"\");\n  Serial.println(\"ESP8285 Environmental Data Server\");\n  Serial.print(\"Connected to \");\n  Serial.println(ssid);\n  Serial.print(\"IP address: \");\n  Serial.println(WiFi.localIP());\n  // print the received signal strength:\n  long rssi = WiFi.RSSI();\n  Serial.print(\"signal strength (RSSI):\");\n  Serial.print(rssi);\n  Serial.println(\" dBm\");\n  }\n*/\nint32_t readBME280Temperature()\n{\n  uint8_t rawData[3];  // 20-bit pressure register data stored here\n  readBytes(BME280_ADDRESS, BME280_TEMP_MSB, 3, &rawData[0]);  \n  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);\n}\n\nint32_t readBME280Pressure()\n{\n  uint8_t rawData[3];  // 20-bit pressure register data stored here\n  readBytes(BME280_ADDRESS, BME280_PRESS_MSB, 3, &rawData[0]);  \n  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);\n}\n\nint32_t readBME280Humidity()\n{\n  uint8_t rawData[2];  // 16-bit humidity register data stored here\n  readBytes(BME280_ADDRESS, BME280_HUM_MSB, 2, &rawData[0]);  \n   return (uint32_t) (((uint32_t) rawData[0] << 24 | (uint32_t) rawData[1] << 16) ) >> 16;\n}\n\n\nvoid BME280Init()\n{\n  // Configure the BME280\n  // Set H oversampling rate\n  writeByte(BME280_ADDRESS, BME280_CTRL_HUM, 0x07 & Hosr);\n  // Set T and P oversampling rates and sensor mode\n  writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);\n  // Set standby time interval in normal mode and bandwidth\n  writeByte(BME280_ADDRESS, BME280_CONFIG, SBy << 5 | IIRFilter << 2);\n  // Read and store calibration data\n  uint8_t calib[26];\n  readBytes(BME280_ADDRESS, BME280_CALIB00, 26, &calib[0]);\n  dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);\n  dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);\n  dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);\n  dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);\n  dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);\n  dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);\n  dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);\n  dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);\n  dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);\n  dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);\n  dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);\n  dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);\n  dig_H1 = calib[25];\n  readBytes(BME280_ADDRESS, BME280_CALIB26, 7, &calib[0]);\n  dig_H2 = ( int16_t)((( int16_t) calib[1] << 8) | calib[0]);\n  dig_H3 = calib[2];\n  dig_H4 = ( int16_t)(((( int16_t) calib[3] << 8) | (0x0F & calib[4]) << 4) >> 4);\n  dig_H5 = ( int16_t)(((( int16_t) calib[5] << 8) | (0xF0 & calib[4]) ) >> 4 );\n  dig_H6 = calib[6];\n}\n\n// Returns temperature in DegC, resolution is 0.01 DegC. Output value of\n// “5123” equals 51.23 DegC.\nint32_t BME280_compensate_T(int32_t adc_T)\n{\n  int32_t var1, var2, T;\n  var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;\n  var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1))) >> 12) * ((int32_t)dig_T3)) >> 14;\n  t_fine = var1 + var2;\n  T = (t_fine * 5 + 128) >> 8;\n  return T;\n}\n\n// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8\n//fractional bits).\n//Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa\nuint32_t BME280_compensate_P(int32_t adc_P)\n{\n  long long var1, var2, p;\n  var1 = ((long long)t_fine) - 128000;\n  var2 = var1 * var1 * (long long)dig_P6;\n  var2 = var2 + ((var1*(long long)dig_P5)<<17);\n  var2 = var2 + (((long long)dig_P4)<<35);\n  var1 = ((var1 * var1 * (long long)dig_P3)>>8) + ((var1 * (long long)dig_P2)<<12);\n  var1 = (((((long long)1)<<47)+var1))*((long long)dig_P1)>>33;\n  if(var1 == 0)\n  {\n    return 0;\n    // avoid exception caused by division by zero\n  }\n  p = 1048576 - adc_P;\n  p = (((p<<31) - var2)*3125)/var1;\n  var1 = (((long long)dig_P9) * (p>>13) * (p>>13)) >> 25;\n  var2 = (((long long)dig_P8) * p)>> 19;\n  p = ((p + var1 + var2) >> 8) + (((long long)dig_P7)<<4);\n  return (uint32_t)p;\n}\n\n// Returns humidity in %RH as unsigned 32 bit integer in Q22.10 format (22integer and 10fractional bits).\n// Output value of “47445”represents 47445/1024= 46.333%RH\nuint32_t BME280_compensate_H(int32_t adc_H)\n{\nint32_t var;\n\nvar = (t_fine - ((int32_t)76800));\nvar = (((((adc_H << 14) - (((int32_t)dig_H4) << 20) - (((int32_t)dig_H5) * var)) +\n((int32_t)16384)) >> 15) * (((((((var * ((int32_t)dig_H6)) >> 10) * (((var *\n((int32_t)dig_H3)) >> 11) + ((int32_t)32768))) >> 10) + ((int32_t)2097152)) * ((int32_t)dig_H2) + 8192) >> 14));\nvar = (var - (((((var >> 15) * (var >> 15)) >> 7) * ((int32_t)dig_H1)) >> 4));\nvar = (var < 0 ? 0 : var); \nvar = (var > 419430400 ? 419430400 : var);\nreturn(uint32_t)(var >> 12);\n}\n\nuint16_t getRGBWdata(uint16_t * destination)\n{\n    for (int j = 0; j < 4; j++)\n    {\n    uint8_t rawData[2] = {0, 0};\n    Wire.beginTransmission(VEML6040_ADDRESS);\n    Wire.write(VEML6040_R_DATA + j);        // Command code for reading rgbw data channels in sequence\n    Wire.endTransmission(false);  // Send the Tx buffer, but send a restart to keep connection alive\n\n    Wire.requestFrom(VEML6040_ADDRESS, 2);  // Read two bytes from slave register address \n    uint8_t i = 0;\n    while (Wire.available()) \n    {\n        rawData[i++] = Wire.read();       // Put read results in the Rx buffer\n    }     \n    Wire.endTransmission();\n    destination[j] = ((uint16_t) rawData[1] << 8) | rawData[0];\n    }\n \n}\n\nvoid enableVEML6040()\n{\n  Wire.beginTransmission(VEML6040_ADDRESS);\n  Wire.write(VEML6040_CONF); // Command code for configuration register\n  Wire.write(IT << 4); // Bit 3 must be 0, bit 0 is 0 for run and 1 for shutdown, LS Byte\n  Wire.write(0x00); // MS Byte\n  Wire.endTransmission();\n}\n \n// simple function to scan for I2C devices on the bus\nvoid I2Cscan() \n{\n    // scan for i2c devices\n  byte error, address;\n  int nDevices;\n\n  Serial.println(\"Scanning...\");\n\n  nDevices = 0;\n  for(address = 1; address < 127; address++ ) \n  {\n    // The i2c_scanner uses the return value of\n    // the Write.endTransmisstion to see if\n    // a device did acknowledge to the address.\n    Wire.beginTransmission(address);\n    error = Wire.endTransmission();\n\n    if (error == 0)\n    {\n      Serial.print(\"I2C device found at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.print(address,HEX);\n      Serial.println(\"  !\");\n\n      nDevices++;\n    }\n    else if (error==4) \n    {\n      Serial.print(\"Unknown error at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.println(address,HEX);\n    }    \n  }\n  if (nDevices == 0)\n    Serial.println(\"No I2C devices found\\n\");\n  else\n    Serial.println(\"done\\n\");\n}\n\n\n// I2C read/write functions for the BMP280 sensors\n\n  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)\n{\n\tWire.beginTransmission(address);  // Initialize the Tx buffer\n\tWire.write(subAddress);           // Put slave register address in Tx buffer\n\tWire.write(data);                 // Put data in Tx buffer\n\tWire.endTransmission();           // Send the Tx buffer\n}\n\n  uint8_t readByte(uint8_t address, uint8_t subAddress)\n{\n\tuint8_t data; // `data` will store the register data\t \n\tWire.beginTransmission(address);         // Initialize the Tx buffer\n\tWire.write(subAddress);\t                 // Put slave register address in Tx buffer\n\tWire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n\tWire.requestFrom(address, 1);  // Read one byte from slave register address \n\tdata = Wire.read();                      // Fill Rx buffer with result\n\treturn data;                             // Return data read from slave register\n}\n\n  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\n{  \n\tWire.beginTransmission(address);   // Initialize the Tx buffer\n\tWire.write(subAddress);            // Put slave register address in Tx buffer\n\tWire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive\n\tuint8_t i = 0;\n  Wire.requestFrom(address, count);  // Read bytes from slave register address \n\twhile (Wire.available()) {\n        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer\n}\n"
  },
  {
    "path": "Bosch/BNO055_MS5637.ino",
    "content": "/* BNO055_MS5637_ESP32 Basic Example Code\n by: Kris Winer\n date: December 14, 2016\n license: Beerware - Use this code however you'd like. If you \n find it useful you can buy me a beer some time.\n \n Demonstrates basic BNO055 functionality including parameterizing the register addresses, \n initializing the sensor, communicating with pressure sensor MS5637, \n getting properly scaled accelerometer, gyroscope, and magnetometer data out. \n \n Addition of 9 DoF sensor fusion using open source Madgwick and Mahony filter algorithms. \n Can compare results to hardware 9 DoF sensor fusion carried out on the BNO055.\n Sketch runs on the 3.3 V ESP32.\n \n This sketch is intended specifically for the BNO055+MS5637 Add-On Shield.\n It uses SDA/SCL on pins 21/22, respectively, and it uses the Wire library.\n \n The Add-on shield can also be used as a stand-alone breakout board for any Arduino, Teensy, or \n other microcontroller by closing the solder jumpers on the back of the board.\n  \n The MS5637 is a simple but high resolution (24-bit) pressure sensor, which can be used in its high resolution\n mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\n only 1 microAmp. The choice will depend on the application.\n \n All sensors communicate via I2C at 400 kHz or higher.\n SDA and SCL should have external pull-up resistors (to 3.3V).\n 4K7 resistors are on the BNO055_MS5637 breakout board.\n \n Hardware setup:\n Breakout Board --------- ESP32\n 3V3 ---------------------- 3.3V\n SDA -----------------------21\n SCL -----------------------22\n GND ---------------------- GND\n \n */\n#include <Wire.h>   \n\n// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet\n// http://www.meas-spec.com/product/pressure/MS5637-02BA03.aspx\n//\n#define MS5637_RESET      0x1E\n#define MS5637_CONVERT_D1 0x40\n#define MS5637_CONVERT_D2 0x50\n#define MS5637_ADC_READ   0x00\n\n// BNO055 Register Map\n// http://ae-bst.resource.bosch.com/media/products/dokumente/bno055/BST_BNO055_DS000_10_Release.pdf\n//\n// BNO055 Page 0\n#define BNO055_CHIP_ID          0x00    // should be 0xA0              \n#define BNO055_ACC_ID           0x01    // should be 0xFB              \n#define BNO055_MAG_ID           0x02    // should be 0x32              \n#define BNO055_GYRO_ID          0x03    // should be 0x0F              \n#define BNO055_SW_REV_ID_LSB    0x04                                                                          \n#define BNO055_SW_REV_ID_MSB    0x05\n#define BNO055_BL_REV_ID        0x06\n#define BNO055_PAGE_ID          0x07\n#define BNO055_ACC_DATA_X_LSB   0x08\n#define BNO055_ACC_DATA_X_MSB   0x09\n#define BNO055_ACC_DATA_Y_LSB   0x0A\n#define BNO055_ACC_DATA_Y_MSB   0x0B\n#define BNO055_ACC_DATA_Z_LSB   0x0C\n#define BNO055_ACC_DATA_Z_MSB   0x0D\n#define BNO055_MAG_DATA_X_LSB   0x0E\n#define BNO055_MAG_DATA_X_MSB   0x0F\n#define BNO055_MAG_DATA_Y_LSB   0x10\n#define BNO055_MAG_DATA_Y_MSB   0x11\n#define BNO055_MAG_DATA_Z_LSB   0x12\n#define BNO055_MAG_DATA_Z_MSB   0x13\n#define BNO055_GYR_DATA_X_LSB   0x14\n#define BNO055_GYR_DATA_X_MSB   0x15\n#define BNO055_GYR_DATA_Y_LSB   0x16\n#define BNO055_GYR_DATA_Y_MSB   0x17\n#define BNO055_GYR_DATA_Z_LSB   0x18\n#define BNO055_GYR_DATA_Z_MSB   0x19\n#define BNO055_EUL_HEADING_LSB  0x1A\n#define BNO055_EUL_HEADING_MSB  0x1B\n#define BNO055_EUL_ROLL_LSB     0x1C\n#define BNO055_EUL_ROLL_MSB     0x1D\n#define BNO055_EUL_PITCH_LSB    0x1E\n#define BNO055_EUL_PITCH_MSB    0x1F\n#define BNO055_QUA_DATA_W_LSB   0x20\n#define BNO055_QUA_DATA_W_MSB   0x21\n#define BNO055_QUA_DATA_X_LSB   0x22\n#define BNO055_QUA_DATA_X_MSB   0x23\n#define BNO055_QUA_DATA_Y_LSB   0x24\n#define BNO055_QUA_DATA_Y_MSB   0x25\n#define BNO055_QUA_DATA_Z_LSB   0x26\n#define BNO055_QUA_DATA_Z_MSB   0x27\n#define BNO055_LIA_DATA_X_LSB   0x28\n#define BNO055_LIA_DATA_X_MSB   0x29\n#define BNO055_LIA_DATA_Y_LSB   0x2A\n#define BNO055_LIA_DATA_Y_MSB   0x2B\n#define BNO055_LIA_DATA_Z_LSB   0x2C\n#define BNO055_LIA_DATA_Z_MSB   0x2D\n#define BNO055_GRV_DATA_X_LSB   0x2E\n#define BNO055_GRV_DATA_X_MSB   0x2F\n#define BNO055_GRV_DATA_Y_LSB   0x30\n#define BNO055_GRV_DATA_Y_MSB   0x31\n#define BNO055_GRV_DATA_Z_LSB   0x32\n#define BNO055_GRV_DATA_Z_MSB   0x33\n#define BNO055_TEMP             0x34\n#define BNO055_CALIB_STAT       0x35\n#define BNO055_ST_RESULT        0x36\n#define BNO055_INT_STATUS       0x37\n#define BNO055_SYS_CLK_STATUS   0x38\n#define BNO055_SYS_STATUS       0x39\n#define BNO055_SYS_ERR          0x3A\n#define BNO055_UNIT_SEL         0x3B\n#define BNO055_OPR_MODE         0x3D\n#define BNO055_PWR_MODE         0x3E\n#define BNO055_SYS_TRIGGER      0x3F\n#define BNO055_TEMP_SOURCE      0x40\n#define BNO055_AXIS_MAP_CONFIG  0x41\n#define BNO055_AXIS_MAP_SIGN    0x42\n#define BNO055_ACC_OFFSET_X_LSB 0x55\n#define BNO055_ACC_OFFSET_X_MSB 0x56\n#define BNO055_ACC_OFFSET_Y_LSB 0x57\n#define BNO055_ACC_OFFSET_Y_MSB 0x58\n#define BNO055_ACC_OFFSET_Z_LSB 0x59\n#define BNO055_ACC_OFFSET_Z_MSB 0x5A\n#define BNO055_MAG_OFFSET_X_LSB 0x5B\n#define BNO055_MAG_OFFSET_X_MSB 0x5C\n#define BNO055_MAG_OFFSET_Y_LSB 0x5D\n#define BNO055_MAG_OFFSET_Y_MSB 0x5E\n#define BNO055_MAG_OFFSET_Z_LSB 0x5F\n#define BNO055_MAG_OFFSET_Z_MSB 0x60\n#define BNO055_GYR_OFFSET_X_LSB 0x61\n#define BNO055_GYR_OFFSET_X_MSB 0x62\n#define BNO055_GYR_OFFSET_Y_LSB 0x63\n#define BNO055_GYR_OFFSET_Y_MSB 0x64\n#define BNO055_GYR_OFFSET_Z_LSB 0x65\n#define BNO055_GYR_OFFSET_Z_MSB 0x66\n#define BNO055_ACC_RADIUS_LSB   0x67\n#define BNO055_ACC_RADIUS_MSB   0x68\n#define BNO055_MAG_RADIUS_LSB   0x69\n#define BNO055_MAG_RADIUS_MSB   0x6A\n//\n// BNO055 Page 1\n#define BNO055_PAGE_ID          0x07\n#define BNO055_ACC_CONFIG       0x08\n#define BNO055_MAG_CONFIG       0x09\n#define BNO055_GYRO_CONFIG_0    0x0A\n#define BNO055_GYRO_CONFIG_1    0x0B\n#define BNO055_ACC_SLEEP_CONFIG 0x0C\n#define BNO055_GYR_SLEEP_CONFIG 0x0D\n#define BNO055_INT_MSK          0x0F\n#define BNO055_INT_EN           0x10\n#define BNO055_ACC_AM_THRES     0x11\n#define BNO055_ACC_INT_SETTINGS 0x12\n#define BNO055_ACC_HG_DURATION  0x13\n#define BNO055_ACC_HG_THRESH    0x14\n#define BNO055_ACC_NM_THRESH    0x15\n#define BNO055_ACC_NM_SET       0x16\n#define BNO055_GYR_INT_SETTINGS 0x17\n#define BNO055_GYR_HR_X_SET     0x18\n#define BNO055_GYR_DUR_X        0x19\n#define BNO055_GYR_HR_Y_SET     0x1A\n#define BNO055_GYR_DUR_Y        0x1B\n#define BNO055_GYR_HR_Z_SET     0x1C\n#define BNO055_GYR_DUR_Z        0x1D\n#define BNO055_GYR_AM_THRESH    0x1E\n#define BNO055_GYR_AM_SET       0x1F\n\n// Using the BNO055_MS5637 breakout board/Teensy 3.1 Add-On Shield, ADO is set to 1 by default \n#define ADO 1\n#if ADO\n#define BNO055_ADDRESS 0x29   //  Device address of BNO055 when ADO = 1\n#define MS5637_ADDRESS 0x76   //  Address of MS5637 altimeter\n#else\n#define BNO055_ADDRESS 0x28   //  Device address of BNO055 when ADO = 0\n#define MS5637_ADDRESS 0x76   //  Address of MS5637 altimeter\n#endif  \n\n#define SerialDebug true      // set to true to get Serial output for debugging\n\n// Set initial input parameters\nenum Ascale {  // ACC Full Scale\n  AFS_2G = 0,\n  AFS_4G,\n  AFS_8G,\n  AFS_18G\n};\n\nenum Abw { // ACC Bandwidth\n  ABW_7_81Hz = 0,\n  ABW_15_63Hz,\n  ABW_31_25Hz,\n  ABW_62_5Hz,\n  ABW_125Hz,    \n  ABW_250Hz,\n  ABW_500Hz,     \n  ABW_1000Hz,    //0x07\n};\n\nenum APwrMode { // ACC Pwr Mode\n  NormalA = 0,  \n  SuspendA,\n  LowPower1A,\n  StandbyA,        \n  LowPower2A,\n  DeepSuspendA\n};\n\nenum Gscale {  // gyro full scale\n  GFS_2000DPS = 0,\n  GFS_1000DPS,\n  GFS_500DPS,\n  GFS_250DPS,\n  GFS_125DPS    // 0x04\n};\n\nenum GPwrMode { // GYR Pwr Mode\n  NormalG = 0,\n  FastPowerUpG,\n  DeepSuspendedG,\n  SuspendG,\n  AdvancedPowerSaveG\n};\n\nenum Gbw { // gyro bandwidth\n  GBW_523Hz = 0,\n  GBW_230Hz,\n  GBW_116Hz,\n  GBW_47Hz,\n  GBW_23Hz,\n  GBW_12Hz,\n  GBW_64Hz,\n  GBW_32Hz\n};\n\nenum OPRMode {  // BNO-55 operation modes\n  CONFIGMODE = 0x00,\n// Sensor Mode\n  ACCONLY,\n  MAGONLY,\n  GYROONLY,\n  ACCMAG,\n  ACCGYRO,\n  MAGGYRO,\n  AMG,            // 0x07\n// Fusion Mode\n  IMU,\n  COMPASS,\n  M4G,\n  NDOF_FMC_OFF,\n  NDOF            // 0x0C\n};\n\nenum PWRMode {\n  Normalpwr = 0,   \n  Lowpower,       \n  Suspendpwr       \n};\n\nenum Modr {         // magnetometer output data rate  \n  MODR_2Hz = 0,     \n  MODR_6Hz,\n  MODR_8Hz,\n  MODR_10Hz,  \n  MODR_15Hz,\n  MODR_20Hz,\n  MODR_25Hz, \n  MODR_30Hz \n};\n\nenum MOpMode { // MAG Op Mode\n  LowPower = 0,\n  Regular,\n  EnhancedRegular,\n  HighAccuracy\n};\n\nenum MPwrMode { // MAG power mode\n  Normal = 0,   \n  Sleep,     \n  Suspend,\n  ForceMode  \n};\n\n#define ADC_256  0x00 // define pressure and temperature conversion rates\n#define ADC_512  0x02\n#define ADC_1024 0x04\n#define ADC_2048 0x06\n#define ADC_4096 0x08\n#define ADC_8192 0x0A\n#define ADC_D1   0x40\n#define ADC_D2   0x50\n\n// Specify sensor configuration\nuint8_t OSR = ADC_8192;       // set pressure amd temperature oversample rate\n//\nuint8_t GPwrMode = NormalG;    // Gyro power mode\nuint8_t Gscale = GFS_250DPS;  // Gyro full scale\n//uint8_t Godr = GODR_250Hz;    // Gyro sample rate\nuint8_t Gbw = GBW_23Hz;       // Gyro bandwidth\n//\nuint8_t Ascale = AFS_2G;      // Accel full scale\n//uint8_t Aodr = AODR_250Hz;    // Accel sample rate\nuint8_t APwrMode = NormalA;    // Accel power mode\nuint8_t Abw = ABW_31_25Hz;    // Accel bandwidth, accel sample rate divided by ABW_divx\n//\n//uint8_t Mscale = MFS_4Gauss;  // Select magnetometer full-scale resolution\nuint8_t MOpMode = Regular;    // Select magnetometer perfomance mode\nuint8_t MPwrMode = Normal;    // Select magnetometer power mode\nuint8_t Modr = MODR_10Hz;     // Select magnetometer ODR when in BNO055 bypass mode\n\nuint8_t PWRMode = Normalpwr;    // Select BNO055 power mode\nuint8_t OPRMode = NDOF;       // specify operation mode for sensors\nuint8_t status;               // BNO055 data status register\nfloat aRes, gRes, mRes;       // scale resolutions per LSB for the sensors\n  \n// Pin definitions\nint intPin = 14;  // These can be changed, 2 and 3 are the Arduinos ext int pins\nint myLed = 5;\n\nuint16_t Pcal[8];         // calibration constants from MS5637 PROM registers\nunsigned char nCRC;       // calculated check sum to ensure PROM integrity\nuint32_t D1 = 0, D2 = 0;  // raw MS5637 pressure and temperature data\ndouble dT, OFFSET, SENS, TT2, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data\nint16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output\nint16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output\nint16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output\nint16_t quatCount[4];   // Stores the 16-bit signed quaternion output\nint16_t EulCount[3];    // Stores the 16-bit signed Euler angle output\nint16_t LIACount[3];    // Stores the 16-bit signed linear acceleration output\nint16_t GRVCount[3];    // Stores the 16-bit signed gravity vector output\nfloat gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0};  // Bias corrections for gyro, accelerometer, and magnetometer\nint16_t tempGCount, tempMCount;      // temperature raw count output of mag and gyro\nfloat   Gtemperature, Mtemperature;  // Stores the BNO055 gyro and LIS3MDL mag internal chip temperatures in degrees Celsius\ndouble Temperature, Pressure;        // stores MS5637 pressures sensor pressure and temperature\n\n// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)\nfloat GyroMeasError = PI * (40.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 40 deg/s)\nfloat GyroMeasDrift = PI * (0.0f  / 180.0f);   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)\n// There is a tradeoff in the beta parameter between accuracy and response speed.\n// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.\n// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.\n// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!\n// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec\n// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; \n// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. \n// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.\nfloat beta = sqrt(3.0f / 4.0f) * GyroMeasError;   // compute beta\nfloat 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\n#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\n#define Ki 0.0f\n\nuint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate\nfloat pitch, yaw, roll;\nfloat Pitch, Yaw, Roll;\nfloat LIAx, LIAy, LIAz, GRVx, GRVy, GRVz;\nfloat deltat = 0.0f, sum = 0.0f;          // integration interval for both filter schemes\nuint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval\nuint32_t Now = 0;                         // used to calculate integration interval\n\nfloat ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values \nfloat q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion\nfloat quat[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion\nfloat eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method\n\n\nvoid setup()\n{\n  Wire.begin(21, 22, 400000); // (SDA, SCL) (21, 22) are default on ESP32, 400 kHz I2C bus speed\n  delay(5000);\n  Serial.begin(115200);\n  \n  // Set up the interrupt pin, its set as active high, push-pull\n  pinMode(intPin, INPUT);\n  pinMode(myLed, OUTPUT);\n  digitalWrite(myLed, HIGH);\n\n  I2Cscan(); // check for I2C devices on the bus8\n  \n  // Read the WHO_AM_I register, this is a good test of communication\n  Serial.println(\"BNO055 9-axis motion sensor...\");\n  byte c = readByte(BNO055_ADDRESS, BNO055_CHIP_ID);  // Read WHO_AM_I register for BNO055\n  Serial.print(\"BNO055 Address = 0x\"); Serial.println(BNO055_ADDRESS, HEX);\n  Serial.print(\"BNO055 WHO_AM_I = 0x\"); Serial.println(BNO055_CHIP_ID, HEX);\n  Serial.print(\"BNO055 \"); Serial.print(\"I AM \"); Serial.print(c, HEX); Serial.println(\" I should be 0xA0\");  \n\n  delay(1000); \n   \n    // Read the WHO_AM_I register of the accelerometer, this is a good test of communication\n  byte d = readByte(BNO055_ADDRESS, BNO055_ACC_ID);  // Read WHO_AM_I register for accelerometer\n  Serial.print(\"BNO055 ACC \"); Serial.print(\"I AM \"); Serial.print(d, HEX); Serial.println(\" I should be 0xFB\");  \n\n  delay(1000); \n  \n  // Read the WHO_AM_I register of the magnetometer, this is a good test of communication\n  byte e = readByte(BNO055_ADDRESS, BNO055_MAG_ID);  // Read WHO_AM_I register for magnetometer\n  Serial.print(\"BNO055 MAG \"); Serial.print(\"I AM \"); Serial.print(e, HEX); Serial.println(\" I should be 0x32\");\n\n  delay(1000);   \n  \n  // Read the WHO_AM_I register of the gyroscope, this is a good test of communication\n  byte f = readByte(BNO055_ADDRESS, BNO055_GYRO_ID);  // Read WHO_AM_I register for LIS3MDL\n  Serial.print(\"BNO055 GYRO \"); Serial.print(\"I AM \"); Serial.print(f, HEX); Serial.println(\" I should be 0x0F\");\n\n  delay(1000); \n\n  if (c == 0xA0) // BNO055 WHO_AM_I should always be 0xA0\n  {  \n    Serial.println(\"BNO055 is online...\");\n    \n    // Check software revision ID\n    byte swlsb = readByte(BNO055_ADDRESS, BNO055_SW_REV_ID_LSB);\n    byte swmsb = readByte(BNO055_ADDRESS, BNO055_SW_REV_ID_MSB);\n    Serial.print(\"BNO055 SW Revision ID: \"); Serial.print(swmsb, HEX); Serial.print(\".\"); Serial.println(swlsb, HEX); \n    Serial.println(\"Should be 03.04\");\n    \n    // Check bootloader version\n    byte blid = readByte(BNO055_ADDRESS, BNO055_BL_REV_ID);\n    Serial.print(\"BNO055 bootloader Version: \"); Serial.println(blid); \n    \n    // Check self-test results\n    byte selftest = readByte(BNO055_ADDRESS, BNO055_ST_RESULT);\n    \n    if(selftest & 0x01) {\n      Serial.println(\"accelerometer passed selftest\"); \n    } else {\n      Serial.println(\"accelerometer failed selftest\"); \n    }\n    if(selftest & 0x02) {\n      Serial.println(\"magnetometer passed selftest\"); \n    } else {\n      Serial.println(\"magnetometer failed selftest\"); \n    }  \n    if(selftest & 0x04) {\n      Serial.println(\"gyroscope passed selftest\"); \n    } else {\n      Serial.println(\"gyroscope failed selftest\"); \n    }      \n    if(selftest & 0x08) {\n      Serial.println(\"MCU passed selftest\"); \n    } else {\n      Serial.println(\"MCU failed selftest\"); \n    }\n      \n    delay(1000);\n  \n  // Reset the MS5637 pressure sensor\n  MS5637Reset();\n  delay(100);\n  Serial.println(\"MS5637 pressure sensor reset...\");\n  // Read PROM data from MS5637 pressure sensor\n  MS5637PromRead(Pcal);\n  Serial.println(\"PROM data read:\");\n  Serial.print(\"C0 = \"); Serial.println(Pcal[0]);\n  unsigned char refCRC = Pcal[0] >> 12;\n  Serial.print(\"C1 = \"); Serial.println(Pcal[1]);\n  Serial.print(\"C2 = \"); Serial.println(Pcal[2]);\n  Serial.print(\"C3 = \"); Serial.println(Pcal[3]);\n  Serial.print(\"C4 = \"); Serial.println(Pcal[4]);\n  Serial.print(\"C5 = \"); Serial.println(Pcal[5]);\n  Serial.print(\"C6 = \"); Serial.println(Pcal[6]);\n  \n  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data\n  Serial.print(\"Checksum = \"); Serial.print(nCRC); Serial.print(\" , should be \"); Serial.println(refCRC);  \n  \n  delay(1000);  \n \n  accelgyroCalBNO055(accelBias, gyroBias);\n  \n  Serial.println(\"Average accelerometer bias (mg) = \"); Serial.println(accelBias[0]); Serial.println(accelBias[1]); Serial.println(accelBias[2]);\n  Serial.println(\"Average gyro bias (dps) = \"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);\n  \n  delay(1000); \n  \n  magCalBNO055(magBias);\n  \n  Serial.println(\"Average magnetometer bias (mG) = \"); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]);\n \n  delay(1000); \n  \n  // Check calibration status of the sensors\n  uint8_t calstat = readByte(BNO055_ADDRESS, BNO055_CALIB_STAT);\n  Serial.println(\"Not calibrated = 0, fully calibrated = 3\");\n  Serial.print(\"System calibration status \"); Serial.println( (0xC0 & calstat) >> 6);\n  Serial.print(\"Gyro   calibration status \"); Serial.println( (0x30 & calstat) >> 4);\n  Serial.print(\"Accel  calibration status \"); Serial.println( (0x0C & calstat) >> 2);\n  Serial.print(\"Mag    calibration status \"); Serial.println( (0x03 & calstat) >> 0);\n  \n  initBNO055(); // Initialize the BNO055\n  Serial.println(\"BNO055 initialized for sensor mode....\"); // Initialize BNO055 for sensor read \n \n  }\n  else\n  {\n    Serial.print(\"Could not connect to BNO055: 0x\");\n    Serial.println(c, HEX);\n    while(1) ; // Loop forever if communication doesn't happen\n  }\n}\n\nvoid loop()\n{  \n    readAccelData(accelCount);  // Read the x/y/z adc values\n    // Now we'll calculate the accleration value into actual mg's\n    ax = (float)accelCount[0]; // - accelBias[0];  // subtract off calculated accel bias\n    ay = (float)accelCount[1]; // - accelBias[1];\n    az = (float)accelCount[2]; // - accelBias[2]; \n\n    readGyroData(gyroCount);  // Read the x/y/z adc values\n    // Calculate the gyro value into actual degrees per second\n    gx = (float)gyroCount[0]/16.; // - gyroBias[0];  // subtract off calculated gyro bias\n    gy = (float)gyroCount[1]/16.; // - gyroBias[1];  \n    gz = (float)gyroCount[2]/16.; // - gyroBias[2];   \n\n    readMagData(magCount);  // Read the x/y/z adc values   \n    // Calculate the magnetometer values in milliGauss\n    mx = (float)magCount[0]/1.6; // - magBias[0];  // get actual magnetometer value in mGauss \n    my = (float)magCount[1]/1.6; // - magBias[1];  \n    mz = (float)magCount[2]/1.6; // - magBias[2];   \n    \n    readQuatData(quatCount);  // Read the x/y/z adc values   \n    // Calculate the quaternion values  \n    quat[0] = (float)(quatCount[0])/16384.;    \n    quat[1] = (float)(quatCount[1])/16384.;  \n    quat[2] = (float)(quatCount[2])/16384.;   \n    quat[3] = (float)(quatCount[3])/16384.;   \n    \n    readEulData(EulCount);  // Read the x/y/z adc values   \n    // Calculate the Euler angles values in degrees\n    Yaw = (float)EulCount[0]/16.;  \n    Roll = (float)EulCount[1]/16.;  \n    Pitch = (float)EulCount[2]/16.;   \n \n    readLIAData(LIACount);  // Read the x/y/z adc values   \n    // Calculate the linear acceleration (sans gravity) values in mg\n    LIAx = (float)LIACount[0];  \n    LIAy = (float)LIACount[1];  \n    LIAz = (float)LIACount[2];   \n\n    readGRVData(GRVCount);  // Read the x/y/z adc values   \n    // Calculate the linear acceleration (sans gravity) values in mg\n    GRVx = (float)GRVCount[0];  \n    GRVy = (float)GRVCount[1];  \n    GRVz = (float)GRVCount[2];   \n    \n  \n  Now = micros();\n  deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update\n  lastUpdate = Now;\n  \n  sum += deltat; // sum for averaging filter update rate\n  sumCount++;\n  \n  // Sensors x, y, and z-axes  for the three sensor: accel, gyro, and magnetometer are all aligned, so\n  // no allowance for any orientation mismatch in feeding the output to the quaternion filter is required.\n  // For the BNO055, the sensor forward is along the x-axis just like\n  // in the LSM9DS0 and MPU9250 sensors. This rotation can be modified to allow any convenient orientation convention.\n  // This is ok by aircraft orientation standards!  \n  // Pass gyro rate as rad/s\n  MadgwickQuaternionUpdate(ax, -ay, -az, gx*PI/180.0f, -gy*PI/180.0f, -gz*PI/180.0f,  -my,  -mx, -mz);\n//  MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, mx, my, mz);\n    \n    // Serial print and/or display at 0.5 s rate independent of data rates\n    delt_t = millis() - count;\n    if (delt_t > 1000) { // update LCD once per half-second independent of read rate\n    \n  if (SerialDebug) {\n  // check BNO-055 error status at 2 Hz rate\n    uint8_t sysstat = readByte(BNO055_ADDRESS, BNO055_SYS_STATUS); // check system status\n    Serial.print(\"System Status = 0x\"); Serial.println(sysstat, HEX);\n    if(sysstat == 0x05) Serial.println(\"Sensor fusion algorithm running\");\n    if(sysstat == 0x06) Serial.println(\"Sensor fusion not algorithm running\");\n    \n    if(sysstat == 0x01) {\n      uint8_t syserr = readByte(BNO055_ADDRESS, BNO055_SYS_ERR);\n      if(syserr == 0x01) Serial.println(\"Peripheral initialization error\");\n      if(syserr == 0x02) Serial.println(\"System initialization error\");\n      if(syserr == 0x03) Serial.println(\"Self test result failed\");\n      if(syserr == 0x04) Serial.println(\"Register map value out of range\");\n      if(syserr == 0x05) Serial.println(\"Register map address out of range\");\n      if(syserr == 0x06) Serial.println(\"Register map write error\");\n      if(syserr == 0x07) Serial.println(\"BNO low power mode no available for selected operation mode\");\n      if(syserr == 0x08) Serial.println(\"Accelerometer power mode not available\");\n      if(syserr == 0x09) Serial.println(\"Fusion algorithm configuration error\");\n      if(syserr == 0x0A) Serial.println(\"Sensor configuration error\");    \n    }  \n  }\n\n    if(SerialDebug) {\n    Serial.print(\"ax = \"); Serial.print((int)ax);  \n    Serial.print(\" ay = \"); Serial.print((int)ay); \n    Serial.print(\" az = \"); Serial.print((int)az); Serial.println(\" mg\");\n    Serial.print(\"gx = \"); Serial.print( gx, 2); \n    Serial.print(\" gy = \"); Serial.print( gy, 2); \n    Serial.print(\" gz = \"); Serial.print( gz, 2); Serial.println(\" deg/s\");\n    Serial.print(\"mx = \"); Serial.print( (int)mx ); \n    Serial.print(\" my = \"); Serial.print( (int)my ); \n    Serial.print(\" mz = \"); Serial.print( (int)mz ); Serial.println(\" mG\");\n    \n    Serial.print(\"qx = \"); Serial.print(q[0]);\n    Serial.print(\" qy = \"); Serial.print(q[1]); \n    Serial.print(\" qz = \"); Serial.print(q[2]); \n    Serial.print(\" qw = \"); Serial.println(q[3]); \n    Serial.print(\"quatw = \"); Serial.print(quat[0]);\n    Serial.print(\" quatx = \"); Serial.print(quat[1]); \n    Serial.print(\" quaty = \"); Serial.print(quat[2]); \n    Serial.print(\" quatz = \"); Serial.println(quat[3]); \n    } \n    \n    if(SerialDebug) {\n      tempGCount = readGyroTempData();  // Read the gyro adc values\n    Gtemperature = (float) tempGCount; // Gyro chip temperature in degrees Centigrade\n   // Print gyro die temperature in degrees Centigrade      \n    Serial.print(\"Gyro temperature is \");  Serial.print(Gtemperature, 1);  Serial.println(\" degrees C\"); // Print T values to tenths of a degree C\n    }\n    \n    D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value\n    D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value\n    \n    dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference\n    OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);\n    SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);\n \n    Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade\n//\n// Second order corrections\n    if(Temperature > 20) \n    {\n      TT2 = 5*dT*dT/pow(2, 38); // correction for high temperatures\n      OFFSET2 = 0;\n      SENS2 = 0;\n    }\n    if(Temperature < 20)                   // correction for low temperature\n    {\n      TT2      = 3*dT*dT/pow(2, 33); \n      OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;\n      SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;\n    } \n    if(Temperature < -15)                      // correction for very low temperature\n    {\n      OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);\n      SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);\n    }\n // End of second order corrections\n //\n     Temperature = Temperature - TT2/100;\n     OFFSET = OFFSET - OFFSET2;\n     SENS = SENS - SENS2;\n \n     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or Pa/100\n  \n    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));\n   \n    if(SerialDebug) {\n    Serial.print(\"Digital temperature value = \"); Serial.print( (float)Temperature, 2); Serial.println(\" C\"); // temperature in degrees Celsius\n    Serial.print(\"Digital temperature value = \"); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(\" F\"); // temperature in degrees Fahrenheit\n    Serial.print(\"Digital pressure value = \"); Serial.print((float) Pressure, 2);  Serial.println(\" mbar\");// pressure in millibar\n    Serial.print(\"Altitude = \"); Serial.print(altitude, 2); Serial.println(\" feet\");\n    }\n    \n  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.\n  // In this coordinate system, the positive z-axis is down toward Earth. \n  // 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.\n  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.\n  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.\n  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.\n  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be\n  // applied in the correct order which for this configuration is yaw, pitch, and then roll.\n  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.\n    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]);   \n    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));\n    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]);\n    pitch *= 180.0f / PI;\n    yaw   *= 180.0f / PI; \n //   yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04\n    roll  *= 180.0f / PI;\n     \n    if(SerialDebug) {\n    Serial.print(\"Software Yaw, Pitch, Roll: \");\n    Serial.print(yaw, 2);\n    Serial.print(\", \");\n    Serial.print(pitch, 2);\n    Serial.print(\", \");\n    Serial.println(roll, 2);\n    \n    Serial.print(\"Hardware Yaw, Pitch, Roll: \");\n    Serial.print(Yaw, 2);\n    Serial.print(\", \");\n    Serial.print(Pitch, 2);\n    Serial.print(\", \");\n    Serial.println(Roll, 2);\n \n    Serial.print(\"Hardware x, y, z linear acceleration: \");\n    Serial.print(LIAx, 2);\n    Serial.print(\", \");\n    Serial.print(LIAy, 2);\n    Serial.print(\", \");\n    Serial.println(LIAz, 2);\n\n    Serial.print(\"Hardware x, y, z gravity vector: \");\n    Serial.print(GRVx, 2);\n    Serial.print(\", \");\n    Serial.print(GRVy, 2);\n    Serial.print(\", \");\n    Serial.println(GRVz, 2);\n \n    \n    Serial.print(\"rate = \"); Serial.print((float)sumCount/sum, 2); Serial.println(\" Hz\");\n    }\n\n     Serial.print(millis()/1000);Serial.print(\",\");\n     Serial.print(yaw, 2); Serial.print(\",\");Serial.print(pitch, 2); Serial.print(\",\");Serial.print(roll, 2); Serial.print(\",\");\n     Serial.print(Yaw, 2); Serial.print(\",\");Serial.print(Pitch, 2); Serial.print(\",\");Serial.print(Roll, 2); Serial.println(\",\");\n  \n    digitalWrite(myLed, !digitalRead(myLed));\n    count = millis(); \n    sumCount = 0;\n    sum = 0;    \n    }\n\n}\n\n//===================================================================================================================\n//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data\n//===================================================================================================================\n\nvoid readAccelData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z accel register data stored here\n  readBytes(BNO055_ADDRESS, BNO055_ACC_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers into data array\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;      // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; \n}\n\n\nvoid readGyroData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  readBytes(BNO055_ADDRESS, BNO055_GYR_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; \n}\n\nint8_t readGyroTempData()\n{\n  return readByte(BNO055_ADDRESS, BNO055_TEMP);  // Read the two raw data registers sequentially into data array \n}\n\nvoid readMagData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  readBytes(BNO055_ADDRESS, BNO055_MAG_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;\n}\n\nvoid readQuatData(int16_t * destination)\n{\n  uint8_t rawData[8];  // x/y/z gyro register data stored here\n  readBytes(BNO055_ADDRESS, BNO055_QUA_DATA_W_LSB, 8, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;\n  destination[3] = ((int16_t)rawData[7] << 8) | rawData[6] ;\n}\n\nvoid readEulData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  readBytes(BNO055_ADDRESS, BNO055_EUL_HEADING_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;\n}\n\nvoid readLIAData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  readBytes(BNO055_ADDRESS, BNO055_LIA_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;\n}\n\nvoid readGRVData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  readBytes(BNO055_ADDRESS, BNO055_GRV_DATA_X_LSB, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;\n}\n\nvoid initBNO055() {\n   // Select BNO055 config mode\n   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );\n   delay(25);\n   // Select page 1 to configure sensors\n   writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x01);\n   // Configure ACC\n   writeByte(BNO055_ADDRESS, BNO055_ACC_CONFIG, APwrMode << 5 | Abw << 2 | Ascale );\n   // Configure GYR\n   writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_0, Gbw << 3 | Gscale );\n   writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_1, GPwrMode);\n   // Configure MAG\n   writeByte(BNO055_ADDRESS, BNO055_MAG_CONFIG, MPwrMode << 5 | MOpMode << 3 | Modr );\n   \n   // Select page 0 to read sensors\n   writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);\n\n   // Select BNO055 gyro temperature source \n   writeByte(BNO055_ADDRESS, BNO055_TEMP_SOURCE, 0x01 );\n\n   // Select BNO055 sensor units (temperature in degrees C, rate in dps, accel in mg)\n   writeByte(BNO055_ADDRESS, BNO055_UNIT_SEL, 0x01 );\n   \n   // Select BNO055 system power mode\n   writeByte(BNO055_ADDRESS, BNO055_PWR_MODE, PWRMode );\n \n   // Select BNO055 system operation mode\n   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );\n   delay(25);\n   \n}\n\nvoid accelgyroCalBNO055(float * dest1, float * dest2) \n{\n  uint8_t data[6]; // data array to hold accelerometer and gyro x, y, z, data\n  uint16_t ii = 0, sample_count = 0;\n  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};\n \n  Serial.println(\"Accel/Gyro Calibration: Put device on a level surface and keep motionless! Wait......\");\n  delay(4000);\n  \n  // Select page 0 to read sensors\n   writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);\n   // Select BNO055 system operation mode as AMG for calibration\n   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );\n   delay(25);\n   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, AMG);\n   \n // In NDF fusion mode, accel full scale is at +/- 4g, ODR is 62.5 Hz, set it the same here\n   writeByte(BNO055_ADDRESS, BNO055_ACC_CONFIG, APwrMode << 5 | Abw << 2 | AFS_4G );\n   sample_count = 256;\n   for(ii = 0; ii < sample_count; ii++) {\n    int16_t accel_temp[3] = {0, 0, 0};\n    readBytes(BNO055_ADDRESS, BNO055_ACC_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array\n    accel_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ; // Form signed 16-bit integer for each sample in FIFO\n    accel_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;\n    accel_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;\n    accel_bias[0]  += (int32_t) accel_temp[0];\n    accel_bias[1]  += (int32_t) accel_temp[1];\n    accel_bias[2]  += (int32_t) accel_temp[2];\n    delay(20);  // at 62.5 Hz ODR, new accel data is available every 16 ms\n   }\n    accel_bias[0]  /= (int32_t) sample_count;  // get average accel bias in mg\n    accel_bias[1]  /= (int32_t) sample_count;\n    accel_bias[2]  /= (int32_t) sample_count;\n    \n  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) 1000;}  // Remove gravity from the z-axis accelerometer bias calculation\n  else {accel_bias[2] += (int32_t) 1000;}\n\n    dest1[0] = (float) accel_bias[0];  // save accel biases in mg for use in main program\n    dest1[1] = (float) accel_bias[1];  // accel data is 1 LSB/mg\n    dest1[2] = (float) accel_bias[2];          \n\n // In NDF fusion mode, gyro full scale is at +/- 2000 dps, ODR is 32 Hz\n   writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_0, Gbw << 3 | GFS_2000DPS );\n   writeByte(BNO055_ADDRESS, BNO055_GYRO_CONFIG_1, GPwrMode);\n   for(ii = 0; ii < sample_count; ii++) {\n    int16_t gyro_temp[3] = {0, 0, 0};\n    readBytes(BNO055_ADDRESS, BNO055_GYR_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array\n    gyro_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ;  // Form signed 16-bit integer for each sample in FIFO\n    gyro_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;\n    gyro_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;\n    gyro_bias[0]  += (int32_t) gyro_temp[0];\n    gyro_bias[1]  += (int32_t) gyro_temp[1];\n    gyro_bias[2]  += (int32_t) gyro_temp[2];\n    delay(35);  // at 32 Hz ODR, new gyro data available every 31 ms\n   }\n    gyro_bias[0]  /= (int32_t) sample_count;  // get average gyro bias in counts\n    gyro_bias[1]  /= (int32_t) sample_count;\n    gyro_bias[2]  /= (int32_t) sample_count;\n \n    dest2[0] = (float) gyro_bias[0]/16.;  // save gyro biases in dps for use in main program\n    dest2[1] = (float) gyro_bias[1]/16.;  // gyro data is 16 LSB/dps\n    dest2[2] = (float) gyro_bias[2]/16.;          \n\n  // Return to config mode to write accelerometer biases in offset register\n  // This offset register is only used while in fusion mode when accelerometer full-scale is +/- 4g\n  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );\n  delay(25);\n  \n  //write biases to accelerometer offset registers ad 16 LSB/dps\n  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_LSB, (int16_t)accel_bias[0] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_MSB, ((int16_t)accel_bias[0] >> 8) & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_LSB, (int16_t)accel_bias[1] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_MSB, ((int16_t)accel_bias[1] >> 8) & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_LSB, (int16_t)accel_bias[2] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_MSB, ((int16_t)accel_bias[2] >> 8) & 0xFF);\n  \n  // Check that offsets were properly written to offset registers\n//  Serial.println(\"Average accelerometer bias = \"); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_X_LSB))); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Y_LSB))); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_ACC_OFFSET_Z_LSB)));\n\n   //write biases to gyro offset registers\n  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_LSB, (int16_t)gyro_bias[0] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_MSB, ((int16_t)gyro_bias[0] >> 8) & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_LSB, (int16_t)gyro_bias[1] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_MSB, ((int16_t)gyro_bias[1] >> 8) & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_LSB, (int16_t)gyro_bias[2] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_MSB, ((int16_t)gyro_bias[2] >> 8) & 0xFF);\n  \n  // Select BNO055 system operation mode\n  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );\n\n // Check that offsets were properly written to offset registers\n//  Serial.println(\"Average gyro bias = \"); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_X_LSB))); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Y_LSB))); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_GYR_OFFSET_Z_LSB)));\n\n   Serial.println(\"Accel/Gyro Calibration done!\");\n}\n\nvoid magCalBNO055(float * dest1) \n{\n  uint8_t data[6]; // data array to hold mag x, y, z, data\n  uint16_t ii = 0, sample_count = 0;\n  int32_t mag_bias[3] = {0, 0, 0};\n  int16_t mag_max[3] = {0x8000, 0x8000, 0x8000}, mag_min[3] = {0x7FFF, 0x7FFF, 0x7FFF};\n \n  Serial.println(\"Mag Calibration: Wave device in a figure eight until done!\");\n  delay(4000);\n  \n  // Select page 0 to read sensors\n   writeByte(BNO055_ADDRESS, BNO055_PAGE_ID, 0x00);\n   // Select BNO055 system operation mode as AMG for calibration\n   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );\n   delay(25);\n   writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, AMG );\n\n // In NDF fusion mode, mag data is in 16 LSB/microTesla, ODR is 20 Hz in forced mode\n   sample_count = 256;\n   for(ii = 0; ii < sample_count; ii++) {\n    int16_t mag_temp[3] = {0, 0, 0};\n    readBytes(BNO055_ADDRESS, BNO055_MAG_DATA_X_LSB, 6, &data[0]);  // Read the six raw data registers into data array\n    mag_temp[0] = (int16_t) (((int16_t)data[1] << 8) | data[0]) ;   // Form signed 16-bit integer for each sample in FIFO\n    mag_temp[1] = (int16_t) (((int16_t)data[3] << 8) | data[2]) ;\n    mag_temp[2] = (int16_t) (((int16_t)data[5] << 8) | data[4]) ;\n    for (int jj = 0; jj < 3; jj++) {\n      if (ii == 0) {\n        mag_max[jj] = mag_temp[jj]; // Offsets may be large enough that mag_temp[i] may not be bipolar! \n        mag_min[jj] = mag_temp[jj]; // This prevents max or min being pinned to 0 if the values are unipolar...\n      } else {\n        if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];\n        if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];\n      }\n    }\n    delay(105);  // at 10 Hz ODR, new mag data is available every 100 ms\n   }\n\n //   Serial.println(\"mag x min/max:\"); Serial.println(mag_max[0]); Serial.println(mag_min[0]);\n //   Serial.println(\"mag y min/max:\"); Serial.println(mag_max[1]); Serial.println(mag_min[1]);\n //   Serial.println(\"mag z min/max:\"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);\n\n    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts\n    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts\n    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts\n    \n    dest1[0] = (float) mag_bias[0] / 1.6;  // save mag biases in mG for use in main program\n    dest1[1] = (float) mag_bias[1] / 1.6;  // mag data is 1.6 LSB/mg\n    dest1[2] = (float) mag_bias[2] / 1.6;          \n\n  // Return to config mode to write mag biases in offset register\n  // This offset register is only used while in fusion mode when magnetometer sensitivity is 16 LSB/microTesla\n  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, CONFIGMODE );\n  delay(25);\n  \n  //write biases to magnetometer offset registers as 16 LSB/microTesla\n  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_LSB, (int16_t)mag_bias[0] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_MSB, ((int16_t)mag_bias[0] >> 8) & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_LSB, (int16_t)mag_bias[1] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_MSB, ((int16_t)mag_bias[1] >> 8) & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_LSB, (int16_t)mag_bias[2] & 0xFF);\n  writeByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_MSB, ((int16_t)mag_bias[2] >> 8) & 0xFF);\n \n  // Check that offsets were properly written to offset registers\n//  Serial.println(\"Average magnetometer bias = \"); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_X_LSB))); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Y_LSB))); \n//  Serial.println((int16_t)((int16_t)readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_MSB) << 8 | readByte(BNO055_ADDRESS, BNO055_MAG_OFFSET_Z_LSB)));\n  // Select BNO055 system operation mode\n  writeByte(BNO055_ADDRESS, BNO055_OPR_MODE, OPRMode );\n\n   Serial.println(\"Mag Calibration done!\");\n}\n\n\n// I2C communication with the MS5637 is a little different from that with the BNO055 and most other sensors\n// For the MS5637, we write commands, and the MS5637 sends data in response, rather than directly reading\n// MS5637 registers\n\n        void MS5637Reset()\n        {\n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n\tWire.write(MS5637_RESET);                // Put reset command in Tx buffer\n\tWire.endTransmission();                  // Send the Tx buffer\n        }\n        \n        void MS5637PromRead(uint16_t * destination)\n        {\n        uint8_t data[2] = {0,0};\n        for (uint8_t ii = 0; ii < 7; ii++) {\n          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer\n          Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive\n//          Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive\n\t  uint8_t i = 0;\n          Wire.requestFrom(MS5637_ADDRESS, 2);   // Read two bytes from slave PROM address \n\t  while (Wire.available()) {\n          data[i++] = Wire.read(); }               // Put read results in the Rx buffer\n          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program\n        }\n        }\n\n        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read\n        {\n        uint8_t data[3] = {0,0,0};\n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(CMD | OSR);                  // Put pressure conversion command in Tx buffer\n        Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive\n//        Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive\n        \n        switch (OSR)\n        {\n          case ADC_256: delay(1); break;  // delay for conversion to complete\n          case ADC_512: delay(3); break;\n          case ADC_1024: delay(4); break;\n          case ADC_2048: delay(6); break;\n          case ADC_4096: delay(10); break;\n          case ADC_8192: delay(20); break;\n        }\n       \n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(0x00);                        // Put ADC read command in Tx buffer\n//        Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive\n        Wire.endTransmission(false);        // Send the Tx buffer, but send a restart to keep connection alive\n\tuint8_t i = 0;\n        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address \n\twhile (Wire.available()) {\n        data[i++] = Wire.read(); }               // Put read results in the Rx buffer\n        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program\n        }\n\n\n\nunsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents\n{\n  int cnt;\n  unsigned int n_rem = 0;\n  unsigned char n_bit;\n  \n  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation\n  n_prom[7] = 0;\n  for(cnt = 0; cnt < 16; cnt++)\n  {\n    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);\n    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);\n    for(n_bit = 8; n_bit > 0; n_bit--)\n    {\n        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;\n        else                  n_rem = (n_rem<<1);\n    }\n  }\n  n_rem = ((n_rem>>12) & 0x000F);\n  return (n_rem ^ 0x00);\n}\n\n// I2C scan function\n\nvoid I2Cscan()\n{\n// scan for i2c devices\n  byte error, address;\n  int nDevices;\n\n  Serial.println(\"Scanning...\");\n\n  nDevices = 0;\n  for(address = 1; address < 127; address++ ) \n  {\n    // The i2c_scanner uses the return value of\n    // the Write.endTransmisstion to see if\n    // a device did acknowledge to the address.\n    Wire.beginTransmission(address);\n    error = Wire.endTransmission();\n\n    if (error == 0)\n    {\n      Serial.print(\"I2C device found at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.print(address,HEX);\n      Serial.println(\"  !\");\n\n      nDevices++;\n    }\n    else if (error==4) \n    {\n      Serial.print(\"Unknown error at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.println(address,HEX);\n    }    \n  }\n  if (nDevices == 0)\n    Serial.println(\"No I2C devices found\\n\");\n  else\n    Serial.println(\"done\\n\");\n    \n}\n\n// I2C read/write functions for the BNO055 sensor\n\n  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)\n{\n\tWire.beginTransmission(address);  // Initialize the Tx buffer\n\tWire.write(subAddress);           // Put slave register address in Tx buffer\n\tWire.write(data);                 // Put data in Tx buffer\n\tWire.endTransmission();           // Send the Tx buffer\n}\n\n  uint8_t readByte(uint8_t address, uint8_t subAddress)\n{\n\tuint8_t data; // `data` will store the register data\t \n\tWire.beginTransmission(address);         // Initialize the Tx buffer\n\tWire.write(subAddress);\t                 // Put slave register address in Tx buffer\n\tWire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n\tWire.requestFrom(address, 1);            // Read one byte from slave register address \n\tdata = Wire.read();                      // Fill Rx buffer with result\n\treturn data;                             // Return data read from slave register\n}\n\n  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\n{  \n\tWire.beginTransmission(address);   // Initialize the Tx buffer\n\tWire.write(subAddress);            // Put slave register address in Tx buffer\n\tWire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive\n\tuint8_t i = 0;\n  Wire.requestFrom(address, count);  // Read bytes from slave register address \n\twhile (Wire.available()) {\n        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer\n}\n"
  },
  {
    "path": "Bosch/readme.md",
    "content": "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.\n\n![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-08-06T00:24:08.896Z-BNO055.top.jpg.855x570_q85_pad_rcrop.jpg)\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_WebServer/BME280.cpp",
    "content": "/* 06/16/2017 Copyright Tlera Corporation\r\n *  \r\n *  Created by Kris Winer\r\n *  \r\n This sketch uses SDA/SCL on pins 42/43 (back pads), respectively, and it uses the Dragonfly STM32L476RE Breakout Board.\r\n The BME280 is a simple but high resolution pressure/humidity/temperature sensor, which can be used in its high resolution\r\n mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\r\n only 1 microAmp. The choice will depend on the application.\r\n \r\n Library may be used freely and without limit with attribution.\r\n \r\n*/\r\n \r\n#include \"BME280.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n#define USBSerial Serial\r\n\r\nBME280::BME280(I2Cdev* i2c_bus)\r\n{\r\n  _i2c_bus = i2c_bus;\r\n}\r\n\r\n\r\nuint8_t BME280::getChipID()\r\n{\r\n  uint8_t c = _i2c_bus->readByte(BME280_ADDRESS, BME280_ID);\r\n  return c;\r\n}\r\n\r\nvoid BME280::resetBME280()\r\n{\r\n  _i2c_bus->writeByte(BME280_ADDRESS, BME280_RESET, 0xB6);\r\n}\r\n\r\n\r\nint32_t BME280::readBME280Temperature()\r\n{\r\n  uint8_t rawData[3];  // 20-bit temperature register data stored here\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_TEMP_MSB, 3, &rawData[0]);  \r\n  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);\r\n}\r\n\r\n\r\nint32_t BME280::readBME280Pressure()\r\n{\r\n  uint8_t rawData[3];  // 20-bit pressure register data stored here\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_PRESS_MSB, 3, &rawData[0]);  \r\n  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);\r\n}\r\n\r\n\r\nint32_t BME280::BME280::readBME280Humidity()\r\n{\r\n  uint8_t rawData[2];  // 16-bit humiditye register data stored here\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_HUM_MSB, 2, &rawData[0]);  \r\n  return (uint32_t) (((uint32_t) rawData[0] << 24 | (uint32_t) rawData[1] << 16) ) >> 16;\r\n}\r\n\r\n\r\nvoid BME280::BME280forced()\r\n{\r\n    uint8_t temp = _i2c_bus->readByte(BME280_ADDRESS, BME280_CTRL_MEAS);\r\n    _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, temp | Forced);\r\n \r\n    while( (_i2c_bus->readByte(BME280_ADDRESS, BME280_STATUS)) & 0x10) { } // wait for conversion byte to clear\r\n}\r\n\r\n\r\nvoid BME280::BME280Init(uint8_t Posr, uint8_t Hosr, uint8_t Tosr, uint8_t Mode, uint8_t IIRFilter, uint8_t SBy)\r\n{\r\n  // Configure the BME280\r\n  // Set H oversampling rate\r\n  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_HUM, 0x07 & Hosr);\r\n  // Set T and P oversampling rates and sensor mode\r\n  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);\r\n  // Set standby time interval in normal mode and bandwidth\r\n  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CONFIG, SBy << 5 | IIRFilter << 2);\r\n  \r\n  uint8_t calib[26];\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_CALIB00, 26, &calib[0]);\r\n  _dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);\r\n  _dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);\r\n  _dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);\r\n  _dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);\r\n  _dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);\r\n  _dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);\r\n  _dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);\r\n  _dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);\r\n  _dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);\r\n  _dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);\r\n  _dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);\r\n  _dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);\r\n  _dig_H1 = calib[25];\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_CALIB26, 7, &calib[0]);\r\n  _dig_H2 = ( int16_t)((( int16_t) calib[1] << 8) | calib[0]);\r\n  _dig_H3 = calib[2];\r\n  _dig_H4 = ( int16_t)(((( int16_t) calib[3] << 8) | (0x0F & calib[4]) << 4) >> 4);\r\n  _dig_H5 = ( int16_t)(((( int16_t) calib[5] << 8) | (0xF0 & calib[4]) ) >> 4 );\r\n  _dig_H6 = calib[6];\r\n  Serial.println(\"Calibration coeficients:\");\r\n  Serial.print(\"_dig_T1 =\"); \r\n  Serial.println(_dig_T1);\r\n  Serial.print(\"_dig_T2 =\"); \r\n  Serial.println(_dig_T2);\r\n  Serial.print(\"_dig_T3 =\"); \r\n  Serial.println(_dig_T3);\r\n  Serial.print(\"_dig_P1 =\"); \r\n  Serial.println(_dig_P1);\r\n  Serial.print(\"_dig_P2 =\"); \r\n  Serial.println(_dig_P2);\r\n  Serial.print(\"_dig_P3 =\"); \r\n  Serial.println(_dig_P3);\r\n  Serial.print(\"_dig_P4 =\"); \r\n  Serial.println(_dig_P4);\r\n  Serial.print(\"_dig_P5 =\"); \r\n  Serial.println(_dig_P5);\r\n  Serial.print(\"_dig_P6 =\"); \r\n  Serial.println(_dig_P6);\r\n  Serial.print(\"_dig_P7 =\"); \r\n  Serial.println(_dig_P7);\r\n  Serial.print(\"_dig_P8 =\"); \r\n  Serial.println(_dig_P8);\r\n  Serial.print(\"_dig_P9 =\"); \r\n  Serial.println(_dig_P9);\r\n  Serial.print(\"_dig_H1 =\"); \r\n  Serial.println(_dig_H1);\r\n  Serial.print(\"_dig_H2 =\"); \r\n  Serial.println(_dig_H2);\r\n  Serial.print(\"_dig_H3 =\"); \r\n  Serial.println(_dig_H3);\r\n  Serial.print(\"_dig_H4 =\"); \r\n  Serial.println(_dig_H4);\r\n  Serial.print(\"_dig_H5 =\"); \r\n  Serial.println(_dig_H5);\r\n  Serial.print(\"_dig_H6 =\"); \r\n  Serial.println(_dig_H6);\r\n}\r\n\r\n\r\n// Returns temperature in DegC, resolution is 0.01 DegC. Output value of\r\n// “5123” equals 51.23 DegC.\r\nint32_t BME280::BME280_compensate_T(int32_t adc_T)\r\n{\r\n  int32_t var1, var2, T;\r\n  var1 = ((((adc_T >> 3) - ((int32_t)_dig_T1 << 1))) * ((int32_t)_dig_T2)) >> 11;\r\n  var2 = (((((adc_T >> 4) - ((int32_t)_dig_T1)) * ((adc_T >> 4) - ((int32_t)_dig_T1))) >> 12) * ((int32_t)_dig_T3)) >> 14;\r\n  _t_fine = var1 + var2;\r\n  T = (_t_fine * 5 + 128) >> 8;\r\n  return T;\r\n}\r\n\r\n\r\n// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8\r\n//fractional bits).\r\n//Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa\r\nuint32_t BME280::BME280_compensate_P(int32_t adc_P)\r\n{\r\n  long long var1, var2, p;\r\n  var1 = ((long long)_t_fine) - 128000;\r\n  var2 = var1 * var1 * (long long)_dig_P6;\r\n  var2 = var2 + ((var1*(long long)_dig_P5)<<17);\r\n  var2 = var2 + (((long long)_dig_P4)<<35);\r\n  var1 = ((var1 * var1 * (long long)_dig_P3)>>8) + ((var1 * (long long)_dig_P2)<<12);\r\n  var1 = (((((long long)1)<<47)+var1))*((long long)_dig_P1)>>33;\r\n  if(var1 == 0)\r\n  {\r\n    return 0;\r\n    // avoid exception caused by division by zero\r\n  }\r\n  p = 1048576 - adc_P;\r\n  p = (((p<<31) - var2)*3125)/var1;\r\n  var1 = (((long long)_dig_P9) * (p>>13) * (p>>13)) >> 25;\r\n  var2 = (((long long)_dig_P8) * p)>> 19;\r\n  p = ((p + var1 + var2) >> 8) + (((long long)_dig_P7)<<4);\r\n  return (uint32_t)p;\r\n}\r\n\r\n\r\n// Returns humidity in %RH as unsigned 32 bit integer in Q22.10 format (22integer and 10fractional bits).\r\n// Output value of “47445”represents 47445/1024= 46.333%RH\r\nuint32_t BME280::BME280_compensate_H(int32_t adc_H)\r\n{\r\nint32_t var;\r\n\r\nvar = (_t_fine - ((int32_t)76800));\r\nvar = (((((adc_H << 14) - (((int32_t)_dig_H4) << 20) - (((int32_t)_dig_H5) * var)) +\r\n((int32_t)16384)) >> 15) * (((((((var * ((int32_t)_dig_H6)) >> 10) * (((var *\r\n((int32_t)_dig_H3)) >> 11) + ((int32_t)32768))) >> 10) + ((int32_t)2097152)) * ((int32_t)_dig_H2) + 8192) >> 14));\r\nvar = (var - (((((var >> 15) * (var >> 15)) >> 7) * ((int32_t)_dig_H1)) >> 4));\r\nvar = (var < 0 ? 0 : var); \r\nvar = (var > 419430400 ? 419430400 : var);\r\nreturn(uint32_t)(var >> 12);\r\n}\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_WebServer/BME280.h",
    "content": "/* 06/16/2017 Copyright Tlera Corporation\r\n *  \r\n *  Created by Kris Winer\r\n *  \r\n This sketch uses SDA/SCL on pins 42/43 (back pads), respectively, and it uses the Dragonfly STM32L476RE Breakout Board.\r\n The BME280 is a simple but high resolution pressure/humidity/temperature sensor, which can be used in its high resolution\r\n mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\r\n only 1 microAmp. The choice will depend on the application.\r\n \r\n Library may be used freely and without limit with attribution.\r\n \r\n*/\r\n  \r\n#ifndef BME280_h\r\n#define BME280_h\r\n\r\n#include \"Arduino.h\"\r\n#include \"I2CDev.h\"\r\n#include <Wire.h>\r\n\r\n/* BME280 registers\r\n*  http://www.mouser.com/ds/2/783/BST-BME280_DS001-11-844833.pdf\r\n*/\r\n#define BME280_HUM_LSB    0xFE\r\n#define BME280_HUM_MSB    0xFD\r\n#define BME280_TEMP_XLSB  0xFC\r\n#define BME280_TEMP_LSB   0xFB\r\n#define BME280_TEMP_MSB   0xFA\r\n#define BME280_PRESS_XLSB 0xF9\r\n#define BME280_PRESS_LSB  0xF8\r\n#define BME280_PRESS_MSB  0xF7\r\n#define BME280_CONFIG     0xF5\r\n#define BME280_CTRL_MEAS  0xF4\r\n#define BME280_STATUS     0xF3\r\n#define BME280_CTRL_HUM   0xF2\r\n#define BME280_RESET      0xE0\r\n#define BME280_ID         0xD0  // should be 0x60\r\n#define BME280_CALIB00    0x88\r\n#define BME280_CALIB26    0xE1\r\n\r\n#define BME280_ADDRESS           0x76   // Address of BMP280 altimeter when ADO = 0\r\n\r\n\r\n#define  P_OSR_01 0x01\r\n#define  P_OSR_02 0x02\r\n#define  P_OSR_04 0x03\r\n#define  P_OSR_08 0x04\r\n#define  P_OSR_16 0x05\r\n\r\n#define  H_OSR_01 0x01\r\n#define  H_OSR_02 0x02\r\n#define  H_OSR_04 0x03\r\n#define  H_OSR_08 0x04\r\n#define  H_OSR_16 0x05\r\n\r\n#define  T_OSR_01 0x01\r\n#define  T_OSR_02 0x02\r\n#define  T_OSR_04 0x03\r\n#define  T_OSR_08 0x04\r\n#define  T_OSR_16 0x05\r\n\r\n#define  full       0x00\r\n#define  BW0_223ODR 0x01\r\n#define  BW0_092ODR 0x02\r\n#define  BW0_042ODR 0x03\r\n#define  BW0_021ODR 0x04  \r\n\r\n#define  BME280Sleep 0x00\r\n#define  Forced      0x01\r\n#define  Forced2     0x02\r\n#define  Normal      0x03\r\n\r\n#define  t_00_5ms 0x00\r\n#define  t_62_5ms 0x01\r\n#define  t_125ms  0x02\r\n#define  t_250ms  0x03\r\n#define  t_500ms  0x04\r\n#define  t_1000ms 0x05\r\n#define  t_10ms   0x06\r\n#define  t_20ms   0x07 \r\n\r\n\r\nclass BME280\r\n{\r\n  public: \r\n  BME280(I2Cdev* i2c_bus);\r\n  uint8_t getChipID();\r\n  void resetBME280();\r\n  int32_t readBME280Temperature();\r\n  int32_t readBME280Pressure();\r\n  int32_t readBME280Humidity();\r\n  void BME280forced();\r\n  void BME280Init(uint8_t Posr, uint8_t Hosr, uint8_t Tosr, uint8_t Mode, uint8_t IIRFilter, uint8_t SBy);\r\n  int32_t  BME280_compensate_T(int32_t adc_T);\r\n  uint32_t BME280_compensate_P(int32_t adc_P);\r\n  uint32_t BME280_compensate_H(int32_t adc_H);\r\n  \r\n  private:\r\n  uint8_t  _dig_H1, _dig_H3, _dig_H6;\r\n  uint16_t _dig_T1, _dig_P1, _dig_H4, _dig_H5;\r\n  int16_t  _dig_T2, _dig_T3, _dig_P2, _dig_P3, _dig_P4, _dig_P5, _dig_P6, _dig_P7, _dig_P8, _dig_P9, _dig_H2;\r\n  int32_t  _t_fine;\r\n  I2Cdev* _i2c_bus;\r\n  };\r\n\r\n#endif\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_WebServer/CCS811.cpp",
    "content": "/* 06/16/2017 Copyright Tlera Corporation\r\n *  \r\n *  Created by Kris Winer\r\n *  \r\n *  The AMS CCS811 is an air quality sensor that provides equivalent CO2 and volatile organic measurements from direct\r\n *  I2C register reads as well as current and voltage (effective resistance of the sensing element). Gas sensors, including \r\n *  this MEMs gas sensor in the CCS811 measure resistance of a substrate that changes when exposed to inert gasses and \r\n *  volatile organic compounds. Changed in concentration vary exponentially with the changes in resistance. The CCS811\r\n *  has an embedded ASIC calibrated against most common indoor pollutants that returns a good estimate of\r\n *  equivalent CO2 concentration in parts per million (400 - 8192 range) and volatile organic componds in parts per billion (0 - 1187).\r\n *  The sensor is quite sensitive to breath and other human emissions.\r\n *  \r\n *  Library may be used freely and without limit with attribution.\r\n *  \r\n */\r\n#include \"CCS811.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n#define USBSerial Serial\r\n\r\nCCS811::CCS811(I2Cdev* i2c_bus)\r\n{\r\n  _i2c_bus = i2c_bus;   \r\n}\r\n\r\n\r\nuint8_t CCS811::getChipID()\r\n{\r\n byte e = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ID);  // Read WHO_AM_I register for CCS8110\r\n return e;\r\n}\r\n\r\n\r\nvoid CCS811::checkCCS811Status() \r\n{\r\n   // Check CCS811 status\r\n  uint8_t status = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_STATUS);\r\n  Serial.print(\"status = 0X\"); Serial.println(status, HEX);\r\n  if(status & 0x80) {Serial.println(\"Firmware is in application mode. CCS811 is ready!\");}\r\n  else { Serial.println(\"Firmware is in boot mode!\");}\r\n  \r\n  if(status & 0x10) {Serial.println(\"Valid application firmware loaded!\");}\r\n  else { Serial.println(\"No application firmware is loaded!\");}\r\n\r\n  if(status & 0x08) {Serial.println(\"New data available!\");}\r\n  else { Serial.println(\"No new data available!\");}\r\n\r\n  if(status & 0x01) {Serial.println(\"Error detected!\");\r\n        uint8_t error = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ERROR_ID);\r\n        if(error & 0x01) Serial.println(\"CCS811 received invalid I2C write request!\");\r\n        if(error & 0x02) Serial.println(\"CCS811 received invalid I2C read request!\");\r\n        if(error & 0x04) Serial.println(\"CCS811 received unsupported mode request!\");\r\n        if(error & 0x08) Serial.println(\"Sensor resistance measurement at maximum range!\");\r\n        if(error & 0x10) Serial.println(\"Heater current is not in range!\");\r\n        if(error & 0x20) Serial.println(\"Heater voltage is not being applied correctly!\");\r\n  }\r\n  else { Serial.println(\"No error detected!\");\r\n  }\r\n  \r\n  Serial.println(\" \");\r\n  \r\n  }\r\n\r\n\r\n  void CCS811::CCS811init(uint8_t AQRate)\r\n  {\r\n    // initialize CCS811 and check version and status\r\n  byte HWVersion = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_HW_VERSION);\r\n  Serial.print(\"CCS811 Hardware Version = 0x\"); Serial.println(HWVersion, HEX); \r\n\r\n  uint8_t FWBootVersion[2] = {0, 0}, FWAppVersion[2] = {0,0};\r\n  _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_FW_BOOT_VERSION, 2, &FWBootVersion[0]);\r\n  Serial.println(\"CCS811 Firmware Boot Version: \"); \r\n  Serial.print(\"Major = \"); Serial.println((FWBootVersion[0] & 0xF0) >> 4); \r\n  Serial.print(\"Minor = \"); Serial.println(FWBootVersion[0] & 0x04); \r\n  Serial.print(\"Trivial = \"); Serial.println(FWBootVersion[1]); \r\n  \r\n  _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_FW_APP_VERSION, 2, &FWAppVersion[0]);\r\n  Serial.println(\"CCS811 Firmware App Version: \"); \r\n  Serial.print(\"Major = \"); Serial.println((FWAppVersion[0] & 0xF0) >> 4); \r\n  Serial.print(\"Minor = \"); Serial.println(FWAppVersion[0] & 0x04); \r\n  Serial.print(\"Trivial = \"); Serial.println(FWAppVersion[1]); \r\n\r\n  // Check CCS811 status\r\n  checkCCS811Status();\r\n  _i2c_bus->writeReg(CCS811_ADDRESS, CCS811_APP_START);\r\n  delay(100);\r\n  checkCCS811Status();\r\n\r\n  // set CCS811 measurement mode\r\n  _i2c_bus->writeByte(CCS811_ADDRESS, CCS811_MEAS_MODE, AQRate << 4 | 0x08); // pulsed heating mode, enable interrupt\r\n  uint8_t measmode = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_MEAS_MODE);\r\n  Serial.print(\"Confirm measurement mode = 0x\"); Serial.println(measmode, HEX);\r\n  }\r\n\r\n\r\n  void CCS811::compensateCCS811(int32_t compHumidity, int32_t compTemp)\r\n  {\r\n      // Update CCS811 humidity and temperature compensation\r\n      uint8_t temp[5] = {0, 0, 0, 0, 0};\r\n      temp[0] = CCS811_ENV_DATA;\r\n      temp[1] = ((compHumidity % 1024) / 100) > 7 ? (compHumidity/1024 + 1)<<1 : (compHumidity/1024)<<1;\r\n      temp[2] = 0;\r\n      if(((compHumidity % 1024) / 100) > 2 && (((compHumidity % 1024) / 100) < 8))\r\n      {\r\n       temp[1] |= 1;\r\n      }\r\n\r\n      compTemp += 2500;\r\n      temp[3] = ((compTemp % 100) / 100) > 7 ? (compTemp/100 + 1)<<1 : (compTemp/100)<<1;\r\n      temp[4] = 0;\r\n      if(((compTemp % 100) / 100) > 2 && (((compTemp % 100) / 100) < 8))\r\n      {\r\n       temp[3] |= 1;\r\n      }\r\n\r\n//      Wire.transfer(CCS811_ADDRESS, &temp[0], 5, NULL, 0);\r\n     _i2c_bus->writeBytes(CCS811_ADDRESS, CCS811_ENV_DATA, 4, &temp[1]);\r\n  }\r\n\r\n   void CCS811::readCCS811Data(uint8_t * destination)\r\n   {\r\n      uint8_t rawData[8] = {0, 0, 0, 0, 0, 0, 0, 0};\r\n      uint8_t status = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_STATUS);\r\n      \r\n      if(status & 0x01) { // check for errors\r\n        uint8_t error = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ERROR_ID);\r\n        if(error & 0x01) Serial.println(\"CCS811 received invalid I2C write request!\");\r\n        if(error & 0x02) Serial.println(\"CCS811 received invalid I2C read request!\");\r\n        if(error & 0x04) Serial.println(\"CCS811 received unsupported mode request!\");\r\n        if(error & 0x08) Serial.println(\"Sensor resistance measurement at maximum range!\");\r\n        if(error & 0x10) Serial.println(\"Heater current is not in range!\");\r\n        if(error & 0x20) Serial.println(\"Heater voltage is not being applied correctly!\");\r\n      }\r\n\r\n      _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_ALG_RESULT_DATA, 8, &rawData[0]);\r\n\r\n   for(int ii = 0; ii < 8; ii++)\r\n   {\r\n    destination[ii] = rawData[ii];\r\n   }\r\n   \r\n   }\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_WebServer/CCS811.h",
    "content": "#ifndef CCS811_h\r\n#define CCS811_h\r\n\r\n#include \"Arduino.h\"\r\n#include \"Wire.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n/* CCS811 Registers\r\nhttp://www.mouser.com/ds/2/588/CCS811_DS000459_3-00-1098798.pdf\r\n*/\r\n#define CCS811_STATUS             0x00\r\n#define CCS811_MEAS_MODE          0x01\r\n#define CCS811_ALG_RESULT_DATA    0x02\r\n#define CCS811_RAW_DATA           0x03\r\n#define CCS811_ENV_DATA           0x05\r\n#define CCS811_NTC                0x06\r\n#define CCS811_THRESHOLDS         0x10\r\n#define CCS811_BASELINE           0x11\r\n#define CCS811_HW_ID              0x20  // WHO_AM_I should be 0x81\r\n#define CCS811_ID                 0x20  // WHO_AM_I should be 0x1X\r\n#define CCS811_HW_VERSION         0x21  \r\n#define CCS811_FW_BOOT_VERSION    0x23\r\n#define CCS811_FW_APP_VERSION     0x24\r\n#define CCS811_ERROR_ID           0xE0\r\n#define CCS811_APP_START          0xF4\r\n#define CCS811_SW_RESET           0xFF\r\n\r\n#define CCS811_ADDRESS            0x5A   // Address of the CCS811 Air Quality Sensor\r\n\r\n#define  dt_idle  0x00\r\n#define  dt_1sec  0x01\r\n#define  dt_10sec 0x02\r\n#define  dt_60sec 0x03\r\n\r\nclass CCS811\r\n{\r\n  public: \r\n  CCS811(I2Cdev* i2c_bus);\r\n  void checkCCS811Status();\r\n  void CCS811init(uint8_t AQRate);\r\n  uint8_t getChipID();\r\n  void compensateCCS811(int32_t compHumidity, int32_t compTemp);\r\n  void readCCS811Data(uint8_t * destination);\r\n  \r\n  private:\r\n  I2Cdev* _i2c_bus;\r\n};\r\n\r\n\r\n#endif\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_WebServer/CCS811_BME280_ESP32C3Mini_WebServer.ino",
    "content": "/* 06/16/2017 Copyright Tlera Corporation\r\n *  \r\n *  Created by Kris Winer\r\n *  \r\n *  The AMS CCS811 is an air quality sensor that provides equivalent CO2 and volatile organic measurements from direct\r\n *  I2C register reads as well as current and voltage (effective resistance of the sensing element). Gas sensors, including \r\n *  this MEMs gas sensor in the CCS811 measure resistance of a substrate that changes when exposed to inert gasses and \r\n *  volatile organic compounds. Changed in concentration vary exponentially with the changes in resistance. The CCS811\r\n *  has an embedded ASIC calibrated against most common indoor pollutants that returns a good estimate of\r\n *  equivalent CO2 concentration in parts per million (400 - 8192 range) and volatile organic componds in parts per billion (0 - 1187).\r\n *  The sensor is quite sensitive to breath and other human emissions.\r\n *  \r\n *  This sketch uses default SDA/SCL pins on the Ladybug development board.\r\n *  The BME280 is a simple but high resolution pressure/humidity/temperature sensor, which can be used in its high resolution\r\n *  mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\r\n *  only 1 microAmp. The choice will depend on the application.\r\n \r\n    Library may be used freely and without limit with attribution.\r\n \r\n  */\r\n#include <Arduino.h>\r\n#include <WiFi.h>\r\n#include <WiFiClient.h>\r\n#include <WebServer.h>\r\n#include <ESPmDNS.h>\r\n#include <driver/adc.h>\r\n#include \"USB.h\"\r\n#include \"BME280.h\"\r\n#include \"CCS811.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n#define Serial USBSerial\r\n\r\nconst char* ssid = \"XXXXXXXXX\";\r\nconst char* password = \"XXXXXXXXXXXX\";\r\n\r\nWebServer server(80);\r\n\r\nString webString=\"\";     // String to display\r\n\r\nvoid handleRoot() {\r\n  server.send(200, \"text/plain\", \"Hello from the environmental monitroing station ESP32!\");\r\n  delay(100);\r\n}\r\n\r\n  String createHTML(float var1, float var2, float var3, float var4, float var5, float var6, float var7, float var8) {\r\n   webString = \"<html><head><meta http-equiv=\\\"Refresh\\\" content=\\\"5\\\"></head><body><UL>\"\r\n\r\n  +String(\"<LI>Temperature = \")+String(var1)+String(\" C</LI>\") \r\n  +String(\"<LI>Temperature = \")+String(var2)+String(\" F</LI>\") \r\n  +String(\"<LI>Pressure = \")+String(var3)+String(\" milliBar</LI>\") \r\n  +String(\"<LI>Altitude = \")+String(var4)+String(\" feet</LI>\") \r\n  +String(\"<LI>Humidity = \")+String(var5)+String(\" %RH</LI>\") \r\n  +String(\"<LI>eCO2 = \")+String(var6)+String(\" ppm</LI>\") \r\n  +String(\"<LI>TVOC = \")+String(var7)+String(\" ppb</LI>\") \r\n  +String(\"<LI>Battery Voltage = \")+String(var8)+String(\" V</LI>\") \r\n     \r\n  +\"</UL></body></html>\" ;\r\n   return webString;\r\n  }\r\n  \r\nvoid handleNotFound() {\r\n  String message = \"File Not Found\\n\\n\";\r\n  message += \"URI: \";\r\n  message += server.uri();\r\n  message += \"\\nMethod: \";\r\n  message += (server.method() == HTTP_GET) ? \"GET\" : \"POST\";\r\n  message += \"\\nArguments: \";\r\n  message += server.args();\r\n  message += \"\\n\";\r\n  for (uint8_t i = 0; i < server.args(); i++) {\r\n    message += \" \" + server.argName(i) + \": \" + server.arg(i) + \"\\n\";\r\n  }\r\n  server.send(404, \"text/plain\", message);\r\n}\r\n\r\n/* create an ESP32 hardware timer */\r\nhw_timer_t * timer = NULL;\r\nvolatile bool alarmFlag = false;\r\n\r\nvoid IRAM_ATTR onTimer(){\r\n  alarmFlag = true;\r\n}\r\n\r\n#define I2C_BUS          Wire               // Define the I2C bus (Wire instance) you wish to use\r\n\r\nI2Cdev                   i2c_0(&I2C_BUS);   // Instantiate the I2Cdev object and point to the desired I2C bus\r\n\r\n#define SerialDebug true  // set to true to get Serial output for debugging\r\n#define       myLed    2 // green led\r\nconst uint8_t myBat =  1;\r\n\r\nfloat VBat = 0.0f;\r\nuint32_t chipId = 0;\r\n\r\n// BME280 definitions\r\n/* Specify BME280 configuration\r\n *  Choices are:\r\n P_OSR_01, P_OSR_02, P_OSR_04, P_OSR_08, P_OSR_16 // pressure oversampling\r\n H_OSR_01, H_OSR_02, H_OSR_04, H_OSR_08, H_OSR_16 // humidity oversampling\r\n T_OSR_01, T_OSR_02, T_OSR_04, T_OSR_08, T_OSR_16 // temperature oversampling\r\n full, BW0_223ODR,BW0_092ODR, BW0_042ODR, BW0_021ODR // bandwidth at 0.021 x sample rate\r\n BME280Sleep, forced,, forced2, normal //operation modes\r\n t_00_5ms = 0, t_62_5ms, t_125ms, t_250ms, t_500ms, t_1000ms, t_10ms, t_20ms // determines sample rate\r\n */\r\nuint8_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\r\n\r\nfloat Temperature, Pressure, Humidity;              // stores BME280 pressures sensor pressure and temperature\r\nint32_t rawPress, rawTemp, rawHumidity, compTemp;   // pressure and temperature raw count output for BME280\r\nuint32_t compHumidity, compPress;                   // variables to hold raw BME280 humidity value\r\n\r\nfloat temperature_C, temperature_F, pressure, humidity, altitude; // Scaled output of the BME280\r\n\r\nBME280 BME280(&i2c_0); // instantiate BME280 class\r\n\r\n\r\n// CCS811 definitions\r\n#define CCS811_intPin  4\r\n#define CCS811_wakePin 7\r\n\r\n/* Specify CCS811 sensor parameters\r\n *  Choices are   dt_idle , dt_1sec, dt_10sec, dt_60sec\r\n */\r\nuint8_t AQRate = dt_10sec;  // set the sample rate\r\nuint8_t rawData[8] = {0, 0, 0, 0, 0, 0, 0, 0};  // array to hold the raw data\r\nuint16_t eCO2 = 0, TVOC = 0;\r\nuint8_t Current = 0;\r\nfloat Voltage = 0.0f;\r\n\r\nvolatile bool newCCS811Data  = true; // boolean flag for interrupt\r\n\r\nCCS811 CCS811(&i2c_0); // instantiate CCS811 class\r\n\r\n\r\nvoid setup()\r\n{\r\n  Serial.begin(115200);\r\n  delay(4000);\r\n  Serial.println(\"Serial enabled!\");\r\n\r\n  pinMode(myLed, OUTPUT);\r\n  digitalWrite(myLed, LOW); // start with led on, active LOW\r\n\r\n  for(int i=0; i<17; i=i+8) {\r\n    chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xFF) << i;\r\n  }\r\n\r\n  Serial.printf(\"ESP32 Chip model = %s Rev %d\\n\", ESP.getChipModel(), ESP.getChipRevision());\r\n  Serial.printf(\"This chip has %d cores\\n\", ESP.getChipCores());\r\n  Serial.print(\"Chip ID: \"); Serial.println(chipId);\r\n\r\n  adc1_config_width(ADC_WIDTH_BIT_12);\r\n  adc1_config_channel_atten(ADC1_CHANNEL_1, ADC_ATTEN_DB_11);\r\n\r\n  pinMode(myBat, INPUT); // battery voltage monitor\r\n\r\n  pinMode(CCS811_intPin, INPUT_PULLUP); // active LOW\r\n\r\n  I2C_BUS.begin(0, 3);               // Set master mode, default on SDA on pin 0/SCL  on pin 3\r\n  I2C_BUS.setClock(400000);          // I2C frequency at 400 kHz\r\n  delay(1000);\r\n\r\n  pinMode(CCS811_wakePin, OUTPUT);\r\n  //Enable the CCS811 for I2C scan\r\n  digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n  \r\n  Serial.println(\"Scan for I2C devices:\");\r\n  i2c_0.I2Cscan();                   // should detect BME280 at 0x76 and CCS811 at 0x5A\r\n  delay(1000);\r\n\r\n  //Disable the CCS811 for I2C scan\r\n  digitalWrite(CCS811_wakePin, HIGH); // set LOW to enable the CCS811 air quality sensor\r\n  delay(1000);\r\n\r\n  // Read the WHO_AM_I register of the BME280 this is a good test of communication\r\n  byte BME280ChipID = BME280.getChipID();  // Read WHO_AM_I register for BME280\r\n  Serial.print(\"BME280 \"); Serial.print(\"I AM \"); Serial.print(BME280ChipID, HEX); Serial.print(\" I should be \"); Serial.println(0x60, HEX);\r\n  Serial.println(\" \");\r\n  delay(1000); \r\n\r\n  // Read the WHO_AM_I register of the CCS811 this is a good test of communication\r\n  digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n  byte CCS811ChipID = CCS811.getChipID();\r\n  digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor\r\n  Serial.print(\"CCS811 \"); Serial.print(\"I AM \"); Serial.print(CCS811ChipID, HEX); Serial.print(\" I should be \"); Serial.println(0x81, HEX);\r\n  Serial.println(\" \");\r\n  delay(1000); \r\n  \r\n  if(BME280ChipID == 0x60 && CCS811ChipID == 0x81 ) {\r\n\r\n   Serial.println(\"BME280+CCS811 are online...\"); Serial.println(\" \");\r\n   digitalWrite(myLed, LOW);\r\n\r\n   BME280.resetBME280();                                                        // reset BME280 before initilization\r\n   delay(100);\r\n   BME280.BME280Init(Posr, Hosr, Tosr, Mode, IIRFilter, SBy);                   // Initialize BME280 altimeter\r\n   BME280.BME280forced();                                                       // get initial data sample, then go back to sleep\r\n\r\n   // initialize CCS811 and check version and status\r\n   digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n   CCS811.CCS811init(AQRate);\r\n   digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor\r\n\r\n   }\r\n   else {\r\n   if(BME280ChipID != 0x60) Serial.println(\" BME280 not functioning!\");    \r\n   if(CCS811ChipID != 0x81) Serial.println(\" CCS811 not functioning!\");  \r\n   while(1) { }; // no point proceeding, so wait here forever...\r\n   }\r\n\r\n  // Set up ESP32 timer\r\n  /* Use 1st timer of 4 */\r\n  /* 1 tick take 1/(80MHZ/80) = 1us so we set divider 80 and count up */\r\n  timer = timerBegin(0, 80, true);\r\n\r\n  /* Attach onTimer function to our timer */\r\n  timerAttachInterrupt(timer, &onTimer, true);\r\n\r\n  /* Set alarm to call onTimer function every second 1 tick is 1us\r\n  => 1 second is 1000000us */\r\n  /* Repeat the alarm (third parameter) */\r\n  timerAlarmWrite(timer, 10000000, true); // update every ten seconds\r\n\r\n  /* Start an alarm */\r\n  timerAlarmEnable(timer);\r\n  Serial.println(\"start timer\");\r\n\r\n  WiFi.mode(WIFI_STA);\r\n  WiFi.begin(ssid, password);\r\n  Serial.println(\"\");\r\n\r\n  // Wait for connection\r\n  while (WiFi.status() != WL_CONNECTED) {\r\n    delay(500);\r\n    Serial.print(\".\");\r\n  }\r\n  Serial.println(\"\");\r\n  Serial.print(\"Connected to \");\r\n  Serial.println(ssid);\r\n  Serial.print(\"IP address: \");\r\n  Serial.println(WiFi.localIP());\r\n\r\n  if (MDNS.begin(\"esp32\")) {\r\n    Serial.println(\"MDNS responder started\");\r\n  }\r\n\r\n  server.on(\"/\", handleRoot);\r\n\r\n  server.onNotFound(handleNotFound);\r\n\r\n  server.on(\"/ESP32Data\", [](){ // define web server data\r\n\r\n    // BME280 Data\r\n    rawTemp =   BME280.readBME280Temperature();\r\n    temperature_C = (float) BME280.BME280_compensate_T(rawTemp)/100.0f; // temperature in Centigrade\r\n    temperature_F = 9.0f*temperature_C/5.0f + 32.0f;\r\n    rawPress =  BME280.readBME280Pressure();\r\n    pressure = (float) BME280.BME280_compensate_P(rawPress)/25600.0f; // Pressure in millibar\r\n    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));\r\n    rawHumidity =  BME280.readBME280Humidity();\r\n    humidity = (float) BME280.BME280_compensate_H(rawHumidity)/1024.0f; // Humidity in %RH\r\n\r\n    // CCS811 Data\r\n    digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n    CCS811.readCCS811Data(rawData);\r\n    CCS811.compensateCCS811(compHumidity, compTemp); // compensate CCS811 using BME280 humidity and temperature\r\n    digitalWrite(CCS811_wakePin, HIGH); // set LOW to enable the CCS811 air quality sensor \r\n\r\n    eCO2 = (uint16_t) ((uint16_t) rawData[0] << 8 | rawData[1]);\r\n    TVOC = (uint16_t) ((uint16_t) rawData[2] << 8 | rawData[3]);\r\n    Current = (rawData[6] & 0xFC) >> 2;\r\n    Voltage = (float) ((uint16_t) ((((uint16_t)rawData[6] & 0x02) << 8) | rawData[7])) * (1.65f/1023.0f); \r\n\r\n    // Battery Voltage\r\n    VBat = 2.0f * 2.60f * 1.15f * ((float) adc1_get_raw((adc1_channel_t)1)) / 4095.0f;\r\n    \r\n    createHTML(temperature_C, temperature_F, pressure, altitude, humidity, eCO2, TVOC, VBat);\r\n    server.send(200, \"text/html\", webString);               // send to someone's browser when asked\r\n  });\r\n\r\n  server.begin();\r\n  Serial.println(\"HTTP server started\");\r\n  \r\n  attachInterrupt(CCS811_intPin,  myinthandler, FALLING); // enable CCS811 interrupt \r\n}\r\n\r\nvoid loop()\r\n{  \r\n    // CCS811 data\r\n    // If intPin goes LOW, all data registers have new data\r\n    if(newCCS811Data == true) {  // On interrupt, read data\r\n    newCCS811Data = false;  // reset newData flag\r\n     \r\n    digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n    CCS811.readCCS811Data(rawData);\r\n    CCS811.compensateCCS811(compHumidity, compTemp); // compensate CCS811 using BME280 humidity and temperature\r\n    digitalWrite(CCS811_wakePin, HIGH); // set LOW to enable the CCS811 air quality sensor \r\n\r\n    eCO2 = (uint16_t) ((uint16_t) rawData[0] << 8 | rawData[1]);\r\n    TVOC = (uint16_t) ((uint16_t) rawData[2] << 8 | rawData[3]);\r\n    Current = (rawData[6] & 0xFC) >> 2;\r\n    Voltage = (float) ((uint16_t) ((((uint16_t)rawData[6] & 0x02) << 8) | rawData[7])) * (1.65f/1023.0f); \r\n    \r\n    Serial.println(\"CCS811:\");\r\n    Serial.print(\"Eq CO2 in ppm = \"); Serial.println(eCO2);\r\n    Serial.print(\"TVOC in ppb = \"); Serial.println(TVOC);\r\n    Serial.print(\"Sensor current (uA) = \"); Serial.println(Current);\r\n    Serial.print(\"Sensor voltage (V) = \"); Serial.println(Voltage, 2);  \r\n    Serial.println(\" \");\r\n  }\r\n            \r\n   /*RTC Timer*/\r\n   if (alarmFlag) { // update serial output whenever there is a timer alarm\r\n      alarmFlag = false;\r\n\r\n    /* BME280 sensor data */\r\n    BME280.BME280forced();  // get one data sample, then go back to sleep\r\n\r\n    rawTemp =  BME280.readBME280Temperature();\r\n    compTemp = BME280.BME280_compensate_T(rawTemp);\r\n    temperature_C = (float) compTemp/100.0f;\r\n    temperature_F = 9.0f*temperature_C/5.0f + 32.0f;\r\n     \r\n    rawPress =  BME280.readBME280Pressure();\r\n    pressure = (float) BME280.BME280_compensate_P(rawPress)/25600.f; // Pressure in mbar\r\n    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));   \r\n   \r\n    rawHumidity =  BME280.readBME280Humidity();\r\n    compHumidity = BME280.BME280_compensate_H(rawHumidity);\r\n    humidity = (float)compHumidity/1024.0f; // Humidity in %RH\r\n \r\n    Serial.println(\"BME280:\");\r\n    Serial.print(\"Altimeter temperature = \"); \r\n    Serial.print( temperature_C, 2); \r\n    Serial.println(\" C\"); // temperature in degrees Celsius\r\n    Serial.print(\"Altimeter temperature = \"); \r\n    Serial.print(temperature_F, 2); \r\n    Serial.println(\" F\"); // temperature in degrees Fahrenheit\r\n    Serial.print(\"Altimeter pressure = \"); \r\n    Serial.print(pressure, 2);  \r\n    Serial.println(\" mbar\");// pressure in millibar\r\n    Serial.print(\"Altitude = \"); \r\n    Serial.print(altitude, 2); \r\n    Serial.println(\" feet\");\r\n    Serial.print(\"Altimeter humidity = \"); \r\n    Serial.print(humidity, 1);  \r\n    Serial.println(\" %RH\");// pressure in millibar\r\n    Serial.println(\" \");\r\n\r\n    uint16_t Counts = adc1_get_raw((adc1_channel_t)1);\r\n    /* ADC at attenuation 11 should rad from 0 to 2.6 V nominally. The resistor divider\r\n     *  is 1/2, and a calibration factor of 1.15 is applied to bring measurements into \r\n     *  agreement with multimeter */\r\n    VBat = 2.0f * 2.60f * 1.15f * ((float) Counts) / 4095.0f;\r\n    Serial.print(\"Battery voltage = \"); Serial.print(VBat, 2); Serial.println(\" V\");\r\n\r\n    server.handleClient(); // update web server page\r\n      \r\n    digitalWrite(myLed, LOW); delay(10); digitalWrite(myLed, HIGH); // blink led at end of loop\r\n    }  \r\n    \r\n yield(); //allow the cpu to switch to other tasks\r\n//     ESP32.sleep();    // time out in deep sleep mode to save power\r\n}\r\n\r\n//===================================================================================================================\r\n//====== Set of useful functions\r\n//===================================================================================================================\r\n\r\nvoid myinthandler()\r\n{\r\n  newCCS811Data = true;\r\n}\r\n\r\n\r\nvoid alarmMatch()\r\n{\r\n  alarmFlag = true;\r\n}\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_WebServer/I2CDev.cpp",
    "content": "/*\r\n * Copyright (c) 2018 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#include \"Arduino.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n#define USBSerial Serial\r\n\r\nI2Cdev::I2Cdev(TwoWire* i2c_bus)                                                                                                             // Class constructor\r\n{\r\n  _i2c_bus = i2c_bus;\r\n}\r\n\r\nI2Cdev::~I2Cdev()                                                                                                                            // Class destructor\r\n{\r\n}\r\n\r\n/**\r\n* @fn: readByte(uint8_t address, uint8_t subAddress)\r\n*\r\n* @brief: Read one byte from an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress\r\n* @returns: unsigned short read\r\n*/\r\nuint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)\r\n{\r\n  uint8_t data = 0;                             // `data` will store the register data   \r\n  _i2c_bus->beginTransmission(address);         // Initialize the Tx buffer\r\n  _i2c_bus->write(subAddress);                  // Put slave register address in Tx buffer\r\n  _i2c_bus->endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\r\n  _i2c_bus->requestFrom(address, 1);            // Read one byte from slave register address  \r\n  data = _i2c_bus->read();                      // Fill Rx buffer with result\r\n  return data;                                  // Return data read from slave register\r\n  \r\n}\r\n\r\n\r\n/**\r\n* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\r\n*\r\n* @brief: Read multiple bytes from an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\r\n{  \r\n  _i2c_bus->beginTransmission(address);   // Initialize the Tx buffer\r\n  _i2c_bus->write(subAddress);            // Put slave register address in Tx buffer\r\n  _i2c_bus->endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive\r\n  uint8_t i = 0;\r\n  _i2c_bus->requestFrom(address, count);  // Read bytes from slave register address \r\n  while (_i2c_bus->available()) {\r\n        dest[i++] = _i2c_bus->read(); }   // Put read results in the Rx buffer\r\n}\r\n\r\n\r\n/**\r\n* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n*\r\n* @brief: Write one byte to an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, data to be written\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n{\r\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\r\n  _i2c_bus->write(regAddr);              // Put slave register address in Tx buffer\r\n  _i2c_bus->write(data);                 // Put data in Tx buffer\r\n  _i2c_bus->endTransmission();           // Send the Tx buffer\r\n}\r\n\r\n/**\r\n* @fn: writeReg(uint8_t devAddr, uint8_t regAddr)\r\n*\r\n* @brief: Write register to an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, data to be written\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::writeReg(uint8_t devAddr, uint8_t regAddr)\r\n{\r\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\r\n  _i2c_bus->write(regAddr);              // Put slave register address in Tx buffer\r\n  _i2c_bus->endTransmission();           // Send the Tx buffer\r\n}\r\n\r\n\r\n/**\r\n* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n*\r\n* @brief: Write multiple bytes to an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, byte count, data array to be written\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)\r\n{\r\n  uint8_t temp[1 + count];\r\n  \r\n  temp[0] = regAddr;\r\n  for (uint8_t ii = 0; ii < count; ii++)\r\n  { \r\n    temp[ii + 1] = dest[ii];\r\n  }\r\n  \r\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\r\n  \r\n  for (uint8_t jj = 0; jj < count + 1; jj++)\r\n  {\r\n  _i2c_bus->write(temp[jj]);            // Put data in Tx buffer\r\n  }\r\n  \r\n  _i2c_bus->endTransmission();          // Send the Tx buffer\r\n}\r\n\r\n\r\n\r\n/**\r\n* @fn:I2Cscan()\r\n* @brief: Scan the I2C bus for active I2C slave devices\r\n* \r\n* @params: void\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::I2Cscan() \r\n{\r\n  // Scan for i2c devices\r\n  byte error, address;\r\n  int nDevices;\r\n\r\n  Serial.println(\"Scanning...\");\r\n\r\n  nDevices = 0;\r\n  for(address = 1; address < 127; address++ ) \r\n  {\r\n    // The i2c_scanner uses the return value of the Wire.endTransmisstion to see if a device did acknowledge to the address.\r\n    _i2c_bus->beginTransmission(address);\r\n    error = _i2c_bus->endTransmission();\r\n\r\n    if (error == 0)\r\n    {\r\n      Serial.print(\"I2C device found at address 0x\");\r\n      if (address<16) \r\n      Serial.print(\"0\");\r\n      Serial.print(address,HEX);\r\n      Serial.println(\"  !\");\r\n      nDevices++;\r\n    }\r\n    else if (error==4) \r\n    {\r\n      Serial.print(\"Unknow error at address 0x\");\r\n      if (address<16) \r\n        Serial.print(\"0\");\r\n      Serial.println(address,HEX);\r\n    }    \r\n  }\r\n  if (nDevices == 0)\r\n    Serial.println(\"No I2C devices found\\n\");\r\n  else\r\n    Serial.println(\"I2C scan complete\\n\");\r\n}\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_WebServer/I2CDev.h",
    "content": "/*\r\n * Copyright (c) 2018 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#ifndef _I2CDEV_H_\r\n#define _I2CDEV_H_\r\n\r\n#include <Wire.h>\r\n\r\nclass I2Cdev {\r\n    public:\r\n                                        I2Cdev(TwoWire*);\r\n                                        ~I2Cdev();                                                                                                                     // Class destructor for durable instances\r\n         uint8_t                        readByte(uint8_t address, uint8_t subAddress);\r\n         void                           readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);\r\n         void                           writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);\r\n         void                           writeReg(uint8_t devAddr, uint8_t regAddr);\r\n         void                           writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);\r\n         void                           I2Cscan();\r\n    private:\r\n         TwoWire*                       _i2c_bus;                                                                                                                      // Class constructor argument\r\n};\r\n\r\n#endif //_I2CDEV_H_\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_WebServer/Readme.md",
    "content": "Environmental Monitoring sketch\n\nDemonstration of a simple webserver pushing data from BME280 (pressure, temperature, humidity) and CCS811 (eCO2 and TVOC) sensors to a local web server via wifi.\n\nThe 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.\n\n![image](https://user-images.githubusercontent.com/6698410/155865582-daab5d08-0a00-4984-9684-b989d95954c0.jpg)\n\n![ESP32Data](https://user-images.githubusercontent.com/6698410/155866112-22a74cbd-d34c-4b75-ac39-dd21a73026d8.jpg)\n\nThe 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.\n\nThe [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.\n\nThe development board [design](https://oshpark.com/shared_projects/wibSiWQn) is open source and available at the OSH Park shared space.\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_deepSleep/BME280.cpp",
    "content": "/* 06/16/2017 Copyright Tlera Corporation\r\n *  \r\n *  Created by Kris Winer\r\n *  \r\n This sketch uses SDA/SCL on pins 42/43 (back pads), respectively, and it uses the Dragonfly STM32L476RE Breakout Board.\r\n The BME280 is a simple but high resolution pressure/humidity/temperature sensor, which can be used in its high resolution\r\n mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\r\n only 1 microAmp. The choice will depend on the application.\r\n \r\n Library may be used freely and without limit with attribution.\r\n \r\n*/\r\n \r\n#include \"BME280.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n#define SerialDebug false  // set to true to get Serial output for debugging\r\n\r\nBME280::BME280(I2Cdev* i2c_bus)\r\n{\r\n  _i2c_bus = i2c_bus;\r\n}\r\n\r\n\r\nuint8_t BME280::getChipID()\r\n{\r\n  uint8_t c = _i2c_bus->readByte(BME280_ADDRESS, BME280_ID);\r\n  return c;\r\n}\r\n\r\nvoid BME280::resetBME280()\r\n{\r\n  _i2c_bus->writeByte(BME280_ADDRESS, BME280_RESET, 0xB6);\r\n}\r\n\r\n\r\nint32_t BME280::readBME280Temperature()\r\n{\r\n  uint8_t rawData[3];  // 20-bit temperature register data stored here\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_TEMP_MSB, 3, &rawData[0]);  \r\n  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);\r\n}\r\n\r\n\r\nint32_t BME280::readBME280Pressure()\r\n{\r\n  uint8_t rawData[3];  // 20-bit pressure register data stored here\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_PRESS_MSB, 3, &rawData[0]);  \r\n  return (uint32_t) (((uint32_t) rawData[0] << 16 | (uint32_t) rawData[1] << 8 | rawData[2]) >> 4);\r\n}\r\n\r\n\r\nint32_t BME280::BME280::readBME280Humidity()\r\n{\r\n  uint8_t rawData[2];  // 16-bit humiditye register data stored here\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_HUM_MSB, 2, &rawData[0]);  \r\n  return (uint32_t) (((uint32_t) rawData[0] << 24 | (uint32_t) rawData[1] << 16) ) >> 16;\r\n}\r\n\r\n\r\nvoid BME280::BME280forced()\r\n{\r\n    uint8_t temp = _i2c_bus->readByte(BME280_ADDRESS, BME280_CTRL_MEAS);\r\n    _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, temp | Forced);\r\n \r\n    while( (_i2c_bus->readByte(BME280_ADDRESS, BME280_STATUS)) & 0x10) { } // wait for conversion byte to clear\r\n}\r\n\r\n\r\nvoid BME280::BME280Init(uint8_t Posr, uint8_t Hosr, uint8_t Tosr, uint8_t Mode, uint8_t IIRFilter, uint8_t SBy)\r\n{\r\n  // Configure the BME280\r\n  // Set H oversampling rate\r\n  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_HUM, 0x07 & Hosr);\r\n  // Set T and P oversampling rates and sensor mode\r\n  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);\r\n  // Set standby time interval in normal mode and bandwidth\r\n  _i2c_bus->writeByte(BME280_ADDRESS, BME280_CONFIG, SBy << 5 | IIRFilter << 2);\r\n  \r\n  uint8_t calib[26];\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_CALIB00, 26, &calib[0]);\r\n  _dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);\r\n  _dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);\r\n  _dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);\r\n  _dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);\r\n  _dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);\r\n  _dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);\r\n  _dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);\r\n  _dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);\r\n  _dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);\r\n  _dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);\r\n  _dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);\r\n  _dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);\r\n  _dig_H1 = calib[25];\r\n  _i2c_bus->readBytes(BME280_ADDRESS, BME280_CALIB26, 7, &calib[0]);\r\n  _dig_H2 = ( int16_t)((( int16_t) calib[1] << 8) | calib[0]);\r\n  _dig_H3 = calib[2];\r\n  _dig_H4 = ( int16_t)(((( int16_t) calib[3] << 8) | (0x0F & calib[4]) << 4) >> 4);\r\n  _dig_H5 = ( int16_t)(((( int16_t) calib[5] << 8) | (0xF0 & calib[4]) ) >> 4 );\r\n  _dig_H6 = calib[6];\r\n  \r\n if(SerialDebug)  {\r\n  USBSerial.println(\"Calibration coeficients:\");\r\n  USBSerial.print(\"_dig_T1 =\"); \r\n  USBSerial.println(_dig_T1);\r\n  USBSerial.print(\"_dig_T2 =\"); \r\n  USBSerial.println(_dig_T2);\r\n  USBSerial.print(\"_dig_T3 =\"); \r\n  USBSerial.println(_dig_T3);\r\n  USBSerial.print(\"_dig_P1 =\"); \r\n  USBSerial.println(_dig_P1);\r\n  USBSerial.print(\"_dig_P2 =\"); \r\n  USBSerial.println(_dig_P2);\r\n  USBSerial.print(\"_dig_P3 =\"); \r\n  USBSerial.println(_dig_P3);\r\n  USBSerial.print(\"_dig_P4 =\"); \r\n  USBSerial.println(_dig_P4);\r\n  USBSerial.print(\"_dig_P5 =\"); \r\n  USBSerial.println(_dig_P5);\r\n  USBSerial.print(\"_dig_P6 =\"); \r\n  USBSerial.println(_dig_P6);\r\n  USBSerial.print(\"_dig_P7 =\"); \r\n  USBSerial.println(_dig_P7);\r\n  USBSerial.print(\"_dig_P8 =\"); \r\n  USBSerial.println(_dig_P8);\r\n  USBSerial.print(\"_dig_P9 =\"); \r\n  USBSerial.println(_dig_P9);\r\n  USBSerial.print(\"_dig_H1 =\"); \r\n  USBSerial.println(_dig_H1);\r\n  USBSerial.print(\"_dig_H2 =\"); \r\n  USBSerial.println(_dig_H2);\r\n  USBSerial.print(\"_dig_H3 =\"); \r\n  USBSerial.println(_dig_H3);\r\n  USBSerial.print(\"_dig_H4 =\"); \r\n  USBSerial.println(_dig_H4);\r\n  USBSerial.print(\"_dig_H5 =\"); \r\n  USBSerial.println(_dig_H5);\r\n  USBSerial.print(\"_dig_H6 =\"); \r\n  USBSerial.println(_dig_H6);\r\n }\r\n}\r\n\r\n\r\n// Returns temperature in DegC, resolution is 0.01 DegC. Output value of\r\n// “5123” equals 51.23 DegC.\r\nint32_t BME280::BME280_compensate_T(int32_t adc_T)\r\n{\r\n  int32_t var1, var2, T;\r\n  var1 = ((((adc_T >> 3) - ((int32_t)_dig_T1 << 1))) * ((int32_t)_dig_T2)) >> 11;\r\n  var2 = (((((adc_T >> 4) - ((int32_t)_dig_T1)) * ((adc_T >> 4) - ((int32_t)_dig_T1))) >> 12) * ((int32_t)_dig_T3)) >> 14;\r\n  _t_fine = var1 + var2;\r\n  T = (_t_fine * 5 + 128) >> 8;\r\n  return T;\r\n}\r\n\r\n\r\n// Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8\r\n//fractional bits).\r\n//Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa\r\nuint32_t BME280::BME280_compensate_P(int32_t adc_P)\r\n{\r\n  long long var1, var2, p;\r\n  var1 = ((long long)_t_fine) - 128000;\r\n  var2 = var1 * var1 * (long long)_dig_P6;\r\n  var2 = var2 + ((var1*(long long)_dig_P5)<<17);\r\n  var2 = var2 + (((long long)_dig_P4)<<35);\r\n  var1 = ((var1 * var1 * (long long)_dig_P3)>>8) + ((var1 * (long long)_dig_P2)<<12);\r\n  var1 = (((((long long)1)<<47)+var1))*((long long)_dig_P1)>>33;\r\n  if(var1 == 0)\r\n  {\r\n    return 0;\r\n    // avoid exception caused by division by zero\r\n  }\r\n  p = 1048576 - adc_P;\r\n  p = (((p<<31) - var2)*3125)/var1;\r\n  var1 = (((long long)_dig_P9) * (p>>13) * (p>>13)) >> 25;\r\n  var2 = (((long long)_dig_P8) * p)>> 19;\r\n  p = ((p + var1 + var2) >> 8) + (((long long)_dig_P7)<<4);\r\n  return (uint32_t)p;\r\n}\r\n\r\n\r\n// Returns humidity in %RH as unsigned 32 bit integer in Q22.10 format (22integer and 10fractional bits).\r\n// Output value of “47445”represents 47445/1024= 46.333%RH\r\nuint32_t BME280::BME280_compensate_H(int32_t adc_H)\r\n{\r\nint32_t var;\r\n\r\nvar = (_t_fine - ((int32_t)76800));\r\nvar = (((((adc_H << 14) - (((int32_t)_dig_H4) << 20) - (((int32_t)_dig_H5) * var)) +\r\n((int32_t)16384)) >> 15) * (((((((var * ((int32_t)_dig_H6)) >> 10) * (((var *\r\n((int32_t)_dig_H3)) >> 11) + ((int32_t)32768))) >> 10) + ((int32_t)2097152)) * ((int32_t)_dig_H2) + 8192) >> 14));\r\nvar = (var - (((((var >> 15) * (var >> 15)) >> 7) * ((int32_t)_dig_H1)) >> 4));\r\nvar = (var < 0 ? 0 : var); \r\nvar = (var > 419430400 ? 419430400 : var);\r\nreturn(uint32_t)(var >> 12);\r\n}\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_deepSleep/BME280.h",
    "content": "/* 06/16/2017 Copyright Tlera Corporation\r\n *  \r\n *  Created by Kris Winer\r\n *  \r\n This sketch uses SDA/SCL on pins 42/43 (back pads), respectively, and it uses the Dragonfly STM32L476RE Breakout Board.\r\n The BME280 is a simple but high resolution pressure/humidity/temperature sensor, which can be used in its high resolution\r\n mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\r\n only 1 microAmp. The choice will depend on the application.\r\n \r\n Library may be used freely and without limit with attribution.\r\n \r\n*/\r\n  \r\n#ifndef BME280_h\r\n#define BME280_h\r\n\r\n#include \"Arduino.h\"\r\n#include \"I2CDev.h\"\r\n#include <Wire.h>\r\n\r\n/* BME280 registers\r\n*  http://www.mouser.com/ds/2/783/BST-BME280_DS001-11-844833.pdf\r\n*/\r\n#define BME280_HUM_LSB    0xFE\r\n#define BME280_HUM_MSB    0xFD\r\n#define BME280_TEMP_XLSB  0xFC\r\n#define BME280_TEMP_LSB   0xFB\r\n#define BME280_TEMP_MSB   0xFA\r\n#define BME280_PRESS_XLSB 0xF9\r\n#define BME280_PRESS_LSB  0xF8\r\n#define BME280_PRESS_MSB  0xF7\r\n#define BME280_CONFIG     0xF5\r\n#define BME280_CTRL_MEAS  0xF4\r\n#define BME280_STATUS     0xF3\r\n#define BME280_CTRL_HUM   0xF2\r\n#define BME280_RESET      0xE0\r\n#define BME280_ID         0xD0  // should be 0x60\r\n#define BME280_CALIB00    0x88\r\n#define BME280_CALIB26    0xE1\r\n\r\n#define BME280_ADDRESS           0x76   // Address of BMP280 altimeter when ADO = 0\r\n\r\n\r\n#define  P_OSR_01 0x01\r\n#define  P_OSR_02 0x02\r\n#define  P_OSR_04 0x03\r\n#define  P_OSR_08 0x04\r\n#define  P_OSR_16 0x05\r\n\r\n#define  H_OSR_01 0x01\r\n#define  H_OSR_02 0x02\r\n#define  H_OSR_04 0x03\r\n#define  H_OSR_08 0x04\r\n#define  H_OSR_16 0x05\r\n\r\n#define  T_OSR_01 0x01\r\n#define  T_OSR_02 0x02\r\n#define  T_OSR_04 0x03\r\n#define  T_OSR_08 0x04\r\n#define  T_OSR_16 0x05\r\n\r\n#define  full       0x00\r\n#define  BW0_223ODR 0x01\r\n#define  BW0_092ODR 0x02\r\n#define  BW0_042ODR 0x03\r\n#define  BW0_021ODR 0x04  \r\n\r\n#define  BME280Sleep 0x00\r\n#define  Forced      0x01\r\n#define  Forced2     0x02\r\n#define  Normal      0x03\r\n\r\n#define  t_00_5ms 0x00\r\n#define  t_62_5ms 0x01\r\n#define  t_125ms  0x02\r\n#define  t_250ms  0x03\r\n#define  t_500ms  0x04\r\n#define  t_1000ms 0x05\r\n#define  t_10ms   0x06\r\n#define  t_20ms   0x07 \r\n\r\n\r\nclass BME280\r\n{\r\n  public: \r\n  BME280(I2Cdev* i2c_bus);\r\n  uint8_t getChipID();\r\n  void resetBME280();\r\n  int32_t readBME280Temperature();\r\n  int32_t readBME280Pressure();\r\n  int32_t readBME280Humidity();\r\n  void BME280forced();\r\n  void BME280Init(uint8_t Posr, uint8_t Hosr, uint8_t Tosr, uint8_t Mode, uint8_t IIRFilter, uint8_t SBy);\r\n  int32_t  BME280_compensate_T(int32_t adc_T);\r\n  uint32_t BME280_compensate_P(int32_t adc_P);\r\n  uint32_t BME280_compensate_H(int32_t adc_H);\r\n  \r\n  private:\r\n  uint8_t  _dig_H1, _dig_H3, _dig_H6;\r\n  uint16_t _dig_T1, _dig_P1, _dig_H4, _dig_H5;\r\n  int16_t  _dig_T2, _dig_T3, _dig_P2, _dig_P3, _dig_P4, _dig_P5, _dig_P6, _dig_P7, _dig_P8, _dig_P9, _dig_H2;\r\n  int32_t  _t_fine;\r\n  I2Cdev* _i2c_bus;\r\n  };\r\n\r\n#endif\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_deepSleep/CCS811.cpp",
    "content": "/* 06/16/2017 Copyright Tlera Corporation\r\n *  \r\n *  Created by Kris Winer\r\n *  \r\n *  The AMS CCS811 is an air quality sensor that provides equivalent CO2 and volatile organic measurements from direct\r\n *  I2C register reads as well as current and voltage (effective resistance of the sensing element). Gas sensors, including \r\n *  this MEMs gas sensor in the CCS811 measure resistance of a substrate that changes when exposed to inert gasses and \r\n *  volatile organic compounds. Changed in concentration vary exponentially with the changes in resistance. The CCS811\r\n *  has an embedded ASIC calibrated against most common indoor pollutants that returns a good estimate of\r\n *  equivalent CO2 concentration in parts per million (400 - 8192 range) and volatile organic compounds in parts per billion (0 - 1187).\r\n *  The sensor is quite sensitive to breath and other human emissions.\r\n *  \r\n *  Library may be used freely and without limit with attribution.\r\n *  \r\n */\r\n#include \"CCS811.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n#define SerialDebug false  // set to true to get Serial output for debugging\r\n\r\nCCS811::CCS811(I2Cdev* i2c_bus)\r\n{\r\n  _i2c_bus = i2c_bus;   \r\n}\r\n\r\n\r\nuint8_t CCS811::getChipID()\r\n{\r\n byte e = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ID);  // Read WHO_AM_I register for CCS8110\r\n return e;\r\n}\r\n\r\n\r\nvoid CCS811::checkCCS811Status() \r\n{\r\n   // Check CCS811 status\r\n  uint8_t status = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_STATUS);\r\n  if(SerialDebug) {USBSerial.print(\"status = 0X\"); USBSerial.println(status, HEX);}\r\n  if(status & 0x80 && SerialDebug) {USBSerial.println(\"Firmware is in application mode. CCS811 is ready!\");}\r\n  else { \r\n    if(SerialDebug) {USBSerial.println(\"Firmware is in boot mode!\");}\r\n   }\r\n  \r\n  if(status & 0x10 && SerialDebug) {USBSerial.println(\"Valid application firmware loaded!\");}\r\n  else { \r\n    if(SerialDebug) {USBSerial.println(\"No application firmware is loaded!\");}\r\n  }\r\n  if(status & 0x08 && SerialDebug) {\r\n   USBSerial.println(\"New data available!\");\r\n  }\r\n  else { \r\n    if(SerialDebug) {USBSerial.println(\"No new data available!\");}\r\n  }\r\n\r\n  if(status & 0x01 && SerialDebug) {USBSerial.println(\"Error detected!\");\r\n        uint8_t error = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ERROR_ID);\r\n        if(error & 0x01 && SerialDebug) USBSerial.println(\"CCS811 received invalid I2C write request!\");\r\n        if(error & 0x02 && SerialDebug) USBSerial.println(\"CCS811 received invalid I2C read request!\");\r\n        if(error & 0x04 && SerialDebug) USBSerial.println(\"CCS811 received unsupported mode request!\");\r\n        if(error & 0x08 && SerialDebug) USBSerial.println(\"Sensor resistance measurement at maximum range!\");\r\n        if(error & 0x10 && SerialDebug) USBSerial.println(\"Heater current is not in range!\");\r\n        if(error & 0x20 && SerialDebug) USBSerial.println(\"Heater voltage is not being applied correctly!\");\r\n  }\r\n  else { \r\n    if(SerialDebug) {USBSerial.println(\"No error detected!\");}\r\n  }\r\n  \r\n  if(SerialDebug) USBSerial.println(\" \");\r\n  \r\n  }\r\n\r\n\r\n  void CCS811::CCS811init(uint8_t AQRate)\r\n  {\r\n    // initialize CCS811 and check version and status\r\n  byte HWVersion = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_HW_VERSION);\r\n    if(SerialDebug) {USBSerial.print(\"CCS811 Hardware Version = 0x\"); USBSerial.println(HWVersion, HEX); }\r\n\r\n  uint8_t FWBootVersion[2] = {0, 0}, FWAppVersion[2] = {0,0};\r\n  _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_FW_BOOT_VERSION, 2, &FWBootVersion[0]);\r\n  if(SerialDebug) {  \r\n    USBSerial.println(\"CCS811 Firmware Boot Version: \"); \r\n    USBSerial.print(\"Major = \"); USBSerial.println((FWBootVersion[0] & 0xF0) >> 4); \r\n    USBSerial.print(\"Minor = \"); USBSerial.println(FWBootVersion[0] & 0x04); \r\n    USBSerial.print(\"Trivial = \"); USBSerial.println(FWBootVersion[1]); \r\n  }\r\n  _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_FW_APP_VERSION, 2, &FWAppVersion[0]);\r\n  if(SerialDebug) {\r\n    USBSerial.println(\"CCS811 Firmware App Version: \"); \r\n    USBSerial.print(\"Major = \"); USBSerial.println((FWAppVersion[0] & 0xF0) >> 4); \r\n    USBSerial.print(\"Minor = \"); USBSerial.println(FWAppVersion[0] & 0x04); \r\n    USBSerial.print(\"Trivial = \"); USBSerial.println(FWAppVersion[1]); \r\n  }\r\n\r\n  // Check CCS811 status\r\n  checkCCS811Status();\r\n  _i2c_bus->writeReg(CCS811_ADDRESS, CCS811_APP_START);\r\n  delay(100);\r\n  checkCCS811Status();\r\n\r\n  // set CCS811 measurement mode\r\n  _i2c_bus->writeByte(CCS811_ADDRESS, CCS811_MEAS_MODE, AQRate << 4 | 0x08); // pulsed heating mode, enable interrupt\r\n  uint8_t measmode = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_MEAS_MODE);\r\n  if(SerialDebug) {USBSerial.print(\"Confirm measurement mode = 0x\"); USBSerial.println(measmode, HEX);}\r\n  }\r\n\r\n\r\n  void CCS811::compensateCCS811(int32_t compHumidity, int32_t compTemp)\r\n  {\r\n      // Update CCS811 humidity and temperature compensation\r\n      uint8_t temp[5] = {0, 0, 0, 0, 0};\r\n      temp[0] = CCS811_ENV_DATA;\r\n      temp[1] = ((compHumidity % 1024) / 100) > 7 ? (compHumidity/1024 + 1)<<1 : (compHumidity/1024)<<1;\r\n      temp[2] = 0;\r\n      if(((compHumidity % 1024) / 100) > 2 && (((compHumidity % 1024) / 100) < 8))\r\n      {\r\n       temp[1] |= 1;\r\n      }\r\n\r\n      compTemp += 2500;\r\n      temp[3] = ((compTemp % 100) / 100) > 7 ? (compTemp/100 + 1)<<1 : (compTemp/100)<<1;\r\n      temp[4] = 0;\r\n      if(((compTemp % 100) / 100) > 2 && (((compTemp % 100) / 100) < 8))\r\n      {\r\n       temp[3] |= 1;\r\n      }\r\n\r\n//      Wire.transfer(CCS811_ADDRESS, &temp[0], 5, NULL, 0);\r\n     _i2c_bus->writeBytes(CCS811_ADDRESS, CCS811_ENV_DATA, 4, &temp[1]);\r\n  }\r\n\r\n   void CCS811::readCCS811Data(uint8_t * destination)\r\n   {\r\n      uint8_t rawData[8] = {0, 0, 0, 0, 0, 0, 0, 0};\r\n      uint8_t status = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_STATUS);\r\n      \r\n      if(status & 0x01) { // check for errors\r\n        uint8_t error = _i2c_bus->readByte(CCS811_ADDRESS, CCS811_ERROR_ID);\r\n        if(error & 0x01 && SerialDebug) USBSerial.println(\"CCS811 received invalid I2C write request!\");\r\n        if(error & 0x02 && SerialDebug) USBSerial.println(\"CCS811 received invalid I2C read request!\");\r\n        if(error & 0x04 && SerialDebug) USBSerial.println(\"CCS811 received unsupported mode request!\");\r\n        if(error & 0x08 && SerialDebug) USBSerial.println(\"Sensor resistance measurement at maximum range!\");\r\n        if(error & 0x10 && SerialDebug) USBSerial.println(\"Heater current is not in range!\");\r\n        if(error & 0x20 && SerialDebug) USBSerial.println(\"Heater voltage is not being applied correctly!\");\r\n      }\r\n\r\n      _i2c_bus->readBytes(CCS811_ADDRESS, CCS811_ALG_RESULT_DATA, 8, &rawData[0]);\r\n\r\n   for(int ii = 0; ii < 8; ii++)\r\n   {\r\n    destination[ii] = rawData[ii];\r\n   }\r\n   \r\n   }\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_deepSleep/CCS811.h",
    "content": "#ifndef CCS811_h\r\n#define CCS811_h\r\n\r\n#include \"Arduino.h\"\r\n#include \"Wire.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n/* CCS811 Registers\r\nhttp://www.mouser.com/ds/2/588/CCS811_DS000459_3-00-1098798.pdf\r\n*/\r\n#define CCS811_STATUS             0x00\r\n#define CCS811_MEAS_MODE          0x01\r\n#define CCS811_ALG_RESULT_DATA    0x02\r\n#define CCS811_RAW_DATA           0x03\r\n#define CCS811_ENV_DATA           0x05\r\n#define CCS811_NTC                0x06\r\n#define CCS811_THRESHOLDS         0x10\r\n#define CCS811_BASELINE           0x11\r\n#define CCS811_HW_ID              0x20  // WHO_AM_I should be 0x81\r\n#define CCS811_ID                 0x20  // WHO_AM_I should be 0x1X\r\n#define CCS811_HW_VERSION         0x21  \r\n#define CCS811_FW_BOOT_VERSION    0x23\r\n#define CCS811_FW_APP_VERSION     0x24\r\n#define CCS811_ERROR_ID           0xE0\r\n#define CCS811_APP_START          0xF4\r\n#define CCS811_SW_RESET           0xFF\r\n\r\n#define CCS811_ADDRESS            0x5A   // Address of the CCS811 Air Quality Sensor\r\n\r\n#define  dt_idle  0x00\r\n#define  dt_1sec  0x01\r\n#define  dt_10sec 0x02\r\n#define  dt_60sec 0x03\r\n\r\nclass CCS811\r\n{\r\n  public: \r\n  CCS811(I2Cdev* i2c_bus);\r\n  void checkCCS811Status();\r\n  void CCS811init(uint8_t AQRate);\r\n  uint8_t getChipID();\r\n  void compensateCCS811(int32_t compHumidity, int32_t compTemp);\r\n  void readCCS811Data(uint8_t * destination);\r\n  \r\n  private:\r\n  I2Cdev* _i2c_bus;\r\n};\r\n\r\n\r\n#endif\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_deepSleep/CCS811_BME280_ESP32C3Mini_deepSleep.ino",
    "content": "/* 06/16/2017 Copyright Tlera Corporation\r\n *  \r\n *  Created by Kris Winer\r\n *  \r\n *  The AMS CCS811 is an air quality sensor that provides equivalent CO2 and volatile organic measurements from direct\r\n *  I2C register reads as well as current and voltage (effective resistance of the sensing element). Gas sensors, including \r\n *  this MEMs gas sensor in the CCS811 measure resistance of a substrate that changes when exposed to inert gasses and \r\n *  volatile organic compounds. Changed in concentration vary exponentially with the changes in resistance. The CCS811\r\n *  has an embedded ASIC calibrated against most common indoor pollutants that returns a good estimate of\r\n *  equivalent CO2 concentration in parts per million (400 - 8192 range) and volatile organic componds in parts per billion (0 - 1187).\r\n *  The sensor is quite sensitive to breath and other human emissions.\r\n *  \r\n  *  The BME280 is a simple but high resolution pressure/humidity/temperature sensor, which can be used in its high resolution\r\n *  mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\r\n *  only 1 microAmp. The choice will depend on the application.\r\n \r\n    Library may be used freely and without limit with attribution.\r\n \r\n  */\r\n#include <Arduino.h>\r\n#include <driver/adc.h>\r\n#include \"BME280.h\"\r\n#include \"CCS811.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n#define uS_TO_S_FACTOR 1000000ULL  /* Conversion factor for micro seconds to seconds */\r\n#define TIME_TO_SLEEP  600        /* Time ESP32 will go to sleep (in seconds) */\r\n\r\nRTC_DATA_ATTR int bootCount = 0;\r\n\r\nvoid print_wakeup_reason(){\r\n  esp_sleep_wakeup_cause_t wakeup_reason;\r\n\r\n  wakeup_reason = esp_sleep_get_wakeup_cause();\r\n\r\n  switch(wakeup_reason)\r\n  {\r\n//      case ESP_SLEEP_WAKEUP_EXT0 : USBSerial.println(\"Wakeup caused by external signal using RTC_IO\"); break;\r\n//      case ESP_SLEEP_WAKEUP_EXT1 : USBSerial.println(\"Wakeup caused by external signal using RTC_CNTL\"); break;\r\n//      case ESP_SLEEP_WAKEUP_TIMER : USBSerial.println(\"Wakeup caused by timer\"); break;\r\n//      case ESP_SLEEP_WAKEUP_TOUCHPAD : USBSerial.println(\"Wakeup caused by touchpad\"); break;\r\n//      case ESP_SLEEP_WAKEUP_ULP : USBSerial.println(\"Wakeup caused by ULP program\"); break;\r\n//      default : USBSerial.printf(\"Wakeup was not caused by deep sleep: %d\\n\",wakeup_reason); break;\r\n  }\r\n}\r\n\r\n#define I2C_BUS          Wire               // Define the I2C bus (Wire instance) you wish to use\r\n\r\nI2Cdev                   i2c_0(&I2C_BUS);   // Instantiate the I2Cdev object and point to the desired I2C bus\r\n\r\n#define SerialDebug false  // set to true to get Serial output for debugging\r\nconst uint8_t myLed =  2;  // green led\r\nconst uint8_t myBat =  1;  // battery monitor\r\n\r\nfloat VBat = 0.0f;\r\nuint32_t chipId = 0;\r\n\r\n// BME280 definitions\r\n/* Specify BME280 configuration\r\n *  Choices are:\r\n P_OSR_01, P_OSR_02, P_OSR_04, P_OSR_08, P_OSR_16 // pressure oversampling\r\n H_OSR_01, H_OSR_02, H_OSR_04, H_OSR_08, H_OSR_16 // humidity oversampling\r\n T_OSR_01, T_OSR_02, T_OSR_04, T_OSR_08, T_OSR_16 // temperature oversampling\r\n full, BW0_223ODR,BW0_092ODR, BW0_042ODR, BW0_021ODR // bandwidth at 0.021 x sample rate\r\n BME280Sleep, forced,, forced2, normal //operation modes\r\n t_00_5ms = 0, t_62_5ms, t_125ms, t_250ms, t_500ms, t_1000ms, t_10ms, t_20ms // determines sample rate\r\n */\r\nuint8_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\r\n\r\nfloat Temperature, Pressure, Humidity;              // stores BME280 pressures sensor pressure and temperature\r\nint32_t rawPress, rawTemp, rawHumidity, compTemp;   // pressure and temperature raw count output for BME280\r\nuint32_t compHumidity, compPress;                   // variables to hold raw BME280 humidity value\r\n\r\nfloat temperature_C, temperature_F, pressure, humidity, altitude; // Scaled output of the BME280\r\n\r\nBME280 BME280(&i2c_0); // instantiate BME280 class\r\n\r\n\r\n// CCS811 definitions\r\n#define CCS811_intPin  4\r\n#define CCS811_wakePin 7\r\n\r\n/* Specify CCS811 sensor parameters\r\n *  Choices are   dt_idle , dt_1sec, dt_10sec, dt_60sec\r\n */\r\n//uint8_t AQRate = dt_60sec;  // set the sample rate\r\nuint8_t AQRate = dt_idle;  // set the sample rate\r\nuint8_t rawData[8] = {0, 0, 0, 0, 0, 0, 0, 0};  // array to hold the raw data\r\nuint16_t eCO2 = 0, TVOC = 0;\r\nuint8_t Current = 0;\r\nfloat Voltage = 0.0f;\r\n\r\nvolatile bool newCCS811Data  = true; // boolean flag for interrupt\r\n\r\nCCS811 CCS811(&i2c_0); // instantiate CCS811 class\r\n\r\n\r\nvoid setup()\r\n{\r\n  if(SerialDebug) USBSerial.begin(115200);\r\n  if(SerialDebug) USBSerial.println(\"Serial enabled!\");\r\n\r\n  pinMode(myLed, OUTPUT);\r\n  digitalWrite(myLed, HIGH); // start with led off, active LOW\r\n  pinMode(myBat, INPUT); // battery voltage monitor\r\n  pinMode(CCS811_intPin, INPUT); // active LOW\r\n\r\n  //Increment boot number and print it every reboot\r\n  ++bootCount;\r\n   if(SerialDebug) USBSerial.println(\"Boot number: \" + String(bootCount));\r\n\r\n  //Print the wakeup reason for ESP32\r\n  print_wakeup_reason();\r\n\r\n  if(bootCount == 1) { /*... Only need to do this part once on startup! */\r\n    for(int i=0; i<17; i=i+8) {\r\n      chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xFF) << i;\r\n    }\r\n\r\n   if(SerialDebug) USBSerial.printf(\"ESP32 Chip model = %s Rev %d\\n\", ESP.getChipModel(), ESP.getChipRevision());\r\n   if(SerialDebug) USBSerial.printf(\"This chip has %d cores\\n\", ESP.getChipCores());\r\n   if(SerialDebug){USBSerial.print(\"Chip ID: \"); USBSerial.println(chipId);}\r\n  } /*... Only need to do this part once on startup! */\r\n\r\n  adc1_config_width(ADC_WIDTH_BIT_12);\r\n  adc1_config_channel_atten(ADC1_CHANNEL_1, ADC_ATTEN_DB_11);\r\n\r\n  I2C_BUS.begin(0, 3);               // Set master mode, default on SDA on pin 0/SCL  on pin 3\r\n  I2C_BUS.setClock(400000);          // I2C frequency at 400 kHz\r\n\r\n  pinMode(CCS811_wakePin, OUTPUT);\r\n  digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor\r\n\r\n  if(bootCount == 1) { /*... Only need to do this part once on startup! */\r\n  //Enable the CCS811 for I2C scan\r\n  digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n  \r\n  if(SerialDebug) USBSerial.println(\"Scan for I2C devices:\");\r\n  i2c_0.I2Cscan();                   // should detect BME280 at 0x76 and CCS811 at 0x5A\r\n\r\n  //Disable the CCS811 for I2C scan\r\n  digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor\r\n  delay(1);\r\n\r\n  // Read the WHO_AM_I register of the BME280 this is a good test of communication\r\n  byte BME280ChipID = BME280.getChipID();  // Read WHO_AM_I register for BME280\r\n  if(SerialDebug){ \r\n    USBSerial.print(\"BME280 \"); USBSerial.print(\"I AM \"); USBSerial.print(BME280ChipID, HEX); USBSerial.print(\" I should be \"); USBSerial.println(0x60, HEX);\r\n    USBSerial.println(\" \");\r\n  }\r\n\r\n  // Read the WHO_AM_I register of the CCS811 this is a good test of communication\r\n  digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n  byte CCS811ChipID = CCS811.getChipID();\r\n  digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor\r\n  if(SerialDebug) {\r\n    USBSerial.print(\"CCS811 \"); USBSerial.print(\"I AM \"); USBSerial.print(CCS811ChipID, HEX); USBSerial.print(\" I should be \"); USBSerial.println(0x81, HEX);\r\n    USBSerial.println(\" \");\r\n  }\r\n  \r\n  if(BME280ChipID == 0x60 && CCS811ChipID == 0x81 ) {\r\n\r\n   if(SerialDebug) {USBSerial.println(\"BME280+CCS811 are online...\"); USBSerial.println(\" \");}\r\n   digitalWrite(myLed, LOW);\r\n\r\n   BME280.resetBME280();                                                        // reset BME280 before initilization\r\n   delay(10);\r\n   BME280.BME280Init(Posr, Hosr, Tosr, Mode, IIRFilter, SBy);                   // Initialize BME280 altimeter\r\n   BME280.BME280forced();                                                       // get initial data sample, then go back to sleep\r\n\r\n   // initialize CCS811 and check version and status\r\n   digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n   CCS811.CCS811init(AQRate);\r\n   digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor\r\n\r\n   }\r\n   else {\r\n   if(BME280ChipID != 0x60 && SerialDebug) USBSerial.println(\" BME280 not functioning!\");    \r\n   if(CCS811ChipID != 0x81 && SerialDebug) USBSerial.println(\" CCS811 not functioning!\");  \r\n   while(1) { }; // no point proceeding, so wait here forever...\r\n   }\r\n  } /*... Only need to do this part once on startup! */\r\n  \r\n\r\n    /*   Put here anything you want to do before going to sleep */\r\n    digitalWrite(CCS811_wakePin, LOW); // set LOW to enable the CCS811 air quality sensor\r\n    CCS811.readCCS811Data(rawData);\r\n    CCS811.compensateCCS811(compHumidity, compTemp); // compensate CCS811 using BME280 humidity and temperature\r\n    digitalWrite(CCS811_wakePin, HIGH); // set HIGH to disable the CCS811 air quality sensor \r\n\r\n    eCO2 = (uint16_t) ((uint16_t) rawData[0] << 8 | rawData[1]);\r\n    TVOC = (uint16_t) ((uint16_t) rawData[2] << 8 | rawData[3]);\r\n    Current = (rawData[6] & 0xFC) >> 2;\r\n    Voltage = (float) ((uint16_t) ((((uint16_t)rawData[6] & 0x02) << 8) | rawData[7])) * (1.65f/1023.0f); \r\n    \r\n     if(SerialDebug){\r\n      USBSerial.println(\"CCS811:\");\r\n      USBSerial.print(\"Eq CO2 in ppm = \"); USBSerial.println(eCO2);\r\n      USBSerial.print(\"TVOC in ppb = \"); USBSerial.println(TVOC);\r\n      USBSerial.print(\"Sensor current (uA) = \"); USBSerial.println(Current);\r\n      USBSerial.print(\"Sensor voltage (V) = \"); USBSerial.println(Voltage, 2);  \r\n      USBSerial.println(\" \");\r\n     }\r\n\r\n    /* BME280 sensor data */\r\n    BME280.BME280Init(Posr, Hosr, Tosr, Mode, IIRFilter, SBy);                   // Initialize BME280 altimeter\r\n    BME280.BME280forced();  // get one data sample, then go back to sleep\r\n\r\n    rawTemp =  BME280.readBME280Temperature();\r\n    compTemp = BME280.BME280_compensate_T(rawTemp);\r\n    temperature_C = (float) compTemp/100.0f;\r\n    temperature_F = 9.0f*temperature_C/5.0f + 32.0f;\r\n     \r\n    rawPress =  BME280.readBME280Pressure();\r\n    pressure = (float) BME280.BME280_compensate_P(rawPress)/25600.f; // Pressure in mbar\r\n    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));   \r\n   \r\n    rawHumidity =  BME280.readBME280Humidity();\r\n    compHumidity = BME280.BME280_compensate_H(rawHumidity);\r\n    humidity = (float)compHumidity/1024.0f; // Humidity in %RH\r\n\r\n    if(SerialDebug) {\r\n    USBSerial.println(\"BME280:\");\r\n    USBSerial.print(\"Altimeter temperature = \"); \r\n    USBSerial.print( temperature_C, 2); \r\n    USBSerial.println(\" C\"); // temperature in degrees Celsius\r\n    USBSerial.print(\"Altimeter temperature = \"); \r\n    USBSerial.print(temperature_F, 2); \r\n    USBSerial.println(\" F\"); // temperature in degrees Fahrenheit\r\n    USBSerial.print(\"Altimeter pressure = \"); \r\n    USBSerial.print(pressure, 2);  \r\n    USBSerial.println(\" mbar\");// pressure in millibar\r\n    USBSerial.print(\"Altitude = \"); \r\n    USBSerial.print(altitude, 2); \r\n    USBSerial.println(\" feet\");\r\n    USBSerial.print(\"Altimeter humidity = \"); \r\n    USBSerial.print(humidity, 1);  \r\n    USBSerial.println(\" %RH\");// pressure in millibar\r\n    USBSerial.println(\" \");\r\n    }\r\n    /* ADC at attenuation 11 should read from 0 to 2.6 V nominally. The resistor divider\r\n     *  is 1/2, and a calibration factor of 1.15 is applied to bring measurements into \r\n     *  agreement with multimeter */\r\n    VBat = 2.0f * 2.60f * 1.15f * ((float) adc1_get_raw((adc1_channel_t)1)) / 4095.0f;\r\n    if(SerialDebug) {USBSerial.print(\"Battery voltage = \"); USBSerial.print(VBat, 2); USBSerial.println(\" V\");}\r\n      \r\n    digitalWrite(myLed, LOW); delay(1); digitalWrite(myLed, HIGH); // blink led at end of loop\r\n\r\n    esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP * uS_TO_S_FACTOR);\r\n    if(SerialDebug) USBSerial.println(\"Setup ESP32 to sleep for every \" + String(TIME_TO_SLEEP) + \" Seconds\");\r\n  \r\n    //Go to sleep now\r\n    if(SerialDebug) USBSerial.println(\"Going to sleep now\");\r\n    if(SerialDebug) USBSerial.flush(); \r\n    esp_deep_sleep_start();\r\n    if(SerialDebug) USBSerial.println(\"This will never be printed\");\r\n}\r\n\r\nvoid loop()\r\n{\r\n}\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_deepSleep/I2CDev.cpp",
    "content": "/*\r\n * Copyright (c) 2018 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#include \"Arduino.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n#define SerialDebug false  // set to true to get Serial output for debugging\r\n\r\nI2Cdev::I2Cdev(TwoWire* i2c_bus)                                                                                                             // Class constructor\r\n{\r\n  _i2c_bus = i2c_bus;\r\n}\r\n\r\nI2Cdev::~I2Cdev()                                                                                                                            // Class destructor\r\n{\r\n}\r\n\r\n/**\r\n* @fn: readByte(uint8_t address, uint8_t subAddress)\r\n*\r\n* @brief: Read one byte from an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress\r\n* @returns: unsigned short read\r\n*/\r\nuint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)\r\n{\r\n  uint8_t data = 0;                             // `data` will store the register data   \r\n  _i2c_bus->beginTransmission(address);         // Initialize the Tx buffer\r\n  _i2c_bus->write(subAddress);                  // Put slave register address in Tx buffer\r\n  _i2c_bus->endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\r\n  _i2c_bus->requestFrom(address, 1);            // Read one byte from slave register address  \r\n  data = _i2c_bus->read();                      // Fill Rx buffer with result\r\n  return data;                                  // Return data read from slave register\r\n  \r\n}\r\n\r\n\r\n/**\r\n* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\r\n*\r\n* @brief: Read multiple bytes from an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\r\n{  \r\n  _i2c_bus->beginTransmission(address);   // Initialize the Tx buffer\r\n  _i2c_bus->write(subAddress);            // Put slave register address in Tx buffer\r\n  _i2c_bus->endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive\r\n  uint8_t i = 0;\r\n  _i2c_bus->requestFrom(address, count);  // Read bytes from slave register address \r\n  while (_i2c_bus->available()) {\r\n        dest[i++] = _i2c_bus->read(); }   // Put read results in the Rx buffer\r\n}\r\n\r\n\r\n/**\r\n* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n*\r\n* @brief: Write one byte to an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, data to be written\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n{\r\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\r\n  _i2c_bus->write(regAddr);              // Put slave register address in Tx buffer\r\n  _i2c_bus->write(data);                 // Put data in Tx buffer\r\n  _i2c_bus->endTransmission();           // Send the Tx buffer\r\n}\r\n\r\n/**\r\n* @fn: writeReg(uint8_t devAddr, uint8_t regAddr)\r\n*\r\n* @brief: Write register to an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, data to be written\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::writeReg(uint8_t devAddr, uint8_t regAddr)\r\n{\r\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\r\n  _i2c_bus->write(regAddr);              // Put slave register address in Tx buffer\r\n  _i2c_bus->endTransmission();           // Send the Tx buffer\r\n}\r\n\r\n\r\n/**\r\n* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n*\r\n* @brief: Write multiple bytes to an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, byte count, data array to be written\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)\r\n{\r\n  uint8_t temp[1 + count];\r\n  \r\n  temp[0] = regAddr;\r\n  for (uint8_t ii = 0; ii < count; ii++)\r\n  { \r\n    temp[ii + 1] = dest[ii];\r\n  }\r\n  \r\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\r\n  \r\n  for (uint8_t jj = 0; jj < count + 1; jj++)\r\n  {\r\n  _i2c_bus->write(temp[jj]);            // Put data in Tx buffer\r\n  }\r\n  \r\n  _i2c_bus->endTransmission();          // Send the Tx buffer\r\n}\r\n\r\n\r\n\r\n/**\r\n* @fn:I2Cscan()\r\n* @brief: Scan the I2C bus for active I2C slave devices\r\n* \r\n* @params: void\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::I2Cscan() \r\n{\r\n  // Scan for i2c devices\r\n  byte error, address;\r\n  int nDevices;\r\n\r\n  if(SerialDebug) USBSerial.println(\"Scanning...\");\r\n\r\n  nDevices = 0;\r\n  for(address = 1; address < 127; address++ ) \r\n  {\r\n    // The i2c_scanner uses the return value of the Wire.endTransmission to see if a device did acknowledge to the address.\r\n    _i2c_bus->beginTransmission(address);\r\n    error = _i2c_bus->endTransmission();\r\n\r\n    if (error == 0 && SerialDebug)\r\n    {\r\n      USBSerial.print(\"I2C device found at address 0x\");\r\n      if (address<16) \r\n      USBSerial.print(\"0\");\r\n      USBSerial.print(address,HEX);\r\n      USBSerial.println(\"  !\");\r\n      nDevices++;\r\n    }\r\n    else if (error==4 && SerialDebug) \r\n    {\r\n      USBSerial.print(\"Unknown error at address 0x\");\r\n      if (address<16) \r\n        USBSerial.print(\"0\");\r\n        USBSerial.println(address,HEX);\r\n    }    \r\n  }\r\n  if (nDevices == 0 && SerialDebug)\r\n    USBSerial.println(\"No I2C devices found\\n\");\r\n  else\r\n    if(SerialDebug) USBSerial.println(\"I2C scan complete\\n\");\r\n}\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_deepSleep/I2CDev.h",
    "content": "/*\r\n * Copyright (c) 2018 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#ifndef _I2CDEV_H_\r\n#define _I2CDEV_H_\r\n\r\n#include <Wire.h>\r\n\r\nclass I2Cdev {\r\n    public:\r\n                                        I2Cdev(TwoWire*);\r\n                                        ~I2Cdev();                                                                                                                     // Class destructor for durable instances\r\n         uint8_t                        readByte(uint8_t address, uint8_t subAddress);\r\n         void                           readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);\r\n         void                           writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);\r\n         void                           writeReg(uint8_t devAddr, uint8_t regAddr);\r\n         void                           writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);\r\n         void                           I2Cscan();\r\n    private:\r\n         TwoWire*                       _i2c_bus;                                                                                                                      // Class constructor argument\r\n};\r\n\r\n#endif //_I2CDEV_H_\r\n"
  },
  {
    "path": "CCS811_BME280_ESP32C3Mini_deepSleep/Readme.md",
    "content": "Sketch showing how to use the ESP32C3Mini1 as an environmental data logger. \n\nI am using a custom ESP32C3Mini1 development board with native USB for programming (no CP2102).\n\nThe CCS811 air quality sensor and BME280 humidity/pressure/temperature sensors are on a breakout board and are configured in setup once, \ntheir 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.\n\n![image](https://user-images.githubusercontent.com/6698410/155865582-daab5d08-0a00-4984-9684-b989d95954c0.jpg)\n\nThe 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.\n\nI 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.  \n\nOne 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.\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32.ino",
    "content": "#include \"LSM6DSM.h\"\n#include \"LIS2MDL.h\"\n#include \"LPS22HB.h\"\n#include \"USFS.h\"\n\n#define I2C_BUS          Wire                           // Define the I2C bus (Wire instance) you wish to use\n\nI2Cdev                   i2c_0(&I2C_BUS);               // Instantiate the I2Cdev object and point to the desired I2C bus\n\nbool SerialDebug = true;  // set to true to get Serial output for debugging\nbool passThru  = false;\n\n#define myLed 5\n#define pinGND 12\n#define pin3V3 13\n\nvoid EM7180intHandler();\nvoid myinthandler1();\nvoid myinthandler2();\nvoid myinthandler3();\n\n// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)\nfloat pi = 3.141592653589793238462643383279502884f;\nfloat GyroMeasError = pi * (40.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 40 deg/s)\nfloat GyroMeasDrift = pi * (0.0f  / 180.0f);   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)\nfloat beta = sqrtf(3.0f / 4.0f) * GyroMeasError;   // compute beta\nfloat 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\nuint32_t delt_t = 0;                      // used to control display output rate\nuint32_t sumCount = 0;                    // used to control display output rate\nfloat pitch, yaw, roll, Yaw, Pitch, Roll;\nfloat a12, a22, a31, a32, a33;            // rotation matrix coefficients for Euler angles and gravity components\nfloat A112, A22, A31, A32, A33;            // rotation matrix coefficients for Hardware Euler angles and gravity components\nfloat deltat = 0.0f, sum = 0.0f;          // integration interval for both filter schemes\nuint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval\nuint32_t Now = 0;                         // used to calculate integration interval\nfloat lin_ax, lin_ay, lin_az;             // linear acceleration (acceleration with gravity component subtracted)\nfloat lin_Ax, lin_Ay, lin_Az;             // Hardware linear acceleration (acceleration with gravity component subtracted)\nfloat q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion\nfloat Q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // hardware quaternion data register\nfloat eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method\n\n//LSM6DSM definitions\n#define LSM6DSM_intPin1 10  // interrupt1 pin definitions, significant motion\n#define LSM6DSM_intPin2 9   // interrupt2 pin definitions, data ready\n\n/* Specify sensor parameters (sample rate is twice the bandwidth)\n * choices are:\n      AFS_2G, AFS_4G, AFS_8G, AFS_16G  \n      GFS_245DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS \n      AODR_12_5Hz, AODR_26Hz, AODR_52Hz, AODR_104Hz, AODR_208Hz, AODR_416Hz, AODR_833Hz, AODR_1660Hz, AODR_3330Hz, AODR_6660Hz\n      GODR_12_5Hz, GODR_26Hz, GODR_52Hz, GODR_104Hz, GODR_208Hz, GODR_416Hz, GODR_833Hz, GODR_1660Hz, GODR_3330Hz, GODR_6660Hz\n*/ \nuint8_t Ascale = AFS_2G, Gscale = GFS_245DPS, AODR = AODR_208Hz, GODR = GODR_416Hz;\n\nfloat aRes, gRes;              // scale resolutions per LSB for the accel and gyro sensor2\nfloat accelBias[3] = {-0.00499, 0.01540, 0.02902}, gyroBias[3] = {-0.50, 0.14, 0.28}; // offset biases for the accel and gyro\nint16_t LSM6DSMData[7];        // Stores the 16-bit signed sensor output\nfloat   Gtemperature;           // Stores the real internal gyro temperature in degrees Celsius\nfloat ax, ay, az, gx, gy, gz;  // variables to hold latest accel/gyro data values \n\nbool newLSM6DSMData = false;\nbool newLSM6DSMTap  = false;\n\nLSM6DSM LSM6DSM(LSM6DSM_intPin1, LSM6DSM_intPin2, &i2c_0); // instantiate LSM6DSM class\n\n\n//LIS2MDL definitions\n#define LIS2MDL_intPin  8 // interrupt for magnetometer data ready\n\n/* Specify sensor parameters (sample rate is twice the bandwidth)\n * choices are: MODR_10Hz, MOIDR_20Hz, MODR_50 Hz and MODR_100Hz\n*/ \nuint8_t MODR = MODR_100Hz;\n\nfloat mRes = 0.0015f;            // mag sensitivity\nfloat magBias[3] = {0,0,0}, magScale[3]  = {0,0,0}; // Bias corrections for magnetometer\nint16_t LIS2MDLData[4];          // Stores the 16-bit signed sensor output\nfloat Mtemperature;              // Stores the real internal chip temperature in degrees Celsius\nfloat mx, my, mz;                // variables to hold latest mag data values \nuint8_t LIS2MDLstatus;\n\nbool newLIS2MDLData = false;\n\nLIS2MDL LIS2MDL(LIS2MDL_intPin, &i2c_0); // instantiate LIS2MDL class\n\n\n// LPS22H definitions\nuint8_t LPS22H_intPin = 5;\n\n/* Specify sensor parameters (sample rate is twice the bandwidth) \n   Choices are P_1Hz, P_10Hz P_25 Hz, P_50Hz, and P_75Hz\n */\nuint8_t PODR = P_25Hz;     // set pressure amd temperature output data rate\nuint8_t LPS22Hstatus;\nfloat temperature, pressure, altitude;\n\nbool newLPS22HData = false;\n\nLPS22H LPS22H(LPS22H_intPin, &i2c_0);\n\n\nconst uint8_t USFS_intPin = 27;\nbool newEM7180Data = false;\nint16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output\nint16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output\nint16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output\nint16_t tempCount, rawPressure, rawTemperature;            // temperature raw count output\nfloat   Temperature, Pressure, Altitude; //  temperature in degrees Celsius, pressure in mbar\nfloat Ax, Ay, Az, Gx, Gy, Gz, Mx, My, Mz; // variables to hold latest sensor data values\n\n\n/* Choose EM7180, LSM6DSM, LIS2MDL sample rates and bandwidths\n   Choices are:\n   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)\n   QRtDiv 0x00, 0x01, 0x02, etc quat rate = gyroRt/(1 + QRtDiv)\n   magRt 8 Hz = 0x08 or 100 Hz 0x64\n   accRt, gyroRt 1000, 500, 250, 200, 125, 100, 50 Hz enter by choosing desired rate\n   and dividing by 10, so 200 Hz would be 200/10 = 20 = 0x14\n   sample rate of barometer is baroRt/2 so for 25 Hz enter 50 = 0x32\n   LSM6DSM accel/gyro rates 0f 833 Hz set Rt variables to 0x53\n*/\nuint8_t accBW = 0x03, gyroBW = 0x03, QRtDiv = 0x03, magRt = 0x64, accRt = 0x53, gyroRt = 0x53, baroRt = 0x32;\n/*\n   Choose sensor full ranges\n   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\n*/\nuint16_t accFS = 0x02, gyroFS = 0x7D0, magFS = 0x3E8;\n\nUSFS USFS(USFS_intPin, passThru, &i2c_0);\n\n\nvoid setup() {  \n  Serial.begin(115200);\n  delay(4000);\n\n  // Configure led\n  pinMode(myLed, OUTPUT);\n  digitalWrite(myLed, HIGH); // start with led on\n\n   pinMode(pinGND, OUTPUT);\n  digitalWrite(pinGND, LOW);\n\n  pinMode(pin3V3, OUTPUT);\n  digitalWrite(pin3V3, HIGH);\n\n  pinMode(USFS_intPin, INPUT);\n  \n  Wire.begin(16, 15, 400000); //(SDA, SCL) (21,22) are default on ESP32, 400 kHz I2C clock\n  delay(1000);\n \n  i2c_0.I2Cscan(); // which I2C device are on the bus?\n\n  if(!passThru)\n  {\n  // Initialize the USFS\n  USFS.getChipID();        // check ROM/RAM version of EM7180\n  USFS.loadfwfromEEPROM(); // load EM7180 firmware from EEPROM\n  USFS.initEM7180(accBW, gyroBW, accFS, gyroFS, magFS, QRtDiv, magRt, accRt, gyroRt, baroRt); // set MPU and MS5637 sensor parameters\n  } // end of \"if(!passThru)\" handling\n\n  if(passThru)\n  {\n  // Read the LSM6DSM Chip ID register, this is a good test of communication\n  Serial.println(\"LSM6DSM accel/gyro...\");\n  byte c = LSM6DSM.getChipID();  // Read CHIP_ID register for LSM6DSM\n  Serial.print(\"LSM6DSM \"); Serial.print(\"I AM \"); Serial.print(c, HEX); Serial.print(\" I should be \"); Serial.println(0x6A, HEX);\n  Serial.println(\" \");\n  delay(1000); \n\n  // Read the LIS2MDL Chip ID register, this is a good test of communication\n  Serial.println(\"LIS2MDL mag...\");\n  byte d = LIS2MDL.getChipID();  // Read CHIP_ID register for LSM6DSM\n  Serial.print(\"LIS2MDL \"); Serial.print(\"I AM \"); Serial.print(d, HEX); Serial.print(\" I should be \"); Serial.println(0x40, HEX);\n  Serial.println(\" \");\n  delay(1000); \n\n  Serial.println(\"LPS22HB barometer...\");\n  uint8_t e = LPS22H.getChipID();\n  Serial.print(\"LPS25H \"); Serial.print(\"I AM \"); Serial.print(e, HEX); Serial.print(\" I should be \"); Serial.println(0xB1, HEX);\n  delay(1000); \n  \n\n  if(c == 0x6A && d == 0x40 && e == 0xB1) // check if all I2C sensors have acknowledged\n  {\n   Serial.println(\"LSM6DSM and LIS2MDL and LPS22HB are online...\"); Serial.println(\" \");\n   \n   digitalWrite(myLed, LOW);\n\n   LSM6DSM.reset();  // software reset LSM6DSM to default registers\n\n   // get sensor resolutions, only need to do this once\n   aRes = LSM6DSM.getAres(Ascale);\n   gRes = LSM6DSM.getGres(Gscale);\n\n   LSM6DSM.init(Ascale, Gscale, AODR, GODR);\n\n   LSM6DSM.selfTest();\n\n   LSM6DSM.offsetBias(gyroBias, accelBias);\n   Serial.println(\"accel biases (mg)\"); Serial.println(1000.0f * accelBias[0]); Serial.println(1000.0f * accelBias[1]); Serial.println(1000.0f * accelBias[2]);\n   Serial.println(\"gyro biases (dps)\"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);\n   delay(1000); \n\n   LIS2MDL.reset(); // software reset LIS2MDL to default registers\n\n   mRes = 0.0015f;  // fixed sensitivity and full scale (+/- 49.152 Gauss); \n   \n   LIS2MDL.init(MODR);\n\n   LIS2MDL.selfTest();\n\n   LIS2MDL.offsetBias(magBias, magScale);\n   Serial.println(\"mag biases (mG)\"); Serial.println(1000.0f * magBias[0]); Serial.println(1000.0f * magBias[1]); Serial.println(1000.0f * magBias[2]); \n   Serial.println(\"mag scale (mG)\"); Serial.println(magScale[0]); Serial.println(magScale[1]); Serial.println(magScale[2]); \n   delay(2000); // add delay to see results before serial spew of data\n\n   LPS22H.Init(PODR);  // Initialize LPS22H altimeter\n   delay(1000);\n\n   digitalWrite(myLed, HIGH);\n   \n  }\n  else \n  {\n  if(c != 0x6A) Serial.println(\" LSM6DSM not functioning!\");\n  if(d != 0x40) Serial.println(\" LIS2MDL not functioning!\");    \n  if(e != 0xB1) Serial.println(\" LPS22HB not functioning!\");   \n\n  while(1){};\n  }\n  }  // end of \"if(passThru)\" handling\n\n\n  if(!passThru)\n  {\n    attachInterrupt(USFS_intPin, EM7180intHandler, RISING);  // define interrupt for INT pin output of EM7180\n \n    USFS.checkEM7180Status();\n  }\n\n  if(passThru)\n  {\n  attachInterrupt(LSM6DSM_intPin2, myinthandler1, RISING);  // define interrupt for intPin2 output of LSM6DSM\n  attachInterrupt(LIS2MDL_intPin , myinthandler2, RISING);  // define interrupt for intPin  output of LIS2MDL\n  attachInterrupt(LPS22H_intPin  , myinthandler3, RISING);  // define interrupt for intPin  output of LPS22HB\n\n  LIS2MDLstatus = LIS2MDL.status();  // read status register to clear interrupt before main loop\n  }\n\n  digitalWrite(myLed, LOW); // turn led off when successfully through setup\n\n}\n\n/* End of setup */\n\nvoid loop() {\n\n   if(passThru)\n   {\n   // If intPin goes high, either all data registers have new data\n   if(newLSM6DSMData == true) {   // On interrupt, read data\n      newLSM6DSMData = false;     // reset newData flag\n\n     LSM6DSM.readData(LSM6DSMData); // INT2 cleared on any read\n   \n   // Now we'll calculate the accleration value into actual g's\n     ax = (float)LSM6DSMData[4]*aRes - accelBias[0];  // get actual g value, this depends on scale being set\n     ay = (float)LSM6DSMData[5]*aRes - accelBias[1];   \n     az = (float)LSM6DSMData[6]*aRes - accelBias[2];  \n\n   // Calculate the gyro value into actual degrees per second\n     gx = (float)LSM6DSMData[1]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set\n     gy = (float)LSM6DSMData[2]*gRes - gyroBias[1];  \n     gz = (float)LSM6DSMData[3]*gRes - gyroBias[2]; \n\n    for(uint8_t i = 0; i < 10; i++) { // iterate a fixed number of times per data read cycle\n    Now = micros();\n    deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update\n    lastUpdate = Now;\n\n    sum += deltat; // sum for averaging filter update rate\n    sumCount++;\n\n    USFS.MadgwickQuaternionUpdate(-ax, ay, az, gx*pi/180.0f, -gy*pi/180.0f, -gz*pi/180.0f,  mx,  my, -mz);\n    }\n    \n   }\n\n    // If intPin goes high, either all data registers have new data\n   if(newLIS2MDLData == true) {   // On interrupt, read data\n      newLIS2MDLData = false;     // reset newData flag\n\n     LIS2MDLstatus = LIS2MDL.status();\n     \n     if(LIS2MDLstatus & 0x08) // if all axes have new data ready\n     {\n      LIS2MDL.readData(LIS2MDLData);  \n   \n   // Now we'll calculate the accleration value into actual G's\n     mx = (float)LIS2MDLData[0]*mRes - magBias[0];  // get actual G value \n     my = (float)LIS2MDLData[1]*mRes - magBias[1];   \n     mz = (float)LIS2MDLData[2]*mRes - magBias[2]; \n     mx *= magScale[0];\n     my *= magScale[1];\n     mz *= magScale[2];  \n     }\n   }\n   }  // end of \"if(passThru)\" handling\n\n   if(!passThru)\n   {\n      /*EM7180*/\n  // If intpin goes high, all data registers have new data\n  if (newEM7180Data == true) { // On interrupt, read data\n    newEM7180Data = false;  // reset newData flag\n\n    // Check event status register, way to chech data ready by polling rather than interrupt\n    uint8_t eventStatus = USFS.checkEM7180Status(); // reading clears the register\n\n    // Check for errors\n    if (eventStatus & 0x02) { // error detected, what is it?\n\n      uint8_t errorStatus = USFS.checkEM7180Errors();\n      if (errorStatus != 0x00) { // is there an error?\n        Serial.print(\" EM7180 sensor status = \"); Serial.println(errorStatus);\n        if (errorStatus == 0x11) Serial.print(\"Magnetometer failure!\");\n        if (errorStatus == 0x12) Serial.print(\"Accelerometer failure!\");\n        if (errorStatus == 0x14) Serial.print(\"Gyro failure!\");\n        if (errorStatus == 0x21) Serial.print(\"Magnetometer initialization failure!\");\n        if (errorStatus == 0x22) Serial.print(\"Accelerometer initialization failure!\");\n        if (errorStatus == 0x24) Serial.print(\"Gyro initialization failure!\");\n        if (errorStatus == 0x30) Serial.print(\"Math error!\");\n        if (errorStatus == 0x80) Serial.print(\"Invalid sample rate!\");\n      }\n\n      // Handle errors ToDo\n\n    }\n\n    // if no errors, see if new data is ready\n    if (eventStatus & 0x10) { // new acceleration data available\n      USFS.readSENtralAccelData(accelCount);\n\n      // Now we'll calculate the accleration value into actual g's\n      Ax = (float)accelCount[0] * 0.000488f; // get actual g value\n      Ay = (float)accelCount[1] * 0.000488f;\n      Az = (float)accelCount[2] * 0.000488f;\n    }\n\n    if (eventStatus & 0x20) { // new gyro data available\n      USFS.readSENtralGyroData(gyroCount);\n\n      // Now we'll calculate the gyro value into actual dps's\n      Gx = (float)gyroCount[0] * 0.153f; // get actual dps value\n      Gy = (float)gyroCount[1] * 0.153f;\n      Gz = (float)gyroCount[2] * 0.153f;\n    }\n\n    if (eventStatus & 0x08) { // new mag data available\n      USFS.readSENtralMagData(magCount);\n\n      // Now we'll calculate the mag value into actual G's\n      Mx = (float)magCount[0] * 0.305176f; // get actual G value\n      My = (float)magCount[1] * 0.305176f;\n      Mz = (float)magCount[2] * 0.305176f;\n    }\n\n    if (eventStatus & 0x04) { // new quaternion data available\n      USFS.readSENtralQuatData(Q);\n    }\n\n    // get MS5637 pressure\n    if (eventStatus & 0x40) { // new baro data available\n      rawPressure = USFS.readSENtralBaroData();\n      Pressure = (float)rawPressure * 0.01f + 1013.25f; // pressure in mBar\n\n      // get MS5637 temperature\n      rawTemperature = USFS.readSENtralTempData();\n      Temperature = (float) rawTemperature * 0.01f; // temperature in degrees C\n    }\n  } \n   } // end of \"if(!passThru)\" handling\n   \n   // end sensor interrupt handling\n    \n   if(passThru)\n   {\n    if(SerialDebug) {\n    Serial.print(\"ax = \"); Serial.print((int)1000*ax);  \n    Serial.print(\" ay = \"); Serial.print((int)1000*ay); \n    Serial.print(\" az = \"); Serial.print((int)1000*az); Serial.println(\" mg\");\n    Serial.print(\"gx = \"); Serial.print( gx, 2); \n    Serial.print(\" gy = \"); Serial.print( gy, 2); \n    Serial.print(\" gz = \"); Serial.print( gz, 2); Serial.println(\" deg/s\");\n    Serial.print(\"mx = \"); Serial.print((int)1000*mx);  \n    Serial.print(\" my = \"); Serial.print((int)1000*my); \n    Serial.print(\" mz = \"); Serial.print((int)1000*mz); Serial.println(\" mG\");\n    \n    Serial.print(\"q0 = \"); Serial.print(q[0]);\n    Serial.print(\" qx = \"); Serial.print(q[1]); \n    Serial.print(\" qy = \"); Serial.print(q[2]); \n    Serial.print(\" qz = \"); Serial.println(q[3]); \n    }\n\n    // get pressure and temperature from the LPS22HB\n    LPS22Hstatus = LPS22H.status();\n\n    if(LPS22Hstatus & 0x01) { // if new pressure data available\n    pressure = (float) LPS22H.readAltimeterPressure()/4096.0f;\n    temperature = (float) LPS22H.readAltimeterTemperature()/100.0f; \n    \n    altitude = 145366.45f*(1.0f - pow((pressure/1013.25f), 0.190284f)); \n\n      if(SerialDebug) {\n      Serial.print(\"Altimeter temperature = \"); Serial.print( temperature, 2); Serial.println(\" C\"); // temperature in degrees Celsius  \n      Serial.print(\"Altimeter temperature = \"); Serial.print(9.0f*temperature/5.0f + 32.0f, 2); Serial.println(\" F\"); // temperature in degrees Fahrenheit\n      Serial.print(\"Altimeter pressure = \"); Serial.print(pressure, 2);  Serial.println(\" mbar\");// pressure in millibar\n      Serial.print(\"Altitude = \"); Serial.print(altitude, 2); Serial.println(\" feet\");\n      }\n    }\n\n    Gtemperature = ((float) LSM6DSMData[0]) / 256.0f + 25.0f; // Gyro chip temperature in degrees Centigrade\n    // Print temperature in degrees Centigrade      \n    if(SerialDebug) {\n      Serial.print(\"Gyro temperature is \");  Serial.print(Gtemperature, 1);  Serial.println(\" degrees C\"); // Print T values to tenths of s degree C\n    }\n\n    LIS2MDLData[3] = LIS2MDL.readTemperature();\n    Mtemperature = ((float) LIS2MDLData[3]) / 8.0f + 25.0f; // Mag chip temperature in degrees Centigrade\n    // Print temperature in degrees Centigrade      \n    if(SerialDebug) {\n      Serial.print(\"Mag temperature is \");  Serial.print(Mtemperature, 1);  Serial.println(\" degrees C\"); // Print T values to tenths of s degree C\n    }\n\n    a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);\n    a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];\n    a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);\n    a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);\n    a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];\n    pitch = -asinf(a32);\n    roll  = atan2f(a31, a33);\n    yaw   = atan2f(a12, a22);\n    pitch *= 180.0f / pi;\n    yaw   *= 180.0f / pi; \n    yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04\n    if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360\n    roll  *= 180.0f / pi;\n    lin_ax = ax + a31;\n    lin_ay = ay + a32;\n    lin_az = az - a33;\n\n    if(SerialDebug) {\n    Serial.print(\"Yaw, Pitch, Roll: \");\n    Serial.print(yaw, 2);\n    Serial.print(\", \");\n    Serial.print(pitch, 2);\n    Serial.print(\", \");\n    Serial.println(roll, 2);\n\n    Serial.print(\"Grav_x, Grav_y, Grav_z: \");\n    Serial.print(-a31*1000.0f, 2);\n    Serial.print(\", \");\n    Serial.print(-a32*1000.0f, 2);\n    Serial.print(\", \");\n    Serial.print(a33*1000.0f, 2);  Serial.println(\" mg\");\n    Serial.print(\"Lin_ax, Lin_ay, Lin_az: \");\n    Serial.print(lin_ax*1000.0f, 2);\n    Serial.print(\", \");\n    Serial.print(lin_ay*1000.0f, 2);\n    Serial.print(\", \");\n    Serial.print(lin_az*1000.0f, 2);  Serial.println(\" mg\");\n    \n    Serial.print(\"rate = \"); Serial.print((float)sumCount/sum, 2); Serial.println(\" Hz\");\n    }\n\n//     Serial.print(millis()/1000);Serial.print(\",\");\n//     Serial.print(yaw, 2); Serial.print(\",\"); Serial.print(pitch, 2); Serial.print(\",\"); Serial.print(roll, 2); Serial.print(\",\"); Serial.println(Pressure, 2);\n\n    sumCount = 0;\n    sum = 0;      \n\n    }  // end of \"if(passThru)\" handling\n\n    if(!passThru)\n    {\n\n      if (SerialDebug) {\n      Serial.print(\"Ax = \"); Serial.print((int)1000 * Ax);\n      Serial.print(\" Ay = \"); Serial.print((int)1000 * Ay);\n      Serial.print(\" Az = \"); Serial.print((int)1000 * Az); Serial.println(\" mg\");\n      Serial.print(\"Gx = \"); Serial.print( Gx, 2);\n      Serial.print(\" Gy = \"); Serial.print( Gy, 2);\n      Serial.print(\" Gz = \"); Serial.print( Gz, 2); Serial.println(\" deg/s\");\n      Serial.print(\"Mx = \"); Serial.print( (int)Mx);\n      Serial.print(\" My = \"); Serial.print( (int)My);\n      Serial.print(\" Mz = \"); Serial.print( (int)Mz); Serial.println(\" mG\");\n\n      Serial.println(\"Hardware quaternions:\");\n      Serial.print(\"Q0 = \"); Serial.print(Q[0]);\n      Serial.print(\" Qx = \"); Serial.print(Q[1]);\n      Serial.print(\" Qy = \"); Serial.print(Q[2]);\n      Serial.print(\" Qz = \"); Serial.println(Q[3]);\n    }\n\n    //Hardware AHRS:\n    A112 =   2.0f * (Q[1] * Q[2] + Q[0] * Q[3]);\n    A22 =   Q[0] * Q[0] + Q[1] * Q[1] - Q[2] * Q[2] - Q[3] * Q[3];\n    A31 =   2.0f * (Q[0] * Q[1] + Q[2] * Q[3]);\n    A32 =   2.0f * (Q[1] * Q[3] - Q[0] * Q[2]);\n    A33 =   Q[0] * Q[0] - Q[1] * Q[1] - Q[2] * Q[2] + Q[3] * Q[3];\n    Pitch = -asinf(A32);\n    Roll  = atan2f(A31, A33);\n    Yaw   = atan2f(A112, A22);\n    Pitch *= 180.0f / pi;\n    Yaw   *= 180.0f / pi;\n    Yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04\n    if (Yaw < 0) Yaw   += 360.0f ; // Ensure yaw stays between 0 and 360\n    Roll  *= 180.0f / pi;\n    lin_Ax = Ax + A31;\n    lin_Ay = Ay + A32;\n    lin_Az = Az - A33;\n\n    if (SerialDebug) {\n      Serial.print(\"Hardware Yaw, pitch, Roll: \");\n      Serial.print(Yaw, 2);\n      Serial.print(\", \");\n      Serial.print(Pitch, 2);\n      Serial.print(\", \");\n      Serial.println(Roll, 2);\n\n      Serial.print(\"Hardware Grav_x, Grav_y, Grav_z: \");\n      Serial.print(-A31 * 1000, 2);\n      Serial.print(\", \");\n      Serial.print(-A32 * 1000, 2);\n      Serial.print(\", \");\n      Serial.print(A33 * 1000, 2);  Serial.println(\" mg\");\n      Serial.print(\"Hardware Lin_ax, Lin_ay, Lin_az: \");\n      Serial.print(lin_Ax * 1000, 2);\n      Serial.print(\", \");\n      Serial.print(lin_Ay * 1000, 2);\n      Serial.print(\", \");\n      Serial.print(lin_Az * 1000, 2);  Serial.println(\" mg\");\n\n      Serial.println(\"MS5637:\");\n      Serial.print(\"Altimeter temperature = \");\n      Serial.print(Temperature, 2);\n      Serial.println(\" C\"); // temperature in degrees Celsius\n      Serial.print(\"Altimeter temperature = \");\n      Serial.print(9.0f * Temperature / 5.0f + 32.0f, 2);\n      Serial.println(\" F\"); // temperature in degrees Fahrenheit\n      Serial.print(\"Altimeter pressure = \");\n      Serial.print(Pressure, 2);\n      Serial.println(\" mbar\");// pressure in millibar\n      Altitude = 145366.45f * (1.0f - powf(((Pressure) / 1013.25f), 0.190284f));\n      Serial.print(\"Altitude = \");\n      Serial.print(Altitude, 2);\n      Serial.println(\" feet\");\n      Serial.println(\" \");\n    }\n    \n    } // end of \"if(!passThru)\" handling\n   \n      digitalWrite(myLed, HIGH); delay(1); digitalWrite(myLed, LOW);  // flash led for 10 milliseconds\n      delay(500);\n \n}  //end of loop\n\n/*  End of main loop */\n\n\nvoid myinthandler1()\n{\n  newLSM6DSMData = true;\n}\n\nvoid myinthandler2()\n{\n  newLIS2MDLData = true;\n}\n\nvoid myinthandler3()\n{\n  newLPS22HData = true;\n}\n\nvoid EM7180intHandler()\n{\n  newEM7180Data = true;\n}\n\n\n\n\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/I2Cdev.cpp",
    "content": "/*\n * Copyright (c) 2018 Tlera Corp.  All rights reserved.\n *\n * Permission is hereby granted, free of charge, to any person obtaining a copy\n * of this software and associated documentation files (the \"Software\"), to\n * deal with the Software without restriction, including without limitation the\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\n * sell copies of the Software, and to permit persons to whom the Software is\n * furnished to do so, subject to the following conditions:\n *\n *  1. Redistributions of source code must retain the above copyright notice,\n *     this list of conditions and the following disclaimers.\n *  2. Redistributions in binary form must reproduce the above copyright\n *     notice, this list of conditions and the following disclaimers in the\n *     documentation and/or other materials provided with the distribution.\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\n *     may be used to endorse or promote products derived from this Software\n *     without specific prior written permission.\n *\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\n * WITH THE SOFTWARE.\n */\n\n#include \"Arduino.h\"\n#include \"I2Cdev.h\"\n\nI2Cdev::I2Cdev(TwoWire* i2c_bus)                                                                                                             // Class constructor\n{\n  _i2c_bus = i2c_bus;\n}\n\nI2Cdev::~I2Cdev()                                                                                                                            // Class destructor\n{\n}\n\n/**\n* @fn: readByte(uint8_t address, uint8_t subAddress)\n*\n* @brief: Read one byte from an I2C device\n* \n* @params: I2C slave device address, Register subAddress\n* @returns: unsigned short read\n*/\nuint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)\n{\n  uint8_t data = 0;                             // `data` will store the register data   \n  _i2c_bus->beginTransmission(address);         // Initialize the Tx buffer\n  _i2c_bus->write(subAddress);                  // Put slave register address in Tx buffer\n  _i2c_bus->endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n  _i2c_bus->requestFrom(address, 1);            // Read one byte from slave register address  \n  data = _i2c_bus->read();                      // Fill Rx buffer with result\n  return data;                                  // Return data read from slave register\n  \n}\n\n\n/**\n* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\n*\n* @brief: Read multiple bytes from an I2C device\n* \n* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data\n* @returns: void\n*/\nvoid I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\n{  \n  _i2c_bus->beginTransmission(address);   // Initialize the Tx buffer\n  _i2c_bus->write(subAddress);            // Put slave register address in Tx buffer\n  _i2c_bus->endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive\n  uint8_t i = 0;\n  _i2c_bus->requestFrom(address, count);  // Read bytes from slave register address \n  while (_i2c_bus->available()) {\n        dest[i++] = _i2c_bus->read(); }   // Put read results in the Rx buffer\n}\n\n\n/**\n* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)\n*\n* @brief: Write one byte to an I2C device\n* \n* @params: I2C slave device address, Register subAddress, data to be written\n* @returns: void\n*/\nvoid I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)\n{\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\n  _i2c_bus->write(regAddr);           // Put slave register address in Tx buffer\n  _i2c_bus->write(data);                 // Put data in Tx buffer\n  _i2c_bus->endTransmission();           // Send the Tx buffer\n}\n\n\n/**\n* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)\n*\n* @brief: Write multiple bytes to an I2C device\n* \n* @params: I2C slave device address, Register subAddress, byte count, data array to be written\n* @returns: void\n*/\nvoid I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)\n{\n  uint8_t temp[1 + count];\n  \n  temp[0] = regAddr;\n  for (uint8_t ii = 0; ii < count; ii++)\n  { \n    temp[ii + 1] = dest[ii];\n  }\n  \n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\n  \n  for (uint8_t jj = 0; jj < count + 1; jj++)\n  {\n  _i2c_bus->write(temp[jj]);            // Put data in Tx buffer\n  }\n  \n  _i2c_bus->endTransmission();           // Send the Tx buffer\n}\n\n\n\n/**\n* @fn:I2Cscan()\n* @brief: Scan the I2C bus for active I2C slave devices\n* \n* @params: void\n* @returns: void\n*/\nvoid I2Cdev::I2Cscan() \n{\n  // Scan for i2c devices\n  byte error, address;\n  int nDevices;\n\n  Serial.println(\"Scanning...\");\n\n  nDevices = 0;\n  for(address = 1; address < 127; address++ ) \n  {\n    // The i2c_scanner uses the return value of the Wire.endTransmisstion to see if a device did acknowledge to the address.\n    _i2c_bus->beginTransmission(address);\n    error = _i2c_bus->endTransmission();\n\n    if (error == 0)\n    {\n      Serial.print(\"I2C device found at address 0x\");\n      if (address<16) \n      Serial.print(\"0\");\n      Serial.print(address,HEX);\n      Serial.println(\"  !\");\n      nDevices++;\n    }\n    else if (error==4) \n    {\n      Serial.print(\"Unknown error at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.println(address,HEX);\n    }    \n  }\n  if (nDevices == 0)\n    Serial.println(\"No I2C devices found\\n\");\n  else\n    Serial.println(\"I2C scan complete\\n\");\n}\n\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/I2Cdev.h",
    "content": "/*\n * Copyright (c) 2018 Tlera Corp.  All rights reserved.\n *\n * Permission is hereby granted, free of charge, to any person obtaining a copy\n * of this software and associated documentation files (the \"Software\"), to\n * deal with the Software without restriction, including without limitation the\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\n * sell copies of the Software, and to permit persons to whom the Software is\n * furnished to do so, subject to the following conditions:\n *\n *  1. Redistributions of source code must retain the above copyright notice,\n *     this list of conditions and the following disclaimers.\n *  2. Redistributions in binary form must reproduce the above copyright\n *     notice, this list of conditions and the following disclaimers in the\n *     documentation and/or other materials provided with the distribution.\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\n *     may be used to endorse or promote products derived from this Software\n *     without specific prior written permission.\n *\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\n * WITH THE SOFTWARE.\n */\n\n#ifndef _I2CDEV_H_\n#define _I2CDEV_H_\n\n#include <Wire.h>\n\nclass I2Cdev {\n    public:\n                                        I2Cdev(TwoWire*);\n                                        ~I2Cdev();                                                                                                                     // Class destructor for durable instances\n         uint8_t                        readByte(uint8_t address, uint8_t subAddress);\n         void                           readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);\n         void                           writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);\n         void                           writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);\n         void                           I2Cscan();\n         \n    private:\n         TwoWire*                       _i2c_bus;                                                                                                                      // Class constructor argument\n};\n\n#endif //_I2CDEV_H_\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LIS2MDL.cpp",
    "content": "/* 09/23/2017 Copyright Tlera Corporation\n\n    Created by Kris Winer\n\n  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.\n  The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution.\n\n  Library may be used freely and without limit with attribution.\n\n*/\n\n#include \"LIS2MDL.h\"\n\nLIS2MDL::LIS2MDL(uint8_t intPin, I2Cdev* i2c_bus)\n{\n  _intPin = intPin;\n  _i2c_bus = i2c_bus;\n}\n\n\nuint8_t LIS2MDL::getChipID()\n{\n  uint8_t c = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);\n  return c;\n}\n\n\nvoid LIS2MDL::reset()\n{\n  // reset device\n  uint8_t temp = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);\n  _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL\n  delay(1);\n  _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL\n  delay(100); // Wait for all registers to reset \n}\n\nvoid LIS2MDL::init(uint8_t MODR)\n{\n // enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)\n _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR<<2);  \n\n // enable low pass filter (bit 0 == 1), set to ODR/4\n _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);  \n\n // enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)\n _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);  \n}\n\n\nuint8_t LIS2MDL::status()\n{\n  // Read the status register of the altimeter  \n  uint8_t temp = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG);   \n  return temp;\n}\n\n\nvoid LIS2MDL::readData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z mag register data stored here\n  _i2c_bus->readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8, &rawData[0]);  // Read the 6 raw data registers into data array\n\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; \n}\n\n\nint16_t LIS2MDL::readTemperature()\n{\n  uint8_t rawData[2];  // x/y/z mag register data stored here\n  _i2c_bus->readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2, &rawData[0]);  // Read the 8 raw data registers into data array\n  int16_t temp = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value\n  return temp;\n}\n\n\nvoid LIS2MDL::offsetBias(float * dest1, float * dest2)\n{\n  int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};\n  int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};\n  float _mRes = 0.0015f;\n  \n  Serial.println(\"Calculate mag offset bias: move all around to sample the complete response surface!\");\n  delay(4000);\n\n  for (int ii = 0; ii < 4000; ii++)\n  {\n    readData(mag_temp);\n       for (int jj = 0; jj < 3; jj++) {\n      if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];\n      if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];\n    }\n    delay(12);\n  }\n\n  _mRes = 0.0015f; // fixed sensitivity\n    // Get hard iron correction\n    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts\n    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts\n    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts\n    \n    dest1[0] = (float) mag_bias[0] * _mRes;  // save mag biases in G for main program\n    dest1[1] = (float) mag_bias[1] * _mRes;   \n    dest1[2] = (float) mag_bias[2] * _mRes;  \n       \n    // Get soft iron correction estimate\n    mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts\n    mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts\n    mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts\n\n    float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];\n    avg_rad /= 3.0f;\n\n    dest2[0] = avg_rad/((float)mag_scale[0]);\n    dest2[1] = avg_rad/((float)mag_scale[1]);\n    dest2[2] = avg_rad/((float)mag_scale[2]);\n  \n   Serial.println(\"Mag Calibration done!\");\n}\n\nvoid LIS2MDL::selfTest()\n{\n  int16_t temp[3] = {0, 0, 0};\n  float magTest[3] = {0., 0., 0.};\n  float magNom[3] = {0., 0., 0.};\n  int32_t sum[3] = {0, 0, 0};\n  float _mRes = 0.0015f;\n    \n  // first, get average response with self test disabled\n  for (int ii = 0; ii < 50; ii++)\n  {\n    readData(temp);\n    sum[0] += temp[0];\n    sum[1] += temp[1];\n    sum[2] += temp[2];\n    delay(50);\n  }\n  \n  magNom[0] = (float) sum[0] / 50.0f;\n  magNom[1] = (float) sum[1] / 50.0f;\n  magNom[2] = (float) sum[2] / 50.0f;\n  \n  uint8_t c = _i2c_bus->readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);\n  _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test\n  delay(100); // let mag respond\n  \n  sum[0] = 0;\n  sum[1] = 0;\n  sum[2] = 0;\n  for (int ii = 0; ii < 50; ii++)\n  {\n    readData(temp);\n    sum[0] += temp[0];\n    sum[1] += temp[1];\n    sum[2] += temp[2];\n    delay(50);\n  }\n  \n  magTest[0] = (float) sum[0] / 50.0f;\n  magTest[1] = (float) sum[1] / 50.0f;\n  magTest[2] = (float) sum[2] / 50.0f;\n  \n  _i2c_bus->writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode\n  delay(100); // let mag respond\n\n  Serial.println(\"Mag Self Test:\");\n  Serial.print(\"Mx results:\"); Serial.print(  (magTest[0] - magNom[0]) * _mRes * 1000.0); Serial.println(\" mG\");\n  Serial.print(\"My results:\"); Serial.println((magTest[0] - magNom[0]) * _mRes * 1000.0);\n  Serial.print(\"Mz results:\"); Serial.println((magTest[1] - magNom[1]) * _mRes * 1000.0);\n  Serial.println(\"Should be between 15 and 500 mG\");\n  delay(2000);  // give some time to read the screen\n}\n\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LIS2MDL.h",
    "content": "/* 09/23/2017 Copyright Tlera Corporation\r\n\r\n    Created by Kris Winer\r\n\r\n  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.\r\n  The LIS2MDL is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution.\r\n\r\n  Library may be used freely and without limit with attribution.\r\n\r\n*/\r\n\r\n#ifndef LIS2MDL_h\r\n#define LIS2MDL_h\r\n\r\n#include \"Arduino.h\"\r\n#include <Wire.h>\r\n#include \"I2Cdev.h\"\r\n\r\n//Register map for LIS2MDL'\r\n// 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\r\n#define LIS2MDL_OFFSET_X_REG_L        0x45\r\n#define LIS2MDL_OFFSET_X_REG_H        0x46\r\n#define LIS2MDL_OFFSET_Y_REG_L        0x47\r\n#define LIS2MDL_OFFSET_Y_REG_H        0x48\r\n#define LIS2MDL_OFFSET_Z_REG_L        0x49\r\n#define LIS2MDL_OFFSET_Z_REG_H        0x4A\r\n#define LIS2MDL_WHO_AM_I              0x4F\r\n#define LIS2MDL_CFG_REG_A             0x60\r\n#define LIS2MDL_CFG_REG_B             0x61\r\n#define LIS2MDL_CFG_REG_C             0x62\r\n#define LIS2MDL_INT_CTRL_REG          0x63\r\n#define LIS2MDL_INT_SOURCE_REG        0x64\r\n#define LIS2MDL_INT_THS_L_REG         0x65\r\n#define LIS2MDL_INT_THS_H_REG         0x66\r\n#define LIS2MDL_STATUS_REG            0x67\r\n#define LIS2MDL_OUTX_L_REG            0x68\r\n#define LIS2MDL_OUTX_H_REG            0x69\r\n#define LIS2MDL_OUTY_L_REG            0x6A\r\n#define LIS2MDL_OUTY_H_REG            0x6B\r\n#define LIS2MDL_OUTZ_L_REG            0x6C\r\n#define LIS2MDL_OUTZ_H_REG            0x6D\r\n#define LIS2MDL_TEMP_OUT_L_REG        0x6E\r\n#define LIS2MDL_TEMP_OUT_H_REG        0x6F\r\n\r\n#define LIS2MDL_ADDRESS               0x1E\r\n\r\n#define MODR_10Hz   0x00\r\n#define MODR_20Hz   0x01\r\n#define MODR_50Hz   0x02\r\n#define MODR_100Hz  0x03\r\n\r\n\r\nclass LIS2MDL\r\n{\r\n  public:\r\n  LIS2MDL(uint8_t intPin,   I2Cdev* i2c_bus);\r\n  uint8_t getChipID();\r\n  void init(uint8_t MODR);\r\n  void offsetBias(float * dest1, float * dest2);\r\n  void reset();\r\n  void selfTest();\r\n  uint8_t status();\r\n  void readData(int16_t * destination);\r\n  int16_t readTemperature();\r\n  void I2Cscan();\r\n  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);\r\n  uint8_t readByte(uint8_t address, uint8_t subAddress);\r\n  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);\r\n  private:\r\n  uint8_t _intPin;\r\n  float _mRes;\r\n  I2Cdev* _i2c_bus;\r\n};\r\n\r\n#endif\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LPS22HB.cpp",
    "content": "/* 09/23/2017 Copyright Tlera Corporation\n\n    Created by Kris Winer\n\n  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.\n  The LPS22HB is a low power barometerr.\n\n  Library may be used freely and without limit with attribution.\n\n*/\n\n#include \"LPS22HB.h\"\n#include \"Wire.h\"\n\nLPS22H::LPS22H(uint8_t intPin, I2Cdev* i2c_bus)\n{\n  _intPin = intPin;\n  _i2c_bus = i2c_bus; \n}\n\nuint8_t LPS22H::getChipID()\n{\n  // Read the WHO_AM_I register of the altimeter this is a good test of communication\n  uint8_t temp = _i2c_bus->readByte(LPS22H_ADDRESS, LPS22H_WHOAMI);  // Read WHO_AM_I register for LPS22H\n  return temp;\n}\n\nuint8_t LPS22H::status()\n{\n  // Read the status register of the altimeter  \n  uint8_t temp = _i2c_bus->readByte(LPS22H_ADDRESS, LPS22H_STATUS);   \n  return temp;\n}\n\nint32_t LPS22H::readAltimeterPressure()\n{\n    uint8_t rawData[3];  // 24-bit pressure register data stored here\n    _i2c_bus->readBytes(LPS22H_ADDRESS, (LPS22H_PRESS_OUT_XL | 0x80), 3, &rawData[0]); // bit 7 must be one to read multiple bytes\n    return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8 | rawData[0]);\n}\n\nint16_t LPS22H::readAltimeterTemperature()\n{\n    uint8_t rawData[2];  // 16-bit pressure register data stored here\n    _i2c_bus->readBytes(LPS22H_ADDRESS, (LPS22H_TEMP_OUT_L | 0x80), 2, &rawData[0]); // bit 7 must be one to read multiple bytes\n    return (int16_t)((int16_t) rawData[1] << 8 | rawData[0]);\n}\n\n\nvoid LPS22H::Init(uint8_t PODR)\n{\n  // set sample rate by setting bits 6:4 \n  // enable low-pass filter by setting bit 3 to one\n  // bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20\n  // make sure data not updated during read by setting block data udate (bit 1) to 1\n    _i2c_bus->writeByte(LPS22H_ADDRESS, LPS22H_CTRL_REG1, PODR << 4 | 0x08 | 0x02);  \n    _i2c_bus->writeByte(LPS22H_ADDRESS, LPS22H_CTRL_REG3, 0x04);  // enable data ready as interrupt source\n}\n\n\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LPS22HB.h",
    "content": "/* 09/23/2017 Copyright Tlera Corporation\n\n    Created by Kris Winer\n\n  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.\n  The LPS22HB is a low power barometerr.\n\n  Library may be used freely and without limit with attribution.\n\n*/\n\n#ifndef LPS22HB_h\n#define LPS22HB_h\n\n#include \"Arduino.h\"\n#include \"Wire.h\"\n#include \"I2Cdev.h\"\n\n// See LPS22H \"MEMS pressure sensor: 260-1260 hPa absolute digital output barometer\" Data Sheet\n// 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\n#define LPS22H_INTERRUPT_CFG 0x0B\n#define LPS22H_THS_P_L       0x0C\n#define LPS22H_THS_P_H       0x0D\n#define LPS22H_WHOAMI        0x0F // should return 0xB1\n#define LPS22H_CTRL_REG1     0x10\n#define LPS22H_CTRL_REG2     0x11\n#define LPS22H_CTRL_REG3     0x12\n#define LPS22H_FIFO_CTRL     0x14\n#define LPS22H_REF_P_XL      0x15\n#define LPS22H_REF_P_L       0x16\n#define LPS22H_REF_P_H       0x17\n#define LPS22H_RPDS_L        0x18\n#define LPS22H_RPDS_H        0x19\n#define LPS22H_RES_CONF      0x1A\n#define LPS22H_INT_SOURCE    0x25\n#define LPS22H_FIFO_STATUS   0x26\n#define LPS22H_STATUS        0x27\n#define LPS22H_PRESS_OUT_XL  0x28\n#define LPS22H_PRESS_OUT_L   0x29\n#define LPS22H_PRESS_OUT_H   0x2A\n#define LPS22H_TEMP_OUT_L    0x2B\n#define LPS22H_TEMP_OUT_H    0x2C\n#define LPS22H_LPFP_RES      0x33\n\n#define LPS22H_ADDRESS 0x5C   // Address of altimeter\n\n// Altimeter output data rate\n#define    P_1shot  0x00;\n#define    P_1Hz    0x01;\n#define    P_10Hz   0x02;\n#define    P_25Hz   0x03;  // 25 Hz output data rate\n#define    P_50Hz   0x04;\n#define    P_75Hz   0x05;\n\nclass LPS22H\n{\n  public: \n  LPS22H(uint8_t intPin, I2Cdev* i2c_bus);\n  void Init(uint8_t PODR);\n  uint8_t getChipID();\n  uint8_t status();\n  int32_t readAltimeterPressure();\n  int16_t readAltimeterTemperature();\n  void I2Cscan();\n  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);\n  uint8_t readByte(uint8_t address, uint8_t subAddress);\n  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);\n  private:\n  uint8_t   _intPin;\n  I2Cdev* _i2c_bus;\n};\n\n#endif\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LSM6DSM.cpp",
    "content": "/* 09/23/2017 Copyright Tlera Corporation\n\n    Created by Kris Winer\n\n  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.\n  The LSM6DSM is a sensor hub with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution.\n\n  Library may be used freely and without limit with attribution.\n\n*/\n\n#include \"LSM6DSM.h\"\n\nLSM6DSM::LSM6DSM(uint8_t intPin1, uint8_t intPin2, I2Cdev* i2c_bus)\n{\n  _intPin1 = intPin1;\n  _intPin2 = intPin2;  \n  _i2c_bus = i2c_bus; \n}\n\n\nuint8_t LSM6DSM::getChipID()\n{\n  uint8_t c = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_WHO_AM_I);\n  return c;\n}\n\nfloat LSM6DSM::getAres(uint8_t Ascale) {\n  switch (Ascale)\n  {\n  // Possible accelerometer scales (and their register bit settings) are:\n  // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11). \n        // Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that 2-bit value:\n    case AFS_2G:\n         _aRes = 2.0f/32768.0f;\n         return _aRes;\n         break;\n    case AFS_4G:\n         _aRes = 4.0f/32768.0f;\n         return _aRes;\n         break;\n    case AFS_8G:\n         _aRes = 8.0f/32768.0f;\n         return _aRes;\n         break;\n    case AFS_16G:\n         _aRes = 16.0f/32768.0f;\n         return _aRes;\n         break;\n  }\n}\n\nfloat LSM6DSM::getGres(uint8_t Gscale) {\n  switch (Gscale)\n  {\n  // Possible gyro scales (and their register bit settings) are:\n  // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11). \n    case GFS_245DPS:\n          _gRes = 245.0f/32768.0f;\n          return _gRes;\n          break;\n    case GFS_500DPS:\n          _gRes = 500.0f/32768.0f;\n          return _gRes;\n          break;\n    case GFS_1000DPS:\n         _gRes = 1000.0f/32768.0f;\n         return _gRes;\n         break;\n    case GFS_2000DPS:\n          _gRes = 2000.0f/32768.0f;\n         return _gRes;\n         break;\n  }\n}\n\n\nvoid LSM6DSM::reset()\n{\n  // reset device\n  uint8_t temp = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);\n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x01); // Set bit 0 to 1 to reset LSM6DSM\n  delay(100); // Wait for all registers to reset \n}\n\n\nvoid LSM6DSM::init(uint8_t Ascale, uint8_t Gscale, uint8_t AODR, uint8_t GODR)\n{\n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL1_XL, AODR << 4 | Ascale << 2);\n  \n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL2_G, GODR << 4 | Gscale << 2);\n \n  uint8_t temp = _i2c_bus->readByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C);\n  // enable block update (bit 6 = 1), auto-increment registers (bit 2 = 1)\n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL3_C, temp | 0x40 | 0x04); \n  // by default, interrupts active HIGH, push pull, little endian data \n  // (can be changed by writing to bits 5, 4, and 1, resp to above register)\n\n   // enable accel LP2 (bit 7 = 1), set LP2 tp ODR/9 (bit 6 = 1), enable input_composite (bit 3) for low noise\n   _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL8_XL, 0x80 | 0x40 | 0x08 );\n\n   // interrupt handling\n    _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_DRDY_PULSE_CFG, 0x80); // latch interrupt until data read\n    _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT1_CTRL, 0x40);      // enable significant motion interrupts on INT1\n    _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_INT2_CTRL, 0x03);      // enable accel/gyro data ready interrupts on INT2  \n}\n\n\nvoid LSM6DSM::selfTest()\n{\n  int16_t temp[7] = {0, 0, 0, 0, 0, 0, 0};\n  int16_t accelPTest[3] = {0, 0, 0}, accelNTest[3] = {0, 0, 0}, gyroPTest[3] = {0, 0, 0}, gyroNTest[3] = {0, 0, 0};\n  int16_t accelNom[3] = {0, 0, 0}, gyroNom[3] = {0, 0, 0};\n\n  readData(temp);\n  accelNom[0] = temp[4];\n  accelNom[1] = temp[5];\n  accelNom[2] = temp[6];\n  gyroNom[0]  = temp[1];\n  gyroNom[1]  = temp[2];\n  gyroNom[2]  = temp[3];\n  \n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x01); // positive accel self test\n  delay(100); // let accel respond\n  readData(temp);\n  accelPTest[0] = temp[4];\n  accelPTest[1] = temp[5];\n  accelPTest[2] = temp[6];\n\n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x03); // negative accel self test\n  delay(100); // let accel respond\n  readData(temp);\n  accelNTest[0] = temp[4];\n  accelNTest[1] = temp[5];\n  accelNTest[2] = temp[6];\n\n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x04); // positive gyro self test\n  delay(100); // let gyro respond\n  readData(temp);\n  gyroPTest[0] = temp[1];\n  gyroPTest[1] = temp[2];\n  gyroPTest[2] = temp[3];\n\n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x0C); // negative gyro self test\n  delay(100); // let gyro respond\n  readData(temp);\n  gyroNTest[0] = temp[1];\n  gyroNTest[1] = temp[2];\n  gyroNTest[2] = temp[3];\n\n  _i2c_bus->writeByte(LSM6DSM_ADDRESS, LSM6DSM_CTRL5_C, 0x00); // normal mode\n  delay(100); // let accel and gyro respond\n\n  Serial.println(\"Accel Self Test:\");\n  Serial.print(\"+Ax results:\"); Serial.print(  (accelPTest[0] - accelNom[0]) * _aRes * 1000.0); Serial.println(\" mg\");\n  Serial.print(\"-Ax results:\"); Serial.println((accelNTest[0] - accelNom[0]) * _aRes * 1000.0);\n  Serial.print(\"+Ay results:\"); Serial.println((accelPTest[1] - accelNom[1]) * _aRes * 1000.0);\n  Serial.print(\"-Ay results:\"); Serial.println((accelNTest[1] - accelNom[1]) * _aRes * 1000.0);\n  Serial.print(\"+Az results:\"); Serial.println((accelPTest[2] - accelNom[2]) * _aRes * 1000.0);\n  Serial.print(\"-Az results:\"); Serial.println((accelNTest[2] - accelNom[2]) * _aRes * 1000.0);\n  Serial.println(\"Should be between 90 and 1700 mg\");\n\n  Serial.println(\"Gyro Self Test:\");\n  Serial.print(\"+Gx results:\"); Serial.print((gyroPTest[0] - gyroNom[0]) * _gRes); Serial.println(\" dps\");\n  Serial.print(\"-Gx results:\"); Serial.println((gyroNTest[0] - gyroNom[0]) * _gRes);\n  Serial.print(\"+Gy results:\"); Serial.println((gyroPTest[1] - gyroNom[1]) * _gRes);\n  Serial.print(\"-Gy results:\"); Serial.println((gyroNTest[1] - gyroNom[1]) * _gRes);\n  Serial.print(\"+Gz results:\"); Serial.println((gyroPTest[2] - gyroNom[2]) * _gRes);\n  Serial.print(\"-Gz results:\"); Serial.println((gyroNTest[2] - gyroNom[2]) * _gRes);\n  Serial.println(\"Should be between 20 and 80 dps\");\n  delay(2000);\n\n \n}\n\n\nvoid LSM6DSM::offsetBias(float * dest1, float * dest2)\n{\n  int16_t temp[7] = {0, 0, 0, 0, 0, 0, 0};\n  int32_t sum[7] = {0, 0, 0, 0, 0, 0, 0};\n    \n  Serial.println(\"Calculate accel and gyro offset biases: keep sensor flat and motionless!\");\n  delay(4000);\n\n  for (int ii = 0; ii < 128; ii++)\n  {\n    readData(temp);\n    sum[1] += temp[1];\n    sum[2] += temp[2];\n    sum[3] += temp[3];\n    sum[4] += temp[4];\n    sum[5] += temp[5];\n    sum[6] += temp[6];\n    delay(50);\n  }\n\n  dest1[0] = sum[1]*_gRes/128.0f;\n  dest1[1] = sum[2]*_gRes/128.0f;\n  dest1[2] = sum[3]*_gRes/128.0f;\n  dest2[0] = sum[4]*_aRes/128.0f;\n  dest2[1] = sum[5]*_aRes/128.0f;\n  dest2[2] = sum[6]*_aRes/128.0f;\n\n  if(dest2[0] > 0.8f)  {dest2[0] -= 1.0f;}  // Remove gravity from the x-axis accelerometer bias calculation\n  if(dest2[0] < -0.8f) {dest2[0] += 1.0f;}  // Remove gravity from the x-axis accelerometer bias calculation\n  if(dest2[1] > 0.8f)  {dest2[1] -= 1.0f;}  // Remove gravity from the y-axis accelerometer bias calculation\n  if(dest2[1] < -0.8f) {dest2[1] += 1.0f;}  // Remove gravity from the y-axis accelerometer bias calculation\n  if(dest2[2] > 0.8f)  {dest2[2] -= 1.0f;}  // Remove gravity from the z-axis accelerometer bias calculation\n  if(dest2[2] < -0.8f) {dest2[2] += 1.0f;}  // Remove gravity from the z-axis accelerometer bias calculation\n\n}\n\n\nvoid LSM6DSM::readData(int16_t * destination)\n{\n  uint8_t rawData[14];  // x/y/z accel register data stored here\n  _i2c_bus->readBytes(LSM6DSM_ADDRESS, LSM6DSM_OUT_TEMP_L, 14, &rawData[0]);  // Read the 14 raw data registers into data array\n  destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  \n  destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; \n  destination[3] = ((int16_t)rawData[7] << 8) | rawData[6] ;   \n  destination[4] = ((int16_t)rawData[9] << 8) | rawData[8] ;  \n  destination[5] = ((int16_t)rawData[11] << 8) | rawData[10] ;  \n  destination[6] = ((int16_t)rawData[13] << 8) | rawData[12] ; \n}\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/LSM6DSM.h",
    "content": "/* 09/23/2017 Copyright Tlera Corporation\n\n    Created by Kris Winer\n\n  This sketch uses SDA/SCL on pins 21/20 (Butterfly default), respectively, and it uses the Butterfly STM32L433CU Breakout Board.\n  The LSM6DSM is a sensor hub with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution.\n\n  Library may be used freely and without limit with attribution.\n\n*/\n\n#ifndef LSM6DSM_h\n#define LSM6DSM_h\n\n#include \"Arduino.h\"\n#include <Wire.h>\n#include \"I2Cdev.h\"\n\n/* LSM6DSM registers\n  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\n*/\n#define LSM6DSM_FUNC_CFG_ACCESS           0x01\n#define LSM6DSM_SENSOR_SYNC_TIME_FRAME    0x04\n#define LSM6DSM_SENSOR_SYNC_RES_RATIO     0x05\n#define LSM6DSM_FIFO_CTRL1                0x06\n#define LSM6DSM_FIFO_CTRL2                0x07\n#define LSM6DSM_FIFO_CTRL3                0x08\n#define LSM6DSM_FIFO_CTRL4                0x09\n#define LSM6DSM_FIFO_CTRL5                0x0A\n#define LSM6DSM_DRDY_PULSE_CFG            0x0B\n#define LSM6DSM_INT1_CTRL                 0x0D\n#define LSM6DSM_INT2_CTRL                 0x0E\n#define LSM6DSM_WHO_AM_I                  0x0F  // should be 0x6A\n#define LSM6DSM_CTRL1_XL                  0x10\n#define LSM6DSM_CTRL2_G                   0x11\n#define LSM6DSM_CTRL3_C                   0x12\n#define LSM6DSM_CTRL4_C                   0x13\n#define LSM6DSM_CTRL5_C                   0x14\n#define LSM6DSM_CTRL6_C                   0x15\n#define LSM6DSM_CTRL7_G                   0x16\n#define LSM6DSM_CTRL8_XL                  0x17\n#define LSM6DSM_CTRL9_XL                  0x18\n#define LSM6DSM_CTRL10_C                  0x19\n#define LSM6DSM_MASTER_CONFIG             0x1A\n#define LSM6DSM_WAKE_UP_SRC               0x1B\n#define LSM6DSM_TAP_SRC                   0x1C\n#define LSM6DSM_D6D_SRC                   0x1D\n#define LSM6DSM_STATUS_REG                0x1E\n#define LSM6DSM_OUT_TEMP_L                0x20\n#define LSM6DSM_OUT_TEMP_H                0x21\n#define LSM6DSM_OUTX_L_G                  0x22\n#define LSM6DSM_OUTX_H_G                  0x23\n#define LSM6DSM_OUTY_L_G                  0x24\n#define LSM6DSM_OUTY_H_G                  0x25\n#define LSM6DSM_OUTZ_L_G                  0x26\n#define LSM6DSM_OUTZ_H_G                  0x27\n#define LSM6DSM_OUTX_L_XL                 0x28\n#define LSM6DSM_OUTX_H_XL                 0x29\n#define LSM6DSM_OUTY_L_XL                 0x2A\n#define LSM6DSM_OUTY_H_XL                 0x2B\n#define LSM6DSM_OUTZ_L_XL                 0x2C\n#define LSM6DSM_OUTZ_H_XL                 0x2D\n#define LSM6DSM_SENSORHUB1_REG            0x2E\n#define LSM6DSM_SENSORHUB2_REG            0x2F\n#define LSM6DSM_SENSORHUB3_REG            0x30\n#define LSM6DSM_SENSORHUB4_REG            0x31\n#define LSM6DSM_SENSORHUB5_REG            0x32\n#define LSM6DSM_SENSORHUB6_REG            0x33\n#define LSM6DSM_SENSORHUB7_REG            0x34\n#define LSM6DSM_SENSORHUB8_REG            0x35\n#define LSM6DSM_SENSORHUB9_REG            0x36\n#define LSM6DSM_SENSORHUB10_REG           0x37\n#define LSM6DSM_SENSORHUB11_REG           0x38\n#define LSM6DSM_SENSORHUB12_REG           0x39\n#define LSM6DSM_FIFO_STATUS1              0x3A\n#define LSM6DSM_FIFO_STATUS2              0x3B\n#define LSM6DSM_FIFO_STATUS3              0x3C\n#define LSM6DSM_FIFO_STATUS4              0x3D\n#define LSM6DSM_FIFO_DATA_OUT_L           0x3E\n#define LSM6DSM_FIFO_DATA_OUT_H           0x3F\n#define LSM6DSM_TIMESTAMP0_REG            0x40\n#define LSM6DSM_TIMESTAMP1_REG            0x41\n#define LSM6DSM_TIMESTAMP2_REG            0x42\n#define LSM6DSM_STEP_TIMESTAMP_L          0x49\n#define LSM6DSM_STEP_TIMESTAMP_H          0x4A\n#define LSM6DSM_STEP_COUNTER_L            0x4B\n#define LSM6DSM_STEP_COUNTER_H            0x4C\n#define LSM6DSM_SENSORHUB13_REG           0x4D\n#define LSM6DSM_SENSORHUB14_REG           0x4E\n#define LSM6DSM_SENSORHUB15_REG           0x4F\n#define LSM6DSM_SENSORHUB16_REG           0x50\n#define LSM6DSM_SENSORHUB17_REG           0x51\n#define LSM6DSM_SENSORHUB18_REG           0x52\n#define LSM6DSM_FUNC_SRC1                 0x53\n#define LSM6DSM_FUNC_SRC2                 0x54\n#define LSM6DSM_WRIST_TILT_IA             0x55\n#define LSM6DSM_TAP_CFG                   0x58\n#define LSM6DSM_TAP_THS_6D                0x59\n#define LSM6DSM_INT_DUR2                  0x5A\n#define LSM6DSM_WAKE_UP_THS               0x5B\n#define LSM6DSM_WAKE_UP_DUR               0x5C\n#define LSM6DSM_FREE_FALL                 0x5D\n#define LSM6DSM_MD1_CFG                   0x5E\n#define LSM6DSM_MD2_CFG                   0x5F\n#define LSM6DSM_MASTER_MODE_CODE          0x60\n#define LSM6DSM_SENS_SYNC_SPI_ERROR_CODE  0x61\n#define LSM6DSM_OUT_MAG_RAW_X_L           0x66\n#define LSM6DSM_OUT_MAG_RAW_X_H           0x67\n#define LSM6DSM_OUT_MAG_RAW_Y_L           0x68\n#define LSM6DSM_OUT_MAG_RAW_Y_H           0x69\n#define LSM6DSM_OUT_MAG_RAW_Z_L           0x6A\n#define LSM6DSM_OUT_MAG_RAW_Z_H           0x6B\n#define LSM6DSM_INT_OIS                   0x6F\n#define LSM6DSM_CTRL1_OIS                 0x70\n#define LSM6DSM_CTRL2_OIS                 0x71\n#define LSM6DSM_CTRL3_OIS                 0x72\n#define LSM6DSM_X_OFS_USR                 0x73\n#define LSM6DSM_Y_OFS_USR                 0x74\n#define LSM6DSM_Z_OFS_USR                 0x75\n\n#define LSM6DSM_ADDRESS           0x6A   // Address of LSM6DSM accel/gyro when ADO = 0\n\n\n#define AFS_2G  0x00\n#define AFS_4G  0x02\n#define AFS_8G  0x03\n#define AFS_16G 0x01\n\n#define GFS_245DPS  0x00\n#define GFS_500DPS  0x01\n#define GFS_1000DPS 0x02\n#define GFS_2000DPS 0x03\n\n#define AODR_12_5Hz  0x01  // same for accel and gyro in normal mode\n#define AODR_26Hz    0x02\n#define AODR_52Hz    0x03\n#define AODR_104Hz   0x04\n#define AODR_208Hz   0x05\n#define AODR_416Hz   0x06\n#define AODR_833Hz   0x07\n#define AODR_1660Hz  0x08\n#define AODR_3330Hz  0x09\n#define AODR_6660Hz  0x0A\n\n#define GODR_12_5Hz  0x01   \n#define GODR_26Hz    0x02\n#define GODR_52Hz    0x03\n#define GODR_104Hz   0x04\n#define GODR_208Hz   0x05\n#define GODR_416Hz   0x06\n#define GODR_833Hz   0x07\n#define GODR_1660Hz  0x08\n#define GODR_3330Hz  0x09\n#define GODR_6660Hz  0x0A\n\n\nclass LSM6DSM\n{\n  public:\n  LSM6DSM(uint8_t intPin1, uint8_t intPin2, I2Cdev* i2c_bus);\n  float getAres(uint8_t Ascale);\n  float getGres(uint8_t Gscale);\n  uint8_t getChipID();\n  void init(uint8_t Ascale, uint8_t Gscale, uint8_t AODR, uint8_t GODR);\n  void offsetBias(float * dest1, float * dest2);\n  void reset();\n  void selfTest();\n  void readData(int16_t * destination);\n  void I2Cscan();\n  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);\n  uint8_t readByte(uint8_t address, uint8_t subAddress);\n  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);\n  private:\n  uint8_t _intPin1;\n  uint8_t _intPin2;\n  float _aRes, _gRes;\n  I2Cdev* _i2c_bus;\n};\n\n#endif\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/USFS.cpp",
    "content": "/* 06/29/2017 Copyright Tlera Corporation\n *  \n *  Created by Kris Winer\n *  \n *  \n *  Library may be used freely and without limit with attribution.\n *  \n */\n#include \"USFS.h\"\n\nUSFS::USFS(uint8_t intPin, bool passThru, I2Cdev* i2c_bus)\n{\n  _intPin = intPin;\n  _passThru = passThru;\n  _i2c_bus = i2c_bus;    \n}\n\nvoid USFS::getChipID()\n{\n  // Read SENtral device information\n  uint16_t ROM1 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ROMVersion1);\n  uint16_t ROM2 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ROMVersion2);\n  Serial.print(\"EM7180 ROM Version: 0x\"); Serial.print(ROM1, HEX); Serial.println(ROM2, HEX); Serial.println(\"Should be: 0xE609\");\n  uint16_t RAM1 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RAMVersion1);\n  uint16_t RAM2 = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RAMVersion2);\n  Serial.print(\"EM7180 RAM Version: 0x\"); Serial.print(RAM1); Serial.println(RAM2);\n  uint8_t PID = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ProductID);\n  Serial.print(\"EM7180 ProductID: 0x\"); Serial.print(PID, HEX); Serial.println(\" Should be: 0x80\");\n  uint8_t RID = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RevisionID);\n  Serial.print(\"EM7180 RevisionID: 0x\"); Serial.print(RID, HEX); Serial.println(\" Should be: 0x02\");\n}\n\n void USFS::loadfwfromEEPROM()\n {\n  // Check which sensors can be detected by the EM7180\n  uint8_t featureflag = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_FeatureFlags);\n    if(featureflag & 0x01)  Serial.println(\"A barometer is installed\");\n    if(featureflag & 0x02)  Serial.println(\"A humidity sensor is installed\");\n    if(featureflag & 0x04)  Serial.println(\"A temperature sensor is installed\");\n    if(featureflag & 0x08)  Serial.println(\"A custom sensor is installed\");\n    if(featureflag & 0x10)  Serial.println(\"A second custom sensor is installed\");\n    if(featureflag & 0x20)  Serial.println(\"A third custom sensor is installed\");\n  \n  delay(1000); // give some time to read the screen\n\n  // Check SENtral status, make sure EEPROM upload of firmware was accomplished\n  byte STAT = (_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)  Serial.println(\"EEPROM detected on the sensor bus!\");\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)  Serial.println(\"EEPROM uploaded config file!\");\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)  Serial.println(\"EEPROM CRC incorrect!\");\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)  Serial.println(\"EM7180 in initialized state!\");\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)  Serial.println(\"No EEPROM detected!\");\n  int count = 0;\n  while(!STAT) {\n    _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ResetRequest, 0x01);\n    delay(500);  \n    count++;  \n    STAT = (_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01);\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x01)  Serial.println(\"EEPROM detected on the sensor bus!\");\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x02)  Serial.println(\"EEPROM uploaded config file!\");\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04)  Serial.println(\"EEPROM CRC incorrect!\");\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x08)  Serial.println(\"EM7180 in initialized state!\");\n    if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x10)  Serial.println(\"No EEPROM detected!\");\n    if(count > 10) break;\n  }\n  \n   if(!(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_SentralStatus) & 0x04))  Serial.println(\"EEPROM upload successful!\");\n }\n\n  uint8_t USFS::checkEM7180Status(){\n  // Check event status register, way to check data ready by polling rather than interrupt\n  uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_EventStatus); // reading clears the register and interrupt\n  return c;\n  }\n\n  uint8_t USFS::checkEM7180Errors(){\n  uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ErrorRegister); // check error register\n  return c;\n  }\n  \n 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)\n {\n  uint16_t EM7180_mag_fs, EM7180_acc_fs, EM7180_gyro_fs; // EM7180 sensor full scale ranges\n  uint8_t param[4];      \n\n  // Enter EM7180 initialized state\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_PassThruControl, 0x00); // make sure pass through mode is off\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // Force initialize\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x00); // set SENtral in initialized state to configure registers\n  \n  //Setup LPF bandwidth (BEFORE setting ODR's)\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ACC_LPF_BW, accBW);   // accBW = 3 = 41Hz\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_GYRO_LPF_BW, gyroBW); // gyroBW = 3 = 41Hz\n // Set accel/gyro/mag desired ODR rates\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_QRateDivisor, QRtDiv); // quat rate = gyroRt/(1 QRTDiv)\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_MagRate, magRt); // 0x64 = 100 Hz\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AccelRate, accRt); // 200/10 Hz, 0x14 = 200 Hz\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_GyroRate, gyroRt); // 200/10 Hz, 0x14 = 200 Hz\n  _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\n\n  // Configure operating mode\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // read scale sensor data\n  // Enable interrupt to host upon certain events\n  // choose host interrupts when any sensor updated (0x40), new gyro data (0x20), new accel data (0x10),\n  // new mag data (0x08), quaternions updated (0x04), an error occurs (0x02), or the SENtral needs to be reset(0x01)\n  _i2c_bus-> writeByte(EM7180_ADDRESS, EM7180_EnableEvents, 0x07);\n  // Enable EM7180 run mode\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_HostControl, 0x01); // set SENtral in normal run mode\n  delay(100);\n\n // EM7180 parameter adjustments\n  Serial.println(\"Beginning Parameter Adjustments\");\n  \n  // Read sensor default FS values from parameter space\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read parameter 74\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process\n  byte param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  while(!(param_xfer==0x4A)) {\n    param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  }\n  param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);\n  param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);\n  param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);\n  param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);\n  EM7180_mag_fs = ((int16_t)(param[1]<<8) | param[0]);\n  EM7180_acc_fs = ((int16_t)(param[3]<<8) | param[2]);\n  Serial.print(\"Magnetometer Default Full Scale Range: +/-\"); Serial.print(EM7180_mag_fs); Serial.println(\"uT\");\n  Serial.print(\"Accelerometer Default Full Scale Range: +/-\"); Serial.print(EM7180_acc_fs); Serial.println(\"g\");\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read  parameter 75\n  param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  while(!(param_xfer==0x4B)) {\n    param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  }\n  param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);\n  param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);\n  param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);\n  param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);\n  EM7180_gyro_fs = ((int16_t)(param[1]<<8) | param[0]);\n  Serial.print(\"Gyroscope Default Full Scale Range: +/-\"); Serial.print(EM7180_gyro_fs); Serial.println(\"dps\");\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm\n  \n  //Disable stillness mode for balancing robot application\n  EM7180_set_integer_param (0x49, 0x00);\n  \n  //Write desired sensor full scale ranges to the EM7180\n  EM7180_set_mag_acc_FS (magFS, accFS); // 1000 uT == 0x3E8, 8 g == 0x08\n  EM7180_set_gyro_FS (gyroFS); // 2000 dps == 0x7D0\n  \n  // Read sensor new FS values from parameter space\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4A); // Request to read  parameter 74\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); // Request parameter transfer process\n  param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  while(!(param_xfer==0x4A)) {\n    param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  }\n  param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);\n  param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);\n  param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);\n  param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);\n  EM7180_mag_fs = ((int16_t)(param[1]<<8) | param[0]);\n  EM7180_acc_fs = ((int16_t)(param[3]<<8) | param[2]);\n  Serial.print(\"Magnetometer New Full Scale Range: +/-\"); Serial.print(EM7180_mag_fs); Serial.println(\"uT\");\n  Serial.print(\"Accelerometer New Full Scale Range: +/-\"); Serial.print(EM7180_acc_fs); Serial.println(\"g\");\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x4B); // Request to read  parameter 75\n  param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  while(!(param_xfer==0x4B)) {\n    param_xfer = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  }\n  param[0] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte0);\n  param[1] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte1);\n  param[2] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte2);\n  param[3] = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SavedParamByte3);\n  EM7180_gyro_fs = ((int16_t)(param[1]<<8) | param[0]);\n  Serial.print(\"Gyroscope New Full Scale Range: +/-\"); Serial.print(EM7180_gyro_fs); Serial.println(\"dps\");\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //End parameter transfer\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // re-enable algorithm\n\n  \n// Read EM7180 status\nuint8_t runStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_RunStatus);\nif(runStatus & 0x01) Serial.println(\" EM7180 run status = normal mode\");\nuint8_t algoStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);\nif(algoStatus & 0x01) Serial.println(\" EM7180 standby status\");\nif(algoStatus & 0x02) Serial.println(\" EM7180 algorithm slow\");\nif(algoStatus & 0x04) Serial.println(\" EM7180 in stillness mode\");\nif(algoStatus & 0x08) Serial.println(\" EM7180 mag calibration completed\");\nif(algoStatus & 0x10) Serial.println(\" EM7180 magnetic anomaly detected\");\nif(algoStatus & 0x20) Serial.println(\" EM7180 unreliable sensor data\");\nuint8_t passthruStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_PassThruStatus);\nif(passthruStatus & 0x01) Serial.print(\" EM7180 in passthru mode!\");\nuint8_t eventStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_EventStatus);\nif(eventStatus & 0x01) Serial.println(\" EM7180 CPU reset\");\nif(eventStatus & 0x02) Serial.println(\" EM7180 Error\");\nif(eventStatus & 0x04) Serial.println(\" EM7180 new quaternion result\");\nif(eventStatus & 0x08) Serial.println(\" EM7180 new mag result\");\nif(eventStatus & 0x10) Serial.println(\" EM7180 new accel result\");\nif(eventStatus & 0x20) Serial.println(\" EM7180 new gyro result\"); \n  \n  delay(1000); // give some time to read the screen\n  \n  // Check sensor status\n  uint8_t sensorStatus = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_SensorStatus);\n  Serial.print(\" EM7180 sensor status = \"); Serial.println(sensorStatus);\n  if(sensorStatus & 0x01) Serial.print(\"Magnetometer not acknowledging!\");\n  if(sensorStatus & 0x02) Serial.print(\"Accelerometer not acknowledging!\");\n  if(sensorStatus & 0x04) Serial.print(\"Gyro not acknowledging!\");\n  if(sensorStatus & 0x10) Serial.print(\"Magnetometer ID not recognized!\");\n  if(sensorStatus & 0x20) Serial.print(\"Accelerometer ID not recognized!\");\n  if(sensorStatus & 0x40) Serial.print(\"Gyro ID not recognized!\");\n  \n  Serial.print(\"Actual MagRate = \"); Serial.print(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualMagRate)); Serial.println(\" Hz\"); \n  Serial.print(\"Actual AccelRate = \"); Serial.print(10*(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualAccelRate))); Serial.println(\" Hz\"); \n  Serial.print(\"Actual GyroRate = \"); Serial.print(10*(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualGyroRate))); Serial.println(\" Hz\"); \n  Serial.print(\"Actual BaroRate = \"); Serial.print(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_ActualBaroRate)); Serial.println(\" Hz\"); \n}\n\nfloat USFS::uint32_reg_to_float (uint8_t *buf)\n{\n  union {\n    uint32_t ui32;\n    float f;\n  } u;\n\n  u.ui32 =     (((uint32_t)buf[0]) +\n               (((uint32_t)buf[1]) <<  8) +\n               (((uint32_t)buf[2]) << 16) +\n               (((uint32_t)buf[3]) << 24));\n  return u.f;\n}\n\nfloat USFS::int32_reg_to_float (uint8_t *buf)\n{\n  union {\n    int32_t i32;\n    float f;\n  } u;\n\n  u.i32 =      (((int32_t)buf[0]) +\n               (((int32_t)buf[1]) <<  8) +\n               (((int32_t)buf[2]) << 16) +\n               (((int32_t)buf[3]) << 24));\n  return u.f;\n}\n\nvoid USFS::float_to_bytes (float param_val, uint8_t *buf) {\n  union {\n    float f;\n    uint8_t comp[sizeof(float)];\n  } u;\n  u.f = param_val;\n  for (uint8_t i=0; i < sizeof(float); i++) {\n    buf[i] = u.comp[i];\n  }\n  //Convert to LITTLE ENDIAN\n  for (uint8_t i=0; i < sizeof(float); i++) {\n    buf[i] = buf[(sizeof(float)-1) - i];\n  }\n}\n\nvoid USFS::EM7180_set_gyro_FS (uint16_t gyro_fs) {\n  uint8_t bytes[4], STAT;\n  bytes[0] = gyro_fs & (0xFF);\n  bytes[1] = (gyro_fs >> 8) & (0xFF);\n  bytes[2] = 0x00;\n  bytes[3] = 0x00;\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Gyro LSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Gyro MSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Unused\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Unused\n  _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\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure\n  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte\n  while(!(STAT==0xCB)) {\n  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  }\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm\n}\n\nvoid USFS::EM7180_set_mag_acc_FS (uint16_t mag_fs, uint16_t acc_fs) {\n  uint8_t bytes[4], STAT;\n  bytes[0] = mag_fs & (0xFF);\n  bytes[1] = (mag_fs >> 8) & (0xFF);\n  bytes[2] = acc_fs & (0xFF);\n  bytes[3] = (acc_fs >> 8) & (0xFF);\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Mag LSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]); //Mag MSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]); //Acc LSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Acc MSB\n  _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\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure\n  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte\n  while(!(STAT==0xCA)) {\n    STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  }\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm\n}\n\nvoid USFS::EM7180_set_integer_param (uint8_t param, uint32_t param_val) {\n  uint8_t bytes[4], STAT;\n  bytes[0] = param_val & (0xFF);\n  bytes[1] = (param_val >> 8) & (0xFF);\n  bytes[2] = (param_val >> 16) & (0xFF);\n  bytes[3] = (param_val >> 24) & (0xFF);\n  param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, param);\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure\n  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte\n  while(!(STAT==param)) {\n    STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  }\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm\n}\n\nvoid USFS::EM7180_set_float_param (uint8_t param, float param_val) {\n  uint8_t bytes[4], STAT;\n  float_to_bytes (param_val, &bytes[0]);\n  param = param | 0x80; //Parameter is the decimal value with the MSB set high to indicate a paramter write processs\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte0, bytes[0]); //Param LSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte1, bytes[1]);\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte2, bytes[2]);\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_LoadParamByte3, bytes[3]); //Param MSB\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, param);\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x80); //Request parameter transfer procedure\n  STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge); //Check the parameter acknowledge register and loop until the result matches parameter request byte\n  while(!(STAT==param)) {\n    STAT = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_ParamAcknowledge);\n  }\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_ParamRequest, 0x00); //Parameter request = 0 to end parameter transfer process\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, 0x00); // Re-start algorithm\n}\n\n\nvoid USFS::readSENtralQuatData(float * destination)\n{\n  uint8_t rawData[16];  // x/y/z quaternion register data stored here\n  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_QX, 16, &rawData[0]); // Read the sixteen raw data registers into data array\n  destination[1] = uint32_reg_to_float (&rawData[0]);\n  destination[2] = uint32_reg_to_float (&rawData[4]);\n  destination[3] = uint32_reg_to_float (&rawData[8]);\n  destination[0] = uint32_reg_to_float (&rawData[12]);   // SENtral stores quats as qx, qy, qz, q0!\n\n}\n\nvoid USFS::readSENtralAccelData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z accel register data stored here\n  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_AX, 6, &rawData[0]);       // Read the six raw data registers into data array\n  destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);  \n  destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]); \n}\n\nvoid USFS::readSENtralGyroData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_GX, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);   // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);  \n  destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]); \n}\n\nvoid USFS::readSENtralMagData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_MX, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);   // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);  \n  destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]); \n}\n\nint16_t USFS::readSENtralBaroData()\n{\n  uint8_t rawData[2];  // x/y/z gyro register data stored here\n  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_Baro, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array\n  return  (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);   // Turn the MSB and LSB into a signed 16-bit value\n}\n\nint16_t USFS::readSENtralTempData()\n{\n  uint8_t rawData[2];  // x/y/z gyro register data stored here\n  _i2c_bus->readBytes(EM7180_ADDRESS, EM7180_Temp, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array\n  return  (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);   // Turn the MSB and LSB into a signed 16-bit value\n}\n\n\nvoid USFS::SENtralPassThroughMode()\n{\n  // First put SENtral in standby mode\n  uint8_t c = _i2c_bus->readByte(EM7180_ADDRESS, EM7180_AlgorithmControl);\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_AlgorithmControl, c | 0x01);\n//  c = readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus);\n//  Serial.print(\"c = \"); Serial.println(c);\n// Verify standby status\n// if(readByte(EM7180_ADDRESS, EM7180_AlgorithmStatus) & 0x01) {\n   Serial.println(\"SENtral in standby mode\"); \n  // Place SENtral in pass-through mode\n  _i2c_bus->writeByte(EM7180_ADDRESS, EM7180_PassThruControl, 0x01); \n  if(_i2c_bus->readByte(EM7180_ADDRESS, EM7180_PassThruStatus) & 0x01) {\n    Serial.println(\"SENtral in pass-through mode\");\n  }\n  else {\n    Serial.println(\"ERROR! SENtral not in pass-through mode!\");\n  }\n }\n \n \n\n// I2C communication with the M24512DFM EEPROM is a little different from I2C communication with the usual motion sensor\n// since the address is defined by two bytes\n\n       void USFS::M24512DFMwriteByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t  data)\n       {\n       Wire.beginTransmission(device_address);   // Initialize the Tx buffer\n       Wire.write(data_address1);                // Put slave register address in Tx buffer\n       Wire.write(data_address2);                // Put slave register address in Tx buffer\n       Wire.write(data);                         // Put data in Tx buffer\n       Wire.endTransmission();                   // Send the Tx buffer\n       }\n\n\n       void USFS::M24512DFMwriteBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest)\n       {\n       if(count > 128) \n       {\n        count = 128;\n        Serial.print(\"Page count cannot be more than 128 bytes!\");\n        }       \n        Wire.beginTransmission(device_address);   // Initialize the Tx buffer\n        Wire.write(data_address1);                // Put slave register address in Tx buffer\n        Wire.write(data_address2);                // Put slave register address in Tx buffer\n        for(uint8_t i=0; i < count; i++) {\n           Wire.write(dest[i]);                      // Put data in Tx buffer\n           }\n        Wire.endTransmission();                   // Send the Tx buffer\n        }\n\n\n        uint8_t USFS::M24512DFMreadByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2)\n        {\n        uint8_t data; // `data` will store the register data   \n        Wire.beginTransmission(device_address);         // Initialize the Tx buffer\n        Wire.write(data_address1);                // Put slave register address in Tx buffer\n        Wire.write(data_address2);                // Put slave register address in Tx buffer\n        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n        Wire.requestFrom(device_address, 1);  // Read one byte from slave register address \n        data = Wire.read();                      // Fill Rx buffer with result\n        return data;                             // Return data read from slave register\n        }\n\n        void USFS::M24512DFMreadBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest)\n        {  \n        uint8_t temp[2] = {data_address1, data_address2};\n        Wire.beginTransmission(device_address);   // Initialize the Tx buffer\n        Wire.write(data_address1);                     // Put slave register address in Tx buffer\n        Wire.write(data_address2);                     // Put slave register address in Tx buffer\n        Wire.endTransmission(false);              // Send the Tx buffer, but send a restart to keep connection alive\n        uint8_t i = 0;\n        Wire.requestFrom(device_address, count);       // Read bytes from slave register address \n        while (Wire.available()) {\n        dest[i++] = Wire.read(); }                // Put read results in the Rx buffer\n        }\n\n\n\n\n\n\n  // Implementation of Sebastian Madgwick's \"...efficient orientation filter for... inertial/magnetic sensor arrays\"\n// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)\n// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute\n// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.\n// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms\n// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!\nvoid USFS::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)\n        {\n            float q1 = _q[0], q2 = _q[1], q3 = _q[2], q4 = _q[3];   // short name local variable for readability\n            float norm;\n            float hx, hy, _2bx, _2bz;\n            float s1, s2, s3, s4;\n            float qDot1, qDot2, qDot3, qDot4;\n\n            // Auxiliary variables to avoid repeated arithmetic\n            float _2q1mx;\n            float _2q1my;\n            float _2q1mz;\n            float _2q2mx;\n            float _4bx;\n            float _4bz;\n            float _2q1 = 2.0f * q1;\n            float _2q2 = 2.0f * q2;\n            float _2q3 = 2.0f * q3;\n            float _2q4 = 2.0f * q4;\n            float _2q1q3 = 2.0f * q1 * q3;\n            float _2q3q4 = 2.0f * q3 * q4;\n            float q1q1 = q1 * q1;\n            float q1q2 = q1 * q2;\n            float q1q3 = q1 * q3;\n            float q1q4 = q1 * q4;\n            float q2q2 = q2 * q2;\n            float q2q3 = q2 * q3;\n            float q2q4 = q2 * q4;\n            float q3q3 = q3 * q3;\n            float q3q4 = q3 * q4;\n            float q4q4 = q4 * q4;\n\n            // Normalise accelerometer measurement\n            norm = sqrt(ax * ax + ay * ay + az * az);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f/norm;\n            ax *= norm;\n            ay *= norm;\n            az *= norm;\n\n            // Normalise magnetometer measurement\n            norm = sqrt(mx * mx + my * my + mz * mz);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f/norm;\n            mx *= norm;\n            my *= norm;\n            mz *= norm;\n\n            // Reference direction of Earth's magnetic field\n            _2q1mx = 2.0f * q1 * mx;\n            _2q1my = 2.0f * q1 * my;\n            _2q1mz = 2.0f * q1 * mz;\n            _2q2mx = 2.0f * q2 * mx;\n            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;\n            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;\n            _2bx = sqrt(hx * hx + hy * hy);\n            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;\n            _4bx = 2.0f * _2bx;\n            _4bz = 2.0f * _2bz;\n\n            // Gradient decent algorithm corrective step\n            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);\n            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);\n            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);\n            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);\n            norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude\n            norm = 1.0f/norm;\n            s1 *= norm;\n            s2 *= norm;\n            s3 *= norm;\n            s4 *= norm;\n\n            // Compute rate of change of quaternion\n            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - _beta * s1;\n            qDot2 = 0.5f * ( q1 * gx + q3 * gz - q4 * gy) - _beta * s2;\n            qDot3 = 0.5f * ( q1 * gy - q2 * gz + q4 * gx) - _beta * s3;\n            qDot4 = 0.5f * ( q1 * gz + q2 * gy - q3 * gx) - _beta * s4;\n\n            // Integrate to yield quaternion\n            q1 += qDot1 * _deltat;\n            q2 += qDot2 * _deltat;\n            q3 += qDot3 * _deltat;\n            q4 += qDot4 * _deltat;\n            norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion\n            norm = 1.0f/norm;\n            _q[0] = q1 * norm;\n            _q[1] = q2 * norm;\n            _q[2] = q3 * norm;\n            _q[3] = q4 * norm;\n\n        }\n  \n  \n  \n // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and\n // measured ones. \n            void USFS::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)\n        {\n            float q1 = _q[0], q2 = _q[1], q3 = _q[2], q4 = _q[3];   // short name local variable for readability\n            float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method\n            float norm;\n            float hx, hy, bx, bz;\n            float vx, vy, vz, wx, wy, wz;\n            float ex, ey, ez;\n            float pa, pb, pc;\n\n            // Auxiliary variables to avoid repeated arithmetic\n            float q1q1 = q1 * q1;\n            float q1q2 = q1 * q2;\n            float q1q3 = q1 * q3;\n            float q1q4 = q1 * q4;\n            float q2q2 = q2 * q2;\n            float q2q3 = q2 * q3;\n            float q2q4 = q2 * q4;\n            float q3q3 = q3 * q3;\n            float q3q4 = q3 * q4;\n            float q4q4 = q4 * q4;   \n\n            // Normalise accelerometer measurement\n            norm = sqrt(ax * ax + ay * ay + az * az);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f / norm;        // use reciprocal for division\n            ax *= norm;\n            ay *= norm;\n            az *= norm;\n\n            // Normalise magnetometer measurement\n            norm = sqrt(mx * mx + my * my + mz * mz);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f / norm;        // use reciprocal for division\n            mx *= norm;\n            my *= norm;\n            mz *= norm;\n\n            // Reference direction of Earth's magnetic field\n            hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);\n            hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);\n            bx = sqrt((hx * hx) + (hy * hy));\n            bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);\n\n            // Estimated direction of gravity and magnetic field\n            vx = 2.0f * (q2q4 - q1q3);\n            vy = 2.0f * (q1q2 + q3q4);\n            vz = q1q1 - q2q2 - q3q3 + q4q4;\n            wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);\n            wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);\n            wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  \n\n            // Error is cross product between estimated direction and measured direction of gravity\n            ex = (ay * vz - az * vy) + (my * wz - mz * wy);\n            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);\n            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);\n            if (_Ki > 0.0f)\n            {\n                eInt[0] += ex;      // accumulate integral error\n                eInt[1] += ey;\n                eInt[2] += ez;\n            }\n            else\n            {\n                eInt[0] = 0.0f;     // prevent integral wind up\n                eInt[1] = 0.0f;\n                eInt[2] = 0.0f;\n            }\n\n            // Apply feedback terms\n            gx = gx + _Kp * ex + _Ki * eInt[0];\n            gy = gy + _Kp * ey + _Ki * eInt[1];\n            gz = gz + _Kp * ez + _Ki * eInt[2];\n\n            // Integrate rate of change of quaternion\n            pa = q2;\n            pb = q3;\n            pc = q4;\n            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * _deltat);\n            q2 = pa + (q1 * gx + pb * gz - pc * gy) *  (0.5f * _deltat);\n            q3 = pb + (q1 * gy - pa * gz + pc * gx) *  (0.5f * _deltat);\n            q4 = pc + (q1 * gz + pa * gy - pb * gx) *  (0.5f * _deltat);\n\n            // Normalise quaternion\n            norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);\n            norm = 1.0f / norm;\n            _q[0] = q1 * norm;\n            _q[1] = q2 * norm;\n            _q[2] = q3 * norm;\n            _q[3] = q4 * norm;\n \n        }\r\n"
  },
  {
    "path": "EM7180/EM7180_LSM6DSM_LIS2MDL_LPS22HB_ESP32/USFS.h",
    "content": "#ifndef USFS_h\n#define USFSh_h\n\n#include \"Arduino.h\"\n#include \"Wire.h\"\n#include \"I2Cdev.h\"   \n\n// EM7180 SENtral register map\n// see http://www.emdeveloper.com/downloads/7180/EMSentral_EM7180_Register_Map_v1_3.pdf\n//\n#define EM7180_QX                 0x00  // this is a 32-bit normalized floating point number read from registers 0x00-03\n#define EM7180_QY                 0x04  // this is a 32-bit normalized floating point number read from registers 0x04-07\n#define EM7180_QZ                 0x08  // this is a 32-bit normalized floating point number read from registers 0x08-0B\n#define EM7180_QW                 0x0C  // this is a 32-bit normalized floating point number read from registers 0x0C-0F\n#define EM7180_QTIME              0x10  // this is a 16-bit unsigned integer read from registers 0x10-11\n#define EM7180_MX                 0x12  // int16_t from registers 0x12-13\n#define EM7180_MY                 0x14  // int16_t from registers 0x14-15\n#define EM7180_MZ                 0x16  // int16_t from registers 0x16-17\n#define EM7180_MTIME              0x18  // uint16_t from registers 0x18-19\n#define EM7180_AX                 0x1A  // int16_t from registers 0x1A-1B\n#define EM7180_AY                 0x1C  // int16_t from registers 0x1C-1D\n#define EM7180_AZ                 0x1E  // int16_t from registers 0x1E-1F\n#define EM7180_ATIME              0x20  // uint16_t from registers 0x20-21\n#define EM7180_GX                 0x22  // int16_t from registers 0x22-23\n#define EM7180_GY                 0x24  // int16_t from registers 0x24-25\n#define EM7180_GZ                 0x26  // int16_t from registers 0x26-27\n#define EM7180_GTIME              0x28  // uint16_t from registers 0x28-29\n#define EM7180_Baro               0x2A  // start of two-byte MS5637 pressure data, 16-bit signed interger\n#define EM7180_BaroTIME           0x2C  // start of two-byte MS5637 pressure timestamp, 16-bit unsigned\n#define EM7180_Temp               0x2E  // start of two-byte MS5637 temperature data, 16-bit signed interger\n#define EM7180_TempTIME           0x30  // start of two-byte MS5637 temperature timestamp, 16-bit unsigned\n#define EM7180_QRateDivisor       0x32  // uint8_t \n#define EM7180_EnableEvents       0x33\n#define EM7180_HostControl        0x34\n#define EM7180_EventStatus        0x35\n#define EM7180_SensorStatus       0x36\n#define EM7180_SentralStatus      0x37\n#define EM7180_AlgorithmStatus    0x38\n#define EM7180_FeatureFlags       0x39\n#define EM7180_ParamAcknowledge   0x3A\n#define EM7180_SavedParamByte0    0x3B\n#define EM7180_SavedParamByte1    0x3C\n#define EM7180_SavedParamByte2    0x3D\n#define EM7180_SavedParamByte3    0x3E\n#define EM7180_ActualMagRate      0x45\n#define EM7180_ActualAccelRate    0x46\n#define EM7180_ActualGyroRate     0x47\n#define EM7180_ActualBaroRate     0x48\n#define EM7180_ActualTempRate     0x49\n#define EM7180_ErrorRegister      0x50\n#define EM7180_AlgorithmControl   0x54\n#define EM7180_MagRate            0x55\n#define EM7180_AccelRate          0x56\n#define EM7180_GyroRate           0x57\n#define EM7180_BaroRate           0x58\n#define EM7180_TempRate           0x59\n#define EM7180_LoadParamByte0     0x60\n#define EM7180_LoadParamByte1     0x61\n#define EM7180_LoadParamByte2     0x62\n#define EM7180_LoadParamByte3     0x63\n#define EM7180_ParamRequest       0x64\n#define EM7180_ROMVersion1        0x70\n#define EM7180_ROMVersion2        0x71\n#define EM7180_RAMVersion1        0x72\n#define EM7180_RAMVersion2        0x73\n#define EM7180_ProductID          0x90\n#define EM7180_RevisionID         0x91\n#define EM7180_RunStatus          0x92\n#define EM7180_UploadAddress      0x94 // uint16_t registers 0x94 (MSB)-5(LSB)\n#define EM7180_UploadData         0x96  \n#define EM7180_CRCHost            0x97  // uint32_t from registers 0x97-9A\n#define EM7180_ResetRequest       0x9B   \n#define EM7180_PassThruStatus     0x9E   \n#define EM7180_PassThruControl    0xA0\n#define EM7180_ACC_LPF_BW         0x5B  //Register GP36\n#define EM7180_GYRO_LPF_BW        0x5C  //Register GP37\n#define EM7180_BARO_LPF_BW        0x5D  //Register GP38\n\n#define EM7180_ADDRESS           0x28   // Address of the EM7180 SENtral sensor hub\n#define M24512DFM_DATA_ADDRESS   0x50   // Address of the 500 page M24512DFM EEPROM data buffer, 1024 bits (128 8-bit bytes) per page\n#define M24512DFM_IDPAGE_ADDRESS 0x58   // Address of the single M24512DFM lockable EEPROM ID page\n#define MPU9250_ADDRESS          0x68   // Device address when ADO = 0\n#define AK8963_ADDRESS           0x0C   //  Address of magnetometer\n#define MS5637_ADDRESS           0x76   // Address of altimeter\n\nclass USFS\n{\n  public: \n  USFS(uint8_t intPin, bool passThru, I2Cdev* i2c_bus);\n  float uint32_reg_to_float (uint8_t *buf);\n  float int32_reg_to_float (uint8_t *buf);\n  void float_to_bytes (float param_val, uint8_t *buf);\n  void EM7180_set_gyro_FS (uint16_t gyro_fs);\n  void EM7180_set_mag_acc_FS (uint16_t mag_fs, uint16_t acc_fs);\n  void EM7180_set_integer_param (uint8_t param, uint32_t param_val);\n  void EM7180_set_float_param (uint8_t param, float param_val);\n  void readSENtralQuatData(float * destination);\n  void readSENtralAccelData(int16_t * destination);\n  void readSENtralGyroData(int16_t * destination);\n  void readSENtralMagData(int16_t * destination);\n  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);\n  int16_t readSENtralBaroData();\n  int16_t readSENtralTempData();\n  void SENtralPassThroughMode();\n  void M24512DFMwriteByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t  data);\n  void M24512DFMwriteBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest);\n  uint8_t M24512DFMreadByte(uint8_t device_address, uint8_t data_address1, uint8_t data_address2);\n  void M24512DFMreadBytes(uint8_t device_address, uint8_t data_address1, uint8_t data_address2, uint8_t count, uint8_t * dest);\n  void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);\n  void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);\n  void getChipID();\n  void loadfwfromEEPROM();\n  uint8_t checkEM7180Status();\n  uint8_t checkEM7180Errors();\n  private:\n  uint8_t _intPin;\n  bool _passThru;\n  float _q[4];\n  float _beta;\n  float _deltat;\n  float _Kp;\n  float _Ki;\n  I2Cdev* _i2c_bus;\n};\n\n#endif\r\n"
  },
  {
    "path": "EM7180/Readme.md",
    "content": "\nSketch 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/).\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/APDS9253.cpp",
    "content": "/******************************************************************************\r\n *\r\n * Copyright (c) 2021 Tlera Corporation  All rights reserved.\r\n *\r\n * This library is open-source and freely available for all to use with attribution.\r\n * \r\n * All rights reserved.\r\n *****************************************************************************\r\n */\r\n#include \"APDS9253.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n\r\nAPDS9253::APDS9253(I2Cdev* i2c_bus)\r\n{\r\n  _i2c_bus = i2c_bus;\r\n}\r\n\r\n// set bit 4 to 1 for software reset\r\nvoid APDS9253::reset()\r\n{\r\n   uint8_t temp = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_MAIN_CTRL);\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, temp | 0x10); \r\n}\r\n\r\n \r\nuint8_t APDS9253::getChipID()\r\n{\r\n   uint8_t temp = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_PART_ID);\r\n   return temp;\r\n}\r\n\r\n\r\nvoid APDS9253::init(uint8_t RGBmode, uint8_t LS_res, uint8_t LS_rate, uint8_t LS_gain)\r\n{\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, RGBmode << 2); \r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_MEAS_RATE, (LS_res << 4) | LS_rate); \r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_GAIN, LS_gain); \r\n\r\n   // configure the interrupt\r\n   // enable interrupt on ALS (green) threshold\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_INT_CFG, 0x14); \r\n   // set persistence to 2 consecutive ALS values below threshold to trigger\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_INT_PST, 0x10); \r\n\r\n   // set lower threshold\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_THRES_LOW_0, 0x0A); // low threshold 10 counts\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_THRES_LOW_1, 0x00); \r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_LS_THRES_LOW_2, 0x00); \r\n}\r\n\r\n\r\nvoid APDS9253::enable()\r\n{\r\n   uint8_t temp = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_MAIN_CTRL);\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, temp & ~(0x02) ); // clear LS_EN bit\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, temp | 0x02 );    // set LS_EN bit\r\n}\r\n\r\n\r\nvoid APDS9253::disable()\r\n{\r\n   uint8_t temp = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_MAIN_CTRL);\r\n   _i2c_bus->writeByte(APDS9253_ADDR, APDS9253_MAIN_CTRL, temp & ~(0x02) ); // clear LS_EN bit\r\n}\r\n\r\n\r\nvoid APDS9253::getRGBiRdata(uint32_t * destination)\r\n{\r\n  uint8_t rawData[3] = {0, 0, 0};\r\n     _i2c_bus->readBytes(APDS9253_ADDR, APDS9253_LS_DATA_IR_0, 3, &rawData[0]);  \r\n     destination[3] = (((uint32_t) (rawData[3] & 0x0F)) << 16)  |  (((uint32_t) rawData[1]) << 8)  | ((uint32_t) rawData[0]);    // ir\r\n     _i2c_bus->readBytes(APDS9253_ADDR, APDS9253_LS_DATA_BLUE_0, 3, &rawData[0]);  \r\n     destination[2] = (((uint32_t) (rawData[3] & 0x0F)) << 16)  |  (((uint32_t) rawData[1]) << 8)  | ((uint32_t) rawData[0]);    // blue\r\n     _i2c_bus->readBytes(APDS9253_ADDR, APDS9253_LS_DATA_GREEN_0, 3, &rawData[0]);  \r\n     destination[1] = (((uint32_t) (rawData[3] & 0x0F)) << 16)  |  (((uint32_t) rawData[1]) << 8)  | ((uint32_t) rawData[0]);    // green\r\n     _i2c_bus->readBytes(APDS9253_ADDR, APDS9253_LS_DATA_RED_0, 3, &rawData[0]);  \r\n     destination[0] = (((uint32_t) (rawData[3] & 0x0F)) << 16)  |  (((uint32_t) rawData[1]) << 8)  | ((uint32_t) rawData[0]);    // red\r\n}\r\n\r\n\r\nuint8_t APDS9253::getStatus()\r\n{\r\n    uint8_t Status = _i2c_bus->readByte(APDS9253_ADDR, APDS9253_MAIN_STATUS);\r\n    return Status;\r\n}\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/APDS9253.h",
    "content": "/******************************************************************************\r\n *\r\n * Copyright (c) 2021 Tlera Corporation  All rights reserved.\r\n *\r\n * This library is open-source and freely available for all to use with attribution.\r\n * \r\n * All rights reserved.\r\n *****************************************************************************\r\n */\r\n\r\n#ifndef APDS9253_h\r\n#define APDS9253_h\r\n\r\n#include \"Arduino.h\"\r\n#include <Wire.h>\r\n#include \"I2Cdev.h\"\r\n\r\n// Register Map\r\n#define APDS9253_MAIN_CTRL       (0x00)\r\n#define APDS9253_LS_MEAS_RATE    (0x04)\r\n#define APDS9253_LS_GAIN         (0x05)\r\n#define APDS9253_PART_ID         (0x06)\r\n#define APDS9253_MAIN_STATUS     (0x07)\r\n#define APDS9253_LS_DATA_IR_0    (0x0A)\r\n#define APDS9253_LS_DATA_IR_1    (0x0B)\r\n#define APDS9253_LS_DATA_IR_2    (0x0C)\r\n#define APDS9253_LS_DATA_GREEN_0 (0x0D)\r\n#define APDS9253_LS_DATA_GREEN_1 (0x0E)\r\n#define APDS9253_LS_DATA_GREEN_2 (0x0F)\r\n#define APDS9253_LS_DATA_BLUE_0  (0x10)\r\n#define APDS9253_LS_DATA_BLUE_1  (0x11)\r\n#define APDS9253_LS_DATA_BLUE_2  (0x12)\r\n#define APDS9253_LS_DATA_RED_0   (0x13)\r\n#define APDS9253_LS_DATA_RED_1   (0x14)\r\n#define APDS9253_LS_DATA_RED_2   (0x15)\r\n#define APDS9253_INT_CFG         (0x19)\r\n#define APDS9253_INT_PST         (0x1A)\r\n#define APDS9253_LS_THRES_UP_0   (0x21)\r\n#define APDS9253_LS_THRES_UP_1   (0x22)\r\n#define APDS9253_LS_THRES_UP_2   (0x23)\r\n#define APDS9253_LS_THRES_LOW_0  (0x24)\r\n#define APDS9253_LS_THRES_LOW_1  (0x25)\r\n#define APDS9253_LS_THRES_LOW_2  (0x26)\r\n#define APDS9253_LS_THRES_VAR    (0x27)\r\n#define APDS9253_DK_CNT_STOR     (0x29)\r\n\r\n#define APDS9253_ADDR      0x52  // I2C address\r\n\r\n#define ALSandIR 0x00\r\n#define RGBiR    0x01\r\n\r\n//LS_res\r\n#define res20bit 0x00 // 400 ms\r\n#define res19bit 0x01 // 200 ms\r\n#define res18bit 0x02 // 100 ms default\r\n#define res17bit 0x03 // 50 ms\r\n#define res16bit 0x04 // 25 ms\r\n//#define res13bit 0x05 // 3.125 ms\r\n\r\n//LS_rate\r\n#define rate40Hz 0x00 // 25 ms\r\n#define rate20Hz 0x01\r\n#define rate10Hz 0x02 // 100 ms default\r\n#define rate5Hz 0x03\r\n#define rate2_5Hz 0x04\r\n#define rate1Hz 0x05\r\n#define rate0_5Hz 0x06 // 2000 ms\r\n\r\n// LS_gain\r\n#define gain1 0x00\r\n#define gain3 0x01\r\n#define gain6 0x02\r\n#define gain9 0x03\r\n#define gain18 0x04\r\n\r\n\r\n/* general accel methods */\r\nclass APDS9253{\r\n public:\r\n    APDS9253(I2Cdev* i2c_bus);\r\n    void reset();\r\n    uint8_t getStatus();\r\n    uint8_t getChipID();\r\n    void init(uint8_t RGBmode, uint8_t LS_res, uint8_t LS_rate, uint8_t LS_gain);\r\n    void enable();\r\n    void disable();\r\n    void getRGBiRdata(uint32_t * destination);\r\n private:\r\n    I2Cdev* _i2c_bus;\r\n};\r\n#endif\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/ESP32C3Mini_EnvSensor.v02b.ino",
    "content": "/* ESP32 C3 Mini Environmental Sensor\r\n \r\n   This example code is in the public domain.\r\n*/\r\n#include \"Arduino.h\"\r\n#include \"LPS22HB.h\"\r\n#include \"HDC2010.h\"\r\n#include \"APDS9253.h\"\r\n#include \"SPIFlash.h\"\r\n#include \"WiFi.h\"\r\n#include \"time.h\"\r\n#include <driver/adc.h> // for battery voltage reading\r\n\r\n#define Serial USBSerial\r\n#define SerialDebug true  // set to true to get Serial output for debugging, set to false to run on battery only\r\n\r\nextern \"C\" void phy_bbpll_en_usb(bool en);\r\n\r\nconst char* ssid       = \"YourNetworkName\";\r\nconst char* password   = \"YourNetworkPassword\";\r\n\r\nconst char* ntpServer = \"pool.ntp.org\";\r\nconst long  gmtOffset_sec = -8*3600; // PST for California\r\nconst int   daylightOffset_sec = 3600;\r\n\r\nstruct tm timeinfo; // local time variables\r\n\r\n/* create an ESP32 hardware timer */\r\nhw_timer_t * timer = NULL;\r\nvolatile bool alarmFlag = false;\r\n\r\nvoid IRAM_ATTR onTimer(){\r\n  alarmFlag = true;\r\n}\r\n\r\n#define I2C_BUS    Wire              // Define the I2C bus (Wire instance) you wish to use\r\n\r\nI2Cdev             i2c_0(&I2C_BUS);   // Instantiate the I2Cdev object and point to the desired I2C bus\r\n\r\n// ESP32 C3 Mini pin configuration\r\nconst uint8_t myLed   =    2;  // blue led active LOW\r\n// define battery voltage monitor pins\r\nconst uint8_t myBat   =    1;\r\nconst uint8_t myBatEn =   10;\r\n\r\n\r\n// Battery voltage monitor definitions\r\nfloat VBat = 0.0f;\r\nuint32_t chipId = 0;\r\nuint16_t ADCCounts = 0;\r\n\r\n// Configure LPS22HB barometer\r\n/* Specify sensor parameters (sample rate is twice the bandwidth) \r\n   Choices are P_1Hz, P_10Hz P_25 Hz, P_50Hz, and P_75Hz\r\n */\r\nuint8_t PODR = P_1Hz;     // set pressure amd temperature output data rate\r\nint32_t rawPressure = 0;\r\nfloat LPSTemperature, LPSPressure, LPSAltitude;\r\nuint8_t LPS22HBstatus, LPSintSource;\r\n\r\nLPS22HB LPS22HB(&i2c_0);  // Instantiate LPS22HB barometer\r\n\r\n\r\n// Configure HCD2010 humidity/temperature sensor\r\n// Choices are:\r\n// freq = ForceMode, Freq_120s, Freq_60s, Freq_10s, Freq_5s, Freq_1s, Freq_0_5s, Freq_0_2s\r\n// tres = TRES_14bit, TRES_11bit, TRES_9bit\r\n// hres = HRES_14bit, HRES_11bit, HRES_9bit\r\nuint8_t freq = ForceMode, tres = TRES_14bit, hres = HRES_14bit;\r\n\r\nfloat HDCTemperature = 0.0f, HDCHumidity = 0.0f;\r\nuint16_t rawHDCTemperature = 0, rawHDCHumidity = 0;\r\nuint8_t HDCStatus;\r\n\r\n\r\nHDC2010 HDC2010(&i2c_0); // Instantiate HDC2010 Humidity sensor\r\n\r\n\r\n// APDS9253 Configuration\r\nuint8_t RGB_mode = RGBiR; // Choice is ALSandIR (green and IR channels only) or RGBiR for all four channels\r\n//rate has to be slower than ADC settle time defines by resolution\r\n// Bright sunlight is maximum ~25 klux so choose gain of 6x and minimum resolution (16 bits)\r\n// that allows ~24 klux maximum to be measured;  a 1 Hz rate costs ~114 uA * 25/1000 ~ 3 uA\r\nuint8_t LS_res = res16bit; // Choices are res20bit (400 ms), res19bit, res18bit (100 ms, default), res17bit, res16bit (25 ms).\r\nuint8_t LS_rate = rate0_5Hz; // Choices are rate40Hz (25 ms), rate20Hz, rate10Hz (100 ms, default), rate5Hz, rate2_5Hz (400 ms), rate1Hz, rate0_5Hz\r\nuint8_t LS_gain = gain6; // Choices are gain1, gain3 (default), gain6, gain9, gain18\r\nuint32_t RGBiRData[4] = {0, 0, 0, 0}; // red, green, blue, ir counts\r\nfloat ambientLight = 0; // ambient (green) light intensity in lux\r\n\r\nfloat ALSluxTable[25]={         // lux per count for ALS depends on gain and resolution chosen\r\n0.136, 0.273, 0.548, 1.099, 2.193,\r\n0.045, 0.090, 0.180, 0.359, 0.722,\r\n0.022, 0.045, 0.090, 0.179, 0.360,\r\n0.015, 0.030, 0.059, 0.119, 0.239,\r\n0.007, 0.015, 0.029, 0.059, 0.117\r\n};\r\n\r\n// Assume all channels have the same lux per LSB scaling\r\nfloat luxScale = ALSluxTable[LS_gain * 5 + LS_res];\r\n\r\nAPDS9253 APDS9253(&i2c_0);  // Instantiate APDS9253 light sensor\r\n\r\n\r\n// SPI flash configuration\r\nconst uint8_t CSPIN  = 4;\r\nuint8_t flash_id[3] = {0, 0, 0};\r\nuint16_t page_number = 0;     // set the page number for flash page write\r\nuint8_t  sector_number = 0;   // set the sector number for sector write\r\nuint8_t  flashPage[256];      // array to hold the data for flash page write\r\n\r\nSPIFlash SPIFlash(CSPIN); // instantiate SPI flash class\r\n\r\n\r\nvoid setup() \r\n{ \r\n  if(SerialDebug) phy_bbpll_en_usb(true); //this brings the USB serial-jtag back to life. \r\n\r\n  if(SerialDebug) Serial.begin(115200);\r\n  if(SerialDebug) delay(4000);\r\n\r\n  //connect to WiFi\r\n  if(SerialDebug) Serial.printf(\"Connecting to %s \", ssid);\r\n  WiFi.begin(ssid, password);\r\n  while (WiFi.status() != WL_CONNECTED) {\r\n      delay(500);\r\n      if(SerialDebug) Serial.print(\".\");\r\n  }\r\n  if(SerialDebug) Serial.println(\" CONNECTED\");\r\n  \r\n  //init and get the time\r\n  configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);\r\n  getLocalTime(&timeinfo);\r\n  if(SerialDebug)Serial.print(\"The current date/time in Danville, California is \");\r\n  if(SerialDebug) Serial.println(&timeinfo, \"%A, %B %d %Y %H:%M:%S\");\r\n\r\n  //disconnect WiFi as it's no longer needed\r\n  WiFi.disconnect(true);\r\n  WiFi.mode(WIFI_OFF);\r\n  \r\n  // Configure led pin\r\n  pinMode(myLed, OUTPUT);\r\n  digitalWrite(myLed, LOW);  // start with led on since active LOW\r\n\r\n  // Check ESP32 ID\r\n  for(int i=0; i<17; i=i+8) {\r\n    chipId |= ((ESP.getEfuseMac() >> (40 - i)) & 0xFF) << i;\r\n  }\r\n\r\n  if(SerialDebug) { \r\n  Serial.printf(\"ESP32 Chip model = %s Rev %d\\n\", ESP.getChipModel(), ESP.getChipRevision());\r\n  Serial.printf(\"This chip has %d cores\\n\", ESP.getChipCores());\r\n  Serial.print(\"Chip ID: \"); Serial.println(chipId); Serial.println(\" \");\r\n  }\r\n\r\n  // Configure ADC for battery voltage monitor\r\n  adc1_config_width(ADC_WIDTH_BIT_12);\r\n  adc1_config_channel_atten(ADC1_CHANNEL_1, ADC_ATTEN_DB_11); // ADC 1/Channel 1 is pin 1 -- myBat\r\n  pinMode(myBat, INPUT);\r\n  pinMode(myBatEn, OUTPUT);\r\n  digitalWrite(myBatEn, HIGH);   \r\n  ADCCounts = adc1_get_raw((adc1_channel_t)1);\r\n  digitalWrite(myBatEn, LOW);   \r\n  /* ADC at attenuation 11 should rad from 0 to 2.6 V nominally. The resistor divider\r\n   *  is 1/2, and a calibration factor of 1.14 is applied to bring measurements into \r\n   *  agreement with multimeter */\r\n  VBat = 2.0f * 2.60f * 1.14f * ((float) ADCCounts) / 4095.0f;\r\n  if(SerialDebug) { \r\n    Serial.print(\"Battery voltage = \"); Serial.print(VBat, 2); Serial.println(\" V\");\r\n    Serial.print(\"Battery ADC Counts = \"); Serial.print(ADCCounts); Serial.println(\" Counts\"); Serial.println(\" \");\r\n  }\r\n\r\n   /* initialize wire bus */\r\n  I2C_BUS.begin(0, 3);                // Set master mode, I2C_BUS.begin(SDA, SCL);\r\n  I2C_BUS.setClock(400000);           // I2C frequency at 400 kHz\r\n  delay(100);\r\n\r\n  I2CscanDevices();                    // should detect all I2C devices on the bus\r\n\r\n  if(SerialDebug) Serial.println(\"LPS22HB barometer...\");\r\n  uint8_t LPS22HB_chipID = LPS22HB.getChipID();\r\n  if(SerialDebug) {\r\n      Serial.print(\"LPS22HB \"); Serial.print(\"I AM \"); Serial.print(LPS22HB_chipID, HEX); Serial.print(\" I should be \"); Serial.println(0xB1, HEX);\r\n      Serial.println(\" \");\r\n  }\r\n  delay(100); \r\n\r\n  if(SerialDebug) Serial.println(\"HDC2010 humidity sensor...\");\r\n  uint16_t HDC2010_devID = HDC2010.getDevID(HDC2010_0_ADDRESS);\r\n  if(SerialDebug) {\r\n    Serial.print(\"DeviceID = 0x0\"); Serial.print(HDC2010_devID, HEX); Serial.println(\". Should be 0x07D0\"); \r\n    Serial.println(\" \");\r\n  }\r\n\r\n  uint16_t HDC2010_manuID = HDC2010.getManuID(HDC2010_0_ADDRESS);\r\n  if(SerialDebug){ \r\n    Serial.print(\"Manufacturer's ID = 0x\"); Serial.print(HDC2010_manuID, HEX); Serial.println(\". Should be 0x5449\");\r\n    Serial.println(\" \");\r\n  }\r\n  delay(100);\r\n\r\n  // Read the APDS9253 Part ID register, this is a good test of communication\r\n  if(SerialDebug)Serial.println(\"APDS9253 RGBiR Light Sensor...\");\r\n  byte APDS9253_ID = APDS9253.getChipID();  // Read PART_ID register for APDS9253\r\n  if(SerialDebug) {\r\n      Serial.print(\"APDS9253 \"); Serial.print(\"chipID = 0x\"); Serial.print(APDS9253_ID, HEX); Serial.print(\", Should be 0x\"); Serial.println(0xC2, HEX);\r\n      Serial.println(\" \");\r\n  }\r\n  delay(100);   \r\n\r\n  if(LPS22HB_chipID == 0xB1 && HDC2010_devID == 0x07D0 && APDS9253_ID == 0xC2) // check if all I2C sensors with WHO_AM_I have acknowledged\r\n  {\r\n   if(SerialDebug) {Serial.println(\"LPS22HB, HDC2010, and APDS9253 are all online...\"); Serial.println(\" \");}\r\n\r\n   LPS22HB.reset();\r\n   LPS22HB.Init(PODR);  // Initialize LPS22HB barometer\r\n   delay(100);\r\n  \r\n   HDC2010.reset(HDC2010_0_ADDRESS);\r\n\r\n   // Configure HCD2010 for auto measurement mode if freq not ForceMode\r\n   // else measurement performed only once each time init is called\r\n   HDC2010.init(HDC2010_0_ADDRESS, hres, tres, freq); \r\n\r\n   if(SerialDebug) {Serial.print(\"Lux per count = \"); Serial.println(luxScale, 3);}\r\n\r\n   APDS9253.reset();\r\n   delay(10);\r\n   APDS9253.init(RGB_mode, LS_res, LS_rate, LS_gain);\r\n   APDS9253.disable();\r\n\r\n   digitalWrite(myLed, HIGH);  // when sensors successfully configured, turn off led\r\n  }\r\n   else \r\n  {\r\n   if(LPS22HB_chipID != 0xB1 && SerialDebug)  Serial.println(\" LPS22HB2 not functioning!\");   \r\n   if(HDC2010_devID != 0x07D0 && SerialDebug) Serial.println(\" HDC20102 not functioning!\");\r\n   if(APDS9253_ID != 0xC2 && SerialDebug)     Serial.println(\" APDS9253 not functioning!\");\r\n  }\r\n\r\n  // configure SPI flash\r\n  pinMode(CSPIN, OUTPUT);\r\n  digitalWrite(CSPIN, HIGH);\r\n\r\n // check SPI Flash ID\r\n  SPIFlash.init(6, 7, 5, 4);               // SPI.begin(SCK, MISO, MOSI, SS)\r\n  SPIFlash.powerUp();                      // MX25R6435FZAI defaults to power down state\r\n  \r\n  SPIFlash.getChipID(flash_id);                    // Verify SPI flash communication\r\n  if(flash_id[0] == 0xC2 && flash_id[1] == 0x28 && flash_id[2] == 0x17 && SerialDebug) {\r\n  Serial.println(\" \");  \r\n  Serial.println(\"Found Macronix MX25R6435FZAI with Chip ID = 0xC2, 0x28, 0x17!\");\r\n  Serial.println(\" \");  \r\n  }\r\n  else {\r\n  if(SerialDebug) {\r\n    Serial.println(\" \");  \r\n    Serial.println(\"no or unknown SPI flash!\");\r\n    Serial.println(\" \"); \r\n  } \r\n  }\r\n\r\n  SPIFlash.powerDown();                    // power down SPI flash\r\n\r\n  // Set up ESP32 timer\r\n  /* Use 1st timer of 4 */\r\n  /* 1 tick take 1/(80MHZ/80) = 1 us so we set divider 80 and count up */\r\n  timer = timerBegin(0, 80, true);\r\n\r\n  /* Attach onTimer function to our timer */\r\n  timerAttachInterrupt(timer, &onTimer, false);\r\n\r\n  /* Set alarm to call onTimer function every second 1 tick is 1us\r\n  => 1 second is 1000000us */\r\n  /* Repeat the alarm (third parameter) */\r\n  timerAlarmWrite(timer, 300000000, true); // set time interval to five minute\r\n\r\n  /* Start an alarm */\r\n  timerAlarmEnable(timer);\r\n  if(SerialDebug) Serial.println(\"start timer\");\r\n  \r\n} // end of setup\r\n\r\n\r\nvoid loop() \r\n{\r\n  /*ESP32 timer*/\r\n  if (alarmFlag) { // update RTC output at the alarm\r\n      alarmFlag = false;\r\n\r\n   /* APDS9253 Data Handling */\r\n   APDS9253.enable(); // enable APDS9253 sensor\r\n   while( !(APDS9253.getStatus() & 0x08) ) {}; // wait for data ready\r\n   APDS9253.getRGBiRdata(RGBiRData); // read light sensor data\r\n   APDS9253.disable(); // disable APDS9253 sensor\r\n \r\n   if(SerialDebug) {\r\n    Serial.print(\"Red raw counts = \");   Serial.println(RGBiRData[0]);\r\n    Serial.print(\"Green raw counts = \"); Serial.println(RGBiRData[1]);\r\n    Serial.print(\"Blue raw counts = \");  Serial.println(RGBiRData[2]);\r\n    Serial.print(\"IR raw counts = \");    Serial.println(RGBiRData[3]);\r\n    Serial.println(\"  \");\r\n\r\n    ambientLight = ((float) RGBiRData[1])*luxScale;\r\n    Serial.print(\"Red intensity = \");   Serial.print(((float) RGBiRData[0])*luxScale); Serial.println(\" lux\");\r\n    Serial.print(\"Green intensity = \"); Serial.print(((float) RGBiRData[1])*luxScale); Serial.println(\" lux\");\r\n    Serial.print(\"Blue intensity = \");  Serial.print(((float) RGBiRData[2])*luxScale); Serial.println(\" lux\");\r\n    Serial.print(\"IR intensity = \");    Serial.print(((float) RGBiRData[3])*luxScale); Serial.println(\" lux\");\r\n    Serial.println(\"  \");\r\n   }\r\n   \r\n   \r\n   /* HDC2010 data handling */\r\n   HDC2010.forcedMode(HDC2010_0_ADDRESS);\r\n   while( !(HDC2010.getIntStatus(HDC2010_0_ADDRESS) & 0x80) ) {}; // wait for HDC2010 data ready bit set\r\n    \r\n   rawHDCTemperature = HDC2010.getRawTemperature(HDC2010_0_ADDRESS); \r\n   HDCTemperature = ((float) rawHDCTemperature) * (165.0f/65536.0f) - 40.0f; // float degrees C, absolute accuracy +/- 0.2 C typical\r\n   rawHDCHumidity    = HDC2010.getRawHumidity(HDC2010_0_ADDRESS);          \r\n   HDCHumidity    = ((float) rawHDCHumidity) * (100.0f/65536.0f);   // float %rel humidity\r\n \r\n   if(SerialDebug) {\r\n     Serial.print(\"HDC2010 Temperature is \"); Serial.print(HDCTemperature, 2); Serial.println(\" degrees C\");\r\n     Serial.print(\"HDC2010 Humidity is \"); Serial.print(HDCHumidity, 2); Serial.println(\" %RH\");\r\n     Serial.println(\" \"); \r\n   }   /* end of HDC2010 data handling*/\r\n\r\n\r\n   /* LPS22HB data handling */\r\n//   LPS22HB.oneShot(); // baro data pretty jittery in one-shot mode, so run at 1 Hz instead...\r\n   while( !(LPS22HB.status() & 0x03) ) {}; // wait for pressure and temperature data ready\r\n\r\n   rawPressure = LPS22HB.readAltimeterPressure();\r\n   LPSPressure = (float) rawPressure/4096.0f;\r\n   LPSTemperature = (float) LPS22HB.readAltimeterTemperature()/100.0f; // absolute accuracy +/- 1.5 C\r\n   LPSAltitude = 145366.45f*(1.0f - pow((LPSPressure/1013.25f), 0.190284f)); \r\n\r\n   if(SerialDebug) {\r\n     Serial.print(\"Baro temperature = \"); Serial.print(LPSTemperature, 2); Serial.print(\" C\"); // temperature in degrees Celsius  \r\n     Serial.println(\" \");\r\n     Serial.print(\"Baro pressure = \"); Serial.print(LPSPressure, 2);  Serial.print(\" mbar\");// pressure in millibar\r\n     Serial.println(\" \");     \r\n     Serial.print(\"Altitude = \"); Serial.print(LPSAltitude, 2); Serial.println(\" ft\");\r\n     Serial.println(\" \");\r\n   }  /* end of LPS22HB interrupt handling */\r\n\r\n     \r\n   // get battery voltage\r\n   digitalWrite(myBatEn, HIGH);   \r\n   ADCCounts = adc1_get_raw((adc1_channel_t)1);\r\n   digitalWrite(myBatEn, LOW);   \r\n   /* ADC at attenuation 11 should rad from 0 to 2.6 V nominally. The resistor divider\r\n    *  is 1/2, and a calibration factor of 1.14 is applied to bring measurements into \r\n    *  agreement with multimeter */\r\n   VBat = 2.0f * 2.60f * 1.14f * ((float) ADCCounts) / 4095.0f;\r\n   if(SerialDebug) { \r\n    Serial.print(\"Battery voltage = \"); Serial.print(VBat, 2); Serial.println(\" V\");\r\n    Serial.print(\"Battery ADC Counts = \"); Serial.print(ADCCounts); Serial.println(\" Counts\"); Serial.println(\" \");\r\n   }\r\n\r\n\r\n   getLocalTime(&timeinfo);\r\n   if(SerialDebug) Serial.print(\"The current date/time in Danville, California is \");\r\n   if(SerialDebug) Serial.println(&timeinfo, \"%A, %B %d %Y %H:%M:%S\"); Serial.println(\" \");\r\n\r\n   // Capture time variables for logging https://www.cplusplus.com/reference/ctime/strftime/\r\n   char timeYear[3];\r\n   strftime(timeYear,3, \"%y\", &timeinfo);\r\n   uint8_t Year = (uint8_t)atoi(timeYear);\r\n   char timeMonth[3];\r\n   strftime(timeMonth,3, \"%m\", &timeinfo);\r\n   uint8_t Month = (uint8_t)atoi(timeMonth);\r\n   char timeDay[3];\r\n   strftime(timeDay,3, \"%d\", &timeinfo);\r\n   uint8_t Day = (uint8_t)atoi(timeDay);\r\n   char timeHour[3];\r\n   strftime(timeHour,3, \"%H\", &timeinfo);\r\n   uint8_t Hour = (uint8_t)atoi(timeHour); \r\n   char timeMinute[3];\r\n   strftime(timeMinute,3, \"%M\", &timeinfo);\r\n   uint8_t Minute = (uint8_t)atoi(timeMinute); \r\n   char timeSecond[3];\r\n   strftime(timeSecond,3, \"%S\", &timeinfo);\r\n   uint8_t Second = (uint8_t)atoi(timeSecond);\r\n\r\n\r\n    /* Log some data to the QSPI flash */\r\n    // Highest page number is 0x7FFF = 32767 for 64 Mbit flash\r\n    // store some data to the SPI flash\r\n    uint8_t bps = 23; // bytes per sector such that 256 bytes per page= sectors per page x bps = 11 x 23 <= 256\r\n      if(sector_number < 11 && page_number < 0x7FFF) {\r\n      \r\n      flashPage[sector_number*bps + 0]  = (rawHDCTemperature & 0xFF00) >> 8;   // raw HDC2010 Temperature\r\n      flashPage[sector_number*bps + 1]  = (rawHDCTemperature & 0x00FF); \r\n      flashPage[sector_number*bps + 2]  = (rawHDCHumidity & 0xFF00) >> 8;      // raw HDC2010 Humidity\r\n      flashPage[sector_number*bps + 3]  = (rawHDCHumidity & 0x00FF);\r\n      \r\n      flashPage[sector_number*bps + 4]  = (rawPressure & 0x00FF0000) >> 16;    // LPS22HB raw Pressure\r\n      flashPage[sector_number*bps + 5]  = (rawPressure & 0x0000FF00) >> 8; \r\n      flashPage[sector_number*bps + 6]  = (rawPressure & 0x000000FF);\r\n      \r\n      flashPage[sector_number*bps + 7]  = (RGBiRData[0] & 0xFF00) >> 8;        // APDS9253 RGBiR data, 16 bit or less \r\n      flashPage[sector_number*bps + 8]  = (RGBiRData[0] & 0x00FF);\r\n      flashPage[sector_number*bps + 9]  = (RGBiRData[1] & 0xFF00) >> 8;\r\n      flashPage[sector_number*bps + 10] = (RGBiRData[1] & 0x00FF);\r\n      flashPage[sector_number*bps + 11] = (RGBiRData[2] & 0xFF00) >> 8;\r\n      flashPage[sector_number*bps + 12] = (RGBiRData[2] & 0x00FF);\r\n      flashPage[sector_number*bps + 13] = (RGBiRData[3] & 0xFF00) >> 8;\r\n      flashPage[sector_number*bps + 14] = (RGBiRData[3] & 0x00FF);\r\n      \r\n      flashPage[sector_number*bps + 15] = (ADCCounts & 0xFF00) >> 8;           // raw VBAT from ADC\r\n      flashPage[sector_number*bps + 16] =  ADCCounts & 0x00FF;\r\n\r\n      flashPage[sector_number*bps + 17] =  Second;                            // Time and date\r\n      flashPage[sector_number*bps + 18] =  Minute;\r\n      flashPage[sector_number*bps + 19] =  Hour;\r\n      flashPage[sector_number*bps + 20] =  Day;\r\n      flashPage[sector_number*bps + 21] =  Month;\r\n      flashPage[sector_number*bps + 22] =  Year;\r\n\r\n      sector_number++;\r\n      }\r\n       \r\n      if(sector_number == 11 && page_number < 0x7FFF)\r\n      {\r\n          SPIFlash.powerUp();\r\n          SPIFlash.flash_page_program(flashPage, page_number);\r\n          if(SerialDebug) {Serial.print(\"***Wrote flash page: \"); Serial.println(page_number);}\r\n          digitalWrite(myLed, LOW); delay(1); digitalWrite(myLed, HIGH); // indicate when flash page is written\r\n          sector_number = 0;\r\n          page_number++;\r\n          SPIFlash.powerDown();  // Put SPI flash into power down mode\r\n      }  \r\n      else if(page_number >= 0x7FFF) \r\n      {\r\n       if(SerialDebug){Serial.println(\"Reached last page of SPI flash!\"); Serial.println(\"Data logging stopped!\");}\r\n      }\r\n  \r\n     digitalWrite(myLed, LOW); delay(1); digitalWrite(myLed, HIGH);\r\n   } // end of time alarm section\r\n\r\n     delay(1);    // wait for alarm\r\n     \r\n} // end of main loop\r\n\r\n\r\n/* Useful functions */\r\n\r\nvoid alarmMatch()\r\n{\r\n  alarmFlag = true;\r\n}\r\n\r\n\r\nvoid  I2CscanDevices() \r\n{\r\n  // Scan for i2c devices\r\n  uint8_t error, address, nDevices;\r\n\r\n  Serial.println(\"Scanning for I2C Devices ...\");\r\n\r\n  nDevices = 0;\r\n  for(address = 1; address < 127; address++ ) \r\n  {\r\n    // The i2c_scanner uses the return value of the Wire.endTransmisstion to see if a device did acknowledge to the address.\r\n    error = i2c_0.pollAddress(address);\r\n\r\n    if (error == 0)\r\n    {\r\n      Serial.print(\"I2C device found at address 0x\");\r\n      if (address<16) \r\n      Serial.print(\"0\");\r\n      Serial.print(address,HEX);\r\n      Serial.println(\"  !\");\r\n      nDevices++;\r\n    }\r\n    else if (error==4) \r\n    {\r\n      Serial.print(\"Unknown error at address 0x\");\r\n      if (address<16) \r\n        Serial.print(\"0\");\r\n        Serial.println(address,HEX);\r\n    }    \r\n  }\r\n  if (nDevices == 0)\r\n    Serial.println(\"No I2C devices found\\n\");\r\n  else\r\n    Serial.println(\"I2C scan complete\\n\");\r\n}\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/HDC2010.cpp",
    "content": "/*\r\n * Copyright (c) 2021 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#include \"HDC2010.h\"\r\n\r\n  HDC2010::HDC2010(I2Cdev* i2c_bus){\r\n  _i2c_bus = i2c_bus;\r\n  }\r\n\r\n\r\n  void HDC2010::reset(uint8_t HDC2010_ADDRESS) {\r\n  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_INT_STATUS, 0x80);  // soft reset device\r\n  delay(1);\r\n  }\r\n\r\n\r\n  uint16_t HDC2010::getDevID(uint8_t HDC2010_ADDRESS)\r\n  {\r\n  uint8_t rawData[2] = {0, 0};\r\n  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_DEV_ID_L); // read Dev ID LSByte\r\n  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_DEV_ID_H); // read Dev ID HSByte\r\n  uint16_t devID = ( (uint16_t) rawData[1] << 8) | rawData[0];\r\n  return devID;\r\n  }\r\n\r\n\r\n  uint16_t HDC2010::getManuID(uint8_t HDC2010_ADDRESS)\r\n  {\r\n  uint8_t rawData[2] = {0, 0};\r\n  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_MANU_ID_L); // read Dev ID LSByte\r\n  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_MANU_ID_H); // read Dev ID HSByte\r\n  uint16_t manuID = ( (uint16_t) rawData[1] << 8) | rawData[0];\r\n  return manuID;\r\n  }\r\n\r\n\r\n  uint8_t HDC2010::getIntStatus(uint8_t HDC2010_ADDRESS)\r\n  {\r\n  uint8_t c  = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_INT_STATUS); // read int status register\r\n  return c;\r\n  }\r\n  \r\n\r\n  void HDC2010::init(uint8_t HDC2010_ADDRESS, uint8_t hres, uint8_t tres, uint8_t freq)\r\n  {\r\n  // set sample frequency (bits 6 - 4), enable interrupt (bit 2 = 1), active HIGH (bit 1 = 1)\r\n  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_CONFIG1, freq << 4 | 0x04 | 0x02);  \r\n  // set temperature resolution (bits 7:6), set humidity resolution (bits 5:4), measure both H and T\r\n  // start measurements by writing 1 to bit 0\r\n  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_CONFIG2, tres << 6 | hres << 4 | 0x01);  \r\n  // enable data ready interrupt (bit 7), can enable interrupt on thresholds here too\r\n  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_INT_EN, 0x80); \r\n  }\r\n\r\n\r\n  float HDC2010::getTemperature(uint8_t HDC2010_ADDRESS)\r\n  {\r\n  uint8_t rawData[2] = {0, 0};\r\n  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_TEMP_L); // read Temp LSByte\r\n  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_TEMP_H); // read Temp HSByte\r\n  uint16_t temp = (uint16_t) ( ((uint16_t) rawData[1] << 8 ) | rawData[0]);\r\n  float out = ((float) temp) * (165.0f/65536.0f) - 40.0f;\r\n  return out;\r\n  }\r\n\r\n\r\n  float HDC2010::getHumidity(uint8_t HDC2010_ADDRESS)\r\n  {\r\n  uint8_t rawData[2] = {0, 0};\r\n  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_HUM_L); // read Temp LSByte\r\n  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_HUM_H); // read Temp HSByte\r\n  uint16_t temp = (uint16_t) ( ((uint16_t) rawData[1] << 8 ) | rawData[0]);\r\n  float out = ((float) temp) * (100.0f/65536.0f);\r\n  return out;\r\n  }\r\n\r\n  uint16_t HDC2010::getRawTemperature(uint8_t HDC2010_ADDRESS)\r\n  {\r\n  uint8_t rawData[2] = {0, 0};\r\n  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_TEMP_L); // read Temp LSByte\r\n  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_TEMP_H); // read Temp HSByte\r\n  uint16_t temp = (uint16_t) ( ((uint16_t) rawData[1] << 8 ) | rawData[0]);\r\n  return temp;\r\n  }\r\n\r\n\r\n  uint16_t HDC2010::getRawHumidity(uint8_t HDC2010_ADDRESS)\r\n  {\r\n  uint8_t rawData[2] = {0, 0};\r\n  rawData[0] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_HUM_L); // read Temp LSByte\r\n  rawData[1] = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_HUM_H); // read Temp HSByte\r\n  uint16_t temp = (uint16_t) ( ((uint16_t) rawData[1] << 8 ) | rawData[0]);\r\n  return temp;\r\n  }\r\n\r\n\r\n  void HDC2010::forcedMode(uint8_t HDC2010_ADDRESS)\r\n  {\r\n  uint8_t temp = _i2c_bus->readByte(HDC2010_ADDRESS, HDC2010_CONFIG2);\r\n  _i2c_bus->writeByte(HDC2010_ADDRESS, HDC2010_CONFIG2, temp | 0x01);   //start measurement, self clearing when done\r\n  }\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/HDC2010.h",
    "content": "/*\r\n * Copyright (c) 2021 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n  \r\n#ifndef HDC2010_h\r\n#define HDC2010_h\r\n\r\n#include \"Arduino.h\"\r\n#include <Wire.h>\r\n#include \"I2Cdev.h\"\r\n\r\n// http://www.ti.com/lit/ds/symlink/hdc2010.pdf\r\n// HDC2010 Registers\r\n#define HDC2010_TEMP_L              0x00\r\n#define HDC2010_TEMP_H              0x01 \r\n#define HDC2010_HUM_L               0x02\r\n#define HDC2010_HUM_H               0x03\r\n#define HDC2010_INT_STATUS          0x04\r\n#define HDC2010_TEMP_MAX            0x05\r\n#define HDC2010_HUM_MAX             0x06\r\n#define HDC2010_INT_EN              0x07\r\n#define HDC2010_TEMP_OFFSET         0x08\r\n#define HDC2010_HUM_OFFSET          0x09\r\n#define HDC2010_TEMP_THR_L          0x0A\r\n#define HDC2010_TEMP_THR_H          0x0B\r\n#define HDC2010_RH_THR_L            0x0C\r\n#define HDC2010_RH_THR_H            0x0D\r\n#define HDC2010_CONFIG1             0x0E\r\n#define HDC2010_CONFIG2             0x0F\r\n#define HDC2010_MANU_ID_L           0xFC\r\n#define HDC2010_MANU_ID_H           0xFD\r\n#define HDC2010_DEV_ID_L            0xFE\r\n#define HDC2010_DEV_ID_H            0xFF\r\n\r\n// I2C address  \r\n#define HDC2010_0_ADDRESS 0x40 // SA0 = 0\r\n#define HDC2010_1_ADDRESS 0x41 // SA0 = 1\r\n\r\n// Measurement Frequency (inverse sample rate)\r\n#define ForceMode    0x00\r\n#define Freq_120s    0x01\r\n#define Freq_60s     0x02\r\n#define Freq_10s     0x03\r\n#define Freq_5s      0x04\r\n#define Freq_1s      0x05 // 1 Hz\r\n#define Freq_0_5s    0x06 // 2 Hz\r\n#define Freq_0_2s    0x07 // 5 Hz\r\n\r\n// Temperature resolution\r\n#define TRES_14bit    0x00\r\n#define TRES_11bit    0x01\r\n#define TRES_9bit     0x02\r\n\r\n// Humidity resolution\r\n#define HRES_14bit    0x00\r\n#define HRES_11bit    0x01\r\n#define HRES_9bit     0x02\r\n \r\n\r\n\r\nclass HDC2010\r\n{\r\n  public: \r\n  HDC2010(I2Cdev* i2c_bus);\r\n  void reset(uint8_t HDC2010_ADDRESS);\r\n  uint16_t getDevID(uint8_t HDC2010_ADDRESS);\r\n  uint16_t getManuID(uint8_t HDC2010_ADDRESS);\r\n  void init(uint8_t HDC2010_ADDRESS, uint8_t hres, uint8_t tres, uint8_t freq);  \r\n  uint8_t getIntStatus(uint8_t HDC2010_ADDRESS);\r\n  void heaterOn(uint8_t HDC2010_ADDRESS);\r\n  void heaterOff(uint8_t HDC2010_ADDRESS);\r\n  float getTemperature(uint8_t HDC2010_ADDRESS);\r\n  float getHumidity(uint8_t HDC2010_ADDRESS);\r\n  uint16_t getRawTemperature(uint8_t HDC2010_ADDRESS);\r\n  uint16_t getRawHumidity(uint8_t HDC2010_ADDRESS);\r\n  void forcedMode(uint8_t HDC2010_ADDRESS);\r\n  \r\n  private:\r\n  // Register read variables\r\n  I2Cdev* _i2c_bus;\r\n  };\r\n\r\n#endif\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/I2Cdev.cpp",
    "content": "/*\r\n * Copyright (c) 2018 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#include \"Arduino.h\"\r\n#include \"I2Cdev.h\"\r\n\r\n\r\nI2Cdev::I2Cdev(TwoWire* i2c_bus)                                                                                                             // Class constructor\r\n{\r\n  _i2c_bus = i2c_bus;\r\n}\r\n\r\nI2Cdev::~I2Cdev()                                                                                                                            // Class destructor\r\n{\r\n}\r\n\r\n/**\r\n* @fn: readByte(uint8_t address, uint8_t subAddress)\r\n*\r\n* @brief: Read one byte from an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress\r\n* @returns: unsigned short read\r\n*/\r\nuint8_t I2Cdev::readByte(uint8_t address, uint8_t subAddress)\r\n{\r\n  uint8_t data = 0;                             // `data` will store the register data   \r\n  _i2c_bus->beginTransmission(address);         // Initialize the Tx buffer\r\n  _i2c_bus->write(subAddress);                  // Put slave register address in Tx buffer\r\n  _i2c_bus->endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\r\n  _i2c_bus->requestFrom(address, 1);            // Read one byte from slave register address  \r\n  data = _i2c_bus->read();                      // Fill Rx buffer with result\r\n  return data;                                  // Return data read from slave register\r\n  \r\n}\r\n\r\n\r\n/**\r\n* @fn: readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\r\n*\r\n* @brief: Read multiple bytes from an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, number of btes to be read, aray to store the read data\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\r\n{  \r\n  _i2c_bus->beginTransmission(address);   // Initialize the Tx buffer\r\n  _i2c_bus->write(subAddress);            // Put slave register address in Tx buffer\r\n  _i2c_bus->endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive\r\n  uint8_t i = 0;\r\n  _i2c_bus->requestFrom(address, count);  // Read bytes from slave register address \r\n  while (_i2c_bus->available()) {\r\n        dest[i++] = _i2c_bus->read(); }   // Put read results in the Rx buffer\r\n}\r\n\r\n\r\n/**\r\n* @fn: writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n*\r\n* @brief: Write one byte to an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, data to be written\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n{\r\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\r\n  _i2c_bus->write(regAddr);           // Put slave register address in Tx buffer\r\n  _i2c_bus->write(data);                 // Put data in Tx buffer\r\n  _i2c_bus->endTransmission();           // Send the Tx buffer\r\n}\r\n\r\n\r\n/**\r\n* @fn: writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t data)\r\n*\r\n* @brief: Write multiple bytes to an I2C device\r\n* \r\n* @params: I2C slave device address, Register subAddress, byte count, data array to be written\r\n* @returns: void\r\n*/\r\nvoid I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest)\r\n{\r\n  uint8_t temp[1 + count];\r\n  \r\n  temp[0] = regAddr;\r\n  for (uint8_t ii = 0; ii < count; ii++)\r\n  { \r\n    temp[ii + 1] = dest[ii];\r\n  }\r\n  \r\n  _i2c_bus->beginTransmission(devAddr);  // Initialize the Tx buffer\r\n  \r\n  for (uint8_t jj = 0; jj < count + 1; jj++)\r\n  {\r\n  _i2c_bus->write(temp[jj]);            // Put data in Tx buffer\r\n  }\r\n  \r\n  _i2c_bus->endTransmission();           // Send the Tx buffer\r\n}\r\n\r\n\r\n\r\nuint8_t I2Cdev::pollAddress(uint8_t address)\r\n{\r\n    _i2c_bus->beginTransmission(address);\r\n    uint8_t error = _i2c_bus->endTransmission();\r\n    return error;\r\n}\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/I2Cdev.h",
    "content": "/*\r\n * Copyright (c) 2018 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#ifndef _I2CDEV_H_\r\n#define _I2CDEV_H_\r\n\r\n#include <Wire.h>\r\n\r\nclass I2Cdev {\r\n    public:\r\n                                        I2Cdev(TwoWire*);\r\n                                        ~I2Cdev();                                                                                                                     // Class destructor for durable instances\r\n         uint8_t                        readByte(uint8_t address, uint8_t subAddress);\r\n         void                           readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);\r\n         void                           writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);\r\n         void                           writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t count, uint8_t *dest);\r\n         uint8_t                        pollAddress(uint8_t address);\r\n    private:\r\n         TwoWire*                       _i2c_bus;                                                                                                                      // Class constructor argument\r\n};\r\n\r\n#endif //_I2CDEV_H_\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/LPS22HB.cpp",
    "content": "/*\r\n * Copyright (c) 2021 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#include \"LPS22HB.h\"\r\n#include \"I2Cdev.h\"\r\n\r\nLPS22HB::LPS22HB(I2Cdev* i2c_bus)\r\n{\r\n  _i2c_bus = i2c_bus;\r\n}\r\n\r\nuint8_t LPS22HB::getChipID()\r\n{\r\n  // Read the WHO_AM_I register of the altimeter this is a good test of communication\r\n  uint8_t temp = _i2c_bus->readByte(LPS22HB_ADDRESS, LPS22HB_WHOAMI);  // Read WHO_AM_I register for LPS22HB\r\n  return temp;\r\n}\r\n\r\n\r\nvoid LPS22HB::reset()\r\n{\r\n _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_CTRL_REG2, 0x04);  // Reset LPS22HB\r\n}\r\n\r\n\r\nuint8_t LPS22HB::status()\r\n{\r\n  // Read the status register of the altimeter  \r\n  uint8_t temp = _i2c_bus->readByte(LPS22HB_ADDRESS, LPS22HB_STATUS);   \r\n  return temp;\r\n}\r\n\r\n\r\nuint8_t LPS22HB::intSource()\r\n{\r\n  // Read the status register of the altimeter  \r\n  uint8_t temp = _i2c_bus->readByte(LPS22HB_ADDRESS, LPS22HB_INT_SOURCE);   \r\n  return temp;\r\n}\r\n\r\n\r\nvoid LPS22HB::Init(uint8_t PODR)\r\n{\r\n  // Before device is powered up via CTRL_REG1 setting of PODR, \r\n  // set low current (~3 uA @ 1 Hz) or low noise mode (default) (~12 uA @ 1 Hz)\r\n  /* uncomment next two lines for low current mode */\r\n    uint8_t temp = _i2c_bus->readByte(LPS22HB_ADDRESS, LPS22HB_RES_CONF);   \r\n   _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_RES_CONF, temp | 0x01);  // write 1 to bit 0 to enable low current mode\r\n    \r\n  // set sample rate by setting bits 6:4 \r\n  // enable low-pass filter by setting bit 3 to one\r\n  // bit 2 == 0 means bandwidth is odr/9, bit 2 == 1 means bandwidth is odr/20\r\n  // make sure data not updated during read by setting block data update (bit 1) to 1\r\n    _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_CTRL_REG1, PODR << 4 | 0x08 | 0x02); \r\n    _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_CTRL_REG2, 0x10); // enable auto increment\r\n\r\n   // interrupt configuration\r\n    _i2c_bus->writeByte(LPS22HB_ADDRESS, LPS22HB_CTRL_REG3, 0x04);  // enable data ready interrupt\r\n}    \r\n\r\n\r\nint32_t LPS22HB::readAltimeterPressure()\r\n{\r\n    uint8_t rawData[3];  // 24-bit pressure register data stored here\r\n    _i2c_bus->readBytes(LPS22HB_ADDRESS, (LPS22HB_PRESS_OUT_XL), 3, &rawData[0]);  \r\n    return (int32_t) ((int32_t) rawData[2] << 16 | (int32_t) rawData[1] << 8 | rawData[0]);\r\n}\r\n\r\n\r\nint16_t LPS22HB::readAltimeterTemperature()\r\n{\r\n    uint8_t rawData[2];  // 16-bit pressure register data stored here\r\n    _i2c_bus->readBytes(LPS22HB_ADDRESS, (LPS22HB_TEMP_OUT_L), 2, &rawData[0]);  \r\n    return (int16_t)((int16_t) rawData[1] << 8 | rawData[0]);\r\n}\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/LPS22HB.h",
    "content": "/*\r\n * Copyright (c) 2021 Tlera Corp.  All rights reserved.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to\r\n * deal with the Software without restriction, including without limitation the\r\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\r\n * sell copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n *  1. Redistributions of source code must retain the above copyright notice,\r\n *     this list of conditions and the following disclaimers.\r\n *  2. Redistributions in binary form must reproduce the above copyright\r\n *     notice, this list of conditions and the following disclaimers in the\r\n *     documentation and/or other materials provided with the distribution.\r\n *  3. Neither the name of Tlera Corp, nor the names of its contributors\r\n *     may be used to endorse or promote products derived from this Software\r\n *     without specific prior written permission.\r\n *\r\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE\r\n * CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\r\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\r\n * WITH THE SOFTWARE.\r\n */\r\n\r\n#ifndef LPS22HB_h\r\n#define LPS22HB_h\r\n\r\n#include \"Arduino.h\"\r\n#include <Wire.h>\r\n#include \"I2Cdev.h\"\r\n\r\n// See LPS22HB \"MEMS pressure sensor: 260-1260 hPa absolute digital output barometer\" Data Sheet\r\n//file:///C:/Users/kris/Documents/Arduino/libraries/LPS22HB.pdf\r\n#define LPS22HB_INTERRUPT_CFG 0x0B\r\n#define LPS22HB_THS_P_L       0x0C\r\n#define LPS22HB_THS_P_H       0x0D\r\n#define LPS22HB_WHOAMI        0x0F // should return 0xB1\r\n#define LPS22HB_CTRL_REG1     0x10\r\n#define LPS22HB_CTRL_REG2     0x11\r\n#define LPS22HB_CTRL_REG3     0x12\r\n#define LPS22HB_FIFO_CTRL     0x14\r\n#define LPS22HB_REF_P_XL      0x15\r\n#define LPS22HB_REF_P_L       0x16\r\n#define LPS22HB_REF_P_H       0x17\r\n#define LPS22HB_RPDS_L        0x18\r\n#define LPS22HB_RPDS_H        0x19\r\n#define LPS22HB_RES_CONF      0x1A\r\n#define LPS22HB_INT_SOURCE    0x25\r\n#define LPS22HB_FIFO_STATUS   0x26\r\n#define LPS22HB_STATUS        0x27\r\n#define LPS22HB_PRESS_OUT_XL  0x28\r\n#define LPS22HB_PRESS_OUT_L   0x29\r\n#define LPS22HB_PRESS_OUT_H   0x2A\r\n#define LPS22HB_TEMP_OUT_L    0x2B\r\n#define LPS22HB_TEMP_OUT_H    0x2C\r\n#define LPS22HB_LPFP_RES      0x33\r\n\r\n#define LPS22HB_ADDRESS 0x5C   // Address of altimeter\r\n\r\n// Altimeter output data rate\r\n#define    P_1shot  0x00;\r\n#define    P_1Hz    0x01;\r\n#define    P_10Hz   0x02;\r\n#define    P_25Hz   0x03;  // 25 Hz output data rate\r\n#define    P_50Hz   0x04;\r\n#define    P_75Hz   0x05;\r\n\r\nclass LPS22HB\r\n{\r\n  public: \r\n  LPS22HB(I2Cdev* i2c_bus);\r\n  void Init(uint8_t PODR);\r\n  uint8_t getChipID();\r\n  uint8_t status();\r\n  uint8_t intSource();\r\n  void reset();\r\n  int32_t readAltimeterPressure();\r\n  int16_t readAltimeterTemperature();\r\n  private:\r\n  I2Cdev* _i2c_bus;\r\n};\r\n\r\n#endif\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/SPIFlash.cpp",
    "content": "/* SPIFlash.cpp\r\nSketch by Kris Winer December 16. 2016\r\n\r\nLicense: Use this sketch any way you choose; if you like it, buy me a beer sometime\r\n\r\nPurpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4\r\nDragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.\r\n\r\nSketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency\r\nand maximum speed.\r\n\r\nSketch based on the work of Pete (El Supremo) as follows:\r\n * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca\r\n *\r\n * Development of this audio library was funded by PJRC.COM, LLC by sales of\r\n * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop\r\n * open source software by purchasing Teensy or other PJRC products.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to deal\r\n * in the Software without restriction, including without limitation the rights\r\n * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r\n * copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n * The above copyright notice, development funding notice, and this permission\r\n * notice shall be included in all copies or substantial portions of the Software.\r\n */\r\n\r\n#include \"SPIFlash.h\"\r\n#include \"SPI.h\"\r\n\r\n\r\nSPIFlash::SPIFlash(uint8_t CSPIN)\r\n{\r\n  _csPin = CSPIN; \r\n}\r\n\r\n\r\nvoid SPIFlash::init(const uint8_t SCK, const uint8_t MISO, const uint8_t MOSI, const uint8_t SS)\r\n{\r\n  SPI.begin(SCK, MISO, MOSI, SS);\r\n  delay(20);\r\n}\r\n\r\n\r\nvoid SPIFlash::getChipID(uint8_t * dest)\r\n{ \r\n//  uint16_t id[3];\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(0x9F);\r\n  dest[0] = SPI.transfer(0);\r\n  dest[1] = SPI.transfer(0);\r\n  dest[2] = SPI.transfer(0);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\nvoid SPIFlash::powerDown()\r\n{\r\n  write_pause();\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_POWER_DOWN);\r\n  digitalWrite(_csPin, HIGH);\r\n  delayMicroseconds(10);\r\n  SPI.endTransaction();\r\n}\r\n\r\n\r\nvoid SPIFlash::powerUp()\r\n{\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  delayMicroseconds(1);\r\n  SPI.transfer(CMD_RELEASE_POWER_DOWN);\r\n  digitalWrite(_csPin, HIGH);\r\n  delayMicroseconds(35);\r\n  SPI.endTransaction();\r\n}\r\n\r\n\r\nvoid SPIFlash::write_pause(void)\r\n{\r\n  if(flash_wait_for_write) {\r\n    while(flash_read_status() & STAT_WIP){\r\n      delayMicroseconds(10);\r\n    }\r\n    flash_wait_for_write = 0;\r\n  }\r\n}\r\n\r\n\r\n//=====================================\r\n// convert a page number to a 24-bit address\r\nint SPIFlash::page_to_address(int pn)\r\n{\r\n  return(pn << 8);\r\n}\r\n\r\n//=====================================\r\n// convert a 24-bit address to a page number\r\nint SPIFlash::address_to_page(int addr)\r\n{\r\n  return(addr >> 8);\r\n}\r\n\r\n//=====================================\r\nvoid SPIFlash::flash_read_id(unsigned char *idt)\r\n{\r\n  write_pause();\r\n  //set control register \r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_READ_ID);\r\n  for(uint16_t i = 0; i < 20; i++) {\r\n    *idt++ = SPI.transfer(0x00);\r\n  }\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\n//=====================================\r\nunsigned char SPIFlash::flash_read_status(void)\r\n{\r\n  unsigned char c;\r\n\r\n// This can't do a write_pause\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);  \r\n  SPI.transfer(CMD_READ_STATUS_REG);\r\n  c = SPI.transfer(0x00);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  return(c);\r\n}\r\n\r\n//=====================================\r\n\r\nvoid SPIFlash::flash_hard_reset(void)\r\n{\r\n  // Make sure that the device is not busy before\r\n  // doing the hard reset sequence\r\n  // At the moment this does NOT check the\r\n  // SUSpend status bit in Status Register 2\r\n  // but the library does not support suspend\r\n  // mode yet anyway\r\n  write_pause();\r\n  \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_RESET_DEVICE );\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  delayMicroseconds(50);\r\n  // Wait for the hard reset to finish\r\n  // Don't use flash_wait_for_write here\r\n  while(flash_read_status() & STAT_WIP);\r\n  // The spec says \"the device will take\r\n  // approximately tRST=30 microseconds\r\n  // to reset\"\r\n}\r\n\r\n//=====================================\r\nvoid SPIFlash::flash_chip_erase(boolean wait)\r\n{\r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_CHIP_ERASE);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  flash_wait_for_write = 1;\r\n  if(wait)write_pause();\r\n}\r\n\r\n//=====================================\r\n// Tse Typ=0.6sec Max=3sec\r\n// measured 549.024ms\r\n// Erase the sector which contains the specified\r\n// page number.\r\n// The smallest unit of memory which can be erased\r\n// is the 4kB sector (which is 16 pages)\r\nvoid SPIFlash::flash_erase_pages_sector(int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause(); \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  \r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_SECTOR_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xff);\r\n  SPI.transfer((address >> 8) & 0xff);\r\n  SPI.transfer(address & 0xff);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();  \r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\n// Erase the 32kb block which contains the specified\r\n// page number.\r\nvoid SPIFlash::flash_erase_pages_block32k(int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_BLOCK32K_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\n// Erase the 64kb block which contains the specified\r\n// page number.\r\nvoid SPIFlash::flash_erase_pages_block64k(int pn)\r\n{\r\n  int address;\r\n  \r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_BLOCK64K_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\nvoid SPIFlash::flash_page_program(unsigned char *wp,int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause(); \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  \r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_PAGE_PROGRAM);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // Now write 256 bytes to the page\r\n  for(uint16_t i = 0; i < 256; i++) {\r\n  SPI.transfer(*wp++);\r\n  }\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\nvoid SPIFlash::flash_read_pages(unsigned char *p,int pn,const int n_pages)\r\n{\r\n  int address;\r\n  unsigned char *rp = p;\r\n  \r\n  write_pause();\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_READ_DATA);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // Now read the page's data bytes\r\n  for(uint16_t i = 0; i < n_pages * 256; i++) {\r\n    *rp++ = SPI.transfer(0);\r\n  }\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\n//=====================================\r\n// Read specified number of pages starting with pn\r\nvoid SPIFlash::flash_fast_read_pages(unsigned char *p,int pn,const int n_pages)\r\n{\r\n  int address;\r\n  unsigned char *rp = p;\r\n  \r\n  write_pause();\r\n// The chip doesn't run at the higher clock speed until\r\n// after the command and address have been sent\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_READ_HIGH_SPEED);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // send dummy byte\r\n  SPI.transfer(0);\r\n  // Now read the number of pages required\r\n  for(uint16_t i = 0; i < n_pages * 256; i++) {\r\n    *rp++ = SPI.transfer(0);\r\n  }\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/ESP32C3Mini_EnvSensor.v02b/SPIFlash.h",
    "content": "/* SPIFlash.h\r\nSketch by Kris Winer December 16. 2016\r\n\r\nLicense: Use this sketch any way you choose; if you like it, buy me a beer sometime\r\n\r\nPurpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4\r\nDragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.\r\n\r\nSketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency\r\nand maximum speed.\r\n\r\nSketch based on the work of Pete (El Supremo) as follows:\r\n * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca\r\n *\r\n * Development of this audio library was funded by PJRC.COM, LLC by sales of\r\n * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop\r\n * open source software by purchasing Teensy or other PJRC products.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to deal\r\n * in the Software without restriction, including without limitation the rights\r\n * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r\n * copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n * The above copyright notice, development funding notice, and this permission\r\n * notice shall be included in all copies or substantial portions of the Software.\r\n */\r\n\r\n#ifndef SPIFlash_h\r\n#define SPIFlash_h\r\n\r\n#include \"Arduino.h\"\r\n#include \"SPI.h\"\r\n\r\n#define STAT_WIP 1\r\n#define STAT_WEL 2\r\n\r\n#define CMD_WRITE_STATUS_REG   0x01\r\n#define CMD_PAGE_PROGRAM       0x02\r\n#define CMD_READ_DATA          0x03\r\n#define CMD_WRITE_DISABLE      0x04 \r\n#define CMD_READ_STATUS_REG    0x05\r\n#define CMD_WRITE_ENABLE       0x06\r\n#define CMD_READ_HIGH_SPEED    0x0B \r\n#define CMD_SECTOR_ERASE       0x20 \r\n#define CMD_BLOCK32K_ERASE     0x52 \r\n#define CMD_RESET_DEVICE       0xF0 \r\n#define CMD_READ_ID            0x9F\r\n#define CMD_RELEASE_POWER_DOWN 0xAB \r\n#define CMD_POWER_DOWN         0xB9 \r\n#define CMD_CHIP_ERASE         0xC7\r\n#define CMD_BLOCK64K_ERASE     0xD8 \r\n\r\n#define clkSpeed 10000000  // 10 MHz\r\n\r\n\r\nclass SPIFlash\r\n{\r\n  public: \r\n  SPIFlash(uint8_t CSPIN);\r\n  void init(const uint8_t SCK, const uint8_t MISO, const uint8_t MOSI, const uint8_t SS);\r\n  void getChipID(uint8_t * dest);\r\n  void powerDown();\r\n  void powerUp();\r\n  void write_pause(void);\r\n  int page_to_address(int pn);\r\n  int address_to_page(int addr);\r\n  void flash_read_id(unsigned char *idt);\r\n  unsigned char flash_read_status(void);\r\n  void flash_hard_reset(void);\r\n  void flash_chip_erase(boolean wait);\r\n  void flash_erase_pages_sector(int pn);\r\n  void flash_erase_pages_block32k(int pn);\r\n  void flash_erase_pages_block64k(int pn);\r\n  void flash_page_program(unsigned char *wp,int pn);\r\n  void flash_read_pages(unsigned char *p,int pn,const int n_pages);\r\n  void flash_fast_read_pages(unsigned char *p,int pn,const int n_pages);\r\n  private:\r\n  uint8_t   _csPin;\r\n  unsigned char flash_wait_for_write = 0;\r\n\r\n};\r\n\r\n#endif\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/Readme.md",
    "content": "**ESP32C3Mini-hosted environmental data logger**\n\nThe 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. \n\nThe set of I2C sensors includes the APDS9253 RGBiR light sensor, HDC2010 humidity and temperature sensor, and LPS22HB barometer.\n\nRather 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. \n\nIn 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. \n\nThe 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.\n\nI 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.\n\nThe 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.\n\nSo 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.\n\nI 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).\n\nSo 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.\n\nIt 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.\n\nI 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.\n\nLastly, some data from an overnight logging test run:\n\n![ESP32C3MiniEnvLogTest3](https://user-images.githubusercontent.com/6698410/166608157-96e9a205-15b8-46f6-a29f-296c916ab96c.jpg)\n\nYou 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...\n\n![ESP32C3Mini top](https://user-images.githubusercontent.com/6698410/166591280-3111662b-efe1-49bb-904c-abd950bf572f.jpg)\n![ESP32C3Mini_bottom](https://user-images.githubusercontent.com/6698410/166591298-9c89f85a-87d2-4b78-b5d7-5e32c969c563.jpg)\n\nProject pcb EAGLE CAD design files available in the OSH Park shared space [here.](https://oshpark.com/shared_projects/6YSyYfg9)\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/SPIFlash_ESP32C3Mini/SPIFlash_ESP32C3Mini.ino",
    "content": "/* SPIFlash_ESP32C3Mini SPIFlash Test\r\nSketch by Kris Winer December 16. 2016\r\n\r\nLicense: Use this sketch any way you choose; if you like it, buy me a beer sometime\r\n\r\nPurpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4\r\nDragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.\r\n\r\nSketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency\r\nand maximum speed.\r\n\r\nSketch based on the work of Pete (El Supremo) as follows:\r\n * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca\r\n *\r\n * Development of this audio library was funded by PJRC.COM, LLC by sales of\r\n * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop\r\n * open source software by purchasing Teensy or other PJRC products.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to deal\r\n * in the Software without restriction, including without limitation the rights\r\n * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r\n * copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n * The above copyright notice, development funding notice, and this permission\r\n * notice shall be included in all copies or substantial portions of the Software.\r\n */\r\n\r\n#include <SPI.h>\r\n\r\n#define Serial USBSerial\r\n\r\n// Highest page number is 0xFFFF = 65535 for 128 Mbit flash\r\n// Highest page number is 0x0EFF =  4095 for   8 Mbit flash\r\nint page_number = 0x0EFF;\r\nunsigned char w_page[256];\r\nunsigned char r_page[256];\r\n\r\nstatic const int spiClk = 10000000; // 10 MHz, integer multiples of 80\r\n\r\n#define CSPIN  4   \r\n\r\n#define STAT_WIP 1\r\n#define STAT_WEL 2\r\n\r\n#define CMD_WRITE_STATUS_REG   0x01\r\n#define CMD_PAGE_PROGRAM       0x02\r\n#define CMD_READ_DATA          0x03\r\n#define CMD_WRITE_DISABLE      0x04//not tested\r\n#define CMD_READ_STATUS_REG    0x05\r\n#define CMD_WRITE_ENABLE       0x06\r\n#define CMD_READ_HIGH_SPEED    0x0B//not tested\r\n#define CMD_SECTOR_ERASE       0x20//not tested\r\n#define CMD_BLOCK32K_ERASE     0x52//not tested\r\n#define CMD_RESET_DEVICE       0xF0//<<-different from winbond\r\n#define CMD_READ_ID            0x9F\r\n#define CMD_RELEASE_POWER_DOWN 0xAB//not tested\r\n#define CMD_POWER_DOWN         0xB9//not tested\r\n#define CMD_CHIP_ERASE         0xC7\r\n#define CMD_BLOCK64K_ERASE     0xD8//not tested\r\n\r\nunsigned char flash_wait_for_write = 0;\r\n\r\nvoid setup(void)\r\n{\r\n  pinMode(CSPIN, OUTPUT);\r\n  digitalWrite(CSPIN, HIGH);\r\n\r\n  SPI.begin(6, 7, 5, 4); // sck, miso, mosi, ss (ss can be any GPIO)\r\n  \r\n  unsigned char id_tab[32];\r\n  unsigned long t_start;\r\n  \r\n  Serial.begin(115200);\r\n  delay(4000);\r\n  Serial.println(\"Serial enabled!\");\r\n\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_RELEASE_POWER_DOWN);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  delay(100);\r\n\r\n  Serial.print(\"ID bytes: \");\r\n  uint16_t id[3];\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(0x9F);\r\n  id[0] = SPI.transfer(0);\r\n  id[1] = SPI.transfer(0);\r\n  id[2] = SPI.transfer(0);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  Serial.print(id[0], HEX); Serial.print(\" \"); Serial.print(id[1], HEX);  Serial.print(\" \");  Serial.println(id[2], HEX); \r\n\r\n  Serial.println(\"Winbond  W25Q80BLUX1G   Chip ID = 0xEF, 0x40, 0x14, 0x0\");\r\n  Serial.println(\"Macronix MX25L12835FZNI Chip ID = 0xC2, 0x20, 0x18, 0xC2\");\r\n  Serial.println(\"Spansion S25FL127S      Chip ID = 0x01, 0x20, 0x18, 0x4D\");\r\n  Serial.println(\" \");\r\n  \r\n/* Initialize the array to 0,1,2,3 etc.*/\r\n  for(uint16_t i = 0; i < 256; i++) {\r\n    w_page[i] = i;\r\n  }\r\n\r\n  delay(100);\r\n  \r\n/* Write the page to page_number - this page MUST be in the erased state*/\r\n  Serial.print(\"Write page:  0x\"); Serial.println(page_number, HEX);  \r\n  t_start = micros();\r\n  flash_page_program(w_page, page_number);\r\n  t_start = micros() - t_start;\r\n  Serial.print(\"time (us) = \"); Serial.println(t_start);\r\n\r\n  delay(100);\r\n  \r\n/* Read back page_number and print its contents which should be 0,1,2,3...*/\r\n  Serial.print(\"Read Page 0x\"); Serial.println(page_number, HEX);\r\n  t_start = micros();\r\n  flash_read_pages(r_page, page_number,1);\r\n  t_start = micros() - t_start;\r\n  Serial.print(\"time (us) = \"); Serial.println(t_start);\r\n\r\n  delay(100);\r\n  \r\n  for(uint16_t i = 0; i < 256; i++) {\r\n    Serial.print(\" 0x\"); Serial.print(r_page[i], HEX);\r\n\tif (i % 16==0) Serial.println();\r\n  }\r\n  Serial.println(\"\");\r\n\r\n  delay(100);\r\n  \r\n/* Erase the sector which includes page_number*/\r\n  t_start = millis();\r\n  flash_chip_erase(true);\r\n  t_start = millis() - t_start;\r\n  Serial.print(\"time (ms) = \"); Serial.println(t_start);\r\n\r\n  delay(100);\r\n  \r\n/* Now read back the page. It should now be all 255.*/\r\n  Serial.print( \"Read Page 0x\"); Serial.println(page_number, HEX);\r\n  t_start = micros();\r\n  flash_read_pages(r_page, page_number,1);\r\n  t_start = micros() - t_start;\r\n  \r\n  delay(100);\r\n  \r\n  Serial.print(\"time (us) = \"); Serial.println(t_start);\r\n  for(uint16_t i = 0; i < 256; i++) {\r\n    Serial.print(\" 0x\"); Serial.print(r_page[i], HEX);\r\n\tif (i % 16==0) Serial.println();\r\n  }\r\n  Serial.println(\"\");\r\n}\r\n\r\nvoid loop(void)\r\n{\r\n  yield();\r\n}\r\n\r\n/*********************************************************************************************/\r\n// Useful functions\r\n/*********************************************************************************************/\r\nvoid write_pause(void)\r\n{\r\n  if(flash_wait_for_write) {\r\n    while(flash_read_status() & STAT_WIP){\r\n      delay(1);\r\n    }\r\n    flash_wait_for_write = 0;\r\n  }\r\n}\r\n\r\n//=====================================\r\n// convert a page number to a 24-bit address\r\nint page_to_address(int pn)\r\n{\r\n  return(pn << 8);\r\n}\r\n\r\n//=====================================\r\n// convert a 24-bit address to a page number\r\nint address_to_page(int addr)\r\n{\r\n  return(addr >> 8);\r\n}\r\n\r\n//=====================================\r\nvoid flash_read_id(unsigned char *idt)\r\n{\r\n  write_pause();\r\n  //set control register \r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_READ_ID);\r\n  for(uint16_t i = 0; i < 20; i++) {\r\n    *idt++ = SPI.transfer(0x00);\r\n  }\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\n//=====================================\r\nunsigned char flash_read_status(void)\r\n{\r\n  unsigned char c;\r\n\r\n// This can't do a write_pause\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);  \r\n  SPI.transfer(CMD_READ_STATUS_REG);\r\n  c = SPI.transfer(0x00);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  return(c);\r\n}\r\n\r\n//=====================================\r\n\r\nvoid flash_hard_reset(void)\r\n{\r\n  // Make sure that the device is not busy before\r\n  // doing the hard reset sequence\r\n  // At the moment this does NOT check the\r\n  // SUSpend status bit in Status Register 2\r\n  // but the library does not support suspend\r\n  // mode yet anyway\r\n  write_pause();\r\n  \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_RESET_DEVICE );\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  delayMicroseconds(50);\r\n  // Wait for the hard reset to finish\r\n  // Don't use flash_wait_for_write here\r\n  while(flash_read_status() & STAT_WIP);\r\n  // The spec says \"the device will take\r\n  // approximately tRST=30 microseconds\r\n  // to reset\"\r\n}\r\n\r\n//=====================================\r\nvoid flash_chip_erase(boolean wait)\r\n{\r\n  uint8_t status = 0x01;\r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n    \r\n  while(status & 0x01) { // check that WEP bit is 0 before erasing\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  status = SPI.transfer(CMD_READ_STATUS_REG);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  }\r\n\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_CHIP_ERASE);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n    \r\n  flash_wait_for_write = 1;\r\n  if(wait)write_pause();\r\n}\r\n\r\n//=====================================\r\n// Tse Typ=0.6sec Max=3sec\r\n// measured 549.024ms\r\n// Erase the sector which contains the specified\r\n// page number.\r\n// The smallest unit of memory which can be erased\r\n// is the 4kB sector (which is 16 pages)\r\nvoid flash_erase_pages_sector(int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause(); \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(CSPIN, HIGH);\r\n  \r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_SECTOR_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xff);\r\n  SPI.transfer((address >> 8) & 0xff);\r\n  SPI.transfer(address & 0xff);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();  \r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\n// Erase the 32kb block which contains the specified\r\n// page number.\r\nvoid flash_erase_pages_block32k(int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(CSPIN, HIGH);\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_BLOCK32K_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\n// Erase the 64kb block which contains the specified\r\n// page number.\r\nvoid flash_erase_pages_block64k(int pn)\r\n{\r\n  int address;\r\n  \r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(CSPIN, HIGH);\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_BLOCK64K_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\nvoid flash_page_program(unsigned char *wp,int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause(); \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  \r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_PAGE_PROGRAM);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // Now write 256 bytes to the page\r\n  for(uint16_t i = 0; i < 256; i++) {\r\n  SPI.transfer(*wp++);\r\n  }\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\nvoid flash_read_pages(unsigned char *p,int pn,const int n_pages)\r\n{\r\n  int address;\r\n  unsigned char *rp = p;\r\n  \r\n  write_pause();\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_READ_DATA);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // Now read the page's data bytes\r\n  for(uint16_t i = 0; i < n_pages * 256; i++) {\r\n    *rp++ = SPI.transfer(0);\r\n  }\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\n//=====================================\r\n// Read specified number of pages starting with pn\r\nvoid flash_fast_read_pages(unsigned char *p,int pn,const int n_pages)\r\n{\r\n  int address;\r\n  unsigned char *rp = p;\r\n  \r\n  write_pause();\r\n// The chip doesn't run at the higher clock speed until\r\n// after the command and address have been sent\r\n  SPI.beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(CSPIN, LOW);\r\n  SPI.transfer(CMD_READ_HIGH_SPEED);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // send dummy byte\r\n  SPI.transfer(0);\r\n  // Now read the number of pages required\r\n  for(uint16_t i = 0; i < n_pages * 256; i++) {\r\n    *rp++ = SPI.transfer(0);\r\n  }\r\n  digitalWrite(CSPIN, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/readSPIFlash_ESP32C3Mini_EnvSensor.v02b/SPIFlash.cpp",
    "content": "/* SPIFlash.cpp\r\nSketch by Kris Winer December 16. 2016\r\n\r\nLicense: Use this sketch any way you choose; if you like it, buy me a beer sometime\r\n\r\nPurpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4\r\nDragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.\r\n\r\nSketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency\r\nand maximum speed.\r\n\r\nSketch based on the work of Pete (El Supremo) as follows:\r\n * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca\r\n *\r\n * Development of this audio library was funded by PJRC.COM, LLC by sales of\r\n * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop\r\n * open source software by purchasing Teensy or other PJRC products.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to deal\r\n * in the Software without restriction, including without limitation the rights\r\n * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r\n * copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n * The above copyright notice, development funding notice, and this permission\r\n * notice shall be included in all copies or substantial portions of the Software.\r\n */\r\n\r\n#include \"SPIFlash.h\"\r\n#include \"SPI.h\"\r\n\r\n\r\nSPIFlash::SPIFlash(uint8_t CSPIN)\r\n{\r\n  _csPin = CSPIN; \r\n}\r\n\r\n\r\nvoid SPIFlash::init(const uint8_t SCK, const uint8_t MISO, const uint8_t MOSI, const uint8_t SS)\r\n{\r\n  SPI.begin(SCK, MISO, MOSI, SS);\r\n  delay(20);\r\n}\r\n\r\n\r\nvoid SPIFlash::getChipID(uint8_t * dest)\r\n{ \r\n//  uint16_t id[3];\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(0x9F);\r\n  dest[0] = SPI.transfer(0);\r\n  dest[1] = SPI.transfer(0);\r\n  dest[2] = SPI.transfer(0);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\nvoid SPIFlash::powerDown()\r\n{\r\n  write_pause();\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_POWER_DOWN);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  delayMicroseconds(10);\r\n}\r\n\r\n\r\nvoid SPIFlash::powerUp()\r\n{\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_RELEASE_POWER_DOWN);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\n\r\nvoid SPIFlash::write_pause(void)\r\n{\r\n  if(flash_wait_for_write) {\r\n    while(flash_read_status() & STAT_WIP){\r\n      delay(1);\r\n    }\r\n    flash_wait_for_write = 0;\r\n  }\r\n}\r\n\r\n\r\n//=====================================\r\n// convert a page number to a 24-bit address\r\nint SPIFlash::page_to_address(int pn)\r\n{\r\n  return(pn << 8);\r\n}\r\n\r\n//=====================================\r\n// convert a 24-bit address to a page number\r\nint SPIFlash::address_to_page(int addr)\r\n{\r\n  return(addr >> 8);\r\n}\r\n\r\n//=====================================\r\nvoid SPIFlash::flash_read_id(unsigned char *idt)\r\n{\r\n  write_pause();\r\n  //set control register \r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_READ_ID);\r\n  for(uint16_t i = 0; i < 20; i++) {\r\n    *idt++ = SPI.transfer(0x00);\r\n  }\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\n//=====================================\r\nunsigned char SPIFlash::flash_read_status(void)\r\n{\r\n  unsigned char c;\r\n\r\n// This can't do a write_pause\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);  \r\n  SPI.transfer(CMD_READ_STATUS_REG);\r\n  c = SPI.transfer(0x00);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  return(c);\r\n}\r\n\r\n//=====================================\r\n\r\nvoid SPIFlash::flash_hard_reset(void)\r\n{\r\n  // Make sure that the device is not busy before\r\n  // doing the hard reset sequence\r\n  // At the moment this does NOT check the\r\n  // SUSpend status bit in Status Register 2\r\n  // but the library does not support suspend\r\n  // mode yet anyway\r\n  write_pause();\r\n  \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_RESET_DEVICE );\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  delayMicroseconds(50);\r\n  // Wait for the hard reset to finish\r\n  // Don't use flash_wait_for_write here\r\n  while(flash_read_status() & STAT_WIP);\r\n  // The spec says \"the device will take\r\n  // approximately tRST=30 microseconds\r\n  // to reset\"\r\n}\r\n\r\n//=====================================\r\nvoid SPIFlash::flash_chip_erase(boolean wait)\r\n{\r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_CHIP_ERASE);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  flash_wait_for_write = 1;\r\n  if(wait)write_pause();\r\n}\r\n\r\n//=====================================\r\n// Tse Typ=0.6sec Max=3sec\r\n// measured 549.024ms\r\n// Erase the sector which contains the specified\r\n// page number.\r\n// The smallest unit of memory which can be erased\r\n// is the 4kB sector (which is 16 pages)\r\nvoid SPIFlash::flash_erase_pages_sector(int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause(); \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  \r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_SECTOR_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xff);\r\n  SPI.transfer((address >> 8) & 0xff);\r\n  SPI.transfer(address & 0xff);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();  \r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\n// Erase the 32kb block which contains the specified\r\n// page number.\r\nvoid SPIFlash::flash_erase_pages_block32k(int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_BLOCK32K_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\n// Erase the 64kb block which contains the specified\r\n// page number.\r\nvoid SPIFlash::flash_erase_pages_block64k(int pn)\r\n{\r\n  int address;\r\n  \r\n  write_pause();\r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_BLOCK64K_ERASE);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\nvoid SPIFlash::flash_page_program(unsigned char *wp,int pn)\r\n{\r\n  int address;\r\n\r\n  write_pause(); \r\n  // Send Write Enable command\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_WRITE_ENABLE);\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  \r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_PAGE_PROGRAM);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // Now write 256 bytes to the page\r\n  for(uint16_t i = 0; i < 256; i++) {\r\n  SPI.transfer(*wp++);\r\n  }\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n  // Indicate that next I/O must wait for this write to finish\r\n  flash_wait_for_write = 1;\r\n}\r\n\r\n//=====================================\r\nvoid SPIFlash::flash_read_pages(unsigned char *p,int pn,const int n_pages)\r\n{\r\n  int address;\r\n  unsigned char *rp = p;\r\n  \r\n  write_pause();\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_READ_DATA);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // Now read the page's data bytes\r\n  for(uint16_t i = 0; i < n_pages * 256; i++) {\r\n    *rp++ = SPI.transfer(0);\r\n  }\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n\r\n//=====================================\r\n// Read specified number of pages starting with pn\r\nvoid SPIFlash::flash_fast_read_pages(unsigned char *p,int pn,const int n_pages)\r\n{\r\n  int address;\r\n  unsigned char *rp = p;\r\n  \r\n  write_pause();\r\n// The chip doesn't run at the higher clock speed until\r\n// after the command and address have been sent\r\n  SPI.beginTransaction(SPISettings(clkSpeed, MSBFIRST, SPI_MODE0));\r\n  digitalWrite(_csPin, LOW);\r\n  SPI.transfer(CMD_READ_HIGH_SPEED);\r\n  // Send the 3 byte address\r\n  address = page_to_address(pn);\r\n  SPI.transfer((address >> 16) & 0xFF);\r\n  SPI.transfer((address >> 8) & 0xFF);\r\n  SPI.transfer(address & 0xFF);\r\n  // send dummy byte\r\n  SPI.transfer(0);\r\n  // Now read the number of pages required\r\n  for(uint16_t i = 0; i < n_pages * 256; i++) {\r\n    *rp++ = SPI.transfer(0);\r\n  }\r\n  digitalWrite(_csPin, HIGH);\r\n  SPI.endTransaction();\r\n}\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/readSPIFlash_ESP32C3Mini_EnvSensor.v02b/SPIFlash.h",
    "content": "/* SPIFlash.h\r\nSketch by Kris Winer December 16. 2016\r\n\r\nLicense: Use this sketch any way you choose; if you like it, buy me a beer sometime\r\n\r\nPurpose: Checks function of a variety of SPI NOR flash memory chips hosted by the STM32L4\r\nDragonfly (STM32L476), Butterfly (STM32L433), and Ladybug (STML432) development boards or their variants.\r\n\r\nSketch takes advantage of the SPI.beginTransaction/SPI.EndTransaction protocol for efficiency\r\nand maximum speed.\r\n\r\nSketch based on the work of Pete (El Supremo) as follows:\r\n * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca\r\n *\r\n * Development of this audio library was funded by PJRC.COM, LLC by sales of\r\n * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop\r\n * open source software by purchasing Teensy or other PJRC products.\r\n *\r\n * Permission is hereby granted, free of charge, to any person obtaining a copy\r\n * of this software and associated documentation files (the \"Software\"), to deal\r\n * in the Software without restriction, including without limitation the rights\r\n * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r\n * copies of the Software, and to permit persons to whom the Software is\r\n * furnished to do so, subject to the following conditions:\r\n *\r\n * The above copyright notice, development funding notice, and this permission\r\n * notice shall be included in all copies or substantial portions of the Software.\r\n */\r\n\r\n#ifndef SPIFlash_h\r\n#define SPIFlash_h\r\n\r\n#include \"Arduino.h\"\r\n#include \"SPI.h\"\r\n\r\n#define STAT_WIP 1\r\n#define STAT_WEL 2\r\n\r\n#define CMD_WRITE_STATUS_REG   0x01\r\n#define CMD_PAGE_PROGRAM       0x02\r\n#define CMD_READ_DATA          0x03\r\n#define CMD_WRITE_DISABLE      0x04 \r\n#define CMD_READ_STATUS_REG    0x05\r\n#define CMD_WRITE_ENABLE       0x06\r\n#define CMD_READ_HIGH_SPEED    0x0B \r\n#define CMD_SECTOR_ERASE       0x20 \r\n#define CMD_BLOCK32K_ERASE     0x52 \r\n#define CMD_RESET_DEVICE       0xF0 \r\n#define CMD_READ_ID            0x9F\r\n#define CMD_RELEASE_POWER_DOWN 0xAB \r\n#define CMD_POWER_DOWN         0xB9 \r\n#define CMD_CHIP_ERASE         0xC7\r\n#define CMD_BLOCK64K_ERASE     0xD8 \r\n\r\n#define clkSpeed 10000000  // 10 MHz\r\n\r\nclass SPIFlash\r\n{\r\n  public: \r\n  SPIFlash(uint8_t CSPIN);\r\n  void init(const uint8_t SCK, const uint8_t MISO, const uint8_t MOSI, const uint8_t SS);\r\n  void getChipID(uint8_t * dest);\r\n  void powerDown();\r\n  void powerUp();\r\n  void write_pause(void);\r\n  int page_to_address(int pn);\r\n  int address_to_page(int addr);\r\n  void flash_read_id(unsigned char *idt);\r\n  unsigned char flash_read_status(void);\r\n  void flash_hard_reset(void);\r\n  void flash_chip_erase(boolean wait);\r\n  void flash_erase_pages_sector(int pn);\r\n  void flash_erase_pages_block32k(int pn);\r\n  void flash_erase_pages_block64k(int pn);\r\n  void flash_page_program(unsigned char *wp,int pn);\r\n  void flash_read_pages(unsigned char *p,int pn,const int n_pages);\r\n  void flash_fast_read_pages(unsigned char *p,int pn,const int n_pages);\r\n  private:\r\n  uint8_t   _csPin;\r\n  unsigned char flash_wait_for_write = 0;\r\n\r\n};\r\n\r\n#endif\r\n"
  },
  {
    "path": "ESP32C3MiniEnvSensor/readSPIFlash_ESP32C3Mini_EnvSensor.v02b/readSPIFlash_ESP32C3Mini_EnvSensor.v02b.ino",
    "content": "/* readSPIFlash_ESP32C3Mini_EnvSensor.v02b\r\n *  \r\n2021 Copyright Tlera Corporation \r\n\r\nApril 30, 2022\r\n\r\nSketch to read the QSPI Flash on the ESP32C3Mini Environmental Sensor v.02b, reconstruct the sensor data, and output CMS-compatible data for plotting.\r\n */\r\n\r\n#include \"SPIFlash.h\"\r\n#include \"SPI.h\"\r\n\r\n#define Serial USBSerial\r\n\r\nfloat HDCTemperature = 0.0f, HDCHumidity = 0.0f;\r\nuint16_t rawHDCTemperature = 0, rawHDCHumidity = 0;\r\nint32_t rawPress = 0;\r\nfloat pressure, altitude; // Scaled output of the LPS22HB\r\nuint8_t Second, Minute, Hour, Day, Month, Year;\r\nfloat VBat = 0.0f;\r\nuint16_t ADCCounts = 0;\r\n\r\nuint8_t LS_gain = 0x02;  \r\nuint8_t LS_res  = 0x04;  \r\nuint32_t RGBiRData[4] = {0, 0, 0, 0}; // red, green, blue, ir counts\r\nfloat ambientLight = 0; // ambient (green) light intensity in lux\r\n\r\nfloat ALSluxTable[25]={         // lux per count for ALS depends on gain and resolution chosen\r\n0.136, 0.273, 0.548, 1.099, 2.193,\r\n0.045, 0.090, 0.180, 0.359, 0.722,\r\n0.022, 0.045, 0.090, 0.179, 0.360,\r\n0.015, 0.030, 0.059, 0.119, 0.239,\r\n0.007, 0.015, 0.029, 0.059, 0.117\r\n};\r\n\r\n// Assume all channels have the same lux per LSB scaling\r\nfloat luxScale = ALSluxTable[LS_gain * 5 + LS_res];\r\n\r\n// SPI flash configuration\r\n//  Highest page number is 0x7FFF = 32767 for 64 Mbit flash\r\nuint16_t max_page_number = 0x7FFF;\r\nconst uint8_t CSPIN  = 4;\r\nuint8_t flash_id[3] = {0, 0, 0};\r\nuint16_t page_number = 0;     // set the page number for flash page write\r\nuint8_t  sector_number = 0;   // set the sector number for sector write\r\nuint8_t  flashPage[256];      // array to hold the data for flash page write\r\nuint8_t  bps = 23;            // bytes per sector such that 256 bytes per page= sectors per page x bps = 11 x 23\r\n\r\nSPIFlash SPIFlash(CSPIN); // instantiate SPI flash class\r\n \r\n\r\nvoid setup(void)\r\n{ \r\n  Serial.begin(115200);\r\n  while (!Serial) { }\r\n  Serial.println(\"Serial enabled!\");\r\n\r\n  // configure SPI flash\r\n  pinMode(CSPIN, OUTPUT);\r\n  digitalWrite(CSPIN, HIGH);\r\n\r\n // check SPI Flash ID\r\n  SPIFlash.init(6, 7, 5, 4);               // SPI.begin(SCK, MISO, MOSI, SS)\r\n  SPIFlash.powerUp();                      // MX25R6435FZAI defaults to power down state\r\n  \r\n  SPIFlash.getChipID(flash_id);                    // Verify SPI flash communication\r\n  if(flash_id[0] == 0xC2 && flash_id[1] == 0x28 && flash_id[2] == 0x17) {\r\n  Serial.println(\" \");  \r\n  Serial.println(\"Found Macronix MX25R6435FZAI with Chip ID = 0xC2, 0x28, 0x17!\");\r\n  Serial.println(\" \");  \r\n  }\r\n  else {\r\n  Serial.println(\" \");  \r\n  Serial.println(\"no or unknown SPI flash!\");\r\n  Serial.println(\" \");  \r\n  }\r\n  delay(4000); // give some time to read the screen\r\n  \r\n  // read the SPI flash\r\n  for(page_number = 0; page_number < 10; page_number++)  { // change the page number limit to correspond to number of pages logged\r\n\r\n//  Serial.print(\"Read Page 0x\"); Serial.println(page_number, HEX);\r\n   SPIFlash.flash_read_pages(flashPage, page_number, 1);\r\n      \r\n   for(sector_number = 0; sector_number < 11; sector_number++) {\r\n    \r\n    rawHDCTemperature =  ((uint16_t) flashPage[sector_number*bps + 0] << 8) | flashPage[sector_number*bps + 1];\r\n    rawHDCHumidity =     ((uint16_t) flashPage[sector_number*bps + 2] << 8) | flashPage[sector_number*bps + 3];\r\n    rawPress =           ((int32_t)  flashPage[sector_number*bps + 4] << 16) |  ((int32_t)flashPage[sector_number*bps + 5] << 8) | flashPage[sector_number*bps + 6];\r\n\r\n    Second =  flashPage[sector_number*bps + 17];\r\n    Minute =  flashPage[sector_number*bps + 18];\r\n    Hour =    flashPage[sector_number*bps + 19];\r\n    Day =     flashPage[sector_number*bps + 20];\r\n    Month =   flashPage[sector_number*bps + 21];\r\n    Year =    flashPage[sector_number*bps + 22];\r\n\r\n    HDCTemperature = ((float) rawHDCTemperature) * (165.0f/65536.0f) - 40.0f; // float degrees C, absolute accuracy +/- 0.2 C typical\r\n    HDCHumidity    = ((float) rawHDCHumidity) * (100.0f/65536.0f);   // float %rel humidity\r\n      \r\n    pressure = (float) rawPress/4096.0f; // Pressure in mbar\r\n    altitude = 145366.45f*(1.0f - powf((pressure/1013.25f), 0.190284f));   \r\n   \r\n\r\n    RGBiRData[0] = ((uint16_t) flashPage[sector_number*bps +  7] << 8) |  flashPage[sector_number*bps + 8];\r\n    RGBiRData[1] = ((uint16_t) flashPage[sector_number*bps +  9] << 8) |  flashPage[sector_number*bps + 10];\r\n    RGBiRData[2] = ((uint16_t) flashPage[sector_number*bps + 11] << 8) |  flashPage[sector_number*bps + 12];\r\n    RGBiRData[3] = ((uint16_t) flashPage[sector_number*bps + 13] << 8) |  flashPage[sector_number*bps + 14];\r\n\r\n    ADCCounts = ((uint16_t) flashPage[sector_number*bps + 15] << 8) |  flashPage[sector_number*bps + 16];\r\n    VBat = 2.0f * 2.60f * 1.14f * ((float) ADCCounts) / 4095.0f;\r\n\r\n    // Output for spreadsheet analysis\r\n    if(Month < 10) {Serial.print(\"0\"); Serial.print(Month);} else Serial.print(Month);\r\n    Serial.print(\"/\");Serial.print(Day); Serial.print(\"/\");Serial.print(2000 + Year); Serial.print(\" \");\r\n    if(Hour < 10) {Serial.print(\"0\"); Serial.print(Hour);} else Serial.print(Hour);\r\n    Serial.print(\":\"); \r\n    if(Minute < 10) {Serial.print(\"0\"); Serial.print(Minute);} else Serial.print(Minute); \r\n    Serial.print(\":\"); \r\n    if(Second < 10) {Serial.print(\"0\"); Serial.print(Second);} else Serial.print(Second); Serial.print(\",\");      \r\n    Serial.print(pressure, 2); Serial.print(\",\"); Serial.print(HDCTemperature, 2); Serial.print(\",\");  \r\n    Serial.print(HDCHumidity, 1); Serial.print(\",\"); \r\n    Serial.print(((float)RGBiRData[0])*luxScale, 2); Serial.print(\",\"); // R\r\n    Serial.print(((float)RGBiRData[1])*luxScale, 2); Serial.print(\",\"); // G\r\n    Serial.print(((float)RGBiRData[2])*luxScale, 2); Serial.print(\",\"); // B\r\n    Serial.print(((float)RGBiRData[3])*luxScale, 2); Serial.print(\",\"); // IR\r\n    Serial.println(VBat, 2);  // Bat voltage  \r\n    }\r\n  \r\n  }\r\n\r\n\r\n}\r\n\r\nvoid loop(void)\r\n{\r\n}\r\n"
  },
  {
    "path": "MPU9250_MS5637/MPU9250_MS5637_AHRS.ino",
    "content": "/* MPU9250_MS5637_ESP32 Basic Example Code\n by: Kris Winer\n date: December 14, 2016\n license: Beerware - Use this code however you'd like. If you \n find it useful you can buy me a beer some time.\n \n Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, \n getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to \n allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and \n Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.\n \n This sketch is intended specifically for the MPU9250+MS5637 Add-on shield.\n It uses SDA/SCL on pins 21/22, respectively, and it uses the Wire library.\n The MS5637 is a simple but high resolution pressure sensor, which can be used in its high resolution\n mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\n only 1 microAmp. The choice will depend on the application.\n \n SDA and SCL should have external pull-up resistors (to 3.3V).\n 4K7 resistors are on the MPU9250+MS5637 breakout board.\n \n Hardware setup:\n MPU9250 Breakout --------- ESP32\n VDD ---------------------- 3.3V\n SDA ----------------------- 21\n SCL ----------------------- 22\n GND ---------------------- GND\n \n */\n#include \"Wire.h\"   \n\n// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet\n#define MS5637_RESET      0x1E\n#define MS5637_CONVERT_D1 0x40\n#define MS5637_CONVERT_D2 0x50\n#define MS5637_ADC_READ   0x00\n\n// 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 \n// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map\n//\n//Magnetometer Registers\n#define AK8963_ADDRESS   0x0C\n#define WHO_AM_I_AK8963  0x00 // should return 0x48\n#define INFO             0x01\n#define AK8963_ST1       0x02  // data ready status bit 0\n#define AK8963_XOUT_L\t   0x03  // data\n#define AK8963_XOUT_H\t   0x04\n#define AK8963_YOUT_L\t   0x05\n#define AK8963_YOUT_H\t   0x06\n#define AK8963_ZOUT_L\t   0x07\n#define AK8963_ZOUT_H\t   0x08\n#define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2\n#define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0\n#define AK8963_ASTC      0x0C  // Self test control\n#define AK8963_I2CDIS    0x0F  // I2C disable\n#define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value\n#define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value\n#define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value\n\n#define SELF_TEST_X_GYRO 0x00                  \n#define SELF_TEST_Y_GYRO 0x01                                                                          \n#define SELF_TEST_Z_GYRO 0x02\n\n/*#define X_FINE_GAIN      0x03 // [7:0] fine gain\n#define Y_FINE_GAIN      0x04\n#define Z_FINE_GAIN      0x05\n#define XA_OFFSET_H      0x06 // User-defined trim values for accelerometer\n#define XA_OFFSET_L_TC   0x07\n#define YA_OFFSET_H      0x08\n#define YA_OFFSET_L_TC   0x09\n#define ZA_OFFSET_H      0x0A\n#define ZA_OFFSET_L_TC   0x0B */\n\n#define SELF_TEST_X_ACCEL 0x0D\n#define SELF_TEST_Y_ACCEL 0x0E    \n#define SELF_TEST_Z_ACCEL 0x0F\n\n#define SELF_TEST_A      0x10\n\n#define XG_OFFSET_H      0x13  // User-defined trim values for gyroscope\n#define XG_OFFSET_L      0x14\n#define YG_OFFSET_H      0x15\n#define YG_OFFSET_L      0x16\n#define ZG_OFFSET_H      0x17\n#define ZG_OFFSET_L      0x18\n#define SMPLRT_DIV       0x19\n#define CONFIG           0x1A\n#define GYRO_CONFIG      0x1B\n#define ACCEL_CONFIG     0x1C\n#define ACCEL_CONFIG2    0x1D\n#define LP_ACCEL_ODR     0x1E   \n#define WOM_THR          0x1F   \n\n#define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms\n#define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]\n#define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms\n\n#define FIFO_EN          0x23\n#define I2C_MST_CTRL     0x24   \n#define I2C_SLV0_ADDR    0x25\n#define I2C_SLV0_REG     0x26\n#define I2C_SLV0_CTRL    0x27\n#define I2C_SLV1_ADDR    0x28\n#define I2C_SLV1_REG     0x29\n#define I2C_SLV1_CTRL    0x2A\n#define I2C_SLV2_ADDR    0x2B\n#define I2C_SLV2_REG     0x2C\n#define I2C_SLV2_CTRL    0x2D\n#define I2C_SLV3_ADDR    0x2E\n#define I2C_SLV3_REG     0x2F\n#define I2C_SLV3_CTRL    0x30\n#define I2C_SLV4_ADDR    0x31\n#define I2C_SLV4_REG     0x32\n#define I2C_SLV4_DO      0x33\n#define I2C_SLV4_CTRL    0x34\n#define I2C_SLV4_DI      0x35\n#define I2C_MST_STATUS   0x36\n#define INT_PIN_CFG      0x37\n#define INT_ENABLE       0x38\n#define DMP_INT_STATUS   0x39  // Check DMP interrupt\n#define INT_STATUS       0x3A\n#define ACCEL_XOUT_H     0x3B\n#define ACCEL_XOUT_L     0x3C\n#define ACCEL_YOUT_H     0x3D\n#define ACCEL_YOUT_L     0x3E\n#define ACCEL_ZOUT_H     0x3F\n#define ACCEL_ZOUT_L     0x40\n#define TEMP_OUT_H       0x41\n#define TEMP_OUT_L       0x42\n#define GYRO_XOUT_H      0x43\n#define GYRO_XOUT_L      0x44\n#define GYRO_YOUT_H      0x45\n#define GYRO_YOUT_L      0x46\n#define GYRO_ZOUT_H      0x47\n#define GYRO_ZOUT_L      0x48\n#define EXT_SENS_DATA_00 0x49\n#define EXT_SENS_DATA_01 0x4A\n#define EXT_SENS_DATA_02 0x4B\n#define EXT_SENS_DATA_03 0x4C\n#define EXT_SENS_DATA_04 0x4D\n#define EXT_SENS_DATA_05 0x4E\n#define EXT_SENS_DATA_06 0x4F\n#define EXT_SENS_DATA_07 0x50\n#define EXT_SENS_DATA_08 0x51\n#define EXT_SENS_DATA_09 0x52\n#define EXT_SENS_DATA_10 0x53\n#define EXT_SENS_DATA_11 0x54\n#define EXT_SENS_DATA_12 0x55\n#define EXT_SENS_DATA_13 0x56\n#define EXT_SENS_DATA_14 0x57\n#define EXT_SENS_DATA_15 0x58\n#define EXT_SENS_DATA_16 0x59\n#define EXT_SENS_DATA_17 0x5A\n#define EXT_SENS_DATA_18 0x5B\n#define EXT_SENS_DATA_19 0x5C\n#define EXT_SENS_DATA_20 0x5D\n#define EXT_SENS_DATA_21 0x5E\n#define EXT_SENS_DATA_22 0x5F\n#define EXT_SENS_DATA_23 0x60\n#define MOT_DETECT_STATUS 0x61\n#define I2C_SLV0_DO      0x63\n#define I2C_SLV1_DO      0x64\n#define I2C_SLV2_DO      0x65\n#define I2C_SLV3_DO      0x66\n#define I2C_MST_DELAY_CTRL 0x67\n#define SIGNAL_PATH_RESET  0x68\n#define MOT_DETECT_CTRL  0x69\n#define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP\n#define PWR_MGMT_1       0x6B // Device defaults to the SLEEP mode\n#define PWR_MGMT_2       0x6C\n#define DMP_BANK         0x6D  // Activates a specific bank in the DMP\n#define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank\n#define DMP_REG          0x6F  // Register in DMP from which to read or to which to write\n#define DMP_REG_1        0x70\n#define DMP_REG_2        0x71 \n#define FIFO_COUNTH      0x72\n#define FIFO_COUNTL      0x73\n#define FIFO_R_W         0x74\n#define WHO_AM_I_MPU9250 0x75 // Should return 0x71\n#define XA_OFFSET_H      0x77\n#define XA_OFFSET_L      0x78\n#define YA_OFFSET_H      0x7A\n#define YA_OFFSET_L      0x7B\n#define ZA_OFFSET_H      0x7D\n#define ZA_OFFSET_L      0x7E\n\n// Using the MPU9250_MS5637 Add-On shield, ADO is set to 0 \n// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1\n#define ADO 0\n#if ADO\n#define MPU9250_ADDRESS 0x69  // Device address when ADO = 1\n#define AK8963_ADDRESS 0x0C   //  Address of magnetometer\n#define MS5637_ADDRESS 0x76   // Address of altimeter\n#else\n#define MPU9250_ADDRESS 0x68  // Device address when ADO = 0\n#define AK8963_ADDRESS 0x0C   //  Address of magnetometer\n#define MS5637_ADDRESS 0x76   // Address of altimeter\n#endif  \n\n#define SerialDebug true  // set to true to get Serial output for debugging\n\n// Set initial input parameters\nenum Ascale {\n  AFS_2G = 0,\n  AFS_4G,\n  AFS_8G,\n  AFS_16G\n};\n\nenum Gscale {\n  GFS_250DPS = 0,\n  GFS_500DPS,\n  GFS_1000DPS,\n  GFS_2000DPS\n};\n\nenum Mscale {\n  MFS_14BITS = 0, // 0.6 mG per LSB\n  MFS_16BITS      // 0.15 mG per LSB\n};\n\n#define ADC_256  0x00 // define pressure and temperature conversion rates\n#define ADC_512  0x02\n#define ADC_1024 0x04\n#define ADC_2048 0x06\n#define ADC_4096 0x08\n#define ADC_8192 0x0A\n#define ADC_D1   0x40\n#define ADC_D2   0x50\n\n// Specify sensor full scale\nuint8_t OSR = ADC_8192;     // set pressure amd temperature oversample rate\nuint8_t Gscale = GFS_250DPS;\nuint8_t Ascale = AFS_2G;\nuint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution\nuint8_t Mmode = 0x06;        // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read\nfloat aRes, gRes, mRes;      // scale resolutions per LSB for the sensors\n\n// Pin definitions\nint intPin = 14;  // can be any pin\nbool newData = false;\nbool newMagData = false;\n\nint myLed = 5;\n\nuint16_t Pcal[8];         // calibration constants from MS5637 PROM registers\nunsigned char nCRC;       // calculated check sum to ensure PROM integrity\nuint32_t D1 = 0, D2 = 0;  // raw MS5637 pressure and temperature data\ndouble dT, OFFSET, SENS, TT2, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data\n\nint16_t MPU9250Data[7]; // used to read all 14 bytes at once from the MPU9250 accel/gyro\nint16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output\nint16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output\nint16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output\nfloat magCalibration[3] = {0, 0, 0};  // Factory mag calibration and mag bias\nfloat 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\nint16_t tempCount;            // temperature raw count output\nfloat   temperature;          // Stores the MPU9250 gyro internal chip temperature in degrees Celsius\ndouble Temperature, Pressure; // stores MS5637 pressures sensor pressure and temperature\nfloat SelfTest[6];            // holds results of gyro and accelerometer self test\n\n// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)\nfloat pi = 3.141592653589793238462643383279502884f;\nfloat GyroMeasError = PI * (4.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 40 deg/s)\nfloat GyroMeasDrift = PI * (0.0f  / 180.0f);   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)\n// There is a tradeoff in the beta parameter between accuracy and response speed.\n// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.\n// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.\n// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!\n// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec\n// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; \n// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. \n// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.\nfloat beta = sqrt(3.0f / 4.0f) * GyroMeasError;   // compute beta\nfloat 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\n#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\n#define Ki 0.0f\n\nuint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate\nfloat pitch, yaw, roll;\nfloat a12, a22, a31, a32, a33;            // rotation matrix coefficients for Euler angles and gravity components\nfloat deltat = 0.0f, sum = 0.0f;          // integration interval for both filter schemes\nuint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval\nuint32_t Now = 0;                         // used to calculate integration interval\n\nfloat ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values \nfloat lin_ax, lin_ay, lin_az;             // linear acceleration (acceleration with gravity component subtracted)\nfloat q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion\nfloat eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method\n\nvoid setup()\n{\n  Serial.begin(115200);\n  delay(4000);\n  \n  Wire.begin(21, 22, 400000); //(SDA, SCL) (21,22) are default on ESP32, 400 kHz I2C clock\n  delay(1000);\n  \n  // Set up the interrupt pin, its set as active high, push-pull\n  pinMode(intPin, INPUT);\n  pinMode(myLed, OUTPUT);\n  digitalWrite(myLed, LOW);\n  \n  I2Cscan();// look for I2C devices on the bus\n    \n  // Read the WHO_AM_I register, this is a good test of communication\n  Serial.println(\"MPU9250 9-axis motion sensor...\");\n  uint8_t c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250\n  Serial.print(\"MPU9250 \"); Serial.print(\"I AM \"); Serial.print(c, HEX); Serial.print(\" I should be \"); Serial.println(0x71, HEX);\n\n  delay(1000); \n\n  if (c == 0x71) // WHO_AM_I should always be 0x71\n  {  \n    Serial.println(\"MPU9250 is online...\");\n    \n    MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values\n    Serial.print(\"x-axis self test: acceleration trim within : \"); Serial.print(SelfTest[0],1); Serial.println(\"% of factory value\");\n    Serial.print(\"y-axis self test: acceleration trim within : \"); Serial.print(SelfTest[1],1); Serial.println(\"% of factory value\");\n    Serial.print(\"z-axis self test: acceleration trim within : \"); Serial.print(SelfTest[2],1); Serial.println(\"% of factory value\");\n    Serial.print(\"x-axis self test: gyration trim within : \"); Serial.print(SelfTest[3],1); Serial.println(\"% of factory value\");\n    Serial.print(\"y-axis self test: gyration trim within : \"); Serial.print(SelfTest[4],1); Serial.println(\"% of factory value\");\n    Serial.print(\"z-axis self test: gyration trim within : \"); Serial.print(SelfTest[5],1); Serial.println(\"% of factory value\");\n    delay(1000);\n    \n   // get sensor resolutions, only need to do this once\n   getAres();\n   getGres();\n   getMres();\n    \n   Serial.println(\" Calibrate gyro and accel\");\n   accelgyrocalMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers\n   Serial.println(\"accel biases (mg)\"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); Serial.println(1000.*accelBias[2]);\n   Serial.println(\"gyro biases (dps)\"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);\n\n  delay(1000);  \n   \n  initMPU9250(); \n  Serial.println(\"MPU9250 initialized for active data mode....\"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature\n  \n  // Read the WHO_AM_I register of the magnetometer, this is a good test of communication\n  byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);  // Read WHO_AM_I register for AK8963\n  Serial.print(\"AK8963 \"); Serial.print(\"I AM \"); Serial.print(d, HEX); Serial.print(\" I should be \"); Serial.println(0x48, HEX);\n \n  delay(1000); \n  \n  // Get magnetometer calibration from AK8963 ROM\n  initAK8963(magCalibration); Serial.println(\"AK8963 initialized for active data mode....\"); // Initialize device for active mode read of magnetometer\n  \n  magcalMPU9250(magBias, magScale);\n  Serial.println(\"AK8963 mag biases (mG)\"); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]); \n  Serial.println(\"AK8963 mag scale (mG)\"); Serial.println(magScale[0]); Serial.println(magScale[1]); Serial.println(magScale[2]); \n  delay(2000); // add delay to see results before serial spew of data\n   \n  if(SerialDebug) {\n//  Serial.println(\"Calibration values: \");\n  Serial.print(\"X-Axis sensitivity adjustment value \"); Serial.println(magCalibration[0], 2);\n  Serial.print(\"Y-Axis sensitivity adjustment value \"); Serial.println(magCalibration[1], 2);\n  Serial.print(\"Z-Axis sensitivity adjustment value \"); Serial.println(magCalibration[2], 2);\n  }\n\n  delay(1000);  \n  \n  // Reset the MS5637 pressure sensor\n  MS5637Reset();\n  delay(100);\n  Serial.println(\"MS5637 pressure sensor reset...\");\n  // Read PROM data from MS5637 pressure sensor\n  MS5637PromRead(Pcal);\n  Serial.println(\"PROM dta read:\");\n  Serial.print(\"C0 = \"); Serial.println(Pcal[0]);\n  unsigned char refCRC = Pcal[0] >> 12;\n  Serial.print(\"C1 = \"); Serial.println(Pcal[1]);\n  Serial.print(\"C2 = \"); Serial.println(Pcal[2]);\n  Serial.print(\"C3 = \"); Serial.println(Pcal[3]);\n  Serial.print(\"C4 = \"); Serial.println(Pcal[4]);\n  Serial.print(\"C5 = \"); Serial.println(Pcal[5]);\n  Serial.print(\"C6 = \"); Serial.println(Pcal[6]);\n  \n  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data\n  Serial.print(\"Checksum = \"); Serial.print(nCRC); Serial.print(\" , should be \"); Serial.println(refCRC);  \n \n  delay(1000);  \n\n  attachInterrupt(intPin, myinthandler, RISING);  // define interrupt for INT pin output of MPU9250\n\n  }\n  else\n  {\n    Serial.print(\"Could not connect to MPU9250: 0x\");\n    Serial.println(c, HEX);\n    while(1) ; // Loop forever if communication doesn't happen\n  }\n  \n}\n\nvoid loop()\n{  \n   // If intPin goes high, all data registers have new data\n   if(newData == true) {  // On interrupt, read data\n     newData = false;  // reset newData flag\n     readMPU9250Data(MPU9250Data); // INT cleared on any read\n   \n    // Now we'll calculate the accleration value into actual g's\n     ax = (float)MPU9250Data[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set\n     ay = (float)MPU9250Data[1]*aRes - accelBias[1];   \n     az = (float)MPU9250Data[2]*aRes - accelBias[2];  \n\n    // Calculate the gyro value into actual degrees per second\n     gx = (float)MPU9250Data[4]*gRes;  // get actual gyro value, this depends on scale being set\n     gy = (float)MPU9250Data[5]*gRes;  \n     gz = (float)MPU9250Data[6]*gRes; \n  \n    newMagData = (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01);\n    if(newMagData == true) { // wait for magnetometer data ready bit to be set\n      readMagData(magCount);  // Read the x/y/z adc values\n  \n    // Calculate the magnetometer values in milliGauss\n    // Include factory calibration per data sheet and user environmental corrections\n      mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0];  // get actual magnetometer value, this depends on scale being set\n      my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];  \n      mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];  \n      mx *= magScale[0];\n      my *= magScale[1];\n      mz *= magScale[2]; \n    }\n    \n    for(uint8_t i = 0; i < 10; i++) { // iterate a fixed number of times per data read cycle\n    Now = micros();\n    deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update\n    lastUpdate = Now;\n\n    sum += deltat; // sum for averaging filter update rate\n    sumCount++;\n\n    MadgwickQuaternionUpdate(-ax, ay, az, gx*pi/180.0f, -gy*pi/180.0f, -gz*pi/180.0f,  my,  -mx, mz);\n    }\n } \n\n   // Serial print and/or display at 0.5 s rate independent of data rates\n    delt_t = millis() - count;\n    if (delt_t > 500) { // update LCD once per half-second independent of read rate\n\n    if(SerialDebug) {\n    Serial.print(\"ax = \"); Serial.print((int)1000*ax);  \n    Serial.print(\" ay = \"); Serial.print((int)1000*ay); \n    Serial.print(\" az = \"); Serial.print((int)1000*az); Serial.println(\" mg\");\n    Serial.print(\"gx = \"); Serial.print( gx, 2); \n    Serial.print(\" gy = \"); Serial.print( gy, 2); \n    Serial.print(\" gz = \"); Serial.print( gz, 2); Serial.println(\" deg/s\");\n    Serial.print(\"mx = \"); Serial.print( (int)mx ); \n    Serial.print(\" my = \"); Serial.print( (int)my ); \n    Serial.print(\" mz = \"); Serial.print( (int)mz ); Serial.println(\" mG\");\n    \n    Serial.print(\"q0 = \"); Serial.print(q[0]);\n    Serial.print(\" qx = \"); Serial.print(q[1]); \n    Serial.print(\" qy = \"); Serial.print(q[2]); \n    Serial.print(\" qz = \"); Serial.println(q[3]); \n    }               \n    tempCount = readTempData();  // Read the gyro adc values\n    temperature = ((float) tempCount) / 333.87f + 21.0f; // Gyro chip temperature in degrees Centigrade\n   // Print temperature in degrees Centigrade      \n    Serial.print(\"Gyro temperature is \");  Serial.print(temperature, 1);  Serial.println(\" degrees C\"); // Print T values to tenths of s degree C\n \n    D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value\n    D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value\n    dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference\n    OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);\n    SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);\n \n    Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade\n//\n// Second order corrections\n    if(Temperature > 20) \n    {\n      TT2 = 5*dT*dT/pow(2, 38); // correction for high temperatures\n      OFFSET2 = 0;\n      SENS2 = 0;\n    }\n    if(Temperature < 20)                   // correction for low temperature\n    {\n      TT2      = 3*dT*dT/pow(2, 33); \n      OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;\n      SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;\n    } \n    if(Temperature < -15)                      // correction for very low temperature\n    {\n      OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);\n      SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);\n    }\n // End of second order corrections\n //\n     Temperature = Temperature - T2/100;\n     OFFSET = OFFSET - OFFSET2;\n     SENS = SENS - SENS2;\n \n     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or kPa\n\n    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));\n   \n    if(SerialDebug) {\n    Serial.print(\"Digital temperature value = \"); Serial.print( (float)Temperature, 2); Serial.println(\" C\"); // temperature in degrees Celsius\n    Serial.print(\"Digital temperature value = \"); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(\" F\"); // temperature in degrees Fahrenheit\n    Serial.print(\"Digital pressure value = \"); Serial.print((float) Pressure, 2);  Serial.println(\" mbar\");// pressure in millibar\n    Serial.print(\"Altitude = \"); Serial.print(altitude, 2); Serial.println(\" feet\");\n    }\n    \n   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.\n  // In this coordinate system, the positive z-axis is down toward Earth. \n  // 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.\n  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.\n  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.\n  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.\n  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be\n  // applied in the correct order which for this configuration is yaw, pitch, and then roll.\n  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.\n    //Software AHRS:\n //   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]);   \n //   pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));\n //   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]);\n //   pitch *= 180.0f / PI;\n //   yaw   *= 180.0f / PI; \n //   yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04\n //   if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360\n //   roll  *= 180.0f / PI;\n    a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);\n    a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];\n    a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);\n    a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);\n    a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];\n    pitch = -asin(a32);\n    roll  = atan2(a31, a33);\n    yaw   = atan2(a12, a22);\n    pitch *= 180.0f / pi;\n    yaw   *= 180.0f / pi; \n    yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04\n    if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360\n    roll  *= 180.0f / pi;\n    lin_ax = ax + a31;\n    lin_ay = ay + a32;\n    lin_az = az - a33;\n    if(SerialDebug) {\n    Serial.print(\"Yaw, Pitch, Roll: \");\n    Serial.print(yaw, 2);\n    Serial.print(\", \");\n    Serial.print(pitch, 2);\n    Serial.print(\", \");\n    Serial.println(roll, 2);\n\n    Serial.print(\"Grav_x, Grav_y, Grav_z: \");\n    Serial.print(-a31*1000.0f, 2);\n    Serial.print(\", \");\n    Serial.print(-a32*1000.0f, 2);\n    Serial.print(\", \");\n    Serial.print(a33*1000.0f, 2);  Serial.println(\" mg\");\n    Serial.print(\"Lin_ax, Lin_ay, Lin_az: \");\n    Serial.print(lin_ax*1000.0f, 2);\n    Serial.print(\", \");\n    Serial.print(lin_ay*1000.0f, 2);\n    Serial.print(\", \");\n    Serial.print(lin_az*1000.0f, 2);  Serial.println(\" mg\");\n\n    Serial.print(\"sumCount = \"); Serial.println(sumCount);\n    Serial.print(\"sum = \"); Serial.println(sum);\n    \n    Serial.print(\"rate = \"); Serial.print((float)sumCount/sum, 2); Serial.println(\" Hz\");\n    }\n \n    digitalWrite(myLed, !digitalRead(myLed));\n    count = millis(); \n    sumCount = 0;\n    sum = 0;    \n    }\n \n}\n\n//===================================================================================================================\n//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data\n//===================================================================================================================\n\nvoid myinthandler()\n{\n  newData = true;\n}\n\nvoid getMres() {\n  switch (Mscale)\n  {\n \t// Possible magnetometer scales (and their register bit settings) are:\n\t// 14 bit resolution (0) and 16 bit resolution (1)\n    case MFS_14BITS:\n          mRes = 10.*4912./8190.; // Proper scale to return milliGauss\n          break;\n    case MFS_16BITS:\n          mRes = 10.*4912./32760.0; // Proper scale to return milliGauss\n          break;\n  }\n}\n\nvoid getGres() {\n  switch (Gscale)\n  {\n \t// Possible gyro scales (and their register bit settings) are:\n\t// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11). \n        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:\n    case GFS_250DPS:\n          gRes = 250.0/32768.0;\n          break;\n    case GFS_500DPS:\n          gRes = 500.0/32768.0;\n          break;\n    case GFS_1000DPS:\n          gRes = 1000.0/32768.0;\n          break;\n    case GFS_2000DPS:\n          gRes = 2000.0/32768.0;\n          break;\n  }\n}\n\nvoid getAres() {\n  switch (Ascale)\n  {\n \t// Possible accelerometer scales (and their register bit settings) are:\n\t// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11). \n        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:\n    case AFS_2G:\n          aRes = 2.0/32768.0;\n          break;\n    case AFS_4G:\n          aRes = 4.0/32768.0;\n          break;\n    case AFS_8G:\n          aRes = 8.0/32768.0;\n          break;\n    case AFS_16G:\n          aRes = 16.0/32768.0;\n          break;\n  }\n}\n\nvoid readMPU9250Data(int16_t * destination)\n{\n  uint8_t rawData[14];  // x/y/z accel register data stored here\n  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, &rawData[0]);  // Read the 14 raw data registers into data array\n  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;  \n  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; \n  destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;   \n  destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;  \n  destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;  \n  destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ; \n}\n\nvoid readAccelData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z accel register data stored here\n  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array\n  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;  \n  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; \n}\n\n\nvoid readGyroData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;  \n  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; \n}\n\nvoid readMagData(int16_t * destination)\n{\n  uint8_t rawData[7];  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition\n  readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array\n  uint8_t c = rawData[6]; // End data read by reading ST2 register\n    if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data\n    destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value\n    destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  // Data stored as little Endian\n    destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; \n   }\n}\n\nint16_t readTempData()\n{\n  uint8_t rawData[2];  // x/y/z gyro register data stored here\n  readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array \n  return ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a 16-bit value\n}\n       \nvoid initAK8963(float * destination)\n{\n  // First extract the factory calibration for each magnetometer axis\n  uint8_t rawData[3];  // x/y/z gyro calibration data stored here\n  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  \n  delay(10);\n  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode\n  delay(10);\n  readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);  // Read the x-, y-, and z-axis calibration values\n  destination[0] =  (float)(rawData[0] - 128)/256. + 1.;   // Return x-axis sensitivity adjustment values, etc.\n  destination[1] =  (float)(rawData[1] - 128)/256. + 1.;  \n  destination[2] =  (float)(rawData[2] - 128)/256. + 1.; \n  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  \n  delay(10);\n  // Configure the magnetometer for continuous read and highest resolution\n  // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,\n  // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates\n  writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR\n  delay(10);\n}\n\n\nvoid initMPU9250()\n{  \n // wake up device\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors \n  delay(100); // Wait for all registers to reset \n\n // get stable time source\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  // Auto select clock source to be PLL gyroscope reference if ready else\n  delay(200); \n  \n // Configure Gyro and Thermometer\n // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; \n // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot\n // be higher than 1 / 0.0059 = 170 Hz\n // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both\n // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz\n  writeByte(MPU9250_ADDRESS, CONFIG, 0x03);  \n\n // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)\n  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);  // Use a 200 Hz rate; a rate consistent with the filter update rate \n                                    // determined inset in CONFIG above\n \n // Set gyroscope full scale range\n // Range selects FS_SEL and GFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3\n  uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value\n // c = c & ~0xE0; // Clear self-test bits [7:5] \n  c = c & ~0x03; // Clear Fchoice bits [1:0] \n  c = c & ~0x18; // Clear GFS bits [4:3]\n  c = c | Gscale << 3; // Set full scale range for the gyro\n // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG\n  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register\n  \n // Set accelerometer full-scale range configuration\n  c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value\n // c = c & ~0xE0; // Clear self-test bits [7:5] \n  c = c & ~0x18;  // Clear AFS bits [4:3]\n  c = c | Ascale << 3; // Set full scale range for the accelerometer \n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value\n\n // Set accelerometer sample rate configuration\n // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for\n // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz\n  c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value\n  c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])  \n  c = c | 0x03;  // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz\n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value\n  \n // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, \n // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting\n\n  // Configure Interrupts and Bypass Enable\n  // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,\n  // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips \n  // can join the I2C bus and all can be controlled by the Arduino as master\n//   writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);    \n   writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x12);  // INT is 50 microsecond pulse and any read to clear  \n   writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);  // Enable data ready (bit 0) interrupt\n   delay(100);\n}\n\n\n// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average\n// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.\nvoid accelgyrocalMPU9250(float * dest1, float * dest2)\n{  \n  uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data\n  uint16_t ii, packet_count, fifo_count;\n  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};\n  \n // reset device\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device\n  delay(100);\n   \n // get stable time source; Auto select clock source to be PLL gyroscope reference if ready \n // else use the internal oscillator, bits 2:0 = 001\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  \n  writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);\n  delay(200);                                    \n\n// Configure device for bias calculation\n  writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts\n  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);      // Disable FIFO\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);   // Turn on internal clock source\n  writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master\n  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes\n  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C);    // Reset FIFO and DMP\n  delay(15);\n  \n// Configure MPU6050 gyro and accelerometer for bias calculation\n  writeByte(MPU9250_ADDRESS, CONFIG, 0x01);      // Set low-pass filter to 188 Hz\n  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);  // Set sample rate to 1 kHz\n  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity\n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity\n \n  uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec\n  uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g\n\n// Configure FIFO to capture accelerometer and gyro data for bias calculation\n  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  \n  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in MPU-9150)\n  delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes\n\n// At end of sample accumulation, turn off FIFO sensor read\n  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO\n  readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count\n  fifo_count = ((uint16_t)data[0] << 8) | data[1];\n  packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging\n  \n  for (ii = 0; ii < packet_count; ii++) {\n    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};\n    readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging\n    accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ;  // Form signed 16-bit integer for each sample in FIFO\n    accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;\n    accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;    \n    gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;\n    gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;\n    gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;\n    \n    accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases\n    accel_bias[1] += (int32_t) accel_temp[1];\n    accel_bias[2] += (int32_t) accel_temp[2];\n    gyro_bias[0]  += (int32_t) gyro_temp[0];\n    gyro_bias[1]  += (int32_t) gyro_temp[1];\n    gyro_bias[2]  += (int32_t) gyro_temp[2];\n            \n}\n    accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases\n    accel_bias[1] /= (int32_t) packet_count;\n    accel_bias[2] /= (int32_t) packet_count;\n    gyro_bias[0]  /= (int32_t) packet_count;\n    gyro_bias[1]  /= (int32_t) packet_count;\n    gyro_bias[2]  /= (int32_t) packet_count;\n    \n  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation\n  else {accel_bias[2] += (int32_t) accelsensitivity;}\n   \n// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup\n  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\n  data[1] = (-gyro_bias[0]/4)       & 0xFF; // Biases are additive, so change sign on calculated average gyro biases\n  data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;\n  data[3] = (-gyro_bias[1]/4)       & 0xFF;\n  data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;\n  data[5] = (-gyro_bias[2]/4)       & 0xFF;\n  \n// Push gyro biases to hardware registers\n  writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);\n  writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);\n  writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);\n  writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);\n  writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);\n  writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);\n  \n// Output scaled gyro biases for display in the main program\n  dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;  \n  dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;\n  dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;\n\n// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain\n// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold\n// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature\n// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that\n// the accelerometer biases calculated above must be divided by 8.\n\n  int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases\n  readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values\n  accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);\n  readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);\n  accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);\n  readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);\n  accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);\n  \n  uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers\n  uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis\n  \n  for(ii = 0; ii < 3; ii++) {\n    if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit\n  }\n  \n  // Construct total accelerometer bias, including calculated average accelerometer bias from above\n  accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)\n  accel_bias_reg[1] -= (accel_bias[1]/8);\n  accel_bias_reg[2] -= (accel_bias[2]/8);\n  \n  data[0] = (accel_bias_reg[0] >> 8) & 0xFF;\n  data[1] = (accel_bias_reg[0])      & 0xFF;\n  data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers\n  data[2] = (accel_bias_reg[1] >> 8) & 0xFF;\n  data[3] = (accel_bias_reg[1])      & 0xFF;\n  data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers\n  data[4] = (accel_bias_reg[2] >> 8) & 0xFF;\n  data[5] = (accel_bias_reg[2])      & 0xFF;\n  data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers\n \n// Apparently this is not working for the acceleration biases in the MPU-9250\n// Are we handling the temperature correction bit properly?\n// Push accelerometer biases to hardware registers\n/*  writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);\n  writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);\n  writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);\n  writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);\n  writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);\n  writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);\n*/\n// Output scaled accelerometer biases for display in the main program\n   dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; \n   dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;\n   dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;\n}\n\n\nvoid magcalMPU9250(float * dest1, float * dest2) \n{\n  uint16_t ii = 0, sample_count = 0;\n  int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};\n  int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};\n\n  Serial.println(\"Mag Calibration: Wave device in a figure eight until done!\");\n  delay(4000);\n  \n    // shoot for ~fifteen seconds of mag data\n    if(Mmode == 0x02) sample_count = 128;  // at 8 Hz ODR, new mag data is available every 125 ms\n    if(Mmode == 0x06) sample_count = 1500;  // at 100 Hz ODR, new mag data is available every 10 ms\n   for(ii = 0; ii < sample_count; ii++) {\n    readMagData(mag_temp);  // Read the mag data   \n    for (int jj = 0; jj < 3; jj++) {\n      if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];\n      if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];\n    }\n    if(Mmode == 0x02) delay(135);  // at 8 Hz ODR, new mag data is available every 125 ms\n    if(Mmode == 0x06) delay(12);  // at 100 Hz ODR, new mag data is available every 10 ms\n    }\n\n//    Serial.println(\"mag x min/max:\"); Serial.println(mag_max[0]); Serial.println(mag_min[0]);\n//    Serial.println(\"mag y min/max:\"); Serial.println(mag_max[1]); Serial.println(mag_min[1]);\n//    Serial.println(\"mag z min/max:\"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);\n\n    // Get hard iron correction\n    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts\n    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts\n    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts\n    \n    dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0];  // save mag biases in G for main program\n    dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];   \n    dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];  \n       \n    // Get soft iron correction estimate\n    mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts\n    mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts\n    mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts\n\n    float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];\n    avg_rad /= 3.0;\n\n    dest2[0] = avg_rad/((float)mag_scale[0]);\n    dest2[1] = avg_rad/((float)mag_scale[1]);\n    dest2[2] = avg_rad/((float)mag_scale[2]);\n  \n   Serial.println(\"Mag Calibration done!\");\n}\n\n\n\n// Accelerometer and gyroscope self test; check calibration wrt factory settings\nvoid MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass\n{\n   uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};\n   uint8_t selfTest[6];\n   int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};\n   float factoryTrim[6];\n   uint8_t FS = 0;\n   \n  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);    // Set gyro sample rate to 1 kHz\n  writeByte(MPU9250_ADDRESS, CONFIG, 0x02);        // Set gyro sample rate to 1 kHz and DLPF to 92 Hz\n  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS);  // Set full scale range for the gyro to 250 dps\n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz\n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g\n\n  for( int ii = 0; ii < 200; ii++) {  // get average current values of gyro and acclerometer\n  \n  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);        // Read the six raw data registers into data array\n  aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value\n  aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  \n  aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; \n  \n    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);       // Read the six raw data registers sequentially into data array\n  gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value\n  gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  \n  gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; \n  }\n  \n  for (int ii =0; ii < 3; ii++) {  // Get average of 200 values and store as average current readings\n  aAvg[ii] /= 200;\n  gAvg[ii] /= 200;\n  }\n  \n// Configure the accelerometer for self-test\n   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g\n   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s\n   delay(25);  // Delay a while to let the device stabilize\n\n  for( int ii = 0; ii < 200; ii++) {  // get average self-test values of gyro and acclerometer\n  \n  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array\n  aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value\n  aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  \n  aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; \n  \n    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value\n  gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  \n  gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; \n  }\n  \n  for (int ii =0; ii < 3; ii++) {  // Get average of 200 values and store as average self-test readings\n  aSTAvg[ii] /= 200;\n  gSTAvg[ii] /= 200;\n  }   \n  \n // Configure the gyro and accelerometer for normal operation\n   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);  \n   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0x00);  \n   delay(25);  // Delay a while to let the device stabilize\n   \n   // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg\n   selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results\n   selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results\n   selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results\n   selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO);  // X-axis gyro self-test results\n   selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO);  // Y-axis gyro self-test results\n   selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO);  // Z-axis gyro self-test results\n\n  // Retrieve factory self-test value from self-test code reads\n   factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation\n   factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation\n   factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation\n   factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation\n   factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation\n   factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation\n \n // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response\n // To get percent, must multiply by 100\n   for (int i = 0; i < 3; i++) {\n     destination[i]   = 100.0f*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.0f;   // Report percent differences\n     destination[i+3] = 100.0f*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.0f; // Report percent differences\n   }\n   \n}\n\n// I2C communication with the MS5637 is a little different from that with the MPU9250 and most other sensors\n// For the MS5637, we write commands, and the MS5637 sends data in response, rather than directly reading\n// MS5637 registers\n\n        void MS5637Reset()\n        {\n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(MS5637_RESET);                // Put reset command in Tx buffer\n        Wire.endTransmission();                  // Send the Tx buffer\n        }\n        \n        void MS5637PromRead(uint16_t * destination)\n        {\n        uint8_t data[2] = {0,0};\n        for (uint8_t ii = 0; ii < 7; ii++) {\n          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer\n          Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n          uint8_t i = 0;\n          Wire.requestFrom(MS5637_ADDRESS, 2);     // Read two bytes from slave PROM address \n          while (Wire.available()) {\n          data[i++] = Wire.read(); }               // Put read results in the Rx buffer\n          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program\n        }\n        }\n\n        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read\n        {\n        uint8_t data[3] = {0,0,0};\n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(CMD | OSR);                   // Put pressure conversion command in Tx buffer\n        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n        \n        switch (OSR)\n        {\n          case ADC_256: delay(1); break;         // delay for conversion to complete\n          case ADC_512: delay(3); break;\n          case ADC_1024: delay(4); break;\n          case ADC_2048: delay(6); break;\n          case ADC_4096: delay(10); break;\n          case ADC_8192: delay(20); break;\n        }\n       \n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(0x00);                        // Put ADC read command in Tx buffer\n        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n        uint8_t i = 0;\n        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address \n        while (Wire.available()) {\n        data[i++] = Wire.read(); }               // Put read results in the Rx buffer\n        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program\n        }\n\n\n\nunsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents\n{\n  int cnt;\n  unsigned int n_rem = 0;\n  unsigned char n_bit;\n  \n  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation\n  n_prom[7] = 0;\n  for(cnt = 0; cnt < 16; cnt++)\n  {\n    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);\n    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);\n    for(n_bit = 8; n_bit > 0; n_bit--)\n    {\n        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;\n        else                  n_rem = (n_rem<<1);\n    }\n  }\n  n_rem = ((n_rem>>12) & 0x000F);\n  return (n_rem ^ 0x00);\n}\n\n\n\n// simple function to scan for I2C devices on the bus\nvoid I2Cscan() \n{\n    // scan for i2c devices\n  byte error, address;\n  int nDevices;\n\n  Serial.println(\"Scanning...\");\n\n  nDevices = 0;\n  for(address = 1; address < 127; address++ ) \n  {\n    // The i2c_scanner uses the return value of\n    // the Write.endTransmisstion to see if\n    // a device did acknowledge to the address.\n    Wire.beginTransmission(address);\n    error = Wire.endTransmission();\n\n    if (error == 0)\n    {\n      Serial.print(\"I2C device found at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.print(address,HEX);\n      Serial.println(\"  !\");\n\n      nDevices++;\n    }\n    else if (error==4) \n    {\n      Serial.print(\"Unknown error at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.println(address,HEX);\n    }    \n  }\n  if (nDevices == 0)\n    Serial.println(\"No I2C devices found\\n\");\n  else\n    Serial.println(\"done\\n\");\n}\n\n\n// I2C read/write functions for the MPU9250 sensors\n\n  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)\n{\n  Wire.beginTransmission(address);  // Initialize the Tx buffer\n  Wire.write(subAddress);           // Put slave register address in Tx buffer\n  Wire.write(data);                 // Put data in Tx buffer\n  Wire.endTransmission();           // Send the Tx buffer\n}\n\n  uint8_t readByte(uint8_t address, uint8_t subAddress)\n{\n  uint8_t data = 0;                        // `data` will store the register data   \n  Wire.beginTransmission(address);         // Initialize the Tx buffer\n  Wire.write(subAddress);                  // Put slave register address in Tx buffer\n  Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n  Wire.requestFrom(address, 1);            // Read two bytes from slave register address on MPU9250 \n  data = Wire.read();                      // Fill Rx buffer with result\n  return data;                             // Return data read from slave register\n}\n\n  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\n{  \n  Wire.beginTransmission(address);   // Initialize the Tx buffer\n  Wire.write(subAddress);            // Put slave register address in Tx buffer\n  Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive\n  uint8_t i = 0;\n  Wire.requestFrom(address, count);  // Read bytes from slave register address \n  while (Wire.available()) {\n        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer\n}\n"
  },
  {
    "path": "MPU9250_MS5637/MS5637.ino",
    "content": "#include \"Wire.h\"   \n\n// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet\n#define MS5637_RESET      0x1E\n#define MS5637_CONVERT_D1 0x40\n#define MS5637_CONVERT_D2 0x50\n#define MS5637_ADC_READ   0x00\n\n#define MS5637_ADDRESS 0x76   // Address of altimeter\n\n#define SerialDebug true  // set to true to get Serial output for debugging\n\n#define ADC_256  0x00 // define pressure and temperature conversion rates\n#define ADC_512  0x02\n#define ADC_1024 0x04\n#define ADC_2048 0x06\n#define ADC_4096 0x08\n#define ADC_8192 0x0A\n#define ADC_D1   0x40\n#define ADC_D2   0x50\n\n// Specify sensor full scale\nuint8_t OSR = ADC_8192;     // set pressure amd temperature oversample rate\n  \n// Pin definitions\nint intPin = 14;  \n\nint myLed = 5;\n\nuint16_t Pcal[8];         // calibration constants from MS5637 PROM registers\nunsigned char nCRC;       // calculated check sum to ensure PROM integrity\nuint32_t D1 = 0, D2 = 0;  // raw MS5637 pressure and temperature data\ndouble dT, OFFSET, SENS, TT2, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data\n    \nint16_t tempCount;            // temperature raw count output\nfloat   temperature;          // Stores the MPU9250 gyro internal chip temperature in degrees Celsius\ndouble Temperature, Pressure; // stores MS5637 pressures sensor pressure and temperature\n\n// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)\nfloat pi = 3.141592653589793238462643383279502884f;\n\nvoid setup()\n{\n  Serial.begin(115200);\n  delay(4000);\n  \n  Wire.begin(21,22); // 21/22 are default on ESP32\n  Wire.setClock(400000); // choose 400 kHz I2C rate\n//  Wire.setClockStretchLimit(1000000);\n  delay(1000);\n  \n  // Set up the interrupt pin, its set as active high, push-pull\n  pinMode(intPin, INPUT);\n  pinMode(myLed, OUTPUT);\n  digitalWrite(myLed, LOW);\n  \n  I2Cscan();// look for I2C devices on the bus\n    \n  // Reset the MS5637 pressure sensor\n  MS5637Reset();\n  delay(100);\n  Serial.println(\"MS5637 pressure sensor reset...\");\n  // Read PROM data from MS5637 pressure sensor\n  MS5637PromRead(Pcal);\n  Serial.println(\"PROM dta read:\");\n  Serial.print(\"C0 = \"); Serial.println(Pcal[0]);\n  unsigned char refCRC = Pcal[0] >> 12;\n  Serial.print(\"C1 = \"); Serial.println(Pcal[1]);\n  Serial.print(\"C2 = \"); Serial.println(Pcal[2]);\n  Serial.print(\"C3 = \"); Serial.println(Pcal[3]);\n  Serial.print(\"C4 = \"); Serial.println(Pcal[4]);\n  Serial.print(\"C5 = \"); Serial.println(Pcal[5]);\n  Serial.print(\"C6 = \"); Serial.println(Pcal[6]);\n  \n  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data\n  Serial.print(\"Checksum = \"); Serial.print(nCRC); Serial.print(\" , should be \"); Serial.println(refCRC);  \n}\n\nvoid loop()\n{  \n\n    D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value\n    D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value\n    dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference\n    OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);\n    SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);\n \n    Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade\n//\n// Second order corrections\n    if(Temperature > 20) \n    {\n      TT2 = 5*dT*dT/pow(2, 38); // correction for high temperatures\n      OFFSET2 = 0;\n      SENS2 = 0;\n    }\n    if(Temperature < 20)                   // correction for low temperature\n    {\n      TT2      = 3*dT*dT/pow(2, 33); \n      OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;\n      SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;\n    } \n    if(Temperature < -15)                      // correction for very low temperature\n    {\n      OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);\n      SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);\n    }\n // End of second order corrections\n //\n     Temperature = Temperature - T2/100;\n     OFFSET = OFFSET - OFFSET2;\n     SENS = SENS - SENS2;\n \n     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or kPa\n\n    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));\n   \n    if(SerialDebug) {\n    Serial.print(\"Digital temperature value = \"); Serial.print( (float)Temperature, 2); Serial.println(\" C\"); // temperature in degrees Celsius\n    Serial.print(\"Digital temperature value = \"); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(\" F\"); // temperature in degrees Fahrenheit\n    Serial.print(\"Digital pressure value = \"); Serial.print((float) Pressure, 2);  Serial.println(\" mbar\");// pressure in millibar\n    Serial.print(\"Altitude = \"); Serial.print(altitude, 2); Serial.println(\" feet\");\n    }   \n \n   digitalWrite(myLed, !digitalRead(myLed));\n   delay(500);\n}\n\n//===================================================================================================================\n//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data\n//===================================================================================================================\n\n// I2C communication with the MS5637 is a little different from that with the MPU9250 and most other sensors\n// For the MS5637, we write commands, and the MS5637 sends data in response, rather than directly reading\n// MS5637 registers\n\n        void MS5637Reset()\n        {\n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(MS5637_RESET);                // Put reset command in Tx buffer\n        Wire.endTransmission();                  // Send the Tx buffer\n        }\n        \n        void MS5637PromRead(uint16_t * destination)\n        {\n        uint8_t data[2] = {0,0};\n        for (uint8_t ii = 0; ii < 7; ii++) {\n          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer\n          Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n          uint8_t i = 0;\n          Wire.requestFrom(MS5637_ADDRESS, 2);     // Read two bytes from slave PROM address \n          while (Wire.available()) {\n          data[i++] = Wire.read(); }               // Put read results in the Rx buffer\n          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program\n        }\n        }\n\n        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read\n        {\n        uint8_t data[3] = {0,0,0};\n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(CMD | OSR);                   // Put pressure conversion command in Tx buffer\n        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n        \n        switch (OSR)\n        {\n          case ADC_256: delay(1); break;         // delay for conversion to complete\n          case ADC_512: delay(3); break;\n          case ADC_1024: delay(4); break;\n          case ADC_2048: delay(6); break;\n          case ADC_4096: delay(10); break;\n          case ADC_8192: delay(20); break;\n        }\n       \n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(0x00);                        // Put ADC read command in Tx buffer\n        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n        uint8_t i = 0;\n        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address \n        while (Wire.available()) {\n        data[i++] = Wire.read(); }               // Put read results in the Rx buffer\n        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program\n        }\n\nunsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents\n{\n  int cnt;\n  unsigned int n_rem = 0;\n  unsigned char n_bit;\n  \n  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation\n  n_prom[7] = 0;\n  for(cnt = 0; cnt < 16; cnt++)\n  {\n    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);\n    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);\n    for(n_bit = 8; n_bit > 0; n_bit--)\n    {\n        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;\n        else                  n_rem = (n_rem<<1);\n    }\n  }\n  n_rem = ((n_rem>>12) & 0x000F);\n  return (n_rem ^ 0x00);\n}\n\n\n// simple function to scan for I2C devices on the bus\nvoid I2Cscan() \n{\n    // scan for i2c devices\n  byte error, address;\n  int nDevices;\n\n  Serial.println(\"Scanning...\");\n\n  nDevices = 0;\n  for(address = 1; address < 127; address++ ) \n  {\n    // The i2c_scanner uses the return value of\n    // the Write.endTransmisstion to see if\n    // a device did acknowledge to the address.\n    Wire.beginTransmission(address);\n    error = Wire.endTransmission();\n\n    if (error == 0)\n    {\n      Serial.print(\"I2C device found at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.print(address,HEX);\n      Serial.println(\"  !\");\n\n      nDevices++;\n    }\n    else if (error==4) \n    {\n      Serial.print(\"Unknown error at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.println(address,HEX);\n    }    \n  }\n  if (nDevices == 0)\n    Serial.println(\"No I2C devices found\\n\");\n  else\n    Serial.println(\"done\\n\");\n}\n"
  },
  {
    "path": "MPU9250_MS5637/quaternionFilters.ino",
    "content": "// Implementation of Sebastian Madgwick's \"...efficient orientation filter for... inertial/magnetic sensor arrays\"\n// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)\n// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute\n// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.\n// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms\n// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!\n        void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)\n        {\n            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability\n            float norm;\n            float hx, hy, _2bx, _2bz;\n            float s1, s2, s3, s4;\n            float qDot1, qDot2, qDot3, qDot4;\n\n            // Auxiliary variables to avoid repeated arithmetic\n            float _2q1mx;\n            float _2q1my;\n            float _2q1mz;\n            float _2q2mx;\n            float _4bx;\n            float _4bz;\n            float _2q1 = 2.0f * q1;\n            float _2q2 = 2.0f * q2;\n            float _2q3 = 2.0f * q3;\n            float _2q4 = 2.0f * q4;\n            float _2q1q3 = 2.0f * q1 * q3;\n            float _2q3q4 = 2.0f * q3 * q4;\n            float q1q1 = q1 * q1;\n            float q1q2 = q1 * q2;\n            float q1q3 = q1 * q3;\n            float q1q4 = q1 * q4;\n            float q2q2 = q2 * q2;\n            float q2q3 = q2 * q3;\n            float q2q4 = q2 * q4;\n            float q3q3 = q3 * q3;\n            float q3q4 = q3 * q4;\n            float q4q4 = q4 * q4;\n\n            // Normalise accelerometer measurement\n            norm = sqrtf(ax * ax + ay * ay + az * az);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f/norm;\n            ax *= norm;\n            ay *= norm;\n            az *= norm;\n\n            // Normalise magnetometer measurement\n            norm = sqrtf(mx * mx + my * my + mz * mz);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f/norm;\n            mx *= norm;\n            my *= norm;\n            mz *= norm;\n\n            // Reference direction of Earth's magnetic field\n            _2q1mx = 2.0f * q1 * mx;\n            _2q1my = 2.0f * q1 * my;\n            _2q1mz = 2.0f * q1 * mz;\n            _2q2mx = 2.0f * q2 * mx;\n            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;\n            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;\n            _2bx = sqrtf(hx * hx + hy * hy);\n            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;\n            _4bx = 2.0f * _2bx;\n            _4bz = 2.0f * _2bz;\n\n            // Gradient decent algorithm corrective step\n            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);\n            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);\n            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);\n            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);\n            norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude\n            norm = 1.0f/norm;\n            s1 *= norm;\n            s2 *= norm;\n            s3 *= norm;\n            s4 *= norm;\n\n            // Compute rate of change of quaternion\n            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;\n            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;\n            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;\n            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;\n\n            // Integrate to yield quaternion\n            q1 += qDot1 * deltat;\n            q2 += qDot2 * deltat;\n            q3 += qDot3 * deltat;\n            q4 += qDot4 * deltat;\n            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion\n            norm = 1.0f/norm;\n            q[0] = q1 * norm;\n            q[1] = q2 * norm;\n            q[2] = q3 * norm;\n            q[3] = q4 * norm;\n\n        }\n  \n  \n  \n // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and\n // measured ones. \n            void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)\n        {\n            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability\n            float norm;\n            float hx, hy, bx, bz;\n            float vx, vy, vz, wx, wy, wz;\n            float ex, ey, ez;\n            float pa, pb, pc;\n\n            // Auxiliary variables to avoid repeated arithmetic\n            float q1q1 = q1 * q1;\n            float q1q2 = q1 * q2;\n            float q1q3 = q1 * q3;\n            float q1q4 = q1 * q4;\n            float q2q2 = q2 * q2;\n            float q2q3 = q2 * q3;\n            float q2q4 = q2 * q4;\n            float q3q3 = q3 * q3;\n            float q3q4 = q3 * q4;\n            float q4q4 = q4 * q4;   \n\n            // Normalise accelerometer measurement\n            norm = sqrtf(ax * ax + ay * ay + az * az);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f / norm;        // use reciprocal for division\n            ax *= norm;\n            ay *= norm;\n            az *= norm;\n\n            // Normalise magnetometer measurement\n            norm = sqrtf(mx * mx + my * my + mz * mz);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f / norm;        // use reciprocal for division\n            mx *= norm;\n            my *= norm;\n            mz *= norm;\n\n            // Reference direction of Earth's magnetic field\n            hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);\n            hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);\n            bx = sqrtf((hx * hx) + (hy * hy));\n            bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);\n\n            // Estimated direction of gravity and magnetic field\n            vx = 2.0f * (q2q4 - q1q3);\n            vy = 2.0f * (q1q2 + q3q4);\n            vz = q1q1 - q2q2 - q3q3 + q4q4;\n            wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);\n            wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);\n            wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  \n\n            // Error is cross product between estimated direction and measured direction of gravity\n            ex = (ay * vz - az * vy) + (my * wz - mz * wy);\n            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);\n            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);\n            if (Ki > 0.0f)\n            {\n                eInt[0] += ex;      // accumulate integral error\n                eInt[1] += ey;\n                eInt[2] += ez;\n            }\n            else\n            {\n                eInt[0] = 0.0f;     // prevent integral wind up\n                eInt[1] = 0.0f;\n                eInt[2] = 0.0f;\n            }\n\n            // Apply feedback terms\n            gx = gx + Kp * ex + Ki * eInt[0];\n            gy = gy + Kp * ey + Ki * eInt[1];\n            gz = gz + Kp * ez + Ki * eInt[2];\n\n            // Integrate rate of change of quaternion\n            pa = q2;\n            pb = q3;\n            pc = q4;\n            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);\n            q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);\n            q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);\n            q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);\n\n            // Normalise quaternion\n            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);\n            norm = 1.0f / norm;\n            q[0] = q1 * norm;\n            q[1] = q2 * norm;\n            q[2] = q3 * norm;\n            q[3] = q4 * norm;\n \n}\n"
  },
  {
    "path": "MPU9250_MS5637/readme.md",
    "content": "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.\n\n![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-07-07T22:15:55.669Z-MPU9250MiniTop.2.jpg.855x570_q85_pad_rcrop.jpg)\n"
  },
  {
    "path": "MPU9250_MS5637_AHRS_UDP/MPU9250_MS5637_AHRS_UDP.ino",
    "content": "/* MPU9250_MS5637_ESP32 Basic Example Code\n by: Kris Winer\n date: December 14, 2016\n license: Beerware - Use this code however you'd like. If you \n find it useful you can buy me a beer some time.\n \n Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, \n getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to \n allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and \n Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.\n \n This sketch is intended specifically for the MPU9250+MS5637 Add-on shield.\n It uses SDA/SCL on pins 21/22, respectively, and it uses the Wire library.\n The MS5637 is a simple but high resolution pressure sensor, which can be used in its high resolution\n mode but with power consumption of 20 microAmp, or in a lower resolution mode with power consumption of\n only 1 microAmp. The choice will depend on the application.\n \n SDA and SCL should have external pull-up resistors (to 3.3V).\n 4K7 resistors are on the MPU9250+MS5637 breakout board.\n \n Hardware setup:\n MPU9250 Breakout --------- ESP32\n VDD ---------------------- 3.3V\n SDA ----------------------- 21\n SCL ----------------------- 22\n GND ---------------------- GND\n \n */\n#include \"Wire.h\"   \n\n// See MS5637-02BA03 Low Voltage Barometric Pressure Sensor Data Sheet\n#define MS5637_RESET      0x1E\n#define MS5637_CONVERT_D1 0x40\n#define MS5637_CONVERT_D2 0x50\n#define MS5637_ADC_READ   0x00\n\n// 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 \n// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map\n//\n//Magnetometer Registers\n#define AK8963_ADDRESS   0x0C\n#define WHO_AM_I_AK8963  0x00 // should return 0x48\n#define INFO             0x01\n#define AK8963_ST1       0x02  // data ready status bit 0\n#define AK8963_XOUT_L\t   0x03  // data\n#define AK8963_XOUT_H\t   0x04\n#define AK8963_YOUT_L\t   0x05\n#define AK8963_YOUT_H\t   0x06\n#define AK8963_ZOUT_L\t   0x07\n#define AK8963_ZOUT_H\t   0x08\n#define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2\n#define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0\n#define AK8963_ASTC      0x0C  // Self test control\n#define AK8963_I2CDIS    0x0F  // I2C disable\n#define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value\n#define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value\n#define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value\n\n#define SELF_TEST_X_GYRO 0x00                  \n#define SELF_TEST_Y_GYRO 0x01                                                                          \n#define SELF_TEST_Z_GYRO 0x02\n\n/*#define X_FINE_GAIN      0x03 // [7:0] fine gain\n#define Y_FINE_GAIN      0x04\n#define Z_FINE_GAIN      0x05\n#define XA_OFFSET_H      0x06 // User-defined trim values for accelerometer\n#define XA_OFFSET_L_TC   0x07\n#define YA_OFFSET_H      0x08\n#define YA_OFFSET_L_TC   0x09\n#define ZA_OFFSET_H      0x0A\n#define ZA_OFFSET_L_TC   0x0B */\n\n#define SELF_TEST_X_ACCEL 0x0D\n#define SELF_TEST_Y_ACCEL 0x0E    \n#define SELF_TEST_Z_ACCEL 0x0F\n\n#define SELF_TEST_A      0x10\n\n#define XG_OFFSET_H      0x13  // User-defined trim values for gyroscope\n#define XG_OFFSET_L      0x14\n#define YG_OFFSET_H      0x15\n#define YG_OFFSET_L      0x16\n#define ZG_OFFSET_H      0x17\n#define ZG_OFFSET_L      0x18\n#define SMPLRT_DIV       0x19\n#define CONFIG           0x1A\n#define GYRO_CONFIG      0x1B\n#define ACCEL_CONFIG     0x1C\n#define ACCEL_CONFIG2    0x1D\n#define LP_ACCEL_ODR     0x1E   \n#define WOM_THR          0x1F   \n\n#define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms\n#define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]\n#define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms\n\n#define FIFO_EN          0x23\n#define I2C_MST_CTRL     0x24   \n#define I2C_SLV0_ADDR    0x25\n#define I2C_SLV0_REG     0x26\n#define I2C_SLV0_CTRL    0x27\n#define I2C_SLV1_ADDR    0x28\n#define I2C_SLV1_REG     0x29\n#define I2C_SLV1_CTRL    0x2A\n#define I2C_SLV2_ADDR    0x2B\n#define I2C_SLV2_REG     0x2C\n#define I2C_SLV2_CTRL    0x2D\n#define I2C_SLV3_ADDR    0x2E\n#define I2C_SLV3_REG     0x2F\n#define I2C_SLV3_CTRL    0x30\n#define I2C_SLV4_ADDR    0x31\n#define I2C_SLV4_REG     0x32\n#define I2C_SLV4_DO      0x33\n#define I2C_SLV4_CTRL    0x34\n#define I2C_SLV4_DI      0x35\n#define I2C_MST_STATUS   0x36\n#define INT_PIN_CFG      0x37\n#define INT_ENABLE       0x38\n#define DMP_INT_STATUS   0x39  // Check DMP interrupt\n#define INT_STATUS       0x3A\n#define ACCEL_XOUT_H     0x3B\n#define ACCEL_XOUT_L     0x3C\n#define ACCEL_YOUT_H     0x3D\n#define ACCEL_YOUT_L     0x3E\n#define ACCEL_ZOUT_H     0x3F\n#define ACCEL_ZOUT_L     0x40\n#define TEMP_OUT_H       0x41\n#define TEMP_OUT_L       0x42\n#define GYRO_XOUT_H      0x43\n#define GYRO_XOUT_L      0x44\n#define GYRO_YOUT_H      0x45\n#define GYRO_YOUT_L      0x46\n#define GYRO_ZOUT_H      0x47\n#define GYRO_ZOUT_L      0x48\n#define EXT_SENS_DATA_00 0x49\n#define EXT_SENS_DATA_01 0x4A\n#define EXT_SENS_DATA_02 0x4B\n#define EXT_SENS_DATA_03 0x4C\n#define EXT_SENS_DATA_04 0x4D\n#define EXT_SENS_DATA_05 0x4E\n#define EXT_SENS_DATA_06 0x4F\n#define EXT_SENS_DATA_07 0x50\n#define EXT_SENS_DATA_08 0x51\n#define EXT_SENS_DATA_09 0x52\n#define EXT_SENS_DATA_10 0x53\n#define EXT_SENS_DATA_11 0x54\n#define EXT_SENS_DATA_12 0x55\n#define EXT_SENS_DATA_13 0x56\n#define EXT_SENS_DATA_14 0x57\n#define EXT_SENS_DATA_15 0x58\n#define EXT_SENS_DATA_16 0x59\n#define EXT_SENS_DATA_17 0x5A\n#define EXT_SENS_DATA_18 0x5B\n#define EXT_SENS_DATA_19 0x5C\n#define EXT_SENS_DATA_20 0x5D\n#define EXT_SENS_DATA_21 0x5E\n#define EXT_SENS_DATA_22 0x5F\n#define EXT_SENS_DATA_23 0x60\n#define MOT_DETECT_STATUS 0x61\n#define I2C_SLV0_DO      0x63\n#define I2C_SLV1_DO      0x64\n#define I2C_SLV2_DO      0x65\n#define I2C_SLV3_DO      0x66\n#define I2C_MST_DELAY_CTRL 0x67\n#define SIGNAL_PATH_RESET  0x68\n#define MOT_DETECT_CTRL  0x69\n#define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP\n#define PWR_MGMT_1       0x6B // Device defaults to the SLEEP mode\n#define PWR_MGMT_2       0x6C\n#define DMP_BANK         0x6D  // Activates a specific bank in the DMP\n#define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank\n#define DMP_REG          0x6F  // Register in DMP from which to read or to which to write\n#define DMP_REG_1        0x70\n#define DMP_REG_2        0x71 \n#define FIFO_COUNTH      0x72\n#define FIFO_COUNTL      0x73\n#define FIFO_R_W         0x74\n#define WHO_AM_I_MPU9250 0x75 // Should return 0x71\n#define XA_OFFSET_H      0x77\n#define XA_OFFSET_L      0x78\n#define YA_OFFSET_H      0x7A\n#define YA_OFFSET_L      0x7B\n#define ZA_OFFSET_H      0x7D\n#define ZA_OFFSET_L      0x7E\n\n// Using the MPU9250_MS5637 Add-On shield, ADO is set to 0 \n// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1\n#define ADO 0\n#if ADO\n#define MPU9250_ADDRESS 0x69  // Device address when ADO = 1\n#define AK8963_ADDRESS 0x0C   //  Address of magnetometer\n#define MS5637_ADDRESS 0x76   // Address of altimeter\n#else\n#define MPU9250_ADDRESS 0x68  // Device address when ADO = 0\n#define AK8963_ADDRESS 0x0C   //  Address of magnetometer\n#define MS5637_ADDRESS 0x76   // Address of altimeter\n#endif  \n\n#define SerialDebug false  // set to true to get Serial output for debugging\n\n// Set initial input parameters\nenum Ascale {\n  AFS_2G = 0,\n  AFS_4G,\n  AFS_8G,\n  AFS_16G\n};\n\nenum Gscale {\n  GFS_250DPS = 0,\n  GFS_500DPS,\n  GFS_1000DPS,\n  GFS_2000DPS\n};\n\nenum Mscale {\n  MFS_14BITS = 0, // 0.6 mG per LSB\n  MFS_16BITS      // 0.15 mG per LSB\n};\n\n#define ADC_256  0x00 // define pressure and temperature conversion rates\n#define ADC_512  0x02\n#define ADC_1024 0x04\n#define ADC_2048 0x06\n#define ADC_4096 0x08\n#define ADC_8192 0x0A\n#define ADC_D1   0x40\n#define ADC_D2   0x50\n\n// Specify sensor full scale\nuint8_t OSR = ADC_8192;     // set pressure amd temperature oversample rate\nuint8_t Gscale = GFS_250DPS;\nuint8_t Ascale = AFS_2G;\nuint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution\nuint8_t Mmode = 0x06;        // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read\nfloat aRes, gRes, mRes;      // scale resolutions per LSB for the sensors\n\n// Pin definitions\nint intPin = 14;  // can be any pin\nbool newData = false;\nbool newMagData = false;\n\nint myLed = 5;\n\nuint16_t Pcal[8];         // calibration constants from MS5637 PROM registers\nunsigned char nCRC;       // calculated check sum to ensure PROM integrity\nuint32_t D1 = 0, D2 = 0;  // raw MS5637 pressure and temperature data\ndouble dT, OFFSET, SENS, TT2, OFFSET2, SENS2;  // First order and second order corrections for raw S5637 temperature and pressure data\n\nint16_t MPU9250Data[7]; // used to read all 14 bytes at once from the MPU9250 accel/gyro\nint16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output\nint16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output\nint16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output\nfloat magCalibration[3] = {0, 0, 0};  // Factory mag calibration and mag bias\nfloat 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\nint16_t tempCount;            // temperature raw count output\nfloat   temperature;          // Stores the MPU9250 gyro internal chip temperature in degrees Celsius\ndouble Temperature, Pressure; // stores MS5637 pressures sensor pressure and temperature\nfloat SelfTest[6];            // holds results of gyro and accelerometer self test\n\n// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)\nfloat pi = 3.141592653589793238462643383279502884f;\nfloat GyroMeasError = PI * (4.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 40 deg/s)\nfloat GyroMeasDrift = PI * (0.0f  / 180.0f);   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)\n// There is a tradeoff in the beta parameter between accuracy and response speed.\n// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.\n// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.\n// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!\n// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec\n// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; \n// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. \n// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.\nfloat beta = sqrt(3.0f / 4.0f) * GyroMeasError;   // compute beta\nfloat 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\n#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\n#define Ki 0.0f\n\nuint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate\nfloat pitch, yaw, roll;\nfloat a12, a22, a31, a32, a33;            // rotation matrix coefficients for Euler angles and gravity components\nfloat deltat = 0.0f, sum = 0.0f;          // integration interval for both filter schemes\nuint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval\nuint32_t Now = 0;                         // used to calculate integration interval\n\nfloat ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values \nfloat lin_ax, lin_ay, lin_az;             // linear acceleration (acceleration with gravity component subtracted)\nfloat q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion\nfloat eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method\n\n\n//#include <ESP8266WiFi.h>  //use for ESP8266\n#include <WiFi.h>       // use for ESP32\n#include <WiFiUdp.h>\n\n#define sendInterval 100\n\nconst char* ssid = \"network-name\";\nconst char* password = \"password\";\nWiFiUDP Udp;\nstatic IPAddress remoteIp = IPAddress();\nstatic uint16_t remotePort = 4210;\n\nunsigned int localUdpPort = 4210;  // local port to listen on\nchar incomingPacket[256];  // buffer for incoming packets\nString payload;\n\n//The command from the PC\nString cmd;\nfloat val_array[21]; \n\nvoid setup()\n{\n  Serial.begin(115200);\n  //Setup wifi and udp connection\n    Serial.println();\n    Serial.printf(\"Connecting to %s \", ssid);\n    \n    // delete old config\n    WiFi.disconnect(true);\n    \n    WiFi.begin(ssid, password);\n    while (WiFi.status() != WL_CONNECTED)\n    {\n        delay(500);\n        Serial.print(\".\");\n    }\n    Serial.println(\" connected\");\n    Udp.begin(localUdpPort);\n    Serial.printf(\"Now listening at IP %s, UDP port %d\\n\", WiFi.localIP().toString().c_str(), localUdpPort);\n    WiFi.localIP().toString().c_str(), localUdpPort;\n\n \n  delay(4000);\n  \n  Wire.begin(21, 22, 400000); //(SDA, SCL) (21,22) are default on ESP32, 400 kHz I2C clock\n  delay(1000);\n  \n  // Set up the interrupt pin, its set as active high, push-pull\n  pinMode(intPin, INPUT);\n  pinMode(myLed, OUTPUT);\n  digitalWrite(myLed, LOW);\n  \n  I2Cscan();// look for I2C devices on the bus\n    \n  // Read the WHO_AM_I register, this is a good test of communication\n  Serial.println(\"MPU9250 9-axis motion sensor...\");\n  uint8_t c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250\n  Serial.print(\"MPU9250 \"); Serial.print(\"I AM \"); Serial.print(c, HEX); Serial.print(\" I should be \"); Serial.println(0x71, HEX);\n\n  delay(1000); \n\n  if (c == 0x71) // WHO_AM_I should always be 0x71\n  {  \n    Serial.println(\"MPU9250 is online...\");\n    \n    MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values\n    Serial.print(\"x-axis self test: acceleration trim within : \"); Serial.print(SelfTest[0],1); Serial.println(\"% of factory value\");\n    Serial.print(\"y-axis self test: acceleration trim within : \"); Serial.print(SelfTest[1],1); Serial.println(\"% of factory value\");\n    Serial.print(\"z-axis self test: acceleration trim within : \"); Serial.print(SelfTest[2],1); Serial.println(\"% of factory value\");\n    Serial.print(\"x-axis self test: gyration trim within : \"); Serial.print(SelfTest[3],1); Serial.println(\"% of factory value\");\n    Serial.print(\"y-axis self test: gyration trim within : \"); Serial.print(SelfTest[4],1); Serial.println(\"% of factory value\");\n    Serial.print(\"z-axis self test: gyration trim within : \"); Serial.print(SelfTest[5],1); Serial.println(\"% of factory value\");\n    delay(1000);\n    \n   // get sensor resolutions, only need to do this once\n   getAres();\n   getGres();\n   getMres();\n    \n   Serial.println(\" Calibrate gyro and accel\");\n   accelgyrocalMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers\n   Serial.println(\"accel biases (mg)\"); Serial.println(1000.*accelBias[0]); Serial.println(1000.*accelBias[1]); Serial.println(1000.*accelBias[2]);\n   Serial.println(\"gyro biases (dps)\"); Serial.println(gyroBias[0]); Serial.println(gyroBias[1]); Serial.println(gyroBias[2]);\n\n  delay(1000);  \n   \n  initMPU9250(); \n  Serial.println(\"MPU9250 initialized for active data mode....\"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature\n  \n  // Read the WHO_AM_I register of the magnetometer, this is a good test of communication\n  byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);  // Read WHO_AM_I register for AK8963\n  Serial.print(\"AK8963 \"); Serial.print(\"I AM \"); Serial.print(d, HEX); Serial.print(\" I should be \"); Serial.println(0x48, HEX);\n \n  delay(1000); \n  \n  // Get magnetometer calibration from AK8963 ROM\n  initAK8963(magCalibration); Serial.println(\"AK8963 initialized for active data mode....\"); // Initialize device for active mode read of magnetometer\n  \n  magcalMPU9250(magBias, magScale);\n  Serial.println(\"AK8963 mag biases (mG)\"); Serial.println(magBias[0]); Serial.println(magBias[1]); Serial.println(magBias[2]); \n  Serial.println(\"AK8963 mag scale (mG)\"); Serial.println(magScale[0]); Serial.println(magScale[1]); Serial.println(magScale[2]); \n  delay(2000); // add delay to see results before serial spew of data\n   \n  if(SerialDebug) {\n//  Serial.println(\"Calibration values: \");\n  Serial.print(\"X-Axis sensitivity adjustment value \"); Serial.println(magCalibration[0], 2);\n  Serial.print(\"Y-Axis sensitivity adjustment value \"); Serial.println(magCalibration[1], 2);\n  Serial.print(\"Z-Axis sensitivity adjustment value \"); Serial.println(magCalibration[2], 2);\n  }\n\n  delay(1000);  \n  \n  // Reset the MS5637 pressure sensor\n  MS5637Reset();\n  delay(100);\n  Serial.println(\"MS5637 pressure sensor reset...\");\n  // Read PROM data from MS5637 pressure sensor\n  MS5637PromRead(Pcal);\n  Serial.println(\"PROM dta read:\");\n  Serial.print(\"C0 = \"); Serial.println(Pcal[0]);\n  unsigned char refCRC = Pcal[0] >> 12;\n  Serial.print(\"C1 = \"); Serial.println(Pcal[1]);\n  Serial.print(\"C2 = \"); Serial.println(Pcal[2]);\n  Serial.print(\"C3 = \"); Serial.println(Pcal[3]);\n  Serial.print(\"C4 = \"); Serial.println(Pcal[4]);\n  Serial.print(\"C5 = \"); Serial.println(Pcal[5]);\n  Serial.print(\"C6 = \"); Serial.println(Pcal[6]);\n  \n  nCRC = MS5637checkCRC(Pcal);  //calculate checksum to ensure integrity of MS5637 calibration data\n  Serial.print(\"Checksum = \"); Serial.print(nCRC); Serial.print(\" , should be \"); Serial.println(refCRC);  \n \n  delay(1000);  \n\n  attachInterrupt(intPin, myinthandler, RISING);  // define interrupt for INT pin output of MPU9250\n\n  }\n  else\n  {\n    Serial.print(\"Could not connect to MPU9250: 0x\");\n    Serial.println(c, HEX);\n    while(1) ; // Loop forever if communication doesn't happen\n  }\n  \n  //Setup wifi and udp connection\n    Serial.println();\n    Serial.printf(\"Connecting to %s \", ssid);\n    \n    // delete old config\n    WiFi.disconnect(true);\n    \n    WiFi.begin(ssid, password);\n    while (WiFi.status() != WL_CONNECTED)\n    {\n        delay(500);\n        Serial.print(\".\");\n    }\n    Serial.println(\" connected\");\n    Udp.begin(localUdpPort);\n    Serial.printf(\"Now listening at IP %s, UDP port %d\\n\", WiFi.localIP().toString().c_str(), localUdpPort);\n    WiFi.localIP().toString().c_str(), localUdpPort;\n\n}\n\nvoid loop()\n{  \n    int packetSize = Udp.parsePacket();\n    if (packetSize)\n    {\n        // receive incoming UDP packets\n        remoteIp = Udp.remoteIP();\n        remotePort = Udp.remotePort();\n        Serial.printf(\"Received %d bytes from %s, port %d\\n\", packetSize, remoteIp.toString().c_str(), remotePort);\n        int len = Udp.read(incomingPacket, 255);\n        incomingPacket[len] = 0;\n        Serial.printf(\"UDP packet contents: %s\\n\", incomingPacket);\n        \n        cmd = incomingPacket[0];\n        if(cmd==\"s\") {\n\t\t\t      Serial.print(\"Hit s to continue...\");\n            Serial.print('\\n');\n        }\n        else if(cmd==\"x\"){\n        WiFi.disconnect(true);\n        Serial.println(\"Disconnected\");\n      }\n    }\n    \n    if(!remotePort){\n        //nobody have connected yet\n        return;\n    }\n    \n\n   // If intPin goes high, all data registers have new data\n   if(newData == true) {  // On interrupt, read data\n     newData = false;  // reset newData flag\n     readMPU9250Data(MPU9250Data); // INT cleared on any read\n   \n    // Now we'll calculate the accleration value into actual g's\n     ax = (float)MPU9250Data[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set\n     ay = (float)MPU9250Data[1]*aRes - accelBias[1];   \n     az = (float)MPU9250Data[2]*aRes - accelBias[2];  \n\n    // Calculate the gyro value into actual degrees per second\n     gx = (float)MPU9250Data[4]*gRes;  // get actual gyro value, this depends on scale being set\n     gy = (float)MPU9250Data[5]*gRes;  \n     gz = (float)MPU9250Data[6]*gRes; \n  \n    newMagData = (readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01);\n    if(newMagData == true) { // wait for magnetometer data ready bit to be set\n      readMagData(magCount);  // Read the x/y/z adc values\n  \n    // Calculate the magnetometer values in milliGauss\n    // Include factory calibration per data sheet and user environmental corrections\n      mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0];  // get actual magnetometer value, this depends on scale being set\n      my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];  \n      mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];  \n      mx *= magScale[0];\n      my *= magScale[1];\n      mz *= magScale[2]; \n    }\n    \n    for(uint8_t i = 0; i < 10; i++) { // iterate a fixed number of times per data read cycle\n      Now = micros();\n      deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update\n      lastUpdate = Now;\n\n      sum += deltat; // sum for averaging filter update rate\n      sumCount++;\n\n      MadgwickQuaternionUpdate(-ax, ay, az, gx*pi/180.0f, -gy*pi/180.0f, -gz*pi/180.0f,  my,  -mx, mz);\n    }\n   } \n\n   // Serial print and/or display at 0.5 s rate independent of data rates\n    delt_t = millis() - count;\n    if (delt_t > sendInterval) { // update LCD once per half-second independent of read rate\n\n      if(SerialDebug) {\n        Serial.print(\"ax = \"); Serial.print((int)1000*ax);  \n        Serial.print(\" ay = \"); Serial.print((int)1000*ay); \n        Serial.print(\" az = \"); Serial.print((int)1000*az); Serial.println(\" mg\");\n        Serial.print(\"gx = \"); Serial.print( gx, 2); \n        Serial.print(\" gy = \"); Serial.print( gy, 2); \n        Serial.print(\" gz = \"); Serial.print( gz, 2); Serial.println(\" deg/s\");\n        Serial.print(\"mx = \"); Serial.print( (int)mx ); \n        Serial.print(\" my = \"); Serial.print( (int)my ); \n        Serial.print(\" mz = \"); Serial.print( (int)mz ); Serial.println(\" mG\");\n    \n        Serial.print(\"q0 = \"); Serial.print(q[0]);\n        Serial.print(\" qx = \"); Serial.print(q[1]); \n        Serial.print(\" qy = \"); Serial.print(q[2]); \n        Serial.print(\" qz = \"); Serial.println(q[3]); \n      }\n                     \n      tempCount = readTempData();  // Read the gyro adc values\n      temperature = ((float) tempCount) / 333.87f + 21.0f; // Gyro chip temperature in degrees Centigrade\n      \n      if(SerialDebug) {\n      // Print temperature in degrees Centigrade      \n        Serial.print(\"Gyro temperature is \");  Serial.print(temperature, 1);  Serial.println(\" degrees C\"); // Print T values to tenths of s degree C\n      }\n      \n      D1 = MS5637Read(ADC_D1, OSR);  // get raw pressure value\n      D2 = MS5637Read(ADC_D2, OSR);  // get raw temperature value\n      dT = D2 - Pcal[5]*pow(2,8);    // calculate temperature difference from reference\n      OFFSET = Pcal[2]*pow(2, 17) + dT*Pcal[4]/pow(2,6);\n      SENS = Pcal[1]*pow(2,16) + dT*Pcal[3]/pow(2,7);\n \n      Temperature = (2000 + (dT*Pcal[6])/pow(2, 23))/100;           // First-order Temperature in degrees Centigrade\n\n      //\n      // Second order corrections\n      if(Temperature > 20) \n      {\n        TT2 = 5*dT*dT/pow(2, 38); // correction for high temperatures\n        OFFSET2 = 0;\n        SENS2 = 0;\n      }\n      if(Temperature < 20)                   // correction for low temperature\n      {\n        TT2      = 3*dT*dT/pow(2, 33); \n        OFFSET2 = 61*(100*Temperature - 2000)*(100*Temperature - 2000)/16;\n        SENS2   = 29*(100*Temperature - 2000)*(100*Temperature - 2000)/16;\n      } \n      if(Temperature < -15)                      // correction for very low temperature\n      {\n        OFFSET2 = OFFSET2 + 17*(100*Temperature + 1500)*(100*Temperature + 1500);\n        SENS2 = SENS2 + 9*(100*Temperature + 1500)*(100*Temperature + 1500);\n      }\n      // End of second order corrections\n      //\n    \n     Temperature = Temperature - T2/100;\n     OFFSET = OFFSET - OFFSET2;\n     SENS = SENS - SENS2;\n \n     Pressure = (((D1*SENS)/pow(2, 21) - OFFSET)/pow(2, 15))/100;  // Pressure in mbar or kPa\n\n    float altitude = 145366.45*(1. - pow((Pressure/1013.25), 0.190284));\n   \n    if(SerialDebug) {\n      Serial.print(\"Digital temperature value = \"); Serial.print( (float)Temperature, 2); Serial.println(\" C\"); // temperature in degrees Celsius\n      Serial.print(\"Digital temperature value = \"); Serial.print(9.*(float) Temperature/5. + 32., 2); Serial.println(\" F\"); // temperature in degrees Fahrenheit\n      Serial.print(\"Digital pressure value = \"); Serial.print((float) Pressure, 2);  Serial.println(\" mbar\");// pressure in millibar\n      Serial.print(\"Altitude = \"); Serial.print(altitude, 2); Serial.println(\" feet\");\n    }\n    \n   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.\n   // In this coordinate system, the positive z-axis is down toward Earth. \n   // 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.\n   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.\n   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.\n   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.\n   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be\n   // applied in the correct order which for this configuration is yaw, pitch, and then roll.\n   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.\n   //Software AHRS:\n   //   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]);   \n   //   pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));\n   //   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]);\n   //   pitch *= 180.0f / PI;\n   //   yaw   *= 180.0f / PI; \n   //   yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04\n   //   if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360\n   //   roll  *= 180.0f / PI;\n    a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);\n    a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];\n    a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);\n    a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);\n    a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];\n    pitch = -asin(a32);\n    roll  = atan2(a31, a33);\n    yaw   = atan2(a12, a22);\n    pitch *= 180.0f / pi;\n    yaw   *= 180.0f / pi; \n    yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04\n    if(yaw < 0) yaw   += 360.0f; // Ensure yaw stays between 0 and 360\n    roll  *= 180.0f / pi;\n    lin_ax = ax + a31;\n    lin_ay = ay + a32;\n    lin_az = az - a33;\n    \n    if(SerialDebug) {\n      Serial.print(\"Yaw, Pitch, Roll: \");\n      Serial.print(yaw, 2);\n      Serial.print(\", \");\n      Serial.print(pitch, 2);\n      Serial.print(\", \");\n      Serial.println(roll, 2);\n\n      Serial.print(\"Grav_x, Grav_y, Grav_z: \");\n      Serial.print(-a31*1000.0f, 2);\n      Serial.print(\", \");\n      Serial.print(-a32*1000.0f, 2);\n      Serial.print(\", \");\n      Serial.print(a33*1000.0f, 2);  Serial.println(\" mg\");\n      Serial.print(\"Lin_ax, Lin_ay, Lin_az: \");\n      Serial.print(lin_ax*1000.0f, 2);\n      Serial.print(\", \");\n      Serial.print(lin_ay*1000.0f, 2);\n      Serial.print(\", \");\n      Serial.print(lin_az*1000.0f, 2);  Serial.println(\" mg\");\n\n      Serial.print(\"sumCount = \"); Serial.println(sumCount);\n      Serial.print(\"sum = \"); Serial.println(sum);\n    \n      Serial.print(\"rate = \"); Serial.print((float)sumCount/sum, 2); Serial.println(\" Hz\");\n    }\n\t\n      val_array[15] = (float)sumCount/sum;        \n      val_array[7] = (gx * M_PI/180);\n      val_array[8] = (gy * M_PI/180);\n      val_array[9] = (gz * M_PI/180);\n      val_array[4] = (ax);\n      val_array[5] = (ay);\n      val_array[6] = (az);\n      val_array[10] = (mx);\n      val_array[11] = (my);\n      val_array[12] = (mz);\n      val_array[0] = (q[0]);\n      val_array[1] = (q[1]);\n      val_array[2] = (q[2]);\n      val_array[3] = (q[3]);\n      val_array[16] = yaw;\n      val_array[18] = 0;\n      val_array[19] = 0;\n      val_array[17] = altitude;\n      val_array[13] = (9.*(float) Temperature/5. + 32., 2);\n      val_array[14] = (float) Pressure;\n\n      payload = \"\";\n      serialPayloadFloatArr(val_array, 20);\n      payload += \"\\r\\n\";\n  \n      int payloadLen = payload.length();\n      byte message[payloadLen];  \n      payload.getBytes(message,payloadLen);\n    \n      Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());\n      Udp.write(message,sizeof(message));\n      Udp.endPacket();\n\n      //delay(sendInterval);\n\t\n      digitalWrite(myLed, !digitalRead(myLed));\n      count = millis(); \n      sumCount = 0;\n      sum = 0;    \n    }\n \n}\n\n//===================================================================================================================\n//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data\n//===================================================================================================================\n\nvoid myinthandler()\n{\n  newData = true;\n}\n\nvoid getMres() {\n  switch (Mscale)\n  {\n \t// Possible magnetometer scales (and their register bit settings) are:\n\t// 14 bit resolution (0) and 16 bit resolution (1)\n    case MFS_14BITS:\n          mRes = 10.*4912./8190.; // Proper scale to return milliGauss\n          break;\n    case MFS_16BITS:\n          mRes = 10.*4912./32760.0; // Proper scale to return milliGauss\n          break;\n  }\n}\n\nvoid getGres() {\n  switch (Gscale)\n  {\n \t// Possible gyro scales (and their register bit settings) are:\n\t// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11). \n        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:\n    case GFS_250DPS:\n          gRes = 250.0/32768.0;\n          break;\n    case GFS_500DPS:\n          gRes = 500.0/32768.0;\n          break;\n    case GFS_1000DPS:\n          gRes = 1000.0/32768.0;\n          break;\n    case GFS_2000DPS:\n          gRes = 2000.0/32768.0;\n          break;\n  }\n}\n\nvoid getAres() {\n  switch (Ascale)\n  {\n \t// Possible accelerometer scales (and their register bit settings) are:\n\t// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11). \n        // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:\n    case AFS_2G:\n          aRes = 2.0/32768.0;\n          break;\n    case AFS_4G:\n          aRes = 4.0/32768.0;\n          break;\n    case AFS_8G:\n          aRes = 8.0/32768.0;\n          break;\n    case AFS_16G:\n          aRes = 16.0/32768.0;\n          break;\n  }\n}\n\nvoid readMPU9250Data(int16_t * destination)\n{\n  uint8_t rawData[14];  // x/y/z accel register data stored here\n  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, &rawData[0]);  // Read the 14 raw data registers into data array\n  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;  \n  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; \n  destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;   \n  destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;  \n  destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;  \n  destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ; \n}\n\nvoid readAccelData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z accel register data stored here\n  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array\n  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;  \n  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; \n}\n\n\nvoid readGyroData(int16_t * destination)\n{\n  uint8_t rawData[6];  // x/y/z gyro register data stored here\n  readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a signed 16-bit value\n  destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;  \n  destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; \n}\n\nvoid readMagData(int16_t * destination)\n{\n  uint8_t rawData[7];  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition\n  readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array\n  uint8_t c = rawData[6]; // End data read by reading ST2 register\n    if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data\n    destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;  // Turn the MSB and LSB into a signed 16-bit value\n    destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;  // Data stored as little Endian\n    destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ; \n   }\n}\n\nint16_t readTempData()\n{\n  uint8_t rawData[2];  // x/y/z gyro register data stored here\n  readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array \n  return ((int16_t)rawData[0] << 8) | rawData[1] ;  // Turn the MSB and LSB into a 16-bit value\n}\n       \nvoid initAK8963(float * destination)\n{\n  // First extract the factory calibration for each magnetometer axis\n  uint8_t rawData[3];  // x/y/z gyro calibration data stored here\n  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  \n  delay(10);\n  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode\n  delay(10);\n  readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);  // Read the x-, y-, and z-axis calibration values\n  destination[0] =  (float)(rawData[0] - 128)/256. + 1.;   // Return x-axis sensitivity adjustment values, etc.\n  destination[1] =  (float)(rawData[1] - 128)/256. + 1.;  \n  destination[2] =  (float)(rawData[2] - 128)/256. + 1.; \n  writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  \n  delay(10);\n  // Configure the magnetometer for continuous read and highest resolution\n  // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,\n  // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates\n  writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR\n  delay(10);\n}\n\n\nvoid initMPU9250()\n{  \n // wake up device\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors \n  delay(100); // Wait for all registers to reset \n\n // get stable time source\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  // Auto select clock source to be PLL gyroscope reference if ready else\n  delay(200); \n  \n // Configure Gyro and Thermometer\n // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; \n // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot\n // be higher than 1 / 0.0059 = 170 Hz\n // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both\n // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz\n  writeByte(MPU9250_ADDRESS, CONFIG, 0x03);  \n\n // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)\n  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);  // Use a 200 Hz rate; a rate consistent with the filter update rate \n                                    // determined inset in CONFIG above\n \n // Set gyroscope full scale range\n // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3\n  uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value\n // c = c & ~0xE0; // Clear self-test bits [7:5] \n  c = c & ~0x02; // Clear Fchoice bits [1:0] \n  c = c & ~0x18; // Clear AFS bits [4:3]\n  c = c | Gscale << 3; // Set full scale range for the gyro\n // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG\n  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register\n  \n // Set accelerometer full-scale range configuration\n  c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value\n // c = c & ~0xE0; // Clear self-test bits [7:5] \n  c = c & ~0x18;  // Clear AFS bits [4:3]\n  c = c | Ascale << 3; // Set full scale range for the accelerometer \n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value\n\n // Set accelerometer sample rate configuration\n // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for\n // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz\n  c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value\n  c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])  \n  c = c | 0x03;  // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz\n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value\n  \n // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, \n // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting\n\n  // Configure Interrupts and Bypass Enable\n  // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,\n  // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips \n  // can join the I2C bus and all can be controlled by the Arduino as master\n//   writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);    \n   writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x12);  // INT is 50 microsecond pulse and any read to clear  \n   writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);  // Enable data ready (bit 0) interrupt\n   delay(100);\n}\n\n\n// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average\n// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.\nvoid accelgyrocalMPU9250(float * dest1, float * dest2)\n{  \n  uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data\n  uint16_t ii, packet_count, fifo_count;\n  int32_t gyro_bias[3]  = {0, 0, 0}, accel_bias[3] = {0, 0, 0};\n  \n // reset device\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device\n  delay(100);\n   \n // get stable time source; Auto select clock source to be PLL gyroscope reference if ready \n // else use the internal oscillator, bits 2:0 = 001\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  \n  writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);\n  delay(200);                                    \n\n// Configure device for bias calculation\n  writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts\n  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);      // Disable FIFO\n  writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);   // Turn on internal clock source\n  writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master\n  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes\n  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C);    // Reset FIFO and DMP\n  delay(15);\n  \n// Configure MPU6050 gyro and accelerometer for bias calculation\n  writeByte(MPU9250_ADDRESS, CONFIG, 0x01);      // Set low-pass filter to 188 Hz\n  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);  // Set sample rate to 1 kHz\n  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity\n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity\n \n  uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec\n  uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g\n\n// Configure FIFO to capture accelerometer and gyro data for bias calculation\n  writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  \n  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO  (max size 512 bytes in MPU-9150)\n  delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes\n\n// At end of sample accumulation, turn off FIFO sensor read\n  writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO\n  readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count\n  fifo_count = ((uint16_t)data[0] << 8) | data[1];\n  packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging\n  \n  for (ii = 0; ii < packet_count; ii++) {\n    int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};\n    readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging\n    accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ;  // Form signed 16-bit integer for each sample in FIFO\n    accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;\n    accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;    \n    gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;\n    gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;\n    gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;\n    \n    accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases\n    accel_bias[1] += (int32_t) accel_temp[1];\n    accel_bias[2] += (int32_t) accel_temp[2];\n    gyro_bias[0]  += (int32_t) gyro_temp[0];\n    gyro_bias[1]  += (int32_t) gyro_temp[1];\n    gyro_bias[2]  += (int32_t) gyro_temp[2];\n            \n}\n    accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases\n    accel_bias[1] /= (int32_t) packet_count;\n    accel_bias[2] /= (int32_t) packet_count;\n    gyro_bias[0]  /= (int32_t) packet_count;\n    gyro_bias[1]  /= (int32_t) packet_count;\n    gyro_bias[2]  /= (int32_t) packet_count;\n    \n  if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation\n  else {accel_bias[2] += (int32_t) accelsensitivity;}\n   \n// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup\n  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\n  data[1] = (-gyro_bias[0]/4)       & 0xFF; // Biases are additive, so change sign on calculated average gyro biases\n  data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;\n  data[3] = (-gyro_bias[1]/4)       & 0xFF;\n  data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;\n  data[5] = (-gyro_bias[2]/4)       & 0xFF;\n  \n// Push gyro biases to hardware registers\n  writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);\n  writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);\n  writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);\n  writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);\n  writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);\n  writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);\n  \n// Output scaled gyro biases for display in the main program\n  dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;  \n  dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;\n  dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;\n\n// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain\n// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold\n// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature\n// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that\n// the accelerometer biases calculated above must be divided by 8.\n\n  int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases\n  readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values\n  accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);\n  readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);\n  accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);\n  readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);\n  accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);\n  \n  uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers\n  uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis\n  \n  for(ii = 0; ii < 3; ii++) {\n    if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit\n  }\n  \n  // Construct total accelerometer bias, including calculated average accelerometer bias from above\n  accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)\n  accel_bias_reg[1] -= (accel_bias[1]/8);\n  accel_bias_reg[2] -= (accel_bias[2]/8);\n  \n  data[0] = (accel_bias_reg[0] >> 8) & 0xFF;\n  data[1] = (accel_bias_reg[0])      & 0xFF;\n  data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers\n  data[2] = (accel_bias_reg[1] >> 8) & 0xFF;\n  data[3] = (accel_bias_reg[1])      & 0xFF;\n  data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers\n  data[4] = (accel_bias_reg[2] >> 8) & 0xFF;\n  data[5] = (accel_bias_reg[2])      & 0xFF;\n  data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers\n \n// Apparently this is not working for the acceleration biases in the MPU-9250\n// Are we handling the temperature correction bit properly?\n// Push accelerometer biases to hardware registers\n/*  writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);\n  writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);\n  writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);\n  writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);\n  writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);\n  writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);\n*/\n// Output scaled accelerometer biases for display in the main program\n   dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; \n   dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;\n   dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;\n}\n\n\nvoid magcalMPU9250(float * dest1, float * dest2) \n{\n  uint16_t ii = 0, sample_count = 0;\n  int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};\n  int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};\n\n  Serial.println(\"Mag Calibration: Wave device in a figure eight until done!\");\n  delay(4000);\n  \n    // shoot for ~fifteen seconds of mag data\n    if(Mmode == 0x02) sample_count = 128;  // at 8 Hz ODR, new mag data is available every 125 ms\n    if(Mmode == 0x06) sample_count = 1500;  // at 100 Hz ODR, new mag data is available every 10 ms\n   for(ii = 0; ii < sample_count; ii++) {\n    readMagData(mag_temp);  // Read the mag data   \n    for (int jj = 0; jj < 3; jj++) {\n      if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];\n      if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];\n    }\n    if(Mmode == 0x02) delay(135);  // at 8 Hz ODR, new mag data is available every 125 ms\n    if(Mmode == 0x06) delay(12);  // at 100 Hz ODR, new mag data is available every 10 ms\n    }\n\n//    Serial.println(\"mag x min/max:\"); Serial.println(mag_max[0]); Serial.println(mag_min[0]);\n//    Serial.println(\"mag y min/max:\"); Serial.println(mag_max[1]); Serial.println(mag_min[1]);\n//    Serial.println(\"mag z min/max:\"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);\n\n    // Get hard iron correction\n    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts\n    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts\n    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts\n    \n    dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0];  // save mag biases in G for main program\n    dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];   \n    dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];  \n       \n    // Get soft iron correction estimate\n    mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts\n    mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts\n    mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts\n\n    float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];\n    avg_rad /= 3.0;\n\n    dest2[0] = avg_rad/((float)mag_scale[0]);\n    dest2[1] = avg_rad/((float)mag_scale[1]);\n    dest2[2] = avg_rad/((float)mag_scale[2]);\n  \n   Serial.println(\"Mag Calibration done!\");\n}\n\n\n\n// Accelerometer and gyroscope self test; check calibration wrt factory settings\nvoid MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass\n{\n   uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};\n   uint8_t selfTest[6];\n   int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};\n   float factoryTrim[6];\n   uint8_t FS = 0;\n   \n  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);    // Set gyro sample rate to 1 kHz\n  writeByte(MPU9250_ADDRESS, CONFIG, 0x02);        // Set gyro sample rate to 1 kHz and DLPF to 92 Hz\n  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS);  // Set full scale range for the gyro to 250 dps\n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz\n  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g\n\n  for( int ii = 0; ii < 200; ii++) {  // get average current values of gyro and acclerometer\n  \n  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);        // Read the six raw data registers into data array\n  aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value\n  aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  \n  aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; \n  \n    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);       // Read the six raw data registers sequentially into data array\n  gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value\n  gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  \n  gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; \n  }\n  \n  for (int ii =0; ii < 3; ii++) {  // Get average of 200 values and store as average current readings\n  aAvg[ii] /= 200;\n  gAvg[ii] /= 200;\n  }\n  \n// Configure the accelerometer for self-test\n   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g\n   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s\n   delay(25);  // Delay a while to let the device stabilize\n\n  for( int ii = 0; ii < 200; ii++) {  // get average self-test values of gyro and acclerometer\n  \n  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array\n  aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value\n  aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  \n  aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; \n  \n    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array\n  gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value\n  gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  \n  gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; \n  }\n  \n  for (int ii =0; ii < 3; ii++) {  // Get average of 200 values and store as average self-test readings\n  aSTAvg[ii] /= 200;\n  gSTAvg[ii] /= 200;\n  }   \n  \n // Configure the gyro and accelerometer for normal operation\n   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);  \n   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0x00);  \n   delay(25);  // Delay a while to let the device stabilize\n   \n   // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg\n   selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results\n   selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results\n   selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results\n   selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO);  // X-axis gyro self-test results\n   selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO);  // Y-axis gyro self-test results\n   selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO);  // Z-axis gyro self-test results\n\n  // Retrieve factory self-test value from self-test code reads\n   factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation\n   factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation\n   factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation\n   factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation\n   factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation\n   factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation\n \n // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response\n // To get percent, must multiply by 100\n   for (int i = 0; i < 3; i++) {\n     destination[i]   = 100.0f*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.0f;   // Report percent differences\n     destination[i+3] = 100.0f*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.0f; // Report percent differences\n   }\n   \n}\n\n// I2C communication with the MS5637 is a little different from that with the MPU9250 and most other sensors\n// For the MS5637, we write commands, and the MS5637 sends data in response, rather than directly reading\n// MS5637 registers\n\n        void MS5637Reset()\n        {\n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(MS5637_RESET);                // Put reset command in Tx buffer\n        Wire.endTransmission();                  // Send the Tx buffer\n        }\n        \n        void MS5637PromRead(uint16_t * destination)\n        {\n        uint8_t data[2] = {0,0};\n        for (uint8_t ii = 0; ii < 7; ii++) {\n          Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n          Wire.write(0xA0 | ii << 1);              // Put PROM address in Tx buffer\n          Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n          uint8_t i = 0;\n          Wire.requestFrom(MS5637_ADDRESS, 2);     // Read two bytes from slave PROM address \n          while (Wire.available()) {\n          data[i++] = Wire.read(); }               // Put read results in the Rx buffer\n          destination[ii] = (uint16_t) (((uint16_t) data[0] << 8) | data[1]); // construct PROM data for return to main program\n        }\n        }\n\n        uint32_t MS5637Read(uint8_t CMD, uint8_t OSR)  // temperature data read\n        {\n        uint8_t data[3] = {0,0,0};\n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(CMD | OSR);                   // Put pressure conversion command in Tx buffer\n        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n        \n        switch (OSR)\n        {\n          case ADC_256: delay(1); break;         // delay for conversion to complete\n          case ADC_512: delay(3); break;\n          case ADC_1024: delay(4); break;\n          case ADC_2048: delay(6); break;\n          case ADC_4096: delay(10); break;\n          case ADC_8192: delay(20); break;\n        }\n       \n        Wire.beginTransmission(MS5637_ADDRESS);  // Initialize the Tx buffer\n        Wire.write(0x00);                        // Put ADC read command in Tx buffer\n        Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n        uint8_t i = 0;\n        Wire.requestFrom(MS5637_ADDRESS, 3);     // Read three bytes from slave PROM address \n        while (Wire.available()) {\n        data[i++] = Wire.read(); }               // Put read results in the Rx buffer\n        return (uint32_t) (((uint32_t) data[0] << 16) | (uint32_t) data[1] << 8 | data[2]); // construct PROM data for return to main program\n        }\n\n\n\nunsigned char MS5637checkCRC(uint16_t * n_prom)  // calculate checksum from PROM register contents\n{\n  int cnt;\n  unsigned int n_rem = 0;\n  unsigned char n_bit;\n  \n  n_prom[0] = ((n_prom[0]) & 0x0FFF);  // replace CRC byte by 0 for checksum calculation\n  n_prom[7] = 0;\n  for(cnt = 0; cnt < 16; cnt++)\n  {\n    if(cnt%2==1) n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);\n    else         n_rem ^= (unsigned short)  (n_prom[cnt>>1]>>8);\n    for(n_bit = 8; n_bit > 0; n_bit--)\n    {\n        if(n_rem & 0x8000)    n_rem = (n_rem<<1) ^ 0x3000;\n        else                  n_rem = (n_rem<<1);\n    }\n  }\n  n_rem = ((n_rem>>12) & 0x000F);\n  return (n_rem ^ 0x00);\n}\n\n\n\n// simple function to scan for I2C devices on the bus\nvoid I2Cscan() \n{\n    // scan for i2c devices\n  byte error, address;\n  int nDevices;\n\n  Serial.println(\"Scanning...\");\n\n  nDevices = 0;\n  for(address = 1; address < 127; address++ ) \n  {\n    // The i2c_scanner uses the return value of\n    // the Write.endTransmisstion to see if\n    // a device did acknowledge to the address.\n    Wire.beginTransmission(address);\n    error = Wire.endTransmission();\n\n    if (error == 0)\n    {\n      Serial.print(\"I2C device found at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.print(address,HEX);\n      Serial.println(\"  !\");\n\n      nDevices++;\n    }\n    else if (error==4) \n    {\n      Serial.print(\"Unknown error at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.println(address,HEX);\n    }    \n  }\n  if (nDevices == 0)\n    Serial.println(\"No I2C devices found\\n\");\n  else\n    Serial.println(\"done\\n\");\n}\n\n\n// I2C read/write functions for the MPU9250 sensors\n\n  void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)\n{\n  Wire.beginTransmission(address);  // Initialize the Tx buffer\n  Wire.write(subAddress);           // Put slave register address in Tx buffer\n  Wire.write(data);                 // Put data in Tx buffer\n  Wire.endTransmission();           // Send the Tx buffer\n}\n\n  uint8_t readByte(uint8_t address, uint8_t subAddress)\n{\n  uint8_t data = 0;                        // `data` will store the register data   \n  Wire.beginTransmission(address);         // Initialize the Tx buffer\n  Wire.write(subAddress);                  // Put slave register address in Tx buffer\n  Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive\n  Wire.requestFrom(address, 1);            // Read two bytes from slave register address on MPU9250 \n  data = Wire.read();                      // Fill Rx buffer with result\n  return data;                             // Return data read from slave register\n}\n\n  void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)\n{  \n  Wire.beginTransmission(address);   // Initialize the Tx buffer\n  Wire.write(subAddress);            // Put slave register address in Tx buffer\n  Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive\n  uint8_t i = 0;\n  Wire.requestFrom(address, count);  // Read bytes from slave register address \n  while (Wire.available()) {\n        dest[i++] = Wire.read(); }         // Put read results in the Rx buffer\n}\n\n\nvoid serialPayloadPrint(float f) {\n    byte * b = (byte *) &f;\n    for(int i=0; i<4; i++) {\n        \n        byte b1 = (b[i] >> 4) & 0x0f;\n        byte b2 = (b[i] & 0x0f);\n        \n        char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;\n        char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;\n        \n        payload += (c1);\n        payload += (c2);\n    }\n}\n\nvoid serialPayloadFloatArr(float * arr, int length) {\n    for(int i=0; i<length; i++) {\n        serialPayloadPrint(arr[i]);\n        payload += \",\";\n    }\n}\r\n"
  },
  {
    "path": "MPU9250_MS5637_AHRS_UDP/quaternionFilters.ino",
    "content": "\n// Implementation of Sebastian Madgwick's \"...efficient orientation filter for... inertial/magnetic sensor arrays\"\n// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)\n// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute\n// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.\n// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms\n// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!\n        void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)\n        {\n            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability\n            float norm;\n            float hx, hy, _2bx, _2bz;\n            float s1, s2, s3, s4;\n            float qDot1, qDot2, qDot3, qDot4;\n\n            // Auxiliary variables to avoid repeated arithmetic\n            float _2q1mx;\n            float _2q1my;\n            float _2q1mz;\n            float _2q2mx;\n            float _4bx;\n            float _4bz;\n            float _2q1 = 2.0f * q1;\n            float _2q2 = 2.0f * q2;\n            float _2q3 = 2.0f * q3;\n            float _2q4 = 2.0f * q4;\n            float _2q1q3 = 2.0f * q1 * q3;\n            float _2q3q4 = 2.0f * q3 * q4;\n            float q1q1 = q1 * q1;\n            float q1q2 = q1 * q2;\n            float q1q3 = q1 * q3;\n            float q1q4 = q1 * q4;\n            float q2q2 = q2 * q2;\n            float q2q3 = q2 * q3;\n            float q2q4 = q2 * q4;\n            float q3q3 = q3 * q3;\n            float q3q4 = q3 * q4;\n            float q4q4 = q4 * q4;\n\n            // Normalise accelerometer measurement\n            norm = sqrtf(ax * ax + ay * ay + az * az);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f/norm;\n            ax *= norm;\n            ay *= norm;\n            az *= norm;\n\n            // Normalise magnetometer measurement\n            norm = sqrtf(mx * mx + my * my + mz * mz);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f/norm;\n            mx *= norm;\n            my *= norm;\n            mz *= norm;\n\n            // Reference direction of Earth's magnetic field\n            _2q1mx = 2.0f * q1 * mx;\n            _2q1my = 2.0f * q1 * my;\n            _2q1mz = 2.0f * q1 * mz;\n            _2q2mx = 2.0f * q2 * mx;\n            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;\n            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;\n            _2bx = sqrtf(hx * hx + hy * hy);\n            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;\n            _4bx = 2.0f * _2bx;\n            _4bz = 2.0f * _2bz;\n\n            // Gradient decent algorithm corrective step\n            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);\n            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);\n            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);\n            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);\n            norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude\n            norm = 1.0f/norm;\n            s1 *= norm;\n            s2 *= norm;\n            s3 *= norm;\n            s4 *= norm;\n\n            // Compute rate of change of quaternion\n            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;\n            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;\n            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;\n            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;\n\n            // Integrate to yield quaternion\n            q1 += qDot1 * deltat;\n            q2 += qDot2 * deltat;\n            q3 += qDot3 * deltat;\n            q4 += qDot4 * deltat;\n            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion\n            norm = 1.0f/norm;\n            q[0] = q1 * norm;\n            q[1] = q2 * norm;\n            q[2] = q3 * norm;\n            q[3] = q4 * norm;\n\n        }\n  \n  \n  \n // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and\n // measured ones. \n            void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)\n        {\n            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability\n            float norm;\n            float hx, hy, bx, bz;\n            float vx, vy, vz, wx, wy, wz;\n            float ex, ey, ez;\n            float pa, pb, pc;\n\n            // Auxiliary variables to avoid repeated arithmetic\n            float q1q1 = q1 * q1;\n            float q1q2 = q1 * q2;\n            float q1q3 = q1 * q3;\n            float q1q4 = q1 * q4;\n            float q2q2 = q2 * q2;\n            float q2q3 = q2 * q3;\n            float q2q4 = q2 * q4;\n            float q3q3 = q3 * q3;\n            float q3q4 = q3 * q4;\n            float q4q4 = q4 * q4;   \n\n            // Normalise accelerometer measurement\n            norm = sqrtf(ax * ax + ay * ay + az * az);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f / norm;        // use reciprocal for division\n            ax *= norm;\n            ay *= norm;\n            az *= norm;\n\n            // Normalise magnetometer measurement\n            norm = sqrtf(mx * mx + my * my + mz * mz);\n            if (norm == 0.0f) return; // handle NaN\n            norm = 1.0f / norm;        // use reciprocal for division\n            mx *= norm;\n            my *= norm;\n            mz *= norm;\n\n            // Reference direction of Earth's magnetic field\n            hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);\n            hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);\n            bx = sqrtf((hx * hx) + (hy * hy));\n            bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);\n\n            // Estimated direction of gravity and magnetic field\n            vx = 2.0f * (q2q4 - q1q3);\n            vy = 2.0f * (q1q2 + q3q4);\n            vz = q1q1 - q2q2 - q3q3 + q4q4;\n            wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);\n            wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);\n            wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);  \n\n            // Error is cross product between estimated direction and measured direction of gravity\n            ex = (ay * vz - az * vy) + (my * wz - mz * wy);\n            ey = (az * vx - ax * vz) + (mz * wx - mx * wz);\n            ez = (ax * vy - ay * vx) + (mx * wy - my * wx);\n            if (Ki > 0.0f)\n            {\n                eInt[0] += ex;      // accumulate integral error\n                eInt[1] += ey;\n                eInt[2] += ez;\n            }\n            else\n            {\n                eInt[0] = 0.0f;     // prevent integral wind up\n                eInt[1] = 0.0f;\n                eInt[2] = 0.0f;\n            }\n\n            // Apply feedback terms\n            gx = gx + Kp * ex + Ki * eInt[0];\n            gy = gy + Kp * ey + Ki * eInt[1];\n            gz = gz + Kp * ez + Ki * eInt[2];\n\n            // Integrate rate of change of quaternion\n            pa = q2;\n            pb = q3;\n            pc = q4;\n            q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);\n            q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);\n            q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);\n            q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);\n\n            // Normalise quaternion\n            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);\n            norm = 1.0f / norm;\n            q[0] = q1 * norm;\n            q[1] = q2 * norm;\n            q[2] = q3 * norm;\n            q[3] = q4 * norm;\n \n        }\n"
  },
  {
    "path": "MPU9250_MS5637_AHRS_UDP/readme.md",
    "content": "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.\n\n![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-07-07T22:15:55.669Z-MPU9250MiniTop.2.jpg.855x570_q85_pad_rcrop.jpg)\n"
  },
  {
    "path": "PWM/ledcWrite_demo_ESP32.ino",
    "content": "/*\n  ledcWrite_demo_ESP32.ino\n  Runs through the full 255 color spectrum for an rgb led \n  Demonstrate ledcWrite functionality for driving leds with PWM on ESP32\n \n  This example code is in the public domain.\n */\n \n// Set up the rgb led names\nuint8_t ledR = 12;\nuint8_t ledG = 14; // internally pulled up\nuint8_t ledB = 13; \n\nuint8_t myLed = 5;  // on-board blue led (also internally pulled up)\n\nuint8_t ledArray[3] = {1, 2, 3}; // three led channels\n\nconst boolean invert = false; // set true if common anode, false if common cathode\n\nuint8_t color = 0;          // a value from 0 to 255 representing the hue\nuint32_t R, G, B;           // the Red Green and Blue color components\nuint8_t brightness = 255;  // 255 is maximum brightness, but can be changed\n\n// the setup routine runs once when you press reset:\nvoid setup() \n{             \n  pinMode(myLed, OUTPUT); \n  digitalWrite(myLed, LOW);// Turn off on-board blue led\n  \n  ledcAttachPin(ledR, 1); // assign RGB led pins to channels\n  ledcAttachPin(ledG, 2);\n  ledcAttachPin(ledB, 3);\n  \n  // Initialize channels \n  // channels 0-15, resolution 1-16 bits, freq limits depend on resolution\n  // ledcSetup(uint8_t channel, uint32_t freq, uint8_t resolution_bits);\n  ledcSetup(1, 12000, 8); // 12 kHz PWM, 8-bit resolution\n  ledcSetup(2, 12000, 8);\n  ledcSetup(3, 12000, 8);\n\n for(uint8_t i=0; i < 3; i++) {\n  // ledcWrite(channel, dutycycle)\n  // For 8-bit resolution duty cycle is 0 - 255\n  ledcWrite(ledArray[i], 255);  // test high output of all leds in sequence\n  delay(1000);\n  ledcWrite(ledArray[i], 0);\n }\n \n}\n\n// void loop runs over and over again\nvoid loop() \n{\n for (color = 0; color < 255; color++) { // Slew through the color spectrum\n\n  hueToRGB(color, brightness);  // call function to convert hue to RGB\n\n  // write the RGB values to the pins\n  ledcWrite(1, R); // write red component to channel 1, etc.\n  ledcWrite(2, G);   \n  ledcWrite(3, B); \n \n  delay(100); // full cycle of rgb over 256 colors takes 26 seconds\n }\n \n digitalWrite(myLed, HIGH); delay(10); digitalWrite(myLed, LOW); // indicate end of cycle\n}\n\n\n// Courtesy http://www.instructables.com/id/How-to-Use-an-RGB-LED/?ALLSTEPS\n// function to convert a color to its Red, Green, and Blue components.\n\nvoid hueToRGB(uint8_t hue, uint8_t brightness)\n{\n    uint16_t scaledHue = (hue * 6);\n    uint8_t segment = scaledHue / 256; // segment 0 to 5 around the\n                                            // color wheel\n    uint16_t segmentOffset =\n      scaledHue - (segment * 256); // position within the segment\n\n    uint8_t complement = 0;\n    uint16_t prev = (brightness * ( 255 -  segmentOffset)) / 256;\n    uint16_t next = (brightness *  segmentOffset) / 256;\n\n    if(invert)\n    {\n      brightness = 255 - brightness;\n      complement = 255;\n      prev = 255 - prev;\n      next = 255 - next;\n    }\n\n    switch(segment ) {\n    case 0:      // red\n        R = brightness;\n        G = next;\n        B = complement;\n    break;\n    case 1:     // yellow\n        R = prev;\n        G = brightness;\n        B = complement;\n    break;\n    case 2:     // green\n        R = complement;\n        G = brightness;\n        B = next;\n    break;\n    case 3:    // cyan\n        R = complement;\n        G = prev;\n        B = brightness;\n    break;\n    case 4:    // blue\n        R = next;\n        G = complement;\n        B = brightness;\n    break;\n   case 5:      // magenta\n    default:\n        R = brightness;\n        G = complement;\n        B = prev;\n    break;\n    }\n}\n"
  },
  {
    "path": "PWM/readme.md",
    "content": "Sketch using the ledcWrite utility to sweep through 256 colors of an rgb led using pins 12, 13, and 14 with setup as follows:\n\n![](https://cloud.githubusercontent.com/assets/6698410/21281621/25fb9a54-c3a3-11e6-9110-46ba3339c284.jpg)\n\nDon't forget the current-limiting resistors, here a 330-Ohm resistor array does the job.\n"
  },
  {
    "path": "README.md",
    "content": "# ESP32 Arduino sketch repository\n\nThese are sketches I have found to run on my own 1.4\" x 0.7\" ESP32 development board pictured below.\n\n![](https://cloud.githubusercontent.com/assets/6698410/21195535/0eff4500-c1e9-11e6-9fa1-deb1077887aa.jpg)\n\nThese 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/).\n"
  },
  {
    "path": "SPI/Readme.md",
    "content": "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.\n\n![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2015-03-16T03:24:10.978Z-S25FL127.front.jpg.855x570_q85_pad_rcrop.jpg)\n\nOutput for the SPIFlash test program should look like this for a Spansion S25FL127 128 Mbit SPI NOR flash:\n\nSerial enabled!\n\nID bytes: 1 20 18 4D\n\nWinbond W25Q80BLUX1G    Chip ID = 0xEF, 0x40, 0x14, 0x0\n\nMacronix MX25L12835FZNI Chip ID = 0xC2, 0x20, 0x18, 0xC2\n\nSpansion S25FL127S      Chip ID = 0x01, 0x20, 0x18, 0x4D\n \nWrite page:  0xFFFF\n\ntime (us) = 4470\n\nRead Page 0xFFFF\n\ntime (us) = 4813\n\n 0x0\n 0x1 0x2 0x3 0x4 0x5 0x6 0x7 0x8 0x9 0xA 0xB 0xC 0xD 0xE 0xF 0x10\n 0x11 0x12 0x13 0x14 0x15 0x16 0x17 0x18 0x19 0x1A 0x1B 0x1C 0x1D 0x1E 0x1F 0x20\n 0x21 0x22 0x23 0x24 0x25 0x26 0x27 0x28 0x29 0x2A 0x2B 0x2C 0x2D 0x2E 0x2F 0x30\n 0x31 0x32 0x33 0x34 0x35 0x36 0x37 0x38 0x39 0x3A 0x3B 0x3C 0x3D 0x3E 0x3F 0x40\n 0x41 0x42 0x43 0x44 0x45 0x46 0x47 0x48 0x49 0x4A 0x4B 0x4C 0x4D 0x4E 0x4F 0x50\n 0x51 0x52 0x53 0x54 0x55 0x56 0x57 0x58 0x59 0x5A 0x5B 0x5C 0x5D 0x5E 0x5F 0x60\n 0x61 0x62 0x63 0x64 0x65 0x66 0x67 0x68 0x69 0x6A 0x6B 0x6C 0x6D 0x6E 0x6F 0x70\n 0x71 0x72 0x73 0x74 0x75 0x76 0x77 0x78 0x79 0x7A 0x7B 0x7C 0x7D 0x7E 0x7F 0x80\n 0x81 0x82 0x83 0x84 0x85 0x86 0x87 0x88 0x89 0x8A 0x8B 0x8C 0x8D 0x8E 0x8F 0x90\n 0x91 0x92 0x93 0x94 0x95 0x96 0x97 0x98 0x99 0x9A 0x9B 0x9C 0x9D 0x9E 0x9F 0xA0\n 0xA1 0xA2 0xA3 0xA4 0xA5 0xA6 0xA7 0xA8 0xA9 0xAA 0xAB 0xAC 0xAD 0xAE 0xAF 0xB0\n 0xB1 0xB2 0xB3 0xB4 0xB5 0xB6 0xB7 0xB8 0xB9 0xBA 0xBB 0xBC 0xBD 0xBE 0xBF 0xC0\n 0xC1 0xC2 0xC3 0xC4 0xC5 0xC6 0xC7 0xC8 0xC9 0xCA 0xCB 0xCC 0xCD 0xCE 0xCF 0xD0\n 0xD1 0xD2 0xD3 0xD4 0xD5 0xD6 0xD7 0xD8 0xD9 0xDA 0xDB 0xDC 0xDD 0xDE 0xDF 0xE0\n 0xE1 0xE2 0xE3 0xE4 0xE5 0xE6 0xE7 0xE8 0xE9 0xEA 0xEB 0xEC 0xED 0xEE 0xEF 0xF0\n 0xF1 0xF2 0xF3 0xF4 0xF5 0xF6 0xF7 0xF8 0xF9 0xFA 0xFB 0xFC 0xFD 0xFE 0xFF\n \ntime (ms) = 42829\n\nRead Page 0xFFFF\n\ntime (us) = 4414\n\n 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF\n"
  },
  {
    "path": "SPI/SPIFlash_ESP32.ino",
    "content": "/* SPIFlash_ESP32.ino\nSketch by Kris Winer December 16, 2016\n\nLicense: Use this sketch any way you choose; if you like it, buy me a beer sometime\n\nPurpose: Checks function of a variety of SPI NOR flash memory chips hosted by the ESP32 development board.\n\nSketch based on the work of Pete (El Supremo) as follows:\n * Copyright (c) 2014, Pete (El Supremo), el_supremo@shaw.ca\n *\n * Development of this audio library was funded by PJRC.COM, LLC by sales of\n * Teensy and Audio Adaptor boards.  Please support PJRC's efforts to develop\n * open source software by purchasing Teensy or other PJRC products.\n *\n * Permission is hereby granted, free of charge, to any person obtaining a copy\n * of this software and associated documentation files (the \"Software\"), to deal\n * in the Software without restriction, including without limitation the rights\n * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\n * copies of the Software, and to permit persons to whom the Software is\n * furnished to do so, subject to the following conditions:\n *\n * The above copyright notice, development funding notice, and this permission\n * notice shall be included in all copies or substantial portions of the Software.\n */\n\n#include <SPI.h>\n\n// Highest page number is 0xffff=65535\nint page_number = 0xFFFF;\nunsigned char w_page[256];\nunsigned char r_page[256];\n\n#define CSPIN  5  // A1 for IU board\n\n#define STAT_WIP 1\n#define STAT_WEL 2\n\n#define CMD_WRITE_STATUS_REG   0x01\n#define CMD_PAGE_PROGRAM       0x02\n#define CMD_READ_DATA          0x03\n#define CMD_WRITE_DISABLE      0x04//not tested\n#define CMD_READ_STATUS_REG    0x05\n#define CMD_WRITE_ENABLE       0x06\n#define CMD_READ_HIGH_SPEED    0x0B//not tested\n#define CMD_SECTOR_ERASE       0x20//not tested\n#define CMD_BLOCK32K_ERASE     0x52//not tested\n#define CMD_RESET_DEVICE       0xF0//<<-different from winbond\n#define CMD_READ_ID            0x9F\n#define CMD_RELEASE_POWER_DOWN 0xAB//not tested\n#define CMD_POWER_DOWN         0xB9//not tested\n#define CMD_CHIP_ERASE         0xC7\n#define CMD_BLOCK64K_ERASE     0xD8//not tested\n\nunsigned char flash_wait_for_write = 0;\n\nvoid setup(void)\n{\n  pinMode(CSPIN, OUTPUT);\n\n  SPI.begin(18, 19, 23, 5); // sck, miso, mosi, ss (ss can be any GPIO)\n  pinMode(CSPIN, OUTPUT);\n  digitalWrite(CSPIN, HIGH);\n  \n  unsigned char id_tab[32];\n  unsigned long t_start;\n  \n  Serial.begin(115200);\n  delay(4000);\n  Serial.println(\"Serial enabled!\");\n\n  Serial.print(\"ID bytes: \");\n  uint8_t id[4];\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(0x9F);\n  id[0] = SPI.transfer(0);\n  id[1] = SPI.transfer(0);\n  id[2] = SPI.transfer(0);\n  id[3] = SPI.transfer(0);\n  digitalWrite(CSPIN, HIGH);\n  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);\n\n  Serial.println(\"Winbond W25Q80BLUX1G    Chip ID = 0xEF, 0x40, 0x14, 0x0\");\n  Serial.println(\"Macronix MX25L12835FZNI Chip ID = 0xC2, 0x20, 0x18, 0xC2\");\n  Serial.println(\"Spansion S25FL127S      Chip ID = 0x01, 0x20, 0x18, 0x4D\");\n  Serial.println(\" \");\n  \n/* Initialize the array to 0,1,2,3 etc.*/\n  for(uint16_t i = 0; i < 256; i++) {\n    w_page[i] = i;\n  }\n  \n/* Write the page to page_number - this page MUST be in the erased state*/\n  Serial.print(\"Write page:  0x\"); Serial.println(page_number, HEX);  \n  t_start = micros();\n  flash_page_program(w_page, page_number);\n  t_start = micros() - t_start;\n  Serial.print(\"time (us) = \"); Serial.println(t_start);\n\n/* Read back page_number and print its contents which should be 0,1,2,3...*/\n  Serial.print(\"Read Page 0x\"); Serial.println(page_number, HEX);\n  t_start = micros();\n  flash_read_pages(r_page, page_number,1);\n  t_start = micros() - t_start;\n  Serial.print(\"time (us) = \"); Serial.println(t_start);\n  \n  for(uint16_t i = 0; i < 256; i++) {\n    Serial.print(\" 0x\"); Serial.print(r_page[i], HEX);\n\tif (i % 16==0) Serial.println();\n  }\n  Serial.println(\"\");\n  \n/* Erase the sector which includes page_number*/\n  t_start = millis();\n  flash_chip_erase(true);\n  t_start = millis() - t_start;\n  Serial.print(\"time (ms) = \"); Serial.println(t_start);\n\n/* Now read back the page. It should now be all 255.*/\n  Serial.print( \"Read Page 0x\"); Serial.println(page_number, HEX);\n  t_start = micros();\n  flash_read_pages(r_page, page_number,1);\n  t_start = micros() - t_start;\n\n  Serial.print(\"time (us) = \"); Serial.println(t_start);\n  for(uint16_t i = 0; i < 256; i++) {\n    Serial.print(\" 0x\"); Serial.print(r_page[i], HEX);\n\tif (i % 16==0) Serial.println();\n  }\n  Serial.println(\"\");\n}\n\nvoid loop(void)\n{\n}\n\n/*********************************************************************************************/\n// Useful functions\n/*********************************************************************************************/\nvoid write_pause(void)\n{\n  if(flash_wait_for_write) {\n    while(flash_read_status() & STAT_WIP);\n    flash_wait_for_write = 0;\n  }\n}\n\n//=====================================\n// convert a page number to a 24-bit address\nint page_to_address(int pn)\n{\n  return(pn << 8);\n}\n\n//=====================================\n// convert a 24-bit address to a page number\nint address_to_page(int addr)\n{\n  return(addr >> 8);\n}\n\n//=====================================\nvoid flash_read_id(unsigned char *idt)\n{\n  write_pause();\n  //set control register \n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_READ_ID);\n  for(uint16_t i = 0; i < 20; i++) {\n    *idt++ = SPI.transfer(0x00);\n  }\n  digitalWrite(CSPIN, HIGH);\n}\n\n//=====================================\nunsigned char flash_read_status(void)\n{\n  unsigned char c;\n\n// This can't do a write_pause\n  digitalWrite(CSPIN, LOW);  \n  SPI.transfer(CMD_READ_STATUS_REG);\n  c = SPI.transfer(0x00);\n  digitalWrite(CSPIN, HIGH);\n  return(c);\n}\n\n//=====================================\n\nvoid flash_hard_reset(void)\n{\n  // Make sure that the device is not busy before\n  // doing the hard reset sequence\n  // At the moment this does NOT check the\n  // SUSpend status bit in Status Register 2\n  // but the library does not support suspend\n  // mode yet anyway\n  write_pause();\n  \n  // Send Write Enable command\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_RESET_DEVICE );\n  digitalWrite(CSPIN, HIGH);\n  delayMicroseconds(50);\n  // Wait for the hard reset to finish\n  // Don't use flash_wait_for_write here\n  while(flash_read_status() & STAT_WIP);\n  // The spec says \"the device will take\n  // approximately tRST=30 microseconds\n  // to reset\"\n}\n\n//=====================================\nvoid flash_chip_erase(boolean wait)\n{\n  write_pause();\n  // Send Write Enable command\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_WRITE_ENABLE);\n  digitalWrite(CSPIN, HIGH);\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_CHIP_ERASE);\n  digitalWrite(CSPIN, HIGH);\n  flash_wait_for_write = 1;\n  if(wait)write_pause();\n}\n\n//=====================================\n// Tse Typ=0.6sec Max=3sec\n// measured 549.024ms\n// Erase the sector which contains the specified\n// page number.\n// The smallest unit of memory which can be erased\n// is the 4kB sector (which is 16 pages)\nvoid flash_erase_pages_sector(int pn)\n{\n  int address;\n\n  write_pause(); \n  // Send Write Enable command\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_WRITE_ENABLE);\n  digitalWrite(CSPIN, HIGH);\n  \n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_SECTOR_ERASE);\n  // Send the 3 byte address\n  address = page_to_address(pn);\n  SPI.transfer((address >> 16) & 0xff);\n  SPI.transfer((address >> 8) & 0xff);\n  SPI.transfer(address & 0xff);\n  digitalWrite(CSPIN, HIGH);\n  // Indicate that next I/O must wait for this write to finish\n  flash_wait_for_write = 1;\n}\n\n//=====================================\n// Erase the 32kb block which contains the specified\n// page number.\nvoid flash_erase_pages_block32k(int pn)\n{\n  int address;\n\n  write_pause();\n  // Send Write Enable command\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_WRITE_ENABLE);\n  digitalWrite(CSPIN, HIGH);\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_BLOCK32K_ERASE);\n  // Send the 3 byte address\n  address = page_to_address(pn);\n  SPI.transfer((address >> 16) & 0xFF);\n  SPI.transfer((address >> 8) & 0xFF);\n  SPI.transfer(address & 0xFF);\n  digitalWrite(CSPIN, HIGH);\n  // Indicate that next I/O must wait for this write to finish\n  flash_wait_for_write = 1;\n}\n\n//=====================================\n// Erase the 64kb block which contains the specified\n// page number.\nvoid flash_erase_pages_block64k(int pn)\n{\n  int address;\n  \n  write_pause();\n  // Send Write Enable command\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_WRITE_ENABLE);\n  digitalWrite(CSPIN, HIGH);\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_BLOCK64K_ERASE);\n  // Send the 3 byte address\n  address = page_to_address(pn);\n  SPI.transfer((address >> 16) & 0xFF);\n  SPI.transfer((address >> 8) & 0xFF);\n  SPI.transfer(address & 0xFF);\n  digitalWrite(CSPIN, HIGH);\n  // Indicate that next I/O must wait for this write to finish\n  flash_wait_for_write = 1;\n}\n\n//=====================================\nvoid flash_page_program(unsigned char *wp,int pn)\n{\n  int address;\n\n  write_pause(); \n  // Send Write Enable command\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_WRITE_ENABLE);\n  digitalWrite(CSPIN, HIGH);\n  \n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_PAGE_PROGRAM);\n  // Send the 3 byte address\n  address = page_to_address(pn);\n  SPI.transfer((address >> 16) & 0xFF);\n  SPI.transfer((address >> 8) & 0xFF);\n  SPI.transfer(address & 0xFF);\n  // Now write 256 bytes to the page\n  for(uint16_t i = 0; i < 256; i++) {\n  SPI.transfer(*wp++);\n  }\n  digitalWrite(CSPIN, HIGH);\n  // Indicate that next I/O must wait for this write to finish\n  flash_wait_for_write = 1;\n}\n\n//=====================================\nvoid flash_read_pages(unsigned char *p,int pn,const int n_pages)\n{\n  int address;\n  unsigned char *rp = p;\n  \n  write_pause();\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_READ_DATA);\n  // Send the 3 byte address\n  address = page_to_address(pn);\n  SPI.transfer((address >> 16) & 0xFF);\n  SPI.transfer((address >> 8) & 0xFF);\n  SPI.transfer(address & 0xFF);\n  // Now read the page's data bytes\n  for(uint16_t i = 0; i < n_pages * 256; i++) {\n    *rp++ = SPI.transfer(0);\n  }\n  digitalWrite(CSPIN, HIGH);\n}\n\n//=====================================\n// Read specified number of pages starting with pn\nvoid flash_fast_read_pages(unsigned char *p,int pn,const int n_pages)\n{\n  int address;\n  unsigned char *rp = p;\n  \n  write_pause();\n// The chip doesn't run at the higher clock speed until\n// after the command and address have been sent\n  digitalWrite(CSPIN, LOW);\n  SPI.transfer(CMD_READ_HIGH_SPEED);\n  // Send the 3 byte address\n  address = page_to_address(pn);\n  SPI.transfer((address >> 16) & 0xFF);\n  SPI.transfer((address >> 8) & 0xFF);\n  SPI.transfer(address & 0xFF);\n  // send dummy byte\n  SPI.transfer(0);\n  // Now read the number of pages required\n  for(uint16_t i = 0; i < n_pages * 256; i++) {\n    *rp++ = SPI.transfer(0);\n  }\n  digitalWrite(CSPIN, HIGH);\n}\n"
  },
  {
    "path": "VISHAY/VEML6040.ino",
    "content": "/* VEML6040 Basic Example Code\n by: Kris Winer\n date: December 14, 2016\n license: Beerware - Use this code however you'd like. If you \n find it useful you can buy me a beer some time.\n \n Demonstrate basic VEML6040 functionality including parameterizing the register addresses, initializing the sensor, \n getting properly scaled rgbw intensity data out. Sketch runs on the 3.3 V ESP32.\n \n From the data sheet: (https://www.vishay.com/docs/84276/veml6040.pdf)\n \n VEML6040 color sensor senses red, green, blue, and white light and incorporates photodiodes, amplifiers, \n and analog / digital circuits into a single chip using CMOS process. With the   color   sensor   applied,   \n the   brightness,   and   color temperature of backlight can be adjusted base on ambient light  source  \n that  makes  panel  looks  more  comfortable  for  end   user’s   eyes.   VEML6040’s   adoption   of   FiltronTM\n technology  achieves  the  closest  ambient  light  spectral  sensitivity to real human eye responses.\n\n VEML6040  provides  excellent  temperature  compensation  capability  for  keeping  the  output  stable  \n under  changing  temperature.   VEML6040’s   function   are   easily   operated   via the simple command format \n of I2C (SMBus compatible) interface  protocol.  VEML6040’s  operating  voltage  ranges  from   2.5   V   to   \n 3.6   V.   VEML6040   is   packaged   in   a   lead  (Pb)-free  4  pin  OPLGA  package  which  offers  the  best market-proven reliability.\n\n SDA and SCL  have external pull-up resistors (to 3.3V).\n 2K2 resistors are on the VEML6040 breakout board.\n \n Hardware setup:\n VEML6040 Breakout ------ ESP32\n VDD ---------------------- 3.3V or any digital pin i.e., digitalWrite(HIGH)\n SDA ----------------------- pin 21\n SCL ----------------------- pin 22\n GND ---------------------- GND or any digital pin i.e., digitalWrite(LOW)\n \n*/\n  \n#include <Wire.h>\n\n////////////////////////////\n// VEML6040 Command Codes //\n////////////////////////////\n#define  VEML6040_CONF\t          0x00 // command codes\n#define  VEML6040_R_DATA   \t\t    0x08  \n#define  VEML6040_G_DATA\t\t      0x09 \n#define  VEML6040_B_DATA\t\t      0x0A\n#define  VEML6040_W_DATA          0x0B\n\n#define VEML6040_ADDRESS          0x10\n\n#define SerialDebug true  // set to true to get Serial output for debugging\n\n// Pin definitions\nint myLed  = 5;                      \nuint16_t count = 0;\n\nenum IT {\n  IT_40 = 0, //   40 ms\n  IT_80,     //   80 ms\n  IT_160,    //  160 ms\n  IT_320,    //  320 ms\n  IT_640,    //  640 ms\n  IT_1280   // 1280 ms\n};\n\n// Specify VEML6040 Integration time\nuint8_t IT = IT_160;\nuint8_t ITime = 160;  // milliseconds\nuint16_t RGBWData[4] = {0, 0, 0, 0};\nfloat GSensitivity = 0.25168/((float) (IT + 1)); // ambient light sensitivity increases with integration time\n\n\nvoid setup()\n{\n  Wire.begin(21, 22, 400000); // (SDA, SCL) (21, 22) are default on ESP32, 400 kHz I2C bus speed\n  delay(4000);\n  Serial.begin(115200);\n  \n  // Set up the interrupt pin, its set as active high, push-pull\n  pinMode(myLed, OUTPUT);\n  digitalWrite(myLed, HIGH);\n\n  I2Cscan();\n  \n  enableVEML6040(); // initalize sensor\n \n  delay(150);\n  \n  digitalWrite(myLed, LOW);\n\n}\n\nvoid loop()\n{  \n  getRGBWdata(RGBWData);\n  Serial.print(\"Red raw counts = \");   Serial.println(RGBWData[0]);\n  Serial.print(\"Green raw counts = \"); Serial.println(RGBWData[1]);\n  Serial.print(\"Blue raw counts = \");  Serial.println(RGBWData[2]);\n  Serial.print(\"White raw counts = \"); Serial.println(RGBWData[3]);\n  Serial.print(\"Inferred IR raw counts = \"); Serial.println(RGBWData[3] - RGBWData[0] - RGBWData[1] - RGBWData[2]);\n  Serial.println(\"  \");\n \n  Serial.print(\"Red   light power density = \"); Serial.print((float)RGBWData[0]/96.0f, 2); Serial.println(\" microWatt/cm^2\");\n  Serial.print(\"Green light power density = \"); Serial.print((float)RGBWData[1]/74.0f, 2); Serial.println(\" microWatt/cm^2\");\n  Serial.print(\"Blue  light power density = \"); Serial.print((float)RGBWData[2]/56.0f, 2); Serial.println(\" microWatt/cm^2\");\n  Serial.println(\"  \");\n\n  Serial.print(\"Ambient light intensity = \"); Serial.print((float)RGBWData[1]*GSensitivity, 2); Serial.println(\" lux\");\n  Serial.println(\"  \");\n\n  // Empirical estimation of the correlated color temperature CCT:\n  // see https://www.vishay.com/docs/84331/designingveml6040.pdf\n  float temp = ( (float) (RGBWData[0] - RGBWData[2])/(float) RGBWData[1] );\n  float CCT = 4278.6f*pow(temp, -1.2455) + 0.5f;\n\n  Serial.print(\"Correlated Color Temperature = \"); Serial.print(CCT, 2); Serial.println(\" Kelvin\");\n  Serial.println(\"  \");\n\n\n  digitalWrite(myLed, !digitalRead(myLed));\n  delay(ITime+100);\n}\n\n//===================================================================================================================\n//====== Set of useful function to access UV data\n//===================================================================================================================\n\nuint16_t getRGBWdata(uint16_t * destination)\n{\n    for (int j = 0; j < 4; j++)\n    {\n    uint8_t rawData[2] = {0, 0};\n    Wire.beginTransmission(VEML6040_ADDRESS);\n    Wire.write(VEML6040_R_DATA + j);        // Command code for reading rgbw data channels in sequence\n    Wire.endTransmission(false);  // Send the Tx buffer, but send a restart to keep connection alive\n\n    Wire.requestFrom(VEML6040_ADDRESS, 2);  // Read two bytes from slave register address \n    uint8_t i = 0;\n    while (Wire.available()) \n    {\n        rawData[i++] = Wire.read();       // Put read results in the Rx buffer\n    }     \n    Wire.endTransmission();\n    destination[j] = ((uint16_t) rawData[1] << 8) | rawData[0];\n    }\n \n}\n\nvoid enableVEML6040()\n{\n  Wire.beginTransmission(VEML6040_ADDRESS);\n  Wire.write(VEML6040_CONF); // Command code for configuration register\n  Wire.write(IT << 4); // Bit 3 must be 0, bit 0 is 0 for run and 1 for shutdown, LS Byte\n  Wire.write(0x00); // MS Byte\n  Wire.endTransmission();\n}\n\n\n// I2C scan function\n\nvoid I2Cscan()\n{\n// scan for i2c devices\n  byte error, address;\n  int nDevices;\n\n  Serial.println(\"Scanning...\");\n\n  nDevices = 0;\n  for(address = 1; address < 127; address++ ) \n  {\n    // The i2c_scanner uses the return value of\n    // the Write.endTransmisstion to see if\n    // a device did acknowledge to the address.\n    Wire.beginTransmission(address);\n    error = Wire.endTransmission();\n\n    if (error == 0)\n    {\n      Serial.print(\"I2C device found at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.print(address,HEX);\n      Serial.println(\"  !\");\n\n      nDevices++;\n    }\n    else if (error==4) \n    {\n      Serial.print(\"Unknown error at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.println(address,HEX);\n    }    \n  }\n  if (nDevices == 0)\n    Serial.println(\"No I2C devices found\\n\");\n  else\n    Serial.println(\"done\\n\");\n    \n}\n"
  },
  {
    "path": "VISHAY/readme.md",
    "content": "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.\n\n![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-05-07T02:47:51.413Z-VEML6040.top1.jpg.855x570_q85_pad_rcrop.jpg)\n"
  },
  {
    "path": "VL53L0x/VL53L0X_ESP32.ino",
    "content": "/* This example shows how to use continuous mode to take\nrange measurements with the VL53L0X. It is based on\nvl53l0x_ContinuousRanging_Example.c from the VL53L0X API.\n\nBasic sketch from Pololu version of ST Microelectronic's API\nsee: https://github.com/pololu/vl53l0x-arduino\n\nFurther modified by Kris Winer for the ESP32 and interrupt data ready, etc.\n\nThe range readings are in units of mm. */\n\n#include <Wire.h>\n#include <VL53L0X.h>\n\nVL53L0X sensor;\n\n// Uncomment this line to use long range mode. This\n// increases the sensitivity of the sensor and extends its\n// potential range, but increases the likelihood of getting\n// an inaccurate reading because of reflections from objects\n// other than the intended target. It works best in dark\n// conditions.\n\n//#define LONG_RANGE\n\n\n// Uncomment ONE of these two lines to get\n// - higher speed at the cost of lower accuracy OR\n// - higher accuracy at the cost of lower speed\n\n//#define HIGH_SPEED\n#define HIGH_ACCURACY\n\n// Pin definitions\nint myLed = 5;\nint intPin = 14;\n\nbool newData = false;\n\nuint32_t delt_t = 0, count = 0, sumCount = 0;  // used to control display output rate\nfloat deltat = 0.0f, sum = 0.0f;          // integration interval for both filter schemes\nuint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval\nuint32_t Now = 0;                         // used to calculate integration interval\n\nvoid setup()\n{\n  Serial.begin(115200);\n  delay(4000);\n  \n Wire.begin(21, 22, 400000); // SDA (21), SCL (22) on ESP32, 400 kHz rate\n \n// Set up the led indicator\n  pinMode(myLed, OUTPUT);\n  digitalWrite(myLed, LOW);\n  pinMode(intPin, INPUT);\n\n  I2Cscan();\n  \n  delay(1000);\n  \n  sensor.init();\n  sensor.setTimeout(500);\n\n  #if defined LONG_RANGE\n  // lower the return signal rate limit (default is 0.25 MCPS)\n  sensor.setSignalRateLimit(0.1);\n  // increase laser pulse periods (defaults are 14 and 10 PCLKs)\n  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);\n  sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);\n  #endif\n\n  #if defined HIGH_SPEED\n  // reduce timing budget to 20 ms (default is about 33 ms)\n  sensor.setMeasurementTimingBudget(20000);  // minimum timing budget 20 ms\n  #elif defined HIGH_ACCURACY\n  // increase timing budget to 200 ms\n  sensor.setMeasurementTimingBudget(200000);\n  #endif\n\n  // Start continuous back-to-back mode (take readings as\n  // fast as possible).  To use continuous timed mode\n  // instead, provide a desired inter-measurement period in\n  // ms (e.g. sensor.startContinuous(100)).\n  sensor.startContinuous();\n\n  attachInterrupt(intPin, myinthandler, FALLING);  // define interrupt for GPI01 pin output of VL53L0X\n\n}\n\nvoid loop()\n{  \n  if (newData) // wait for data ready interrupt\n  {\n     newData = false; // reset data ready flag\n     Now = micros(); // capture interrupt time\n     // calculate time between last interrupt and current one, convert to sample data rate, and print to serial monitor\n     Serial.print(\"data rate = \"); Serial.print(1000000./(Now - lastUpdate)); Serial.println(\" Hz\");\n\n     Serial.print(sensor.readRangeContinuousMillimeters()); // prit range in mm to serial monitor\n     if (sensor.timeoutOccurred()) { Serial.print(\" TIMEOUT\"); }\n\n     Serial.println();\n  }\n  lastUpdate = Now;\n}\n\n/****************************************************************************************************/\n/* Useful functions*/\n/****************************************************************************************************/\nvoid myinthandler()\n{\n  newData = true; // set the new data ready flag to true on interrupt\n}\n\n// I2C scan function\nvoid I2Cscan()\n{\n// scan for i2c devices\n  byte error, address;\n  int nDevices;\n\n  Serial.println(\"Scanning...\");\n\n  nDevices = 0;\n  for(address = 1; address < 127; address++ ) \n  {\n    // The i2c_scanner uses the return value of\n    // the Write.endTransmisstion to see if\n    // a device did acknowledge to the address.\n    Wire.beginTransmission(address);\n    error = Wire.endTransmission();\n\n    if (error == 0)\n    {\n      Serial.print(\"I2C device found at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.print(address,HEX);\n      Serial.println(\"  !\");\n\n      nDevices++;\n    }\n    else if (error==4) \n    {\n      Serial.print(\"Unknown error at address 0x\");\n      if (address<16) \n        Serial.print(\"0\");\n      Serial.println(address,HEX);\n    }    \n  }\n  if (nDevices == 0)\n    Serial.println(\"No I2C devices found\\n\");\n  else\n    Serial.println(\"done\\n\");\n    \n}\n"
  },
  {
    "path": "VL53L0x/readme.md",
    "content": "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).\n\n![](https://d3s5r33r268y59.cloudfront.net/44691/products/thumbs/2016-06-16T03:54:45.593Z-VL53L0X.group2.jpg.2560x2560_q85.jpg)\n"
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/AverageCompassVals.pde",
    "content": "/*==============================================================================\r\n \r\n Copyright (c) 2010-2013 Christopher Baker <http://christopherbaker.net>\r\n \r\n Permission is hereby granted, free of charge, to any person obtaining a copy\r\n of this software and associated documentation files (the \"Software\"), to deal\r\n in the Software without restriction, including without limitation the rights\r\n to use, copy, modify, merge, publish, distribute, sublicense, and/or sell\r\n copies of the Software, and to permit persons to whom the Software is\r\n furnished to do so, subject to the following conditions:\r\n \r\n The above copyright notice and this permission notice shall be included in\r\n all copies or substantial portions of the Software.\r\n \r\n THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\r\n IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\r\n FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\r\n AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\r\n LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\r\n OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN\r\n THE SOFTWARE.\r\n \r\n https://github.com/SAIC-ATS/Algorithms.git\r\n  \r\n ==============================================================================*/\r\n\r\n\r\nint historyLength = 20;\r\n\r\nfloat[] history = new float[historyLength]; // keep our history values\r\n\r\nfloat[] historyX = new float[historyLength];\r\nfloat[] historyY = new float[historyLength];\r\n\r\nfloat averageAngle = 0;\r\nfloat yamartinoAverageAngle = 0;\r\n\r\nfloat currentAngle = 0;\r\n\r\nvoid calculateMathematicalAverageOfHistory() {\r\n    float sum = 0;\r\n    float sq_sum = 0;\r\n    for(int i = 0; i < history.length; ++i) {\r\n       sum += history[i];\r\n       sq_sum += history[i] * history[i];\r\n    }\r\n    \r\n    averageAngle = sum / history.length;\r\n\r\n}\r\n\r\nvoid calculateYamartinoAverageOfHistory() {\r\n\r\n  float sumX = 0;\r\n  float sumY = 0;\r\n  \r\n  for (int i = 0; i < history.length; i++) {\r\n    sumX += historyX[i];\r\n    sumY += historyY[i];\r\n  }\r\n\r\n  float meanX = sumX / history.length;\r\n  float meanY = sumY / history.length;\r\n\r\n  yamartinoAverageAngle = myAtan2(sumY, sumX);\r\n}\r\n\r\nvoid addItemsToHistoryBuffers(float input) {\r\n  addToHistory(history,input);\r\n  addToHistory(historyX,cos(input));\r\n  addToHistory(historyY,sin(input));\r\n}\r\n\r\nvoid addToHistory(float[] buffer, float input) {\r\n  // delete the oldest value from the history\r\n  // add one value to the history (the input)\r\n  // take the average of the history and return it;\r\n\r\n  // shift the values to the left in the array\r\n  for (int i = buffer.length - 1; i >= 0; i--) {\r\n    if (i == 0) {\r\n      buffer[0] = input;\r\n    } \r\n    else {\r\n      buffer[i] = buffer[i-1];\r\n    }\r\n  }\r\n}\r\n\r\n\r\nfloat myAtan2(float y, float x) {\r\n  float t = atan2(y, x);\r\n  return t > 0 ? t : 2 * PI + t;\r\n}"
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/FreeIMU_cube_UDP_ES32.pde",
    "content": "/**\r\nVisualize a cube which will assumes the orientation described\r\nin a quaternion coming from the serial port.\r\n\r\nINSTRUCTIONS: \r\nThis program has to be run when you have the FreeIMU_serial\r\nprogram running on your Arduino and the Arduino connected to your PC.\r\nRemember to set the serialPort variable below to point to the name the\r\nArduino serial port has in your system. You can get the port using the\r\nArduino IDE from Tools->Serial Port: the selected entry is what you have\r\nto use as serialPort variable.\r\n\r\n\r\nCopyright (C) 2011-2012 Fabio Varesano - http://www.varesano.net/\r\nModified for FreeIMU-Upades Library starting in 2013-2017, Michael J Smorto -\r\nhttps://github.com/mjs513/FreeIMU-Updates\r\n\r\nThis program is free software: you can redistribute it and/or modify\r\nit under the terms of the version 3 GNU General Public License as\r\npublished by the Free Software Foundation.\r\n\r\nThis program is distributed in the hope that it will be useful,\r\nbut WITHOUT ANY WARRANTY; without even the implied warranty of\r\nMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r\nGNU General Public License for more details.\r\n\r\nYou should have received a copy of the GNU General Public License\r\nalong with this program.  If not, see <http://www.gnu.org/licenses/>.\r\n\r\n*/\r\n\r\n// import UDP library\r\nimport hypermedia.net.*;\r\n\r\nUDP udp;  // define the UDP object\r\n//UDP udpS;\r\n\r\n    String ip       = \"xxx.xxx.x.xxx\";  // the remote IP address of device\r\n    int port        = 4210;    // the destination port\r\n\r\nint BaudRate=115200;\r\n\r\n\r\n\r\n//Settup Stop Watch\r\nStopWatchTimer sw = new StopWatchTimer();\r\n\r\n// These are needed for the moving average calculation\r\nfloat[] data = new float[32];\r\nfloat total = 0, average = 0;\r\nint p = 0, n = 0;\r\n\r\n//LPF\r\nfloat filterFactor = 0.05;\r\n\r\n//Moving average Heading\r\nfloat corr_heading;\r\n\r\n//set motiondetect types\r\nfloat accnorm,accnorm_var_test, motionDetect1;\r\nint accnorm_test, omegax, omegay, omegaz, omega_test, motionDetect;\r\n\r\n//Movingaverage filters for motion detection\r\n//float accnorm_test_avg, accnorm_var,motion_detect_ma, accnorm_avg;\r\n\r\nMovingAverage accnorm_test_avg = new MovingAverage(5);\r\nMovingAverage accnorm_var = new MovingAverage(7);\r\nMovingAverage motion_detect_ma = new MovingAverage(7);\r\nMovingAverage accnorm_avg = new MovingAverage(5);\r\n\r\nfloat [] q = new float [4];\r\nfloat [] acc = new float [3];\r\nfloat [] gyro = new float [3];\r\nfloat [] magn = new float [3];\r\nfloat [] ypr = new float [3];\r\nfloat temp; float press; float altitude; \r\nfloat dt, heading;\r\n\r\n// Altitude - Accel Complimentary filter setup\r\nfloat[] dyn_acc = new float[3];\r\nfloat fused_alt;\r\n\r\n// GPS Variables\r\nfloat hdop, lat, longt, cog, sog, gpsalt, gpschars;\r\nfloat hdop_val, loc_val, gpsalt_val, sog_val, cog_val;\r\n\r\nfloat S;\r\nfloat A;\r\n\r\nString seapresscmd = \"99\";\r\nfloat STATIONALTFT = 36.0;\r\nfloat sea_press = 1013.25;            //Input local sea level pressure\r\nfloat declinationAngle = -13.1603;   //Flushing, NY magnetic declination in degrees\r\nfloat SEA_PRESS  = 1013.25;          //default sea level pressure level in mb\r\nfloat KNOWNALT   = 65.0;            //default known altitude, \r\nfloat INHG       = 0.02952998751;    //convert mb to in/Hg constant\r\nfloat MB         = 33.8638815;       //convert in/Hg to mb constant\r\nfloat FTMETERS   = 0.3048;\r\nfloat METERS2FT  = 3.2808399;\r\nfloat PI         = 3.14159;\r\n\r\nfloat [] hq = null;\r\nfloat [] Euler = new float [3]; // psi, theta, phi\r\n\r\nint lf = 10; // 10 is '\\n' in ASCII\r\nbyte[] inBuffer = new byte[22]; // this is the number of chars on each line from the Arduino (including /r/n)\r\n\r\n//---------------------------------------------------\r\nfloat Sample_X;\r\nfloat Sample_Y; \r\nfloat Sample_Z; \r\nlong [] Sensor_Data = new long [8]; \r\nshort countx; short county ;\r\n\r\nfloat [] accelerationx = new float [2];\r\nfloat [] accelerationy = new float [2];\r\nfloat [] velocityx = new float [2];\r\nfloat [] velocityy = new float [2];\r\nfloat [] positionX= new float [2]; \r\nfloat [] positionY = new float [2]; \r\nfloat [] positionZ = new float [2]; \r\nlong direction; \r\nfloat sstatex; float sstatey;\r\n\r\nfloat motionDetect_transition, motionDetect_old;\r\n\r\nint calib = 0;\r\nint cube_odo = 0;\r\n//-------------------------------------\r\n\r\nPFont font;\r\nfinal int VIEW_SIZE_X = 900, VIEW_SIZE_Y = 600;\r\n\r\nfinal int burst = 32;\r\nint count = 0;\r\n\r\nvoid myDelay(int time) {\r\n  try {\r\n    Thread.sleep(time);\r\n  } catch (InterruptedException e) { }\r\n}\r\n\r\nvoid setup() \r\n{\r\n  size(900, 600, OPENGL);\r\n\r\n  // create a new datagram connection on port 6000\r\n  // and wait for incomming message\r\n  udp = new UDP( this, port );\r\n//  udpS = new UDP(this, portS );\r\n  udp.log( true ); \t\t// <-- printout the connection activity\r\n  //udpS.log( true );\r\n  udp.listen( true );\r\n//  udpS.listen(true );\r\n\r\n  \r\n  // The font must be located in the sketch's \"data\" directory to load successfully\r\n  font = loadFont(\"CourierNew36.vlw\"); \r\n  \r\n}\r\n\r\nfloat decodeFloat(String inString) {\r\n  byte [] inData = new byte[4];\r\n  \r\n  if(inString.length() == 8) {\r\n    inData[0] = (byte) unhex(inString.substring(0, 2));\r\n    inData[1] = (byte) unhex(inString.substring(2, 4));\r\n    inData[2] = (byte) unhex(inString.substring(4, 6));\r\n    inData[3] = (byte) unhex(inString.substring(6, 8));\r\n  }\r\n      \r\n  int intbits = (inData[3] << 24) | ((inData[2] & 0xff) << 16) | ((inData[1] & 0xff) << 8) | (inData[0] & 0xff);\r\n  return Float.intBitsToFloat(intbits);\r\n}\r\n\r\n////////////////////////////////////////////////////////////////////////\r\n/**\r\n * To perform any action on datagram reception, you need to implement this \r\n * handler in your code. This method will be automatically called by the UDP \r\n * object each time he receive a nonnull message.\r\n * By default, this method have just one argument (the received message as \r\n * byte[] array), but in addition, two arguments (representing in order the \r\n * sender IP address and his port) can be set like below.\r\n */\r\n// void receive( byte[] data ) { \t\t\t// <-- default handler\r\nvoid receive( byte[] data) {\t// <-- extended handler\r\n  \r\n  // get the \"real\" message =\r\n  // forget the \";\\n\" at the end <-- !!! only for a communication with Pd !!!\r\n  data = subset(data, 0, data.length-2);\r\n  String message = new String( data );\r\n  \r\n  // print the result\r\n  //println( \"receive: \\\"\"+message+\"\\\" from \"+ip+\" on port \"+port );\r\n  \r\n    if (message != null && message.length() > 0) {\r\n      String [] inputStringArr = split(message, \",\");\r\n      if(inputStringArr.length >= 18) { // q1,q2,q3,q4,\\r\\n so we have 5 elements\r\n        q[0] = decodeFloat(inputStringArr[0]);\r\n        q[1] = decodeFloat(inputStringArr[1]);\r\n        q[2] = decodeFloat(inputStringArr[2]);\r\n        q[3] = decodeFloat(inputStringArr[3]);\r\n        acc[0] = decodeFloat(inputStringArr[4]);\r\n        acc[1] = decodeFloat(inputStringArr[5]);\r\n        acc[2] = decodeFloat(inputStringArr[6]);\r\n        gyro[0] = decodeFloat(inputStringArr[7]);\r\n        gyro[1] = decodeFloat(inputStringArr[8]);\r\n        gyro[2] = decodeFloat(inputStringArr[9]);\r\n        magn[0] = decodeFloat(inputStringArr[10]);\r\n        magn[1] = decodeFloat(inputStringArr[11]);    \r\n        magn[2] = decodeFloat(inputStringArr[12]);\r\n        temp = decodeFloat(inputStringArr[13]);\r\n        press = decodeFloat(inputStringArr[14]);\r\n        //dt = (1./decodeFloat(inputStringArr[15]))/4;\r\n        dt = (1./decodeFloat(inputStringArr[15]));\r\n        heading = decodeFloat(inputStringArr[16]);\r\n        //dt = tnew - told;\r\n        //told = tnew;\r\n        if(heading < -9990) {\r\n            heading = 0;\r\n        }\r\n        altitude = decodeFloat(inputStringArr[17]);\r\n        motionDetect1 = decodeFloat(inputStringArr[18]);\r\n      }\r\n    }\r\n}\r\n\r\n/** \r\n * on key pressed event:\r\n * send the current key value over the network\r\n */\r\nvoid keyPressed() {\r\n    \r\n    String message  = str( key );\t// the message to send\r\n\r\n    if(key == 's'){\r\n       udp.send(\"s\" + \"\\n\", ip, port);\r\n    } else if(key == 'g'){\r\n      udp.send( \"g\" + \"\\n\", ip, port ); \r\n    } else if(key == 'v'){\r\n      udp.send( \"v\" + \"\\n\", ip, port ); \r\n    } else if(key == '1'){\r\n      udp.send( \"1\" + \"\\n\", ip, port ); \r\n    } else if(key == '2'){\r\n      udp.send( \"2\" + \"\\n\", ip, port );\r\n    } else if(key == 't'){\r\n      udp.send( \"t\" + \"\\n\", ip, port ); \r\n    } else if(key == 'f'){\r\n      udp.send( \"f\" + \";\\n\", ip, port ); \r\n    } else if(key == 'x'){\r\n      udp.send( \"x\" + \"\\n\", ip, port ); \r\n    } else {\r\n      println(\"NOT VALID COMMAND\");\r\n    }\r\n\r\n  if(key == 'h') {\r\n    println(\"pressed h\");\r\n    \r\n    // set hq the home quaternion as the quatnion conjugate coming from the sensor fusion\r\n    hq = quatConjugate(q);\r\n    sw.start();\r\n  }\r\n  else if(key == 'n') {\r\n    println(\"pressed n\");\r\n    hq = null;\r\n  }\r\n  else if(key == 'w') {\r\n    println(\"pressed w\"); \r\n    sw.start();\r\n  }  \r\n}\r\n\r\nvoid buildBoxShape() {\r\n  //box(60, 10, 40);\r\n  noStroke();\r\n  beginShape(QUADS);\r\n  \r\n  //Z+ (to the drawing area)\r\n  fill(#00ff00);\r\n  vertex(-30, -5, 20);\r\n  vertex(30, -5, 20);\r\n  vertex(30, 5, 20);\r\n  vertex(-30, 5, 20);\r\n  \r\n  //Z-\r\n  fill(#0000ff);\r\n  vertex(-30, -5, -20);\r\n  vertex(30, -5, -20);\r\n  vertex(30, 5, -20);\r\n  vertex(-30, 5, -20);\r\n  \r\n  //X-\r\n  fill(#ff0000);\r\n  vertex(-30, -5, -20);\r\n  vertex(-30, -5, 20);\r\n  vertex(-30, 5, 20);\r\n  vertex(-30, 5, -20);\r\n  \r\n  //X+\r\n  fill(#ffff00);\r\n  vertex(30, -5, -20);\r\n  vertex(30, -5, 20);\r\n  vertex(30, 5, 20);\r\n  vertex(30, 5, -20);\r\n  \r\n  //Y-\r\n  fill(#ff00ff);\r\n  vertex(-30, -5, -20);\r\n  vertex(30, -5, -20);\r\n  vertex(30, -5, 20);\r\n  vertex(-30, -5, 20);\r\n  \r\n  //Y+\r\n  fill(#00ffff);\r\n  vertex(-30, 5, -20);\r\n  vertex(30, 5, -20);\r\n  vertex(30, 5, 20);\r\n  vertex(-30, 5, 20);\r\n  \r\n  endShape();\r\n}\r\n\r\n\r\nvoid drawCube() {  \r\n  pushMatrix();\r\n    translate(VIEW_SIZE_X/2, VIEW_SIZE_Y/2 + 50, 0);\r\n    scale(5,5,5);\r\n    \r\n    // a demonstration of the following is at \r\n    // http://www.varesano.net/blog/fabio/ahrs-sensor-fusion-orientation-filter-3d-graphical-rotating-cube\r\n    rotateZ(-Euler[2]);\r\n    rotateX(-Euler[1]);\r\n    rotateY(-Euler[0]);\r\n    \r\n    buildBoxShape();\r\n    \r\n  popMatrix();\r\n}\r\n\r\n\r\nvoid draw() {\r\n  background(#000000);\r\n  fill(#ffffff);\r\n  \r\n  \r\n  if(hq != null) { // use home quaternion\r\n    quaternionToEuler(quatProd(hq, q), Euler);\r\n\t//println(quatProd(hq,q));println();\r\n    text(\"Disable home position by pressing \\\"n\\\"\", 20, VIEW_SIZE_Y - 30);\r\n  }\r\n  else {\r\n    quaternionToEuler(q, Euler);\r\n    text(\"Point FreeIMU's X axis to your monitor then press \\\"h\\\"\", 20, VIEW_SIZE_Y - 30);\r\n  }\r\n\r\n  fused_alt = altitude + STATIONALTFT/METERS2FT;\r\n\r\n  text(\"Temp: \" + temp + \"\\n\" + \"Press: \" + press + \"\\n\" + \"   Alt: \" + nfp((fused_alt),3,2), 20, VIEW_SIZE_Y - 110);\r\n  text(\"DeltaT: \" + dt, 180, VIEW_SIZE_Y - 110);\r\n  \r\n  textFont(font, 20);\r\n  textAlign(LEFT, TOP);\r\n  text(\"Q:\\n\" + q[0] + \"\\n\" + q[1] + \"\\n\" + q[2] + \"\\n\" + q[3], 20, 20);\r\n  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);\r\n\r\n  //Compass averaging\r\n  //currentAngle = myAtan2(mouseY-height/2, mouseX-width/2) + radians(myNoise); \r\n  addItemsToHistoryBuffers(radians(heading));\r\n  calculateMathematicalAverageOfHistory();\r\n  calculateYamartinoAverageOfHistory(); \r\n  \r\n  //corr_heading = heading;\r\n  corr_heading = degrees(yamartinoAverageAngle);\r\n  \r\n  text(\"Heading \" + nfp(((corr_heading)),4,1),400,20); \r\n\r\n  text( \"Elapsed Time: \" + sw.hour() + \":\" + sw.minute() + \":\" + sw.second(), 500, 40);\r\n  \r\n  text(\"Acc:\\n\" + nfp(acc[0],1,6) + \"\\n\" + nfp(acc[1],1,6) + \"\\n\" + nfp(acc[2],1,6) + \"\\n\", 20, 130);\r\n  text(\"Gyro:\\n\" + nfp(gyro[0],1,6) + \"\\n\" + nfp(gyro[1],1,6) + \"\\n\" + nfp(gyro[2],1,6) + \"\\n\", 20, 220);\r\n  text(\"Magn:\\n\" + nfp(magn[0],1,6) + \"\\n\" + nfp(magn[1],1,6) + \"\\n\" + nfp(magn[2],1,6) + \"\\n\", 20, 310);\r\n\r\n\r\n  text(MotionDetect(),VIEW_SIZE_X-125,VIEW_SIZE_Y-125) ;\r\n  if(MotionDetect() > 0 ){\r\n    fill(#FF0000);\r\n  } else {\r\n    fill(#FFFFFF)\r\n  ; }\r\n  rect(VIEW_SIZE_X-100,VIEW_SIZE_Y-100,50,50);\r\n\r\n  if(cube_odo == 0) { \r\n     drawCube();\r\n    } else {\r\n\tposition();\r\n        text(\"px:  \" + positionX[0] + \"\\n\" + \"py:  \" + positionY[0], 200, 200);\r\n   }\r\n  //myPort.write(\"q\" + 1);\r\n}\r\n\r\n// See Sebastian O.H. Madwick report \r\n// \"An efficient orientation filter for inertial and intertial/magnetic sensor arrays\" Chapter 2 Quaternion representation\r\n\r\nvoid quaternionToEuler(float [] q, float [] euler) {\r\n  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\r\n  euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta\r\n  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\r\n}\r\n\r\nfloat [] quatProd(float [] a, float [] b) {\r\n  float [] q = new float[4];\r\n  \r\n  q[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];\r\n  q[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];\r\n  q[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];\r\n  q[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];\r\n  \r\n  return q;\r\n}\r\n\r\n// returns a quaternion from an axis angle representation\r\nfloat [] quatAxisAngle(float [] axis, float angle) {\r\n  float [] q = new float[4];\r\n  \r\n  float halfAngle = angle / 2.0;\r\n  float sinHalfAngle = sin(halfAngle);\r\n  q[0] = cos(halfAngle);\r\n  q[1] = -axis[0] * sinHalfAngle;\r\n  q[2] = -axis[1] * sinHalfAngle;\r\n  q[3] = -axis[2] * sinHalfAngle;\r\n  \r\n  return q;\r\n}\r\n\r\n// return the quaternion conjugate of quat\r\nfloat [] quatConjugate(float [] quat) {\r\n  float [] conj = new float[4];\r\n  \r\n  conj[0] = quat[0];\r\n  conj[1] = -quat[1];\r\n  conj[2] = -quat[2];\r\n  conj[3] = -quat[3];\r\n  \r\n  return conj;\r\n}\r\n\r\nvoid getYawPitchRollRad() {\r\n  //float q[4]; // quaternion\r\n  float gx, gy, gz; // estimated gravity direction\r\n  \r\n  gx = 2 * (q[1]*q[3] - q[0]*q[2]);\r\n  gy = 2 * (q[0]*q[1] + q[2]*q[3]);\r\n  gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];\r\n  \r\n  ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);\r\n  ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));\r\n  ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));\r\n}\r\n\r\n//=============================================================\r\nvoid gravityCompensateDynAcc() {\r\n  float[] g = new float[3];\r\n  \r\n  // get expected direction of gravity in the sensor frame\r\n  g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);\r\n  g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);\r\n  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];\r\n  \r\n  // compensate accelerometer readings with the expected direction of gravity\r\n  dyn_acc[0] = acc[0] - g[0];\r\n  dyn_acc[1] = acc[1] - g[1];\r\n  dyn_acc[2] = acc[2] - g[2];\r\n}\r\n \r\n\r\n//=============================================================\r\n// converted from Michael Shimniok Data Bus code\r\n// http://mbed.org/users/shimniok/code/AVC_20110423/\r\n\r\nfloat clamp360(float x) {\r\n    while ((x) >= 360.0) (x) -= 360.0; \r\n    while ((x) < 0) (x) += 360.0; \r\n    return x;\r\n}\r\n\r\n\r\n\r\n//==============================================================\r\n//\r\nfloat HeadingAvgCorr(float newx, float oldx) {\r\n    while ((newx + 180.0) < oldx) (newx) += 360.0;\r\n    while ((newx - 180.0) > oldx) (newx) -= 360.0;\r\n    while ((newx) == 360.0) (newx) = 0.0;\r\n    return newx;\r\n}\r\n\r\n//==============================================================\r\n//SMA filter\r\n// Use the next value and calculate the \r\n// moving average \r\npublic void AddNewValue(float value){\r\n  total -= data[p];\r\n  data[p] = value;\r\n  total += value;\r\n  p = ++p % data.length;\r\n  if(n < data.length) n++;\r\n  average = total / n;\r\n} \r\n\r\n//=======================================\r\npublic float iround(float number, float decimal) {\r\n  int ix;\r\n  ix = round(number*pow(10, decimal));\r\n  return float(ix)/pow(10, decimal);\r\n} "
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/Kalman.pde",
    "content": "public class MyKalman {\r\n//private float Q = 0.001;\r\n//private float R = 0.1;\r\nprivate float Q = 0.0000005;\r\nprivate float R = 0.01;\r\nprivate float P = 1, X = 0, K;\r\n \r\nprivate void measurementUpdate(){\r\nK = (P + Q) / (P + Q + R);\r\nP = R * (P + Q) / (R + P + Q);\r\n}\r\n \r\npublic float update(float measurement){\r\nmeasurementUpdate();\r\nfloat result = X + (measurement - X) * K;\r\nX = result;\r\n \r\nreturn result;\r\n}\r\n} "
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/MathUtils.java",
    "content": "\r\n/**\r\n *                     ProScene (version 1.1.1)      \r\n *    Copyright (c) 2010-2012 by National University of Colombia\r\n *                 @author Jean Pierre Charalambos      \r\n *           http://www.disi.unal.edu.co/grupos/remixlab/\r\n *                           \r\n * This java package provides classes to ease the creation of interactive 3D\r\n * scenes in Processing.\r\n * \r\n * This source file is free software; you can redistribute it and/or modify it\r\n * under the terms of the GNU General Public License as published by the Free\r\n * Software Foundation; either version 3 of the License, or (at your option)\r\n * any later version.\r\n * \r\n * This code is distributed in the hope that it will be useful, but WITHOUT\r\n * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or\r\n * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for\r\n * more details.\r\n * \r\n * A copy of the GNU General Public License is available on the World Wide Web\r\n * at <http://www.gnu.org/copyleft/gpl.html>. You can also obtain it by\r\n * writing to the Free Software Foundation, 51 Franklin Street, Suite 500\r\n * Boston, MA 02110-1335, USA.\r\n */\r\n\r\n\r\nimport processing.core.*;\r\n\r\n/**\r\n * Utility class that complements the PVector and PMatrix classes.\r\n */\r\npublic class MathUtils /**implements PConstants*/ {\r\n\t/**\r\n\t * Utility function that simply projects {@code src} on the axis of direction\r\n\t * {@code direction} that passes through the origin.\r\n\t * <p>\r\n\t * {@code direction} does not need to be normalized (but must be non null).\r\n\t */\r\n\tpublic static PVector projectVectorOnAxis(PVector src, PVector direction) {\r\n\t\tfloat directionSquaredNorm = squaredNorm(direction);\r\n\t\tif (directionSquaredNorm < 1E-10f)\r\n\t\t\tthrow new RuntimeException(\"Direction squared norm is nearly 0\");\r\n\r\n\t\tfloat modulation = src.dot(direction) / directionSquaredNorm;\r\n\t\treturn PVector.mult(direction, modulation);\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that simply projects {@code src} on the plane whose normal\r\n\t * is {@code normal} that passes through the origin.\r\n\t * <p>\r\n\t * {@code normal} does not need to be normalized (but must be non null).\r\n\t */\r\n\tpublic static PVector projectVectorOnPlane(PVector src, PVector normal) {\r\n\t\tfloat normalSquaredNorm = squaredNorm(normal);\r\n\t\tif (normalSquaredNorm < 1E-10f)\r\n\t\t\tthrow new RuntimeException(\"Normal squared norm is nearly 0\");\r\n\r\n\t\tfloat modulation = src.dot(normal) / normalSquaredNorm;\r\n\t\treturn PVector.sub(src, PVector.mult(normal, modulation));\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the squared norm of the PVector.\r\n\t */\r\n\tpublic static float squaredNorm(PVector v) {\r\n\t\treturn (v.x * v.x) + (v.y * v.y) + (v.z * v.z);\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns a PVector orthogonal to {@code v}. Its\r\n\t * {@code mag()} depends on the PVector, but is zero only for a {@code null}\r\n\t * PVector. Note that the function that associates an {@code\r\n\t * orthogonalVector()} to a PVector is not continuous.\r\n\t */\r\n\tpublic static PVector orthogonalVector(PVector v) {\r\n\t\t// Find smallest component. Keep equal case for null values.\r\n\t\tif ((PApplet.abs(v.y) >= 0.9f * PApplet.abs(v.x))\r\n\t\t\t\t&& (PApplet.abs(v.z) >= 0.9f * PApplet.abs(v.x)))\r\n\t\t\treturn new PVector(0.0f, -v.z, v.y);\r\n\t\telse if ((PApplet.abs(v.x) >= 0.9f * PApplet.abs(v.y))\r\n\t\t\t\t&& (PApplet.abs(v.z) >= 0.9f * PApplet.abs(v.y)))\r\n\t\t\treturn new PVector(-v.z, 0.0f, v.x);\r\n\t\telse\r\n\t\t\treturn new PVector(-v.y, v.x, 0.0f);\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the PMatrix3D representation of the 4x4\r\n\t * {@code m} given in European format.\r\n\t */\r\n\tpublic static final PMatrix3D fromMatrix(float[][] m) {\r\n\t\treturn new PMatrix3D(m[0][0], m[0][1], m[0][2], m[0][3], m[1][0], m[1][1],\r\n\t\t\t\tm[1][2], m[1][3], m[2][0], m[2][1], m[2][2], m[2][3], m[3][0], m[3][1],\r\n\t\t\t\tm[3][2], m[3][3]);\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the PMatrix3D representation of the 4x4\r\n\t * {@code matrix} given in OpenGL format.\r\n\t */\r\n\tpublic static final PMatrix3D fromOpenGLMatrix(float[][] matrix) {\r\n\t\treturn fromMatrix(transpose4x4Matrix(matrix));\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the PMatrix3D representation of the 16\r\n\t * {@code array} given in European format.\r\n\t */\r\n\tpublic static final PMatrix3D fromArray(float[] a) {\r\n\t\treturn new PMatrix3D(a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8],\r\n\t\t\t\ta[9], a[10], a[11], a[12], a[13], a[14], a[15]);\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the PMatrix3D representation of the 16\r\n\t * {@code array} given in OpenGL format.\r\n\t */\r\n\tpublic static final PMatrix3D fromOpenGLArray(float[] array) {\r\n\t\treturn fromArray(from4x4MatrixToArray(transpose4x4Matrix(fromArrayTo4x4Matrix(array))));\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the [4][4]float matrix representation\r\n\t * (European format) of the given PMatrix3D.\r\n\t */\r\n\tpublic static final float[][] toMatrix(PMatrix3D pM) {\r\n\t\tfloat[] array = new float[16];\r\n\t\tpM.get(array);\r\n\t\treturn fromArrayTo4x4Matrix(array);\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the [16]array representation (OpenGL format)\r\n\t * of the given PMatrix3D.\r\n\t */\r\n\tpublic static final float[] toOpenGLArray(PMatrix3D pM) {\r\n\t\treturn from4x4MatrixToArray(toOpenGLMatrix(pM));\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the [4][4]float matrix representation (OpenGL\r\n\t * format) of the given PMatrix3D.\r\n\t */\r\n\tpublic static final float[][] toOpenGLMatrix(PMatrix3D pM) {\r\n\t\treturn transpose4x4Matrix(toMatrix(pM));\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the transpose of the given 3X3 matrix.\r\n\t */\r\n\tpublic static final float[][] transpose3x3Matrix(float[][] m) {\r\n\t\tfloat[][] matrix = new float[4][4];\r\n\t\tfor (int i = 0; i < 3; ++i)\r\n\t\t\tfor (int j = 0; j < 3; ++j)\r\n\t\t\t\tmatrix[i][j] = m[j][i];\r\n\t\treturn matrix;\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the transpose of the given 4X4 matrix.\r\n\t */\r\n\tpublic static final float[][] transpose4x4Matrix(float[][] m) {\r\n\t\tfloat[][] matrix = new float[4][4];\r\n\t\tfor (int i = 0; i < 4; ++i)\r\n\t\t\tfor (int j = 0; j < 4; ++j)\r\n\t\t\t\tmatrix[i][j] = m[j][i];\r\n\t\treturn matrix;\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the [4][4] float matrix version of the given\r\n\t * {@code m} array.\r\n\t */\r\n\tpublic static final float[][] fromArrayTo4x4Matrix(float[] m) {\r\n\t\t// m should be of size [16]\r\n\t\tfloat[][] mat = new float[4][4];\r\n\t\tfor (int i = 0; i < 4; ++i)\r\n\t\t\tfor (int j = 0; j < 4; ++j)\r\n\t\t\t\tmat[i][j] = m[i * 4 + j];\r\n\t\treturn mat;\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the [16] float array version of the given\r\n\t * {@code mat} matrix.\r\n\t */\r\n\tpublic static final float[] from4x4MatrixToArray(float[][] mat) {\r\n\t\tfloat[] m = new float[16];\r\n\t\tfor (int i = 0; i < 4; ++i)\r\n\t\t\tfor (int j = 0; j < 4; ++j)\r\n\t\t\t\tm[i * 4 + j] = mat[i][j];\r\n\t\treturn m;\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the 3x3 upper left sub-matrix of the given\r\n\t * PMatrix3D.\r\n\t */\r\n\tpublic static final float[][] get3x3UpperLeftMatrixFromPMatrix3D(PMatrix3D pM) {\r\n\t\tfloat[][] m = new float[3][3];\r\n\t\tm[0][0] = pM.m00;\r\n\t\tm[0][1] = pM.m01;\r\n\t\tm[0][2] = pM.m02;\r\n\t\tm[1][0] = pM.m10;\r\n\t\tm[1][1] = pM.m11;\r\n\t\tm[1][2] = pM.m12;\r\n\t\tm[2][0] = pM.m20;\r\n\t\tm[2][1] = pM.m21;\r\n\t\tm[2][2] = pM.m22;\r\n\t\treturn m;\r\n\t}\t\r\n}"
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/MotionDetect.pde",
    "content": "public float MotionDetect() {\r\n  \r\n    /*###################################################################\r\n    \r\n       accelerationsquared euclidean norm analysis\r\n    \r\n    ################################################################### */\r\n    accnorm = (acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);\r\n    //if((accnorm >=0.96) && (accnorm <= 0.99)){\r\n    if((accnorm >=0.94) && (accnorm <= 1.03)){  \r\n        accnorm_test = 0;\r\n    } else {\r\n        accnorm_test = 1; }\r\n    //take average of 5 to 10 points\r\n    accnorm_avg.newNum(accnorm);\r\n    accnorm_test_avg.newNum(accnorm_test);\r\n\r\n    /* ####################################################################\r\n    #\r\n    #   squared norm analysis to determine suddenly changes signal\r\n    #\r\n    ##################################################################### */\r\n    accnorm_var.newNum(sq(accnorm-accnorm_avg.getAvg()));\r\n    if(accnorm_var.getAvg() < 0.0005) {\r\n        accnorm_var_test = 0;\r\n    }else {\r\n        accnorm_var_test = 1; }\r\n\r\n    /*####################################################################\r\n    #\r\n    #   angular rate analysis in order to disregard linear acceleration\r\n    #\r\n    ################################################################### */\r\n    if ((gyro[0] >=-0.005) && (gyro[0] <= 0.005)) {\r\n        omegax = 0;\r\n    } else {\r\n        omegax = 1; }\r\n        \r\n    if((gyro[1] >= -0.005) && (gyro[1] <= 0.005)) {\r\n        omegay = 0;\r\n    } else {\r\n        omegay = 1; }\r\n        \r\n    if((gyro[2] >= -0.005) && (gyro[2] <= 0.005)) {\r\n        omegaz = 0;\r\n    } else {\r\n        omegaz = 1; }\r\n        \r\n    if ((omegax+omegay+omegaz) > 0) {\r\n        omega_test = 1;\r\n    } else {\r\n        omega_test = 0; }\r\n\r\n\r\n    /* ####################################################################\r\n    #\r\n    # combined movement detector\r\n    #\r\n    #################################################################### */\r\n    if ((accnorm_test_avg.getAvg() + omega_test + accnorm_var_test) > 0) {\r\n        motionDetect = 1;\r\n    } else {\r\n        motionDetect = 0; }\r\n    \r\n    //####################################################################    \r\n    \r\n    motion_detect_ma.newNum(motionDetect);\r\n    \r\n    if(motion_detect_ma.getAvg() > 0.5) {\r\n      return 1;\r\n    } else {\r\n      return 0; }\r\n      \r\n     //return omegaz;\r\n}"
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/ODO.pde",
    "content": "\r\n/**************************************************************************\r\n//This function allows movement end detection. If a certain number of \r\n//acceleration samples are  equal to zero we can assume movement has stopped.\r\n//Accumulated Error generated in the velocity calculations is eliminated by \r\n//resetting the velocity variables. This stops position increment and greatly\r\n//eliminates position error.\r\n//***************************************************************************/  \r\n\r\nvoid movement_end_check()   \r\n{\r\n\r\n  if (accelerationx[1] == 0)   //we count the number of acceleration samples that equals zero \r\n{ countx++;}\r\n  else { countx = 0;}\r\n\r\n  if (countx >= 25)             //if this number exceeds 25, we can assume that velocity is zero\r\n   {    velocityx[1] = 0;\r\n        velocityx[0] = 0;\r\n    }\r\n   \r\n   if (accelerationy[1] == 0)   //we do the same for the Y axis\r\n   { county++;}\r\n   else {  county = 0;}\r\n  \r\n  if (county >= 25)\r\n   { \r\n     velocityy[1] = 0;\r\n     velocityy[0] = 0;\r\n    } \r\n}\r\n\r\n//***********************************************************************\r\n//***********************************************************************\r\n//This function transforms acceleration to a proportional position by \r\n//integrating the acceleration data twice. It also adjusts sensibility by   \r\n//multiplying the \"positionX\" and \"positionY\" variables. \r\n//This integration algorithm carries error, which is compensated in the \r\n//\"movenemt end check\" subroutine. Faster sampling frequency implies less \r\n//error but requires more memory. Keep in  mind that the same process is \r\n//applied to the X and Y axis. \r\n//******************************************************************************/  \r\n \r\nvoid position()  \r\n{\r\n  int count2=0;\r\n    \r\n  do{\r\n     //ADC GetAllAxis();\r\n     gravity_compensate();\r\n     \r\n     accelerationx[0] = accelerationx[0] + Sample_X; //filtering routine for noise attenuation\r\n     accelerationy[0] = accelerationy[0] + Sample_Y; //64 samples are averaged. The resulting \r\n     //average represents the acceleration of \r\n     //an instant\r\n      count2++;                                       \r\n\r\n    } while (count2 != 0x40);                   // 64 sums of the acceleration sample\r\n  \r\n\r\n  accelerationx[0]= accelerationx[0]/64;       // division by 64\r\n  accelerationy[0]= accelerationy[0]/64;\r\n  \r\n  accelerationx[0] = accelerationx[0] - (int)sstatex;  //eliminating zero reference \r\n                                                       //offset of the acceleration data\r\n  accelerationy[0] = accelerationy[0] - (int)sstatey;  //to obtain positive and negative \r\n                                                       //acceleration\r\n  \r\n  if ((accelerationx[0] <=0.010)&&(accelerationx[0] >= -0.010))  //Discrimination window applied \r\n          {accelerationx[0] = 0;}                        // to the X axis acceleration \r\n                                                         //variable\r\n               \r\n  if ((accelerationy[0] <=0.010) && (accelerationy[0] >= -0.010)) \r\n                {accelerationy[0] = 0;} \r\n  \r\n  //first X integration:\r\n  velocityx[0]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] - accelerationx[0] )/2);  \r\n\r\n  //second X integration:\r\n  positionX[0]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])/2);\r\n\r\n  //first Y integration:       \r\n  velocityy[0] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])/2);\r\n\r\n  //second Y integration:       \r\n  positionY[0] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])/2);  \r\n\r\n  accelerationx[1] = accelerationx[0];   //The current acceleration value must be sent \r\n                                         //to the previous acceleration \r\n  accelerationy[1] = accelerationy[0];   //variable in order to introduce the new \r\n                                         //acceleration value.\r\n    \r\n  velocityx[1] = velocityx[0];         //Same done for the velocity variable\r\n  velocityy[1] = velocityy[0];\r\n  \r\n  /* positionX[1] = positionX[1]*262144;       //The idea behind this shifting (multiplication) \r\n                                         //is a sensibility adjustment. <<18\r\n  positionY[1] = positionY[1]*262144;      //Some applications require adjustments to a \r\n                                        //particular situation \r\n                                        //i.e. mouse application \r\n  */\r\n  \r\n  //data_transfer();\r\n  \r\n  /*  \r\n  positionX[1] = positionX[1]/262144;      //once the variables are sent them must return to \r\n  positionY[1] = positionY[1]/262144;      //their original state\r\n  */\r\n  //text(\"px:  \" + positionX[0] + \"\\n\" + \"py:  \" + positionY[0], 200, 200);\r\n  \r\n  movement_end_check();\r\n  \r\n  /*  \r\n  positionX[0] = positionX[1];          //actual position data must be sent to the  \r\n  positionY[0] = positionY[1];     //previous position\r\n  */  \r\n\r\n  direction = 0;                        // data variable to direction variable reset\r\n\r\n\r\n}    \r\n\r\n//*****************************************************************************/\r\n\r\n// compensate the accelerometer readings from gravity. \r\n// @param q the quaternion representing the orientation of a 9DOM MARG sensor array\r\n// @param acc the readings coming from an accelerometer expressed in g\r\n//\r\n// Code snippet from Fabio Versano - blog on same topic\r\n//\r\n// @return a 3d vector representing dynamic acceleration expressed in g\r\nvoid gravity_compensate(){\r\n  float [] g = {0.0, 0.0, 0.0};\r\n  \r\n  // get expected direction of gravity\r\n  g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);\r\n  g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);\r\n  g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];\r\n  \r\n  // compensate accelerometer readings with the expected direction of gravity\r\n  //return [acc[0] - g[0], acc[1] - g[1], acc[2] - g[2]]\r\n  Sample_X = acc[0] - g[0];\r\n  Sample_Y = acc[1] - g[1];\r\n\r\n }"
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/Quaternion.java",
    "content": "\r\n/**\r\n *                     ProScene (version 1.1.1)      \r\n *    Copyright (c) 2010-2012 by National University of Colombia\r\n *                 @author Jean Pierre Charalambos      \r\n *           http://www.disi.unal.edu.co/grupos/remixlab/\r\n *                           \r\n * This java package provides classes to ease the creation of interactive 3D\r\n * scenes in Processing.\r\n * \r\n * This source file is free software; you can redistribute it and/or modify it\r\n * under the terms of the GNU General Public License as published by the Free\r\n * Software Foundation; either version 3 of the License, or (at your option)\r\n * any later version.\r\n * \r\n * This code is distributed in the hope that it will be useful, but WITHOUT\r\n * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or\r\n * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for\r\n * more details.\r\n * \r\n * A copy of the GNU General Public License is available on the World Wide Web\r\n * at <http://www.gnu.org/copyleft/gpl.html>. You can also obtain it by\r\n * writing to the Free Software Foundation, 51 Franklin Street, Suite 500\r\n * Boston, MA 02110-1335, USA.\r\n */\r\n\r\n\r\nimport processing.core.*;\r\n\r\n/**\r\n * A 4 element unit quaternion represented by single precision floating point\r\n * x,y,z,w coordinates.\r\n * \r\n */\r\n\r\npublic class Quaternion extends PApplet {\r\n\t/**\r\n\t * The x coordinate, i.e., the x coordinate of the vector part of the\r\n\t * Quaternion.\r\n\t */\r\n\tpublic float x;\r\n\r\n\t/**\r\n\t * The y coordinate, i.e., the y coordinate of the vector part of the\r\n\t * Quaternion.\r\n\t */\r\n\tpublic float y;\r\n\r\n\t/**\r\n\t * The z coordinate, i.e., the z coordinate of the vector part of the\r\n\t * Quaternion.\r\n\t */\r\n\tpublic float z;\r\n\r\n\t/**\r\n\t * The w coordinate which corresponds to the scalar part of the Quaternion.\r\n\t */\r\n\tpublic float w;\r\n\r\n\t/**\r\n\t * Constructs and initializes a Quaternion to (0.0,0.0,0.0,1.0), i.e., an\r\n\t * identity rotation.\r\n\t */\r\n\tpublic Quaternion () {\r\n\t\tthis.x = 0;\r\n\t\tthis.y = 0;\r\n\t\tthis.z = 0;\r\n\t\tthis.w = 1;\r\n\t}\r\n\r\n\t/**\r\n\t * Default constructor for Quaternion(float x, float y, float z, float w,\r\n\t * boolean normalize), with {@code normalize=true}.\r\n\t * \r\n\t */\r\n\tpublic Quaternion (float x, float y, float z, float w) {\r\n\t\tthis(x, y, z, w, true);\r\n\t}\r\n\r\n\t/**\r\n\t * Constructs and initializes a Quaternion from the specified xyzw\r\n\t * coordinates.\r\n\t * \r\n\t * @param x\r\n\t *          the x coordinate\r\n\t * @param y\r\n\t *          the y coordinate\r\n\t * @param z\r\n\t *          the z coordinate\r\n\t * @param w\r\n\t *          the w scalar component\r\n\t * @param normalize\r\n\t *          tells whether or not the constructed Quaternion should be\r\n\t *          normalized.\r\n\t */\r\n\tpublic Quaternion(float x, float y, float z, float w, boolean normalize) {\r\n\t\tif (normalize) {\r\n\t\t\tfloat mag = PApplet.sqrt(x * x + y * y + z * z + w * w);\r\n\t\t\tif (mag > 0.0f) {\r\n\t\t\t\tthis.x = x / mag;\r\n\t\t\t\tthis.y = y / mag;\r\n\t\t\t\tthis.z = z / mag;\r\n\t\t\t\tthis.w = w / mag;\r\n\t\t\t} else {\r\n\t\t\t\tthis.x = 0f;\r\n\t\t\t\tthis.y = 0f;\r\n\t\t\t\tthis.z = 0f;\r\n\t\t\t\tthis.w = 1f;\r\n\t\t\t}\r\n\t\t} else {\r\n\t\t\tthis.x = x;\r\n\t\t\tthis.y = y;\r\n\t\t\tthis.z = z;\r\n\t\t\tthis.w = w;\r\n\t\t}\r\n\t}\r\n\r\n\t/**\r\n\t * Default constructor for Quaternion(float[] q, boolean normalize) with\r\n\t * {@code normalize=true}.\r\n\t * \r\n\t */\r\n\tpublic Quaternion(float[] q) {\r\n\t\tthis(q, true);\r\n\t}\r\n\r\n\t/**\r\n\t * Constructs and initializes a Quaternion from the array of length 4.\r\n\t * \r\n\t * @param q\r\n\t *          the array of length 4 containing xyzw in order\r\n\t */\r\n\tpublic Quaternion(float[] q, boolean normalize) {\r\n\t\tif (normalize) {\r\n\t\t\tfloat mag = PApplet.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3]\r\n\t\t\t\t\t* q[3]);\r\n\t\t\tif (mag > 0.0f) {\r\n\t\t\t\tthis.x = q[0] / mag;\r\n\t\t\t\tthis.y = q[1] / mag;\r\n\t\t\t\tthis.z = q[2] / mag;\r\n\t\t\t\tthis.w = q[3] / mag;\r\n\t\t\t} else {\r\n\t\t\t\tthis.x = 0;\r\n\t\t\t\tthis.y = 0;\r\n\t\t\t\tthis.z = 0;\r\n\t\t\t\tthis.w = 1;\r\n\t\t\t}\r\n\t\t} else {\r\n\t\t\tthis.x = q[0];\r\n\t\t\tthis.y = q[1];\r\n\t\t\tthis.z = q[2];\r\n\t\t\tthis.w = q[3];\r\n\t\t}\r\n\t}\r\n\r\n\t/**\r\n\t * Copy constructor.\r\n\t * \r\n\t * @param q1\r\n\t *          the Quaternion containing the initialization x y z w data\r\n\t */\r\n\tpublic Quaternion(Quaternion q1) {\r\n\t\tset(q1);\r\n\t}\r\n\r\n\t/**\r\n\t * Copy constructor. If {@code normalize} is {@code true} this Quaternion is\r\n\t * {@link #normalize()}.\r\n\t * \r\n\t * @param q1\r\n\t *          the Quaternion containing the initialization x y z w data\r\n\t */\r\n\tpublic Quaternion(Quaternion q1, boolean normalize) {\r\n\t\tset(q1, normalize);\r\n\t}\r\n\r\n\t/**\r\n\t * Convenience function that simply calls {@code set(q1, true);}\r\n\t * \r\n\t * @see #set(Quaternion, boolean)\r\n\t */\r\n\tpublic void set(Quaternion q1) {\r\n\t\tset(q1, true);\r\n\t}\r\n\r\n\t/**\r\n\t * Set this Quaternion from quaternion {@code q1}. If {@code normalize} is\r\n\t * {@code true} this Quaternion is {@link #normalize()}.\r\n\t */\r\n\tpublic void set(Quaternion q1, boolean normalize) {\r\n\t\tthis.x = q1.x;\r\n\t\tthis.y = q1.y;\r\n\t\tthis.z = q1.z;\r\n\t\tthis.w = q1.w;\r\n\t\tif (normalize)\r\n\t\t\tthis.normalize();\r\n\t}\r\n\r\n\t/**\r\n\t * Constructs and initializes a Quaternion from the specified rotation\r\n\t * {@link #axis() axis} (non null) and {@link #angle() angle} (in radians).\r\n\t * \r\n\t * @param axis\r\n\t *          the PVector representing the axis\r\n\t * @param angle\r\n\t *          the angle in radians\r\n\t * \r\n\t * @see #fromAxisAngle(PVector, float)\r\n\t */\r\n\tpublic Quaternion(PVector axis, float angle) {\r\n\t\tfromAxisAngle(axis, angle);\r\n\t}\r\n\r\n\t/**\r\n\t * Constructs a Quaternion that will rotate from the {@code from} direction to\r\n\t * the {@code to} direction.\r\n\t * \r\n\t * @param from\r\n\t *          the first PVector\r\n\t * @param to\r\n\t *          the second PVector\r\n\t * \r\n\t * @see #fromTo(PVector, PVector)\r\n\t */\r\n\tpublic Quaternion(PVector from, PVector to) {\r\n\t\tfromTo(from, to);\r\n\t}\r\n\r\n\t/**\r\n\t * Sets the value of this Quaternion to the conjugate of itself.\r\n\t */\r\n\tpublic final void conjugate() {\r\n\t\tthis.x = -this.x;\r\n\t\tthis.y = -this.y;\r\n\t\tthis.z = -this.z;\r\n\t}\r\n\r\n\t/**\r\n\t * Sets the value of this Quaternion to the conjugate of Quaternion q1.\r\n\t * \r\n\t * @param q1\r\n\t *          the source vector\r\n\t */\r\n\tpublic final void conjugate(Quaternion q1) {\r\n\t\tthis.x = -q1.x;\r\n\t\tthis.y = -q1.y;\r\n\t\tthis.z = -q1.z;\r\n\t\tthis.w = q1.w;\r\n\t}\r\n\r\n\t/**\r\n\t * Negates all the coefficients of the Quaternion.\r\n\t */\r\n\tpublic final void negate() {\r\n\t\tthis.x = -this.x;\r\n\t\tthis.y = -this.y;\r\n\t\tthis.z = -this.z;\r\n\t\tthis.w = -this.w;\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the \"dot\" product of this Quaternion and {@code b}:\r\n\t * <p>\r\n\t * {@code this.x * b.x + this.y * b.y + this.z * b.z + this.w * b.w}\r\n\t * \r\n\t * @param b\r\n\t *          the Quaternion\r\n\t */\r\n\tpublic final float dotProduct(Quaternion b) {\r\n\t\treturn this.x * b.x + this.y * b.y + this.z * b.z + this.w * b.w;\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the \"dot\" product of {@code a} and {@code b}:\r\n\t * <p>\r\n\t * {@code a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w}\r\n\t * \r\n\t * @param a\r\n\t *          the first Quaternion\r\n\t * @param b\r\n\t *          the second Quaternion\r\n\t */\r\n\tpublic final static float dotProduct(Quaternion a, Quaternion b) {\r\n\t\treturn a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;\r\n\t}\r\n\r\n\t/**\r\n\t * Sets the value of this Quaternion to the Quaternion product of itself and\r\n\t * {@code q1}, (i.e., {@code this = this * q1}).\r\n\t * \r\n\t * @param q1\r\n\t *          the other Quaternion\r\n\t */\r\n\tpublic final void multiply(Quaternion q1) {\r\n\t\tfloat x, y, w;\r\n\r\n\t\tw = this.w * q1.w - this.x * q1.x - this.y * q1.y - this.z * q1.z;\r\n\t\tx = this.w * q1.x + q1.w * this.x + this.y * q1.z - this.z * q1.y;\r\n\t\ty = this.w * q1.y + q1.w * this.y - this.x * q1.z + this.z * q1.x;\r\n\t\tthis.z = this.w * q1.z + q1.w * this.z + this.x * q1.y - this.y * q1.x;\r\n\t\tthis.w = w;\r\n\t\tthis.x = x;\r\n\t\tthis.y = y;\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the Quaternion which is product of quaternions {@code q1} and\r\n\t * {@code q2}.\r\n\t * \r\n\t * @param q1\r\n\t *          the first Quaternion\r\n\t * @param q2\r\n\t *          the second Quaternion\r\n\t */\r\n\tpublic final static Quaternion multiply(Quaternion q1, Quaternion q2) {\r\n\t\tfloat x, y, z, w;\r\n\t\tw = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;\r\n\t\tx = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;\r\n\t\ty = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;\r\n\t\tz = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;\r\n\t\treturn new Quaternion(x, y, z, w);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the image of {@code v} by the rotation of this vector. Same as\r\n\t * {@code this.rotate(v).}\r\n\t * \r\n\t * @param v\r\n\t *          the PVector\r\n\t * \r\n\t * @see #rotate(PVector)\r\n\t * @see #inverseRotate(PVector)\r\n\t */\r\n\tpublic final PVector multiply(PVector v) {\r\n\t\treturn this.rotate(v);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the image of {@code v} by the rotation {@code q1}. Same as {@code\r\n\t * q1.rotate(v).}\r\n\t * \r\n\t * @param q1\r\n\t *          the Quaternion\r\n\t * \r\n\t * @param v\r\n\t *          the PVector\r\n\t * \r\n\t * @see #rotate(PVector)\r\n\t * @see #inverseRotate(PVector)\r\n\t */\r\n\tpublic static final PVector multiply(Quaternion q1, PVector v) {\r\n\t\treturn q1.rotate(v);\r\n\t}\r\n\r\n\t/**\r\n\t * Multiplies this Quaternion by the inverse of Quaternion {@code q1} and\r\n\t * places the value into this Quaternion (i.e., {@code this = this * q^-1}).\r\n\t * The value of the argument Quaternion is preserved.\r\n\t * \r\n\t * @param q1\r\n\t *          the other Quaternion\r\n\t */\r\n\tpublic final void multiplyInverse(Quaternion q1) {\r\n\t\tQuaternion tempQuat = new Quaternion(q1);\r\n\t\ttempQuat.invert();\r\n\t\tthis.multiply(tempQuat);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the product of Quaternion {@code q1} by the inverse of Quaternion\r\n\t * {@code q2} (i.e., {@code q1 * q2^-1}). The value of both argument\r\n\t * quaternions is preserved.\r\n\t * \r\n\t * @param q1\r\n\t *          the first Quaternion\r\n\t * @param q2\r\n\t *          the second Quaternion\r\n\t */\r\n\tpublic static final Quaternion multiplyInverse(Quaternion q1, Quaternion q2) {\r\n\t\tQuaternion tempQuat = new Quaternion(q2);\r\n\t\ttempQuat.invert();\r\n\t\treturn Quaternion.multiply(q1, tempQuat);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the inverse Quaternion (inverse rotation).\r\n\t * <p>\r\n\t * The result has a negated {@link #axis()} direction and the same\r\n\t * {@link #angle()}.\r\n\t * <p>\r\n\t * A composition of a Quaternion and its {@link #inverse()} results in an\r\n\t * identity function. Use {@link #invert()} to actually modify the Quaternion.\r\n\t * \r\n\t * @see #invert()\r\n\t */\r\n\tpublic final Quaternion inverse() {\r\n\t\tQuaternion tempQuat = new Quaternion(this);\r\n\t\ttempQuat.invert();\r\n\t\treturn tempQuat;\r\n\t}\r\n\r\n\t/**\r\n\t * Sets the value of this Quaternion to the inverse of itself.\r\n\t * \r\n\t * @see #inverse()\r\n\t */\r\n\tpublic final void invert() {\r\n\t\tfloat sqNorm = squaredNorm(this);\r\n\t\tthis.w /= sqNorm;\r\n\t\tthis.x /= -sqNorm;\r\n\t\tthis.y /= -sqNorm;\r\n\t\tthis.z /= -sqNorm;\r\n\t}\r\n\r\n\t/**\r\n\t * Sets the value of this Quaternion to the Quaternion inverse of {@code q1}.\r\n\t * \r\n\t * @param q1\r\n\t *          the Quaternion to be inverted\r\n\t */\r\n\tpublic final void invert(Quaternion q1) {\r\n\t\tfloat sqNorm = squaredNorm(q1);\r\n\t\tthis.w = q1.w / sqNorm;\r\n\t\tthis.x = -q1.x / sqNorm;\r\n\t\tthis.y = -q1.y / sqNorm;\r\n\t\tthis.z = -q1.z / sqNorm;\r\n\t}\r\n\r\n\t/**\r\n\t * Normalizes the value of this Quaternion in place and return its {@code\r\n\t * norm}.\r\n\t */\r\n\tpublic final float normalize() {\r\n\t\tfloat norm = PApplet.sqrt(this.x * this.x + this.y * this.y + this.z\r\n\t\t\t\t* this.z + this.w * this.w);\r\n\t\tif (norm > 0.0f) {\r\n\t\t\tthis.x /= norm;\r\n\t\t\tthis.y /= norm;\r\n\t\t\tthis.z /= norm;\r\n\t\t\tthis.w /= norm;\r\n\t\t} else {\r\n\t\t\tthis.x = (float) 0.0;\r\n\t\t\tthis.y = (float) 0.0;\r\n\t\t\tthis.z = (float) 0.0;\r\n\t\t\tthis.w = (float) 1.0;\r\n\t\t}\r\n\t\treturn norm;\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the image of {@code v} by the Quaternion rotation.\r\n\t * \r\n\t * @param v\r\n\t *          the PVector\r\n\t */\r\n\tpublic final PVector rotate(PVector v) {\r\n\t\tfloat q00 = 2.0f * x * x;\r\n\t\tfloat q11 = 2.0f * y * y;\r\n\t\tfloat q22 = 2.0f * z * z;\r\n\r\n\t\tfloat q01 = 2.0f * x * y;\r\n\t\tfloat q02 = 2.0f * x * z;\r\n\t\tfloat q03 = 2.0f * x * w;\r\n\r\n\t\tfloat q12 = 2.0f * y * z;\r\n\t\tfloat q13 = 2.0f * y * w;\r\n\r\n\t\tfloat q23 = 2.0f * z * w;\r\n\r\n\t\treturn new PVector((1.0f - q11 - q22) * v.x + (q01 - q23) * v.y\r\n\t\t\t\t+ (q02 + q13) * v.z, (q01 + q23) * v.x + (1.0f - q22 - q00) * v.y\r\n\t\t\t\t+ (q12 - q03) * v.z, (q02 - q13) * v.x + (q12 + q03) * v.y\r\n\t\t\t\t+ (1.0f - q11 - q00) * v.z);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the image of {@code v} by the Quaternion {@link #inverse()}\r\n\t * rotation.\r\n\t * <p>\r\n\t * {@link #rotate(PVector)} performs an inverse transformation.\r\n\t * \r\n\t * @param v\r\n\t *          the PVector\r\n\t */\r\n\tpublic final PVector inverseRotate(PVector v) {\r\n\t\tQuaternion tempQuat = new Quaternion(x, y, z, w);\r\n\t\ttempQuat.invert();\r\n\t\treturn tempQuat.rotate(v);\r\n\t}\r\n\r\n\t/**\r\n\t * Sets the Quaternion as a rotation of {@link #axis() axis} and\r\n\t * {@link #angle() angle} (in radians).\r\n\t * <p>\r\n\t * The {@code axis} does not need to be normalized. A null {@code axis} will\r\n\t * result in an identity Quaternion.\r\n\t * \r\n\t * @param axis\r\n\t *          the PVector representing the axis\r\n\t * @param angle\r\n\t *          the angle in radians\r\n\t */\r\n\tpublic void fromAxisAngle(PVector axis, float angle) {\r\n\t\tfloat norm = axis.mag();\r\n\t\tif (norm < 1E-8f) {\r\n\t\t\t// Null rotation\r\n\t\t\tthis.x = 0.0f;\r\n\t\t\tthis.y = 0.0f;\r\n\t\t\tthis.z = 0.0f;\r\n\t\t\tthis.w = 1.0f;\r\n\t\t} else {\r\n\t\t\tfloat sin_half_angle = PApplet.sin(angle / 2.0f);\r\n\t\t\tthis.x = sin_half_angle * axis.x / norm;\r\n\t\t\tthis.y = sin_half_angle * axis.y / norm;\r\n\t\t\tthis.z = sin_half_angle * axis.z / norm;\r\n\t\t\tthis.w = PApplet.cos(angle / 2.0f);\r\n\t\t}\r\n\t}\r\n\r\n\t/**\r\n\t * Same as {@link #fromEulerAngles(PVector)}.\r\n\t */\r\n\tpublic void fromTaitBryan(PVector angles) {\r\n\t\tfromEulerAngles(angles);\r\n\t}\r\n\r\n\t/**\r\n\t * Same as {@link #fromEulerAngles(float, float, float)}.\r\n\t */\r\n\tpublic void fromTaitBryan(float roll, float pitch, float yaw) {\r\n\t\tfromEulerAngles(roll, pitch, yaw);\r\n\t}\r\n\r\n\t/**\r\n\t * Convenience function that simply calls {@code fromEulerAngles(angles.x,\r\n\t * angles.y, angles.z)}.\r\n\t * \r\n\t * @see #fromEulerAngles(float, float, float)\r\n\t * @see #eulerAngles()\r\n\t */\r\n\tpublic void fromEulerAngles(PVector angles) {\r\n\t\tfromEulerAngles(angles.x, angles.y, angles.z);\r\n\t}\r\n\r\n\t/**\r\n\t * Converts Euler rotation angles {@code roll}, {@code pitch} and {@code yaw},\r\n\t * respectively defined to the x, y and z axes, to this Quaternion. In the\r\n\t * convention used here these angles represent a composition of extrinsic\r\n\t * rotations (rotations about the reference frame axes), which is also known\r\n\t * as {@link #taitBryanAngles()} (See\r\n\t * http://en.wikipedia.org/wiki/Euler_angles and\r\n\t * http://en.wikipedia.org/wiki/Tait-Bryan_angles). {@link #eulerAngles()}\r\n\t * performs the inverse operation.\r\n\t * <p>\r\n\t * Each rotation angle is converted to an axis-angle pair, with the axis\r\n\t * corresponding to one of the Euclidean axes. The axis-angle pairs are\r\n\t * converted to quaternions and multiplied together. The order of the\r\n\t * rotations is: y->z->x which follows the convention found here:\r\n\t * http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm.\r\n\t * \r\n\t * @see #eulerAngles()\r\n\t */\r\n\tpublic void fromEulerAngles(float roll, float pitch, float yaw) {\r\n\t\tQuaternion qx = new Quaternion(new PVector(1, 0, 0), roll);\r\n\t\tQuaternion qy = new Quaternion(new PVector(0, 1, 0), pitch);\r\n\t\tQuaternion qz = new Quaternion(new PVector(0, 0, 1), yaw);\r\n\t\tset(qy);\r\n\t\tmultiply(qz);\r\n\t\tmultiply(qx);\r\n\t}\r\n\r\n\t/**\r\n\t * Same as {@link #eulerAngles()}.\r\n\t */\r\n\tpublic PVector taitBryanAngles() {\r\n\t\treturn eulerAngles();\r\n\t}\r\n\r\n\t/**\r\n\t * Converts this Quaternion to Euler rotation angles {@code roll}, {@code\r\n\t * pitch} and {@code yaw} in radians.\r\n\t * {@link #fromEulerAngles(float, float, float)} performs the inverse\r\n\t * operation. The code was adapted from:\r\n\t * http://www.euclideanspace.com/maths/geometry\r\n\t * /rotations/conversions/quaternionToEuler/index.htm.\r\n\t * <p>\r\n\t * <b>Attention:</b> This method assumes that this Quaternion is normalized.\r\n\t * \r\n\t * @return the PVector holding the roll (x coordinate of the vector), pitch (y\r\n\t *         coordinate of the vector) and yaw angles (z coordinate of the\r\n\t *         vector). <b>Note:</b> The order of the rotations that would produce\r\n\t *         this Quaternion (i.e., as with {@code fromEulerAngles(roll, pitch,\r\n\t *         yaw)}) is: y->z->x.\r\n\t * \r\n\t * @see #fromEulerAngles(float, float, float)\r\n\t */\r\n\tpublic PVector eulerAngles() {\r\n\t\tfloat roll, pitch, yaw;\r\n\t\tfloat test = x * y + z * w;\r\n\t\tif (test > 0.499) { // singularity at north pole\r\n\t\t\tpitch = 2 * PApplet.atan2(x, w);\r\n\t\t\tyaw = PApplet.PI / 2;\r\n\t\t\troll = 0;\r\n\t\t\treturn new PVector(roll, pitch, yaw);\r\n\t\t}\r\n\t\tif (test < -0.499) { // singularity at south pole\r\n\t\t\tpitch = -2 * PApplet.atan2(x, w);\r\n\t\t\tyaw = -PApplet.PI / 2;\r\n\t\t\troll = 0;\r\n\t\t\treturn new PVector(roll, pitch, yaw);\r\n\t\t}\r\n\t\tfloat sqx = x * x;\r\n\t\tfloat sqy = y * y;\r\n\t\tfloat sqz = z * z;\r\n\t\tpitch = PApplet.atan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz);\r\n\t\tyaw = PApplet.asin(2 * test);\r\n\t\troll = PApplet.atan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz);\r\n\t\treturn new PVector(roll, pitch, yaw);\r\n\t}\r\n\r\n\t/**\r\n\t * public PVector eulerAngles() { //This quaternion does not need to be\r\n\t * normalized. See:\r\n\t * //http://www.euclideanspace.com/maths/geometry/rotations/conversions\r\n\t * /quaternionToEuler/index.htm float roll, pitch, yaw; float sqw = w*w; float\r\n\t * sqx = x*x; float sqy = y*y; float sqz = z*z; float unit = sqx + sqy + sqz +\r\n\t * sqw; // if normalised is one, otherwise is correction factor float test =\r\n\t * x*y + z*w; if (test > 0.499*unit) { // singularity at north pole pitch = 2\r\n\t * * PApplet.atan2(x,w); yaw = PApplet.PI/2; roll = 0; return new\r\n\t * PVector(roll, pitch, yaw); } if (test < -0.499*unit) { // singularity at\r\n\t * south pole pitch = -2 * PApplet.atan2(x,w); yaw = - PApplet.PI/2; roll = 0;\r\n\t * return new PVector(roll, pitch, yaw); } pitch = PApplet.atan2(2*y*w-2*x*z ,\r\n\t * sqx - sqy - sqz + sqw); yaw = PApplet.asin(2*test/unit); roll =\r\n\t * PApplet.atan2(2*x*w-2*y*z , -sqx + sqy - sqz + sqw); return new\r\n\t * PVector(roll, pitch, yaw); } //\r\n\t */\r\n\r\n\t/**\r\n\t * Sets the Quaternion as a rotation from the {@code from} direction to the\r\n\t * {@code to} direction.\r\n\t * <p>\r\n\t * <b>Attention:</b> this rotation is not uniquely defined. The selected axis\r\n\t * is usually orthogonal to {@code from} and {@code to}, minimizing the\r\n\t * rotation angle. This method is robust and can handle small or almost\r\n\t * identical vectors.\r\n\t * \r\n\t * @see #fromAxisAngle(PVector, float)\r\n\t */\r\n\tpublic void fromTo(PVector from, PVector to) {\r\n\t\tfloat fromSqNorm = MathUtils.squaredNorm(from);\r\n\t\tfloat toSqNorm = MathUtils.squaredNorm(to);\r\n\t\t// Identity Quaternion when one vector is null\r\n\t\tif ((fromSqNorm < 1E-10f) || (toSqNorm < 1E-10f)) {\r\n\t\t\tthis.x = this.y = this.z = 0.0f;\r\n\t\t\tthis.w = 1.0f;\r\n\t\t} else {\r\n\r\n\t\t\tPVector axis = from.cross(to);\r\n\r\n\t\t\tfloat axisSqNorm = MathUtils.squaredNorm(axis);\r\n\r\n\t\t\t// Aligned vectors, pick any axis, not aligned with from or to\r\n\t\t\tif (axisSqNorm < 1E-10f)\r\n\t\t\t\taxis = MathUtils.orthogonalVector(from);\r\n\r\n\t\t\tfloat angle = PApplet.asin(PApplet.sqrt(axisSqNorm\r\n\t\t\t\t\t/ (fromSqNorm * toSqNorm)));\r\n\r\n\t\t\tif (from.dot(to) < 0.0)\r\n\t\t\t\tangle = PI - angle;\r\n\r\n\t\t\tfromAxisAngle(axis, angle);\r\n\t\t}\r\n\t}\r\n\r\n\t/**\r\n\t * Set the Quaternion from a (supposedly correct) 3x3 rotation matrix.\r\n\t * <p>\r\n\t * The matrix is expressed in European format: its three columns are the\r\n\t * images by the rotation of the three vectors of an orthogonal basis.\r\n\t * <p>\r\n\t * {@link #fromRotatedBasis(PVector, PVector, PVector)} sets a Quaternion from\r\n\t * the three axis of a rotated frame. It actually fills the three columns of a\r\n\t * matrix with these rotated basis vectors and calls this method.\r\n\t * \r\n\t * @param m\r\n\t *          the 3*3 matrix of float values\r\n\t */\r\n\tpublic final void fromRotationMatrix(float m[][]) {\r\n\t\t// Compute one plus the trace of the matrix\r\n\t\tfloat onePlusTrace = 1.0f + m[0][0] + m[1][1] + m[2][2];\r\n\r\n\t\tif (onePlusTrace > 1E-5f) {\r\n\t\t\t// Direct computation\r\n\t\t\tfloat s = PApplet.sqrt(onePlusTrace) * 2.0f;\r\n\t\t\tthis.x = (m[2][1] - m[1][2]) / s;\r\n\t\t\tthis.y = (m[0][2] - m[2][0]) / s;\r\n\t\t\tthis.z = (m[1][0] - m[0][1]) / s;\r\n\t\t\tthis.w = 0.25f * s;\r\n\t\t} else {\r\n\t\t\t// Computation depends on major diagonal term\r\n\t\t\tif ((m[0][0] > m[1][1]) & (m[0][0] > m[2][2])) {\r\n\t\t\t\tfloat s = PApplet.sqrt(1.0f + m[0][0] - m[1][1] - m[2][2]) * 2.0f;\r\n\t\t\t\tthis.x = 0.25f * s;\r\n\t\t\t\tthis.y = (m[0][1] + m[1][0]) / s;\r\n\t\t\t\tthis.z = (m[0][2] + m[2][0]) / s;\r\n\t\t\t\tthis.w = (m[1][2] - m[2][1]) / s;\r\n\t\t\t} else if (m[1][1] > m[2][2]) {\r\n\t\t\t\tfloat s = PApplet.sqrt(1.0f + m[1][1] - m[0][0] - m[2][2]) * 2.0f;\r\n\t\t\t\tthis.x = (m[0][1] + m[1][0]) / s;\r\n\t\t\t\tthis.y = 0.25f * s;\r\n\t\t\t\tthis.z = (m[1][2] + m[2][1]) / s;\r\n\t\t\t\tthis.w = (m[0][2] - m[2][0]) / s;\r\n\t\t\t} else {\r\n\t\t\t\tfloat s = PApplet.sqrt(1.0f + m[2][2] - m[0][0] - m[1][1]) * 2.0f;\r\n\t\t\t\tthis.x = (m[0][2] + m[2][0]) / s;\r\n\t\t\t\tthis.y = (m[1][2] + m[2][1]) / s;\r\n\t\t\t\tthis.z = 0.25f * s;\r\n\t\t\t\tthis.w = (m[0][1] - m[1][0]) / s;\r\n\t\t\t}\r\n\t\t}\r\n\t\tnormalize();\r\n\t}\r\n\r\n\t/**\r\n\t * Set the Quaternion from a (supposedly correct) 3x3 rotation matrix given in\r\n\t * the upper left 3x3 sub-matrix of the PMatrix3D.\r\n\t * \r\n\t * @see #fromRotationMatrix(float[][])\r\n\t */\r\n\tpublic final void fromMatrix(PMatrix3D pM) {\r\n\t\tfromRotationMatrix(MathUtils.get3x3UpperLeftMatrixFromPMatrix3D(pM));\r\n\t}\r\n\r\n\t/**\r\n\t * Sets the Quaternion from the three rotated vectors of an orthogonal basis.\r\n\t * <p>\r\n\t * The three vectors do not have to be normalized but must be orthogonal and\r\n\t * direct (i,e., {@code X^Y=k*Z, with k>0}).\r\n\t * \r\n\t * @param X\r\n\t *          the first PVector\r\n\t * @param Y\r\n\t *          the second PVector\r\n\t * @param Z\r\n\t *          the third PVector\r\n\t * \r\n\t * @see #fromRotationMatrix(float[][])\r\n\t * @see #Quaternion(PVector, PVector)\r\n\t * \r\n\t */\r\n\tpublic final void fromRotatedBasis(PVector X, PVector Y, PVector Z) {\r\n\t\tfloat m[][] = new float[3][3];\r\n\t\tfloat normX = X.mag();\r\n\t\tfloat normY = Y.mag();\r\n\t\tfloat normZ = Z.mag();\r\n\r\n\t\tfor (int i = 0; i < 3; ++i) {\r\n\t\t\tm[i][0] = (X.array())[i] / normX;\r\n\t\t\tm[i][1] = (Y.array())[i] / normY;\r\n\t\t\tm[i][2] = (Z.array())[i] / normZ;\r\n\t\t}\r\n\r\n\t\tfromRotationMatrix(m);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the normalized axis direction of the rotation represented by the\r\n\t * Quaternion.\r\n\t * <p>\r\n\t * The result is null for an identity Quaternion.\r\n\t * \r\n\t * @see #angle()\r\n\t */\r\n\tpublic final PVector axis() {\r\n\t\tPVector res = new PVector(this.x, this.y, this.z);\r\n\t\tfloat sinus = res.mag();\r\n\t\tif (sinus > 1E-8f)\r\n\t\t\tres.div(sinus);\r\n\t\tif (PApplet.acos(this.w) <= HALF_PI)\r\n\t\t\treturn res;\r\n\t\telse {\r\n\t\t\tres.x = -res.x;\r\n\t\t\tres.y = -res.y;\r\n\t\t\tres.z = -res.z;\r\n\t\t\treturn res;\r\n\t\t}\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the {@code angle} (in radians) of the rotation represented by the\r\n\t * Quaternion.\r\n\t * <p>\r\n\t * This value is always in the range {@code [0-pi]}. Larger rotational angles\r\n\t * are obtained by inverting the {@link #axis()} direction.\r\n\t * \r\n\t * @see #axis()\r\n\t */\r\n\tpublic final float angle() {\r\n\t\tfloat angle = 2.0f * PApplet.acos(this.w);\r\n\t\treturn (angle <= PI) ? angle : 2.0f * PI - angle;\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the 3x3 rotation matrix associated with the Quaternion.\r\n\t * <p>\r\n\t * <b>Attention:</b> The method returns the European mathematical\r\n\t * representation of the rotation matrix.\r\n\t * \r\n\t * @see #inverseRotationMatrix()\r\n\t * \r\n\t */\r\n\tpublic final float[][] rotationMatrix() {\r\n\t\treturn MathUtils.get3x3UpperLeftMatrixFromPMatrix3D(matrix());\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the PMatrix3D (processing matrix) which represents the rotation\r\n\t * matrix associated with the Quaternion.\r\n\t * \r\n\t * @see #rotationMatrix()\r\n\t */\r\n\tpublic final PMatrix3D matrix() {\r\n\r\n\t\tfloat q00 = 2.0f * this.x * this.x;\r\n\t\tfloat q11 = 2.0f * this.y * this.y;\r\n\t\tfloat q22 = 2.0f * this.z * this.z;\r\n\r\n\t\tfloat q01 = 2.0f * this.x * this.y;\r\n\t\tfloat q02 = 2.0f * this.x * this.z;\r\n\t\tfloat q03 = 2.0f * this.x * this.w;\r\n\r\n\t\tfloat q12 = 2.0f * this.y * this.z;\r\n\t\tfloat q13 = 2.0f * this.y * this.w;\r\n\r\n\t\tfloat q23 = 2.0f * this.z * this.w;\r\n\r\n\t\tfloat m00 = 1.0f - q11 - q22;\r\n\t\tfloat m01 = q01 - q23;\r\n\t\tfloat m02 = q02 + q13;\r\n\r\n\t\tfloat m10 = q01 + q23;\r\n\t\tfloat m11 = 1.0f - q22 - q00;\r\n\t\tfloat m12 = q12 - q03;\r\n\r\n\t\tfloat m20 = q02 - q13;\r\n\t\tfloat m21 = q12 + q03;\r\n\t\tfloat m22 = 1.0f - q11 - q00;\r\n\r\n\t\tfloat m30 = 0.0f;\r\n\t\tfloat m31 = 0.0f;\r\n\t\tfloat m32 = 0.0f;\r\n\r\n\t\tfloat m03 = 0.0f;\r\n\t\tfloat m13 = 0.0f;\r\n\t\tfloat m23 = 0.0f;\r\n\t\tfloat m33 = 1.0f;\r\n\r\n\t\treturn new PMatrix3D(m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22,\r\n\t\t\t\tm23, m30, m31, m32, m33);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the associated inverse rotation processing PMatrix3D. This is\r\n\t * simply {@link #matrix()} of the {@link #inverse()}.\r\n\t * <p>\r\n\t * <b>Attention:</b> The result is only valid until the next call to\r\n\t * {@link #inverseMatrix()}. Use it immediately (as in {@code\r\n\t * applyMatrix(q.inverseMatrix())}).\r\n\t */\r\n\tpublic final PMatrix3D inverseMatrix() {\r\n\t\tQuaternion tempQuat = new Quaternion(x, y, z, w);\r\n\t\ttempQuat.invert();\r\n\t\treturn tempQuat.matrix();\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the 3x3 inverse rotation matrix associated with the Quaternion.\r\n\t * <p>\r\n\t * <b>Attention:</b> This is the classical mathematical rotation matrix.\r\n\t */\r\n\tpublic final float[][] inverseRotationMatrix() {\r\n\t\treturn MathUtils.get3x3UpperLeftMatrixFromPMatrix3D(inverseMatrix());\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the logarithm of the Quaternion.\r\n\t * \r\n\t * @see #exp()\r\n\t */\r\n\tpublic final Quaternion log() {\r\n\t\t// Warning: this method should not normalize the Quaternion\r\n\t\tfloat len = PApplet.sqrt(this.x * this.x + this.y * this.y + this.z\r\n\t\t\t\t* this.z);\r\n\r\n\t\tif (len < 1E-6f)\r\n\t\t\treturn new Quaternion(this.x, this.y, this.z, 0.0f, false);\r\n\t\telse {\r\n\t\t\tfloat coef = PApplet.acos(this.w) / len;\r\n\t\t\treturn new Quaternion(this.x * coef, this.y * coef, this.z * coef, 0.0f,\r\n\t\t\t\t\tfalse);\r\n\t\t}\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the exponential of the Quaternion.\r\n\t * \r\n\t * @see #log()\r\n\t */\r\n\tpublic final Quaternion exp() {\r\n\t\tfloat theta = PApplet.sqrt(this.x * this.x + this.y * this.y + this.z\r\n\t\t\t\t* this.z);\r\n\r\n\t\tif (theta < 1E-6f)\r\n\t\t\treturn new Quaternion(this.x, this.y, this.z, PApplet.cos(theta));\r\n\t\telse {\r\n\t\t\tfloat coef = PApplet.sin(theta) / theta;\r\n\t\t\treturn new Quaternion(this.x * coef, this.y * coef, this.z * coef,\r\n\t\t\t\t\tPApplet.cos(theta));\r\n\t\t}\r\n\t}\r\n\r\n\t/**\r\n\t * Returns a random unit Quaternion.\r\n\t * <p>\r\n\t * You can create a randomly directed unit vector using:\r\n\t * <p>\r\n\t * {@code PVector randomDir = new PVector(1.0f, 0.0f, 0.0f);} <br>\r\n\t * {@code randomDir = Quaternion.multiply(Quaternion.randomQuaternion(),\r\n\t * randomDir);}\r\n\t */\r\n\tpublic final static Quaternion randomQuaternion() {\r\n\t\tfloat seed = (float) Math.random();\r\n\t\tfloat r1 = PApplet.sqrt(1.0f - seed);\r\n\t\tfloat r2 = PApplet.sqrt(seed);\r\n\t\tfloat t1 = 2.0f * PI * (float) Math.random();\r\n\t\tfloat t2 = 2.0f * PI * (float) Math.random();\r\n\r\n\t\treturn new Quaternion(PApplet.sin(t1) * r1, PApplet.cos(t1) * r1, PApplet\r\n\t\t\t\t.sin(t2)\r\n\t\t\t\t* r2, PApplet.cos(t2) * r2);\r\n\t}\r\n\r\n\t/**\r\n\t * Wrapper function that simply calls {@code slerp(a, b, t, true)}.\r\n\t * <p>\r\n\t * See {@link #slerp(Quaternion, Quaternion, float, boolean)} for details.\r\n\t */\r\n\tpublic static final Quaternion slerp(Quaternion a, Quaternion b, float t) {\r\n\t\treturn Quaternion.slerp(a, b, t, true);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the slerp interpolation of quaternions {@code a} and {@code b}, at\r\n\t * time {@code t}.\r\n\t * <p>\r\n\t * {@code t} should range in {@code [0,1]}. Result is a when {@code t=0 } and\r\n\t * {@code b} when {@code t=1}.\r\n\t * <p>\r\n\t * When {@code allowFlip} is true (default) the slerp interpolation will\r\n\t * always use the \"shortest path\" between the quaternions' orientations, by\r\n\t * \"flipping\" the source Quaternion if needed (see {@link #negate()}).\r\n\t * \r\n\t * @param a\r\n\t *          the first Quaternion\r\n\t * @param b\r\n\t *          the second Quaternion\r\n\t * @param t\r\n\t *          the t interpolation parameter\r\n\t * @param allowFlip\r\n\t *          tells whether or not the interpolation allows axis flip\r\n\t */\r\n\tpublic static final Quaternion slerp(Quaternion a, Quaternion b, float t,\r\n\t\t\tboolean allowFlip) {\r\n\t\t// Warning: this method should not normalize the Quaternion\r\n\t\tfloat cosAngle = Quaternion.dotProduct(a, b);\r\n\r\n\t\tfloat c1, c2;\r\n\t\t// Linear interpolation for close orientations\r\n\t\tif ((1.0 - PApplet.abs(cosAngle)) < 0.01) {\r\n\t\t\tc1 = 1.0f - t;\r\n\t\t\tc2 = t;\r\n\t\t} else {\r\n\t\t\t// Spherical interpolation\r\n\t\t\tfloat angle = PApplet.acos(PApplet.abs(cosAngle));\r\n\t\t\tfloat sinAngle = PApplet.sin(angle);\r\n\t\t\tc1 = PApplet.sin(angle * (1.0f - t)) / sinAngle;\r\n\t\t\tc2 = PApplet.sin(angle * t) / sinAngle;\r\n\t\t}\r\n\r\n\t\t// Use the shortest path\r\n\t\tif (allowFlip && (cosAngle < 0.0))\r\n\t\t\tc1 = -c1;\r\n\r\n\t\treturn new Quaternion(c1 * a.x + c2 * b.x, c1 * a.y + c2 * b.y, c1 * a.z\r\n\t\t\t\t+ c2 * b.z, c1 * a.w + c2 * b.w, false);\r\n\t}\r\n\r\n\t/**\r\n\t * Returns the slerp interpolation of the two quaternions {@code a} and\r\n\t * {@code b}, at time {@code t}, using tangents {@code tgA} and {@code tgB}.\r\n\t * <p>\r\n\t * The resulting Quaternion is \"between\" {@code a} and {@code b} (result is\r\n\t * {@code a} when {@code t=0} and {@code b} for {@code t=1}).\r\n\t * <p>\r\n\t * Use {@link #squadTangent(Quaternion, Quaternion, Quaternion)} to define the\r\n\t * Quaternion tangents {@code tgA} and {@code tgB}.\r\n\t * \r\n\t * @param a\r\n\t *          the first Quaternion\r\n\t * @param tgA\r\n\t *          the first tangent Quaternion\r\n\t * @param tgB\r\n\t *          the second tangent Quaternion\r\n\t * @param b\r\n\t *          the second Quaternion\r\n\t * @param t\r\n\t *          the t interpolation parameter\r\n\t */\r\n\tpublic static final Quaternion squad(Quaternion a, Quaternion tgA,\r\n\t\t\tQuaternion tgB, Quaternion b, float t) {\r\n\t\tQuaternion ab = Quaternion.slerp(a, b, t);\r\n\t\tQuaternion tg = Quaternion.slerp(tgA, tgB, t, false);\r\n\t\treturn Quaternion.slerp(ab, tg, 2.0f * t * (1.0f - t), false);\r\n\t}\r\n\r\n\t/**\r\n\t * Simply returns {@code log(a. inverse() * b)}.\r\n\t * <p>\r\n\t * Useful for {@link #squadTangent(Quaternion, Quaternion, Quaternion)}.\r\n\t * \r\n\t * @param a\r\n\t *          the first Quaternion\r\n\t * @param b\r\n\t *          the second Quaternion\r\n\t */\r\n\tpublic static final Quaternion lnDif(Quaternion a, Quaternion b) {\r\n\t\tQuaternion dif = a.inverse();\r\n\t\tdif.multiply(b);\r\n\r\n\t\tdif.normalize();\r\n\t\treturn dif.log();\r\n\t}\r\n\r\n\t/**\r\n\t * Returns a tangent Quaternion for {@code center}, defined by {@code before}\r\n\t * and {@code after} quaternions.\r\n\t * \r\n\t * @param before\r\n\t *          the first Quaternion\r\n\t * @param center\r\n\t *          the second Quaternion\r\n\t * @param after\r\n\t *          the third Quaternion\r\n\t */\r\n\tpublic static final Quaternion squadTangent(Quaternion before,\r\n\t\t\tQuaternion center, Quaternion after) {\r\n\t\tQuaternion l1 = Quaternion.lnDif(center, before);\r\n\t\tQuaternion l2 = Quaternion.lnDif(center, after);\r\n\t\tQuaternion e = new Quaternion();\r\n\r\n\t\te.x = -0.25f * (l1.x + l2.x);\r\n\t\te.y = -0.25f * (l1.y + l2.y);\r\n\t\te.z = -0.25f * (l1.z + l2.z);\r\n\t\te.w = -0.25f * (l1.w + l2.w);\r\n\r\n\t\treturn Quaternion.multiply(center, e.exp());\r\n\t}\r\n\r\n\t/**\r\n\t * Utility function that returns the squared norm of the Quaternion.\r\n\t */\r\n\tpublic static float squaredNorm(Quaternion q) {\r\n\t\treturn (q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w);\r\n\t}\r\n}"
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/SMA.pde",
    "content": "\r\nimport java.util.LinkedList;\r\nimport java.util.Queue;\r\npublic class MovingAverage {\r\n    private final Queue<Float> window = new LinkedList<Float>();\r\n    private final int period;\r\n    private float sum;\r\n \r\n    public MovingAverage(int period) {\r\n        assert period > 0 : \"Period must be a positive integer\";\r\n        this.period = period;\r\n    }\r\n \r\n    public void newNum(float num) {\r\n        sum += num;\r\n        window.add(num);\r\n        if (window.size() > period) {\r\n            sum -= window.remove();\r\n        }\r\n    }\r\n \r\n    public float getAvg() {\r\n        if (window.isEmpty()) return 0; // technically the average is undefined\r\n        return sum / window.size();\r\n    }\r\n}\r\n\r\npublic class EMA {\r\n    private float alpha;\r\n    private float oldValue;\r\n    public EMA(float alpha) {\r\n        this.alpha = alpha;\r\n    }\r\n\r\n    public float getAvg(float value){\r\n        //if(oldValue == null) {\r\n        //    oldValue = value;\r\n        //    return value;\r\n        //}\r\n        float newValue = oldValue + alpha * (value - oldValue);\r\n        oldValue = newValue;\r\n        return newValue;\r\n    }\r\n}"
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/StopWatch.pde",
    "content": "// This class based on code found here: http://www.goldb.org/stopwatchjava.html\r\n// http://forum.processing.org/one/topic/timer-in-processing.html\r\n\r\nclass StopWatchTimer {\r\n  int startTime = 0, stopTime = 0;\r\n  boolean running = false;  \r\n   \r\n    void start() {\r\n        startTime = millis();\r\n        running = true;\r\n    }\r\n    void stop() {\r\n        stopTime = millis();\r\n        running = false;\r\n    }\r\n    int getElapsedTime() {\r\n        int elapsed;\r\n        if (running) {\r\n             elapsed = (millis() - startTime);\r\n        }\r\n        else {\r\n            elapsed = (stopTime - startTime);\r\n        }\r\n        return elapsed;\r\n    }\r\n    int second() {\r\n      return (getElapsedTime() / 1000) % 60;\r\n    }\r\n    int minute() {\r\n      return (getElapsedTime() / (1000*60)) % 60;\r\n    }\r\n    int hour() {\r\n      return (getElapsedTime() / (1000*60*60)) % 24;\r\n    }\r\n}"
  },
  {
    "path": "extras/FreeIMU_cube_UDP_ES32/Unused.pde",
    "content": "/*\r\n//****************************************************************************\r\n//The purpose of the calibration routine is to obtain the value of the \r\n//reference threshold. It consists on a 1024 samples average in no-movement \r\n//condition.\r\n//*****************************************************************************\r\n\r\nvoid Calibrate() \r\n{\r\n  int count1;\r\n  count1 = 0;\r\n  do {  \r\n  //ADC_GetAllAxis();\r\n  gravity_compensate();\r\n  sstatex = sstatex + Sample_X;             // Accumulate Samples\r\n  sstatey = sstatey + Sample_Y;\r\n  println(Sample_X);\r\n  count1++;\r\n  } while(count1 != 0x0400);                   // 1024 times\r\n\r\n  sstatex=sstatex / 1024;                      // division between l024\r\n  sstatey=sstatey / 1024;\r\n  \r\n}\r\n\r\n*/\r\n\r\n/* \r\n//**************************************************************************/\r\n//**************************************************************************/ \r\n//This function obtains magnitude and direction \r\n//In   this   particular   protocol   direction   and   magnitude   are   sent   in   separate   variables. \r\n//Management can be done in many other different ways.\r\n//**************************************************************************/  \r\n\r\n/* void data_transfer() \r\n{\r\n \r\n float positionXbkp; \r\n float positionYbkp;\r\n int delay;\r\n float [] posx_seg = new float [4];\r\n float [] posy_seg = new float [4]; \r\n \r\n \r\n  if (positionX[1] >= 0) {                          //This line compares the sign of the X direction data \r\n    direction = (direction | 0x10);                 //if its positive the most significant byte \r\n    posx_seg[0]= positionX[1] & 0x000000FF;         //is set to 1 else it is set to 8\r\n    posx_seg[1]= (positionX[1]>>8) & 0x000000FF;    //the data is also managed in the \r\n                                                    // subsequent lines in order to \r\n    posx_seg[2]= (positionX[1]>>16) & 0x000000FF;   // be sent. The 32 bit variable must be \r\n    posx_seg[3]= (positionX[1]>>24) & 0x000000FF;   // split into 4 different 8 bit \r\n                                                    // variables in order to be sent via   \r\n                                                    // the 8 bit SCI frame\r\n\r\n      }\r\n\r\n    else {direction = (direction | 0x80);\r\n             positionXbkp = positionX[1]-1;\r\n             positionXbkp = positionXbkp & 0xFFFFFFFF;\r\n             posx_seg[0] = positionXbkp & 0x000000FF;\r\n             posx_seg[1] = (positionXbkp >> 8) & 0x000000FF;\r\n             posx_seg[2] = (positionXbkp >> 16) & 0x000000FF;\r\n             posx_seg[3] = (positionXbkp >> 24) & 0x000000FF;\r\n             } \r\n  \r\n  if (positionY[1] >=0 ) {                            //  Same management than in the previous case \r\n             direction= (direction | 0x08);        // but with the Y data.\r\n             posy_seg[0]= positionY[1] & 0x000000FF;\r\n             posy_seg[1]= (positionY[1]>>8) & 0x000000FF;\r\n             posy_seg[2]= (positionY[1]>>16) & 0x000000FF;\r\n             posy_seg[3]= (positionY[1]>>24) & 0x000000FF;\r\n             }\r\n  \r\n  else {direction= (direction | 0x01);\r\n             positionYbkp = positionY[1]-1;\r\n             positionYbkp = positionYbkp & 0xFFFFFFFF;\r\n             posy_seg[0] = positionYbkp & 0x000000FF;\r\n             posy_seg[1] = (positionYbkp >> 8) & 0x000000FF;\r\n             posy_seg[2] = (positionYbkp >> 16) & 0x000000FF;\r\n             posy_seg[3] = (positionYbkp >> 24) & 0x000000FF;\r\n             } \r\n\r\n  delay =0x0100;\r\n  \r\n  Sensor_Data[0] = 0x03;\r\n  Sensor_Data[1] = direction;\r\n  Sensor_Data[2] = posx_seg[3];\r\n  Sensor_Data[3] = posy_seg[3];\r\n  Sensor_Data[4] = 0x01;\r\n  Sensor_Data[7] = 0x01;\r\n  Sensor_Data[6] = 9999;\r\n  \r\n  //while (--delay);                                   \r\n  \r\n  //SCITxMsg(Sensor_Data);         //  Data transferring function   \r\n  //while (SCIC2 & 0x08);\r\n\r\n} */\r\n\r\n\r\n//************************************************************************\r\n//************************************************************************\r\n//This function returns data format to its original state. When obtaining \r\n//the magnitude and  direction of the position, an inverse two's complement \r\n//is made. This function makes the two's complement in order to return the \r\n//data to it original state.\r\n//It is important to notice that the sensibility adjustment is greatly \r\n//impacted here, the amount of \"ones\" inserted in the mask must be equivalent\r\n//to the \"ones\" lost in the shifting made in the previous function upon the \r\n//sensibility modification. \r\n//************************************************************************\r\n\r\n/* void data_reintegration() \r\n{\r\n  if (direction >= 10) \r\n   {\r\n    positionX[1]= positionX[1] | 0xFFFFC000; }   // l8 \"ones\" inserted. Same size as the \r\n                  //amount of shifts \r\n    \r\n    direction = direction & 0x01;\r\n    if (direction == 1)   \r\n      {  positionY[1]= positionY[1] | 0xFFFFC000;  }\r\n } */"
  }
]