From c907e7a9dc113c19f3a8dcf9348f6401cbf85f86 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Mon, 25 Jun 2018 07:48:19 +0200
Subject: [PATCH] FlighttaskManualAlititude: replace SENS_FLOW_MINRNG with
 hagl_min

---
 src/lib/FlightTasks/tasks/FlightTask.cpp      |  5 +++--
 .../tasks/FlightTaskManualAltitude.cpp        | 19 ++++++++++++-------
 .../tasks/FlightTaskManualAltitude.hpp        |  1 -
 3 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp
index 4c41726dc6..c3fc9cfbbf 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.cpp
@@ -96,12 +96,13 @@ bool FlightTask::_evaluateVehicleLocalPosition()
 		// distance to bottom
 		_dist_to_bottom = NAN;
 
-		if (_sub_vehicle_local_position->get().dist_bottom_valid) {
+		if (_sub_vehicle_local_position->get().dist_bottom_valid
+		    && PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) {
 			_dist_to_bottom =  _sub_vehicle_local_position->get().dist_bottom;
 		}
 
 		// estimator specified vehicle limits
-		
+
 
 		// We don't check here if states are valid or not.
 		// Validity checks are done in the sub-classes.
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index f5c54c9cf8..efa3810cf8 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -54,7 +54,14 @@ bool FlightTaskManualAltitude::activate()
 	_position_setpoint(2) = _position(2);
 	_velocity_setpoint(2) = 0.0f;
 	_setDefaultConstraints();
-	_constraints.min_distance_to_ground = SENS_FLOW_MINRNG.get();
+
+	if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) {
+		_constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
+
+	} else {
+		_constraints.min_distance_to_ground = -INFINITY;
+	}
+
 	return ret;
 }
 
@@ -92,7 +99,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 			// Ensure that minimum altitude is respected if
 			// there is a distance sensor and distance to bottome is below minimum.
-			if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < SENS_FLOW_MINRNG.get()) {
+			if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _constraints.min_distance_to_ground) {
 				_terrain_following(apply_brake, stopped);
 
 			} else {
@@ -116,21 +123,19 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 void FlightTaskManualAltitude::_respectMinAltitude()
 {
-	const bool respectAlt = _sub_vehicle_local_position->get().limit_hagl
-				&& PX4_ISFINITE(_dist_to_bottom)
-				&& _dist_to_bottom < SENS_FLOW_MINRNG.get();
+	const bool respectAlt = PX4_ISFINITE(_dist_to_bottom)
+				&& _dist_to_bottom < _constraints.min_distance_to_ground;
 
 	// Height above ground needs to be limited (flow / range-finder)
 	if (respectAlt) {
 		// increase altitude to minimum flow distance
 		_position_setpoint(2) = _position(2)
-					- (SENS_FLOW_MINRNG.get() - _dist_to_bottom);
+					- (_constraints.min_distance_to_ground - _dist_to_bottom);
 	}
 }
 
 void FlightTaskManualAltitude::_terrain_following(bool apply_brake, bool stopped)
 {
-
 	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) {
 		// User wants to break and vehicle reached zero velocity. Lock height to ground.
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
index b5c2b31747..7e960c0a94 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
@@ -62,7 +62,6 @@ protected:
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
-					(ParamFloat<px4::params::SENS_FLOW_MINRNG>) SENS_FLOW_MINRNG,
 					(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE
 				       )
 private: