AC_AttitudeControl: boost PD on roll and pitch when slew limit is hit

boost angle P when slew limit is hit
move to a single parameter for thrust-gain boosting
add PD scaling support

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -150,9 +150,18 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT),
 
+    // @Param: THR_G_BOOST
+    // @DisplayName: Throttle-gain boost
+    // @Description: Throttle-gain boost ratio. A value of 0 means no boosting is applied, a value of 1 means full boosting is applied. Describes the ratio increase that is applied to angle P and PD on pitch and roll.
+    // @Range: 0 1
+    // @User: Advanced
+    AP_GROUPINFO("THR_G_BOOST", 21, AC_AttitudeControl, _throttle_gain_boost, 0.0f),
+
     AP_GROUPEND
 };
 
+constexpr Vector3f AC_AttitudeControl::VECTORF_111;
+
 // get the slew yaw rate limit in deg/s
 float AC_AttitudeControl::get_slew_yaw_max_degs() const
 {
@@ -1006,6 +1015,13 @@ bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const
 Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f &attitude_error_rot_vec_rad)
 {
     Vector3f rate_target_ang_vel;
+
+    // Boost Angle P one very rapid throttle changes
+    if (_motors.get_throttle_slew_rate() > AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH) {
+        float angle_p_boost = constrain_float((_throttle_gain_boost + 1.0f) * (_throttle_gain_boost + 1.0f), 1.0, 4.0);
+        set_angle_P_scale_mult(Vector3f(angle_p_boost, angle_p_boost, 1.0f));
+    }
+
     // Compute the roll angular velocity demand from the roll angle error
     const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x;
     if (_use_sqrt_controller && !is_zero(get_accel_roll_max_radss())) {
@@ -1032,7 +1048,7 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f
 
     // reset angle P scaling, saving used value for logging
     _angle_P_scale_used = _angle_P_scale;
-    _angle_P_scale = Vector3f{1,1,1};
+    _angle_P_scale = VECTORF_111;
 
     return rate_target_ang_vel;
 }

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -44,6 +44,7 @@
 #define AC_ATTITUDE_CONTROL_MAX                         5.0f    // maximum throttle mix default
 
 #define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT             0.5f  // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control.  Higher favours Attitude over pilot input
+#define AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH          1.0f  // default angle-p/pd throttle boost threshold
 
 class AC_AttitudeControl {
 public:
@@ -396,9 +397,20 @@ public:
     // get the value of the angle P scale that was used in the last loop, for logging
     const Vector3f &get_angle_P_scale_logging(void) const { return _angle_P_scale_used; }
     
+    // setup a one loop PD scale multiplier, multiplying by any
+    // previously applied scale from this loop. This allows for more
+    // than one type of scale factor to be applied for different
+    // purposes
+    void set_PD_scale_mult(const Vector3f &pd_scale) { _pd_scale *= pd_scale; }
+
+    // get the value of the PD scale that was used in the last loop, for logging
+    const Vector3f &get_PD_scale_logging(void) const { return _pd_scale_used; }
+
     // User settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
+    static constexpr Vector3f VECTORF_111{1.0f,1.0f,1.0f};
+
 protected:
 
     // Update rate_target_ang_vel using attitude_error_rot_vec_rad
@@ -445,6 +457,9 @@ protected:
     // rate controller input smoothing time constant
     AP_Float            _input_tc;
 
+    // angle_p/pd boost multiplier
+    AP_Float            _throttle_gain_boost;
+
     // Intersampling period in seconds
     float               _dt;
 
@@ -521,6 +536,12 @@ protected:
     // angle scale used for last loop, used for logging
     Vector3f            _angle_P_scale_used;
 
+    // PD scaling vector for roll, pitch, yaw
+    Vector3f            _pd_scale{1,1,1};
+
+    // PD scale used for last loop, used for logging
+    Vector3f            _pd_scale_used;
+
     // References to external libraries
     const AP_AHRS_View&  _ahrs;
     const AP_MultiCopter &_aparm;

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@@ -349,18 +349,27 @@ void AC_AttitudeControl_Multi::rate_controller_run()
 
     Vector3f gyro_latest = _ahrs.get_gyro_latest();
 
-    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll) + _actuator_sysid.x);
+    // Boost PD on very rapid throttle changes
+    if (_motors.get_throttle_slew_rate() > AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH) {
+        const float pd_boost = constrain_float(_throttle_gain_boost + 1.0f, 1.0, 2.0);
+        set_PD_scale_mult(Vector3f(pd_boost, pd_boost, 1.0f));
+    }
+
+    _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x,  _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
     _motors.set_roll_ff(get_rate_roll_pid().get_ff());
 
-    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y, _dt, _motors.limit.pitch) + _actuator_sysid.y);
+    _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, gyro_latest.y,  _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
     _motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
 
-    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z, _dt, _motors.limit.yaw) + _actuator_sysid.z);
+    _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, gyro_latest.z,  _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
     _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
 
     _sysid_ang_vel_body.zero();
     _actuator_sysid.zero();
 
+    _pd_scale_used = _pd_scale;
+    _pd_scale = VECTORF_111;
+
     control_monitor_update();
 }