From 3d27ecca9291004b60a78c3e12286a9726d70407 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 24 May 2016 00:42:14 +0930 Subject: [PATCH] AC_AttitudeControl: add TC for Alt_Hold angle limit --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 1 - libraries/AC_AttitudeControl/AC_AttitudeControl.h | 12 +++++++----- .../AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 13 ++++++++----- .../AC_AttitudeControl/AC_AttitudeControl_Heli.h | 3 +++ .../AC_AttitudeControl_Multi.cpp | 14 +++++++++++--- .../AC_AttitudeControl/AC_AttitudeControl_Multi.h | 3 +++ 6 files changed, 32 insertions(+), 14 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 949e4b0aae..cdd753f7c0 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -595,7 +595,6 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits) void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff) { _throttle_in = throttle_in; - _throttle_in_filt.apply(throttle_in, _dt); _motors.set_throttle_filter_cutoff(filter_cutoff); if (reset_attitude_control) { relax_bf_rate_controller(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 073240e181..50dbc6c83c 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -40,7 +40,8 @@ #define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default -#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_ALTHOLD_LEAN_ANGLE_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude +#define AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX 0.8f // Max throttle 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 @@ -60,7 +61,6 @@ public: _dt(dt), _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), @@ -198,6 +198,9 @@ public: // Enable or disable body-frame feed forward void accel_limiting(bool enable_or_disable); + // Update Alt_Hold angle maximum + virtual void update_althold_lean_angle_max(float throttle_in) = 0; + // Set output throttle virtual void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) = 0; @@ -341,7 +344,6 @@ protected: Vector3f _ang_vel_target_rads; // throttle provided as input to attitude controller. This does not include angle boost. - // Used only for logging. float _throttle_in = 0.0f; // This represents the throttle increase applied for tilt compensation. @@ -351,8 +353,8 @@ protected: // Specifies whether the attitude controller should use the acceleration limit bool _att_ctrl_use_accel_limit; - // Filtered throttle input - used to limit lean angle when throttle is saturated - LowPassFilterFloat _throttle_in_filt; + // Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold + float _althold_lean_angle_max; 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 diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index e6c3afea59..cd18ef98d3 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -235,11 +235,15 @@ void AC_AttitudeControl_Heli::rate_controller_run() // get lean angle max for pilot input that prioritises altitude hold over lean angle float AC_AttitudeControl_Heli::get_althold_lean_angle_max() const { - // calc maximum tilt angle based on throttle - float ret = acosf(constrain_float(_throttle_in_filt.get()/0.9f, 0.0f, 1.0f)); - // TEMP: convert to centi-degrees for public interface - return degrees(ret) * 100.0f; + return degrees(_althold_lean_angle_max) * 100.0f; +} + +// Update Alt_Hold angle maximum +void AC_AttitudeControl_Heli::update_althold_lean_angle_max(float throttle_in) +{ + float althold_lean_angle_max = acos(constrain_float(_throttle_in/AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX, 0.0f, 1.0f)); + _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT))*(_throttle_in-_althold_lean_angle_max); } // @@ -407,7 +411,6 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_rads) void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff) { _throttle_in = throttle_in; - _throttle_in_filt.apply(throttle_in, _dt); _motors.set_throttle_filter_cutoff(filter_cutoff); _motors.set_throttle(throttle_in); // Clear angle_boost for logging purposes diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 51c6d2fc64..2751d6316a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -79,6 +79,9 @@ public: // NOTE: returns centi-degrees float get_althold_lean_angle_max() const; + // Update Alt_Hold angle maximum + void update_althold_lean_angle_max(float throttle_in) override; + // use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage void use_leaky_i(bool leaky_i) { _flags_heli.leaky_i = leaky_i; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 454fcb0e3e..a25443c079 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -149,22 +149,30 @@ AC_AttitudeControl_Multi::AC_AttitudeControl_Multi(AP_AHRS &ahrs, const AP_Vehic // get lean angle max for pilot input that prioritises altitude hold over lean angle float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const +{ + return ToDeg(_althold_lean_angle_max) * 100.0f; +} + +// Update Alt_Hold angle maximum +void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in) { // calc maximum tilt angle based on throttle float thr_max = _motors_multi.get_throttle_thrust_max(); // divide by zero check if (is_zero(thr_max)) { - return 0.0f; + _althold_lean_angle_max = 0.0f; + return; } - return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max), 0.0f, 1.0f))) * 100.0f; + float althold_lean_angle_max = acos(constrain_float(_throttle_in/(AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_THROTTLE_MAX * thr_max), 0.0f, 1.0f)); + _althold_lean_angle_max = _althold_lean_angle_max + (_dt/(_dt+AC_ATTITUDE_CONTROL_ALTHOLD_LEAN_ANGLE_TC_DEFAULT))*(_throttle_in-_althold_lean_angle_max); } void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff) { _throttle_in = throttle_in; - _throttle_in_filt.apply(throttle_in, _dt); + update_althold_lean_angle_max(throttle_in); _motors.set_throttle_filter_cutoff(filter_cutoff); if (apply_angle_boost) { // Apply angle boost diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 1530e53db0..50d1dd873a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -55,6 +55,9 @@ public: // get lean angle max for pilot input that prioritizes altitude hold over lean angle float get_althold_lean_angle_max() const; + // Update Alt_Hold angle maximum + void update_althold_lean_angle_max(float throttle_in) override; + // Set output throttle void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;