 void pid_compute() {
 
   //ROLL calculations
-  pid_roll_setpoint = 0;
-  if (input0 > THROTTLE_RMID + 10)pid_roll_setpoint = (input0 - THROTTLE_RMID + 10) / 2.0;
-  else if (input0 < THROTTLE_RMID - 10)pid_roll_setpoint = (input0 - THROTTLE_RMID - 10) / 2.0;
-
   roll_error = roll_speed - pid_roll_setpoint;
 
   Proportional_roll = ROLL_PID_KP * roll_error;

    roll_speed = gyroscope.x() * 180 / pi;
    pitch_speed = gyroscope.y() * 180 / pi;
    yaw_speed = gyroscope.z() * 180 / pi;
  } else {
    imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
    roll_angle = euler.z();
    pitch_angle = euler.y();
    yaw_angle = euler.x();
  }


  //MOTORS
  throttle = map(input2, THROTTLE_RMIN, THROTTLE_RMAX, MOTOR_ZERO_LEVEL, MOTOR_MAX_LEVEL);
  //PID
  pid_compute();


  ///ARM switch
  if (input4 > 1500) {
    motorFR = throttle + pid_pitch_out + pid_roll_out - pid_yaw_out;
    if (motorFR > 2000) motorFR = 2000;
    else if (motorFR < 1000) motorFR = 1000;

    motorFL = throttle + pid_pitch_out - pid_roll_out + pid_yaw_out;
    if (motorFL > 2000) motorFL = 2000;
    else if (motorFL < 1000) motorFL = 1000;

    motorBL = throttle - pid_pitch_out - pid_roll_out - pid_yaw_out;
    if (motorBL > 2000) motorBL = 2000;
    else if (motorBL < 1000) motorBL = 1000;

    motorBR = throttle - pid_pitch_out + pid_roll_out + pid_yaw_out;
    if (motorBR > 2000) motorBR = 2000;
    else if (motorBR < 1000) motorBR = 1000;

    esc_1.writeMicroseconds(motorFR);
    esc_2.writeMicroseconds(motorFL);

// PID
bool auto_stabilisation_mode = false; // Activer ou pas le mode auto-stabilisé
int dt = 3;
float pid_roll_out,   pid_roll_setpoint,  roll_error, Proportional_roll,  Integral_roll,  Derivative_roll,  last_roll_error, roll_angle_adjust = 0;
float pid_pitch_out,  pid_pitch_setpoint, pitch_error, Proportional_pitch, Integral_pitch, Derivative_pitch, last_pitch_error, pitch_angle_adjust = 0;
float pid_yaw_out,    pid_yaw_setpoint,   yaw_error,  Proportional_yaw,   Integral_yaw,   Derivative_yaw,   last_yaw_error = 0;

// MOTORS
int motorFR, motorBL, motorFL, motorBR;

// RX
int throttle;
volatile int input0, input1, input2, input3, input4;
volatile unsigned long chrono_start0, chrono_start1, chrono_start2, chrono_start3, chrono_start4;
volatile int last_interrupt_time0, last_interrupt_time1, last_interrupt_time2, last_interrupt_time3, last_interrupt_time4;
