From 1639e22b749f350936059d93b7b8937403c1f66f Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 4 Jun 2016 13:29:38 +0900 Subject: [PATCH] AC_AttitudeControl: set desired_vel for reporting purposes even when not used --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 9cfb395ab3..5642cf797f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -140,11 +140,14 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt) // do not use z-axis desired velocity feed forward _flags.use_desvel_ff_z = false; - _vel_desired.z = 0.0f; // adjust desired alt if motors have not hit their limits if ((alt_change<0 && !_motors.limit.throttle_lower) || (alt_change>0 && !_motors.limit.throttle_upper)) { - _pos_target.z += constrain_float(alt_change, _speed_down_cms*dt, _speed_up_cms*dt); + if (!is_zero(dt)) { + float climb_rate_cms = constrain_float(alt_change/dt, _speed_down_cms, _speed_up_cms); + _pos_target.z += climb_rate_cms*dt; + _vel_desired.z = climb_rate_cms; // recorded for reporting purposes + } } // do not let target get too far from current altitude