AC_PosControl: add support for sysid in loiter

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -618,7 +618,10 @@ void AC_PosControl::update_xy_controller()
     // Position Controller
 
     const Vector3f &curr_pos = _inav.get_position_neu_cm();
-    Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, curr_pos);
+    Vector3f comb_pos = curr_pos;
+    comb_pos.x += _disturb_pos.x;
+    comb_pos.y += _disturb_pos.y;
+    Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, comb_pos);
 
     // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
     vel_target *= ahrsControlScaleXY;
@@ -628,7 +631,10 @@ void AC_PosControl::update_xy_controller()
     // Velocity Controller
 
     const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
-    Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), curr_vel, _dt, _limit_vector.xy());
+    Vector2f comb_vel = curr_vel;
+    comb_vel.x += _disturb_vel.x;
+    comb_vel.y += _disturb_vel.y;
+    Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), comb_vel, _dt, _limit_vector.xy());
 
     // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
     accel_target *= ahrsControlScaleXY;
@@ -654,6 +660,9 @@ void AC_PosControl::update_xy_controller()
     // update angle targets that will be passed to stabilize controller
     accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
     calculate_yaw_and_rate_yaw();
+
+    _disturb_pos.zero();
+    _disturb_vel.zero();
 }
 
 

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -394,6 +394,12 @@ class AC_PosControl
     // get earth-frame Z-axis acceleration with gravity removed in cm/s/s with +ve being up
     float get_z_accel_cmss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f; }
 
+    // set disturbance north
+    void set_disturb_pos_cm(Vector2f disturb_pos) {_disturb_pos = disturb_pos;}
+
+    // set disturbance north
+    void set_disturb_vel_cms(Vector2f disturb_vel) {_disturb_vel = disturb_vel;}
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
@@ -447,6 +453,8 @@ class AC_PosControl
     float       _jerk_max_xy_cmsss;       // Jerk limit of the xy kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target
     float       _jerk_max_z_cmsss;        // Jerk limit of the z kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target
     float       _vel_z_control_ratio = 2.0f;    // confidence that we have control in the vertical axis
+    Vector2f    _disturb_pos;
+    Vector2f    _disturb_vel;
 
     // output from controller
     float       _roll_target;           // desired roll angle in centi-degrees calculated by position controller