From bfb7476d738dee51a9db7b3b64003ff641c558b8 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 25 Jan 2022 01:11:05 +1030 Subject: [PATCH] AC_AttitudeControl: AC_PosControl_Sub: Increase Jerk with Accel when out of velocity range --- .../AC_AttitudeControl/AC_PosControl_Sub.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp index 00f6186b75..cf5a879d6c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp @@ -23,14 +23,9 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo sqrt_controller(_alt_max-_pos_target.z, 0.0f, _accel_max_z_cmss, 0.0f)); } - // calculated increased maximum acceleration if over speed - float accel_z_cms = _accel_max_z_cmss; - if (_vel_desired.z < _vel_max_down_cms && !is_zero(_vel_max_down_cms)) { - accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_down_cms; - } - if (_vel_desired.z > _vel_max_up_cms && !is_zero(_vel_max_up_cms)) { - accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _vel_max_up_cms; - } + // calculated increased maximum acceleration and jerk if over speed + float accel_max_z_cmss = _accel_max_z_cmss * calculate_overspeed_gain(); + float jerk_max_xy_cmsss = _jerk_max_xy_cmsss * calculate_overspeed_gain(); // adjust desired alt if motors have not hit their limits update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error()); @@ -42,8 +37,8 @@ void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool fo shape_vel_accel(vel, accel, _vel_desired.z, _accel_desired.z, - -accel_z_cms, accel_z_cms, - _jerk_max_xy_cmsss, _dt, limit_output); + -accel_max_z_cmss, accel_max_z_cmss, + jerk_max_xy_cmsss, _dt, limit_output); update_vel_accel(vel, accel, _dt, 0.0, 0.0); }