diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ec099b4a100a..05481080b826 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 @@ class MulticopterAttitudeControl : public ModuleBase (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 @@ class MulticopterAttitudeControl : public ModuleBase (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 @@ class MulticopterAttitudeControl : public ModuleBase (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 */ 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 ec6472afdba6..3bc41a1d320b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -99,11 +99,20 @@ MulticopterAttitudeControl::parameters_updated() // Store some of the parameters in a more convenient way & precompute often-used values _attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get())); - // rate gains - _rate_p = Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get()); - _rate_i = Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get()); + // Rate gains + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const float p_k = _param_mc_rollrate_k.get(); + const float q_k = _param_mc_pitchrate_k.get(); + const float r_k = _param_mc_yawrate_k.get(); + + _rate_p = Vector3f(_param_mc_rollrate_p.get() * p_k, _param_mc_pitchrate_p.get() * q_k, + _param_mc_yawrate_p.get() * r_k); + _rate_i = Vector3f(_param_mc_rollrate_i.get() * p_k, _param_mc_pitchrate_i.get() * q_k, + _param_mc_yawrate_i.get() * r_k); _rate_int_lim = Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()); - _rate_d = Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()); + _rate_d = Vector3f(_param_mc_rollrate_d.get() * p_k, _param_mc_pitchrate_d.get() * q_k, + _param_mc_yawrate_d.get() * r_k); _rate_ff = Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()); if (fabsf(_lp_filters_d.get_cutoff_freq() - _param_mc_dterm_cutoff.get()) > 0.01f) { 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 d6049f0d8e8e..0b811c9deb38 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,20 @@ 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. Increase if the response is sluggish, decrease if + * it is overshoothing. + * + * @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 +202,20 @@ 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. Increase if the response is sluggish, decrease if + * it is overshoothing. + * + * @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 +291,20 @@ 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. Increase if the response is sluggish, decrease if + * it is overshoothing. + * + * @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 *