From c2be4b2b2993d56adb04c3f638ce4eaacb88c6de Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 27 Jan 2017 14:17:43 +0100 Subject: [PATCH] mc_pos_control: added exponential curve to manual x,y velocity setpoint controlled by parameter MPC_XY_MAN_EXPO that is disabled (0) by default --- .../mc_pos_control/mc_pos_control_main.cpp | 10 ++++++++-- .../mc_pos_control/mc_pos_control_params.c | 15 +++++++++++++++ 2 files changed, 23 insertions(+), 2 deletions(-) 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 4673ec8145..ec880958c1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -202,6 +202,7 @@ private: param_t acc_down_max; param_t alt_mode; param_t opt_recover; + param_t xy_vel_man_expo; } _params_handles; /**< handles for interesting parameters */ @@ -228,6 +229,7 @@ private: float acc_down_max; float vel_max_up; float vel_max_down; + float xy_vel_man_expo; uint32_t alt_mode; int opt_recover; @@ -523,6 +525,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.acc_down_max = param_find("MPC_ACC_DOWN_MAX"); _params_handles.alt_mode = param_find("MPC_ALT_MODE"); _params_handles.opt_recover = param_find("VT_OPT_RECOV_EN"); + _params_handles.xy_vel_man_expo = param_find("MPC_XY_MAN_EXPO"); /* fetch initial parameter values */ parameters_update(true); @@ -638,6 +641,9 @@ MulticopterPositionControl::parameters_update(bool force) _params.acc_up_max = v; param_get(_params_handles.acc_down_max, &v); _params.acc_down_max = v; + param_get(_params_handles.xy_vel_man_expo, &v); + _params.xy_vel_man_expo = v; + /* * increase the maximum horizontal acceleration such that stopping * within 1 s from full speed is feasible @@ -944,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(0) = _manual.x; - req_vel_sp(1) = _manual.y; + req_vel_sp(0) = math::expo(_manual.x, _params.xy_vel_man_expo); + req_vel_sp(1) = math::expo(_manual.y, _params.xy_vel_man_expo); } if (_control_mode.flag_control_altitude_enabled) { 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 80202f63c9..68b87d2550 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -492,3 +492,18 @@ PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 5.0f); * @group Multicopter Position Control */ PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); + +/** + * Manual control stick exponential curve sensitivity attenuation with small velocity setpoints + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * @min 0 + * @max 1 + * @value 0 Purely linear input curve (default) + * @value 1 Purely cubic input curve + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.0f);