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

Navigator: re-introduce min loiter alt, but only apply it for Loiters w/o a specified altitude setpoint

Browse Source 
Also applies to Loiters that are started due to the previous mode being over (Takeoff,
VTOL_Takeoff, Mission).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-11-22 13:46:25 -05:00
committed by Daniel Agar
parent 8157d83bfc
commit 9cd3eae0aa
3 changed files with 29 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+11 -1
View File
@@ -772,7 +772,17 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
// check if minimum loiter altitude is specified, and enforce it if so
float loiter_altitude_amsl = _navigator->get_global_position()->alt;
if (_navigator->get_loiter_min_alt() > FLT_EPSILON) {
loiter_altitude_amsl = math::max(loiter_altitude_amsl,
_navigator->get_home_position()->alt + _navigator->get_loiter_min_alt());
}
item->altitude = loiter_altitude_amsl;
item->loiter_radius = _navigator->get_loiter_radius();
}