From 0b391fdcfcd332d05fe4ad06cd1e0b9c6aa10a7c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 Jul 2020 14:58:37 +0200 Subject: [PATCH] mc_att_control: add gradual3 function to cover hover thrust rescaling --- src/lib/mathlib/math/Functions.hpp | 25 +++++++++++++++++++ .../mc_att_control/mc_att_control_main.cpp | 13 +++------- 2 files changed, 29 insertions(+), 9 deletions(-) diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 05c4c1f451..530c4b98ac 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -147,4 +147,29 @@ const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, } } +/* + * Constant, linear, linear, constant function with the three corner points as parameters + * y_high ------- + * / + * / + * y_middle / + * / + * / + * / + * y_low ------- + * x_low x_middle x_high + */ +template +const T gradual3(const T &value, + const T &x_low, const T &x_middle, const T &x_high, + const T &y_low, const T &y_middle, const T &y_high) +{ + if (value < x_middle) { + return gradual(value, x_low, x_middle, y_low, y_middle); + + } else { + return gradual(value, x_middle, x_high, y_middle, y_high); + } +} + } /* namespace math */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 0dfa87f38c..0af4095381 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -107,7 +107,7 @@ MulticopterAttitudeControl::parameters_updated() float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { - float throttle_min = _vehicle_land_detected.landed ? 0.0f : _param_mpc_manthr_min.get(); + const float throttle_min = _vehicle_land_detected.landed ? 0.0f : _param_mpc_manthr_min.get(); // throttle_stick_input is in range [0, 1] switch (_param_mpc_thr_curve.get()) { @@ -115,14 +115,9 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) return throttle_min + throttle_stick_input * (_param_mpc_thr_max.get() - throttle_min); default: // 0 or other: rescale to hover throttle at 0.5 stick - if (throttle_stick_input < 0.5f) { - return (_param_mpc_thr_hover.get() - throttle_min) / 0.5f * throttle_stick_input + - throttle_min; - - } else { - return (_param_mpc_thr_max.get() - _param_mpc_thr_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) + - _param_mpc_thr_max.get(); - } + return math::gradual3(throttle_stick_input, + 0.f, .5f, 1.f, + throttle_min, _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()); } }