dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <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
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 38 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
src/modules/navigator/mission.cpp
View File

@ -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();
}

2
src/modules/navigator/mission.h
View File

@ -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

9
src/modules/navigator/navigator.h
View File

@ -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

36
src/modules/navigator/navigator_main.cpp
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:

8
src/modules/navigator/rtl.cpp
View File

@ -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.