From f1a1c9d7daa6712dc39906babc57004b0e72d47e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 17 Aug 2016 11:52:45 -0600 Subject: [PATCH] add breakpoint and slope params for TPA --- .../mc_att_control/mc_att_control_main.cpp | 14 +++++++++- .../mc_att_control/mc_att_control_params.c | 28 +++++++++++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) 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 69e5ebe484..dd486c5347 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -185,6 +185,8 @@ private: param_t pitch_rate_i; param_t pitch_rate_d; param_t pitch_rate_ff; + param_t tpa_breakpoint; + param_t tpa_slope; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; @@ -217,6 +219,9 @@ private: math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ + float tpa_breakpoint; /**< Throttle PID Attenuation breakpoint */ + float tpa_slope; /**< Throttle PID Attenuation slope */ + float roll_rate_max; float pitch_rate_max; float yaw_rate_max; @@ -381,6 +386,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); + _params_handles.tpa_breakpoint = param_find("MC_TPA_BREAK"); + _params_handles.tpa_slope = param_find("MC_TPA_SLOPE"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); @@ -477,6 +484,11 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; + param_get(_params_handles.tpa_breakpoint, &v); + _params.tpa_breakpoint = v; + param_get(_params_handles.tpa_slope, &v); + _params.tpa_slope = v; + /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; @@ -772,7 +784,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) rates(2) = _ctrl_state.yaw_rate; /* throttle pid attenuation factor */ - float tpa = fminf(1.0f, 1.5f - fabsf(_v_rates_sp.thrust)); + float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint))); /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; 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 a631d9c1d4..5105abc275 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -375,3 +375,31 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f); + +/** + * Threshold for Throttle PID Attenuation (TPA) + * + * Magnitude of throttle setpoint at which to begin attenuating roll/pitch P gain + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_TPA_BREAK, 1.0f); + +/** + * Slope for Throttle PID Attenuation (TPA) + * + * Rate at which to attenuate roll/pitch P gain + * Attenuation factor is 1.0 when throttle magnitude is below the setpoint + * Above the setpoint, the attenuation factor is (1 - slope*(abs(throttle)-breakpoint)) + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_TPA_SLOPE, 1.0f);