AC_PosControl: freeze feed forward for alt control in Auto

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -315,6 +315,7 @@ void AC_PosControl::rate_to_accel_z()
     	if(!_limit.freeze_ff_z) {
     		_accel_feedforward.z = (_vel_target.z - _vel_last.z)/_dt;
         } else {
+    		// stop the feed forward being calculated during a known discontinuity
     		_limit.freeze_ff_z = false;
     	}
     } else {
@@ -701,6 +702,7 @@ void AC_PosControl::rate_to_accel_xy(float dt)
     		_accel_feedforward.x = (_vel_target.x - _vel_last.x)/dt;
     		_accel_feedforward.y = (_vel_target.y - _vel_last.y)/dt;
         } else {
+    		// stop the feed forward being calculated during a known discontinuity
     		_limit.freeze_ff_xy = false;
     	}
     } else {

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -179,6 +179,12 @@ public:
     /// trigger_xy - used to notify the position controller than an update has been made to the position or desired velocity so that the position controller will run as soon as possible after the update
     void trigger_xy() { _flags.force_recalc_xy = true; }
 
+    /// freeze_ff_z - used to stop the feed forward being calculated during a known discontinuity
+    void freeze_ff_z() { _limit.freeze_ff_z = true; }
+
+    /// freeze_ff_xy - used to stop the feed forward being calculated during a known discontinuity
+    void freeze_ff_xy() { _limit.freeze_ff_xy = true; }
+
     // is_active_xy - returns true if the xy position controller has been run very recently
     bool is_active_xy() const;
 

libraries/AC_WPNav/AC_WPNav.cpp

@@ -560,6 +560,7 @@ void AC_WPNav::update_wpnav()
         // advance the target if necessary
         advance_wp_target_along_track(dt);
         _pos_control.trigger_xy();
+        _pos_control.freeze_ff_z();
     }else{
         // run horizontal position controller
         _pos_control.update_xy_controller(false);