From ba3303dc6102fb08422e6f8c2dc51d38ee1efa35 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 Nov 2018 15:19:35 +0900 Subject: [PATCH] AC_PosControl: set-alt-target-with-slew sets desired to 0 once at target This resolves and issue with the set-alt-target-with-slew method leaving the z-axis desired velocity at the max speed-up or speed-down this causes a jump in throttle if the user switches to Loiter after the vehicle has reached its target --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 2b2654c96c..c7a9aba426 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -286,6 +286,9 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt) _pos_target.z += climb_rate_cms*dt; _vel_desired.z = climb_rate_cms; // recorded for reporting purposes } + } else { + // recorded for reporting purposes + _vel_desired.z = 0.0f; } // do not let target get too far from current altitude