From a4d05085a799bc935ab3390e70b97aa4eeac0c2e Mon Sep 17 00:00:00 2001 From: Konrad Date: Tue, 24 Oct 2023 10:54:14 +0200 Subject: [PATCH] Navigator: Don't switch to RTL if already in landing phase of mission. --- src/modules/navigator/mission.cpp | 42 +++++++++++++++--------- src/modules/navigator/navigator_main.cpp | 1 + 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 0853d8cce7..b80cb2b096 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -92,25 +92,37 @@ Mission::on_activation() bool Mission::isLanding() { - // vehicle is currently landing if - // mission valid, still flying, and in the landing portion of mission (past land start marker) - bool on_landing_stage = get_land_start_available() && _mission.current_seq > get_land_start_index(); + if (get_land_start_available()) { + static constexpr size_t max_num_next_items{1u}; + int32_t next_mission_items_index[max_num_next_items]; + size_t num_found_items; - // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the - // distance to the WP is below the loiter radius + acceptance. - if (_mission.current_seq == get_land_start_index() && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { - const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items); - // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. - const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) - && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : - _navigator->get_loiter_radius(); + // vehicle is currently landing if + // mission valid, still flying, and in the landing portion of mission (past land start marker) + bool on_landing_stage = (num_found_items > 0U) && _mission.current_seq > next_mission_items_index[0U]; - on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); + // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the + // distance to the WP is below the loiter radius + acceptance. + if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U] + && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. + const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) + && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : + _navigator->get_loiter_radius(); + + on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); + } + + return _navigator->get_mission_result()->valid && on_landing_stage; + + } else { + return false; } - - return _navigator->get_mission_result()->valid && on_landing_stage; } bool diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7fd03f3e5d..100998bc3f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -717,6 +717,7 @@ void Navigator::run() // If we are already in mission landing, do not switch. if (_navigation_mode == &_mission && _mission.isLanding()) { navigation_mode_new = &_mission; + break; } else { _pos_sp_triplet_published_invalid_once = false;