From 052524728b678ba491c7328a66906a90d5492c6d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 23 Aug 2021 09:34:33 +0930 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: limit initial acceleration --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index abbdd7c2a9..7a4ec34a42 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -514,8 +514,8 @@ void AC_PosControl::init_xy() const Vector3f &curr_accel = _ahrs.get_accel_ef_blended() * 100.0f; - _accel_desired.x = curr_accel.x; - _accel_desired.y = curr_accel.y; + _accel_desired.xy() = curr_accel.xy(); + _accel_desired.xy().limit_length(_accel_max_xy_cmss); lean_angles_to_accel_xy(_accel_target.x, _accel_target.y); @@ -801,7 +801,7 @@ void AC_PosControl::init_z() _pid_vel_z.set_integrator(0.0f); _accel_desired.z = constrain_float(-(curr_accel.z + GRAVITY_MSS) * 100.0f, -_accel_max_z_cmss, _accel_max_z_cmss); - _accel_target.z = -(curr_accel.z + GRAVITY_MSS) * 100.0f; + _accel_target.z = _accel_desired.z; _pid_accel_z.reset_filter(); // initialise vertical offsets