UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-25 20:28:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(navigator): do not descend to the loiter altitude after the climbout

Browse Source 
The loiter established after a takeoff was always commanded at the takeoff
altitude. With a time-based climbout the vehicle can be above
that already. Take the higher of the two to prevent diving down right after climb.

Signed-off-by: mahima-yoga <mahima@auterion.com>
This commit is contained in:
mahima-yoga
2026-08-28 10:25:22 +02:00
committed by Silvan Fuhrer
parent 1d14de2b12
commit 3f87fc6cb1
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/navigator/takeoff.cpp
View File

@@ -82,7 +82,10 @@ Takeoff::on_active()
_mission_item.time_inside = 1.f;
_mission_item.loiter_radius = _navigator->get_default_loiter_rad();
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.altitude = _loiter_altitude_msl;
// the climbout does not necessarily end at the loiter altitude, and the vehicle should
// not descend back to it if it ended up higher
_mission_item.altitude = math::max(_loiter_altitude_msl, _navigator->get_global_position()->alt);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
const bool loiter_lat_valid = PX4_ISFINITE(_loiter_position_lat_lon(0))
@@ -126,7 +129,7 @@ Takeoff::on_active()
lateral_acceptance_reached = distance_to_loiter < _navigator->get_acceptance_radius() + mission_item_loiter_radius_abs;
}
const bool vertical_acceptance_reached = _navigator->get_global_position()->alt >= _loiter_altitude_msl -
const bool vertical_acceptance_reached = _navigator->get_global_position()->alt >= _mission_item.altitude -
_navigator->get_altitude_acceptance_radius();
if (lateral_acceptance_reached && vertical_acceptance_reached) {