From 8b09eaf1244a1d90e8345926f3430361e577ae16 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 3 Oct 2017 09:01:47 +0200 Subject: [PATCH] mc_att_control: use SuperExpo input curve for acro mode to further enhance the stick feel --- .../mc_att_control/mc_att_control_main.cpp | 12 ++++++++---- .../mc_att_control/mc_att_control_params.c | 18 +++++++++++++++++- 2 files changed, 25 insertions(+), 5 deletions(-) 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 0d827b273b..810e561064 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -219,6 +219,7 @@ private: param_t acro_pitch_max; param_t acro_yaw_max; param_t acro_expo; + param_t acro_superexpo; param_t rattitude_thres; param_t vtol_type; @@ -258,7 +259,8 @@ private: math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ math::Vector<3> auto_rate_max; /**< attitude rate limits in auto modes */ matrix::Vector3f acro_rate_max; /**< max attitude rates in acro mode */ - float acro_expo; + float acro_expo; /**< function parameter for expo stick curve shape */ + float acro_superexpo; /**< function parameter for superexpo stick curve shape */ float rattitude_thres; int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ bool vtol_opt_recovery_enabled; @@ -495,6 +497,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); _params_handles.acro_expo = param_find("MC_ACRO_EXPO"); + _params_handles.acro_superexpo = param_find("MC_ACRO_SUPEXPO"); _params_handles.rattitude_thres = param_find("MC_RATT_TH"); _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.roll_tc = param_find("MC_ROLL_TC"); @@ -644,6 +647,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); param_get(_params_handles.acro_expo, &_params.acro_expo); + param_get(_params_handles.acro_superexpo, &_params.acro_superexpo); /* stick deflection needed in rattitude mode to control rates not angles */ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); @@ -1246,9 +1250,9 @@ MulticopterAttitudeControl::task_main() if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ matrix::Vector3f man_rate_sp; - man_rate_sp(0) = math::expo(_manual_control_sp.y, _params.acro_expo); - man_rate_sp(1) = math::expo(-_manual_control_sp.x, _params.acro_expo); - man_rate_sp(2) = math::expo(_manual_control_sp.r, _params.acro_expo); + man_rate_sp(0) = math::superexpo(_manual_control_sp.y, _params.acro_expo, _params.acro_superexpo); + man_rate_sp(1) = math::superexpo(-_manual_control_sp.x, _params.acro_expo, _params.acro_superexpo); + man_rate_sp(2) = math::superexpo(_manual_control_sp.r, _params.acro_expo, _params.acro_superexpo); man_rate_sp = man_rate_sp.emult(_params.acro_rate_max); _rates_sp = math::Vector<3>(man_rate_sp.data()); _thrust_sp = _manual_control_sp.z; diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 248574a8bf..e1032200ab 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -406,7 +406,8 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 700.0f); PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 700.0f); /** - * Acro expo factor + * Acro Expo factor + * applied to input of all axis: roll, pitch, yaw * * 0 Purely linear input curve * 1 Purely cubic input curve @@ -418,6 +419,21 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 700.0f); */ PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); +/** + * Acro SuperExpo factor + * applied to input of all axis: roll, pitch, yaw + * + * 0 Pure Expo function + * 0.7 resonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); + /** * Threshold for Rattitude mode *