From ccd0be82bac2e50fe1666a2b5867bd6a2e3313eb Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@px4.io>
Date: Wed, 18 Apr 2018 17:24:11 +0200
Subject: [PATCH] FlightTaskManualAltitude: if flow is on, ensure minimum
 altitude

---
 src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp | 6 ++++++
 src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp | 3 ++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index e2d0bab5e8..309f029004 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -63,6 +63,12 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 	if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) {
 		_position_setpoint(2) = _position(2);
 
+		if ((PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < SENS_FLOW_MINRNG.get())) {
+			// if vehicle wants to keep altitude but is below minimum flow distance,
+			// increase altitude to minimum flow distance
+			_position_setpoint(2) = _position(2) - (SENS_FLOW_MINRNG.get() - _dist_to_bottom);
+		}
+
 	} else if (!apply_brake_z) {
 		_position_setpoint(2) = NAN;
 	}
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
index f00d0d7403..2cfef2a64d 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
@@ -54,7 +54,8 @@ protected:
 	void _scaleSticks() override; /**< scales sticks to velocity in z */
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
-					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z
+					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
+					(ParamFloat<px4::params::SENS_FLOW_MINRNG>) SENS_FLOW_MINRNG
 				       )
 
 };