Need help with Quadcopter PID and PWM

Hello everyone,

I need some help understanding how PID output and PWM pulse affect quadcopter angle. I understand that PID needs to be calculated for yaw, pitch and roll angles.

Basically PID error output = desired angle - measured angle. I get measured angles (yaw, pitch, roll) from either
a complimentary filter/Madgwick AHRS.

  1. How do I calculate desired angles from the joysticks of and RC controller because it gives values from 0-1023 or a PWM pulse between 1000-2000 uS?? Or should I set desired ANGLE in PID Setpoint but PID controller has no Unit also?

  2. What should be the maximum angle of yaw/pitch/roll of a quadcopter before it stalls and how do I code it?

  3. Considering the following code for sending PWM pulses to the ESC :

esc_1 = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW)
esc_2 = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
esc_3 = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW)
esc_4 = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW)

What will happen if throttle is max at 2000, above that a pid output is added, the final pulse will cross beyond 2000 uS, so how do I control that?

  1. How does the PWM pulse to each esc or rather the motor speeds affect the quadcopter desired angle?

Hello,

I am with a similar problem with a pid for a quad. As far as i read you will need two pids per axis
https://forum.arduino.cc/index.php?topic=455500.0

Hope it add something to your project.

I am on the same strugle haha

The basis of quadcopter stabilisation is the 3-axis gyroscope measuring the rate of rotation, for example degrees per second for roll, pitch and yaw axes.

The most basic multi-rotor mode is known variously as either rate, gyro or acro mode and only uses the gyroscope for flight. The input pulses from a standard RC receiver channels (airelon, elevator and rudder), which usually range from 1100us minimum through to 1900us maximum at around 50Hz, are converted (using Arduino’s map function) to ± the rate of rotation, be it degrees per second, radians per second, or whatever you’ve chosen. This pilot input acts as the setpoint to your roll, pitch and yaw rate PIDs. The measured input to your PIDs comes from your gyroscope also for example in degrees per second.

After multiplication by an appropriate gain factor the PIDs’ outputs are mixed as you’ve described above, with the resultant roll, pitch and yaw outputs being added to and subtracted from the throttle. To find the correct PID gain values for the rate PIDs it’s necessary to first test fly the quadcopter using rate mode before progressing further. Setting the pilot’s maximum input rate to around ±180°/s to ±250°/s for roll and pitch and around ±270°/s for yaw for a full stick deflection is a good starting point.

Rate mode won’t automatically level your copter, but does provide the most freeflowing flight from a pilot’s perspective.

To automatically level the quadcopter known variously as auto-level, stabilize or angle mode, requires it to know which way is up. This is achieved using a 3-axis accelerometer that measures gravity. This time the pulses from the RC receiver’s roll (airelon) and pitch (elevator) channels (pilot’s input) are converted to an angle, say ±45°, (again using the Arduino map function). Yaw is still controlled using rate, say degrees per second as before. The roll and pitch inputs act as the setpoints to the corresponding outer roll and pitch angle PIDs, whose outputs are then in turn used as the setpoint to the inner rate roll and pitch control loops. The measured input to these two outer PIDs comes from an estimation of your roll and pitch angle (in degrees). This is achieved by fusing the angle calculated from the integral of the gyroscope (degrees/s * time = degrees) with the angle calculated from your 3-axis acclerometer outputs. As you mention, it’s possible to do this using a complementary or Madgwick filter.

The reason for fusing the gyro and acceleromter outputs is that the gyroscope is stable in the short term, but drifts over time, while conversely the accelerometer is noisy in the short term, but gives a stable angle estimation over time. By combining the two you can overcome their deficiencies.

The effect of auto-level mode is to automatically level the quadcopter when your sticks are released.

There are two ways to account for the maximum throttle. The traditional way has been to limit the throttle to say 90%, thereby giving your motor mixing some headroom, when adding the roll, pitch and yaw from the PIDs. The latest method is to detect when amount by which the motor mixing exceeds the maximum throttle, then reduce the throttle to allow the addition of the PID outputs to stay withing the motor’s maximum.

Finally, I’d suggest using multi-rotor specific ESCs, or at least using those flashed with multi-rotor firmware and updating them at a frequency greater than say around 200Hz or more. Those used for planes or helicopters have an input filter that will adversely affect the quadcopter’s stability.

MartinL thanks for the clarifications.

I am working with that and i have this code, would you mind checking it:

Wire.beginTransmission(MPU);
  Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); //A partir del 0x3B, se piden 6 registros
  AcX = Wire.read() << 8 | Wire.read(); //Cada valor ocupa 2 registros
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();

  //A partir de los valores del acelerometro, se calculan los angulos Y, X
  //respectivamente, con la formula de la tangente.
  Acc[1] = atan(-1 * (AcX / A_R) / sqrt(pow((AcY / A_R), 2) + pow((AcZ / A_R), 2))) * RAD_TO_DEG;
  Acc[0] = atan((AcY / A_R) / sqrt(pow((AcX / A_R), 2) + pow((AcZ / A_R), 2))) * RAD_TO_DEG;

  //Leer los valores del Giroscopio
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 4, true); //A diferencia del Acelerometro, solo se piden 4 registros
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  //Calculo del angulo del Giroscopio
  Gy[0] = GyX / G_R;
  Gy[1] = GyY / G_R;

  //Aplicar el Filtro Complementario
  Angle[0] = 0.98 * (Angle[0] + Gy[0] * 0.010) + 0.02 * Acc[0];
  Angle[1] = 0.98 * (Angle[1] + Gy[1] * 0.010) + 0.02 * Acc[1];

  InputxGyro = Angle[0];


  double gapx = SetpointxGyro - InputxGyro; //distance x away from SetpointxGyro

 
  //Ajusta x
  myPIDxGyro.Compute();

  SetpointxAcc = OutputxGyro;
  InputxAcc = AcX;

  myPIDxAcc.Compute();

   potenciaxpos = constrain(potenciaGeneral + abs(OutputxAcc), 1001, 2000);
    potenciaxneg = constrain(potenciaGeneral - abs(OutputxAcc), 1001, 2000);


    if (OutputxAcc < 0) {
      motor2.writeMicroseconds(potenciaxpos);
      motor4.writeMicroseconds(potenciaxneg);
    }
    if (OutputxAcc > 0) {
      motor4.writeMicroseconds(abs(potenciaxpos));
      motor2.writeMicroseconds(abs(potenciaxneg));
    }

  delay(25); //Nuestra dt sera, pues, 0.025, que es el intervalo de tiempo en cada medida

Hi rossi86m,

In principle your code looks to be on the right track. The equations to convert the accelerometer outputs to angles seem to be correct.

To get a good auto-level you'll also need to account for how the yaw motion couples with roll and pitch axes in the complementary filter. For more information check out Joop Brokking's excellent auto-level tutorials on Youtube: MPU-6050 6dof IMU tutorial for auto-leveling quadcopters with Arduino source code - YouTube.

Might I suggest using either analogWrite() or better still hardware PWM for your motor outputs, as the servo library only operates at 50Hz, which isn't fast enough for multi-rotor ESCs. I'd also remove any delay() functions to run the loop() function at maximum speed.

MartinL:
Hi rossi86m,

In principle your code looks to be on the right track. The equations to convert the accelerometer outputs to angles look good.

To get a good auto-level you'll also need to account for how the yaw motion couples with roll and pitch axes in the complementary filter. For more information check out Joop Brokking's excellent auto-level tutorials on Youtube: MPU-6050 6dof IMU tutorial for auto-leveling quadcopters with Arduino source code - YouTube.

Might I suggest using either analogWrite() or better still hardware PWM for your motor outputs, as the servo library only operates at 50Hz, which isn't fast enough for multi-rotor ESCs. I'd also remove any delay() functions to run the loop() function at maximum speed.

Thanks for the advice i will modify that and see how it works

But for a theorical perspective what i do not understand is this
SetpointxAcc = OutputxGyro;
InputxAcc = AcX;

In fact OutputxGyro is in degrees and Acx is un degrees/s. How those work together?

Thanks,
Marco

MartinL,

Thank you... Great answer, right on point. Will implement it in code.

One more question : Do I convert RC input to angles (degrees) or degrees per second (by dividing it with delta t)? Also what should be the maximum angle/angular rate I should map the RC inputs to: +/- 90, +/-180 or some other values?

Also have I got it correct: There should be two loops in the PID, the inner controls the yaw using angular rate and the outer two loops control the pitch and roll in angles? I have seen this advice somewhere else too where they are telling to use cascaded PID.

But what should be the output from these two different PID loops because the units are different: the inner uses degrees/sec and outer uses degrees. So how do I set the setpoints in such a scenario?

Hi Rossi,

I would suggest you see this entire series of Joop Brokking:

I learnt a lot from his videos.

The code you have put up here needs a lot of changes. Check Joop Brokking’s website, download the v2 code and study it before you start writing you own code.

Hi kaustavthakur,

One more question : Do I convert RC input to angles (degrees) or degrees per second (by dividing it with delta t)? Also what should be the maximum angle/angular rate I should map the RC inputs to: +/- 90, +/-180 or some other values?

No, just use the Arduino map function to generate the range you require from the RC receiver inputs. For rate mode say ±180°/s for roll, pitch and ±270°/s yaw and for auto-level use ±45°:

if (flightMode == STABLE)
{
  rxRoll = map(airelonPulsewidth, 1075, 1869, -45, 45);
  rxPitch = map(elevatorPulsewidth, 1088, 1880, -45, 45);   
}
else
{
  rxRoll = map(airelonPulsewidth, 1076, 1872, -180, 180);
  rxPitch = map(elevatorPulsewidth, 1088, 1889, -180, 180);
}
int rxYaw = map(rudderPulsewidth, 1076, 1873, -270, 270);

Also have I got it correct: There should be two loops in the PID, the inner controls the yaw using angular rate and the outer two loops control the pitch and roll in angles? I have seen this advice somewhere else too where they are telling to use cascaded PID.

Yes, three inner rate PID control loops for pitch, roll and yaw and two further outer angle control loops for pitch and roll only, yaw continues to use only the rate control loop, as you can't estimate yaw angle (heading) without a compass (magnetometer). The output of the outer control loops act as the set points of the inner.

But what should be the output from these two different PID loops because the units are different: the inner uses degrees/sec and outer uses degrees. So how do I set the setpoints in such a scenario?

I pondered on this question for quite a while, but the answer is that the PID control loop doesn't know or care what units you're working in, so long as the set point (pilot input) and measured input (gyroscope) are comparable. As the output is scaled by your control loop gain, it can be in whatever units you want it to be, so long as the output is within the expected range of whatever you're trying to control.

For example your outer loops take the desired input roll and pitch angle from the pilot, (where you want to be) and compares to your measured input from you angle estimation, (where you are now). If we only consider the proportional gain, in other words the P bit of the control loop then we simply subtract the gyroscope from the pilot input and multiply by the P gain. The outer control loop doesn't know what units the output is in, you're simply scaling the gain until the output meets the expected range of whatever you're controlling, in our case the set point of an inner PID loop. The inner control loop compares this value with the measure rate from the gyroscope. Likewise the units output from the inner control loops happen to be values in microseconds (µs) that are added or subtracted during motor mixing.

Personally, I'd start off by implementing rate mode. Converting the pilot input to degrees per second, or whatever units you prefer, using only the gyroscope as the measured input and implement the inner control PID loops. The tricky bit is finding the correct gain values for your PIDs, although initially you should be able to get the quad airborne using only the P gain, then add I (Integral) gain afterwards.