 
       calculate_pid();
       updateServo(); //update pwn signals
+      compute_time=micros()-lastUpdate;
     }
     
     if(micros() - lastWrite >= 500000)

}

void gyro_signalen(){
  gyro.readGyroData();
  gyro_pitch = (M_pi/180)*gyro.gyroData.x*gyro.gRes;
  gyro_pitch *= -1;
  gyro_roll = (M_pi/180)*gyro.gyroData.y*gyro.gRes;
  gyro_roll *= -1;
  gyro_yaw = (M_pi/180)*gyro.gyroData.z*gyro.gRes;
  gyro_yaw *= -1;

}

void calculate_pid(){

      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");
    }
    
