AC_AttitudeControl: move in throttle vs attitude prioritisation

Previously this prioritisation of throttle vs attitude was done in the
AP_Motors library

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -42,6 +42,12 @@
 
 #define AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ   1.0f    // filter (in hz) of throttle filter used to limit lean angle so that vehicle does not lose altitude
 
+#define AC_ATTITUDE_CONTROL_MIN_DEFAULT   0.1f    // minimum throttle mix
+#define AC_ATTITUDE_CONTROL_MID_DEFAULT   0.5f    // manual throttle mix
+#define AC_ATTITUDE_CONTROL_MAX_DEFAULT   0.5f    // 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
+
 class AC_AttitudeControl {
 public:
     AC_AttitudeControl( AP_AHRS &ahrs,
@@ -55,6 +61,8 @@ public:
         _angle_boost(0),
         _att_ctrl_use_accel_limit(true),
         _throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ),
+        _throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
+        _throttle_rpy_mix(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
         _ahrs(ahrs),
         _aparm(aparm),
         _motors(motors)
@@ -346,6 +354,9 @@ protected:
     // Filtered throttle input - used to limit lean angle when throttle is saturated
     LowPassFilterFloat  _throttle_in_filt;
 
+    float               _throttle_rpy_mix_desired;  // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
+    float               _throttle_rpy_mix;          // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
+
     // References to external libraries
     const AP_AHRS&      _ahrs;
     const AP_Vehicle::MultiCopter &_aparm;

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@@ -120,6 +120,20 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = {
     // @Units: Hz
     AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 3, AC_AttitudeControl_Multi, AC_PID),
 
+    // @Param: THR_MIX_MIN
+    // @DisplayName: Throttle Mix Minimum
+    // @Description: Throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
+    // @Range: 0.1 0.25
+    // @User: Advanced
+    AP_GROUPINFO("THR_MIX_MIN", 4, AC_AttitudeControl_Multi, _thr_mix_min, AC_ATTITUDE_CONTROL_MIN_DEFAULT),
+
+    // @Param: THR_MIX_MAX
+    // @DisplayName: Throttle Mix Maximum
+    // @Description: Throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
+    // @Range: 0.5 0.9
+    // @User: Advanced
+    AP_GROUPINFO("THR_MIX_MAX", 5, AC_AttitudeControl_Multi, _thr_mix_max, AC_ATTITUDE_CONTROL_MAX_DEFAULT),
+
     AP_GROUPEND
 };
 
@@ -153,12 +167,14 @@ void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_an
     _throttle_in_filt.apply(throttle_in, _dt);
     _motors.set_throttle_filter_cutoff(filter_cutoff);
     if (apply_angle_boost) {
-        _motors.set_throttle(get_throttle_boosted(throttle_in));
+        // Apply angle boost
+        throttle_in = get_throttle_boosted(throttle_in);
     }else{
-        _motors.set_throttle(throttle_in);
         // Clear angle_boost for logging purposes
         _angle_boost = 0.0f;
     }
+    _motors.set_throttle(throttle_in);
+    _motors.set_throttle_ave_max(get_throttle_ave_max(throttle_in));
 }
 
 // returns a throttle including compensation for roll/pitch angle
@@ -180,3 +196,24 @@ float AC_AttitudeControl_Multi::get_throttle_boosted(float throttle_in)
     _angle_boost = constrain_float(throttle_out - throttle_in,-1.0f,1.0f);
     return throttle_out;
 }
+
+// returns a throttle including compensation for roll/pitch angle
+// throttle value should be 0 ~ 1
+float AC_AttitudeControl_Multi::get_throttle_ave_max(float throttle_in)
+{
+    return MAX(throttle_in, throttle_in*MAX(0.0f,1.0f-_throttle_rpy_mix)+_motors.get_throttle_hover()*_throttle_rpy_mix);
+}
+
+// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
+void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
+{
+    // slew _throttle_rpy_mix to _throttle_rpy_mix_desired
+    if (_throttle_rpy_mix < _throttle_rpy_mix_desired) {
+        // increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
+        _throttle_rpy_mix += MIN(2.0f*_dt, _throttle_rpy_mix_desired-_throttle_rpy_mix);
+    } else if (_throttle_rpy_mix > _throttle_rpy_mix_desired) {
+        // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
+        _throttle_rpy_mix -= MIN(0.5f*_dt, _throttle_rpy_mix-_throttle_rpy_mix_desired);
+    }
+    _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, 1.0f);
+}

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h

@@ -52,7 +52,7 @@ public:
     AC_PID& get_rate_pitch_pid() { return _pid_rate_pitch; }
     AC_PID& get_rate_yaw_pid() { return _pid_rate_yaw; }
 
-    // get lean angle max for pilot input that prioritises altitude hold over lean angle
+    // get lean angle max for pilot input that prioritizes altitude hold over lean angle
     float get_althold_lean_angle_max() const;
 
     // Set output throttle
@@ -61,13 +61,33 @@ public:
     // calculate total body frame throttle required to produce the given earth frame throttle
     float get_throttle_boosted(float throttle_in);
 
+    // calculate total body frame throttle required to produce the given earth frame throttle
+    float get_throttle_ave_max(float throttle_in);
+
+    // set desired throttle vs attitude mixing (actual mix is slewed towards this value over 1~2 seconds)
+    //  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
+    //  has no effect when throttle is above hover throttle
+    void set_throttle_mix_min() { _throttle_rpy_mix_desired = _thr_mix_min; }
+    void set_throttle_mix_mid() { _throttle_rpy_mix_desired = AC_ATTITUDE_CONTROL_MID_DEFAULT; }
+    void set_throttle_mix_max() { _throttle_rpy_mix_desired = _thr_mix_max; }
+
+    // get_throttle_rpy_mix - get low throttle compensation value
+    bool is_throttle_mix_min() const { return (_throttle_rpy_mix < 1.25f*_thr_mix_min); }
+
+
     // user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
 
+    // update_throttle_rpy_mix - updates thr_low_comp value towards the target
+    void update_throttle_rpy_mix();
+
     AP_MotorsMulticopter& _motors_multi;
     AC_PID                _pid_rate_roll;
     AC_PID                _pid_rate_pitch;
     AC_PID                _pid_rate_yaw;
+
+    AP_Float              _thr_mix_min;     // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
+    AP_Float              _thr_mix_max;     // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
 };