From 9defb72733cf2cafd704f18a97b464963e3d7a7d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 23 Nov 2021 15:51:01 +1100 Subject: [PATCH] AC_AttitudeControl: fixed limiting of throttle mix values use constrain instead of reset to keep as much of user requested value as possible also raise limit for MIX_MIN to 0.5 after discussion with Leonard --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 2 ++ .../AC_AttitudeControl/AC_AttitudeControl_Multi.cpp | 10 +++++----- .../AC_AttitudeControl/AC_AttitudeControl_Sub.cpp | 10 +++++----- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 67ed8edd25..8c5ef6c49e 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -39,6 +39,8 @@ #define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix default #define AC_ATTITUDE_CONTROL_MAN_DEFAULT 0.1f // manual throttle mix default #define AC_ATTITUDE_CONTROL_MAX_DEFAULT 0.5f // maximum throttle mix default +#define AC_ATTITUDE_CONTROL_MIN_LIMIT 0.5f // min throttle mix upper limit +#define AC_ATTITUDE_CONTROL_MAN_LIMIT 4.0f // man throttle mix upper limit #define AC_ATTITUDE_CONTROL_MAX 5.0f // 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 diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index d28206cd8f..0ac8e41b90 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -354,18 +354,18 @@ void AC_AttitudeControl_Multi::rate_controller_run() void AC_AttitudeControl_Multi::parameter_sanity_check() { // sanity check throttle mix parameters - if (_thr_mix_man < 0.1f || _thr_mix_man > 4.0f) { + if (_thr_mix_man < 0.1f || _thr_mix_man > AC_ATTITUDE_CONTROL_MAN_LIMIT) { // parameter description recommends thr-mix-man be no higher than 0.9 but we allow up to 4.0 // which can be useful for very high powered copters with very low hover throttle - _thr_mix_man.set_and_save(AC_ATTITUDE_CONTROL_MAN_DEFAULT); + _thr_mix_man.set_and_save(constrain_float(_thr_mix_man, 0.1, AC_ATTITUDE_CONTROL_MAN_LIMIT)); } - if (_thr_mix_min < 0.1f || _thr_mix_min > 0.25f) { - _thr_mix_min.set_and_save(AC_ATTITUDE_CONTROL_MIN_DEFAULT); + if (_thr_mix_min < 0.1f || _thr_mix_min > AC_ATTITUDE_CONTROL_MIN_LIMIT) { + _thr_mix_min.set_and_save(constrain_float(_thr_mix_min, 0.1, AC_ATTITUDE_CONTROL_MIN_LIMIT)); } if (_thr_mix_max < 0.5f || _thr_mix_max > AC_ATTITUDE_CONTROL_MAX) { // parameter description recommends thr-mix-max be no higher than 0.9 but we allow up to 5.0 // which can be useful for very high powered copters with very low hover throttle - _thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT); + _thr_mix_max.set_and_save(constrain_float(_thr_mix_max, 0.5, AC_ATTITUDE_CONTROL_MAX)); } if (_thr_mix_min > _thr_mix_max) { _thr_mix_min.set_and_save(AC_ATTITUDE_CONTROL_MIN_DEFAULT); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp index 8e9b184c0e..bd0e6e30e5 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp @@ -367,18 +367,18 @@ void AC_AttitudeControl_Sub::rate_controller_run() void AC_AttitudeControl_Sub::parameter_sanity_check() { // sanity check throttle mix parameters - if (_thr_mix_man < 0.1f || _thr_mix_man > 4.0f) { + if (_thr_mix_man < 0.1f || _thr_mix_man > AC_ATTITUDE_CONTROL_MAN_LIMIT) { // parameter description recommends thr-mix-man be no higher than 0.9 but we allow up to 4.0 // which can be useful for very high powered copters with very low hover throttle - _thr_mix_man.set_and_save(AC_ATTITUDE_CONTROL_MAN_DEFAULT); + _thr_mix_man.set_and_save(constrain_float(_thr_mix_man, 0.1, AC_ATTITUDE_CONTROL_MAN_LIMIT)); } - if (_thr_mix_min < 0.1f || _thr_mix_min > 0.25f) { - _thr_mix_min.set_and_save(AC_ATTITUDE_CONTROL_MIN_DEFAULT); + if (_thr_mix_min < 0.1f || _thr_mix_min > AC_ATTITUDE_CONTROL_MIN_LIMIT) { + _thr_mix_min.set_and_save(constrain_float(_thr_mix_min, 0.1, AC_ATTITUDE_CONTROL_MIN_LIMIT)); } if (_thr_mix_max < 0.5f || _thr_mix_max > AC_ATTITUDE_CONTROL_MAX) { // parameter description recommends thr-mix-max be no higher than 0.9 but we allow up to 5.0 // which can be useful for very high powered copters with very low hover throttle - _thr_mix_max.set_and_save(AC_ATTITUDE_CONTROL_MAX_DEFAULT); + _thr_mix_max.set_and_save(constrain_float(_thr_mix_max, 0.5, AC_ATTITUDE_CONTROL_MAX)); } if (_thr_mix_min > _thr_mix_max) { _thr_mix_min.set_and_save(AC_ATTITUDE_CONTROL_MIN_DEFAULT);