// =====================================================
// Embedded Nerd - MPU6050 Calibration Guide
// https://embeddednerd.com
// =====================================================

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

const int NUM_SAMPLES = 2000;
const int GRAVITY_1G = 16384;

// Calibration offsets
long accelXOffset = 0;
long accelYOffset = 0;
long accelZOffset = 0;

long gyroXOffset = 0;
long gyroYOffset = 0;
long gyroZOffset = 0;

void calibrateMPU6050() {

  long accelXSum = 0;
    long accelYSum = 0;
      long accelZSum = 0;

        long gyroXSum = 0;
          long gyroYSum = 0;
            long gyroZSum = 0;

              Serial.println("Keep the MPU6050 completely still.");
                Serial.println("Starting calibration in 3 seconds...");

                  delay(3000);

                    Serial.println();
                      Serial.println("Collecting calibration samples...");

                        for (int i = 0; i < NUM_SAMPLES; i++) {

                            int16_t ax, ay, az;
                                int16_t gx, gy, gz;

                                    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

                                        accelXSum += ax;
                                            accelYSum += ay;
                                                accelZSum += az;

                                                    gyroXSum += gx;
                                                        gyroYSum += gy;
                                                            gyroZSum += gz;

                                                                delay(2);
                                                                  }

                                                                    long accelXAverage = accelXSum / NUM_SAMPLES;
                                                                      long accelYAverage = accelYSum / NUM_SAMPLES;
                                                                        long accelZAverage = accelZSum / NUM_SAMPLES;

                                                                          long gyroXAverage = gyroXSum / NUM_SAMPLES;
                                                                            long gyroYAverage = gyroYSum / NUM_SAMPLES;
                                                                              long gyroZAverage = gyroZSum / NUM_SAMPLES;

                                                                                // Accelerometer bias.
                                                                                  // X and Y should be approximately 0.
                                                                                    // Z should be approximately +1g when the sensor is level.
                                                                                      accelXOffset = accelXAverage;
                                                                                        accelYOffset = accelYAverage;
                                                                                          accelZOffset = accelZAverage - GRAVITY_1G;

                                                                                            // Gyroscope bias.
                                                                                              // All three axes should be approximately 0 when stationary.
                                                                                                gyroXOffset = gyroXAverage;
                                                                                                  gyroYOffset = gyroYAverage;
                                                                                                    gyroZOffset = gyroZAverage;

                                                                                                      Serial.println();
                                                                                                        Serial.println("Calibration complete.");
                                                                                                          Serial.println();

                                                                                                            Serial.println("Accelerometer Offsets:");
                                                                                                              Serial.print("X: ");
                                                                                                                Serial.println(accelXOffset);
                                                                                                                  Serial.print("Y: ");
                                                                                                                    Serial.println(accelYOffset);
                                                                                                                      Serial.print("Z: ");
                                                                                                                        Serial.println(accelZOffset);

                                                                                                                          Serial.println();

                                                                                                                            Serial.println("Gyroscope Offsets:");
                                                                                                                              Serial.print("X: ");
                                                                                                                                Serial.println(gyroXOffset);
                                                                                                                                  Serial.print("Y: ");
                                                                                                                                    Serial.println(gyroYOffset);
                                                                                                                                      Serial.print("Z: ");
                                                                                                                                        Serial.println(gyroZOffset);

                                                                                                                                          Serial.println();
                                                                                                                                          }

                                                                                                                                          void setup() {

                                                                                                                                            Serial.begin(115200);

                                                                                                                                              Wire.begin();

                                                                                                                                                mpu.initialize();

                                                                                                                                                  if (!mpu.testConnection()) {
                                                                                                                                                      Serial.println("MPU6050 connection failed.");
                                                                                                                                                          while (1);
                                                                                                                                                            }

                                                                                                                                                              Serial.println("MPU6050 connected successfully.");
                                                                                                                                                                Serial.println();

                                                                                                                                                                  calibrateMPU6050();
                                                                                                                                                                  }

                                                                                                                                                                  void loop() {

                                                                                                                                                                    int16_t ax, ay, az;
                                                                                                                                                                      int16_t gx, gy, gz;

                                                                                                                                                                        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

                                                                                                                                                                          long calibratedAx = ax - accelXOffset;
                                                                                                                                                                            long calibratedAy = ay - accelYOffset;
                                                                                                                                                                              long calibratedAz = az - accelZOffset;

                                                                                                                                                                                long calibratedGx = gx - gyroXOffset;
                                                                                                                                                                                  long calibratedGy = gy - gyroYOffset;
                                                                                                                                                                                    long calibratedGz = gz - gyroZOffset;

                                                                                                                                                                                      Serial.print("Accel: ");
                                                                                                                                                                                        Serial.print(calibratedAx);
                                                                                                                                                                                          Serial.print(" | ");
                                                                                                                                                                                            Serial.print(calibratedAy);
                                                                                                                                                                                              Serial.print(" | ");
                                                                                                                                                                                                Serial.print(calibratedAz);

                                                                                                                                                                                                  Serial.print("    Gyro: ");
                                                                                                                                                                                                    Serial.print(calibratedGx);
                                                                                                                                                                                                      Serial.print(" | ");
                                                                                                                                                                                                        Serial.print(calibratedGy);
                                                                                                                                                                                                          Serial.print(" | ");
                                                                                                                                                                                                            Serial.println(calibratedGz);

                                                                                                                                                                                                              delay(500);
                                                                                                                                                                                                              }