# Quadcopter stable with angular velocity but not with absolute angle

I'm making a flight controller for quadcopter. It is very stable when regulated by angular velocity, but it is horrible when regulated by angle. What are the potential problems when regulating a quadcopter by angle?

## System description:

Input:

I'm using MPU6050 configured with low-pass filter for 185Hz cut-off with 2ms delay on both the 3-axis acceleration and angular velocity data. The angular velocity is filtered again with low-pass filter:

(AV = AVn * alpha + AVn-1 * (1-alpha)), alpha = 0.8


The accelerometer angle is calculated using:

pitch_a = atan2(x_a, sign(z_a) * (sqrt(sqr(z) + zqr(y) * 0.001))) * 180/pi;
roll_a = atan2(y_a, (sqrt(sqr(x) + zqr(z))) * 180/pi;


The angular velocity is integrated over the last fused angle:

pitch_g = pitch + av.y * dt;
roll_g = roll + av.x * dt;


And the resulting angle is fused with the calculated accelerometer angle using complementary filter with alpha = 0.98.

This is done with new sensor values every 2ms.

Regulation:

I'm using cascaded PI-P controller. The inner P controller's input is angular velocity and its output is motor power difference. The outer PI controller's input is absolute angle and its output is angular velocity. The outer controller is disabled when controlling the quadcopter by angular velocity. The inner controller runs every 2ms with accuracy of 8us and output limit of +-70. The outer controller runs every 8.5ms with accuracy of ~100us and output limit of +-250. I'm using this library, slightly modified to run when I call it, output integers, use microseconds and zero the integral sum when at very low throttle.

The setpoint is sent from the remote every 50ms, slightly filtered.

Output:

The output from the inner controller is added and subtracted to the according motor. The motors drivers' are controlled with 8-bit 490Hz PWM ranging from 127 to 254.

When controlling the quadcopter by angular velocity it flies stable with P value ranging from 0.2 to 0.8, noisy sensor input and even with one different motor. When controlling it with angle it behaves randomly. It starts to pitch or roll from level orientation, it seems to respond well to step input when it has to pitch or roll but returns to level very slow and overshoots... The angle control feels like a very bad angular velocity control (even tho the angle input is cleaner and it shouldn't have noticeable delay based on the fusion equation). The P range I have tried is approximately 0.5 to 2.0 and the I range is approximately 0 to 0.3. The inner controller runs 4.25 times faster the outer controller.

What can be the problem?

You could try using the MPU6050 integrated DMP for angle measurements. That will possibly help to locate the issue.

• Thanks for the advice. The DMP has smaller translation error, but overall it is comparable to the complementary filter calculations. – Vasil Kalchev Mar 13 '18 at 16:17
• What about convergence time? – Rafael Bachmann Mar 14 '18 at 14:52
• It is almost the same (unless the complementary filter's alpha is not too close to 1). – Vasil Kalchev Mar 15 '18 at 22:42
• Your answer helped me a lot. The DMP signal was cleaner, so I reduced the alpha on the accelerometer's low-pass filter to 0.15, which made the quadcopter as good as I think can be using complementary filter for sensor fusion. The problem that remains is that linear acceleration makes the gyroscope output false angular velocity which keeps the quadcopter in rotated position. – Vasil Kalchev Mar 15 '18 at 22:54

I feel you are feeding Gyro angle which is not under the control of complimentary filter, as accumulation of pitch and roll would have drifts which complimentary filter will not be able to recover. Hence my suggestion is to implement as below:

pitch = 0.98 *(pitch + av.y*dt) + 0.02*pitch_a
roll = 0.98 *(roll + av.x*dt) + 0.02*roll_a


I believe your accumulation (basically integration) is causing problem of drift. See if you can implement as above.

• Yes, it is implemented like this. – Vasil Kalchev Mar 12 '18 at 7:48
• Vasil, Based on equation that you wrote , pitch_g = pitch + av.y * dt; roll_g = roll + av.x * dt; This is different from what i suggested. As in your math, you are allowing gyro angles to get accumulated , whereas in math that i suggested , it accumulates with help of complimentary filter. So there is difference. Please write it down on paper and see. – Vinay Mar 13 '18 at 5:47
• One more suggestion is implement Quaternion, which will tell you behaviour in better way. – Vinay Mar 13 '18 at 5:50
• av.y is integrated over the last filtered angle pitch and then pitch_g is fused with the accelerometer angle as in your answer. I don't understand how I can use a quaternion to output a difference in motor power. – Vasil Kalchev Mar 13 '18 at 9:47
• You can refer to this : robotics.stackexchange.com/a/10374/13865 – Vinay Mar 13 '18 at 11:12