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

…ISA standard)

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 */
 
@@ -281,6 +284,7 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 	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] */

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;

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
  *