From 550a4a9c1a1b94eda6d46dc0174387630fd533aa Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 24 Jan 2022 12:43:22 +1030 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: Init desired accel to zero --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 6f70812697..8b6e83b509 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -509,15 +509,16 @@ void AC_PosControl::init_xy() _vel_target.y = curr_vel.y; - const Vector3f &curr_accel = _ahrs.get_accel_ef_blended() * 100.0f; - _accel_desired.xy() = curr_accel.xy(); - _accel_desired.xy().limit_length(_accel_max_xy_cmss); + // Set desired accel to zero because raw acceleration is prone to noise + _accel_desired.xy().zero(); lean_angles_to_accel_xy(_accel_target.x, _accel_target.y); // initialise I terms from lean angles _pid_vel_xy.reset_filter(); - _pid_vel_xy.set_integrator(_accel_target - _accel_desired); + // initialise the I term to _accel_target - _accel_desired + // _accel_desired is zero and can be removed from the equation + _pid_vel_xy.set_integrator(_accel_target); // initialise ekf xy reset handler init_ekf_xy_reset();