fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 00:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

add breakpoint and slope params for TPA

Browse Source
This commit is contained in:
Mark Whitehorn
2016-08-17 11:52:45 -06:00
committed by Lorenz Meier
parent 5c78af0f36
commit f1a1c9d7da
2 changed files with 41 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+13 -1
View File
@@ -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;
src/modules/mc_att_control/mc_att_control_params.c
+28
View File
@@ -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);