diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8c5078084749..08a200eda02f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -211,8 +211,14 @@ void Mission::setActiveMissionItems() // prevent fixed wing lateral guidance from loitering at a waypoint as part of a mission landing if the altitude // is not achieved. - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() && - _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && isLanding() && + _mission_item.nav_cmd == NAV_CMD_WAYPOINT; + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (fw_on_mission_landing || mc_landing_after_transition) { pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cce3031ba1bf..201eeb6123f6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1145,8 +1145,13 @@ float Navigator::get_altitude_acceptance_radius() const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) - && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { + const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current; + + if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) { + alt_acceptance_radius = curr_sp.alt_acceptance_radius; + + } else if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) + && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; } diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 2c4c172f8b4d..c264c6fc88b4 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -355,6 +355,7 @@ void RtlDirect::set_rtl_item() pos_yaw_sp.alt = loiter_altitude; pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled + altitude_acceptance_radius = FLT_MAX; setMoveToPositionMissionItem(_mission_item, pos_yaw_sp); _navigator->reset_position_setpoint(pos_sp_triplet->previous); diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index d5256535c500..6b7f2f649a6d 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -215,8 +215,14 @@ void RtlDirectMissionLand::setActiveMissionItems() // prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude // is not achieved. - if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && MissionBase::isLanding() - && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && isLanding() && + _mission_item.nav_cmd == NAV_CMD_WAYPOINT; + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (fw_on_mission_landing || mc_landing_after_transition) { pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; } } diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index eb5f3d102246..707219b1b37a 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -155,6 +155,13 @@ void RtlMissionFastReverse::setActiveMissionItems() } mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type == + vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + if (mc_landing_after_transition) { + pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; + } } issue_command(_mission_item);