//--------------------------------------------------------------------------
// imu calibration code 
// klaus-peter neitzke
// last modification 11 march 2019
// arduino and mpu6050
//--------------------------------------------------------------------------

#include <Wire.h>

#define MPU6050      0x68 
#define DLPF_CONFIG  0x1A
#define ACCEL_CONFIG 0x1C 
#define GYRO_CONFIG  0x1B
#define ACCEL_XOUT_H 0x3B
#define GYRO_XOUT_H  0x43
#define PWR_MGMT_1   0x6B

#define toRead 6
byte reading[toRead];

#define accSens    3 // 0 = 2g,       1 = 4g,       2 = 8g,        3 = 16g
                     // 16384         8192          4096           2048
#define gyroSens   3 // 0 = 250deg/s, 1 = 500deg/s, 2 = 1000deg/s, 3 = 2000deg/s
                     // 131.072       65.536        32.768         16.384
#define filterFreq 2 // 0=256hz, 1=188hz, 2=98hz, 3=42hz, 4=20hz, 5=10hz, 6=5hz

//--------------------------------------------------------------------------
// variables
//--------------------------------------------------------------------------
 double pitch_g_sens, roll_g_sens, yaw_g_sens, hx_meas, hy_meas, hz_meas;
 double sum_pitch_g_sens, sum_roll_g_sens, sum_yaw_g_sens, 
        sum_hx_meas, sum_hy_meas, sum_hz_meas;
        
 long int printtime, printtime_old;
        
//--------------------------------------------------------------------------
// setup
//--------------------------------------------------------------------------
void setup()
 {    
   Wire.begin();
   Serial.begin(38400);
   writeTo(MPU6050, PWR_MGMT_1, 0); // wake up call
   writeTo(MPU6050, ACCEL_CONFIG, accSens << 3); // scaling accelerometer
   writeTo(MPU6050, GYRO_CONFIG, gyroSens << 3); // scaling gyroscope
   writeTo(MPU6050, DLPF_CONFIG, filterFreq);    // digital low pass filter
  
   printtime_old= 0; 
 }

void writeTo(byte device, byte address, byte value)
 {
   Wire.beginTransmission(device);
   Wire.write(address);
   Wire.write(value);
   Wire.endTransmission();
 }

void readFrom(byte device, byte address, byte bytes, byte reading[])
 {
   Wire.beginTransmission(device);
   Wire.write(address);
   Wire.endTransmission();
   Wire.requestFrom(device, bytes);
   while (bytes > Wire.available())
    {
    }
   for (int i = 0; i < bytes; i++)
    {
      reading[i] = Wire.read();
    }
 }

//--------------------------------------------------------------------------
// read the imu
//--------------------------------------------------------------------------
void read_imu()
 {
   readFrom(MPU6050, ACCEL_XOUT_H, toRead, reading);
   hx_meas =  (float)((reading[2] << 8) | reading[3])/2048 + 0.000; // g
   hy_meas = -(float)((reading[0] << 8) | reading[1])/2048 + 0.000; // g
   hz_meas =  (float)((reading[4] << 8) | reading[5])/2048 + 0.000; // g 

   readFrom(MPU6050, GYRO_XOUT_H, toRead, reading);
   pitch_g_sens = -(float)((reading[2] << 8) | reading[3])/16.384 + 0.00; // deg/sec
   roll_g_sens  = -(float)((reading[0] << 8) | reading[1])/16.384 + 0.00; // deg/sec
   yaw_g_sens   = -(float)((reading[4] << 8) | reading[5])/16.384 + 0.00; // deg/sec
 }

//--------------------------------------------------------------------------
// write the results
//--------------------------------------------------------------------------
void write_imu()
 { 
   printtime= micros();
   
   Serial.print(hx_meas,3); Serial.print("   ");
   Serial.print(hy_meas,3); Serial.print("   ");
   Serial.print(hz_meas,3); Serial.print("   ");
   Serial.print(pitch_g_sens,2); Serial.print("   ");
   Serial.print(roll_g_sens,2);  Serial.print("   ");
   Serial.print(yaw_g_sens,2);   Serial.print("   ");

   Serial.println(); 
 }

//--------------------------------------------------------------------------
// global routine
//--------------------------------------------------------------------------
void loop()
 { 
  int i;
  float delta;
  
   sum_pitch_g_sens= 0;
   sum_roll_g_sens=  0;
   sum_yaw_g_sens=   0;
   sum_hx_meas=      0;
   sum_hy_meas=      0;
   sum_hz_meas=      0;
   
   for (i=1; i<1000; i++)
    {
      read_imu();
      
      sum_pitch_g_sens= sum_pitch_g_sens + pitch_g_sens;
      sum_roll_g_sens= sum_roll_g_sens   + roll_g_sens;
      sum_yaw_g_sens= sum_yaw_g_sens     + yaw_g_sens;
      sum_hx_meas= sum_hx_meas           + hx_meas;
      sum_hy_meas= sum_hy_meas           + hy_meas;
      sum_hz_meas= sum_hz_meas           + hz_meas;
    }
    
      pitch_g_sens= sum_pitch_g_sens / i;
      roll_g_sens= sum_roll_g_sens   / i;
      yaw_g_sens= sum_yaw_g_sens     / i;
      hx_meas= sum_hx_meas           / i;
      hy_meas= sum_hy_meas           / i;
      hz_meas= sum_hz_meas           / i;    
    
   write_imu();

   delta= (float)((printtime-printtime_old)/1000);
   delta= delta / i;
   
   delta = 1/delta*1000;
   
   Serial.print(delta,1); Serial.print("   ");
   
   printtime_old= printtime;  
 }
 
//--------------------------------------------------------------------------
// end of program
//--------------------------------------------------------------------------