From ea0e413b043b9d0c3e386e9090aa12e6a9518e76 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 23 Nov 2016 14:43:19 +0900 Subject: [PATCH] AC_AttControl: do not limit rate if ATC_ACCEL_MAX param is zero --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index bd92f261b4..23f6084b85 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -480,8 +480,12 @@ float AC_AttitudeControl::input_shaping_angle(float error_angle, float smoothing float ang_vel = sqrt_controller(error_angle, smoothing_gain, accel_max); // Acceleration is limited directly to smooth the beginning of the curve. - float delta_ang_vel = accel_max * _dt; - return constrain_float(ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel); + if (accel_max > 0) { + float delta_ang_vel = accel_max * _dt; + return constrain_float(ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel); + } else { + return ang_vel; + } } // limits the acceleration and deceleration of a velocity request