// MydroneProject from Carbon Aeronautics (GitHub)
// Project 5
// Sensing the Rotational Rate and Calibrate the Gyroscope

#include <Wire.h>   // Include the Wire library

float RateRoll, RatePitch, RateYaw;   // declare the global variables

// Declare the calibration global variables
float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
int RateCalibrationNumber;

void gyro_signals(void) {
  Wire.beginTransmission(0x68);
  Wire.write(0x1A); // Switch on the Low-pass filter
  Wire.write(0x05);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1B); // Set the sensitivity scale factor
  Wire.write(0x8);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x43); // Access registers storing Gyro measurements
  Wire.endTransmission();
  Wire.requestFrom(0x68, 6);

  int16_t GyroX = Wire.read()<<8|Wire.read(); // Read the gyro measurements around the X axis
  int16_t GyroY = Wire.read()<<8|Wire.read(); // Read the gyro measurements around the Y axis
  int16_t GyroZ = Wire.read()<<8|Wire.read(); // Read the gyro measurements around the Z axis

  RateRoll=(float)GyroX/65.5;  // Convert the measurements units to °/s
  RatePitch=(float)GyroY/65.5;
  RateYaw=(float)GyroZ/65.5;

}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(57600);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);

  Wire.setClock(400000);  // Set the clock speed of I2C
  Wire.begin();
  delay(250);

  Wire.beginTransmission(0x68); // Start the Gyro in power mode
  Wire.begin(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  // Perform the Calibration measurements
  for (RateCalibrationNumber=0;RateCalibrationNumber<2000;RateCalibrationNumber ++) {
    gyro_signals();
    RateCalibrationRoll+=RateRoll;
    RateCalibrationPitch+=RatePitch;
    RateCalibrationYaw+=RateYaw;
    delay(1);
  }
  RateCalibrationRoll/=2000;    // Calculate the calibration values
  RateCalibrationPitch/=2000;
  RateCalibrationYaw/=2000;


}

void loop() {
  // put your main code here, to run repeatedly:
  gyro_signals(); // Call the predefined function to read the gyro measurements
  // Correct the measured values
  RateRoll = RateRoll-RateCalibrationRoll;
  RatePitch = RatePitch-RateCalibrationPitch;
  RateYaw = RateYaw-RateCalibrationYaw;
  
  Serial.print("Roll rate [degree/s]= ");
  Serial.println(RateRoll);
  Serial.print("Pitch rate [degree/s]= ");
  Serial.println(RatePitch);
  Serial.print("Yaw rate [degree/s]= ");
  Serial.println(RateYaw);
  delay(50);

}