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

navigator: check hagl failsafe centrally

Browse Source
This commit is contained in:
bresch
2024-08-16 16:26:20 +02:00
committed by Mathieu Bresciani
parent 4f66410d24
commit ea673b0b5b
6 changed files with 37 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl_direct.cpp
-10
View File
@@ -158,16 +158,6 @@ void RtlDirect::set_rtl_item()
{
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if (_global_pos_sub.get().terrain_alt_valid
&& ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) {
// Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t");
events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL");
_navigator->trigger_hagl_failsafe(getNavigatorStateId());
_rtl_state = RTLState::IDLE;
}
const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon,
_global_pos_sub.get().lat, _global_pos_sub.get().lon);
const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt);