AC_AttitudeControl: limit ATC_MOT_MIX_MAX in case of a fly away

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -304,7 +304,7 @@ public:
     // control rpy throttle mix
     virtual void set_throttle_mix_min() {}
     virtual void set_throttle_mix_man() {}
-    virtual void set_throttle_mix_max() {}
+    virtual void set_throttle_mix_max(float ratio) {}
     virtual void set_throttle_mix_value(float value) {}
     virtual float get_throttle_mix(void) const { return 0; }
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@@ -280,6 +280,12 @@ void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_an
     _motors.set_throttle_avg_max(get_throttle_avg_max(MAX(throttle_in, _throttle_in)));
 }
 
+void AC_AttitudeControl_Multi::set_throttle_mix_max(float ratio)
+{
+    ratio = constrain_float(ratio, 0.0f, 1.0f);
+    _throttle_rpy_mix_desired = (1.0f - ratio) * _thr_mix_min + ratio * _thr_mix_max;
+}
+
 // returns a throttle including compensation for roll/pitch angle
 // throttle value should be 0 ~ 1
 float AC_AttitudeControl_Multi::get_throttle_boosted(float throttle_in)

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h

@@ -65,7 +65,7 @@ public:
     //  has no effect when throttle is above hover throttle
     void set_throttle_mix_min() override { _throttle_rpy_mix_desired = _thr_mix_min; }
     void set_throttle_mix_man() override { _throttle_rpy_mix_desired = _thr_mix_man; }
-    void set_throttle_mix_max() override { _throttle_rpy_mix_desired = _thr_mix_max; }
+    void set_throttle_mix_max(float ratio) override;
     void set_throttle_mix_value(float value) override { _throttle_rpy_mix_desired = _throttle_rpy_mix = value; }
     float get_throttle_mix(void) const override { return _throttle_rpy_mix; }
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h

@@ -49,7 +49,7 @@ public:
     //  has no effect when throttle is above hover throttle
     void set_throttle_mix_min() override { _throttle_rpy_mix_desired = _thr_mix_min; }
     void set_throttle_mix_man() override { _throttle_rpy_mix_desired = _thr_mix_man; }
-    void set_throttle_mix_max() override { _throttle_rpy_mix_desired = _thr_mix_max; }
+    void set_throttle_mix_max(float ratio) override { _throttle_rpy_mix_desired = _thr_mix_max; }
 
     // are we producing min throttle?
     bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f*_thr_mix_min); }

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -252,7 +252,8 @@ void AC_PosControl::set_max_speed_z(float speed_down, float speed_up)
     // ensure speed_down is always negative
     speed_down = -fabsf(speed_down);
 
-    if ((fabsf(_speed_down_cms - speed_down) > 1.0f) || (fabsf(_speed_up_cms - speed_up) > 1.0f)) {
+    // only update if there is a minimum of 1cm/s change and is valid
+    if (((fabsf(_speed_down_cms - speed_down) > 1.0f) || (fabsf(_speed_up_cms - speed_up) > 1.0f)) && is_positive(_speed_up_cms) && is_negative(_speed_down_cms) ) {
         _speed_down_cms = speed_down;
         _speed_up_cms = speed_up;
         _flags.recalc_leash_z = true;
@@ -617,6 +618,12 @@ void AC_PosControl::run_z_controller()
 
     // send throttle to attitude controller with angle boost
     _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
+
+    // _speed_down_cms is checked to be non-zero when set
+    float error_ratio = _vel_error.z/_speed_down_cms;
+
+    _vel_z_control_ratio += _dt*0.1f*(0.5-error_ratio);
+    _vel_z_control_ratio = constrain_float(_vel_z_control_ratio, 0.0f, 2.0f);
 }
 
 ///

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -129,6 +129,9 @@ public:
     /// get_alt_error - returns altitude error in cm
     float get_alt_error() const;
 
+    /// get_vel_z_error_ratio - returns the proportion of error relative to the maximum request
+    float get_vel_z_control_ratio() const { return constrain_float(_vel_z_control_ratio, 0.0f, 1.0f); }
+    
     // returns horizontal error in cm
     float get_horizontal_error() const;
 
@@ -403,6 +406,7 @@ protected:
     float       _leash;                 // horizontal leash length in cm.  target will never be further than this distance from the vehicle
     float       _leash_down_z;          // vertical leash down in cm.  target will never be further than this distance below the vehicle
     float       _leash_up_z;            // vertical leash up in cm.  target will never be further than this distance above the vehicle
+    float       _vel_z_control_ratio = 2.0f;   // confidence that we have control in the vertical axis
 
     // output from controller
     float       _roll_target;           // desired roll angle in centi-degrees calculated by position controller