From 48f5f8faa5f968e960352af49a07d838fa64a7a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 8 Oct 2018 13:58:56 +0200 Subject: [PATCH] mc pos control: add MPC_THR_CURVE param The hover thrust param also changed the thrust scaling in Stabilized mode. However if the hover thrust is very low (e.g. below 20%), the throttle stick input becomes very distorted. --- .../FlightTaskManualStabilized.cpp | 18 ++++++++++----- .../FlightTaskManualStabilized.hpp | 3 ++- .../mc_pos_control/mc_pos_control_params.c | 22 +++++++++++++++++++ 3 files changed, 36 insertions(+), 7 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp index 4e712355df..239823d8a4 100644 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp @@ -145,16 +145,22 @@ void FlightTaskManualStabilized::_updateSetpoints() float FlightTaskManualStabilized::_throttleCurve() { - /* Scale stick z from [-1,1] to [min thrust, max thrust] - * with hover throttle at 0.5 stick */ + // Scale stick z from [-1,1] to [min thrust, max thrust] float throttle = -((_sticks(2) - 1.0f) * 0.5f); - if (throttle < 0.5f) { - return (_throttle_hover.get() - _throttle_min_stabilized.get()) / 0.5f * throttle + _throttle_min_stabilized.get(); + switch (_throttle_curve.get()) { + case 1: // no rescaling + return throttle; - } else { - return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle - 1.0f) + _throttle_max.get(); + default: // 0 or other: rescale to hover throttle at 0.5 stick + if (throttle < 0.5f) { + return (_throttle_hover.get() - _throttle_min_stabilized.get()) / 0.5f * throttle + _throttle_min_stabilized.get(); + + } else { + return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle - 1.0f) + _throttle_max.get(); + } } + } bool FlightTaskManualStabilized::update() diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp index 41f28c581b..60b9247f34 100644 --- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp +++ b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp @@ -77,6 +77,7 @@ private: (ParamFloat) _tilt_max_man, /**< maximum tilt allowed for manual flight */ (ParamFloat) _throttle_min_stabilized, /**< minimum throttle for stabilized */ (ParamFloat) _throttle_max, /**< maximum throttle that always has to be satisfied in flight*/ - (ParamFloat) _throttle_hover /**< throttle value at which vehicle is at hover equilibrium */ + (ParamFloat) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */ + (ParamInt) _throttle_curve /**< throttle curve behavior */ ) }; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index b0525ad53a..1fa0efcb2f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -71,6 +71,28 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); */ PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); +/** + * Thrust curve in Manual Mode + * + * This parameter defines how the throttle stick input is mapped to commanded thrust + * in Manual/Stabilized flight mode. + * + * In case the default is used ('Rescale to hover thrust'), the stick input is linearly + * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). + * + * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful + * in case the hover thrust is very low and the default would lead to too much distortion + * (e.g. if hover thrust is set to 20%, 80% of the upper thrust range is squeezed into the + * upper half of the stick range). + * + * Note: in case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. + * + * @value 0 Rescale to hover thrust + * @value 1 No Rescale + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_THR_CURVE, 0); + /** * Maximum thrust in auto thrust control *