Merge remote-tracking branch 'upstream/master'

2 files changed, 46 insertions, 28 deletions

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 1f0d88bcd..e779239ba 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
@@ -112,32 +112,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 			_reset_yaw_sp = true;
 		}
 
-		/* move yaw setpoint in all modes */
-		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;
-		}
-
 		/* reset yaw setpoint to current position if needed */
 		if (_reset_yaw_sp) {
 			_reset_yaw_sp = false;
@@ -147,6 +121,31 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		}
 
 		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
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 a46163e6f..b7fce5029 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -245,6 +245,10 @@ MulticopterPositionControl::reset_alt_sp()
 		_reset_alt_sp = false;
 		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
 
+		//XXX hack until #1741 is in/ported
+		/* reset yaw sp */
+		_att_sp_msg.data().yaw_body = _att->data().yaw;
+
 		//XXX: port this once a mavlink like interface is available
 		// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
 		PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2));
@@ -297,6 +301,22 @@ 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);
@@ -627,8 +647,7 @@ 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().pitch_body = 0.0f;
-			_att_sp_msg.data().yaw_body = _att->data().yaw;
+			// _att_sp_msg.data().yaw_body = _att->data().yaw;
 			_att_sp_msg.data().thrust = 0.0f;
 
 			_att_sp_msg.data().timestamp = get_time_micros();