   gyro.getGres();
 
   //calibrate gyro
-  digitalWrite(13, HIGH);
-  gyro.calibrate();
-  digitalWrite(13, LOW);
+  for (cal_int = 0; cal_int < 2000 ; cal_int ++){              //Take 2000 readings for calibration.
+    if(cal_int % 15 == 0)digitalWrite(13, !digitalRead(13));   //Change the led status to indicate calibration.
+    gyro_signalen();                                           //Read the gyro output.
+    gyro_roll_cal += gyro_roll;                                //Ad roll value to gyro_roll_cal.
+    gyro_pitch_cal += gyro_pitch;                              //Ad pitch value to gyro_pitch_cal.
+    gyro_yaw_cal += gyro_yaw;                                  //Ad yaw value to gyro_yaw_cal.
+    delayMicroseconds(1000);                                   //Wait 1000us.
+    delay(3);                                                  //Wait 3 milliseconds before the next loop.
+  }
+
+  //Now that we have 2000 measures, we need to devide by 2000 to get the average gyro offset.
+  gyro_roll_cal /= 2000;                                       //Divide the roll total by 2000.
+  gyro_pitch_cal /= 2000;                                      //Divide the pitch total by 2000.
+  gyro_yaw_cal /= 2000;                                        //Divide the yaw total by 2000.
+  
 
   pid_i_mem_roll = 0;
   pid_last_roll_d_error = 0;

float pid_d_gain_pitch = pid_d_gain_roll;  //Gain setting for the pitch D-controller.
int pid_max_pitch = pid_max_roll;          //Maximum output of the PID-controller (+/-)

float pid_p_gain_yaw = 4.0;                //Gain setting for the pitch P-controller. //4.0
float pid_i_gain_yaw = 0.02;               //Gain setting for the pitch I-controller. //0.02
float pid_d_gain_yaw = 0.0;                //Gain setting for the pitch D-controller.
int pid_max_yaw = 400;                     //Maximum output of the PID-controller (+/-)



      Serial.print(",");
      Serial.print(gyro_yaw);
      Serial.print(",");
      Serial.print(pid_output_pitch);
      Serial.print(",");
      Serial.print(pid_output_roll);
      Serial.print(",");
      Serial.print(pid_output_yaw);
      Serial.print(",");
      Serial.print(target_pitch);
      Serial.print(",");
      Serial.print(target_roll);
      Serial.print(",");
      Serial.print(target_yaw);
      Serial.print(",");
      Serial.print(compute_time);
      Serial.println("*00\n");
    }
    
