fix navigator bug: "Takeoff to x meters above home." after mission ended #14020
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.
Fixes #14011
The problem described there actually goes deeper: the message is also sent when switching to mission mode while disarmed and with a valid mission. Even worse: if the first mission item is a command, it will be published at the wrong time.
The root cause is that the (mission) state machine is executed too early/at the wrong time. This is fixed with this PR by going into navigator idle mode while disarmed (i.e. not entering mission until armed).
All existing arming checks in navigator are unaffected by this, except that there is a slight risk that if takeoff is detected immediately upon arming, the first takeoff waypoint is converted into a position waypoint (however the position controller has its own takeoff logic anyway).
Another consequence is that mc_pos_control starts to complain about invalid setpoint triplets while disarmed. The PR resolves this by also accepting invalid triplets and interpret them as loiter.
The risk with this is that it could hide problems, where navigator sends invalid triplets when it should not.
Best reviewed by commits.
This has been quite extensively SITL + flight tested, MC + VTOL. Thanks to @julianoes and @sfuhrer for the support.
Log: https://logs.px4.io/plot_app?log=66a6062f-cbdb-47cd-83f3-fc71724b58c5