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

rtl_direct: loiter hold should track altitude as best effort but not enforce it

Browse Source
This commit is contained in:
Konrad 2024-09-23 10:03:50 +02:00 committed by KonradRudin
parent cacbfcecd8
commit 02e50bb5dc
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/navigator/rtl_direct.cpp
View File

@ -220,6 +220,8 @@ void RtlDirect::set_rtl_item()
const bool is_close_to_destination = destination_dist < _param_rtl_min_dist.get();
float altitude_acceptance_radius = static_cast<float>(NAN);
switch (_rtl_state) {
case RTLState::CLIMBING: {
PositionYawSetpoint pos_yaw_sp {
@ -293,6 +295,11 @@ void RtlDirect::set_rtl_item()
if (_param_rtl_land_delay.get() < -FLT_EPSILON) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t");
events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering");
} else {
/* Set the altitude tracking to best effort but not strictly enforce it */
altitude_acceptance_radius = FLT_MAX;
}
break;
@ -370,6 +377,7 @@ void RtlDirect::set_rtl_item()
} else {
// Convert mission item to current position setpoint and make it valid.
if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
pos_sp_triplet->current.alt_acceptance_radius = altitude_acceptance_radius;
_navigator->set_position_setpoint_triplet_updated();
}
}