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();