From 94fb334d8f6625a00ad28fe456c98c11ddc315c8 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 5 Apr 2023 10:11:14 +0200 Subject: [PATCH] Navigator: fix logic around when to switch to mission landing for RTL (#21390) * Navigator: change way of telling logic if RLT was started just now * Navigator: change logic around when to engage Mission mode for RTL To find out if we're currently on a mission landing, check if we either are past the land start marker OR currently land start marker is current WP and vehicle is already in LOITER mode. * Navigator: do not engage RTL at all if already on mission landing * Navigator: consider to be on mission landing if the LOITER_TO_ALT and dist small To find out if we're currently on a mission landing, check if we either are past the land start marker OR currently land start marker is current WP, the type is LOITER_TO_ALT and the vehicle is inside loiter. --------- Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission.cpp | 39 +++++++++++------------- src/modules/navigator/mission.h | 2 -- src/modules/navigator/navigator.h | 9 +----- src/modules/navigator/navigator_main.cpp | 36 +++++++++++----------- src/modules/navigator/rtl.cpp | 8 +---- 5 files changed, 38 insertions(+), 56 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index baef5b3304..b5caaef4b2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -96,12 +96,6 @@ void Mission::mission_init() void Mission::on_inactive() { - // if we were executing an landing but have been inactive for 2 seconds, then make the landing invalid - // this prevents RTL to just continue at the current mission index - if (_navigator->getMissionLandingInProgress() && (hrt_absolute_time() - _time_mission_deactivated) > 2_s) { - _navigator->setMissionLandingInProgress(false); - } - /* Without home a mission can't be valid yet anyway, let's wait. */ if (!_navigator->home_global_position_valid()) { return; @@ -180,8 +174,6 @@ Mission::on_inactivation() _navigator->get_precland()->on_inactivation(); } - _time_mission_deactivated = hrt_absolute_time(); - /* reset so current mission item gets restarted if mission was paused */ _work_item_type = WORK_ITEM_TYPE_DEFAULT; } @@ -476,15 +468,12 @@ Mission::land_start() { // if not currently landing, jump to do_land_start if (_land_start_available) { - if (_navigator->getMissionLandingInProgress()) { + if (landing()) { return true; } else { set_current_mission_index(get_land_start_index()); - - const bool can_land_now = landing(); - _navigator->setMissionLandingInProgress(can_land_now); - return can_land_now; + return landing(); } } @@ -495,10 +484,24 @@ bool Mission::landing() { // vehicle is currently landing if - // mission valid, still flying, and in the landing portion of mission + // mission valid, still flying, and in the landing portion of mission (past land start marker) const bool mission_valid = _navigator->get_mission_result()->valid; - const bool on_landing_stage = _land_start_available && (_current_mission_index >= get_land_start_index()); + bool on_landing_stage = _land_start_available && _current_mission_index > get_land_start_index(); + + // 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 (_current_mission_index == 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); + + // 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 mission_valid && on_landing_stage; } @@ -1718,12 +1721,6 @@ Mission::set_mission_item_reached() { _navigator->get_mission_result()->seq_reached = _current_mission_index; _navigator->set_mission_result_updated(); - - // let the navigator know that we are currently executing the mission landing. - // Using the method landing() itself is not accurate as it only give information about the mission index - // but the vehicle could still be very far from the actual landing items - _navigator->setMissionLandingInProgress(landing()); - reset_mission_item_reached(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 821b6b25a7..0cdddae4fb 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -267,8 +267,6 @@ private: bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ - hrt_abstime _time_mission_deactivated{0}; - enum { MISSION_TYPE_NONE, MISSION_TYPE_MISSION diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d80beb9333..9a08ed946d 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -259,10 +259,6 @@ public: void set_mission_failure_heading_timeout(); - void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; } - - bool getMissionLandingInProgress() { return _mission_landing_in_progress; } - bool is_planned_mission() const { return _navigation_mode == &_mission; } bool on_mission_landing() { return _mission.landing(); } @@ -348,7 +344,7 @@ private: vehicle_local_position_s _local_pos{}; /**< local vehicle position */ vehicle_status_s _vstatus{}; /**< vehicle status */ - uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/ + bool _rtl_activated{false}; // Publications geofence_result_s _geofence_result{}; @@ -393,9 +389,6 @@ private: float _mission_cruising_speed_fw{-1.0f}; float _mission_throttle{NAN}; - bool _mission_landing_in_progress{false}; /**< this flag gets set if the mission is currently executing on a landing pattern - * if mission mode is inactive, this flag will be cleared after 2 seconds */ - traffic_buffer_s _traffic_buffer{}; bool _is_capturing_images{false}; // keep track if we need to stop capturing images diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 10349a6cb8..0e1187cf27 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -664,24 +664,22 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: { _pos_sp_triplet_published_invalid_once = false; - - const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + const bool rtl_activated_now = !_rtl_activated; switch (_rtl.get_rtl_type()) { case RTL::RTL_TYPE_MISSION_LANDING: case RTL::RTL_TYPE_CLOSEST: - - if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_LOITER - && _rtl.getShouldEngageMissionForLanding()) { + if (on_mission_landing() && _rtl.getShouldEngageMissionForLanding()) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED - && !get_land_detected()->landed) { - start_mission_landing(); - } - navigation_mode_new = &_mission; + if (rtl_activated_now) { + mavlink_log_info(get_mavlink_log_pub(), "RTL to Mission landing, continue landing\t"); + events::send(events::ID("rtl_land_at_mission_continue_landing"), events::Log::Info, + "RTL to Mission landing, continue landing"); + } + } else { navigation_mode_new = &_rtl; } @@ -706,7 +704,7 @@ void Navigator::run() } } - if (rtl_activated) { + if (rtl_activated_now) { mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t"); events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info, "RTL Mission activated, continue mission"); @@ -726,11 +724,11 @@ void Navigator::run() // The seconds condition is required so that when no mission was uploaded and one is available the closest // mission item is determined and also that if the user changes the active mission index while rtl is active // always that waypoint is tracked first. - if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) { + if ((_navigation_mode != &_mission) && (rtl_activated_now || _mission.get_mission_waypoints_changed())) { _mission.set_closest_item_as_current(); } - if (rtl_activated) { + if (rtl_activated_now) { mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t"); events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info, "RTL Mission activated, fly mission in reverse"); @@ -739,7 +737,7 @@ void Navigator::run() navigation_mode_new = &_mission; } else { - if (rtl_activated) { + if (rtl_activated_now) { mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t"); events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info, "RTL Mission activated, fly to home"); @@ -752,7 +750,7 @@ void Navigator::run() break; default: - if (rtl_activated) { + if (rtl_activated_now) { mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t"); events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated"); } @@ -762,6 +760,7 @@ void Navigator::run() } + _rtl_activated = true; break; } @@ -800,14 +799,15 @@ void Navigator::run() break; } + if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + _rtl_activated = false; + } + // Do not execute any state machine while we are disarmed if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { navigation_mode_new = nullptr; } - // update the vehicle status - _previous_nav_state = _vstatus.nav_state; - /* we have a new navigation mode: reset triplet */ if (_navigation_mode != navigation_mode_new) { // We don't reset the triplet in the following two cases: diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index ccc4bc4d8f..b9ac759cb8 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -152,7 +152,7 @@ void RTL::find_RTL_destination() double dist_squared = coord_dist_sq(dlat, dlon); // always find closest destination if in hover and VTOL - if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->getMissionLandingInProgress())) { + if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->on_mission_landing())) { // compare home position to landing position to decide which is closer if (dist_squared < min_dist_squared) { @@ -282,12 +282,6 @@ void RTL::on_activation() // For safety reasons don't go into RTL if landed. _rtl_state = RTL_STATE_LANDED; - } else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) { - // we were just on a mission landing, set _rtl_state past RTL_STATE_LOITER such that navigator will engage mission mode, - // which will continue executing the landing - _rtl_state = RTL_STATE_LAND; - - } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { // If lower than return altitude, climb up first.