From 3f87fc6cb152d72f4960d0a00699ef89d8dfd4b9 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Fri, 28 Aug 2026 10:25:22 +0200 Subject: [PATCH] fix(navigator): do not descend to the loiter altitude after the climbout 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 --- src/modules/navigator/takeoff.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 40b97f5f871..15530c5ad32 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -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) {