From 8fd27fddcdf53559b67f25642bd4aaee905ac3e4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 22 Feb 2017 11:53:05 +0100 Subject: [PATCH] mc_pos_control: moved responsibility of the x,y stick deadzone to a mathematical function to prevent a setpoint step when moving the stick over the border of the deadzone and to enable very small inputs even from a control stick that needs a big deadzone --- src/lib/mathlib/math/Expo.hpp | 24 ++++++++++++++++++- .../mc_pos_control/mc_pos_control_main.cpp | 10 ++++---- 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/src/lib/mathlib/math/Expo.hpp b/src/lib/mathlib/math/Expo.hpp index 0b858a7d40..c418cd71b8 100644 --- a/src/lib/mathlib/math/Expo.hpp +++ b/src/lib/mathlib/math/Expo.hpp @@ -35,7 +35,7 @@ * @file Expo.hpp * * So called exponential curve function implementation. - * It's essentially a linear combination between a linear and a cubic function. + * It is essentially a linear combination between a linear and a cubic function. */ #pragma once @@ -46,6 +46,11 @@ namespace math { +// Type-safe signum function +template int sign(T val) { + return (T(0) < val) - (val < T(0)); +} + template inline const _Tp expo(const _Tp &value, const _Tp &e) { @@ -53,4 +58,21 @@ inline const _Tp expo(const _Tp &value, const _Tp &e) return (1-e)*x + e*x*x*x; } +template +inline const _Tp deadzone(const _Tp &value, const _Tp &dz) +{ + _Tp x = constrain(value ,(_Tp)-1, (_Tp)1); + // Rescale the input such that we get a piecewise linear function that will be continuous with applied deadzone + _Tp out = (x-sign(x)*dz)/(1-dz); + // apply the deadzone (values zero around the middle) + return out * (fabsf(x) > dz); +} + +template +inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz) +{ + _Tp x = constrain(value ,(_Tp)-1, (_Tp)1); + return expo(deadzone(x, dz),e); +} + } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 31b999e29b..d53e694f40 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -950,8 +950,8 @@ MulticopterPositionControl::control_manual(float dt) if (_control_mode.flag_control_position_enabled) { /* set horizontal velocity setpoint with roll/pitch stick */ - req_vel_sp_xy(0) = math::expo(_manual.x, _params.xy_vel_man_expo); - req_vel_sp_xy(1) = math::expo(_manual.y, _params.xy_vel_man_expo); + req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_xy_dz); + req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_xy_dz); /* reset position setpoint to current position if needed */ reset_pos_sp(); @@ -1017,15 +1017,15 @@ MulticopterPositionControl::control_manual(float dt) if (_pos_hold_engaged) { /* only switch back to velocity control if user moves stick */ - _pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < _params.hold_xy_dz) - && (fabsf(req_vel_sp_xy(1)) < _params.hold_xy_dz); + _pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON) + && (fabsf(req_vel_sp_xy(1)) < FLT_EPSILON); } else { /* check if we switch to pos_hold_engaged */ float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)); bool smooth_pos_transition = _control_mode.flag_control_position_enabled && - (fabsf(req_vel_sp_xy(0)) < _params.hold_xy_dz && fabsf(req_vel_sp_xy(1)) < _params.hold_xy_dz) && + (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON && fabsf(req_vel_sp_xy(1)) < FLT_EPSILON) && (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy); /* during transition predict setpoint forward */