From 1fb4c12cd0cc318fa21f20d755897c665867740d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 9 Jun 2016 18:39:29 +0900 Subject: [PATCH] AC_AttitudeControl: move in throttle vs attitude prioritisation Previously this prioritisation of throttle vs attitude was done in the AP_Motors library --- .../AC_AttitudeControl/AC_AttitudeControl.h | 11 +++++ .../AC_AttitudeControl_Multi.cpp | 41 ++++++++++++++++++- .../AC_AttitudeControl_Multi.h | 22 +++++++++- 3 files changed, 71 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 2e470211ee..258cd8c435 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/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; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 6a05558900..e5e13ac11f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/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); +} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index a47ab3751f..430c34a0c7 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/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) };