# Stabilising a quadcopter using YPR

I'm using the MPU-6050 accelerometer + gyro with the library I2Cdev which outputs: quaternion, euler angles and YPR angles. The equations used for calculating the YPR are:

uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
data[0] = atan2(2 * q -> x * q -> y - 2 * q -> w * q -> z, 2 * q -> w * q -> w + 2 * q -> x * q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan(gravity -> x / sqrt(gravity -> y * gravity -> y + gravity -> z * gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan(gravity -> y / sqrt(gravity -> x * gravity -> x + gravity -> z * gravity -> z));
return 0;
}


I want to stabilize a quadcopter with these values and 3 PID regulators like this:

• FL = Throttle + (-PitchPID) + (-RollPID) + (+YawPID)
• FR = Throttle + (-PitchPID) + (+RollPID) + (-YawPID)
• RL = Throttle + (+PitchPID) + (-RollPID) + (+YawPID)
• RR = Throttle + (+PitchPID) + (+RollPID) + (-YawPID)

The pitch and roll values are between -90 and +90 degrees (0 degrees is horizontal and +-90 is vertical). The problem is that when the quad starts tipping over, the error will start decreasing and will stabilize upside down.

• Welcome to Robotics, VasilKalchev! If your quadcopter stabilizes upside down, it sounds like you have a sign error. How have you defined your axes? How did you setup your PID loops? Do you have $\mbox{Ref} - \mbox{Fbk}$ or $\mbox{Fbk} - \mbox{Ref}$? Does your quadcopter flip on only the roll axis, only the pitch axis, or does it flip along both axes? What troubleshooting steps have you taken so far? – Chuck May 24 '17 at 10:43
• I am only guessing that it will stabilize upside down. For the PID setup, I check if the pitch/roll is positive or negative, the PID input gets the absolute value, then I have an if that increments the throttle of a pair of the motors based on the sign of the input that I checked earlier. – Vasil Kalchev May 24 '17 at 12:17
• I don't think that the problem is in this part of the code, but with the lack of information that the MPU-6050 is returning. When the quad's pitch is above 90 degrees relative to horizontal, the MPU-6050's calculated value starts decrementing to zero. – Vasil Kalchev May 24 '17 at 12:23

You can't use an absolute value for the feedback in a PID controller.

Integral error accumulates the control error, which is the difference between what you want and what you have. So, consider a basic case where you want level flight (reference = 0).

If you take the absolute value of the feedback, then in ALL cases your feedback is either zero or positive. That means that the control error (reference - feedback), is ALWAYS NEGATIVE.

The integral error term can ONLY accumulate negative values. Typically you would overshoot the reference, at which point the sign would flip on the feedback and you could begin accumulating POSITIVE values, which would "bleed" the integral error term.

You have no means to reduce integral error. It will be a large negative number that will only ever get more negative (until you reset it or it overflows).

You can't just flip the sign on the output of the PID controller when a feedback signal changes signs because that doesn't do anything to correct the error terms that drive the controller.

Contributing to the problems you're having with the IMU are the facts that it looks like you're not converting from the quaternion to Euler angles correctly, and maybe also that you're not even getting a quaternion.

Any time you see an atan function your first thought should be, "Something's wrong." The atan function cannot differentiate between $(-y/x)$ and $(y/-x)$, so it can't always give you a correct solution.

I can see where you got the calculations and I understand that a sqrt() function will always return a positive value, but that's a problem too, isn't it? If your denominator is always positive then you know that you're never going to get a value of $-x$ and thus you know that you're not going to get a full range of angles from the calculation.

I would suggest using the code provided on Wikipedia if you're going to do this conversion yourself:

static void toEulerianAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
double ysqr = q.y() * q.y();

// roll (x-axis rotation)
double t0 = +2.0 * (q.w() * q.x() + q.y() * q.z());
double t1 = +1.0 - 2.0 * (q.x() * q.x() + ysqr);
roll = std::atan2(t0, t1);

// pitch (y-axis rotation)
double t2 = +2.0 * (q.w() * q.y() - q.z() * q.x());
t2 = t2 > 1.0 ? 1.0 : t2;
t2 = t2 < -1.0 ? -1.0 : t2;
pitch = std::asin(t2);

// yaw (z-axis rotation)
double t3 = +2.0 * (q.w() * q.z() + q.x() * q.y());
double t4 = +1.0 - 2.0 * (ysqr + q.z() * q.z());
yaw = std::atan2(t3, t4);
}


But again, back to the source of the quaternion - how are you getting that value? From the documentation page you linked:

Currently, the source code available will only provide basic device configuration and raw accel/gryo readings.

When I searched the PDF files at the bottom of the product's website, there were NO mentions of "quaternion" or "Euler" or "angle," so I'm not sure what the "motion processing" feature of the chip is or what it's supposed to do.

So, in summary:

1. Double check how/where/if you are actually getting a quaternion,
2. Check the math on how you are converting a quaternion to an Euler angle and verify that math checks out for all angles, and
3. Always use signed feedback for a PID controller.
• It looks like the quaternion is calculated in the DMP of the device. The library converts quaternion to euler using the equations you posted, but I am using the function that converts them to YPR because it gives better results (I think it does some sensor fusion). The calculations for the pitch/roll do return negative values. According to Wikipedia, arcsin and arctan return values between -90 and 90 degrees, so using the euler equations will also produce the problem I am asking about. – Vasil Kalchev May 24 '17 at 15:43
• @VasilKalchev - Can you find in the documentation where the DMP calculates a quaternion, or in which registers the quaternion is located? I could not. I understand the device advertises a motion processor, but I couldn't find anywhere in their documentation that states you can read anything other than accelerometer or gyroscope outputs. This would seem to be confirmed by the statement on the I2CDev website that states you can only get accelerometer and gyro outputs, which again leads me to ask, "What is the source of your quaternion?" Also, +/-90 degrees is not a full range of motion. – Chuck May 24 '17 at 16:27
• Even if I am getting perfect quaternion I then have to convert it. Converting it using the equations you posted or the equations that I use creates the problem that I am asking about. I know -90 to 90 degrees is not full range of motion, that's the problem. – Vasil Kalchev May 24 '17 at 16:33
• @VasilKalchev - Please post/link test data showing a series of quaternions that are the inputs to your function. We can compare outputs. I'll challenge you to consider the following point: if your problem were the limited domain ($\pm \pi/2$), then you would only have issues when you hit that limit, right? I think you are using PID wrong by taking the absolute value of the feedback, and probably also that you don't actually have a real feedback (because it doesn't exist). If your only question is about Euler angle conversions, then please let me know and I'll focus more on that. – Chuck May 25 '17 at 1:05
• Yes, the limited sensor output is the problem. I get your point about the PID, but I'm not doing the PID now. – Vasil Kalchev May 25 '17 at 7:11