navigator: fix triplet resetting/publication logic #13534
Conversation
The navigator has a notion of resetting the triplets which means the triplet setpoints are set to invalid and therefore not to be used by downstream modules such as flight tasks. However, before this patch, the triplets were not published if invalid meaning that a valid triplet would stay the truth until a new valid triplet would get published. E.g. this lead to the corner case case where we publish a valid triplet with an IDLE setpoint on ground in HOLD and then don't update the triplet while flying in POSCTL mode. Later, when RTL is engaged, the flight task will use IDLE until navigator (which runs slower) has published the next triplet. The fix involves two main changes: - Publish invalid triplets to avoid stale triplets. - Avoid publishing the triplet on every iteration in manual modes by not setting `_pos_sp_triplet_published_invalid_once = false`. When testing this I realized that a mission upload during RTL would stop RTL. This is because the mission is updated while the mission navigation mode is not active and reset_triplets() is called from there. This is now only done for the case where we are actually in mission navigation mode. The fact that a mission is updated when not active also seems wrong and is something to fix another time.
|
@PX4/testflights please test this. In particular test the following scenarious:
Thx! |
There was a problem hiding this comment.
Looking through the changes it's much more logical now. Also I did some SITL testing of the original cases we had when finding the issue and it always published exactly as expected. Thanks for looking into it @julianoes 👍
|
@Tony3dr see test instructions above! Would be great if you could test this. And make sure to use a stock slow SD card. |
Tested on PixRacer V4:
|
Tested on NXP FMUK66 v3:
|
|
For reference here's dataman postflight perf counters after flying a mission on an fmu-v4. https://review.px4.io/plot_app?log=e64b7ac4-a54e-45d3-9a83-842c78832982
Which helps explain navigator total cycle time. |
|
@jorge789 @Junkim3DR @Tony3dr thanks a lot for the test flights! I've looked through all the logs mainly looking for throttle spikes to 0, as well as checking the warning messages logged at the bottom in flight review. I've found two flights with throttle spikes to:
Note: |
@julianoes I realized I missed your message. We missed to use a stock SD card. |
The testing is still valuable and ok. But if you could run through these 4 tests on just one vehicle (with a slow SD card) again that would be great, e.g. on the Pixracer. |
Tested on Master on PixRacer V4:Followed the instructions and all 4 flights we noticed no issues. Fly a mission, then halfway during the mission, change the mission plan, upload it and fly that. Fly a mission, switch to HOLD, back to mission, then RTL. Fly a mission, switch to POSCTL, back into mission, then RTL. Set vehicle to HOLD mode on ground, then arm, don't take off, disarm again. Then performed a flight in POSCTL and trigger RTL after a while. Note: When I was going to switch to RTL by mistake activate kill switch luckily nothing happened, nothing to worry about. SD Card 4 gig/Class 4 @julianoes, the previous test performed were on a Class 10/8gig |
|
@jorge789 thanks for testing this again. It looks fine for this PR. However, in two flights there are some thrust spikes to 0 where the land_detector found ground_dedected: FYI @bresch |
The navigator has a notion of resetting the triplets which means the triplet setpoints are set to invalid and therefore not to be used by downstream modules such as flight tasks.
However, before this patch, the triplets were not published if invalid meaning that a valid triplet would stay the truth until a new valid triplet would get published.
E.g. this lead to the corner case case where we publish a valid triplet with an IDLE setpoint on ground in HOLD and then don't update the triplet while flying in POSCTL mode. Later, when RTL is engaged, the flight task will use IDLE until navigator (which runs slower) has published the next triplet.
The fix involves two main changes:
_pos_sp_triplet_published_invalid_once = false.When testing this I realized that a mission upload during RTL would stop RTL. This is because the mission is updated while the mission navigation mode is not active and reset_triplets() is called from there. This is now only done for the case where we are actually in mission navigation
mode. The fact that a mission is updated when not active also seems wrong and is something to fix another time.
Fixes #13533.