I am having big time troubles calibrating my drones PID

Hi!

I am building a drone with theese parts:

Everything seems to work "okey" but the problem is the PID, I have read a lot on how to setup a PID so I think mine looks pretty ok, but I cannot manage to propperly calibrate it, every page Ive looked up on how to calibrate it it says that you should set the Derivative first till it goes crazy, then back 25% at least till it runs smooth, then you should gain the Prospect (gain) with +0.2 every step. But here is where it goes crazy, it just goes back and forward all crazy. Here is my code:

#include <MPU9250.h>
#include <Servo.h>
#include <nRF24L01.h>
#include <SPI.h>
#include <RF24.h>
#include <printf.h>

#define CE_PIN   9
#define CSN_PIN 10


// Create Radio and define pins
RF24 radio(CE_PIN, CSN_PIN);

// Create Radio channels
const uint64_t pAddress = 0xB00B1E5000LL;

// Create a boolean for checkConnectionLost()
bool lostConnection = false;

//Radio recived array
int recivedDataArray[4];

// Create timer
float elapsedTime, time, timePrev;
long loop_timer;
float previousMessageMillis;

Servo motor_LF;
Servo motor_RF;
Servo motor_LB;
Servo motor_RB;

MPU9250 mpu;

int input_throttle = 1000;
int input_yaw = 0;
int input_pitch = 0;
int input_roll = 0;

/* Roll PID */
float roll_PID, pid_throttle_L_F, pid_throttle_L_B, pid_throttle_R_F, pid_throttle_R_B, roll_error, roll_previous_error;
float roll_pid_p = 0;
float roll_pid_i = 0;
float roll_pid_d = 0;
/* Roll PID Constants */
double roll_kp = 1.2;
double roll_ki = 0;
double roll_kd = 30;
float roll_desired_angle = 0;

/* Pitch PID */
float pitch_PID, pitch_error, pitch_previous_error;
float pitch_pid_p = 0;
float pitch_pid_i = 0;
float pitch_pid_d = 0;
/* Pitch PID Constants */
double pitch_kp = roll_kp;
double pitch_ki = roll_ki;
double pitch_kd = roll_kd;
float pitch_desired_angle = 0;

void imuSetup() {
  if (!mpu.setup(0x68)) {  // change to your own address
        while (1) {
            Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
            delay(5000);
        }
    }
}

void setup() {
  Serial.begin(115200);
  while (!Serial);
  printf_begin();
  
  imuSetup();

  if (!radio.begin()) {
    Serial.println(F("radio hardware is not responding!!"));
    while (1) {} 
  }
  radio.setPALevel(RF24_PA_MAX);
  radio.openReadingPipe(1, pAddress);
 
  radio.printDetails();
  radio.startListening();

  delay(1000);

  motor_LF.attach(5);
  motor_RF.attach(6);
  motor_LB.attach(7);
  motor_RB.attach(8);

  motor_LF.writeMicroseconds(1000);
  motor_RF.writeMicroseconds(1000);
  motor_LB.writeMicroseconds(1000);
  motor_RB.writeMicroseconds(1000);
}

void loop() {
  if (radio.available()) {
    radio.read( &recivedDataArray, sizeof(recivedDataArray) );
    //Serial.println("Recieved array:");
    previousMessageMillis = millis();
    for (byte i = 0; i < 4; i++) {
      //Serial.println(recivedDataArray[i]);
    }
    input_throttle = recivedDataArray[0];
    input_yaw = recivedDataArray[1];
    pitch_desired_angle = -recivedDataArray[2];
    roll_desired_angle = recivedDataArray[3];
    //Serial.println();
  }

  if (input_throttle > 1600) {
    input_throttle = 1600;
  }

  PIDControl();

  /* Start Timer */
  timePrev = time;
  time = millis();
  elapsedTime = (time - timePrev) / 1000;

  if (millis() - previousMessageMillis >= 400) {
    Serial.println("Connection Lost- Reseting PID");
    PID_reset();
    motor_LF.writeMicroseconds(1000);
    motor_RF.writeMicroseconds(1000);
    motor_LB.writeMicroseconds(1000);
    motor_RB.writeMicroseconds(1000);
  } else {
    motor_LF.writeMicroseconds(pid_throttle_L_F);
    motor_RF.writeMicroseconds(pid_throttle_R_F);
    motor_LB.writeMicroseconds(pid_throttle_L_B);
    motor_RB.writeMicroseconds(pid_throttle_R_B);
  }

  //print_roll_pitch_yaw();
  print_pid();
}

void print_roll_pitch_yaw() {
  if (mpu.update()) {
    Serial.print("Yaw, Pitch, Roll: ");
    Serial.print(mpu.getYaw(), 2);
    Serial.print(", ");
    Serial.print(mpu.getPitch() + 20, 2);
    Serial.print(", ");
    Serial.println(mpu.getRoll() + 44, 2);
  }
}

void print_pid() {
  /*
    Serial.print("ROLL: ");
    Serial.println(roll_PID);
    Serial.print("PITCH: ");
    Serial.println(pitch_PID);
  */

  Serial.print("LF: ");
  Serial.println(pid_throttle_L_F);
  Serial.print("RF: ");
  Serial.println(pid_throttle_R_F);
  Serial.print("LB: ");
  Serial.println(pid_throttle_L_B);
  Serial.print("RB: ");
  Serial.println(pid_throttle_R_B);

}

void PIDControl() {
  if (input_throttle > 1000) {
    if (mpu.update()) {
      /* PID */
      roll_error = roll_desired_angle + mpu.getRoll() + 44;
      pitch_error = pitch_desired_angle + mpu.getPitch() + 20;

      /* Prospect */
      roll_pid_p = roll_kp * roll_error;
      pitch_pid_p = pitch_kp * pitch_error;

      /* Integral */
      if (-3 < roll_error < 3)
      {
        roll_pid_i = roll_pid_i + (roll_ki * roll_error);
      }
      if (-3 < pitch_error < 3)
      {
        pitch_pid_i = pitch_pid_i + (pitch_ki * pitch_error);
      }

      /* Derivate  OLD */
      
      roll_pid_d = roll_kd * (roll_error - roll_previous_error);
      pitch_pid_d = pitch_kd * (pitch_error - pitch_previous_error);
      
      
      /* PID summery */
      roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
      pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;

      /* Regulate PID output for ESCs */
      if (roll_PID < -300) {
        roll_PID = -300;
      }
      if (roll_PID > 300) {
        roll_PID = 300;
      }
      if (pitch_PID < -300) {
        pitch_PID = -300;
      }
      if (pitch_PID > 300) {
        pitch_PID = 300;
      }

      /* Set the throttle PID for each motor */
      pid_throttle_L_F = input_throttle + roll_PID + pitch_PID;
      pid_throttle_R_F = input_throttle - roll_PID + pitch_PID;
      pid_throttle_L_B = input_throttle + roll_PID - pitch_PID;
      pid_throttle_R_B = input_throttle - roll_PID - pitch_PID;

      /*
        Serial.print("ROLL: ");
        Serial.println(roll_PID);
        Serial.print("PITCH: ");
        Serial.println(pitch_PID);
      */

      /* Regulate throttle for ESCs */
      //Right front
      if (pid_throttle_R_F < 1100)
      {
        pid_throttle_R_F = 1100;
      }
      if (pid_throttle_R_F > 2000)
      {
        pid_throttle_R_F = 2000;
      }

      //Left front
      if (pid_throttle_L_F < 1100)
      {
        pid_throttle_L_F = 1100;
      }
      if (pid_throttle_L_F > 2000)
      {
        pid_throttle_L_F = 2000;
      }

      //Right back
      if (pid_throttle_R_B < 1100)
      {
        pid_throttle_R_B = 1100;
      }
      if (pid_throttle_R_B > 2000)
      {
        pid_throttle_R_B = 2000;
      }

      //Left back
      if (pid_throttle_L_B < 1100)
      {
        pid_throttle_L_B = 1100;
      }
      if (pid_throttle_L_B > 2000)
      {
        pid_throttle_L_B = 2000;
      }

      /* Save Previous Error */
      roll_previous_error = roll_error;
      pitch_previous_error = pitch_error;
    }
  }
  else {
    PID_reset();
  }
}

void PID_reset() {
  pid_throttle_L_F = 1000;
  pid_throttle_L_B = 1000;
  pid_throttle_R_F = 1000;
  pid_throttle_R_B = 1000;

  roll_error = 0;
  pitch_error = 0;
  roll_previous_error = 0;
  pitch_previous_error = 0;
}

An here is my schematic (not beautiful :slight_smile: )

5a0ccfa0387babccd2d57bc466ffe15a1943033f_2_375x500

If anyone has any tips or tricks it would be awesome! Thank you so much beforehand!

Best regards Max