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

rtl_mission_fast: fix FW landing by setting previous wp in landing

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan
2025-11-03 12:07:06 +01:00
parent a5e76f97a9
commit 487d8ae215
2 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl_direct_mission_land.cpp
-1
View File
@@ -206,7 +206,6 @@ void RtlDirectMissionLand::setActiveMissionItems()
_mission_item.time_inside = 0.0f;
}
if (num_found_items > 0) {
mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
}
src/modules/navigator/rtl_mission_fast.cpp
+7 -3
View File
@@ -91,6 +91,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) &&
@@ -158,17 +159,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;