mc_att_control: yaw dead zone fixed, added MC_YAW_FF (yaw feed-forward) parameter

2 files changed, 24 insertions, 3 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 e37fe18b8..dc3da0f46 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -156,6 +156,9 @@ private:
 		param_t yaw_rate_p;
 		param_t yaw_rate_i;
 		param_t yaw_rate_d;
+		param_t yaw_ff;
+
+		param_t rc_scale_yaw;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -163,6 +166,9 @@ private:
 		math::Vector<3> rate_p;				/**< P gain for angular rate error */
 		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 rc_scale_yaw;
 	}		_params;
 
 	/**
@@ -284,6 +290,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.yaw_rate_p	= 	param_find("MC_YAWRATE_P");
 	_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.rc_scale_yaw	= 	param_find("RC_SCALE_YAW");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -342,6 +351,10 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.yaw_rate_d, &v);
 	_params.rate_d(2) = v;
 
+	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
+
+	param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
+
 	return OK;
 }
 
@@ -461,9 +474,16 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//	reset_yaw_sp = true;
 			//}
 		} else {
-			if (_manual_control_sp.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual_control_sp.yaw) {
+			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
+			if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
 				/* move yaw setpoint */
-				yaw_sp_move_rate = _manual_control_sp.yaw;
+				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
+				if (_manual_control_sp.yaw > 0.0f) {
+					yaw_sp_move_rate -= YAW_DEADZONE;
+				} else {
+					yaw_sp_move_rate += YAW_DEADZONE;
+				}
+				yaw_sp_move_rate *= _params.rc_scale_yaw;
 				_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 				_v_att_sp.R_valid = false;
 				publish_att_sp = true;
@@ -592,7 +612,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	_rates_sp = _params.p.emult(e_R);
 
 	/* feed forward yaw setpoint rate */
-	_rates_sp(2) += yaw_sp_move_rate * yaw_w;
+	_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 a170365ee..0a89c3f4e 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -51,3 +51,4 @@ 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);
+PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);