AC_PosControl: add set_target_to_stopping_point_xy method

Fixed bug in get_stopping_point_xy in which it would update Z-axis
target if vehicle was moving less than 10cm/s horizontally

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -136,6 +136,9 @@ float AC_PosControl::get_alt_error() const
 /// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home
 void AC_PosControl::set_target_to_stopping_point_z()
 {
+    // check if z leash needs to be recalculated
+    calc_leash_length_z();
+
     get_stopping_point_z(_pos_target);
 }
 
@@ -383,6 +386,15 @@ void AC_PosControl::set_pos_target(const Vector3f& position)
     //_pitch_target = constrain_int32(_ahrs.pitch_sensor,-_attitude_control.lean_angle_max(),_attitude_control.lean_angle_max());
 }
 
+/// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
+void AC_PosControl::set_target_to_stopping_point_xy()
+{
+    // check if xy leash needs to be recalculated
+    calc_leash_length_xy();
+
+    get_stopping_point_xy(_pos_target);
+}
+
 /// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
 ///     distance_max allows limiting distance to stopping point
 ///     results placed in stopping_position vector
@@ -402,7 +414,8 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
 
     // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
     if (vel_total < 10.0f || kP <= 0.0f || _accel_cms <= 0.0f) {
-        stopping_point = curr_pos;
+        stopping_point.x = curr_pos.x;
+        stopping_point.y = curr_pos.y;
         return;
     }
 

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -166,6 +166,9 @@ public:
     ///     when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step
     void update_xy_controller(bool use_desired_velocity);
 
+    /// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
+    void set_target_to_stopping_point_xy();
+
     /// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
     ///     distance_max allows limiting distance to stopping point
     ///		results placed in stopping_position vector