fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:33: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/mission.cpp
+18 -21
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();
}