diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 967e280bfd..d7c42516e4 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -52,12 +52,27 @@ RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : } +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; +} + void RtlMissionFastReverse::on_activation() { _home_pos_sub.update(); - _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; + // 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) { + _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(); + } if (_land_detected_sub.get().landed) { // already landed, no need to do anything, invalidate the position mission item. diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index 889dd8716b..f1301a5cca 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -57,6 +57,7 @@ public: void on_activation() override; void on_active() override; + void on_inactive() override; rtl_time_estimate_s calc_rtl_time_estimate() override; @@ -65,5 +66,7 @@ private: void setActiveMissionItems() override; void handleLanding(WorkItemType &new_work_item_type); + int _mission_index_prior_rtl{-1}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ };