From deaa83bba00c51f9e75793eb2692da3e4fe301f3 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 2 May 2017 21:11:14 +1000
Subject: [PATCH] mc_pos_control: Use vertical position derivative when in
 velocity control mode

Stops vertical velocity bias errors preventing the vehicle from landing.
Use of the vertical derivative is blended in so that for zero vertical velocity set point, the  local_position.vz is used and when the magnitude of the vertical velocity setpoint exceeds the landing speed, the  local_position.z_deriv is used.
---
 .../mc_pos_control/mc_pos_control_main.cpp       | 16 ++++++++++++++++
 1 file changed, 16 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 f395c48a4d..b7654bb569 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -294,6 +294,8 @@ private:
 	float _acceleration_state_dependent_z; /**< acceleration limit applied in manual mode in z */
 	float _manual_jerk_limit_xy; /**< jerk limit in manual mode dependent on stick input */
 	float _manual_jerk_limit_z; /**< jerk limit in manual mode in z */
+	float _z_derivative; /**< velocity in z that agrees with position rate */
+
 	float _takeoff_vel_limit; /**< velocity limit value which gets ramped up */
 
 	// counters for reset events on position and velocity states
@@ -470,6 +472,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_acceleration_state_dependent_z(0.0f),
 	_manual_jerk_limit_xy(1.0f),
 	_manual_jerk_limit_z(1.0f),
+	_z_derivative(0.0f),
 	_takeoff_vel_limit(0.0f),
 	_z_reset_counter(0),
 	_xy_reset_counter(0),
@@ -2347,8 +2350,21 @@ MulticopterPositionControl::update_velocity_derivative()
 		} else {
 			_vel(2) = _local_pos.vz;
 		}
+
+		if (!_run_alt_control) {
+			/* set velocity to the derivative of position
+			 * because it has less bias but blend it in across the landing speed range*/
+			float weighting = fminf(fabsf(_vel_sp(2)) / _params.land_speed, 1.0f);
+			_vel(2) = _z_derivative * weighting + _vel(2) * (1.0f - weighting);
+
+		}
+
 	}
 
+	if(PX4_ISFINITE(_local_pos.z_deriv)){
+		_z_derivative = _local_pos.z_deriv;
+	};
+
 	_vel_err_d(0) = _vel_x_deriv.update(-_vel(0));
 	_vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
 	_vel_err_d(2) = _vel_z_deriv.update(-_vel(2));