Tutorial MPU9250 Arduino to OLED U8g2 Library

/* MPU9250 Basic Example Code
 by: Kris Winer
 date: April 1, 2014
 Modified by Brent Wilkins July 19, 2016
 */

#include "quaternionFilters.h"
#include "MPU9250.h"
#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h>

#define SerialDebug true  // Set to true to get Serial output for debugging
#define I2Cclock 400000
#define I2Cport Wire
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0  // Use either this line or the next to select which I2C address your device is using
//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1

MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock);
U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE);

void setup() {
  Wire.begin();
  u8g2.begin();

  byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);

  if (c == 0x71) {

    u8g2.firstPage();
    do {
      u8g2.setFont(u8g2_font_6x13_tr);
      u8g2.setCursor(0, 13);
      u8g2.print("MPU9250 is Online");
    } while (u8g2.nextPage());
    delay(500);


    // Start by performing self test and reporting values
    myIMU.MPU9250SelfTest(myIMU.selfTest);

    // Calibrate gyro and accelerometers, load biases in bias registers
    myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);

    // Initialize device for active mode read of acclerometer, gyro, and temp
    myIMU.initMPU9250();

    // Read the WHO_AM_I register of the magnetometer
    byte d = myIMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);

    if (d != 0x48) {
      u8g2.firstPage();
      do {
        u8g2.setFont(u8g2_font_6x13_tr);
        u8g2.setCursor(0, 13);
        u8g2.print("Failed...");
      } while (u8g2.nextPage());
      delay(500);
    }


    // Get magnetometer calibration from AK8963 ROM
    // Initialize device for active mode read of magnetometer
    myIMU.initAK8963(myIMU.factoryMagCalibration);

    // Get sensor resolutions, only need to do this once
    myIMU.getAres();
    myIMU.getGres();
    myIMU.getMres();

  } else {
    u8g2.firstPage();
    do {
      u8g2.setFont(u8g2_font_6x13_tr);
      u8g2.setCursor(0, 13);
      u8g2.print("MPU9250 is Offline");
    } while (u8g2.nextPage());
    delay(2000);
  }
}

void loop() {
  // If intPin goes high, all data registers have new data
  // On interrupt, check if data ready interrupt
  if (myIMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
    myIMU.readAccelData(myIMU.accelCount);  // Read the x/y/z adc values

    // Now we'll calculate the accleration value into actual g's
    // This depends on scale being set
    myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;  // - myIMU.accelBias[0];
    myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;  // - myIMU.accelBias[1];
    myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;  // - myIMU.accelBias[2];

    myIMU.readGyroData(myIMU.gyroCount);  // Read the x/y/z adc values

    // Calculate the gyro value into actual degrees per second
    // This depends on scale being set
    myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
    myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
    myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;

    myIMU.readMagData(myIMU.magCount);  // Read the x/y/z adc values

    // Calculate the magnetometer values in milliGauss
    // Include factory calibration per data sheet and user environmental
    // corrections
    // Get actual magnetometer value, this depends on scale being set
    myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes
                 * myIMU.factoryMagCalibration[0]
               - myIMU.magBias[0];
    myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes
                 * myIMU.factoryMagCalibration[1]
               - myIMU.magBias[1];
    myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes
                 * myIMU.factoryMagCalibration[2]
               - myIMU.magBias[2];
  }  // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01)

  // Must be called before updating quaternions!
  myIMU.updateTime();

  //Pass gyro rate as rad/s
  MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
                         myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
                         myIMU.mx, myIMU.mz, myIMU.deltat);


  myIMU.delt_t = millis() - myIMU.count;
  if (myIMU.delt_t > 500) {

    u8g2.firstPage();
    do {
      //Set font for tittle
      u8g2.setFont(u8g2_font_6x13_tr);
      u8g2.drawStr(10, 14, "Akcel");
      u8g2.drawStr(53, 14, "Gyro");
      u8g2.drawStr(90, 14, "Magnet");

      u8g2.setCursor(0, 25);
      u8g2.print("X");
      u8g2.setCursor(0, 38);
      u8g2.print("Y");
      u8g2.setCursor(0, 50);
      u8g2.print("Z");

      //Set font for data
      u8g2.setFont(u8g2_font_5x7_tr);

      //Show Accelerometer Data
      u8g2.setCursor(10, 25);
      u8g2.print(1000 * myIMU.ax);
      u8g2.setCursor(10, 37);
      u8g2.print(1000 * myIMU.ay);
      u8g2.setCursor(10, 49);
      u8g2.print(1000 * myIMU.az);

      //Show Gyroscope Data
      u8g2.setCursor(53, 25);
      u8g2.print(myIMU.gx, 1);
      u8g2.setCursor(53, 37);
      u8g2.print(myIMU.gy, 1);
      u8g2.setCursor(53, 49);
      u8g2.print(myIMU.gz, 1);

      //Show Magnetometer Data
      u8g2.setCursor(90, 25);
      u8g2.print(myIMU.mx);
      u8g2.setCursor(90, 37);
      u8g2.print(myIMU.my);
      u8g2.setCursor(90, 49);
      u8g2.print(myIMU.mz);


      // Read the adc values
      myIMU.tempCount = myIMU.readTempData();

      // Temperature in degrees Centigrade
      myIMU.temperature = ((float)myIMU.tempCount) / 333.87 + 21.0;

      // Print temperature in degrees Centigrade
      // Show Temperature Data
      u8g2.setCursor(0, 62);
      u8g2.print("Temperature:");
      u8g2.setCursor(65, 62);
      u8g2.print(myIMU.temperature, 2);
      u8g2.setCursor(95, 62);
      u8g2.print("C");

    } while (u8g2.nextPage());

    myIMU.count = millis();
  }
}