Sensor Calibration

Adafruit tutorial - calibrating sensors

Zero Offset

Zero Offset (also called zero-point error, or just offset) is the amount of deviation in output or reading from the exact value at the lowest point of the measurement range.   Zero Drift is the same deviation, but measured over a much longer time.  

Below is an example of acceleration data for the X direction before and after zero offset removal.   The 0.6 m/s^2 variability (+/- 0.3 m/s^2) of each signal is the zero noise.  

Zero Noise

Zero Noise is the the variability in the measured sensor output present with no external stimulus.   It should NOT include zero offset.   Calculate the range (max - min) and the standard deviation of the zero noise and use those values to compare sensors.  

 

Magnetometer Calibration

If you are using the magnetometer to detect compass direction, then it is essential to calibrate the sensor.   See magnetiometer calibration for details on how to calibrate a magnetometer.  

 

Zero Offset + Zero Noise + Magnetometer Calibration

 /*
  Accelerometer / Gyroscope / Magnetometer Calibration

  Zero Offset (also called zero-point error, or just offset) is the 
  amount of deviation in output or reading from the exact value at 
  the lowest point of the measurement range.   Zero Drift is the same 
  deviation, but measured over a much longer time.  

  Zero Noise is the the variability in the measured sensor output
  present with no external stimulus is present.  

  This sketch will measure and then calculate the zero offset to be
  added to future measurements.  It will also measure the range and
  the standard deviation of the zero noise (with the zero offset
  removed). 

  It will also calibrate a magnetometer, compensating for hard iron and
  soft iron distortion, and optionally magnetic declination.
  
  The only external libraries required
  are those for the sensor(s). 
  
*/

/////////////////////////////////////////////////////////////////////////
// Adafruit LSM6DS (ISM330DHCX) + LIS3MDL

//Install these libraries:
//  Adafruit_Sensor
//  Adafruit LSM6DS   LSM6DSOX, ISM330DHCX, LSM6DSO32, LSM6DS33   
//                    https://github.com/adafruit/Adafruit_LSM6DS
//  Adafruit LIS3MDL  https://github.com/adafruit/Adafruit_LIS3MDL

#include <Adafruit_LIS3MDL.h>   // magnetometer 
Adafruit_LIS3MDL magn;
//For LIS3MDL: Examples -> Adafruit LIS3MDL -> lis3mdl_demo

// LSM6DS library for accel, gyro, temp
// See library folder 'Adafruit_LSM6DS' for include options, and https://learn.adafruit.com/lsm6dsox-and-ism330dhc-6-dof-imu?view=all
#include <Adafruit_ISM330DHCX.h>
Adafruit_ISM330DHCX imu;
//For ISM330DHCX: File -> Examples -> Adafruit LSM6DS -> adafruit_ism330dhcx_test

#include <Wire.h>
#include <Adafruit_Sensor.h>

#define MAGNETOMETER_EXISTS true

unsigned long samples = 0;

float degPerSec(float radPerSec) {
  // Ref: 0.006 rad/s = 0.35 deg/s
  float f = radPerSec * 180.0 / M_PI;
  return f;
}

struct xyzReading {
  float x;
  float y;
  float z;
};

// acceleration in m/s^2
xyzReading accelMax = {0.0, 0.0, 0.0};
xyzReading accelMin = {0.0, 0.0, 0.0};
xyzReading accelAvg = {0.0, 0.0, 0.0};
xyzReading accelOffset = {0.0, 0.0, 0.0};
xyzReading accelZero = {0.0, 0.0, 9.80665};

// angular velocity in rad/s
xyzReading gyroMax = {0.0, 0.0, 0.0};
xyzReading gyroMin = {0.0, 0.0, 0.0};
xyzReading gyroAvg = {0.0, 0.0, 0.0};
xyzReading gyroOffset = {0.0, 0.0, 0.0};
xyzReading gyroZero = {0.0, 0.0, 0.0};


void getZeroOffset() {
  // Measure the zero offset of the sensor and determine the correction
  // factor that should be applied to future measurements. 
  // NOTE: This must be performed when the sensor is operating at it's
  //       minimum measurement range. 
  //       Temperature variation can account for the majority of a sensor's offset variation. 
  //
  // Methodology:
  //  Sample the sensor for 5 seconds (or longer if at least 30 samples
  //  are not measured) and determine the average value.  
  //  The expected zero value is accelZero. 
  //  The correction factor is the difference of the accelZero value and 
  //  the average value.  Add this correction factor to all future
  //  measurements in order to correct for zero offset. 
  //  See: D:\Documents\portable_apps\Arduino\sketchbook\sensors\sensor calibration & zero offset.xlsx

  Serial.println("\ngetZeroOffset() - calculate the zero offset");
  Serial.println("You have 9 seconds to make sure the sensor is in a settled condition...");
  delay(9000);
  Serial.println("Calibrating the accelerometer and gyroscope...");

  // Take continuous readings for 5 seconds, getting the average.
  unsigned long sampleDuration = 5000;  // milliseconds
  unsigned long sampleTime = millis();
  samples = 0;
  while (millis() - sampleTime < sampleDuration) {
    sensors_event_t a;  // m/s^2
    sensors_event_t g;  // rad/s
    sensors_event_t t;  // °C
    imu.getEvent(&a, &g, &t);
    //sensors_event_t a, m, g;
    //gyro.getEvent(&g);
    //accelmagn.getEvent(&a, &m);
    
    samples++;
    accelAvg.x += a.acceleration.x;
    accelAvg.y += a.acceleration.y;
    accelAvg.z += a.acceleration.z;
    gyroAvg.x += g.gyro.x;
    gyroAvg.y += g.gyro.y;
    gyroAvg.z += g.gyro.z;
  }
  if (samples < 30) {
    Serial.print("ERROR: Only "); 
    Serial.print(samples);
    Serial.println(" samples measured.  Increase sample time!");
    return;
  }
  
  // Calculate the average
  accelAvg.x = accelAvg.x / samples;
  accelAvg.y = accelAvg.y / samples;
  accelAvg.z = accelAvg.z / samples;
  gyroAvg.x = gyroAvg.x / samples;
  gyroAvg.y = gyroAvg.y / samples;
  gyroAvg.z = gyroAvg.z / samples;
  Serial.print("Avg for ");
  Serial.print(samples);
  Serial.println(" samples:");
  Serial.print("  X: "); Serial.print(accelAvg.x); Serial.println(" m/s^2");
  Serial.print("  Y: "); Serial.print(accelAvg.y); Serial.println(" m/s^2");
  Serial.print("  Z: "); Serial.print(accelAvg.z); Serial.println(" m/s^2\n");
  Serial.print("  X: "); Serial.print(gyroAvg.x); Serial.println(" deg/s");
  Serial.print("  Y: "); Serial.print(gyroAvg.y); Serial.println(" deg/s");
  Serial.print("  Z: "); Serial.print(gyroAvg.z); Serial.println(" deg/s\n");
  
  // Calculate the offset correction factor
  accelOffset.x = accelZero.x - accelAvg.x;
  accelOffset.y = accelZero.y - accelAvg.y;
  accelOffset.z = accelZero.z - accelAvg.z;
  gyroOffset.x = gyroZero.x - gyroAvg.x;
  gyroOffset.y = gyroZero.y - gyroAvg.y;
  gyroOffset.z = gyroZero.z - gyroAvg.z;
  Serial.println("Offset correction factor:");
  Serial.print("  X: "); Serial.print(accelOffset.x,6); Serial.println(" m/s^2");
  Serial.print("  Y: "); Serial.print(accelOffset.y,6); Serial.println(" m/s^2");
  Serial.print("  Z: "); Serial.print(accelOffset.z,6); Serial.println(" m/s^2\n");
  Serial.print("  X: "); Serial.print(gyroOffset.x,6); Serial.println(" deg/s");
  Serial.print("  Y: "); Serial.print(gyroOffset.y,6); Serial.println(" deg/s");
  Serial.print("  Z: "); Serial.print(gyroOffset.z,6); Serial.println(" deg/s\n");
  // IMPORTANT:  Save and use the accelOffset values
  Serial.println("ADD THESE TO ACCEL/GYRO SKETCH:");
  Serial.print("xyzReading accelOffset = {");
  Serial.print(accelOffset.x, 6);
  Serial.print(", ");
  Serial.print(accelOffset.y, 6);
  Serial.print(", ");
  Serial.print(accelOffset.z, 6);
  Serial.println("};");
  Serial.print("xyzReading gyroOffset = {");
  Serial.print(gyroOffset.x, 6);
  Serial.print(", ");
  Serial.print(gyroOffset.y, 6);
  Serial.print(", ");
  Serial.print(gyroOffset.z, 6);
  Serial.println("};\n");

  // Demonstrate what the correction would do...
  // (this is how accelOffset should be applied to future measurements).
  // AND get the min/max values.
  sampleTime = millis();
  samples = 0;
  accelAvg.x = 0; accelAvg.y = 0; accelAvg.z = 0;
  accelMin.x = 0; accelMin.y = 0; accelMin.z = 0;
  accelMax.x = 0; accelMax.y = 0; accelMax.z = 0;  
  gyroAvg.x = 0; gyroAvg.y = 0; gyroAvg.z = 0;
  gyroMin.x = 0; gyroMin.y = 0; gyroMin.z = 0;
  gyroMax.x = 0; gyroMax.y = 0; gyroMax.z = 0;
  
  while (millis() - sampleTime < sampleDuration) {
    sensors_event_t a;  // m/s^2
    sensors_event_t g;  // rad/s
    sensors_event_t t;  // °C
    imu.getEvent(&a, &g, &t);
    
    accelAvg.x += a.acceleration.x + accelOffset.x;
    accelAvg.y += a.acceleration.y + accelOffset.y;
    accelAvg.z += a.acceleration.z + accelOffset.z;

    gyroAvg.x += g.gyro.x + gyroOffset.x;
    gyroAvg.y += g.gyro.y + gyroOffset.y;
    gyroAvg.z += g.gyro.z + gyroOffset.z;

    if (samples == 0) {
      accelMax.x = a.acceleration.x + accelOffset.x;
      accelMax.y = a.acceleration.y + accelOffset.y;
      accelMax.z = a.acceleration.z + accelOffset.z;

      gyroMax.x = g.gyro.x + gyroOffset.x;
      gyroMax.y = g.gyro.y + gyroOffset.y;
      gyroMax.z = g.gyro.z + gyroOffset.z;
    } else {
      //if (fabs(a.acceleration.x + accelOffset.x) > fabs(accelMax.x)) accelMax.x = a.acceleration.x + accelOffset.x;
      accelMax.x = max(a.acceleration.x + accelOffset.x, accelMax.x);
      accelMax.y = max(a.acceleration.y + accelOffset.x, accelMax.y);
      accelMax.z = max(a.acceleration.z + accelOffset.z, accelMax.z);
      
      gyroMax.x = max(g.gyro.x + gyroOffset.x, gyroMax.x);
      gyroMax.y = max(g.gyro.y + gyroOffset.y, gyroMax.y);
      gyroMax.z = max(g.gyro.z + gyroOffset.z, gyroMax.z);
    }
    
    if (samples == 0) {
      accelMin.x = a.acceleration.x + accelOffset.x;
      accelMin.y = a.acceleration.y + accelOffset.y;
      accelMin.z = a.acceleration.z + accelOffset.z;
      
      gyroMin.x = g.gyro.x + gyroOffset.x;
      gyroMin.y = g.gyro.y + gyroOffset.y;
      gyroMin.z = g.gyro.z + gyroOffset.z;
    } else {
      accelMin.x = min(a.acceleration.x + accelOffset.x, accelMin.x);
      accelMin.y = min(a.acceleration.y + accelOffset.y, accelMin.y);
      accelMin.z = min(a.acceleration.z + accelOffset.z, accelMin.z);

      gyroMin.x = min(g.gyro.x + gyroOffset.x, gyroMin.x);
      gyroMin.y = min(g.gyro.y + gyroOffset.y, gyroMin.y);
      gyroMin.z = min(g.gyro.z + gyroOffset.z, gyroMin.z);
    }
     samples++;   
  } // while()
  // Calculate the average
  accelAvg.x = accelAvg.x / samples;
  accelAvg.y = accelAvg.y / samples;
  accelAvg.z = accelAvg.z / samples;

  gyroAvg.x = gyroAvg.x / samples;
  gyroAvg.y = gyroAvg.y / samples;
  gyroAvg.z = gyroAvg.z / samples;
  
  Serial.print("Avg for ");
  Serial.print(samples);
  Serial.println(" samples (using zero offset correction):");
  Serial.print("  X: "); Serial.print(accelAvg.x); Serial.println(" m/s^2");
  Serial.print("  Y: "); Serial.print(accelAvg.y); Serial.println(" m/s^2");
  Serial.print("  Z: "); Serial.print(accelAvg.z); Serial.println(" m/s^2");
  Serial.print("  X: "); Serial.print(gyroAvg.x); Serial.println(" deg/s");
  Serial.print("  Y: "); Serial.print(gyroAvg.y); Serial.println(" deg/s");
  Serial.print("  Z: "); Serial.print(gyroAvg.z); Serial.println(" deg/s\n");

  Serial.println("Range (max - min) using zero offset correction:");
  Serial.print("  X: "); Serial.print(accelMax.x-accelMin.x,6); Serial.println(" m/s^2");
  Serial.print("  Y: "); Serial.print(accelMax.y-accelMin.y,6); Serial.println(" m/s^2");
  Serial.print("  Z: "); Serial.print(accelMax.z-accelMin.z,6); Serial.println(" m/s^2");
  Serial.print("  X: "); Serial.print(gyroMax.x-gyroMin.x,6); Serial.println(" deg/s");
  Serial.print("  Y: "); Serial.print(gyroMax.y-gyroMin.y,6); Serial.println(" deg/s");
  Serial.print("  Z: "); Serial.print(gyroMax.z-gyroMin.z,6); Serial.println(" deg/s\n");

  accelMin.x = 0; accelMin.y = 0; accelMin.z = 0;
  accelMax.x = 0; accelMax.y = 0; accelMax.z = 0;
  gyroMin.x = 0; gyroMin.y = 0; gyroMin.z = 0;
  gyroMax.x = 0; gyroMax.y = 0; gyroMax.z = 0;
  samples = 0;

  // Calculate the standard deviation of the measured values
  // with the offset applied, and using the prior calculated
  // average for the accelerometer and gyroscope.
  xyzReading accelStdDev = {0.0, 0.0, 0.0};
  xyzReading gyroStdDev = {0.0, 0.0, 0.0};
  sampleTime = millis();
  
  //Serial.println("\nRaw Acceleration Data Corrected With Zero Offset Applied:");
  while (millis() - sampleTime < sampleDuration) {
    sensors_event_t a;  // m/s^2
    sensors_event_t g;  // rad/s
    sensors_event_t t;  // °C
    imu.getEvent(&a, &g, &t);

    //Serial.print(samples); Serial.print(";"); Serial.print(a.acceleration.x + accelOffset.x,6); Serial.print(";"); Serial.print(a.acceleration.y + accelOffset.y,6); Serial.print(";"); Serial.println(a.acceleration.z + accelOffset.z,6);
    accelStdDev.x += sq(a.acceleration.x + accelOffset.x - accelAvg.x);
    accelStdDev.y += sq(a.acceleration.y + accelOffset.y - accelAvg.y);
    accelStdDev.z += sq(a.acceleration.z + accelOffset.z - accelAvg.z);

    gyroStdDev.x += sq(g.gyro.x + gyroOffset.x - gyroAvg.x);
    gyroStdDev.y += sq(g.gyro.y + gyroOffset.y - gyroAvg.y);
    gyroStdDev.z += sq(g.gyro.z + gyroOffset.z - gyroAvg.z);    
    samples++;
  }
  //Serial.println("");
  // population standard deviation
  accelStdDev.x = sqrt(accelStdDev.x / samples);
  accelStdDev.y = sqrt(accelStdDev.y / samples);
  accelStdDev.z = sqrt(accelStdDev.z / samples);

  gyroStdDev.x = sqrt(gyroStdDev.x / samples);
  gyroStdDev.y = sqrt(gyroStdDev.y / samples);
  gyroStdDev.z = sqrt(gyroStdDev.z / samples);

  Serial.print("StdDev for ");
  Serial.print(samples);
  Serial.println(" samples (using zero offset correction):");
  Serial.print("  X: "); Serial.print(accelStdDev.x,6); Serial.println(" m/s^2");
  Serial.print("  Y: "); Serial.print(accelStdDev.y,6); Serial.println(" m/s^2");
  Serial.print("  Z: "); Serial.print(accelStdDev.z,6); Serial.println(" m/s^2");
  Serial.print("  X: "); Serial.print(gyroStdDev.x,6); Serial.println(" deg/s");
  Serial.print("  Y: "); Serial.print(gyroStdDev.y,6); Serial.println(" deg/s");
  Serial.print("  Z: "); Serial.print(gyroStdDev.z,6); Serial.println(" deg/s\n");

  accelAvg.x = 0; accelAvg.y = 0; accelAvg.z = 0;
  accelMin.x = 0; accelMin.y = 0; accelMin.z = 0;
  accelMax.x = 0; accelMax.y = 0; accelMax.z = 0;
  gyroAvg.x = 0; gyroAvg.y = 0; gyroAvg.z = 0;
  gyroMin.x = 0; gyroMin.y = 0; gyroMin.z = 0;
  gyroMax.x = 0; gyroMax.y = 0; gyroMax.z = 0;
  samples = 0;
  
} // getZeroOffset()


#if MAGNETOMETER_EXISTS
  // corrections below in micro-Tesla (uT)
  xyzReading magHardIronOffset = {0.0, 0.0, 0.0};
  xyzReading magSoftIronScale = {0.0, 0.0, 0.0};

  // If you have the magnetic declination to correct for the difference between
  // magnetic north and geographic north, then revise the next line to reflect
  // that offset. 
  float mag_decl = 0.0;
  
  float magnetometerHeading(float raw_mag_x, float raw_mag_y) {
    // raw_mag_x and raw_mag_y in micro-Tesla (uT)
    // If mag_decl = 0.0, then returns the magnetic heading in degrees,
    // otherwise the geographic heading in degrees. 
    
      // Calculate angle for heading, assuming the magnetometer is parallel
      // to the ground and +Y points toward the magnetic heading of interest.
      //float heading = -1 * (atan2(raw_mag_x, raw_mag_y) * 180.0) / M_PI;
      
      // Calculate angle for heading, assuming the magnetometer is parallel
      // to the ground and +X points toward the magnetic heading of interest.
      float heading = -1 * (atan2(raw_mag_y, raw_mag_x) * 180.0) / M_PI;
  
      // Apply any magnetic declination to get the geographic heading
      heading += mag_decl;
      
      // Convert heading to 0..360 degrees
      if (heading < 0) {
        heading += 360.0;
      }
  
      return heading;
      
  } // magnetometerHeading()  

  void magnetometerCalibration() {
    // This calibration will create a set of correction factors 
    // (magHardIronOffset.x, magHardIronOffset.y, magHardIronOffset.z
    //  and to magSoftIronScale.x, magSoftIronScale.y, magSoftIronScale.z)
    // to compensate for hard and soft iron distortion. &nbsp;
    // Note that in order to use a magnetometer for as geographic heading,
    // it is necessary to compensate for magnetic declination. 
    // This function uses the 'scale biases' method to calculate the soft
    // iron correction factor.  (see: https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration)
  
    // Take continuous readings for 15 seconds while the magnetometer
    // is oriented in a 3D figure eight pattern
    unsigned long sampleDuration = 25000;  // milliseconds
    unsigned long sampleTime = millis();
    xyzReading magnMax = {0.0, 0.0, 0.0};
    xyzReading magnMin = {0.0, 0.0, 0.0};
    samples = 0;
    Serial.println("\nYou have 9 seconds to get the magnetometer ready to be oriented in a 3D figure eight pattern...");
    delay(9000);
    Serial.println("Acquiring magnetometer calibration data...");
    while (millis() - sampleTime < sampleDuration) {
      
      sensors_event_t m; 
      magn.getEvent(&m);  // uTesla
      //sensors_event_t a, m, g;
      //gyro.getEvent(&g);
      //accelmagn.getEvent(&a, &m);
      
      // Get the max/min values..
      if (samples == 0) {
        magnMax.x = m.magnetic.x;
        magnMax.y = m.magnetic.y;
        magnMax.z = m.magnetic.z;
        magnMin.x = m.magnetic.x;
        magnMin.y = m.magnetic.y;
        magnMin.z = m.magnetic.z;
      } else {
        magnMax.x = max(magnMax.x, m.magnetic.x);
        magnMax.y = max(magnMax.y, m.magnetic.y);
        magnMax.z = max(magnMax.z, m.magnetic.z);
        magnMin.x = min(magnMin.x, m.magnetic.x);
        magnMin.y = min(magnMin.y, m.magnetic.y);
        magnMin.z = min(magnMin.z, m.magnetic.z);
      }
      samples++;
    }
    if (samples < 30) {
      Serial.print("ERROR: Only "); 
      Serial.print(samples);
      Serial.println(" samples measured.  Increase sample time!");
      return;
    }
    Serial.println(""); 
    Serial.print("Magnetometer calibration results for ");
    Serial.print(samples);
    Serial.println(" samples:");  
  
    // Calculate the hard iron offset
    magHardIronOffset.x = (magnMax.x + magnMin.x) / 2.0;
    magHardIronOffset.y = (magnMax.y + magnMin.y) / 2.0;
    magHardIronOffset.z = (magnMax.z + magnMin.z) / 2.0;
    Serial.print("Hard iron offset:");  
    Serial.print("\tX: "); Serial.print(magHardIronOffset.x);
    Serial.print(" \tY: "); Serial.print(magHardIronOffset.y); 
    Serial.print(" \tZ: "); Serial.print(magHardIronOffset.z); 
    Serial.println(" uTesla");
  
    // Calculate the soft iron offset
    xyzReading magAvgDelta = {0.0, 0.0, 0.0};
    magAvgDelta.x = (magnMax.x - magnMin.x) / 2.0;
    magAvgDelta.y = (magnMax.y - magnMin.y) / 2.0;
    magAvgDelta.z = (magnMax.z - magnMin.z) / 2.0;
    float avg_delta = (magAvgDelta.x + magAvgDelta.y + magAvgDelta.z) / 3.0;
    magSoftIronScale.x = avg_delta / magAvgDelta.x;
    magSoftIronScale.y = avg_delta / magAvgDelta.y;
    magSoftIronScale.z = avg_delta / magAvgDelta.z;
    Serial.print("Soft iron scale:");  
    Serial.print("\tX: "); Serial.print(magSoftIronScale.x);
    Serial.print(" \tY: "); Serial.print(magSoftIronScale.y); 
    Serial.print(" \tZ: "); Serial.print(magSoftIronScale.z); 
    Serial.println(" uTesla\n");
  
    Serial.print("xyzReading magHardIronOffset = {");
    Serial.print(magHardIronOffset.x, 4);
    Serial.print(", ");
    Serial.print(magHardIronOffset.y, 4);
    Serial.print(", ");
    Serial.print(magHardIronOffset.z, 4);
    Serial.println("};");
    Serial.print("xyzReading magSoftIronScale = {");
    Serial.print(magSoftIronScale.x, 4);
    Serial.print(", ");
    Serial.print(magSoftIronScale.y, 4);
    Serial.print(", ");
    Serial.print(magSoftIronScale.z, 4);
    Serial.println("};\n");
    
  } // magnetometerCalibration()
  
#endif


void sampleData() {
  // Continuously sample data as fast as loop() will allow.
  // Record the data to accelMax, accelMin, accelAvg 
  // (using the offset correction), and also update 'samples'.
  // See also TimerA().
  
  sensors_event_t a;  // m/s^2
  sensors_event_t g;  // rad/s
  sensors_event_t t;  // °C
  sensors_event_t m;  // micro-Tesla (uT)
  imu.getEvent(&a, &g, &t);
  // sensors_event_t accel, mag, gyro, temp, dist, lux, press, RH, I, V
  //sensors_event_t a, m, g;
  //gyro.getEvent(&g);
  //accelmagn.getEvent(&a, &m);

  accelAvg.x += a.acceleration.x + accelOffset.x;
  accelAvg.y += a.acceleration.y + accelOffset.y;
  accelAvg.z += a.acceleration.z + accelOffset.z;

  gyroAvg.x += g.gyro.x + gyroOffset.x;
  gyroAvg.y += g.gyro.y + gyroOffset.y;
  gyroAvg.z += g.gyro.z + gyroOffset.z;

  if (samples == 0) {
    accelMax.x = a.acceleration.x + accelOffset.x;
    accelMax.y = a.acceleration.y + accelOffset.y;
    accelMax.z = a.acceleration.z + accelOffset.z;
    gyroMax.x = g.gyro.x + gyroOffset.x;
    gyroMax.y = g.gyro.y + gyroOffset.y;
    gyroMax.z = g.gyro.z + gyroOffset.z;
    
    accelMin.x = a.acceleration.x + accelOffset.x;
    accelMin.y = a.acceleration.y + accelOffset.y;
    accelMin.z = a.acceleration.z + accelOffset.z;
    gyroMin.x = g.gyro.x + gyroOffset.x;
    gyroMin.y = g.gyro.y + gyroOffset.y;
    gyroMin.z = g.gyro.z + gyroOffset.z;
  } else {
    accelMax.x = max(a.acceleration.x + accelOffset.x, accelMax.x);
    accelMax.y = max(a.acceleration.y + accelOffset.x, accelMax.y);
    accelMax.z = max(a.acceleration.z + accelOffset.z, accelMax.z);
    
    gyroMax.x = max(g.gyro.x + gyroOffset.x, gyroMax.x);
    gyroMax.y = max(g.gyro.y + gyroOffset.y, gyroMax.y);
    gyroMax.z = max(g.gyro.z + gyroOffset.z, gyroMax.z);

    accelMin.x = min(a.acceleration.x + accelOffset.x, accelMin.x);
    accelMin.y = min(a.acceleration.y + accelOffset.y, accelMin.y);
    accelMin.z = min(a.acceleration.z + accelOffset.z, accelMin.z);

    gyroMin.x = min(g.gyro.x + gyroOffset.x, gyroMin.x);
    gyroMin.y = min(g.gyro.y + gyroOffset.y, gyroMin.y);
    gyroMin.z = min(g.gyro.z + gyroOffset.z, gyroMin.z);
  }
  samples++;

} //  sampleData()

//////////////////////////////////////////////////////////////////////////////
// TimerA
// 1000000 us = 1000 ms = 1 sec = 1 Hz
const unsigned long timerAinterval = 1000;  
unsigned long timerAlap = millis();  // timer

void timerA() {
  //  Timer A
  
  if (timerAlap > millis())  timerAlap = millis();
  if (millis() - timerAlap > timerAinterval) { 
  
    accelAvg.x = accelAvg.x / samples;
    accelAvg.y = accelAvg.y / samples;
    accelAvg.z = accelAvg.z / samples;

    gyroAvg.x = gyroAvg.x / samples;
    gyroAvg.y = gyroAvg.y / samples;
    gyroAvg.z = gyroAvg.z / samples;

    sensors_event_t a;  // m/s^2
    sensors_event_t g;  // rad/s
    sensors_event_t t;  // °C
    imu.getEvent(&a, &g, &t);
    #if MAGNETOMETER_EXISTS
    sensors_event_t m;  // micro-Tesla (uT)
    magn.getEvent(&m);  
    #endif

    Serial.print("\t"); Serial.print(accelAvg.x); Serial.print("\t"); Serial.print(accelAvg.y); Serial.print("\t"); Serial.print(accelAvg.z); Serial.print("\t"); 
    Serial.print("\t"); Serial.print(gyroAvg.x*180/M_PI); Serial.print("\t"); Serial.print(gyroAvg.y*180/M_PI); Serial.print("\t"); Serial.print(gyroAvg.z*180/M_PI); Serial.print("\t\t"); 

    // Determine if the sensor is at rest...
    if ((accelAvg.z > accelZero.z*0.95 && accelAvg.z < accelZero.z*1.05) || fabs(accelMax.x)-fabs(accelMin.x) < 1.0 || fabs(accelMax.y)-fabs(accelMin.y) < 1.0 ) {
      // Sensor is not in motion and is in the standard orientation (+z = 9.81 m/s^2)    
      // atan2() returns a value in the range of -PI to +PI radians (+/- 180 deg)
      double roll = atan2(-1*accelAvg.x ,(sqrt((pow(accelAvg.y,2)) + (pow(accelAvg.z,2)))));    // radians
      double pitch = atan2 (accelAvg.y ,(sqrt((pow(accelAvg.x,2)) + (pow(accelAvg.z,2)))));     // radians
      // Degrees +/- 0..180
      Serial.print(roll * 180.0 / M_PI, 1); Serial.print("\t");
      Serial.print(pitch * 180.0 / M_PI, 1); Serial.print("\t");
      #if MAGNETOMETER_EXISTS
        float Yh = ((m.magnetic.y - magHardIronOffset.y) * magSoftIronScale.y * cos(roll)) - ((m.magnetic.z - magHardIronOffset.z) * magSoftIronScale.z * sin(roll));
        float Xh = ((m.magnetic.x - magHardIronOffset.x) * magSoftIronScale.x * cos(pitch))+((m.magnetic.y - magHardIronOffset.y) * magSoftIronScale.y * sin(roll)*sin(pitch)) + ((m.magnetic.z - magHardIronOffset.z) * magSoftIronScale.z * cos(roll) * sin(pitch));
        double yaw = atan2(Yh, Xh);
        Serial.print(yaw * 180.0 / M_PI, 1); Serial.print("\t\t");
        //float magnetometerHeading(float raw_mag_x, raw_mag_y)   returned degrees are 0..360
        Serial.print(magnetometerHeading((m.magnetic.x - magHardIronOffset.x) * magSoftIronScale.x, (m.magnetic.y - magHardIronOffset.y) * magSoftIronScale.y)); 
      #endif
    } else {
      Serial.print("\tSensor in motion");
    } // sensor motion detection   
    Serial.println("");

    accelAvg.x = 0; accelAvg.y = 0; accelAvg.z = 0;
    accelMin.x = 0; accelMin.y = 0; accelMin.z = 0;
    accelMax.x = 0; accelMax.y = 0; accelMax.z = 0;
    gyroAvg.x = 0; gyroAvg.y = 0; gyroAvg.z = 0;
    gyroMin.x = 0; gyroMin.y = 0; gyroMin.z = 0;
    gyroMax.x = 0; gyroMax.y = 0; gyroMax.z = 0;
    samples = 0;
    
    timerAlap = millis(); // reset the timer
  }
} // timerA()

/////////////////////////////////////////////////////////////////////////



void setup() {

  Serial.begin(115200);
  while (!Serial) {
    digitalWrite(13, HIGH);
    delay(1);
    digitalWrite(13, LOW);
  }
  Serial.println("\nSerial ready\n");

  //////////////////////////////////////////////////////////////////////////////
  // Adafruit LSM6DS (ISM330DHCX) + LIS3MDL  (modify based on the sensor being used)
  // Initialize  
  delay(1000);
  digitalWrite(13, HIGH);
  if (!imu.begin_I2C()) {
    Serial.println("LSM6DS (ISM330DHCX) initialization failure");
    while (1) delay(10);
  }
  digitalWrite(13, LOW);
  Serial.println("LSM6DS (ISM330DHCX) initialized");

  imu.setAccelRange(LSM6DS_ACCEL_RANGE_16_G);
  Serial.print("Accelerometer range set to: ");
  switch (imu.getAccelRange()) {
  case LSM6DS_ACCEL_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case LSM6DS_ACCEL_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case LSM6DS_ACCEL_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case LSM6DS_ACCEL_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }

  // imu.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
  Serial.print("Gyro range set to: ");
  switch (imu.getGyroRange()) {
  case LSM6DS_GYRO_RANGE_125_DPS:
    Serial.println("125 degrees/s");
    break;
  case LSM6DS_GYRO_RANGE_250_DPS:
    Serial.println("250 degrees/s");
    break;
  case LSM6DS_GYRO_RANGE_500_DPS:
    Serial.println("500 degrees/s");
    break;
  case LSM6DS_GYRO_RANGE_1000_DPS:
    Serial.println("1000 degrees/s");
    break;
  case LSM6DS_GYRO_RANGE_2000_DPS:
    Serial.println("2000 degrees/s");
    break;
  case ISM330DHCX_GYRO_RANGE_4000_DPS:
    Serial.println("4000 degrees/s");
    break;
  }

  imu.setAccelDataRate(LSM6DS_RATE_104_HZ);
  Serial.print("Accelerometer data rate set to: ");
  switch (imu.getAccelDataRate()) {
  case LSM6DS_RATE_SHUTDOWN:
    Serial.println("0 Hz");
    break;
  case LSM6DS_RATE_12_5_HZ:
    Serial.println("12.5 Hz");
    break;
  case LSM6DS_RATE_26_HZ:
    Serial.println("26 Hz");
    break;
  case LSM6DS_RATE_52_HZ:
    Serial.println("52 Hz");
    break;
  case LSM6DS_RATE_104_HZ:
    Serial.println("104 Hz");
    break;
  case LSM6DS_RATE_208_HZ:
    Serial.println("208 Hz");
    break;
  case LSM6DS_RATE_416_HZ:
    Serial.println("416 Hz");
    break;
  case LSM6DS_RATE_833_HZ:
    Serial.println("833 Hz");
    break;
  case LSM6DS_RATE_1_66K_HZ:
    Serial.println("1.66 KHz");
    break;
  case LSM6DS_RATE_3_33K_HZ:
    Serial.println("3.33 KHz");
    break;
  case LSM6DS_RATE_6_66K_HZ:
    Serial.println("6.66 KHz");
    break;
  }

  imu.setGyroDataRate(LSM6DS_RATE_104_HZ);
  Serial.print("Gyro data rate set to: ");
  switch (imu.getGyroDataRate()) {
  case LSM6DS_RATE_SHUTDOWN:
    Serial.println("0 Hz");
    break;
  case LSM6DS_RATE_12_5_HZ:
    Serial.println("12.5 Hz");
    break;
  case LSM6DS_RATE_26_HZ:
    Serial.println("26 Hz");
    break;
  case LSM6DS_RATE_52_HZ:
    Serial.println("52 Hz");
    break;
  case LSM6DS_RATE_104_HZ:
    Serial.println("104 Hz");
    break;
  case LSM6DS_RATE_208_HZ:
    Serial.println("208 Hz");
    break;
  case LSM6DS_RATE_416_HZ:
    Serial.println("416 Hz");
    break;
  case LSM6DS_RATE_833_HZ:
    Serial.println("833 Hz");
    break;
  case LSM6DS_RATE_1_66K_HZ:
    Serial.println("1.66 KHz");
    break;
  case LSM6DS_RATE_3_33K_HZ:
    Serial.println("3.33 KHz");
    break;
  case LSM6DS_RATE_6_66K_HZ:
    Serial.println("6.66 KHz");
    break;
  }

  imu.configInt1(false, false, true); // accelerometer DRDY on INT1
  imu.configInt2(false, true, false); // gyro DRDY on INT2

  // Adafruit LIS3MDL magnetometer 
  digitalWrite(13, HIGH);
  if (!magn.begin_I2C()) {
    Serial.println("LIS3MDL initialization failure");
    while (1) delay(10);
  }
  digitalWrite(13, LOW);
  Serial.println("LIS3MDL initialized");
  magn.setPerformanceMode(LIS3MDL_MEDIUMMODE);
  Serial.print("Performance mode set to: ");
  switch (magn.getPerformanceMode()) {
    case LIS3MDL_LOWPOWERMODE: Serial.println("Low"); break;
    case LIS3MDL_MEDIUMMODE: Serial.println("Medium"); break;
    case LIS3MDL_HIGHMODE: Serial.println("High"); break;
    case LIS3MDL_ULTRAHIGHMODE: Serial.println("Ultra-High"); break;
  }

  magn.setOperationMode(LIS3MDL_CONTINUOUSMODE);
  Serial.print("Operation mode set to: ");
  // Single shot mode will complete conversion and go into power down
  switch (magn.getOperationMode()) {
    case LIS3MDL_CONTINUOUSMODE: Serial.println("Continuous"); break;
    case LIS3MDL_SINGLEMODE: Serial.println("Single mode"); break;
    case LIS3MDL_POWERDOWNMODE: Serial.println("Power-down"); break;
  }

  magn.setDataRate(LIS3MDL_DATARATE_155_HZ);
  // You can check the datarate by looking at the frequency of the DRDY pin
  Serial.print("Data rate set to: ");
  switch (magn.getDataRate()) {
    case LIS3MDL_DATARATE_0_625_HZ: Serial.println("0.625 Hz"); break;
    case LIS3MDL_DATARATE_1_25_HZ: Serial.println("1.25 Hz"); break;
    case LIS3MDL_DATARATE_2_5_HZ: Serial.println("2.5 Hz"); break;
    case LIS3MDL_DATARATE_5_HZ: Serial.println("5 Hz"); break;
    case LIS3MDL_DATARATE_10_HZ: Serial.println("10 Hz"); break;
    case LIS3MDL_DATARATE_20_HZ: Serial.println("20 Hz"); break;
    case LIS3MDL_DATARATE_40_HZ: Serial.println("40 Hz"); break;
    case LIS3MDL_DATARATE_80_HZ: Serial.println("80 Hz"); break;
    case LIS3MDL_DATARATE_155_HZ: Serial.println("155 Hz"); break;
    case LIS3MDL_DATARATE_300_HZ: Serial.println("300 Hz"); break;
    case LIS3MDL_DATARATE_560_HZ: Serial.println("560 Hz"); break;
    case LIS3MDL_DATARATE_1000_HZ: Serial.println("1000 Hz"); break;
  }
  
  magn.setRange(LIS3MDL_RANGE_4_GAUSS);
  Serial.print("Range set to: ");
  switch (magn.getRange()) {
    case LIS3MDL_RANGE_4_GAUSS: Serial.println("+-4 gauss"); break;
    case LIS3MDL_RANGE_8_GAUSS: Serial.println("+-8 gauss"); break;
    case LIS3MDL_RANGE_12_GAUSS: Serial.println("+-12 gauss"); break;
    case LIS3MDL_RANGE_16_GAUSS: Serial.println("+-16 gauss"); break;
  }

  magn.setIntThreshold(500);
  magn.configInterrupt(false, false, true, // enable z axis
                          true, // polarity
                          false, // don't latch
                          true); // enabled!

  
  //////////////////////////////////////////////////////////////////////////////

  #if MAGNETOMETER_EXISTS
    magnetometerCalibration();
  #endif

  // Calculate the zero offset for the sensor, and measure the
  // standard deviation of the zero noise. 
  getZeroOffset();

  Serial.println("\nSetup complete\n");
  timerAlap = millis(); // reset the timer
  samples = 0;

  #if MAGNETOMETER_EXISTS
    Serial.println("m/s^2\tX\tY\tZ\tdeg/s\tX\tY\tZ\t\tRoll\tPitch\tYaw deg\t\tMagnetic deg");
  #else
    Serial.println("m/s^2\tX\tY\tZ\tdeg/s\tX\tY\tZ\t\tRoll\tPitch");
  #endif

} // setup()


void loop() {

  sampleData();
  
  timerA();
  
} // loop()

 

Using The Zero Offset + Zero Noise + Magnetometer Calibration Results

 /*
  Accelerometer / Gyroscope / Magnetometer 

  Before using this sketch, you must calibrate the sensors using:
    sensor_calib_zero-offset_imu_accel_gyro_mag.ino

  and then update the following with the results:

    xyzReading accelOffset = {0.0, 0.0, 0.0};
    xyzReading gyroOffset = {0.0, 0.0, 0.0};
    xyzReading magHardIronOffset = {0.0, 0.0, 0.0};
    xyzReading magSoftIronScale = {0.0, 0.0, 0.0};
    
   
*/

/////////////////////////////////////////////////////////////////////////
// Adafruit LSM6DS (ISM330DHCX) + LIS3MDL

//Install these libraries:
//  Adafruit_Sensor
//  Adafruit LSM6DS   LSM6DSOX, ISM330DHCX, LSM6DSO32, LSM6DS33   
//                    https://github.com/adafruit/Adafruit_LSM6DS
//  Adafruit LIS3MDL  https://github.com/adafruit/Adafruit_LIS3MDL

#include <Adafruit_LIS3MDL.h>   // magnetometer 
Adafruit_LIS3MDL magn;
//For LIS3MDL: Examples -> Adafruit LIS3MDL -> lis3mdl_demo

// LSM6DS library for accel, gyro, temp
// See library folder 'Adafruit_LSM6DS' for include options, and https://learn.adafruit.com/lsm6dsox-and-ism330dhc-6-dof-imu?view=all
#include <Adafruit_ISM330DHCX.h>
Adafruit_ISM330DHCX imu;
//For ISM330DHCX: File -> Examples -> Adafruit LSM6DS -> adafruit_ism330dhcx_test

#include <Wire.h>
#include <Adafruit_Sensor.h>

#define MAGNETOMETER_EXISTS true


unsigned long samples = 0;

float degPerSec(float radPerSec) {
  // Ref: 0.006 rad/s = 0.35 deg/s
  //float f = radPerSec * 180.0 / PI;
  float f = radPerSec * 180.0 / M_PI;
  return f;
}

struct xyzReading {
  float x;
  float y;
  float z;
};

// acceleration in m/s^2
xyzReading accelMax = {0.0, 0.0, 0.0};
xyzReading accelMin = {0.0, 0.0, 0.0};
xyzReading accelAvg = {0.0, 0.0, 0.0};
// Update accelOffset by running: 'sensor_calib_zero-offset_imu_accel_gyro_mag.ino'
xyzReading accelOffset = {0.076220, 0.194032, 0.038702};
xyzReading accelZero = {0.0, 0.0, 9.80665};

// angular velocity in rad/s
xyzReading gyroMax = {0.0, 0.0, 0.0};
xyzReading gyroMin = {0.0, 0.0, 0.0};
xyzReading gyroAvg = {0.0, 0.0, 0.0};
// Update gyroOffset by running: 'sensor_calib_zero-offset_imu_accel_gyro_mag.ino'
xyzReading gyroOffset = {-0.005534, 0.008544, 0.008752};
xyzReading gyroZero = {0.0, 0.0, 0.0};

// Update magHardIronOffset & magSoftIronScale by running: 'sensor_calib_zero-offset_imu_accel_gyro_mag.ino'
xyzReading magHardIronOffset = {0.0, 0.0, 0.0};
xyzReading magSoftIronScale = {0.0, 0.0, 0.0};

// If you have the magnetic declination to correct for the difference between
// magnetic north and geographic north, then revise the next line to reflect
// that offset. 
float mag_decl = 0.0;

float magnetometerHeading(float raw_mag_x, float raw_mag_y) {
  // If mag_decl = 0.0, then returns the magnetic heading in degrees,
  // otherwise the geographic heading in degrees. 
  
    // Calculate angle for heading, assuming the magnetometer is parallel
    // to the ground and +Y points toward the magnetic heading of interest.
    //float heading = -1 * (atan2(raw_mag_x, raw_mag_y) * 180.0) / M_PI;
    
    // Calculate angle for heading, assuming the magnetometer is parallel
    // to the ground and +X points toward the magnetic heading of interest.
    float heading = -1 * (atan2(raw_mag_y, raw_mag_x) * 180.0) / M_PI;

    // Apply any magnetic declination to get the geographic heading
    heading += mag_decl;
    
    // Convert heading to 0..360 degrees
    if (heading < 0) {
      heading += 360.0;
    }

    return heading;
    
} // magnetometerHeading()


void sampleData() {
  // Continuously sample data as fast as loop() will allow.
  // Record the data to accelMax, accelMin, accelAvg 
  // (using the offset correction), and also update 'samples'.
  // See also TimerA().
  
  // Below varies by library. 
  sensors_event_t a;  // m/s^2
  sensors_event_t g;  // rad/s
  sensors_event_t t;  // °C
  imu.getEvent(&a, &g, &t);
  // sensors_event_t accel, mag, gyro, temp, dist, lux, press, RH, I, V
  //sensors_event_t a, m, g;
  //gyro.getEvent(&g);
  //accelmagn.getEvent(&a, &m);

  accelAvg.x += a.acceleration.x + accelOffset.x;
  accelAvg.y += a.acceleration.y + accelOffset.y;
  accelAvg.z += a.acceleration.z + accelOffset.z;

  gyroAvg.x += g.gyro.x + gyroOffset.x;
  gyroAvg.y += g.gyro.y + gyroOffset.y;
  gyroAvg.z += g.gyro.z + gyroOffset.z;

  if (samples == 0) {
    accelMax.x = a.acceleration.x + accelOffset.x;
    accelMax.y = a.acceleration.y + accelOffset.y;
    accelMax.z = a.acceleration.z + accelOffset.z;
    gyroMax.x = g.gyro.x + gyroOffset.x;
    gyroMax.y = g.gyro.y + gyroOffset.y;
    gyroMax.z = g.gyro.z + gyroOffset.z;
    
    accelMin.x = a.acceleration.x + accelOffset.x;
    accelMin.y = a.acceleration.y + accelOffset.y;
    accelMin.z = a.acceleration.z + accelOffset.z;
    gyroMin.x = g.gyro.x + gyroOffset.x;
    gyroMin.y = g.gyro.y + gyroOffset.y;
    gyroMin.z = g.gyro.z + gyroOffset.z;
  } else {
    accelMax.x = max(a.acceleration.x + accelOffset.x, accelMax.x);
    accelMax.y = max(a.acceleration.y + accelOffset.x, accelMax.y);
    accelMax.z = max(a.acceleration.z + accelOffset.z, accelMax.z);
    
    gyroMax.x = max(g.gyro.x + gyroOffset.x, gyroMax.x);
    gyroMax.y = max(g.gyro.y + gyroOffset.y, gyroMax.y);
    gyroMax.z = max(g.gyro.z + gyroOffset.z, gyroMax.z);

    accelMin.x = min(a.acceleration.x + accelOffset.x, accelMin.x);
    accelMin.y = min(a.acceleration.y + accelOffset.y, accelMin.y);
    accelMin.z = min(a.acceleration.z + accelOffset.z, accelMin.z);

    gyroMin.x = min(g.gyro.x + gyroOffset.x, gyroMin.x);
    gyroMin.y = min(g.gyro.y + gyroOffset.y, gyroMin.y);
    gyroMin.z = min(g.gyro.z + gyroOffset.z, gyroMin.z);
  }
  samples++;

} //  sampleData()

//////////////////////////////////////////////////////////////////////////////
// TimerA
// 1000000 us = 1000 ms = 1 sec = 1 Hz
const unsigned long timerAinterval = 1000;  
unsigned long timerAlap = millis();  // timer

void timerA() {
  //  Timer A
  
  if (timerAlap > millis())  timerAlap = millis();
  if (millis() - timerAlap > timerAinterval) { 
  
    accelAvg.x = accelAvg.x / samples;
    accelAvg.y = accelAvg.y / samples;
    accelAvg.z = accelAvg.z / samples;

    gyroAvg.x = gyroAvg.x / samples;
    gyroAvg.y = gyroAvg.y / samples;
    gyroAvg.z = gyroAvg.z / samples;

    // Below varies by library. 
    sensors_event_t a;  // m/s^2
    sensors_event_t g;  // rad/s
    sensors_event_t t;  // °C
    sensors_event_t m;  // micro-Tesla (uT)
    imu.getEvent(&a, &g, &t);
    #if MAGNETOMETER_EXISTS
      magn.getEvent(&m);  
    #endif
    // sensors_event_t accel, mag, gyro, temp, cm, lux, press, RH, I, V
    //sensors_event_t a, m, g;
    //gyro.getEvent(&g);
    //accelmagn.getEvent(&a, &m);

    Serial.print("\t"); Serial.print(accelAvg.x); Serial.print("\t"); Serial.print(accelAvg.y); Serial.print("\t"); Serial.print(accelAvg.z); Serial.print("\t"); 
    Serial.print("\t"); Serial.print(gyroAvg.x*180/M_PI); Serial.print("\t"); Serial.print(gyroAvg.y*180/M_PI); Serial.print("\t"); Serial.print(gyroAvg.z*180/M_PI); Serial.print("\t\t"); 

    // Determine if the sensor is at rest...
    if ((accelAvg.z > accelZero.z*0.95 && accelAvg.z < accelZero.z*1.05) || fabs(accelMax.x)-fabs(accelMin.x) < 1.0 || fabs(accelMax.y)-fabs(accelMin.y) < 1.0 ) {
      // Sensor is not in motion and is in the standard orientation (+z = 9.81 m/s^2)    
      // atan2() returns a value in the range of -PI to +PI radians (+/- 180 deg)
      double roll = atan2(-1*accelAvg.x ,(sqrt((pow(accelAvg.y,2)) + (pow(accelAvg.z,2)))));    // radians
      double pitch = atan2 (accelAvg.y ,(sqrt((pow(accelAvg.x,2)) + (pow(accelAvg.z,2)))));     // radians
      // Degrees +/- 0..180
      Serial.print(roll * 180.0 / M_PI, 1); Serial.print("\t");
      Serial.print(pitch * 180.0 / M_PI, 1); Serial.print("\t");
      #if MAGNETOMETER_EXISTS
        float Yh = ((m.magnetic.y - magHardIronOffset.y) * magSoftIronScale.y * cos(roll)) - ((m.magnetic.z - magHardIronOffset.z) * magSoftIronScale.z * sin(roll));
        float Xh = ((m.magnetic.x - magHardIronOffset.x) * magSoftIronScale.x * cos(pitch))+((m.magnetic.y - magHardIronOffset.y) * magSoftIronScale.y * sin(roll)*sin(pitch)) + ((m.magnetic.z - magHardIronOffset.z) * magSoftIronScale.z * cos(roll) * sin(pitch));
        double yaw = atan2(Yh, Xh);
        Serial.print(yaw * 180.0 / M_PI, 1); Serial.print("\t\t");
        //float magnetometerHeading(float raw_mag_x, raw_mag_y)   returned degrees are 0..360
        Serial.print(magnetometerHeading((m.magnetic.x - magHardIronOffset.x) * magSoftIronScale.x, (m.magnetic.y - magHardIronOffset.y) * magSoftIronScale.y)); 
      #endif
    } else {
      Serial.print("\tSensor in motion");
    } // sensor motion detection   
    Serial.println("");

    accelAvg.x = 0; accelAvg.y = 0; accelAvg.z = 0;
    accelMin.x = 0; accelMin.y = 0; accelMin.z = 0;
    accelMax.x = 0; accelMax.y = 0; accelMax.z = 0;
    gyroAvg.x = 0; gyroAvg.y = 0; gyroAvg.z = 0;
    gyroMin.x = 0; gyroMin.y = 0; gyroMin.z = 0;
    gyroMax.x = 0; gyroMax.y = 0; gyroMax.z = 0;
    samples = 0;
    
    timerAlap = millis(); // reset the timer
  }
} // timerA()

/////////////////////////////////////////////////////////////////////////



void setup() {

  Serial.begin(115200);
  while (!Serial) {
    digitalWrite(13, HIGH);
    delay(1);
    digitalWrite(13, LOW);
  }
  Serial.println("\nSerial ready\n");

  //////////////////////////////////////////////////////////////////////////////
  // Adafruit LSM6DS (ISM330DHCX) + LIS3MDL  (modify based on the sensor being used)
  // Initialize  
  delay(1000);
  digitalWrite(13, HIGH);
  if (!imu.begin_I2C()) {
    Serial.println("LSM6DS (ISM330DHCX) initialization failure");
    while (1) delay(10);
  }
  digitalWrite(13, LOW);
  Serial.println("LSM6DS (ISM330DHCX) initialized");

  imu.setAccelRange(LSM6DS_ACCEL_RANGE_16_G);
  Serial.print("Accelerometer range set to: ");
  switch (imu.getAccelRange()) {
  case LSM6DS_ACCEL_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case LSM6DS_ACCEL_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case LSM6DS_ACCEL_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case LSM6DS_ACCEL_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }

  // imu.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS);
  Serial.print("Gyro range set to: ");
  switch (imu.getGyroRange()) {
  case LSM6DS_GYRO_RANGE_125_DPS:
    Serial.println("125 degrees/s");
    break;
  case LSM6DS_GYRO_RANGE_250_DPS:
    Serial.println("250 degrees/s");
    break;
  case LSM6DS_GYRO_RANGE_500_DPS:
    Serial.println("500 degrees/s");
    break;
  case LSM6DS_GYRO_RANGE_1000_DPS:
    Serial.println("1000 degrees/s");
    break;
  case LSM6DS_GYRO_RANGE_2000_DPS:
    Serial.println("2000 degrees/s");
    break;
  case ISM330DHCX_GYRO_RANGE_4000_DPS:
    Serial.println("4000 degrees/s");
    break;
  }

  imu.setAccelDataRate(LSM6DS_RATE_104_HZ);
  Serial.print("Accelerometer data rate set to: ");
  switch (imu.getAccelDataRate()) {
  case LSM6DS_RATE_SHUTDOWN:
    Serial.println("0 Hz");
    break;
  case LSM6DS_RATE_12_5_HZ:
    Serial.println("12.5 Hz");
    break;
  case LSM6DS_RATE_26_HZ:
    Serial.println("26 Hz");
    break;
  case LSM6DS_RATE_52_HZ:
    Serial.println("52 Hz");
    break;
  case LSM6DS_RATE_104_HZ:
    Serial.println("104 Hz");
    break;
  case LSM6DS_RATE_208_HZ:
    Serial.println("208 Hz");
    break;
  case LSM6DS_RATE_416_HZ:
    Serial.println("416 Hz");
    break;
  case LSM6DS_RATE_833_HZ:
    Serial.println("833 Hz");
    break;
  case LSM6DS_RATE_1_66K_HZ:
    Serial.println("1.66 KHz");
    break;
  case LSM6DS_RATE_3_33K_HZ:
    Serial.println("3.33 KHz");
    break;
  case LSM6DS_RATE_6_66K_HZ:
    Serial.println("6.66 KHz");
    break;
  }

  imu.setGyroDataRate(LSM6DS_RATE_104_HZ);
  Serial.print("Gyro data rate set to: ");
  switch (imu.getGyroDataRate()) {
  case LSM6DS_RATE_SHUTDOWN:
    Serial.println("0 Hz");
    break;
  case LSM6DS_RATE_12_5_HZ:
    Serial.println("12.5 Hz");
    break;
  case LSM6DS_RATE_26_HZ:
    Serial.println("26 Hz");
    break;
  case LSM6DS_RATE_52_HZ:
    Serial.println("52 Hz");
    break;
  case LSM6DS_RATE_104_HZ:
    Serial.println("104 Hz");
    break;
  case LSM6DS_RATE_208_HZ:
    Serial.println("208 Hz");
    break;
  case LSM6DS_RATE_416_HZ:
    Serial.println("416 Hz");
    break;
  case LSM6DS_RATE_833_HZ:
    Serial.println("833 Hz");
    break;
  case LSM6DS_RATE_1_66K_HZ:
    Serial.println("1.66 KHz");
    break;
  case LSM6DS_RATE_3_33K_HZ:
    Serial.println("3.33 KHz");
    break;
  case LSM6DS_RATE_6_66K_HZ:
    Serial.println("6.66 KHz");
    break;
  }

  imu.configInt1(false, false, true); // accelerometer DRDY on INT1
  imu.configInt2(false, true, false); // gyro DRDY on INT2

  // Adafruit LIS3MDL magnetometer 
  digitalWrite(13, HIGH);
  if (!magn.begin_I2C()) {
    Serial.println("LIS3MDL initialization failure");
    while (1) delay(10);
  }
  digitalWrite(13, LOW);
  Serial.println("LIS3MDL initialized");
  magn.setPerformanceMode(LIS3MDL_MEDIUMMODE);
  Serial.print("Performance mode set to: ");
  switch (magn.getPerformanceMode()) {
    case LIS3MDL_LOWPOWERMODE: Serial.println("Low"); break;
    case LIS3MDL_MEDIUMMODE: Serial.println("Medium"); break;
    case LIS3MDL_HIGHMODE: Serial.println("High"); break;
    case LIS3MDL_ULTRAHIGHMODE: Serial.println("Ultra-High"); break;
  }

  magn.setOperationMode(LIS3MDL_CONTINUOUSMODE);
  Serial.print("Operation mode set to: ");
  // Single shot mode will complete conversion and go into power down
  switch (magn.getOperationMode()) {
    case LIS3MDL_CONTINUOUSMODE: Serial.println("Continuous"); break;
    case LIS3MDL_SINGLEMODE: Serial.println("Single mode"); break;
    case LIS3MDL_POWERDOWNMODE: Serial.println("Power-down"); break;
  }

  magn.setDataRate(LIS3MDL_DATARATE_155_HZ);
  // You can check the datarate by looking at the frequency of the DRDY pin
  Serial.print("Data rate set to: ");
  switch (magn.getDataRate()) {
    case LIS3MDL_DATARATE_0_625_HZ: Serial.println("0.625 Hz"); break;
    case LIS3MDL_DATARATE_1_25_HZ: Serial.println("1.25 Hz"); break;
    case LIS3MDL_DATARATE_2_5_HZ: Serial.println("2.5 Hz"); break;
    case LIS3MDL_DATARATE_5_HZ: Serial.println("5 Hz"); break;
    case LIS3MDL_DATARATE_10_HZ: Serial.println("10 Hz"); break;
    case LIS3MDL_DATARATE_20_HZ: Serial.println("20 Hz"); break;
    case LIS3MDL_DATARATE_40_HZ: Serial.println("40 Hz"); break;
    case LIS3MDL_DATARATE_80_HZ: Serial.println("80 Hz"); break;
    case LIS3MDL_DATARATE_155_HZ: Serial.println("155 Hz"); break;
    case LIS3MDL_DATARATE_300_HZ: Serial.println("300 Hz"); break;
    case LIS3MDL_DATARATE_560_HZ: Serial.println("560 Hz"); break;
    case LIS3MDL_DATARATE_1000_HZ: Serial.println("1000 Hz"); break;
  }
  
  magn.setRange(LIS3MDL_RANGE_4_GAUSS);
  Serial.print("Range set to: ");
  switch (magn.getRange()) {
    case LIS3MDL_RANGE_4_GAUSS: Serial.println("+-4 gauss"); break;
    case LIS3MDL_RANGE_8_GAUSS: Serial.println("+-8 gauss"); break;
    case LIS3MDL_RANGE_12_GAUSS: Serial.println("+-12 gauss"); break;
    case LIS3MDL_RANGE_16_GAUSS: Serial.println("+-16 gauss"); break;
  }

  magn.setIntThreshold(500);
  magn.configInterrupt(false, false, true, // enable z axis
                          true, // polarity
                          false, // don't latch
                          true); // enabled!

  //////////////////////////////////////////////////////////////////////////////

  Serial.println("\nSetup complete\n");
  timerAlap = millis(); // reset the timer
  samples = 0;

  Serial.println("MAX accel/gyro values:|");
  #if MAGNETOMETER_EXISTS
    Serial.println("m/s^2\tX\tY\tZ\tdeg/s\tX\tY\tZ\t\tRoll\tPitch\tYaw deg\t\tMagnetic deg");
  #else
    Serial.println("m/s^2\tX\tY\tZ\tdeg/s\tX\tY\tZ\t\tRoll\tPitch");
  #endif
  
} // setup()


void loop() {

  sampleData();
  
  timerA();
  
} // loop()

 

 

---

Do you need help developing or customizing a IoT product for your needs?   Send me an email requesting a free one hour phone / web share consultation.  

 

The information presented on this website is for the author's use only.   Use of this information by anyone other than the author is offered as guidelines and non-professional advice only.   No liability is assumed by the author or this web site.  

Sensors

Components