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

Navigator: Don't switch to RTL if already in landing phase of mission.

Browse Source
This commit is contained in:
Konrad 2023-10-24 10:54:14 +02:00 committed by Roman Bapst
parent c1214c847f
commit a4d05085a7
2 changed files with 28 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

1
src/modules/navigator/navigator_main.cpp
View File

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