Merge pull request #1823 from PX4/multiplatform_1741_1801

Multiplatform 1741 and 1801

10 files changed, 137 insertions, 200 deletions

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
index ecbf98c9e..b5a551109 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
@@ -89,9 +89,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 			.yaw_rate_d	= px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT),
 			.yaw_ff		= px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT),
 			.yaw_rate_max	= px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT),
-			.man_roll_max	= px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MC_MAN_R_MAX_DEFAULT),
-			.man_pitch_max	= px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MC_MAN_P_MAX_DEFAULT),
-			.man_yaw_max	= px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MC_MAN_Y_MAX_DEFAULT),
 			.acro_roll_max	= px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT),
 			.acro_pitch_max	= px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT),
 			.acro_yaw_max	= px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT)
@@ -146,11 +143,6 @@ MulticopterAttitudeControl::parameters_update()
 	_params.yaw_ff = _params_handles.yaw_ff.update();
 	_params.yaw_rate_max = math::radians(_params_handles.yaw_rate_max.update());
 
-	/* manual control scale */
-	_params.man_roll_max = math::radians(_params_handles.man_roll_max.update());
-	_params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update());
-	_params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update());
-
 	/* acro control scale */
 	_params.acro_rate_max(0) = math::radians(_params_handles.acro_roll_max.update());
 	_params.acro_rate_max(1) = math::radians(_params_handles.acro_pitch_max.update());
@@ -186,18 +178,6 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti
 	if (_v_control_mode->data().flag_control_attitude_enabled) {
 		control_attitude(dt);
 
-		/* publish the attitude setpoint if needed */
-		if (_publish_att_sp && _v_status->data().is_rotary_wing) {
-			_v_att_sp_mod.data().timestamp = px4::get_time_micros();
-
-			if (_att_sp_pub != nullptr) {
-				_att_sp_pub->publish(_v_att_sp_mod);
-
-			} else {
-				_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
-			}
-		}
-
 		/* publish attitude rates setpoint */
 		_v_rates_sp_mod.data().roll = _rates_sp(0);
 		_v_rates_sp_mod.data().pitch = _rates_sp(1);
@@ -224,9 +204,6 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti
 						    _manual_control_sp->data().r).emult(_params.acro_rate_max);
 			_thrust_sp = _manual_control_sp->data().z;
 
-			/* reset yaw setpoint after ACRO */
-			_reset_yaw_sp = true;
-
 			/* publish attitude rates setpoint */
 			_v_rates_sp_mod.data().roll = _rates_sp(0);
 			_v_rates_sp_mod.data().pitch = _rates_sp(1);
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h
index e44317316..52ba369c1 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h
@@ -110,9 +110,6 @@ private:
 		px4::ParameterFloat yaw_ff;
 		px4::ParameterFloat yaw_rate_max;
 
-		px4::ParameterFloat man_roll_max;
-		px4::ParameterFloat man_pitch_max;
-		px4::ParameterFloat man_yaw_max;
 		px4::ParameterFloat acro_roll_max;
 		px4::ParameterFloat acro_pitch_max;
 		px4::ParameterFloat acro_yaw_max;
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
index e779239ba..5db8f77ac 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -55,7 +55,6 @@ using namespace std;
 #endif
 
 MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
-	_v_att_sp_mod(),
 	_v_rates_sp_mod(),
 	_actuators(),
 	_publish_att_sp(false)
@@ -67,9 +66,6 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_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;
 	_params.acro_rate_max.zero();
 
 	_rates_prev.zero();
@@ -87,101 +83,11 @@ MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
 
 void MulticopterAttitudeControlBase::control_attitude(float dt)
 {
-	float yaw_sp_move_rate = 0.0f;
-	_publish_att_sp = false;
-
-
-	if (_v_control_mode->data().flag_control_manual_enabled) {
-		/* manual input, set or modify attitude setpoint */
-
-		if (_v_control_mode->data().flag_control_velocity_enabled
-		    || _v_control_mode->data().flag_control_climb_rate_enabled) {
-			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			//XXX get rid of memcpy
-			memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
-		}
-
-		if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
-			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
-			_publish_att_sp = true;
-		}
-
-		if (!_armed->data().armed) {
-			/* reset yaw setpoint when disarmed */
-			_reset_yaw_sp = true;
-		}
-
-		/* reset yaw setpoint to current position if needed */
-		if (_reset_yaw_sp) {
-			_reset_yaw_sp = false;
-			_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
-			_v_att_sp_mod.data().R_valid = false;
-			// _publish_att_sp = true;
-		}
-
-		if (!_v_control_mode->data().flag_control_velocity_enabled) {
-
-			if (_v_att_sp_mod.data().thrust < 0.1f) {
-				// TODO
-				//if (_status.condition_landed) {
-				/* reset yaw setpoint if on ground */
-				//	reset_yaw_sp = true;
-				//}
-			} else {
-				/* move yaw setpoint */
-				yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
-				_v_att_sp_mod.data().yaw_body = _wrap_pi(
-							     _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
-				float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-				float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
-
-				if (yaw_offs < -yaw_offs_max) {
-					_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
-
-				} else if (yaw_offs > yaw_offs_max) {
-					_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
-				}
-
-				_v_att_sp_mod.data().R_valid = false;
-				// _publish_att_sp = true;
-			}
-			/* update attitude setpoint if not in position control mode */
-			_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
-			_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
-					       * _params.man_pitch_max;
-			_v_att_sp_mod.data().R_valid = false;
-			// _publish_att_sp = true;
-		}
-
-	} else {
-		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		//XXX get rid of memcpy
-		memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data()));
-
-		/* reset yaw setpoint after non-manual control mode */
-		_reset_yaw_sp = true;
-	}
-
-	_thrust_sp = _v_att_sp_mod.data().thrust;
+	_thrust_sp = _v_att_sp->data().thrust;
 
 	/* construct attitude setpoint rotation matrix */
 	math::Matrix<3, 3> R_sp;
-
-	if (_v_att_sp_mod.data().R_valid) {
-		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp_mod.data().R_body[0]);
-
-	} else {
-		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
-				_v_att_sp_mod.data().yaw_body);
-
-		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
-		       sizeof(_v_att_sp_mod.data().R_body));
-		_v_att_sp_mod.data().R_valid = true;
-	}
+	R_sp.set(_v_att_sp->data().R_body);
 
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
@@ -190,11 +96,11 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	/* all input data is ready, run controller itself */
 
 	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
-	math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
-	math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+	math::Vector <3> R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector <3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
 
 	/* axis and sin(angle) of desired rotation */
-	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
+	math::Vector <3> e_R = R.transposed() * (R_z % R_sp_z);
 
 	/* calculate angle error */
 	float e_R_z_sin = e_R.length();
@@ -209,7 +115,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	if (e_R_z_sin > 0.0f) {
 		/* get axis-angle representation */
 		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
-		math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
+		math::Vector <3> e_R_z_axis = e_R / e_R_z_sin;
 
 		e_R = e_R_z_axis * e_R_z_angle;
 
@@ -224,9 +130,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		e_R_cp(2, 1) = e_R_z_axis(0);
 
 		/* rotation matrix for roll/pitch only rotation */
-		R_rp = R
-		       * (_I + e_R_cp * e_R_z_sin
-			  + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
 
 	} else {
 		/* zero roll/pitch rotation */
@@ -234,8 +138,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	}
 
 	/* R_rp and R_sp has the same Z axis, calculate yaw error */
-	math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
-	math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	math::Vector <3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+	math::Vector <3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
 	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
 
 	if (e_R_z_cos < 0.0f) {
@@ -243,7 +147,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		 * calculate angle and axis for R -> R_sp rotation directly */
 		math::Quaternion q;
 		q.from_dcm(R.transposed() * R_sp);
-		math::Vector < 3 > e_R_d = q.imag();
+		math::Vector <3> e_R_d = q.imag();
 		e_R_d.normalize();
 		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
 
@@ -260,7 +164,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 				       _params.yaw_rate_max);
 
 	/* feed forward yaw setpoint rate */
-	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+	_rates_sp(2) += _v_att_sp->data().yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+
 }
 
 void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
index 124147079..4e33018b4 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
@@ -97,8 +97,6 @@ protected:
 	px4::Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
 	px4::Subscriber<px4_vehicle_status> *_v_status;			    /**< vehicle status */
 
-	px4_vehicle_attitude_setpoint	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
-								  that gets published eventually */
 	px4_vehicle_rates_setpoint	_v_rates_sp_mod;	/**< vehicle rates setpoint
 								  that gets published eventually*/
 	px4_actuator_controls_0		_actuators;	/**< actuator controls */
@@ -121,9 +119,6 @@ protected:
 		float yaw_ff; /**< yaw control feed-forward */
 		float yaw_rate_max; /**< max yaw rate */
 
-		float man_roll_max;
-		float man_pitch_max;
-		float man_yaw_max;
 		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
 
 		int32_t autostart_id;
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
index a0b1706f2..395ae83b0 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
@@ -190,35 +190,6 @@ PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
 PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
 
 /**
- * Max manual roll
- *
- * @unit deg
- * @min 0.0
- * @max 90.0
- * @group Multicopter Attitude Control
- */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
-
-/**
- * Max manual pitch
- *
- * @unit deg
- * @min 0.0
- * @max 90.0
- * @group Multicopter Attitude Control
- */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
-
-/**
- * Max manual yaw rate
- *
- * @unit deg/s
- * @min 0.0
- * @group Multicopter Attitude Control
- */
-PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
-
-/**
  * Max acro roll rate
  *
  * @unit deg/s
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
index 59dd5240f..ff962dbf1 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
@@ -57,9 +57,6 @@
 #define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
 #define PARAM_MC_YAW_FF_DEFAULT 0.5f
 #define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
-#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f
-#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f
-#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f
 #define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
 #define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
 #define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
index 1fe75b90c..2e14b744f 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -41,6 +41,9 @@
 
 #include "mc_pos_control.h"
 #include "mc_pos_control_params.h"
+/* The following inclue is needed because the pos controller depens on a parameter from attitude control to set a
+ * reasonable yaw setpoint in manual mode */
+#include <mc_att_control_multiplatform/mc_att_control_params.h>
 
 #define TILT_COS_MAX	0.7f
 #define SIGMA		0.000001f
@@ -82,7 +85,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
 		.xy_ff		    = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT),
 		.tilt_max_air	    = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT),
 		.land_speed	    = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT),
-		.tilt_max_land	    = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT)
+		.tilt_max_land	    = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT),
+		.man_roll_max	    = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT),
+		.man_pitch_max	    = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT),
+		.man_yaw_max	    = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT),
+		.mc_att_yaw_p	    = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT)
 	}),
 	_ref_alt(0.0f),
 	_ref_timestamp(0),
@@ -99,7 +106,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	 * Do subscriptions
 	 */
 	_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0);
-	_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
 	_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
 	_parameter_update = _n.subscribe<px4_parameter_update>(
 			&MulticopterPositionControl::handle_parameter_update, this, 1000);
@@ -146,6 +152,13 @@ MulticopterPositionControl::parameters_update()
 	_params.land_speed = _params_handles.land_speed.update();
 	_params.tilt_max_land = math::radians(_params_handles.tilt_max_land.update());
 
+	/* manual control scale */
+	_params.man_roll_max = math::radians(_params_handles.man_roll_max.update());
+	_params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update());
+	_params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update());
+
+	_params.mc_att_yaw_p = _params_handles.mc_att_yaw_p.update();
+
 	_params.pos_p(0) = _params.pos_p(1) = _params_handles.xy_p.update();
 	_params.pos_p(2) = _params_handles.z_p.update();
 	_params.vel_p(0) = _params.vel_p(1) = _params_handles.xy_vel_p.update();
@@ -287,22 +300,6 @@ MulticopterPositionControl::control_manual(float dt)
 		_sp_move_rate /= sp_move_norm;
 	}
 
-	/* move yaw setpoint */
-	//XXX hardcoded hack until #1741 is in/ported (the values stem
-	//from default param values, see how yaw setpoint is moved in the attitude controller)
-	float yaw_sp_move_rate = _manual_control_sp->data().r * 120.0f * M_DEG_TO_RAD_F;
-	_att_sp_msg.data().yaw_body = _wrap_pi(
-			_att_sp_msg.data().yaw_body + yaw_sp_move_rate * dt);
-	float yaw_offs_max = 120.0f * M_DEG_TO_RAD_F / 2.0f;
-	float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
-
-	if (yaw_offs < -yaw_offs_max) {
-		_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - yaw_offs_max);
-
-	} else if (yaw_offs > yaw_offs_max) {
-		_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + yaw_offs_max);
-	}
-
 	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
 	math::Matrix<3, 3> R_yaw_sp;
 	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
@@ -569,6 +566,7 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 	static bool reset_int_z = true;
 	static bool reset_int_z_manual = false;
 	static bool reset_int_xy = true;
+	static bool reset_yaw_sp = true;
 	static bool was_armed = false;
 	static uint64_t t_prev = 0;
 
@@ -582,8 +580,10 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 		_reset_alt_sp = true;
 		reset_int_z = true;
 		reset_int_xy = true;
+		reset_yaw_sp = true;
 	}
 
+	/* Update previous arming state */
 	was_armed = _control_mode->data().flag_armed;
 
 	update_ref();
@@ -627,7 +627,8 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 			_att_sp_msg.data().R_valid = true;
 
 			_att_sp_msg.data().roll_body = 0.0f;
-			// _att_sp_msg.data().yaw_body = _att->data().yaw;
+			_att_sp_msg.data().pitch_body = 0.0f;
+			_att_sp_msg.data().yaw_body = _att->data().yaw;
 			_att_sp_msg.data().thrust = 0.0f;
 
 			_att_sp_msg.data().timestamp = get_time_micros();
@@ -967,6 +968,61 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 		reset_int_xy = true;
 	}
 
+	/* generate attitude setpoint from manual controls */
+	if(_control_mode->data().flag_control_manual_enabled && _control_mode->data().flag_control_attitude_enabled) {
+
+		/* reset yaw setpoint to current position if needed */
+		if (reset_yaw_sp) {
+			reset_yaw_sp = false;
+			_att_sp_msg.data().yaw_body = _att->data().yaw;
+		}
+
+		/* do not move yaw while arming */
+		else if (_manual_control_sp->data().z > 0.1f)
+		{
+			const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
+
+			_att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
+			_att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt);
+			float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
+			if (yaw_offs < - YAW_OFFSET_MAX) {
+				_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX);
+
+			} else if (yaw_offs > YAW_OFFSET_MAX) {
+				_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX);
+			}
+		}
+
+		/* Control roll and pitch directly if we no aiding velocity controller is active */
+		if(!_control_mode->data().flag_control_velocity_enabled) {
+			_att_sp_msg.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
+			_att_sp_msg.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max;
+		}
+
+		/* Control climb rate directly if no aiding altitude controller is active */
+		if(!_control_mode->data().flag_control_climb_rate_enabled) {
+			_att_sp_msg.data().thrust = _manual_control_sp->data().z;
+		}
+
+		/* Construct attitude setpoint rotation matrix */
+		math::Matrix<3,3> R_sp;
+		R_sp.from_euler(_att_sp_msg.data().roll_body,_att_sp_msg.data().pitch_body,_att_sp_msg.data().yaw_body);
+		_att_sp_msg.data().R_valid = true;
+		memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body));
+		_att_sp_msg.data().timestamp = get_time_micros();
+	}
+	else {
+		reset_yaw_sp = true;
+	}
+
+	/* publish attitude setpoint */
+	if (_att_sp_pub != nullptr) {
+		_att_sp_pub->publish(_att_sp_msg);
+
+	} else {
+		_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+	}
+
 	/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
 	reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled;
 }
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
index b754ccf01..54c6de155 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
@@ -88,19 +88,18 @@ protected:
 
 	Publisher<px4_vehicle_attitude_setpoint>	*_att_sp_pub;			/**< attitude setpoint publication */
 	Publisher<px4_vehicle_local_position_setpoint>	*_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
-	Publisher<px4_vehicle_global_velocity_setpoint>		*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
+	Publisher<px4_vehicle_global_velocity_setpoint>	*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
 
-	Subscriber<px4_vehicle_attitude> *_att;			    /**< vehicle attitude */
-	Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp;	    /**< vehicle attitude setpoint */
-	Subscriber<px4_vehicle_control_mode> *_control_mode;	    /**< vehicle control mode */
-	Subscriber<px4_parameter_update> *_parameter_update;	    /**< parameter update */
-	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;   /**< manual control setpoint */
-	Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
-	Subscriber<px4_vehicle_local_position> *_local_pos;			    /**< local position */
-	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;			    /**< local position */
-	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;			    /**< local position */
-	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;			    /**< local position */
+	Subscriber<px4_vehicle_attitude> *_att;				    /**< vehicle attitude */
+	Subscriber<px4_vehicle_control_mode> *_control_mode;		    /**< vehicle control mode */
+	Subscriber<px4_parameter_update> *_parameter_update;		    /**< parameter update */
+	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;	    /**< manual control setpoint */
+	Subscriber<px4_actuator_armed> *_armed;				    /**< actuator arming status */
+	Subscriber<px4_vehicle_local_position> *_local_pos;		    /**< local position */
+	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;	    /**< local position */
+	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;	    /**< local position */
+	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;   /**< local position */
 
 	px4_vehicle_attitude_setpoint _att_sp_msg;
 	px4_vehicle_local_position_setpoint _local_pos_sp_msg;
@@ -126,6 +125,10 @@ protected:
 		px4::ParameterFloat tilt_max_air;
 		px4::ParameterFloat land_speed;
 		px4::ParameterFloat tilt_max_land;
+		px4::ParameterFloat man_roll_max;
+		px4::ParameterFloat man_pitch_max;
+		px4::ParameterFloat man_yaw_max;
+		px4::ParameterFloat mc_att_yaw_p;   // needed for calculating reasonable attitude setpoints in manual
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -134,6 +137,10 @@ protected:
 		float tilt_max_air;
 		float land_speed;
 		float tilt_max_land;
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
+		float mc_att_yaw_p;
 
 		math::Vector<3> pos_p;
 		math::Vector<3> vel_p;
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
index c741a7f0a..709085662 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c
@@ -210,3 +210,32 @@ PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND);
  */
 PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED);
 
+/**
+ * Max manual roll
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX);
+
+/**
+ * Max manual pitch
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX);
+
+/**
+ * Max manual yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX);
+
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
index fec3bcb7c..8c8b707ae 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h
@@ -58,4 +58,7 @@
 #define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f
 #define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f
 #define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f
+#define PARAM_MPC_MAN_R_MAX_DEFAULT 35.0f
+#define PARAM_MPC_MAN_P_MAX_DEFAULT 35.0f
+#define PARAM_MPC_MAN_Y_MAX_DEFAULT 120.0f