diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 1b41c4fe16..02601391af 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -257,10 +257,13 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) } else if ((_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB || _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND || _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { - _mission_item.nav_cmd = NAV_CMD_LAND; + + const bool loiter_indefinitely = _param_rtl_land_delay.get() < -FLT_EPSILON; + _mission_item.lat = _home_pos_sub.get().lat; _mission_item.lon = _home_pos_sub.get().lon; _mission_item.yaw = NAN; + _mission_item.altitude_is_relative = false; // make previous and next setpoints invalid, such that there will be no line following. // If the vehicle drifted off the path during back-transition it should just go straight to the landing point. @@ -272,14 +275,13 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; _mission_item.altitude = _global_pos_sub.get().alt; - _mission_item.altitude_is_relative = false; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; } else { - _mission_item.altitude = _home_pos_sub.get().alt; - _mission_item.altitude_is_relative = false; + _mission_item.altitude = loiter_indefinitely ? _home_pos_sub.get().alt + _param_rtl_descend_alt.get() : _global_pos_sub.get().alt; + _mission_item.nav_cmd = loiter_indefinitely ? NAV_CMD_WAYPOINT : NAV_CMD_LAND; _mission_item.land_precision = _param_rtl_pld_md.get(); diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index a688d18a8d..8f96b95dc6 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -76,6 +76,8 @@ private: uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ DEFINE_PARAMETERS_CUSTOM_PARENT( RtlBase, - (ParamInt) _param_rtl_pld_md + (ParamInt) _param_rtl_pld_md, + (ParamFloat) _param_rtl_descend_alt, + (ParamFloat) _param_rtl_land_delay ) };