Developer

### ArduCopter 2.9 PID Loops for STABILIZE, ACRO and ALT_HOLD

Hi all,

I thought I would attempt to draw up a couple of diagrams of the PID loop in the ArduCopter 2.9 release.

Alt Hold Controller

What we are trying to do with this controller is remove the need for the different stages of altitude changing that was in there before. This controller can handle both the altitude hold function as well as the fast altitude changes. We have tested it up to 5 m/s. This lets us do the very fast reduction in altitude I think you were asking for. It also does this with very little overshoot.

We use the square root of distance relationship to slow the airframe at reasonable accelerations to remove the very large accelerations that a linear distance vs velocity relationship applied. We took this one step further by combining the linear relation P loop with the square root relationship. This provides a smooth transition between the two modes in both acceleration and velocity.

The linear distance calculation is the distance that the square root relationship needs to be moved in order for the linear velocity relationship to be a tangent to the square root velocity relationship. This provides a step free transition between the two at the linear distance x 2. Acceleration of the two curves is also equal at this distance meaning that the motors don't pulse as we swap between the two curves.

The graph below is for the default Alt Hold parameters.

The 250 cm/s/s is the maximum acceleration that will be required of the copter while still allowing the copter to stop without overshoot. The copter will not reach these accelerations unless it is instructed to travel over approximately 125 cm/s. This shouldn't happen during Alt Hold but can happen during Altitude changes. This limit was chosen because it should be achievable for all reasonably designed copters. To not be capable of this acceleration the copter would have a cruise throttle of over 700.

Using this controller you can still move the requested altitude slowly however if we need to stop or change direction it is able to still follow that request. In fact, the controller is not limited in the acceleration it can ask for. In an emergency this controller will be able to do it in less than 1 second and 2.5m. (I realize these are simple calculations and this situation only occurs if the user asks for stupid things).

We now have different speed limits for up and down but currently they are set to the same thing. We did this because the controller can now handle much faster rates so we didn't think we needed to limit it to such small values going down. I assume this was done for safety.

The final thing I should mention is the design of the controller with respect to the pid loops. This is a (P/sqrt) -> PD -> PI (Position -> Velocity -> Acceleration) controller by default. However, there may be advantages to using (P/sqrt) -> PD -> PID with correct filtering.

So why no I term in position. This is because the I terms in these loops will reduce the following error when using this controller with a moving set point. The current design of the controller ensures that the distance between the moving altitude request and the current altitude is enough for the controller to stop the aircraft in that distance without accelerating beyond 250 cm/s/s. This is how we achieve such a small overshoot from very fast decent rates. This is also why we don't use an I term in the Velocity controller, we don't want overshoots due to I term build up.

The other parameter that is critical in this function is the velocity D term. This has the effect of minimizing the error between the requested velocity and the desired velocity during deceleration. This is set as large as it can be without noticing any jerkiness or oscillation during altitude changes.

I hope this helps people understand how a number of these control loops work.

Enjoy,

Leonard

• Hi guys! I would love to understand the earth frame controller. There is still one thing which I can not understand properly.

I checked code and also all documentation which I could find. There is written  for earth frame controller that they use ROLL,PITCH,YAW, this three angles are extracted from DCM matrix on the line:

https://github.com/diydrones/ardupilot/blob/master/libraries/AP_AHR...

the function name is  "_body_dcm_matrix.to_euler" what I am not sure is completely right.

Euler angles are rotations around already rotated axis it means: first rotate yaw around Z axis, than rotate pitch around NEW Y(rotated) axis and roll around NEW X(rotated) axis.

ROLL/PITCH/YAW are rotations around earth axis - axis are not changing.

Nice animation which shows it really good:

but still I am not sure which interpretation is correct.

I will summarize my question. The yaw pitch roll angles used for Earth frame controller are rotations around earth frame or around body frame?  (check video i posted here)

Thank you for any kind of response!

• hi,

the work you have done is very impressive it will be more helpful to understand the inner functions of controller if you share the matlab code for the functions you have used in your controller?

thanks.

• Developer

Opps, the D terms are all zero in the Alt Hold controller.

Sorry

• Developer

Hi Baris,

Currently the D terms are all zero and I have not seen any need to change this so I believe we will remove the D term in the future.

However, this is present in a number of controllers and we generally handle this by attempting to not change the setpoint instantly.

The improvements to ensure smooth operation and avoiding derivative kick is a gradual process though because the code is not structured to do this easily yet. The next version 3.2 will be a major rewrite of the control code and will have a couple of steps in the right direction in this regard. It won't be until 3.3 will be where things start to get very polished though.

The other thing to keep in mind is the limited processing capabilities of the APM. The PX4 based boards will let us go much further.

Finally we need to ensure that the controllers don't become difficult to set up for the average Joe! This is always a major consideration when we are designing the control loops!!

• Hi Leonard,

From the PID schematics I understand that D term in acceleration is calculated from error which means it causes derivative kick when you change the setpoint. so how you are handling this derivative kick issue

• Hello Leonard,

It's quite long this post is not active, but still I can not find any other Stabilize PID schematics. Trying to understand better the control loops and wanted to ask what is the difference between roll attitude(degx100) and roll rate(degx100/s)? I read that rate data is raw gyro data. Is it the same data, just differentiated?

Thanks

• Francois, I initially tried a yaw controller without the feedforward, and while it worked OK and was precise for aerial photography applications, people found that it was "laggy" and didn't like the feel.  Basically, the yaw didn't move until an error had built up, and then didn't stop until the error was zero. The feedforward sharpens it up.

• Developer

Hi Francois,

The feed forward ensures that you get instant response from the copter when you push the stick. If we just moved the desired heading we would find that even if we pushed maximum stick the copter would initially start to move slowly until it fell far enough behind the desired point for the stabilize controller to request the same rate as the pilot. Then when we let go of the stick the copter would continue to move because the desired point is further in front.

By using the feed forward the stabilize portion of the loop only deals with external disturbances and when the pilot requests rates or accelerations that the copter can't achieve.

Does this make sense?

• Leonard

May I ask what the rationale is behind the feedforward link in the first part of the yaw control?  It does not quite make sense to me.

Francois

• Developer

Gutzmann, I am very interested in how you have done the simulink simulation!

Jack, We are getting a very good position out of the accelerometer integration provided there isn't massive amounts of noise. The integration is quiet good at ignoring the noise. Even the Alt Hold that uses acceleration filtered at 2 Hz doesn't suffer noticeably from noise.

Thanks everybody for their feedback!

Leoanrd