FW Position Controller: navigateWaypoint(): fix corner case of vehicle not turning back to waypoint

[sfuhrer]

Solved Problem

Fixes #21327

Vehicle fly away when the currently active WP is the current vehicle position, relative to the line from previous to next waypoint. That happens e.g. when the vehicle loitered to achieve altitude and is then "passed" the current WP when the altitude is accepted.

Solution

There was already logic in place that handled the case of the vehicle being behind the previous WP, but no one where the vehicle is ahead of the current WP. I added that now. The logic changes the guidance from tracking a path between two WP to fly directly to the active WP.

Further, in the second commit I removed the check for "if further than acceptance from waypoint" to apply the special handling. It seems dangerous to me to keep it, what happens if Navigator doesn't accept the WP once within this acceptance radius? In my eyes we should always fly straight to the waypoint instead of doing path following in this case. EDIT: removed this change again.

Changelog Entry

For release notes:

Bugfix FW Position Controller: navigateWaypoint(): fix corner case of vehicle not turning back to waypoint

[tstastny]

Ok for now - let's try and schedule an audit of our current navigator fw pos ctrl waypoint handling distribution for the next release to nail down a bit clearer interfaces, and also exactly what behaviors we want to write down as requirements.