#include <Wire.h>
float roll_rate,pitch_rate,yaw_rate;

float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
// int RateCalibrationNumber;

void read_gyro(){
  Wire.beginTransmission(0x68);
  Wire.write(0x1A); //low pass filer
  Wire.write(0x05);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x43); //address of roll_rate
  Wire.endTransmission();

  Wire.requestFrom(0x68,6);
  int16_t x_raw = Wire.read()<<8|Wire.read();
  int16_t y_raw = Wire.read()<<8|Wire.read();
  int16_t z_raw = Wire.read()<<8|Wire.read();

  roll_rate = (float)(x_raw/131);
  pitch_rate = (float)(y_raw/131);
  yaw_rate = (float)(z_raw/131);
}

void setup() {
  Serial.begin(115200);
  Wire.setClock(400000);
  Wire.begin();
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  delay(100);

  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x00); //sensitivity set to 250 degrees/sec
  Wire.endTransmission();

  for (int i=0;i<2000;i++){
    read_gyro();
    RateCalibrationRoll+=roll_rate;
    RateCalibrationPitch+=pitch_rate;
    RateCalibrationYaw+=yaw_rate;
    delay(1);
  }
  RateCalibrationRoll/=2000;
  RateCalibrationPitch/=2000;
  RateCalibrationYaw/=2000;
}

void loop() {
  read_gyro();
  roll_rate-=RateCalibrationRoll;
  pitch_rate-=RateCalibrationPitch;
  yaw_rate-=RateCalibrationYaw;
  Serial.print("Roll rate = ");
  Serial.print(roll_rate);
  Serial.println(" degrees/sec");
  Serial.print("Pitch rate = ");
  Serial.print(pitch_rate);
  Serial.println(" degrees/sec");
  Serial.print("yaw rate = ");
  Serial.print(yaw_rate);
  Serial.println(" degrees/sec");
  delay(100); 
}