Waypoint Turn Smoothing not reliable enough

[b14ckyy]

Current Behavior

Navigation Turn Smoothing is a very nice feature and in conjunction with the WP Path Tracking it allows much greater precision for fixed wing navigation, but in its current state, the WP turn smoothing is not working well at all. Very tight angles are completely ignored and flown as if no smoothing is on. Angles bigger than 90° initiate the smoothing turn just a few meters before the waypoint, making the plane twitch in one direction for a second and then fly the turn unsmoothed, nevertheless.

Here are a few samples of the exact same mission, no wind in the HITL Simulation but the behavior is exactly the same in the real world.

WP Tracking on, turn smoothing off:

WP Tracking on, turn smoothing on

WP Tracking on, turn smoothing on-cut

WP Tracking off, turn smoothing on

Here is the mission as reference:

The behavior of the smoothing algorythm is identical on all waypoints no matter what setting is set. I set the WP Radius down to 150cm as this is also Firmware default for this test but even with a bigger radius this makes no difference.

WP1: is smoothed a little bit sometimes, depends on the approach angle after launch
WP2: smoothing kicks in just about 10m before the actual WP position, has no real effect
WP 3-5 turn smoothing is completely ignored and the WP is just passed right through the center
WP6 works best so far, although not perfect, starts the smoothing turn and also the cut turn just a little bit late
WP7 is like WP2, a short twitch to the left but then flown like smoothing would be off
WP8 also initiates the smoothing turn too late but it does smooth it, just not as good as WP6
WP9 is a RTH WP and does actually initiate the smoothing at a good distance, just gets overwritten by the RTH climb first then, that's expected

Steps to Reproduce

1. Plan a WP mission with different angles and long enough legs to allow the plane to settle the course
2. Run the mission multiple times and watch how each turn is executed

Expected behavior

WP with very big angles between 120-180 degrees should not smooth at all or always CUT the corner. that makes sense as it allows a direct entry onto the next leg path. All smaller angle turns should properly execute the smoothing turn outside of the waypoint to have a smooth entry into the next leg for proper path tracking.
For the WP Cut feature, this should always be identical on all WP angles and also work reliably.

Suggested solution(s)

maybe its a bug in the code that has to be fixed? I am not sure. Bit this should be looked at again before the 6.0 RC phase.

Additional context

tagging @breadoven as this was his feature
Maybe we can also look again into the WP Path tracking. I found no way on any real plane or the simulator, to eliminate the overshoot. It is always there unless I set the tracking precision to 10 and the approach angle to 80° but then the whole tracking is basically useless in wind. May be nice to get this big overshoot reduced by aligning to the path earlier. The WP turn smoothing though is the bigger problem here.

[breadoven]

The smoothing is a bit a hit and miss. It should be noted it only works for turns between 30 and 120 degrees. 30 degrees was used simply because it didn't seem there was enough time to make such a shallow turn for the smoothing to make much difference. This could be reduced though to see if it improves things. The 120 degree limit was used because most planes will struggle to turn so tightly without overshooting anyway. If you use the CUT option it will turn very early missing the waypoint completely. The theory behind the smoothing is sound but the reaction and control in the short time available to make the turn is the problem (PosHold turns take time to settle down even without wind). Adding a "fiddle" factor to start the turns a bit earlier might be the easiest solution. I'll have a play around using the HITL sim, saves burning up batteries.

As for the Tracking Accuracy I'm not sure it's really possible to eliminate the overshoot entirely, just too many unpredictable factors at play, wind being the biggest. Applying some filtering or using PID control might help but I'm not sure it'll make it behave perfectly even then.

[b14ckyy]

regarding the angles.
For the CUT option I thought the algorithm will take the turn radius into account to decide when the cut turn starts, so the end of the turn will end up right on the next leg if possible? I mean we have 2 tangents and therefore the ideal turn point to cut the corner, But I agree that the trurn point distance from the WP has to be limited to not turn too early. Maybe the earliest turn point should be 1x loiter radius away from the WP. Then only in turns smaller than than 90°angles (very tight turns) would overshoot the target leg but all bigger angles would have nice cuts directly into the final leg.

On the other hand, the Normal smoothing that makes a loiter turn around the WP as a center point would work even on very tight turns close to a 180° direction change.

The question is, what the target for the WP Turn smoothing is. Should it just reduce overshoot a bit to look cleaner or should it work in conjunction with the Path tracking to fly most efficient waypoint missions.

In my opinion, if someone uses the Path Tracking feature, their goal is not to have waypoints at very specific positions that have to be passed but instead the Tracks are the actual target. And to allow that to happen as precise as possible, symmetrical turn paths are more important than hitting the actual WP.

So for the smoothing, my suggestion is to not fix the smoothing variant but just make it dynamic. Tight turns with lower than 90° angles use the outside turn, and shallower angles bigger than 90° use the CUT option, with the tangential angle of the loiter radius to determine the turn point as in the image above (blue lines).
With this option there is no angle limit needed. Of course wind will still play a role and mess with the results but this is something that can be addressed with nav system updates in the future.

---

regarding the path tracking overshoot. I don't know how the math behind that works right now. But what if you dynamically reduce the approach angle based on the distance from the path with again loiter radius as a reference distance? So with a loiter radius of 70m, and nav_fw_wp_tracking_max_angle=60, the 60° angle will be used if the distance to the target path is greater than 70m. If the plane gets closer the path the nav_fw_wp_tracking_max_angle get dynamically reduced down to minimum when the distance is 0. This should dampen the approach and make the plane align correctly before overshooting.

[breadoven]

The main problem with the smoothing option is a delay in the turning reaction which means the plane doesn't start to turn for some distance after the theoretical turn initiation point so the turn doesn't establish itself fully until it's too late causing overshoot. You need to initiate the turn earlier to compensate for this. I've tested the outside turn with an earlier initiation and it works better so I think I'll work out way of factoring this in. I'm not sure about the Cut turn though. It causes problems detecting the WP has been reached if you try and initiate it earlier because it misses the WP by too great a margin even for detection based on bearing to the WP. Might be better to remove it if it isn't possible to get it to work more reliably.

The tracking accuracy already reduces the approach angle to the course line as you get closer to it due to the way it works. You'll only get the nav_fw_wp_tracking_max_angle value when you're some distance away. The method used calculates 2 errors, distance to course line and a heading error based on the current plane heading relative to the course line heading (error = 0 when the plane is aligned with the course line). These errors are added together to give an overall error correction. The 2 errors oppose each other, i.e. the distance error tends to increase the angle of approach to the course line and the heading error tends to reduce the angle of approach to the course line. The overall error is a balance between the 2 with a bias toward the bigger error at any given moment. If you're aligned on the course line both errors should be zero.

I think given the improved accuracy in actually following the waypoint course line the overshoot isn't that significant really. I'll have a look at improving it all the same.