 }
 
 void gyro_signalen(){
+  
   gyro.readGyroData();
-  gyro_pitch = (M_pi/180)*gyro.gyroData.x*gyro.gRes;
+  gyro_pitch = gyro.gyroData.x*gyro.gRes;
   gyro_pitch *= -1;
-  gyro_roll = (M_pi/180)*gyro.gyroData.y*gyro.gRes;
+  if(cal_int == 2000)gyro_pitch -= gyro_pitch_cal;  
+  gyro_roll = gyro.gyroData.y*gyro.gRes;
   gyro_roll *= -1;
-  gyro_yaw = (M_pi/180)*gyro.gyroData.z*gyro.gRes;
+  if(cal_int == 2000)gyro_roll -= gyro_roll_cal; 
+  gyro_yaw = gyro.gyroData.z*gyro.gRes;
   gyro_yaw *= -1;
-
+  if(cal_int == 2000)gyro_yaw -= gyro_yaw_cal;    
 }
 
 void calculate_pid(){

    for(it=startIndex;it<=stopIndex;it++) {
      if(buffer[it] == ',' || buffer[it] == '*') {
        strncpy (tempBuffer, buffer+startIndex, it-startIndex );
        double yaw_tmp = max(0,min(20,strtod(tempBuffer,NULL)));
        target_yaw = (yaw_tmp - 10)*10;
        break;
      }
    }

float pid_i_mem_pitch, pid_pitch_setpoint, gyro_pitch_input, pid_output_pitch, pid_last_pitch_d_error;
float pid_i_mem_yaw, pid_yaw_setpoint, gyro_yaw_input, pid_output_yaw, pid_last_yaw_d_error;

uint32_t lastWrite,lastUpdate,compute_time;

int cal_int;
double gyro_roll_cal, gyro_pitch_cal, gyro_yaw_cal;
byte highByte, lowByte;
/**
 * Modes:
 * 0 = standby -> send no pulse or idle
