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

Fix max-hagl restriction to position/altitude control (#23667)

Browse Source 
* fix max-hagl restriction to position/altitude control

* max hagl vel restriction in ManAcc position mode

* use interpolate func, change naming

* simplyfied vertical vel limitation

* move velocity-constraint adjustment to StickAccelXY
This commit is contained in:
Marco Hauswirth
2025-01-20 15:50:21 +01:00
committed by GitHub
parent 0d22905558
commit 4a5aa1e947
15 changed files with 90 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+2 -1
View File
@@ -1026,7 +1026,8 @@ void MissionBlock::updateMaxHaglFailsafe()
const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;
if (_navigator->get_global_position()->terrain_alt_valid
&& ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) {
&& ((target_alt - _navigator->get_global_position()->terrain_alt)
> math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy))) {
// 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");
events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL");