fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 14:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: fix logic around when to switch to mission landing for RTL (#21390)

Browse Source 
* 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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-04-05 10:11:14 +02:00
committed by GitHub
parent fb3b4a3151
commit 94fb334d8f
5 changed files with 38 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+18 -18
View File
@@ -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: