FW Position Controller: navigateWaypoint(): fix corner case of vehicle not turning back to waypoint #21635
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
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.
![image](https://private-user-images.githubusercontent.com/26798987/240613513-443a40e0-aad1-4d51-a0e5-3ec680cff222.png?jwt=eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJpc3MiOiJnaXRodWIuY29tIiwiYXVkIjoicmF3LmdpdGh1YnVzZXJjb250ZW50LmNvbSIsImtleSI6ImtleTUiLCJleHAiOjE3MjQ2OTYyMTEsIm5iZiI6MTcyNDY5NTkxMSwicGF0aCI6Ii8yNjc5ODk4Ny8yNDA2MTM1MTMtNDQzYTQwZTAtYWFkMS00ZDUxLWEwZTUtM2VjNjgwY2ZmMjIyLnBuZz9YLUFtei1BbGdvcml0aG09QVdTNC1ITUFDLVNIQTI1NiZYLUFtei1DcmVkZW50aWFsPUFLSUFWQ09EWUxTQTUzUFFLNFpBJTJGMjAyNDA4MjYlMkZ1cy1lYXN0LTElMkZzMyUyRmF3czRfcmVxdWVzdCZYLUFtei1EYXRlPTIwMjQwODI2VDE4MTE1MVomWC1BbXotRXhwaXJlcz0zMDAmWC1BbXotU2lnbmF0dXJlPWQ5YmYyOTIyYTM5MzFmMDcxMDc2MDIxNDE1NTkxNTdlOWZiYjEzYzQ3MTI4ZDY3N2MzZTdkNDU5NWMxNGVjZjkmWC1BbXotU2lnbmVkSGVhZGVycz1ob3N0JmFjdG9yX2lkPTAma2V5X2lkPTAmcmVwb19pZD0wIn0.YUA0r0rNNQO6fg8T3KgDE6ue8yH4sAVMxqqkd5cMLzo)
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: