Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+27 -15
View File
@@ -92,25 +92,37 @@ Mission::on_activation()
bool bool
Mission::isLanding() Mission::isLanding()
{ {
// vehicle is currently landing if if (get_land_start_available()) {
// mission valid, still flying, and in the landing portion of mission (past land start marker) static constexpr size_t max_num_next_items{1u};
bool on_landing_stage = get_land_start_available() && _mission.current_seq > get_land_start_index(); 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 getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items);
// 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);
// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. // vehicle is currently landing if
const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) // mission valid, still flying, and in the landing portion of mission (past land start marker)
&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : bool on_landing_stage = (num_found_items > 0U) && _mission.current_seq > next_mission_items_index[0U];
_navigator->get_loiter_radius();
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 bool
src/modules/navigator/navigator_main.cpp
+1
View File
@@ -717,6 +717,7 @@ void Navigator::run()
// If we are already in mission landing, do not switch. // If we are already in mission landing, do not switch.
if (_navigation_mode == &_mission && _mission.isLanding()) { if (_navigation_mode == &_mission && _mission.isLanding()) {
navigation_mode_new = &_mission; navigation_mode_new = &_mission;
break;
} else { } else {
_pos_sp_triplet_published_invalid_once = false; _pos_sp_triplet_published_invalid_once = false;