From 17e96554ece973af6660f45ee4995d8956c46f4e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 24 Oct 2025 10:10:33 +0200 Subject: [PATCH] Navigator: fix RTL_TYP=2 with NAV_CMD_CONDITION_GATE (#25648) * rtl_mission_fast: make sure to set a position item on activation * rtl_mission_fast_reverse: make sure to set a position item on activation --------- Co-authored-by: Konrad Rudin --- src/modules/navigator/rtl_mission_fast.cpp | 25 ++++++++++++++++--- src/modules/navigator/rtl_mission_fast.h | 2 +- .../navigator/rtl_mission_fast_reverse.cpp | 19 +++++++++++--- .../navigator/rtl_mission_fast_reverse.h | 2 +- 4 files changed, 38 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index ccfc91cfde..a8a7809549 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -57,7 +57,7 @@ void RtlMissionFast::on_inactive() MissionBase::on_inactive(); _vehicle_status_sub.update(); _mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ? - _mission.current_seq : -1; + _mission.current_seq : INT32_C(-1); } void RtlMissionFast::on_activation() @@ -65,13 +65,30 @@ void RtlMissionFast::on_activation() _home_pos_sub.update(); // set mission item to closest item if not already in mission - if (_mission_index_prior_rtl < 0) { + if (_mission_index_prior_rtl < INT32_C(0)) { _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; } else { - setMissionIndex(_mission_index_prior_rtl); - _is_current_planned_mission_item_valid = isMissionValid(); + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(_mission_index_prior_rtl, &next_mission_item_index, num_found_items, UINT8_C(1)); + + if (num_found_items > 0U) { + setMissionIndex(next_mission_item_index); + _is_current_planned_mission_item_valid = isMissionValid(); + + } else { + // No more position items left. Set it to the land item if it exists + if (_mission.land_index > 0) { + setMissionIndex(_mission.land_index); + _is_current_planned_mission_item_valid = isMissionValid(); + + } else { + // Nothing we can do, set the validity to false to trigger end of mission reaction + _is_current_planned_mission_item_valid = false; + } + } } if (_land_detected_sub.get().landed) { diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h index d1231022b6..38814f96d0 100644 --- a/src/modules/navigator/rtl_mission_fast.h +++ b/src/modules/navigator/rtl_mission_fast.h @@ -64,7 +64,7 @@ private: bool setNextMissionItem() override; void setActiveMissionItems() override; - int _mission_index_prior_rtl{-1}; + int32_t _mission_index_prior_rtl{INT32_C(-1)}; uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ }; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 6abf4cf10f..1b41c4fe16 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -57,7 +57,7 @@ void RtlMissionFastReverse::on_inactive() MissionBase::on_inactive(); _vehicle_status_sub.update(); _mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ? - _mission.current_seq : -1; + _mission.current_seq : INT32_C(-1); } void RtlMissionFastReverse::on_inactivation() @@ -71,13 +71,24 @@ void RtlMissionFastReverse::on_activation() _home_pos_sub.update(); // set mission item to closest item if not already in mission. If we are in mission, set to the previous item. - if (_mission_index_prior_rtl < 0) { + if (_mission_index_prior_rtl < INT32_C(0)) { _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; } else { - setMissionIndex(math::max(_mission_index_prior_rtl - 1, 0)); - _is_current_planned_mission_item_valid = isMissionValid(); + int32_t previous_mission_item_index; + size_t num_found_items{0U}; + getPreviousPositionItems(math::max(_mission_index_prior_rtl - INT32_C(1), INT32_C(0)), &previous_mission_item_index, + num_found_items, UINT8_C(1)); + + if (num_found_items > 0U) { + setMissionIndex(previous_mission_item_index); + _is_current_planned_mission_item_valid = isMissionValid(); + + } else { + // No prior position items, so try to go to the first one. + _is_current_planned_mission_item_valid = (goToNextPositionItem(false) == PX4_OK); + } } if (_land_detected_sub.get().landed) { diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index 0351f8dbf8..a688d18a8d 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -69,7 +69,7 @@ private: void setActiveMissionItems() override; void handleLanding(WorkItemType &new_work_item_type); - int _mission_index_prior_rtl{-1}; + int32_t _mission_index_prior_rtl{INT32_C(-1)}; bool _in_landing_phase{false};