Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Ignore max HAGL failsafe in front transition (#25982)

Browse Source 
* mission_block: readibility improvement

* mission_block: ignore max hagl failsafe in front transition
This commit is contained in:
Balduin
2025-11-26 09:14:06 +01:00
committed by GitHub
parent 7bb12b15b5
commit a6d5c78d10
1 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+11 -3
View File
@@ -1027,10 +1027,18 @@ void MissionBlock::updateFailsafeChecks()
void MissionBlock::updateMaxHaglFailsafe() void MissionBlock::updateMaxHaglFailsafe()
{ {
const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;
const float max_alt = math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy);
const float terrain_alt = _navigator->get_global_position()->terrain_alt;
const bool terrain_alt_valid = _navigator->get_global_position()->terrain_alt_valid;
if (_navigator->get_global_position()->terrain_alt_valid // If the HAGL failsafe is declared during front transition, we enter a
&& ((target_alt - _navigator->get_global_position()->terrain_alt) // FW hold at the current low altitude while not having finished the
> math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy))) { // transition. This is dangerous and worse than possibly fusing neither
// optical flow nor airspeed for a couple seconds, so we bypass the
// failsafe here.
const bool in_transition_to_fw = _navigator->get_vstatus()->in_transition_to_fw;
if (!in_transition_to_fw && terrain_alt_valid && (target_alt - terrain_alt) > max_alt) {
// Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level)
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t");
events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL");