From 759d2a3dff05b56aacb616294a9253cdc77a5ed6 Mon Sep 17 00:00:00 2001
From: tumbili <bapstr@ethz.ch>
Date: Mon, 8 Jun 2015 15:51:09 +0200
Subject: [PATCH 1/9] mc position control: - directly use commanded velocity
 for control - use position error to derive desired velocity when in position
 hold mode

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 61 +++++++++++++++++--
 1 file changed, 57 insertions(+), 4 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 36d87b9383..ec833fc3c4 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -90,6 +90,9 @@
 #define SIGMA			0.000001f
 #define MIN_DIST		0.01f
 #define MANUAL_THROTTLE_MAX_MULTICOPTER	0.9f
+#define VEL_XY_THRESH 	0.5f	// max xy velocity for which pos hold in xy is engaged
+#define VEL_Z_THRESH 	0.5f	// max z velocity for which pos hold in z is engaged
+#define VEL_CMD_THRESH 	0.1f	// min velocity (in xy/z) which is interpreted as velocity command
 
 /**
  * Multicopter position control app start / stop handling function
@@ -206,6 +209,8 @@ private:
 	bool _reset_pos_sp;
 	bool _reset_alt_sp;
 	bool _mode_auto;
+	bool _pos_hold_engaged;
+	bool _alt_hold_engaged;
 
 	math::Vector<3> _pos;
 	math::Vector<3> _pos_sp;
@@ -325,7 +330,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
 
 	_reset_pos_sp(true),
 	_reset_alt_sp(true),
-	_mode_auto(false)
+	_mode_auto(false),
+	_pos_hold_engaged(false),
+	_alt_hold_engaged(false)
 {
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
 	memset(&_att, 0, sizeof(_att));
@@ -666,7 +673,6 @@ MulticopterPositionControl::control_manual(float dt)
 	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
 
 	/* move position setpoint */
-	_pos_sp += _sp_move_rate * dt;
 
 	/* check if position setpoint is too far from actual position */
 	math::Vector<3> pos_sp_offs;
@@ -1061,9 +1067,56 @@ MulticopterPositionControl::task_main()
 
 			} else {
 				/* run position & altitude controllers, calculate velocity setpoint */
-				math::Vector<3> pos_err = _pos_sp - _pos;
 
-				_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
+				// check if we can engage position hold, do xy and z separately
+				float xy_vel_sp = sqrtf(_sp_move_rate(0)*_sp_move_rate(0) + _sp_move_rate(1)*_sp_move_rate(1));
+				float xy_vel = sqrtf(_vel(0)*_vel(0) + _vel(1)*_vel(1));
+
+				/* in manual mode: if user commands velocity in xy plane then use this directly
+					as velocity setpoint. If user commands zero velocity and vehicle has dropped it's speed
+					then lock to current position and use position error to calculate velocity setpoint.
+					Do this separately for horizontal / vertical movement.
+					TODO: Consider doing all three axes separately.
+				*/
+
+				// horizontal movement
+				if (xy_vel_sp < VEL_CMD_THRESH && _control_mode.flag_control_manual_enabled) {
+					if (xy_vel < VEL_XY_THRESH && !_pos_hold_engaged) {
+						_pos_hold_engaged = true;
+						_pos_sp(0) = _pos(0);
+						_pos_sp(1) = _pos(1);
+					}
+				} else {
+					_pos_hold_engaged = false;
+				}
+
+				// vertical movement
+				if (fabsf(_sp_move_rate(2)) < VEL_CMD_THRESH && _control_mode.flag_control_manual_enabled) {
+					if (fabsf(_vel(2)) < VEL_Z_THRESH && !_alt_hold_engaged) {
+						_alt_hold_engaged = true;
+						_pos_sp(2) = _pos(2);
+					}
+				} else {
+					_alt_hold_engaged = false;
+				}
+
+				math::Vector<3> pos_err;
+				if (_pos_hold_engaged || !_control_mode.flag_control_manual_enabled) {
+					pos_err(0) = _pos_sp(0) - _pos(0);
+					pos_err(1) = _pos_sp(1) - _pos(1);
+					_vel_sp(0) = pos_err(0) * _params.pos_p(0) + _vel_ff(0);
+					_vel_sp(1) = pos_err(1) * _params.pos_p(1) + _vel_ff(1);
+				} else {
+					_vel_sp(0) = _sp_move_rate(0);
+					_vel_sp(1) = _sp_move_rate(1);
+				}
+
+				if (_alt_hold_engaged || !_control_mode.flag_control_manual_enabled) {
+					pos_err(2) = _pos_sp(2) - _pos(2);
+					_vel_sp(2) = pos_err(2) * _params.pos_p(2) + _vel_ff(2);
+				} else {
+					_vel_sp(2) = _sp_move_rate(2);
+				}
 
 				/* make sure velocity setpoint is saturated in xy*/
 				float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + 

From 324c27b9414f1636622cdf5f4bf5989cd2a09f13 Mon Sep 17 00:00:00 2001
From: v01d <phreakuencies@gmail.com>
Date: Mon, 28 Sep 2015 16:59:27 -0300
Subject: [PATCH 2/9] velocity control implemented: tested in SITL under manual
 mode

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 197 ++++++++----------
 1 file changed, 89 insertions(+), 108 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index ec833fc3c4..fbf876fa10 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -218,7 +218,6 @@ private:
 	math::Vector<3> _vel_sp;
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
 	math::Vector<3> _vel_ff;
-	math::Vector<3> _sp_move_rate;
 
 	/**
 	 * Update our local parameter cache.
@@ -361,7 +360,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel_sp.zero();
 	_vel_prev.zero();
 	_vel_ff.zero();
-	_sp_move_rate.zero();
 
 	_params_handles.thr_min		= param_find("MPC_THR_MIN");
 	_params_handles.thr_max		= param_find("MPC_THR_MAX");
@@ -591,9 +589,9 @@ MulticopterPositionControl::reset_pos_sp()
 		_reset_pos_sp = false;
 		/* shift position setpoint to make attitude setpoint continuous */
 		_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
-				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
+				- _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0);
 		_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
-				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
+				- _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
 }
@@ -603,7 +601,7 @@ MulticopterPositionControl::reset_alt_sp()
 {
 	if (_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);
+		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
 	}
 }
@@ -634,31 +632,20 @@ MulticopterPositionControl::limit_pos_sp_offset()
 void
 MulticopterPositionControl::control_manual(float dt)
 {
-	_sp_move_rate.zero();
+	math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
+	req_vel_sp.zero();
 
 	if (_control_mode.flag_control_altitude_enabled) {
-		/* move altitude setpoint with throttle stick */
-		_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
+		/* set vertical velocity setpoint with throttle stick */
+		req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); // D
 	}
 
 	if (_control_mode.flag_control_position_enabled) {
-		/* move position setpoint with roll/pitch stick */
-		_sp_move_rate(0) = _manual.x;
-		_sp_move_rate(1) = _manual.y;
+		/* set horizontal velocity setpoint with roll/pitch stick */
+		req_vel_sp(0) = _manual.x;
+		req_vel_sp(1) = _manual.y;
 	}
 
-	/* limit setpoint move rate */
-	float sp_move_norm = _sp_move_rate.length();
-
-	if (sp_move_norm > 1.0f) {
-		_sp_move_rate /= sp_move_norm;
-	}
-
-	/* _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.yaw_body);
-	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
-
 	if (_control_mode.flag_control_altitude_enabled) {
 		/* reset alt setpoint to current altitude if needed */
 		reset_alt_sp();
@@ -669,29 +656,75 @@ MulticopterPositionControl::control_manual(float dt)
 		reset_pos_sp();
 	}
 
-	/* feed forward setpoint move rate with weight vel_ff */
-	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+	/* limit velocity setpoint */
+	float req_vel_sp_norm = req_vel_sp.length();
 
-	/* move position setpoint */
-
-	/* check if position setpoint is too far from actual position */
-	math::Vector<3> pos_sp_offs;
-	pos_sp_offs.zero();
-
-	if (_control_mode.flag_control_position_enabled) {
-		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	if (req_vel_sp_norm > 1.0f) {
+		req_vel_sp /= req_vel_sp_norm;
 	}
 
-	if (_control_mode.flag_control_altitude_enabled) {
-		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	/* _req_vel_sp 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.yaw_body);
+	math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity
+
+	/*
+	 * assisted velocity mode: user controls velocity, but if	velocity is small enough, position
+	 * hold is activated for the corresponding axis
+	 */
+
+	/* horizontal axes */
+	if (_control_mode.flag_control_position_enabled)
+	{
+		/* check for pos. hold */
+		if (fabsf(req_vel_sp(0)) < VEL_CMD_THRESH && fabsf(req_vel_sp(1)) < VEL_CMD_THRESH)
+		{
+			if (!_pos_hold_engaged && fabsf(_vel(0)) < VEL_XY_THRESH && fabsf(_vel(1)) < VEL_XY_THRESH)
+			{
+				_pos_hold_engaged = true;
+				_pos_sp(0) = _pos(0);
+				_pos_sp(1) = _pos(1);
+			}
+		}
+		else {
+			_pos_hold_engaged = false;
+		}
+
+		/* compute velocity/position setpoint */
+		if (_pos_hold_engaged) {
+			_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
+			_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
+		}
+		else {
+			_vel_sp(0) = req_vel_sp_scaled(0);
+			_vel_sp(1) = req_vel_sp_scaled(1);
+		}
 	}
 
-	float pos_sp_offs_norm = pos_sp_offs.length();
+	/* vertical axis */
+	if (_control_mode.flag_control_altitude_enabled)
+	{
+		/* check for pos. hold */
+		if (fabsf(req_vel_sp(2)) < VEL_CMD_THRESH)
+		{
+			if (!_alt_hold_engaged && fabsf(_vel(2)) < VEL_Z_THRESH)
+			{
+				_alt_hold_engaged = true;
+				_pos_hold_engaged = true;
+				_pos_sp(2) = _pos(2);
+			}
+		}
+		else {
+			_alt_hold_engaged = false;
+		}
 
-	if (pos_sp_offs_norm > 1.0f) {
-		pos_sp_offs /= pos_sp_offs_norm;
-		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+		/* compute velocity/position setpoint */
+		if (_alt_hold_engaged) {
+			_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
+		}
+		else {
+			_vel_sp(2) = req_vel_sp_scaled(2);
+		}
 	}
 }
 
@@ -716,8 +749,8 @@ MulticopterPositionControl::control_offboard(float dt)
 			reset_pos_sp();
 
 			/* set position setpoint move rate */
-			_sp_move_rate(0) = _pos_sp_triplet.current.vx;
-			_sp_move_rate(1) = _pos_sp_triplet.current.vy;
+			_vel_sp(0) = _pos_sp_triplet.current.vx;
+			_vel_sp(1) = _pos_sp_triplet.current.vy;
 		}
 
 		if (_pos_sp_triplet.current.yaw_valid) {
@@ -734,15 +767,8 @@ MulticopterPositionControl::control_offboard(float dt)
 			reset_alt_sp();
 
 			/* set altitude setpoint move rate */
-			_sp_move_rate(2) = _pos_sp_triplet.current.vz;
+			_vel_sp(2) = _pos_sp_triplet.current.vz;
 		}
-
-		/* feed forward setpoint move rate with weight vel_ff */
-		_vel_ff = _sp_move_rate.emult(_params.vel_ff);
-
-		/* move position setpoint */
-		_pos_sp += _sp_move_rate * dt;
-
 	} else {
 		reset_pos_sp();
 		reset_alt_sp();
@@ -1026,7 +1052,6 @@ MulticopterPositionControl::task_main()
 			_vel(2) = _local_pos.vz;
 
 			_vel_ff.zero();
-			_sp_move_rate.zero();
 
 			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
@@ -1066,57 +1091,7 @@ MulticopterPositionControl::task_main()
 				}
 
 			} else {
-				/* run position & altitude controllers, calculate velocity setpoint */
-
-				// check if we can engage position hold, do xy and z separately
-				float xy_vel_sp = sqrtf(_sp_move_rate(0)*_sp_move_rate(0) + _sp_move_rate(1)*_sp_move_rate(1));
-				float xy_vel = sqrtf(_vel(0)*_vel(0) + _vel(1)*_vel(1));
-
-				/* in manual mode: if user commands velocity in xy plane then use this directly
-					as velocity setpoint. If user commands zero velocity and vehicle has dropped it's speed
-					then lock to current position and use position error to calculate velocity setpoint.
-					Do this separately for horizontal / vertical movement.
-					TODO: Consider doing all three axes separately.
-				*/
-
-				// horizontal movement
-				if (xy_vel_sp < VEL_CMD_THRESH && _control_mode.flag_control_manual_enabled) {
-					if (xy_vel < VEL_XY_THRESH && !_pos_hold_engaged) {
-						_pos_hold_engaged = true;
-						_pos_sp(0) = _pos(0);
-						_pos_sp(1) = _pos(1);
-					}
-				} else {
-					_pos_hold_engaged = false;
-				}
-
-				// vertical movement
-				if (fabsf(_sp_move_rate(2)) < VEL_CMD_THRESH && _control_mode.flag_control_manual_enabled) {
-					if (fabsf(_vel(2)) < VEL_Z_THRESH && !_alt_hold_engaged) {
-						_alt_hold_engaged = true;
-						_pos_sp(2) = _pos(2);
-					}
-				} else {
-					_alt_hold_engaged = false;
-				}
-
-				math::Vector<3> pos_err;
-				if (_pos_hold_engaged || !_control_mode.flag_control_manual_enabled) {
-					pos_err(0) = _pos_sp(0) - _pos(0);
-					pos_err(1) = _pos_sp(1) - _pos(1);
-					_vel_sp(0) = pos_err(0) * _params.pos_p(0) + _vel_ff(0);
-					_vel_sp(1) = pos_err(1) * _params.pos_p(1) + _vel_ff(1);
-				} else {
-					_vel_sp(0) = _sp_move_rate(0);
-					_vel_sp(1) = _sp_move_rate(1);
-				}
-
-				if (_alt_hold_engaged || !_control_mode.flag_control_manual_enabled) {
-					pos_err(2) = _pos_sp(2) - _pos(2);
-					_vel_sp(2) = pos_err(2) * _params.pos_p(2) + _vel_ff(2);
-				} else {
-					_vel_sp(2) = _sp_move_rate(2);
-				}
+				/* run position & altitude controllers, position/velocity setpoint already computed in control_* functions */
 
 				/* make sure velocity setpoint is saturated in xy*/
 				float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + 
@@ -1133,17 +1108,23 @@ MulticopterPositionControl::task_main()
 					_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
 				}
 
-				if (!_control_mode.flag_control_altitude_enabled) {
-					_reset_alt_sp = true;
-					_vel_sp(2) = 0.0f;
-				}
-
 				if (!_control_mode.flag_control_position_enabled) {
 					_reset_pos_sp = true;
+				}
+
+				if (!_control_mode.flag_control_altitude_enabled) {
+					_reset_alt_sp = true;
+				}
+
+				if (!_control_mode.flag_control_velocity_enabled) {
 					_vel_sp(0) = 0.0f;
 					_vel_sp(1) = 0.0f;
 				}
 
+				if (!_control_mode.flag_control_climb_rate_enabled) {
+					_vel_sp(2) = 0.0f;
+				}
+
 				/* use constant descend rate when landing, ignore altitude setpoint */
 				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 					_vel_sp(2) = _params.land_speed;
@@ -1201,7 +1182,7 @@ MulticopterPositionControl::task_main()
 					math::Vector<3> vel_err = _vel_sp - _vel;
 
 					/* derivative of velocity error, not includes setpoint acceleration */
-					math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+					math::Vector<3> vel_err_d = (_vel - _vel_prev) / dt;
 
 					/* thrust vector in NED frame */
 					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;

From da3087e54cfb5a13ffda70794ee4e7a1f2bb2140 Mon Sep 17 00:00:00 2001
From: v01d <phreakuencies@gmail.com>
Date: Tue, 29 Sep 2015 11:08:46 -0300
Subject: [PATCH 3/9] expose pos-hold parameters (also allows different
 behaviours)

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 26 ++++++++++++++---
 .../mc_pos_control/mc_pos_control_params.c    | 28 +++++++++++++++++++
 2 files changed, 50 insertions(+), 4 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index fbf876fa10..de81716a1b 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -180,6 +180,9 @@ private:
 		param_t man_pitch_max;
 		param_t man_yaw_max;
 		param_t mc_att_yaw_p;
+		param_t hold_dz;
+		param_t hold_max_xy;
+		param_t hold_max_z;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -192,6 +195,9 @@ private:
 		float man_pitch_max;
 		float man_yaw_max;
 		float mc_att_yaw_p;
+		float hold_dz;
+		float hold_max_xy;
+		float hold_max_z;
 
 		math::Vector<3> pos_p;
 		math::Vector<3> vel_p;
@@ -382,6 +388,10 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX");
 	_params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX");
 	_params_handles.mc_att_yaw_p = param_find("MC_YAW_P");
+	_params_handles.hold_dz = param_find("MPC_HOLD_DZ");
+	_params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY");
+	_params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z");
+
 
 	/* fetch initial parameter values */
 	parameters_update(true);
@@ -469,6 +479,13 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.z_ff, &v);
 		v = math::constrain(v, 0.0f, 1.0f);
 		_params.vel_ff(2) = v;
+		param_get(_params_handles.hold_dz, &v);
+		v = math::constrain(v, 0.0f, 1.0f);
+		_params.hold_dz = v;
+		param_get(_params_handles.hold_max_xy, &v);
+		_params.hold_max_xy = (v < 0.0f ? 0.0f : v);
+		param_get(_params_handles.hold_max_z, &v);
+		_params.hold_max_z = (v < 0.0f ? 0.0f : v);
 
 		_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
 
@@ -677,9 +694,10 @@ MulticopterPositionControl::control_manual(float dt)
 	if (_control_mode.flag_control_position_enabled)
 	{
 		/* check for pos. hold */
-		if (fabsf(req_vel_sp(0)) < VEL_CMD_THRESH && fabsf(req_vel_sp(1)) < VEL_CMD_THRESH)
+		if (fabsf(req_vel_sp(0)) < _params.hold_dz && fabsf(req_vel_sp(1)) < _params.hold_dz)
 		{
-			if (!_pos_hold_engaged && fabsf(_vel(0)) < VEL_XY_THRESH && fabsf(_vel(1)) < VEL_XY_THRESH)
+			if (!_pos_hold_engaged && (_params.hold_max_xy == 0.0f ||
+																 (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
 			{
 				_pos_hold_engaged = true;
 				_pos_sp(0) = _pos(0);
@@ -705,9 +723,9 @@ MulticopterPositionControl::control_manual(float dt)
 	if (_control_mode.flag_control_altitude_enabled)
 	{
 		/* check for pos. hold */
-		if (fabsf(req_vel_sp(2)) < VEL_CMD_THRESH)
+		if (fabsf(req_vel_sp(2)) < _params.hold_dz)
 		{
-			if (!_alt_hold_engaged && fabsf(_vel(2)) < VEL_Z_THRESH)
+			if (!_alt_hold_engaged && (_params.hold_max_z == 0.0f || fabsf(_vel(2)) < _params.hold_max_z))
 			{
 				_alt_hold_engaged = true;
 				_pos_hold_engaged = true;
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index d29d11c65d..c99acfc89d 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -265,3 +265,31 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f);
 
+/**
+ * Deadzone of X,Y,Z where position hold is enabled
+ *
+ * @unit percent
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f);
+
+/**
+ * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check)
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f);
+
+/**
+ * Maximum vertical velocity for which position hold is enabled (use 0 to disable check)
+ *
+ * @unit m/s
+ * @min 0.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f);
+

From 42ccc654b58bec06945a6f7cf64013092e7ecb54 Mon Sep 17 00:00:00 2001
From: v01d <phreakuencies@gmail.com>
Date: Tue, 29 Sep 2015 11:58:51 -0300
Subject: [PATCH 4/9] fix float comparison to 0.0f

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index de81716a1b..971eba063a 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -696,7 +696,7 @@ MulticopterPositionControl::control_manual(float dt)
 		/* check for pos. hold */
 		if (fabsf(req_vel_sp(0)) < _params.hold_dz && fabsf(req_vel_sp(1)) < _params.hold_dz)
 		{
-			if (!_pos_hold_engaged && (_params.hold_max_xy == 0.0f ||
+			if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON ||
 																 (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
 			{
 				_pos_hold_engaged = true;
@@ -725,7 +725,7 @@ MulticopterPositionControl::control_manual(float dt)
 		/* check for pos. hold */
 		if (fabsf(req_vel_sp(2)) < _params.hold_dz)
 		{
-			if (!_alt_hold_engaged && (_params.hold_max_z == 0.0f || fabsf(_vel(2)) < _params.hold_max_z))
+			if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
 			{
 				_alt_hold_engaged = true;
 				_pos_hold_engaged = true;

From c010962861e9f2f654158a112edce30cefe5fc40 Mon Sep 17 00:00:00 2001
From: v01d <phreakuencies@gmail.com>
Date: Tue, 29 Sep 2015 12:25:07 -0300
Subject: [PATCH 5/9] change unit of MPC_HOLD_DZ parameter to "%"

---
 src/modules/mc_pos_control/mc_pos_control_params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index c99acfc89d..f68de3c645 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -268,7 +268,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f);
 /**
  * Deadzone of X,Y,Z where position hold is enabled
  *
- * @unit percent
+ * @unit %
  * @min 0.0
  * @max 1.0
  * @group Multicopter Position Control

From 9c49b30118ce489eae91422918a8064e4866ac46 Mon Sep 17 00:00:00 2001
From: v01d <phreakuencies@gmail.com>
Date: Tue, 29 Sep 2015 15:39:36 -0300
Subject: [PATCH 6/9] run position controller in main_task, otherwise position
 setpoints will not be followed

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 42 +++++++++++--------
 1 file changed, 25 insertions(+), 17 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 971eba063a..78e180c69a 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -90,9 +90,6 @@
 #define SIGMA			0.000001f
 #define MIN_DIST		0.01f
 #define MANUAL_THROTTLE_MAX_MULTICOPTER	0.9f
-#define VEL_XY_THRESH 	0.5f	// max xy velocity for which pos hold in xy is engaged
-#define VEL_Z_THRESH 	0.5f	// max z velocity for which pos hold in z is engaged
-#define VEL_CMD_THRESH 	0.1f	// min velocity (in xy/z) which is interpreted as velocity command
 
 /**
  * Multicopter position control app start / stop handling function
@@ -217,6 +214,8 @@ private:
 	bool _mode_auto;
 	bool _pos_hold_engaged;
 	bool _alt_hold_engaged;
+	bool _run_pos_control;
+	bool _run_alt_control;
 
 	math::Vector<3> _pos;
 	math::Vector<3> _pos_sp;
@@ -337,7 +336,9 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_reset_alt_sp(true),
 	_mode_auto(false),
 	_pos_hold_engaged(false),
-	_alt_hold_engaged(false)
+	_alt_hold_engaged(false),
+	_run_pos_control(true),
+	_run_alt_control(true)
 {
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
 	memset(&_att, 0, sizeof(_att));
@@ -708,12 +709,9 @@ MulticopterPositionControl::control_manual(float dt)
 			_pos_hold_engaged = false;
 		}
 
-		/* compute velocity/position setpoint */
-		if (_pos_hold_engaged) {
-			_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
-			_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
-		}
-		else {
+		/* set requested velocity setpoint */
+		if (!_pos_hold_engaged) {
+			_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
 			_vel_sp(0) = req_vel_sp_scaled(0);
 			_vel_sp(1) = req_vel_sp_scaled(1);
 		}
@@ -728,7 +726,6 @@ MulticopterPositionControl::control_manual(float dt)
 			if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
 			{
 				_alt_hold_engaged = true;
-				_pos_hold_engaged = true;
 				_pos_sp(2) = _pos(2);
 			}
 		}
@@ -736,11 +733,9 @@ MulticopterPositionControl::control_manual(float dt)
 			_alt_hold_engaged = false;
 		}
 
-		/* compute velocity/position setpoint */
-		if (_alt_hold_engaged) {
-			_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
-		}
-		else {
+		/* set requested velocity setpoint */
+		if (!_alt_hold_engaged) {
+			_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
 			_vel_sp(2) = req_vel_sp_scaled(2);
 		}
 	}
@@ -1071,6 +1066,11 @@ MulticopterPositionControl::task_main()
 
 			_vel_ff.zero();
 
+			/* by default, run position/altitude controller. the control_* functions
+			 * can disable this and run velocity controllers directly in this cycle */
+			_run_pos_control = true;
+			_run_alt_control = true;
+
 			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
@@ -1109,7 +1109,15 @@ MulticopterPositionControl::task_main()
 				}
 
 			} else {
-				/* run position & altitude controllers, position/velocity setpoint already computed in control_* functions */
+				/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
+				if (_run_pos_control) {
+					_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
+					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
+				}
+
+				if (_run_alt_control) {
+					_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
+				}
 
 				/* make sure velocity setpoint is saturated in xy*/
 				float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + 

From 44bb50e9eae36713a37624e9686eca62221a0360 Mon Sep 17 00:00:00 2001
From: v01d <phreakuencies@gmail.com>
Date: Tue, 29 Sep 2015 15:51:05 -0300
Subject: [PATCH 7/9] separate _DZ parameter to XY and Z. this allows disabling
 pos hold for XY and not Z

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 20 ++++++++++++-------
 .../mc_pos_control/mc_pos_control_params.c    | 14 +++++++++++--
 2 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 78e180c69a..adaf4a3fb6 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -177,7 +177,8 @@ private:
 		param_t man_pitch_max;
 		param_t man_yaw_max;
 		param_t mc_att_yaw_p;
-		param_t hold_dz;
+		param_t hold_xy_dz;
+		param_t hold_z_dz;
 		param_t hold_max_xy;
 		param_t hold_max_z;
 	}		_params_handles;		/**< handles for interesting parameters */
@@ -192,7 +193,8 @@ private:
 		float man_pitch_max;
 		float man_yaw_max;
 		float mc_att_yaw_p;
-		float hold_dz;
+		float hold_xy_dz;
+		float hold_z_dz;
 		float hold_max_xy;
 		float hold_max_z;
 
@@ -389,7 +391,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX");
 	_params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX");
 	_params_handles.mc_att_yaw_p = param_find("MC_YAW_P");
-	_params_handles.hold_dz = param_find("MPC_HOLD_DZ");
+	_params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ");
+	_params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ");
 	_params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY");
 	_params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z");
 
@@ -480,9 +483,12 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.z_ff, &v);
 		v = math::constrain(v, 0.0f, 1.0f);
 		_params.vel_ff(2) = v;
-		param_get(_params_handles.hold_dz, &v);
+		param_get(_params_handles.hold_xy_dz, &v);
 		v = math::constrain(v, 0.0f, 1.0f);
-		_params.hold_dz = v;
+		_params.hold_xy_dz = v;
+		param_get(_params_handles.hold_z_dz, &v);
+		v = math::constrain(v, 0.0f, 1.0f);
+		_params.hold_z_dz = v;
 		param_get(_params_handles.hold_max_xy, &v);
 		_params.hold_max_xy = (v < 0.0f ? 0.0f : v);
 		param_get(_params_handles.hold_max_z, &v);
@@ -695,7 +701,7 @@ MulticopterPositionControl::control_manual(float dt)
 	if (_control_mode.flag_control_position_enabled)
 	{
 		/* check for pos. hold */
-		if (fabsf(req_vel_sp(0)) < _params.hold_dz && fabsf(req_vel_sp(1)) < _params.hold_dz)
+		if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz)
 		{
 			if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON ||
 																 (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
@@ -721,7 +727,7 @@ MulticopterPositionControl::control_manual(float dt)
 	if (_control_mode.flag_control_altitude_enabled)
 	{
 		/* check for pos. hold */
-		if (fabsf(req_vel_sp(2)) < _params.hold_dz)
+		if (fabsf(req_vel_sp(2)) < _params.hold_z_dz)
 		{
 			if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
 			{
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index f68de3c645..09843de1f1 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -266,14 +266,24 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f);
 PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f);
 
 /**
- * Deadzone of X,Y,Z where position hold is enabled
+ * Deadzone of X,Y sticks where position hold is enabled
  *
  * @unit %
  * @min 0.0
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f);
+
+/**
+ * Deadzone of Z stick where altitude hold is enabled
+ *
+ * @unit %
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f);
 
 /**
  * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check)

From 8133f8d61626efe2679bdd21c6ebaee12d48022f Mon Sep 17 00:00:00 2001
From: v01d <phreakuencies@gmail.com>
Date: Tue, 29 Sep 2015 16:05:34 -0300
Subject: [PATCH 8/9] disable pos/alt controllers when following velocity
 setpoints in offboard mode

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index adaf4a3fb6..299330eebf 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -770,6 +770,8 @@ MulticopterPositionControl::control_offboard(float dt)
 			/* set position setpoint move rate */
 			_vel_sp(0) = _pos_sp_triplet.current.vx;
 			_vel_sp(1) = _pos_sp_triplet.current.vy;
+
+			_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
 		}
 
 		if (_pos_sp_triplet.current.yaw_valid) {
@@ -787,6 +789,8 @@ MulticopterPositionControl::control_offboard(float dt)
 
 			/* set altitude setpoint move rate */
 			_vel_sp(2) = _pos_sp_triplet.current.vz;
+
+			_run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */
 		}
 	} else {
 		reset_pos_sp();

From b3f912ecc1e9c8b410130ab689cf9a7a61c6b851 Mon Sep 17 00:00:00 2001
From: tumbili <bapstr@ethz.ch>
Date: Fri, 16 Oct 2015 12:38:53 +0200
Subject: [PATCH 9/9] reset position setpoint when in velocity control mode

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 299330eebf..60747854cd 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -707,12 +707,12 @@ MulticopterPositionControl::control_manual(float dt)
 																 (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
 			{
 				_pos_hold_engaged = true;
-				_pos_sp(0) = _pos(0);
-				_pos_sp(1) = _pos(1);
 			}
 		}
 		else {
 			_pos_hold_engaged = false;
+			_pos_sp(0) = _pos(0);
+			_pos_sp(1) = _pos(1);
 		}
 
 		/* set requested velocity setpoint */
@@ -732,11 +732,11 @@ MulticopterPositionControl::control_manual(float dt)
 			if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
 			{
 				_alt_hold_engaged = true;
-				_pos_sp(2) = _pos(2);
 			}
 		}
 		else {
 			_alt_hold_engaged = false;
+			_pos_sp(2) = _pos(2);
 		}
 
 		/* set requested velocity setpoint */