Copter: improve yaw in Circle mode and when using ROI

[rmackay9]

When using Circle mode or ROI, we want the copter's nose to point at the center of the circle or at the ROI point (unless a 3D gimbal is attached in which case we want to point the camera at the point).

The current code attempts to do this but some users have reported that the vehicle's nose does not point exactly at the point but instead lags slightly. They have also reported that yaw/heading error becomes worse the faster the vehicle moves.

This can be reproduced in SITL by doing the following:

• create a mission as shown in the screen shot below (LOITER_TURNS with no lat/lon specified)
• arm and takeoff in Guided, then switch to Auto
• vehicle will backup (by CIRCLE_RADIUS) and then begin rotating around the point

As shown below, the vehicle is circling around home but it's heading does not point directly at home

The underlying issue is that the yaw controller is constantly playing catch-up with the target point so we need to enhance the yaw controller so that it accepts both a target yaw (like it does now) and also a desired yaw rate. This new desired yaw rate should then be added to the output of the yaw angular P controller and fed into the lower level yaw rate controller.

The desired yaw rate that we provide to this enhanced yaw controller depends upon whether we are in Circle mode or doing an ROI. If we are in Circle mode then we should pass in the target rotation rate around the circle (i.e. CIRCLE_RATE). If we are doing ROI then a slightly more complex calculation is required based on the vehicle's target horizontal velocity vector (not its actual velocity vector except in manual modes where we don't have a target) and the position of the vehicle and the ROI point.

To provide some more background, the yaw controller consists of two nested PID controllers. The upper angular controller (a simple P controller) and then a lower level (PID) rate control.

We can imagine that if the vehicle is circling around a target point (like it does in Circle mode) then the vehicle's yaw rotation rate should be the same as the vehicle's rotation rate around the circle. Because of the nested PID controllers though, the vehicle's yaw rotation can only be induced by a yaw error (a difference between the target yaw and the vehicle's actual yaw) thus there must be a yaw error, the vehicle definitely will not point exactly at the target.

@amilcarlucas, this is the underlying issue causing the slightly incorrect heading that we discussed at the developer unconference.

[rmackay9]

Upon further investigation I've found that Circle mode was using a simplified calculation for the yaw. This PR greatly improves the heading: https://github.com/ArduPilot/ardupilot/pull/11191.

@amilcarlucas, can you confirm that you saw a heading error when using ROI as well? Or was it just in Circle mode / Loiter_Turns that you were seeing issues?

[amilcarlucas]

Yes I was using ROI when I saw the error. Do you think the error whould be smaller if I did not set a ROI at the center of the circle?

[rmackay9]

@amilcarlucas, with the new PR linked above I think the performance will be slightly better without the ROI command. The only difference is that ROI updates at 100hz while Circle's built in heading calculating updates at 400hz. We could/should make the ROI command work as well by removing this counter which is only there to reduce some CPU overhead.

Do you think you could test it to see if it's better? Alternatively if you have a log of the issue I think we should be able to see that the ATT.Yaw is always slightly behind ATT.DesYaw and I think this is caused by the missing rate feedforward that I discuss at the top of this issue.

[amilcarlucas]

The counter that you are refering to is kind of addressed by #11172 :) If we change it to 400 Hz I do not need to fix #11172 :)

[rmackay9]

@amilcarlucas, cool, I'll get rid of that slow down code for ROI..

[amilcarlucas]

The Yaw controller is now running at 400Hz, I need to re-test this one in SITL