From f61a6c81fe3060503587768b9de2dbbb7a257032 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 18 Jun 2019 01:06:22 +0930 Subject: [PATCH] AC_AttitudeControl: limit ATC_MOT_MIX_MAX in case of a fly away --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 2 +- .../AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 6 ++++++ libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h | 2 +- libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h | 2 +- libraries/AC_AttitudeControl/AC_PosControl.cpp | 9 ++++++++- libraries/AC_AttitudeControl/AC_PosControl.h | 4 ++++ 6 files changed, 21 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index a4e143338f..f02393e74c 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/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; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 4069a2bdcb..17a654a76d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/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) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index b174ab77a1..28bccb33cf 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/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; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h index 13c3193131..b460daf26b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.h +++ b/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); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index abc367276e..9aae19e4dc 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/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); } /// diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index f681af2a1c..6384a4dfb1 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/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