From d24c415fd7a8f63351c903bada8e9ad9f372799a Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 12 Jul 2019 14:43:10 +0200 Subject: [PATCH] PID rate controller - Add controller gain to support Ideal PID form (ISA standard) --- src/modules/mc_att_control/mc_att_control.hpp | 4 ++ .../mc_att_control/mc_att_control_main.cpp | 10 +++- .../mc_att_control/mc_att_control_params.c | 60 +++++++++++++++++++ 3 files changed, 71 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ec099b4a10..d6019cb281 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -216,6 +216,7 @@ private: (ParamFloat) _param_mc_rr_int_lim, (ParamFloat) _param_mc_rollrate_d, (ParamFloat) _param_mc_rollrate_ff, + (ParamFloat) _param_mc_rollrate_k, (ParamFloat) _param_mc_pitch_p, (ParamFloat) _param_mc_pitchrate_p, @@ -223,6 +224,7 @@ private: (ParamFloat) _param_mc_pr_int_lim, (ParamFloat) _param_mc_pitchrate_d, (ParamFloat) _param_mc_pitchrate_ff, + (ParamFloat) _param_mc_pitchrate_k, (ParamFloat) _param_mc_yaw_p, (ParamFloat) _param_mc_yawrate_p, @@ -230,6 +232,7 @@ private: (ParamFloat) _param_mc_yr_int_lim, (ParamFloat) _param_mc_yawrate_d, (ParamFloat) _param_mc_yawrate_ff, + (ParamFloat) _param_mc_yawrate_k, (ParamFloat) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */ @@ -281,6 +284,7 @@ private: matrix::Vector3f _rate_int_lim; /**< integrator state limit for rate loop */ matrix::Vector3f _rate_d; /**< D gain for angular rate error */ matrix::Vector3f _rate_ff; /**< Feedforward gain for desired rates */ + matrix::Vector3f _rate_k; /**< Rate controller global gain */ matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ 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 ec6472afdb..8cef9d335b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -106,6 +106,10 @@ MulticopterAttitudeControl::parameters_updated() _rate_d = Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()); _rate_ff = Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()); + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + _rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); + if (fabsf(_lp_filters_d.get_cutoff_freq() - _param_mc_dterm_cutoff.get()) > 0.01f) { _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); _lp_filters_d.reset(_rates_prev); @@ -446,9 +450,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* apply low-pass filtering to the rates for D-term */ Vector3f rates_filtered(_lp_filters_d.apply(rates)); - _att_control = rates_p_scaled.emult(rates_err) + - _rates_int - - rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt + + _att_control = _rate_k.emult(rates_p_scaled.emult(rates_err) + + _rates_int - + rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt) + _rate_ff.emult(_rates_sp); _rates_prev = 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 d6049f0d8e..52304cdc47 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -114,6 +114,26 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); +/** + * Roll rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error + * + MC_ROLLRATE_I * error_integral + * + MC_ROLLRATE_D * error_derivative) + * Set MC_ROLLRATE_P=1 to implement a PID in the ideal form. + * Set MC_ROLLRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f); + /** * Pitch P gain * @@ -188,6 +208,26 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); +/** + * Pitch rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error + * + MC_PITCHRATE_I * error_integral + * + MC_PITCHRATE_D * error_derivative) + * Set MC_PITCHRATE_P=1 to implement a PID in the ideal form. + * Set MC_PITCHRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f); + /** * Yaw P gain * @@ -263,6 +303,26 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); +/** + * Yaw rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_YAWRATE_K * (MC_YAWRATE_P * error + * + MC_YAWRATE_I * error_integral + * + MC_YAWRATE_D * error_derivative) + * Set MC_YAWRATE_P=1 to implement a PID in the ideal form. + * Set MC_YAWRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); + /** * Max roll rate *