From aeae5462ca176f1dd6d140a26abdf57a1dda4291 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 16 Dec 2024 10:57:54 +0300 Subject: [PATCH 1/2] navigator: set alt acceptance radius to infinity for land waypoint after backtransition -> avoid vehicle with depleted battery from not reaching the alt setpoint and getting stuck Signed-off-by: RomanBapst --- src/modules/navigator/mission.cpp | 10 ++++++++-- src/modules/navigator/navigator_main.cpp | 9 +++++++-- src/modules/navigator/rtl_direct.cpp | 1 + src/modules/navigator/rtl_direct_mission_land.cpp | 10 ++++++++-- src/modules/navigator/rtl_mission_fast_reverse.cpp | 7 +++++++ 5 files changed, 31 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8c5078084749..6957f0dd99bc 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 && + 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..6c9d5c5d3608 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 && + 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..a0489e1806de 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 && + 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); From b9b82ecce7b69a9d3ad993ebbda8a0dd89fe08e7 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 20 Dec 2024 09:47:24 +0300 Subject: [PATCH 2/2] added check for vtol Signed-off-by: RomanBapst --- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/rtl_direct_mission_land.cpp | 2 +- src/modules/navigator/rtl_mission_fast_reverse.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 6957f0dd99bc..08a200eda02f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -215,7 +215,7 @@ void Mission::setActiveMissionItems() && 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_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) { diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 6c9d5c5d3608..6b7f2f649a6d 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -219,7 +219,7 @@ void RtlDirectMissionLand::setActiveMissionItems() && 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_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) { diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index a0489e1806de..707219b1b37a 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -156,7 +156,7 @@ 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_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) {