AC_PosControl: avoidance wip

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -611,6 +611,15 @@ void AC_PosControl::stop_vel_xy_stabilisation()
     _pid_vel_xy.reset_I();
 }
 
+/// stop at specified position to avoid hitting something
+void AC_PosControl::set_avoidance_pos_xy(const Vector2p& stop_pos_xy_cm)
+{
+    Vector2f vel_input;
+    Vector2f accel_input;
+    shape_pos_vel_accel_xy(stop_pos_xy_cm, vel_input, accel_input, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(),
+                           _vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, false);
+}
+
 // is_active_xy - returns true if the xy position controller has bee n run in the previous 5 loop times
 bool AC_PosControl::is_active_xy() const
 {

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -138,6 +138,9 @@ class AC_PosControl
     /// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system
     void stop_vel_xy_stabilisation();
 
+    /// stop at specified position to avoid hitting somethingn
+    void set_avoidance_pos_xy(const Vector2p& stop_pos_xy_cm);
+
     /// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors.
     ///     Position and velocity errors are converted to velocity and acceleration targets using PID objects
     ///     Desired velocity and accelerations are added to these corrections as they are calculated