Smooth yaw in auto mode by limiting acceleration

[dawr68]

Solved Problem

Currently, only a slew rate is applied to the yaw setpoint, which causes issues on highly responsive platforms (e.g., helicopters) due to steep changes in the yaw setpoint.

Solution

Smooth the yaw setpoint by limiting the yaw acceleration.

Results

Applying an acceleration limit leads to more realistic yaw and yaw rate setpoints, enabling improved yaw tracking performance.

[MaEtUgR]

I rebased and added my suggestions. I also did some testing in SITL SIH and realized we should adjust the default parameters such that this change shows the significant improvement I was looking for. The current defaults only make sense with the current limited implementation that doesn't consider acceleration.

EDIT: I checked the rough timing for a 180° turn which is likely the worst case for an autonomous yaw change (otherwise you start to rotate the other direction) and assuming the drone could accelerate indefinitely with the previous slew rate it would by default turn in (180°) / (45°/s) = 4s which it of course never managed and overshot so it probably took ~5-6s. If we have a high (100°/s) limit with the new implementation but limit the acceleration to have a smooth turn it roughly results in the following timings if it can track the demanded yaw acceleration:

Acceleration	Time to turn 180°
5°/s^2	~12s
10°/s^2	~9s
15°/s^2	~7s
20°/s^2	~6s
25°/s^2	~5s
30°/s^2	~4.8s
35°/s^2	~4.65s
40°/s^2	~4.38s
45°/s^2	~4.17s
50°/s^2	~4s
55°/s^2	~3.78s
60°/s^2	~3.55s

I suggest to go with 20°/s^2 for a start and adjust that in case we see undesired either too slow or too aggressive yawing.

[MaEtUgR]

I added the HeadingSmoothing also to the Orbit flight task because it's the only other place that uses the maximum autonomous yaw rate and still has a slew rate. And then suggested the 60°/s and 20°/s^2 defaults. The acceleration could possibly still be increased a bit if we deem it not quick enough but I kept it relatively low such that even with not-so-great yaw tracking you have minimal overshoot.

[dawr68]

Test flown today on a 10" multicopter. The results are promising, however we can possibly increase default yaw acceleration as the vehicle didn't reach maximum yaw rate.

With proposed default values, the vehicle was smoothly tracking the trajectory and adjusting heading.

[MaEtUgR]

@dawr68 Thanks for the real world testing you provided. Now that you checked again and tested I feel confident to merge. If the defaults turn out to be limiting in some use case I'm happy to follow up with adjustments.