PID rate controller - Add controller gain to support Ideal PID form (…

…ISA standard)
PX4 · Jul 12, 2019 · e85d297 · e85d297
1 parent 7be5d15
commit e85d297
Show file tree

Hide file tree

Showing 3 changed files with 58 additions and 4 deletions.
diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
@@ -216,20 +216,23 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 		(ParamFloat<px4::params::MC_RR_INT_LIM>) _param_mc_rr_int_lim,
 		(ParamFloat<px4::params::MC_ROLLRATE_D>) _param_mc_rollrate_d,
 		(ParamFloat<px4::params::MC_ROLLRATE_FF>) _param_mc_rollrate_ff,
+		(ParamFloat<px4::params::MC_ROLLRATE_K>) _param_mc_rollrate_k,
 
 		(ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
 		(ParamFloat<px4::params::MC_PITCHRATE_P>) _param_mc_pitchrate_p,
 		(ParamFloat<px4::params::MC_PITCHRATE_I>) _param_mc_pitchrate_i,
 		(ParamFloat<px4::params::MC_PR_INT_LIM>) _param_mc_pr_int_lim,
 		(ParamFloat<px4::params::MC_PITCHRATE_D>) _param_mc_pitchrate_d,
 		(ParamFloat<px4::params::MC_PITCHRATE_FF>) _param_mc_pitchrate_ff,
+		(ParamFloat<px4::params::MC_PITCHRATE_K>) _param_mc_pitchrate_k,
 
 		(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
 		(ParamFloat<px4::params::MC_YAWRATE_P>) _param_mc_yawrate_p,
 		(ParamFloat<px4::params::MC_YAWRATE_I>) _param_mc_yawrate_i,
 		(ParamFloat<px4::params::MC_YR_INT_LIM>) _param_mc_yr_int_lim,
 		(ParamFloat<px4::params::MC_YAWRATE_D>) _param_mc_yawrate_d,
 		(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
+		(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
 
 		(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _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
@@ -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
@@ -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
  *