From a2c0391bcce3e035fe9b0421c852eb18d481a337 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Tue, 15 Nov 2016 01:02:27 -0800 Subject: [PATCH] Rework TPA to have per-component setup and use more stable and intuitive function This also adds a ZMR250 config. --- ROMFS/px4fmu_common/init.d/4002_qavr5 | 4 +- ROMFS/px4fmu_common/init.d/4009_qav250 | 4 +- ROMFS/px4fmu_common/init.d/4050_generic_250 | 4 +- ROMFS/px4fmu_common/init.d/4080_zmr250 | 55 ++++++++ ROMFS/px4fmu_common/mixers/zmr250.main.mix | 10 ++ .../mc_att_control/mc_att_control_main.cpp | 72 ++++++++-- .../mc_att_control/mc_att_control_params.c | 124 +++++++++++++----- 7 files changed, 222 insertions(+), 51 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/4080_zmr250 create mode 100644 ROMFS/px4fmu_common/mixers/zmr250.main.mix diff --git a/ROMFS/px4fmu_common/init.d/4002_qavr5 b/ROMFS/px4fmu_common/init.d/4002_qavr5 index 5f1ca5904d..dd8908beea 100644 --- a/ROMFS/px4fmu_common/init.d/4002_qavr5 +++ b/ROMFS/px4fmu_common/init.d/4002_qavr5 @@ -28,8 +28,8 @@ then param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.5 - param set MC_TPA_BREAK 0.7 - param set MC_TPA_SLOPE 1.0 + param set MC_TPA_BREAK_P 0.7 + param set MC_TPA_RATE_P 0.3 param set PWM_MIN 1075 param set MPC_THR_MIN 0.06 param set MPC_MANTHR_MIN 0.06 diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 index f579400b83..cee544525e 100644 --- a/ROMFS/px4fmu_common/init.d/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -39,6 +39,6 @@ then param set MPC_MANTHR_MIN 0.06 param set CBRK_IO_SAFETY 22027 param set ATT_BIAS_MAX 0.0 - param set MC_TPA_BREAK 0.5 - param set MC_TPA_SLOPE 1.0 + param set MC_TPA_BREAK_P 0.5 + param set MC_TPA_RATE_P 0.5 fi diff --git a/ROMFS/px4fmu_common/init.d/4050_generic_250 b/ROMFS/px4fmu_common/init.d/4050_generic_250 index 1d81866b5a..62e06ccfc2 100644 --- a/ROMFS/px4fmu_common/init.d/4050_generic_250 +++ b/ROMFS/px4fmu_common/init.d/4050_generic_250 @@ -39,8 +39,8 @@ then param set MC_YAWRATE_MAX 400.0 param set MC_ACRO_R_MAX 360.0 param set MC_ACRO_P_MAX 360.0 - param set MC_TPA_BREAK 0.5 - param set MC_TPA_SLOPE 1.0 + param set MC_TPA_BREAK_P 0.5 + param set MC_TPA_RATE_P 0.5 param set PWM_MIN 1075 diff --git a/ROMFS/px4fmu_common/init.d/4080_zmr250 b/ROMFS/px4fmu_common/init.d/4080_zmr250 new file mode 100644 index 0000000000..57fd3a29be --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4080_zmr250 @@ -0,0 +1,55 @@ +#!nsh +# +# @name ZMR250 Racer +# +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# +# @maintainer Anton Matosov +# + +sh /etc/init.d/rc.mc_defaults + +set MIXER zmr250 +set PWM_OUT 1234 + +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 2.0 + param set MC_ROLLRATE_P 0.05 + param set MC_ROLLRATE_I 0.2 + param set MC_ROLLRATE_D 0.0015 + param set MC_ROLL_TC 0.18 + + param set MC_PITCH_P 2.0 + param set MC_PITCHRATE_P 0.05 + param set MC_PITCHRATE_I 0.2 + param set MC_PITCHRATE_D 0.0015 + param set MC_PITCH_TC 0.18 + + param set MC_YAW_P 1.0 + param set MC_YAWRATE_P 0.15 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.5 + + param set MC_ACRO_R_MAX 1080.0 + param set MC_ACRO_P_MAX 1080.0 + param set MC_ACRO_Y_MAX 1080.0 + + param set MC_TPA_BREAK_P 0.5 + param set MC_TPA_BREAK_D 0.7 + param set MC_TPA_RATE_P 0.5 + param set MC_TPA_RATE_D 0.5 + + param set PWM_MIN 1075 + param set PWM_RATE 400 + param set PWM_DISARMED 900 + param set FAILSAFE 100 + + # param set NAV_RCL_ACT 6 # Lockdown + + param set CBRK_IO_SAFETY 22027 +fi diff --git a/ROMFS/px4fmu_common/mixers/zmr250.main.mix b/ROMFS/px4fmu_common/mixers/zmr250.main.mix new file mode 100644 index 0000000000..dd743cc501 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/zmr250.main.mix @@ -0,0 +1,10 @@ +# R: +R: 4x 7654 10000 10000 0 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 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 2b0a9988f1..8e6365270f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -100,6 +100,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +#define TPA_RATE_LOWER_LIMIT 0.05f #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f #define ATTITUDE_TC_DEFAULT 0.2f @@ -188,8 +189,12 @@ 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 tpa_breakpoint_p; + param_t tpa_breakpoint_i; + param_t tpa_breakpoint_d; + param_t tpa_rate_p; + param_t tpa_rate_i; + param_t tpa_rate_d; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; @@ -224,8 +229,12 @@ 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 tpa_breakpoint_p; /**< Throttle PID Attenuation breakpoint */ + float tpa_breakpoint_i; /**< Throttle PID Attenuation breakpoint */ + float tpa_breakpoint_d; /**< Throttle PID Attenuation breakpoint */ + float tpa_rate_p; /**< Throttle PID Attenuation slope */ + float tpa_rate_i; /**< Throttle PID Attenuation slope */ + float tpa_rate_d; /**< Throttle PID Attenuation slope */ float roll_rate_max; float pitch_rate_max; @@ -289,6 +298,11 @@ private: */ void control_attitude_rates(float dt); + /** + * Throttle PID attenuation. + */ + math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_rate); + /** * Check for vehicle status updates. */ @@ -398,8 +412,12 @@ 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.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P"); + _params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I"); + _params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D"); + _params_handles.tpa_rate_p = param_find("MC_TPA_RATE_P"); + _params_handles.tpa_rate_i = param_find("MC_TPA_RATE_I"); + _params_handles.tpa_rate_d = param_find("MC_TPA_RATE_D"); _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"); @@ -496,10 +514,12 @@ 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; + param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p); + param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i); + param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d); + param_get(_params_handles.tpa_rate_p, &_params.tpa_rate_p); + param_get(_params_handles.tpa_rate_i, &_params.tpa_rate_i); + param_get(_params_handles.tpa_rate_d, &_params.tpa_rate_d); /* yaw gains */ param_get(_params_handles.yaw_p, &v); @@ -790,6 +810,27 @@ MulticopterAttitudeControl::control_attitude(float dt) } } +/* + * Throttle PID attenuation + * Function visualization available here https://www.desmos.com/calculator/gn4mfoddje + * Input: 'tpa_breakpoint', 'tpa_rate', '_thrust_sp' + * Output: 'pidAttenuationPerAxis' vector + */ +math::Vector<3> +MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate) +{ + /* throttle pid attenuation factor */ + float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint); + tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa)); + + math::Vector<3> pidAttenuationPerAxis; + pidAttenuationPerAxis(AXIS_INDEX_ROLL) = tpa; + pidAttenuationPerAxis(AXIS_INDEX_PITCH) = tpa; + pidAttenuationPerAxis(AXIS_INDEX_YAW) = 1.0; + + return pidAttenuationPerAxis; +} + /* * Attitude rates controller. * Input: '_rates_sp' vector, '_thrust_sp' @@ -809,13 +850,16 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) rates(1) = _ctrl_state.pitch_rate; rates(2) = _ctrl_state.yaw_rate; - /* throttle pid attenuation factor */ - float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint))); + math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p)); + math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i)); + math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d)); /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + + _att_control = rates_p_scaled.emult(rates_err) + + _rates_int + + rates_d_scaled.emult(_rates_prev - rates) / dt + _params.rate_ff.emult(_rates_sp); _rates_sp_prev = _rates_sp; @@ -825,7 +869,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) { for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + float rate_i = _rates_int(i) + rates_i_scaled(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT && 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 d529e31f5c..9f6722833a 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -72,6 +72,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_TC, 0.2f); * * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. * + * @unit 1/s * @min 0.0 * @max 8 * @decimal 2 @@ -138,7 +139,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); * @min 0.0 * @max 10 * @decimal 2 - * @increment 0.0005 + * @increment 0.1 * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); @@ -377,36 +378,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f); 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); - - -/** - * Whether to scale outputs by battery power level + * Battery power level scaler * * This compensates for voltage drop of the battery over time by attempting to * normalize performance across the operating range of the battery. The copter @@ -418,3 +390,93 @@ PARAM_DEFINE_FLOAT(MC_TPA_SLOPE, 1.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); + +/** + * TPA P Breakpoint + * + * 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_P, 1.0f); + +/** + * TPA I Breakpoint + * + * Throttle PID Attenuation (TPA) + * Magnitude of throttle setpoint at which to begin attenuating roll/pitch I gain + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_TPA_BREAK_I, 1.0f); + +/** + * TPA D Breakpoint + * + * Throttle PID Attenuation (TPA) + * Magnitude of throttle setpoint at which to begin attenuating roll/pitch D gain + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_TPA_BREAK_D, 1.0f); + +/** + * TPA Rate P + * + * 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 - rate * (throttle - breakpoint) / (1.0 - breakpoint)) + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_TPA_RATE_P, 0.0f); + +/** + * TPA Rate I + * + * Throttle PID Attenuation (TPA) + * Rate at which to attenuate roll/pitch I gain + * Attenuation factor is 1.0 when throttle magnitude is below the setpoint + * Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint)) + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_TPA_RATE_I, 0.0f); + +/** + * TPA Rate D + * + * Throttle PID Attenuation (TPA) + * Rate at which to attenuate roll/pitch D gain + * Attenuation factor is 1.0 when throttle magnitude is below the setpoint + * Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint)) + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f);