AC_AttitudeControl: added set_xy_control_scale_factor()

allows for systemID to reduce XY gains in pos controlled modes

Author: tridge

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -679,6 +679,10 @@ void AC_PosControl::update_xy_controller()
     // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
     vel_target *= ahrsControlScaleXY;
 
+    if (_xy_control_scale_factor > 0) {
+        vel_target *= _xy_control_scale_factor;
+    }
+
     _vel_target.xy() = vel_target;
     _vel_target.xy() += _vel_desired.xy() + _vel_offset.xy();
 
@@ -694,6 +698,12 @@ void AC_PosControl::update_xy_controller()
     // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
     accel_target *= ahrsControlScaleXY;
 
+    if (_xy_control_scale_factor > 0) {
+        accel_target *= _xy_control_scale_factor;
+    }
+    
+    _xy_control_scale_factor = 0;
+
     // pass the correction acceleration to the target acceleration output
     _accel_target.xy() = accel_target;
     _accel_target.xy() += _accel_desired.xy() + _accel_offset.xy();

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -140,6 +140,11 @@ 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();
 
+    /// set a single loop XY control scale factor. Set to zero to disable
+    void set_xy_control_scale_factor(float xy_scale_factor) {
+        _xy_control_scale_factor = xy_scale_factor;
+    }
+
     /// 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
@@ -540,6 +545,7 @@ class AC_PosControl
     float       _vel_z_control_ratio = 2.0f;    // confidence that we have control in the vertical axis
     Vector2f    _disturb_pos;           // position disturbance generated by system ID mode
     Vector2f    _disturb_vel;           // velocity disturbance generated by system ID mode
+    float       _xy_control_scale_factor; // single loop scale factor for XY control
 
     // output from controller
     float       _roll_target;           // desired roll angle in centi-degrees calculated by position controller