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

Navigator: Support straight line mission landings (#23576)

Browse Source 
* introduced altitude acceptance radius in position setpoint for fixed
wing guidance
- allows navigator to explicitly set the altitude acceptance radius
- needed for staright line landing support

* added ignore_alt_acceptance to position setpoint message to allow guidance
logic to ignore altitude error on waypoint
- can be useful to prevent loitering at a waypoint within a mission landing sequence

* only set altitude acceptance radius to infinity for a waypoint inside a mission landing
for fixed wing vehicles

* navigator: return altitude acceptance radius from triplet if it's valid

* FixedWingPositionControl: check if alt acceptance radius provided in position setpoint
is larger 0

---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Alvaro Fernandez <alvaro@auterion.com>
This commit is contained in:
Roman Bapst
2024-09-10 17:44:24 +02:00
committed by GitHub
co-authored by Alvaro Fernandez
parent 03aec2e188
commit c94c1ce4d2
10 changed files with 75 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+7 -35
View File
@@ -91,41 +91,6 @@ Mission::on_activation()
MissionBase::on_activation();
}
bool
Mission::isLanding()
{
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;
getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items);
// 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];
// 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;
}
}
bool
Mission::set_current_mission_index(uint16_t index)
@@ -244,6 +209,13 @@ void Mission::setActiveMissionItems()
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
}
// prevent fixed wing lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
// is not achieved.
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() &&
_mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
}
// Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout
// This is done by setting the position triplet's next position's valid flag to false,
// which makes the FlightTask disregard the next position