mc_att_control: ATTRATE_I / YAWRATE_I implemented

2 files changed, 60 insertions, 39 deletions

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 e8f89923d..20c7f8859 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -86,6 +86,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 
 #define MIN_TAKEOFF_THROTTLE	0.3f
 #define YAW_DEADZONE	0.01f
+#define RATES_I_LIMIT	0.5f
 
 class MulticopterAttitudeControl
 {
@@ -133,18 +134,21 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	math::Matrix<3, 3> _K;					/**< diagonal gain matrix for position error */
-	math::Matrix<3, 3> _K_rate_p;				/**< diagonal gain matrix for angular rate error */
-	math::Matrix<3, 3> _K_rate_d;				/**< diagonal gain matrix for angular rate derivative */
+	math::Vector<3> _K_p;					/**< P gain for position error */
+	math::Vector<3> _K_rate_p;				/**< P gain for angular rate error */
+	math::Vector<3> _K_rate_i;				/**< I gain for angular rate error */
+	math::Vector<3> _K_rate_d;				/**< D gain for angular rate error */
 
 	math::Vector<3> _rates_prev;			/**< angular rates on previous step */
 
 	struct {
 		param_t att_p;
+		param_t yaw_p;
 		param_t att_rate_p;
+		param_t att_rate_i;
 		param_t att_rate_d;
-		param_t yaw_p;
 		param_t yaw_rate_p;
+		param_t yaw_rate_i;
 		param_t yaw_rate_d;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -229,17 +233,19 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	memset(&_control_mode, 0, sizeof(_control_mode));
 	memset(&_arming, 0, sizeof(_arming));
 
-	_K.zero();
+	_K_p.zero();
 	_K_rate_p.zero();
 	_K_rate_d.zero();
 
 	_rates_prev.zero();
 
 	_parameter_handles.att_p		= 	param_find("MC_ATT_P");
+	_parameter_handles.yaw_p		=	param_find("MC_YAW_P");
 	_parameter_handles.att_rate_p	= 	param_find("MC_ATTRATE_P");
+	_parameter_handles.att_rate_i	= 	param_find("MC_ATTRATE_I");
 	_parameter_handles.att_rate_d	= 	param_find("MC_ATTRATE_D");
-	_parameter_handles.yaw_p		=	param_find("MC_YAWPOS_P");
 	_parameter_handles.yaw_rate_p	= 	param_find("MC_YAWRATE_P");
+	_parameter_handles.yaw_rate_i	= 	param_find("MC_YAWRATE_I");
 	_parameter_handles.yaw_rate_d	= 	param_find("MC_YAWRATE_D");
 
 	/* fetch initial parameter values */
@@ -273,31 +279,31 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
 int
 MulticopterAttitudeControl::parameters_update()
 {
-	float att_p;
-	float att_rate_p;
-	float att_rate_d;
-	float yaw_p;
-	float yaw_rate_p;
-	float yaw_rate_d;
-
-	param_get(_parameter_handles.att_p, &att_p);
-	param_get(_parameter_handles.att_rate_p, &att_rate_p);
-	param_get(_parameter_handles.att_rate_d, &att_rate_d);
-	param_get(_parameter_handles.yaw_p, &yaw_p);
-	param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p);
-	param_get(_parameter_handles.yaw_rate_d, &yaw_rate_d);
-
-	_K(0, 0) = att_p;
-	_K(1, 1) = att_p;
-	_K(2, 2) = yaw_p;
-
-	_K_rate_p(0, 0) = att_rate_p;
-	_K_rate_p(1, 1) = att_rate_p;
-	_K_rate_p(2, 2) = yaw_rate_p;
-
-	_K_rate_d(0, 0) = att_rate_d;
-	_K_rate_d(1, 1) = att_rate_d;
-	_K_rate_d(2, 2) = yaw_rate_d;
+	float v;
+
+	param_get(_parameter_handles.att_p, &v);
+	_K_p(0) = v;
+	_K_p(1) = v;
+	param_get(_parameter_handles.yaw_p, &v);
+	_K_p(2) = v;
+
+	param_get(_parameter_handles.att_rate_p, &v);
+	_K_rate_p(0) = v;
+	_K_rate_p(1) = v;
+	param_get(_parameter_handles.yaw_rate_p, &v);
+	_K_rate_p(2) = v;
+
+	param_get(_parameter_handles.att_rate_i, &v);
+	_K_rate_i(0) = v;
+	_K_rate_i(1) = v;
+	param_get(_parameter_handles.yaw_rate_i, &v);
+	_K_rate_i(2) = v;
+
+	param_get(_parameter_handles.att_rate_d, &v);
+	_K_rate_d(0) = v;
+	_K_rate_d(1) = v;
+	param_get(_parameter_handles.yaw_rate_d, &v);
+	_K_rate_d(2) = v;
 
 	return OK;
 }
@@ -403,6 +409,10 @@ MulticopterAttitudeControl::task_main()
 	math::Vector<3> rates;
 	rates.zero();
 
+	/* angular rates integral error */
+	math::Vector<3> rates_int;
+	rates_int.zero();
+
 	/* identity matrix */
 	math::Matrix<3, 3> I;
 	I.identity();
@@ -621,13 +631,24 @@ MulticopterAttitudeControl::task_main()
 			}
 
 			/* angular rates setpoint*/
-			math::Vector<3> rates_sp = _K * e_R;
+			math::Vector<3> rates_sp = _K_p.emult(e_R);
 
 			/* feed forward yaw setpoint rate */
 			rates_sp(2) += yaw_sp_move_rate * yaw_w;
-			math::Vector<3> control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f);
+			math::Vector<3> rates_err = rates_sp - rates;
+			math::Vector<3> control = _K_rate_p.emult(rates_err) + _K_rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int;
 			_rates_prev = rates;
 
+			/* update integral */
+			for (int i = 0; i < 3; i++) {
+				float rate_i = rates_int(i) + _K_rate_i(i) * rates_err(i) * dt;
+				if (isfinite(rate_i)) {
+					if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) {
+						rates_int(i) = rate_i;
+					}
+				}
+			}
+
 			/* publish the attitude rates setpoint */
 			_rates_sp.roll = rates_sp(0);
 			_rates_sp.pitch = rates_sp(1);
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 299cef6c8..a170365ee 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -41,13 +41,13 @@
 
 #include <systemlib/param/param.h>
 
-PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
-PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f);
 PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
-PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
+PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);