**NEW** MPU6050_calibration Faster and Exact using PID (PI actually)

Hello, Everyone, I created this to get a better starting point with my MPU6050 and MPU9250. It works GREAT!!!
I would like your testing feedback and advice to improve this.
Why I changed from Luis's work.
Luis Ródenas' original sketch would calculate offsets after averaging them together which gets close... But we can't quite get there.
I found that Luis' code could be improved. I am using direct feedback through the MPU's firmware to test settings and improve upon the last test. Each pass tightens up the results. after a little over 400 readings which is less than 3.5 seconds, we are almost there. +-2 (Example: offset of 100 +-2) from the best we can get.
This program uses a simple PI (PID Proportional, Integral Dirivitive) to make changes after each sample I repeat this process with smaller PID multipliers allowing for quick a fine-tuning of the results with almost identical results each time I try.

Everything quickly switches to floating point numbers during calculations for more accurate feedback through the PI loop.

Simplified MPU6050 PI loop (Clipped from my attached program):

while(x){  
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  az = az - 16384; //remove Gravity   
    a[0] = ax;
    a[1] = ay;
    a[2] = az;
    g[0] = gx;
    g[1] = gy;
    g[2] = gz;
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Other Cheching stuff here
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 /* PI of PID Calculations */
    for (int i = 0; i < 3; i++) { // PI Calculations
      Error[i] = Setpoint - g[i];
      PTerm[i] = kPGyro *  (float)GyroTuneDirection * Error[i];
      ITerm[i] += Error[i] *  0.002 * (kIGyro * (float)GyroTuneDirection); // Integral term 1000 Calculations a second = 0.001
      gyro_offset[i] = (PTerm[i] + ITerm[i]); //Compute PID Output

      Error[i + 3] = Setpoint - a[i] ;
      PTerm[i + 3] = kPAccel * (float)AccelTuneDirection * Error[i + 3];
      ITerm[i + 3] += Error[i + 3] * 0.002 * (kIAccel * (float)AccelTuneDirection); // Integral term 1000 Calculations a second = 0.001
      accel_offset[i] = (PTerm[i + 3] + ITerm[i + 3] ); //Compute PID Output
    }
    // Set and Store Values
    setOffset(round(accel_offset[0] / 8), round(accel_offset[1] / 8), round(accel_offset[2] / 8), round(gyro_offset[0] / 4), round(gyro_offset[1] / 4), round(gyro_offset[2]) / 4);
}

void setOffset(int ax_, int ay_, int az_, int gx_, int gy_, int gz_) {
  mpu.setXAccelOffset(ax_);
  mpu.setYAccelOffset(ay_);
  mpu.setZAccelOffset(az_);

  mpu.setXGyroOffset(gx_);
  mpu.setYGyroOffset(gy_);
  mpu.setZGyroOffset(gz_);
}

Some insights:
I discovered that you could simply start off with this and get a 100% improvement in your readings
How to make a Quick and dirty tuning that will get you closer than factory settings:

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Get a single Set of readings while on a flat surface
  az = az - 16384; //remove Gravity
  mpu.setXAccelOffset(-(ax >> 3)); // Invert and Divide by 8 (1000B (8) >> 3 = 0001B (1))
  mpu.setYAccelOffset(-(ay >> 3));
  mpu.setZAccelOffset(-(az >> 3));
  mpu.setXGyroOffset(-(gx >> 2)); // Invert and Divide by 4 (1000B (8) >> 2 = 0010B (2))
  mpu.setYGyroOffset(-(gy >> 2));
  mpu.setZGyroOffset(-(gz >> 2));

I discovered that you can change the offsets at any time and then get a reading with shifted/updated results!
With this in mind, we can use direct feedback to calibrate our MPU6050 and tune it to perfection!!!
Now that I'm close, I run a PI (of the PID) routine with a stepping routine (Smaller Corrections) that softens the PI loop slowing changes to help land the offsets quickly.
Please Enjoy.
Z

UPDATE!
Added MPU6050_calibration_Z_V2_3.ino This updated version provides a super quick calibration and averages 10 calibrations together to get in my opinion an excelent set of offsets.

MPU6050_calibration_Z_V2.1.ino (13.1 KB)

MPU6050_calibration_Z_V2_3.ino (16.1 KB)

Thanks for sharing!!!

zoomx:
Thanks for sharing!!!

You're Welcome.
Let me know if you have any suggestions

Works well and converges much faster than the IMU_Zero calibration example in the MPU6050 library (by many multiples of time). The calibration values resulting from the two methods are similar +/-1.

Thanks for sharing. You should consider submitting this to be included in the MPU6050 library.

marco_c:
Works well and converges much faster than the IMU_Zero calibration example in the MPU6050 library (by many multiples of time). The calibration values resulting from the two methods are similar +/-1.

Thanks for sharing. You should consider submitting this to be included in the MPU6050 library.

Thank you, How do I submit this to be included in the MPU6050 library?

I've discovered an additional improvement in speed and some additional cleanup:

  int x;
  mpu.initialize();
  mpu.setFullScaleGyroRange(0); // 2000
  x = mpu.testConnection();
  PRINTLN(x ? F("connection successful") : F("connection failed"));

Change the above lines to below:

  byte ID,x;
  ID = mpu.getDeviceID();// Who am I
  x = ID == ((byte) MPUAddress) >> 1; // the returned id is equal to the address missing the changable first bit 1.
  PRINT(x ? F("connection successful on: 0x") : F("connection failedon: 0x"));
  PRINTLN(MPUAddress,HEX);
  // almost the same as mpu.initialize();
  mpu.setClockSource(MPU6050_CLOCK_PLL_XGYRO);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setSleepEnabled(false);

MPU6050_calibration_Z_V2.2.ino (13.4 KB)

How do I submit this to be included in the MPU6050 library?

You need to contact the authors on github and ask if they are interested. The address is usually included in the library.info file as the "maintainer" tag if it is there, otherwise you need to find them using a search engine.

zhomeslice:
Thank you, How do I submit this to be included in the MPU6050 library?

Perhaps fork the library, add your changes, and then submit a merge request.

Power_Broker:
Perhaps fork the library, add your changes, and then submit a merge request.

I just updated the code and improved the performance dramatically. I chose a different ending method that allowed me to nail down the final results faster.

MPU6050_calibration_Z_V2_3.ino (16.1 KB)

Hello Everyone
I've one-upped my MPU6050 Calibration code and created a complete Simple_MPU6050 library with Auto Calibration and overflow prevention. I've also upgraded and preconfigured the DMP Version 6.1.2 to work with no tweaking. I have seen V 6.1.2 elsewhere but it required a ton of memory to implement.
This with all the bells and whistles and it works out of the box takes up less memory.

Please Give it a spin:

Z

Here is an Example Sketch :

//#include "MPU_ReadMacros.h"
//#inc;ude "MPU_WriteMacros.h"
#include "Simple_MPU6050.h"
#define MPU6050_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board
#define MPU6050_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)
#define MPU6050_DEFAULT_ADDRESS     MPU6050_ADDRESS_AD0_LOW

Simple_MPU6050 mpu;
ENABLE_MPU_OVERFLOW_PROTECTION();
/*             _________________________________________________________*/
//               X Accel  Y Accel  Z Accel   X Gyro   Y Gyro   Z Gyro

#define OFFSETS      0,       0,       0,       0,       0,       0
//#define OFFSETS     1133,     -44,    1056,      50,       9,      20 // My Last offsets. you will want to use your own as these are only for my specific MPU6050
//#define CalibrationLoops 2 //(Default is 8) 1 - 2 Calibration loops are great for a quick tuneup (at 100 readings each) , 6+ Loops will completely find the correct calibration offsets! 
/*             _________________________________________________________*/

//***************************************************************************************
//******************                Print Funcitons                **********************
//***************************************************************************************

//Gyro, Accel and Quaternion 
int PrintAllValues(int16_t *gyro, int16_t *accel, int32_t *quat, uint16_t SpamDelay = 100) {
  Quaternion q;
  VectorFloat gravity;
  float ypr[3] = { 0, 0, 0 };
  float xyz[3] = { 0, 0, 0 };
  spamtimer(SpamDelay) {// non blocking delay before printing again. This skips the following code when delay time (ms) hasn't been met
    mpu.GetQuaternion(&q, quat);
    mpu.GetGravity(&gravity, &q);
    mpu.GetYawPitchRoll(ypr, &q, &gravity);
    mpu.ConvertToDegrees(ypr, xyz);
    Serial.printfloatx(F("Yaw")  , xyz[0], 9, 4, F(",   ")); //printfloatx is a Helper Macro that works with Serial.print that I created (See #define above)
    Serial.printfloatx(F("Pitch"), xyz[1], 9, 4, F(",   "));
    Serial.printfloatx(F("Roll") , xyz[2], 9, 4, F(",   "));
    Serial.printfloatx(F("ax")   , accel[0], 5, 0, F(",   "));
    Serial.printfloatx(F("ay")   , accel[1], 5, 0, F(",   "));
    Serial.printfloatx(F("az")   , accel[2], 5, 0, F(",   "));
    Serial.printfloatx(F("gx")   , gyro[0],  5, 0, F(",   "));
    Serial.printfloatx(F("gy")   , gyro[1],  5, 0, F(",   "));
    Serial.printfloatx(F("gz")   , gyro[2],  5, 0, F("\n"));
  }
}

//***************************************************************************************
//******************              Callback Funciton                **********************
//***************************************************************************************

void print_Values (int16_t *gyro, int16_t *accel, int32_t *quat, uint32_t *timestamp) {
  uint8_t Spam_Delay = 100; // Built in Blink without delay timer preventing Serial.print SPAM
  PrintAllValues(gyro, accel, quat, Spam_Delay);
}
//***************************************************************************************
//******************                Setup and Loop                 **********************
//***************************************************************************************
void setup() {
  uint8_t val;
  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately
  Serial.println("Start: ");
  mpu.SetAddress(MPU6050_ADDRESS_AD0_LOW).TestConnection(1); // Lets test for Communication
  mpu.load_DMP_Image(OFFSETS); // Does it all for you
  Serial.print(F("Setup Complete in "));
  mpu.CalibrateGyro(CalibrationLoops); // Default is 8
  mpu.CalibrateAccel(CalibrationLoops); // Default is 8
  mpu.PrintActiveOffsets();  // Gets the actual offsets being used in the MPU6050
  mpu.on_FIFO(print_Values); // Load Callback function
  Serial.print(F("full calibration Completed"));

}

void loop() {
  mpu.dmp_read_fifo();// Must be in loop
}

marco_c:
Works well and converges much faster than the IMU_Zero calibration example in the MPU6050 library (by many multiples of time). The calibration values resulting from the two methods are similar +/-1.

Thanks for sharing. You should consider submitting this to be included in the MPU6050 library.

Jeff Rowberg merged it!!! It is now part of his library! The Calibration routine is even simpler and smaller than ever!

void MPU6050::CalibrateGyro(int Loops = 6) {
  int16_t Reading, Offset;
  float Error, PTerm, ITerm[3];
  double kP = 0.3;
  double kI = 90;
  float x;
  x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
  kP *= x;
  kI *= x;
  
  PID( 0x43,  0x13,  kP, kI,  Loops);
}
/**
  @brief      Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings
*/
void MPU6050::CalibrateAccel(int Loops = 6) {
//  int16_t Reading, Offset;
//  float Error, PTerm, ITerm[3];
  double kP = 0.15;
  double kI = 8;
  float x;
  x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
  kP *= x;
  kI *= x;
  PID( 0x3B,  0x06,  kP, kI,  Loops);
}
void MPU6050::PID(uint8_t ReadAddress, uint8_t SaveAddress, float kP,float kI, uint8_t Loops) {
  int16_t Reading, Offset;
  float Error, PTerm, ITerm[3];
  float x;
  for (int i = 0; i < 3; i++) {
	I2Cdev::readWords(devAddr, SaveAddress + (i * 2), 1, &Reading); // reads 1 or more 16 bit integers (Word)
//    MPUi2cReadInt(SaveAddress + (i * 2), &Reading); // XG_OFFSET_H_READ: Load the active Gyro offset to fine tune
    ITerm[i] = Reading * 8;
  }
  for (int L = 0; L < Loops; L++) {
    for (int c = 0; c < 100; c++) {// 100 PI Calculations
      for (int i = 0; i < 3; i++) {
		I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, &Reading); // reads 1 or more 16 bit integers (Word)
//        MPUi2cReadInt(ReadAddress + (i * 2), &Reading);
        if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity
        if (abs(Reading) < 25000) {
          Error = 0 - Reading ;
          PTerm = kP * Error;
          ITerm[i] += Error * 0.002 * kI; // Integral term 1000 Calculations a second = 0.001
          Offset = round((PTerm + ITerm[i] ) / 8); //Compute PID Output
		  I2Cdev::writeWords(devAddr, SaveAddress + (i * 2), 1,  &Offset);
        //  MPUi2cWriteInt(SaveAddress + (i * 2), Offset);
        }
      }
      delay(1);
    }
    Serial.write('.');
    kP *= .95;
    kI *= .95;
    for (int i = 0; i < 3; i++){
	 Offset = round((ITerm[i]) / 8);
	 I2Cdev::writeWords(devAddr, SaveAddress + (i * 2), 1, &Offset );
	// MPUi2cWriteInt(SaveAddress + (i * 2), round((ITerm[i]) / 8)); // ITerm is more of a running total rather than a reaction.
	}
  }
  resetFIFO();
  resetDMP();
}

Added Calibration Routine, Simplified DMP initialization, and Cleaned up blocking code
#446 by ZHomeSlice was merged 14 minutes ago

Thanks

Well done!

Small Update:
Jeff has a script IMU_Zero in the mpu6050 library examples folder. It performs calibration using the old averaging method. I've added my PID Version to this script and it is now a part of the IMU Zero Sketch in addition to the 2016 averaging code.

A tidbit on how PID (PI actually) tuning works.
When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it to the integral value. Each reading narrows the error down to the desired offset. The greater the error from set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the integral math. The derivative is not used because of the noise and because the sensor is stationary. With the noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100 readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to the fact it reacts to any noise.