diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index 63ffb4ea1a..14b0b57a67 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -50,15 +50,11 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
     AP_Param::setup_object_defaults(this, var_info);
 
     // initialise flags
-#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
-    _flags.slow_cpu = false;
-#else
-    _flags.slow_cpu = true;
-#endif
     _flags.recalc_leash_xy = true;
     _flags.recalc_leash_z = true;
     _flags.reset_desired_vel_to_pos = true;
     _flags.reset_rate_to_accel_xy = true;
+    _flags.reset_accel_to_lean_xy = true;
     _flags.reset_rate_to_accel_z = true;
     _flags.reset_accel_to_throttle = true;
 }
@@ -520,6 +516,7 @@ void AC_PosControl::init_xy_controller(bool reset_I)
     // flag reset required in rate to accel step
     _flags.reset_desired_vel_to_pos = true;
     _flags.reset_rate_to_accel_xy = true;
+    _flags.reset_accel_to_lean_xy = true;
 }
 
 /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
@@ -571,6 +568,7 @@ void AC_PosControl::init_vel_controller_xyz()
     // flag reset required in rate to accel step
     _flags.reset_desired_vel_to_pos = true;
     _flags.reset_rate_to_accel_xy = true;
+    _flags.reset_accel_to_lean_xy = true;
 
     // set target position in xy axis
     const Vector3f& curr_pos = _inav.get_position();
@@ -803,6 +801,15 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
     float accel_right, accel_forward;
     float lean_angle_max = _attitude_control.lean_angle_max();
 
+    // reset accel to current desired acceleration
+    if (_flags.reset_accel_to_lean_xy) {
+        _accel_target_jerk_limited.x = _accel_target.x;
+        _accel_target_jerk_limited.y = _accel_target.y;
+        _accel_target_filtered.x = _accel_target.x;
+        _accel_target_filtered.y = _accel_target.y;
+        _flags.reset_accel_to_lean_xy = false;
+    }
+
     // apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
     float max_delta_accel = dt * POSCONTROL_JERK_LIMIT_CMSSS;
 
@@ -812,8 +819,8 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
 
     if(accel_change_length > max_delta_accel) {
         accel_change *= max_delta_accel/accel_change_length;
-        _accel_target_jerk_limited += accel_change;
     }
+    _accel_target_jerk_limited += accel_change;
 
     // 5Hz lowpass filter on NE accel
     float freq_cut = POSCONTROL_ACCEL_FILTER_HZ * ekfNavVelGainScaler;
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
index 0d4015f6bd..a823883a4c 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.h
+++ b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -263,15 +263,15 @@ private:
 
     // general purpose flags
     struct poscontrol_flags {
-            uint8_t recalc_leash_z  : 1;    // 1 if we should recalculate the z axis leash length
-            uint8_t recalc_leash_xy : 1;    // 1 if we should recalculate the xy axis leash length
-            uint8_t slow_cpu        : 1;    // 1 if we are running on a slow_cpu machine.  xy position control is broken up into multiple steps
-            uint8_t reset_desired_vel_to_pos: 1;    // 1 if we should reset the rate_to_accel_xy step
-            uint8_t reset_rate_to_accel_xy  : 1;    // 1 if we should reset the rate_to_accel_xy step
-            uint8_t reset_rate_to_accel_z   : 1;    // 1 if we should reset the rate_to_accel_z step
-            uint8_t reset_accel_to_throttle : 1;    // 1 if we should reset the accel_to_throttle step of the z-axis controller
-            uint8_t freeze_ff_xy    : 1;    // 1 use to freeze feed forward during step updates
-            uint8_t freeze_ff_z     : 1;    // 1 use to freeze feed forward during step updates
+            uint16_t recalc_leash_z     : 1;    // 1 if we should recalculate the z axis leash length
+            uint16_t recalc_leash_xy    : 1;    // 1 if we should recalculate the xy axis leash length
+            uint16_t reset_desired_vel_to_pos   : 1;    // 1 if we should reset the rate_to_accel_xy step
+            uint16_t reset_rate_to_accel_xy     : 1;    // 1 if we should reset the rate_to_accel_xy step
+            uint16_t reset_accel_to_lean_xy     : 1;    // 1 if we should reset the accel to lean angle step
+            uint16_t reset_rate_to_accel_z      : 1;    // 1 if we should reset the rate_to_accel_z step
+            uint16_t reset_accel_to_throttle    : 1;    // 1 if we should reset the accel_to_throttle step of the z-axis controller
+            uint16_t freeze_ff_xy       : 1;    // 1 use to freeze feed forward during step updates
+            uint16_t freeze_ff_z        : 1;    // 1 use to freeze feed forward during step updates
     } _flags;
 
     // limit flags structure