ESC slowing down after time using Servo library

Hi!

I have just built my drone and it seems to work quite well with the PID stabilization, but I am having trouble with the thrust of the propellers. When I give full thrust from my remote I do in the code servo.writeMicroseconds(1600), the motors speed up for about 1.5s then they slow down a lot, but in the serial monitor I see that I still send a 1600 signal. I am using this ESC https://hobbyking.com/en_us/hobbyking-20a-2-4s-esc-3a-ubec.html?___store=en_us and here is my Drone 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;
}

Thanks beforehand!
Best regards Max

Have you monitored the motor supply voltage to see if it drops?

Posting a schematic may help. I assume you have monitored the motor supply voltage and that it is stable. Are the motors or anything else getting warm or hot?

Here is my schematic, it is not beautiful, the thing that is missing is the 3s battery, the ESC has a built in 5v regulator, so the battery is wired to the ESCs then powering the arduino from the 5v signal.

I measured the battery voltage and it is dropping when I have full throttle, I am trying to recharge it but I belive it is broken, I have ordered a new battery, I will update this when I get the battery! Thanks everyone!

Good start on the schematic. You missed the ground on the ProMini, I hope it is connected to all the other grounds. Have a happy holiday!

1 Like