From 8ecd1d9a37ddfc10fe8e8d73d6cfb2f0e9d6f2fa Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 29 Jan 2023 11:14:36 +0100 Subject: [PATCH] AC_AttitudeControl: move THR_G_BOOST to Multicopter only refactor PD and Angle P boosting into update_throttle_gain_boost() call update_throttle_gain_boost() from rate_controller_run() --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 13 --------- .../AC_AttitudeControl/AC_AttitudeControl.h | 3 -- .../AC_AttitudeControl_Multi.cpp | 29 +++++++++++++++---- .../AC_AttitudeControl_Multi.h | 6 ++++ 4 files changed, 29 insertions(+), 22 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 0985cd84b0..e1dfc24312 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -150,13 +150,6 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @User: Standard AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT), - // @Param: THR_G_BOOST - // @DisplayName: Throttle-gain boost - // @Description: Throttle-gain boost ratio. A value of 0 means no boosting is applied, a value of 1 means full boosting is applied. Describes the ratio increase that is applied to angle P and PD on pitch and roll. - // @Range: 0 1 - // @User: Advanced - AP_GROUPINFO("THR_G_BOOST", 21, AC_AttitudeControl, _throttle_gain_boost, 0.0f), - AP_GROUPEND }; @@ -1016,12 +1009,6 @@ Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f { Vector3f rate_target_ang_vel; - // Boost Angle P one very rapid throttle changes - if (_motors.get_throttle_slew_rate() > AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH) { - float angle_p_boost = constrain_float((_throttle_gain_boost + 1.0f) * (_throttle_gain_boost + 1.0f), 1.0, 4.0); - set_angle_P_scale_mult(Vector3f(angle_p_boost, angle_p_boost, 1.0f)); - } - // Compute the roll angular velocity demand from the roll angle error const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x; if (_use_sqrt_controller && !is_zero(get_accel_roll_max_radss())) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 03f6f29412..b9edc4a170 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -457,9 +457,6 @@ protected: // rate controller input smoothing time constant AP_Float _input_tc; - // angle_p/pd boost multiplier - AP_Float _throttle_gain_boost; - // Intersampling period in seconds float _dt; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 75326e0a34..d17b2ec17d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -232,6 +232,13 @@ const AP_Param::GroupInfo AC_AttitudeControl_Multi::var_info[] = { // @User: Advanced AP_GROUPINFO("THR_MIX_MAN", 6, AC_AttitudeControl_Multi, _thr_mix_man, AC_ATTITUDE_CONTROL_MAN_DEFAULT), + // @Param: THR_G_BOOST + // @DisplayName: Throttle-gain boost + // @Description: Throttle-gain boost ratio. A value of 0 means no boosting is applied, a value of 1 means full boosting is applied. Describes the ratio increase that is applied to angle P and PD on pitch and roll. + // @Range: 0 1 + // @User: Advanced + AP_GROUPINFO("THR_G_BOOST", 7, AC_AttitudeControl_Multi, _throttle_gain_boost, 0.0f), + AP_GROUPEND }; @@ -312,6 +319,19 @@ float AC_AttitudeControl_Multi::get_throttle_avg_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_gain_boost - boost angle_p/pd each cycle on high throttle slew +void AC_AttitudeControl_Multi::update_throttle_gain_boost() +{ + // Boost PD and Angle P on very rapid throttle changes + if (_motors.get_throttle_slew_rate() > AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH) { + const float pd_boost = constrain_float(_throttle_gain_boost + 1.0f, 1.0, 2.0); + set_PD_scale_mult(Vector3f(pd_boost, pd_boost, 1.0f)); + + const float angle_p_boost = constrain_float((_throttle_gain_boost + 1.0f) * (_throttle_gain_boost + 1.0f), 1.0, 4.0); + set_angle_P_scale_mult(Vector3f(angle_p_boost, angle_p_boost, 1.0f)); + } +} + // update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value void AC_AttitudeControl_Multi::update_throttle_rpy_mix() { @@ -342,6 +362,9 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix() void AC_AttitudeControl_Multi::rate_controller_run() { + // boost angle_p/pd each cycle on high throttle slew + update_throttle_gain_boost(); + // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration) update_throttle_rpy_mix(); @@ -349,12 +372,6 @@ void AC_AttitudeControl_Multi::rate_controller_run() Vector3f gyro_latest = _ahrs.get_gyro_latest(); - // Boost PD on very rapid throttle changes - if (_motors.get_throttle_slew_rate() > AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH) { - const float pd_boost = constrain_float(_throttle_gain_boost + 1.0f, 1.0, 2.0); - set_PD_scale_mult(Vector3f(pd_boost, pd_boost, 1.0f)); - } - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, gyro_latest.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); _motors.set_roll_ff(get_rate_roll_pid().get_ff()); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 5977b6ff06..883c6193bf 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -83,6 +83,9 @@ public: protected: + // boost angle_p/pd each cycle on high throttle slew + void update_throttle_gain_boost(); + // update_throttle_rpy_mix - updates thr_low_comp value towards the target void update_throttle_rpy_mix(); @@ -97,4 +100,7 @@ protected: AP_Float _thr_mix_man; // throttle vs attitude control prioritisation used when using manual throttle (higher values mean we prioritise attitude control over throttle) 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) + + // angle_p/pd boost multiplier + AP_Float _throttle_gain_boost; };