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: publish navigator_state

Browse Source 
feedback to commander
This commit is contained in:
bresch
2024-08-14 11:08:02 +02:00
committed by Mathieu Bresciani
parent 1fa878ad88
commit 9f69e9ee6c
6 changed files with 68 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl_direct.cpp
+10
View File
@@ -158,6 +158,16 @@ 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, landing\t");
events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing");
_navigator->trigger_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);