Quadcopter PID tuning self balancing problems HELP

Projects Programming
1

I’m currently building a drone from scratch. The frame is an F450, powered by four A2122 930KV motors and 30A Simonk ESCs. My main flight controller is an ESP32-S3, paired with an MPU6050 (providing gyroscope and accelerometer data) and a logic level shifter to convert the 3.3V PWM signals to 5V.

The issue I’m facing is that during testing on a tuning rig (similar to IMG1 ), the drone consistently falls on one of the motor's sides. (Like 45 degrees on Pitch and 45 degrees on Roll Axis)

For control, I’m using a standard cascaded PID system:

  • Outer PID: input = desired angle, output = desired angular rate
  • Inner PID: input = desired angular rate, output = PWM signal
  • Yaw axis: uses only the inner PID (since I don’t have a magnetic compass, I can’t calculate the yaw angle).

Currently, both the Roll and Pitch axes use the same cascaded PID values:

  • Outer PID: P = 1.2, I = 0, D = 0.00008(Pitch and Roll)
  • Inner PID: P = 2.3, I = 0, D = 0.00002(Pitch and Roll)
  • Yaw PID: P = 0.8, I = 0, D = 0(inner PID only, input = angular rate, output = PWM).

More Information

  • MPU6050: Already calibrated both Gyro and Accelerometer, and using a complementary filter
  • PWM 1000-2000us calibrate every time the drone is connected to the battery
  • MPU6050 250Hz(Ideal Condition), Drone Motor(500Hz), whole system including PID 250-300Hz(Rough Estimation)
  • Starting level (0° Roll and Pitch), the drone slowly tilts toward one motor. The controller sometimes overcorrects, causing instability, or fails to provide sufficient PWM, letting the drone continue tipping toward that motor’s side.

At this point, I don't know if it's my hardware problem or my software problem. I have been tuning the PID system for a month now, and I'm starting to lose hope. Please help Thanks

Main.ino (Relevant Part)

void setup()
{
    Serial.begin(115200);
    delay(2000);
    Serial.println("Program started...");
    MyController.InitDrone(TopLeft,TopRight,BottomLeft,BottomRight,SDA_PIN,SCL_PIN);///Init
    MyController.InitPID(OuterPID,InnerPID,YawPID);//Init
    delay(100);
    WifiManager.bootServer();

}
void loop()
{
    if(WifiManager.waitForClient())//Tuning PID via Wifi
    {

        while(WifiManager.isClientConnected())
        {
            MyController.ApplyThrust(1500);//PWM
            MyController.AutoBalance(0,0,0);///Angle Pitch Roll Yaw
            MyController.SubmitPWM();

            delay(1);
           /// MyController.PrintAll4Motors();


            if(WifiManager.ReceiveModifyPID(UpdatedOuterPID,UpdatedInnerPID,UpdateYawPID))///Updating the PID
            {   
                    MyController.EnableProtection = false;
                    OuterPID.Kp = UpdatedOuterPID.P;
                    OuterPID.Ki = UpdatedOuterPID.I;
                    OuterPID.Kd = UpdatedOuterPID.D;
                    
                    InnerPID.Kp = UpdatedInnerPID.P;
                    InnerPID.Ki = UpdatedInnerPID.I;
                    InnerPID.Kd = UpdatedInnerPID.D;

                    YawPID.Kp = UpdateYawPID.P;
                    YawPID.Ki = UpdateYawPID.I;
                    YawPID.Kd = UpdateYawPID.D;


                    MyController.InitPID(OuterPID,InnerPID,YawPID);



            }
        }
        MyController.ShutDownMotors();

 

    }
    MyController.ShutDownMotors();
    
}

PID + Drone Controller Part

#include "Drone_Control_System.h" //Header File

PID_System::PID_System()
    : Kp(0), Ki(0), Kd(0),
      prevError(0), integral(0),
      OutMin(-1000), OutMax(1000),
      FeedBackValue(0), Input(0)
{
    lastTime = millis();
}

void PID_System::SetPID(float P, float I, float D) {
    Kp = P;
    Ki = I;
    Kd = D;
    integral=0;
    lastTime = millis();


}

void PID_System::SetOutputLimit(float Min, float Max) {
    OutMin = Min;
    OutMax = Max;
}

void PID_System::EntryPoint(float In) {
    Input = In;
}

void PID_System::Feedback(float Detected) {
    FeedBackValue = Detected;
}

float PID_System::PID_Output() {
    unsigned long now = millis();
    float timeChange = (float)(now - lastTime) / 1000.0f; 
    if (timeChange <= 0) timeChange = 0.00001f;

    float error = Input - FeedBackValue;
    integral += error * timeChange;
    float derivative = (error - prevError) / timeChange;
    float output = (Kp * error) + (Ki * integral) + (Kd * derivative);

    if (output > OutMax) output = OutMax;
    else if (output < OutMin) output = OutMin;

    prevError = error;
    lastTime = now;
    return output;
}

// ================= Drone Controller ==================

void Drone_Controller::InitDrone(uint16_t TopLeftPin,uint16_t TopRightPin, uint16_t BottomLeftPin, uint16_t BottomRightPin,uint16_t SDA_Pin, uint16_t SCL_Pin)
{
   if(Debug) Serial.println("Start Calibrating MPU6050 DO NOT MOVE IT");
  InternalGyro.InitGyro(3, 8);       // custom SDA/SCL pins
  InternalGyro.SetUpdateInterval(4); // 4ms
   if(Debug) Serial.println("Finished Calibrating MPU6050");
  

   if(EnableFlight)
   {

        ESP32PWM::allocateTimer(0);
        ESP32PWM::allocateTimer(1);
        ESP32PWM::allocateTimer(2);
        ESP32PWM::allocateTimer(3);
        delay(1000);
        TopLeft_ESC.attach(TopLeftPin,MIN_PULSE,MAX_PULSE);
        delay(1000);
        TopRight_ESC.attach(TopRightPin,MIN_PULSE,MAX_PULSE);
        delay(1000);
        BottomLeft_ESC.attach(BottomLeftPin,MIN_PULSE,MAX_PULSE);
        delay(1000);
        BottomRight_ESC.attach(BottomRightPin,MIN_PULSE,MAX_PULSE);
        delay(1000);
        TopLeft_ESC.setPeriodHertz(MotorHZ);//500HZ
        delay(100);
        TopRight_ESC.setPeriodHertz(MotorHZ);
        delay(100);
        BottomLeft_ESC.setPeriodHertz(MotorHZ);
        delay(100);
        BottomRight_ESC.setPeriodHertz(MotorHZ);
        delay(100);
        //Calibrating ESC
        TopLeft_ESC.writeMicroseconds(MAX_PULSE);//2000us
        TopRight_ESC.writeMicroseconds(MAX_PULSE);//2000
        BottomLeft_ESC.writeMicroseconds(MAX_PULSE);//2000us
        BottomRight_ESC.writeMicroseconds(MAX_PULSE);//2000us

        delay(5000);

        TopLeft_ESC.writeMicroseconds(MIN_PULSE);//1000us
        TopRight_ESC.writeMicroseconds(MIN_PULSE);///1000
        BottomLeft_ESC.writeMicroseconds(MIN_PULSE);///1000
        BottomRight_ESC.writeMicroseconds(MIN_PULSE);//1000

        delay(2000);



    
   }
}

void Drone_Controller::InitPID(PID_Values RotationRate, PID_Values PWM , PID_Values Yaw)
{
  PitchRotationRate_PID.SetPID(RotationRate.Kp, RotationRate.Ki, RotationRate.Kd);//Input Angle Output Angle Rate
  PitchPWM_PID.SetPID(PWM.Kp, PWM.Ki, PWM.Kd);//Input Angle Rate Output PWM

  RollRotationRate_PID.SetPID(RotationRate.Kp, RotationRate.Ki,  RotationRate.Kd);//Input Angle Output Angle Rate
  RollPWM_PID.SetPID(PWM.Kp, PWM.Ki, PWM.Kd);//Input Angle Rate Output PWM
  YawRotationPWM_PID.SetPID(Yaw.Kp,Yaw.Ki,Yaw.Kd);//Input Angle Rate Output PWM

  YawRotationPWM_PID.SetOutputLimit(-350, 350);//PWM

  PitchRotationRate_PID.SetOutputLimit(-350,350);//Angle Rate
  PitchPWM_PID.SetOutputLimit(-350,350);//PWM
  RollRotationRate_PID.SetOutputLimit(-350,350);//Angle Rate
  RollPWM_PID.SetOutputLimit(-350,350);//PWM
}

void Drone_Controller::ApplyThrust(int PWM)
{
  InternalMotorsPWM.TLeft = PWM;
  InternalMotorsPWM.TRight= PWM;
  InternalMotorsPWM.BLeft = PWM;
  InternalMotorsPWM.BRight = PWM;
}

void Drone_Controller::AutoBalance(float DesirePitchAngle, float DesireRollAngle,float DesireYawRotateRate)
{

  InternalGyro.GyroSignal();
  //Pitch
  CurrentPitchAngle = constrain(InternalGyro.CompAnglePitch,-20,20);///20degrees
  CurrentPitchAngleRate = InternalGyro.RatePitch;

  //Roll
  CurrentRollAngle = constrain(InternalGyro.CompAngleRoll,-20,20);///20degrees 
  CurrentRollAngleRate = InternalGyro.RateRoll;
  
  //Yaw
  CurrentYawAngleRate = InternalGyro.RateYaw;


  //Pitch PID
  
  PitchRotationRate_PID.EntryPoint(DesirePitchAngle); ///Input Degree Output Angle Rate
  PitchRotationRate_PID.Feedback(CurrentPitchAngle);  
  float DesirePitchRate = PitchRotationRate_PID.PID_Output();

  
  PitchPWM_PID.EntryPoint(DesirePitchRate);///Input Angle Rate Output PWM
  PitchPWM_PID.Feedback(CurrentPitchAngleRate);
  float Pitch_OutputPWM = PitchPWM_PID.PID_Output();




  //Roll PID
  
  RollRotationRate_PID.EntryPoint(DesireRollAngle);///Input Degree Output Angle Rate
  RollRotationRate_PID.Feedback(CurrentRollAngle);
  float DesireRollRate = RollRotationRate_PID.PID_Output();

  RollPWM_PID.EntryPoint(DesireRollRate);///Input Angle Rate Output PWM
  RollPWM_PID.Feedback(CurrentRollAngleRate);
  float Roll_OutputPWM = RollPWM_PID.PID_Output();

  //Yaw
 
  YawRotationPWM_PID.EntryPoint(DesireYawRotateRate);//Input Angle Rate Output PWM
  YawRotationPWM_PID.Feedback(CurrentYawAngleRate);
  float Yaw_OutputPWM = YawRotationPWM_PID.PID_Output();

  
  
  
  InternalMotorsPWM.TLeft +=  Pitch_OutputPWM+ Roll_OutputPWM + Yaw_OutputPWM; //CCW
  InternalMotorsPWM.BLeft +=  Pitch_OutputPWM + (-1*Roll_OutputPWM) + (-1*Yaw_OutputPWM);//CW

  InternalMotorsPWM.TRight += (-1*Pitch_OutputPWM) + Roll_OutputPWM + (-1* Yaw_OutputPWM);//CW
  InternalMotorsPWM.BRight += (-1*Pitch_OutputPWM) + (-1*Roll_OutputPWM)+ Yaw_OutputPWM;//CCW

  if(fabs(CurrentPitchAngle) > PitchAngleProtection || fabs(CurrentRollAngle) > RollAngleProtection) //30Degrees
  {
    EnableProtection = true;
  }

  /*
   Serial.print(" TL:");
  Serial.print(InternalMotorsPWM.TLeft);
  Serial.print(" TR:");  
  Serial.print(InternalMotorsPWM.TRight);
  Serial.print(" BL:");
  Serial.print(InternalMotorsPWM.BLeft);
  Serial.print(" BR:");
  Serial.print(InternalMotorsPWM.BRight);
  Serial.print(" ");
  Serial.print(DesireRollRate);
  Serial.print(" ");
  Serial.println(DesirePitchRate);

  */


}

void Drone_Controller::SubmitPWM()
{
  Bounded();

  if(EnableProtection)
  {
    ShutDownMotors();
    return;
  }
   if(EnableFlight)
   {
        TopLeft_ESC.writeMicroseconds(InternalMotorsPWM.TLeft);
        TopRight_ESC.writeMicroseconds(InternalMotorsPWM.TRight);
        BottomLeft_ESC.writeMicroseconds(InternalMotorsPWM.BLeft);
        BottomRight_ESC.writeMicroseconds(InternalMotorsPWM.BRight);
   }
}

void Drone_Controller::ShutDownMotors()
{

    InternalMotorsPWM.TLeft =MIN_PULSE;
    InternalMotorsPWM.TRight=MIN_PULSE;
    InternalMotorsPWM.BLeft =MIN_PULSE;
    InternalMotorsPWM.BRight=MIN_PULSE;

  if(EnableFlight)
  {
      

        TopLeft_ESC.writeMicroseconds(MIN_PULSE);
        TopRight_ESC.writeMicroseconds(MIN_PULSE);
        BottomLeft_ESC.writeMicroseconds(MIN_PULSE);
        BottomRight_ESC.writeMicroseconds(MIN_PULSE);
  }
}

void Drone_Controller::Bounded()
{
  InternalMotorsPWM.TLeft  = constrain(InternalMotorsPWM.TLeft, 1000, 1999);
  InternalMotorsPWM.TRight = constrain(InternalMotorsPWM.TRight,1000,1999);
  InternalMotorsPWM.BLeft  = constrain(InternalMotorsPWM.BLeft, 1000, 1999);
  InternalMotorsPWM.BRight = constrain(InternalMotorsPWM.BRight,1000,1999);
}
void Drone_Controller::PrintAll4Motors()
{
  if(!EnableProtection)
  {

    /*
    Serial.print("Pitch Angle: ");
    Serial.print(CurrentPitchAngle);
    Serial.print("  Roll Angle:");
    Serial.print(CurrentRollAngle);
    Serial.print("  Yaw Rate");
    Serial.println(CurrentYawAngleRate);
      
      Serial.print("  Top Left: ");
      Serial.print(InternalMotorsPWM.TLeft);
      Serial.print("  Top Right: ");
      Serial.print(InternalMotorsPWM.TRight);
      Serial.print("  Bottom Left: ");
      Serial.print(InternalMotorsPWM.BLeft);
      Serial.print("  Bottom Right: ");
      Serial.println(InternalMotorsPWM.BRight);

      delay(50);
      */
  }

 
  
}

Some CMD Outputs:

Roll Angle Pitch Angle Yaw Angle Rate Top Left Motor(PWM) Top Right Motor Bottom Left Motor Bottom Right Motor
3.36 20.65 -0.17 1437 1545 1454 1562
20.25 1.95 -0.60 1445 1449 1549 1555
1.89 -20.38 -0.49 1550 1443 1556 1450
-20.24 -1.89 0.07 1566 1543 1456 1433
20.40 -19.82 15.16 1457 1527 1497 1517
21.65 20.84 -0.40 1388 1500 1498 1612
-1.16 2.36 32.58 1471 1540 1511 1476

Roll is Top(Negative) Down(Postive)
Pitch is Left(Negavie) Right(Postive)

All help is appreciated, thanks

2

That suggests a major problem with the code, the motor wiring, or both.

PID tuning will not help at all, so the first step is to use a static test stand with the model in various fixed orientations. Put in print statements to see the conditions under which the orientation feedback and/or motor response is failing.

1 Like
3

Without an annotated schematic we can only guess at the hardware side, and I can conceive of a lot of combinations. For faster and more accurate help post the annotated schematic showing all connections, power, ground, power sources and any hardware such as resistors etc.

4

it's always more difficult and time consuming to test complicated code directly on the target.

have you tried building a simulation on your laptop and developing the code on the laptop.

5

Upon closer reading of the first post, I see you have some sort of static test stand. So that is a good start, you just need to spend some time studying and interpreting the output of debug print statements to see what is going wrong.

You can't expect forum members to make sense of the table posted under the comment "some CMD Outputs".

NOTE: PID does not perform well if the time interval between samples is not constant. Best to use standard implementations, instead of inventing your own.

    float timeChange = (float)(now - lastTime) / 1000.0f; 
    if (timeChange <= 0) timeChange = 0.00001f;
1 Like
7

Thanks for the reply, a few crucial information that I forgot to mention are that first, when I tested my drone on one axises at a time(RIG1 similar to top bar, only one axis are unrestricted), the drone was able to become stable on both roll and pitch axis(Both Pitch and Roll share the same PID values). But when I try to put my drone on the previously mentioned tuning rig(RIG2 Roll, Pitch, and Yaw are unrestricted), it becomes out of control(I hypothesize the drone only will spin on the Yaw axis, since both the Pitch and Roll axises are "Stable" on RIG1).

Today I try to tune the PID system on RIG2. The drone still pivots towards one of the motors and out of control. I don't know if it's due to the tangential velocity and centripetal force, but it was able to reach the desired angles after a while. Of course, shortly after it becomes unstable again. In the PID term, it takes a really long time to converge, but when it converges, it overshoots and therefore becomes unstable again. I think tangential velocity from the drone spinning in a circle acts as the I values in the PID systems, since actual I values within the systems are 0.

New PID values
Angle PID 12 0 0.0008 (-400,400 Min and Max for both output and integral windup)
Angle Rate PID 0.625 0 0.00002 (-400, 400 Min and Max for both output and integral windup)
Yaw Rate PID 0 0 0 (-400, 400 Max and Min for both output and integral windup)

8

Thanks for the reply, no I haven't built any simulation model on my computer, I was wondering if you could recommend me to some free Drone simulation model for me to test my Drone PID values on first. Thanks

9

Thanks for the reply. For the schematic of drone layout, it's the first image on this Link(With 4 motors instead of 2).

10

Do you have the propeller rotation adjusted? If so, please show me the diagram.

rotor4x
rotor4x263×259 20 KB

11

Check whether you are defining the Euler angles correctly and applying the derived rotational corrections in the appropriate order.

Twelve different Euler angle rotation systems have been defined and are in popular usage. Failure is guaranteed if you mix them up.

With the 6DOF MPU-6050 sensor, the yaw angle origin is not defined.

12

Top Left motor:CCW, Top Right motor: CW, Bottom Left Motor: CW, Bottom Right Motor:CCW, well I don’t think it matters since it’s an X configuration drone so yeah.

13

I think I define my angle correctly since I throughly tested out MPU6050, also will imbalanced props causes the drone to be unstable(I didn’t tested nor balance the props, I think with modern system, balancing props is more optional). Another problem I’m going to test out trmw is that, if my drone testing speed on RIG2 causes it to be unstable, since my RIG2 doesn’t have the ability to go up or down, it’s literally a ball bearing on top of a pole so yeah.

14

The order of applying Euler angle rotations has nothing at all to do with the MPU6050.

If you don't understand why that is the case, review 3D rotation theory and the various angular conventions.

Also, keep in mind that the complementary filter is useful only for approximately level flight, and is long obsolete. Good luck with your project.

15

i know of none

you would need to develop an environment for testing your code in simulation. you would maintain the code as it is used in the simulation to be capable of running on the target, but possibly adding hooks to obtain internal values and logs!

that code would run within an environment (a program on your laptop) along with a model for the droid that has the same inertia as the droid as well as a mechanism to induce disturbances in the model due to external factors such as wind, collisions and even failures of a motor.

yes, this would be an effort. you would learn a lot about what is going on within and external to the droid. the result would be a more stable, trustworthy and safe(!) control system

... in less time than by trial and error testing directly on the target

16

Hi everyone,

I’m currently building my own flight controller using an ESP32-S3, a logic level shifter, and an MPU6050. I’ve managed to get the drone to self-"balance", but I’m still having trouble fine-tuning the PID values and would really appreciate some help or guidance.

Here’s a breakdown of my setup:

  • The drone uses a cascade PID control system for the Roll and Pitch axes.
    • The outer PID loop takes the desired angle as input and outputs the desired angular rate.
    • The inner PID loop then takes this desired angular rate as input and outputs the PWM signal for the motors.
  • The Yaw axis uses a single PID controller that directly takes the desired angular rate as input and outputs PWM.

System details:

  • Drone frame: F450
  • Motors: A2122 930KV
  • ESCs: 30A Simonk
  • MPU6050 calibrated for both the gyroscope and accelerometer
  • Complementary filter applied to reduce vibration noise
  • Motors calibrated from 1000 µs to 2000 µs
  • MPU6050 runs theoretically at 250 Hz
  • The flight control and PID loops run at a tested frequency of 126 Hz
  • All PID outputs are clamped between -350 and 350
  • Final motor PWM outputs are clamped between 1150 µs and 1999 µs
  • All motors operate at a theoretical frequency of 500 Hz

Current PID values:

  • Cascade System:(Both Roll and Pitch PIDs share the same values)
    • Outer PID: P = 7.03, I = 0, D = 0.0088
    • Inner PID: P = 0.67, I = 0, D = 0
  • Yaw PID: P = 3.09, I = 0, D = 0.0001

Current behavior:
The drone is able to self-balance, but it still suffers from spinning around the yaw axis and occasionally losing control, but it is able to correct itself after a while.

Goal:
My goal is for the drone to be very stable, without spinning on the yaw axis or oscillating, and to maintain consistent control at all times.

Video Link

Thanks.

17

@ilovepizzaandpasta ,

Your two or more topics on the same or similar subject have been merged.

Please do not duplicate your questions as doing so wastes the time and effort of the volunteers trying to help you as they are then answering the same thing in different places.

Please create one topic only for your question and choose the forum category carefully. If you have multiple questions about the same project then please ask your questions in the one topic as the answers to one question provide useful context for the others, and also you won’t have to keep explaining your project repeatedly.

Repeated duplicate posting could result in a temporary or permanent ban from the forum.

Could you take a few moments to Learn How To Use The Forum

It will help you get the best out of the forum in the future.

Thank you.

18

Make sure you look at Kris Winer's MPU6050 and MPU9250 pages which discusses the calibration of the units as well as sensor-fusion (and has Arduino code to help)

The MPU6050 has factory calculated offsets you must read and apply as part of the device initialization - and that is just the starting point for calibration. You will want to ensure you have zeroed both the acceleration and angular rates AND you have calibrated the range properly for each axis (e.g. if using the 2g accel range, each axis has a range of -1 to 1 -- not -0.94 to 1.06)

Also be aware that there are differences in manufacturing between each unit and the MEMS sensors themselves. This is what the factory calibration values try and mitigate, but they are never 100% accurate. There are also units that just don't behave in one axis very well. I've probably seen 1 out of 5 that just isn't usable.

Take your time with the calibration of the MPU, getting it right is like getting the cornerstone in a foundation set properly. If it's off, the rest of the building is off. Similarly, if your MPU isn't calibrated in both value and range, your drone attitude control code will end of filled with a lot of ugly hacks to compensate for the bad data.

The MPU6050 datasheet and separate register reference manual are your friends. Kris's documentation helps put it all together.