mc_att_control: MC_YAWRATE_MAX parameter added

2 files changed, 29 insertions, 6 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 36d95bf06..900c730e1 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -71,7 +71,7 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
-#include <mathlib/mathlib.h>
+#include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 
 /**
@@ -156,6 +156,7 @@ private:
 		param_t yaw_rate_i;
 		param_t yaw_rate_d;
 		param_t yaw_ff;
+		param_t yaw_rate_max;
 
 		param_t man_roll_max;
 		param_t man_pitch_max;
@@ -168,6 +169,7 @@ private:
 		math::Vector<3> rate_i;				/**< I gain for angular rate error */
 		math::Vector<3> rate_d;				/**< D gain for angular rate error */
 		float yaw_ff;						/**< yaw control feed-forward */
+		float yaw_rate_max;					/**< max yaw rate */
 
 		float man_roll_max;
 		float man_pitch_max;
@@ -276,6 +278,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.rate_p.zero();
 	_params.rate_i.zero();
 	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_params.man_roll_max = 0.0f;
+	_params.man_pitch_max = 0.0f;
+	_params.man_yaw_max = 0.0f;
 
 	_rates_prev.zero();
 	_rates_sp.zero();
@@ -298,7 +305,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
 	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
 	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
-
+	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
 	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
 	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
 	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
@@ -367,15 +374,16 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_d(2) = v;
 
 	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
+	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
 
 	/* manual control scale */
 	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
 	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
 	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
-
-	_params.man_roll_max *= M_PI / 180.0;
-	_params.man_pitch_max *= M_PI / 180.0;
-	_params.man_yaw_max *= M_PI / 180.0;
+	_params.man_roll_max = math::radians(_params.man_roll_max);
+	_params.man_pitch_max = math::radians(_params.man_pitch_max);
+	_params.man_yaw_max = math::radians(_params.man_yaw_max);
 
 	return OK;
 }
@@ -626,6 +634,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	/* calculate angular rates setpoint */
 	_rates_sp = _params.att_p.emult(e_R);
 
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
+
 	/* feed forward yaw setpoint rate */
 	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
 }
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 e52c39368..19134c7b4 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -175,6 +175,18 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
 
 /**
+ * Max yaw rate
+ *
+ * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
+
+/**
  * Max manual roll
  *
  * @unit deg