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: Fix mission RTL for fixed-wing by setting previous waypoint correctly (#25600)

Browse Source 
This aligns setActiveMissionItems() in rtl_direct_mission_land.cpp and in rtl_mission_fast.cpp with what was already in mission.cpp. It probably was on oversight when the RTL restructure happened. The FW landing requires the previous waypoint to be correctly set, that's why it was only noticeable there.

* Fix position setpoint update logic in Mission RTL

Currently, when proceeding to the landing point the previous setpoint is not updated, which results in an unexpected and off course landing pattern in fixed wing. (see #25436)

* Change to work more like `mission.cpp`

* Fix rtl_direct_misssion_land formatting for style guide

* rtl_mission_fast: fix FW landing by setting previous wp in landing

Signed-off-by: Silvan <silvan@auterion.com>

---------

Signed-off-by: Silvan <silvan@auterion.com>
Co-authored-by: Silvan <silvan@auterion.com>
This commit is contained in:
Matthew Berk 2025-11-03 10:52:26 -06:00 committed by GitHub
parent cfe4cc82ea
commit 9702a2a899
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/navigator/rtl_direct_mission_land.cpp
View File

@ -122,6 +122,7 @@ void RtlDirectMissionLand::setActiveMissionItems()
{
WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
// Climb to altitude
if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
@ -203,8 +204,6 @@ void RtlDirectMissionLand::setActiveMissionItems()
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
pos_sp_triplet->previous = pos_sp_triplet->current;
}
if (num_found_items > 0) {
@ -213,6 +212,12 @@ void RtlDirectMissionLand::setActiveMissionItems()
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
// Only set the previous position item if the current one really changed
if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
!position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
pos_sp_triplet->previous = current_setpoint_copy;
}
// prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
// is not achieved.
const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING

10
src/modules/navigator/rtl_mission_fast.cpp
View File

@ -108,6 +108,7 @@ void RtlMissionFast::setActiveMissionItems()
{
WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT};
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
/* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/
if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) &&
@ -175,17 +176,20 @@ void RtlMissionFast::setActiveMissionItems()
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
pos_sp_triplet->previous = pos_sp_triplet->current;
}
if (num_found_items > 0) {
mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
}
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
// Only set the previous position item if the current one really changed
if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) &&
!position_setpoint_equal(&pos_sp_triplet->current, &current_setpoint_copy)) {
pos_sp_triplet->previous = current_setpoint_copy;
}
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;