.....-... Gyro Drift Calculation Source Code ...........

======== File-Name: 8_Gyro_Drift_Calcul.ino ===========

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++ Begin Gyro Drift Calculation +++++++++++++++++++
//             Last Update: 21-08-2021  
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// Start calculating MPU6050 Gyro Drift for Driftcompensation 

 void  Calc_GyroDrift()
   {
   Serial.println("Start calculating Gyro Drift.......");  

  short hgyro[7];  //used for gyro-drift calcul
  int n = 1000;    //was 4000, number of measurements for averaging drift

  GyroX_Drift= 0;
  GyroY_Drift= 0;
  GyroZ_Drift= 0;

  for (i = 1; i < n ; i++) //loop to calculate the average Drift
    {
    pwm.setPWM(PCApin[6], 4096, 6);  //ON   
    Serial.println(" Gyro Drift Calc");
    readMPU6050(hgyro);
    Serial.print(i);     
    Serial.print(" ");   
    GyroX_Drift += (float)hgyro[4];
    GyroY_Drift += (float)hgyro[5];
    GyroZ_Drift += (float)hgyro[6];
    delay(20);
    pwm.setPWM(PCApin[6], 0, 4096);  //OFF
//  beep();
    }

  GyroX_Drift = GyroX_Drift / (n - 1);
  GyroY_Drift = GyroY_Drift / (n - 1);
  GyroZ_Drift = GyroZ_Drift / (n - 1);
  
  Serial.println("End calculating the Gyro Drift.");

  Serial.print("X-Drift= ");
  Serial.print(GyroX_Drift,1);
  Serial.print("  Y-Drift= ");
  Serial.print(GyroY_Drift,1);
  Serial.print("  Z-Drift= ");
  Serial.println(GyroZ_Drift,1);
  Serial.println(" ");
  delay(2000);
 }

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++ End Gyro Drift Calculation +++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++