From c0d1aa1654468c6f52223fb7fec40ff2ba6fd170 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 15 Jan 2020 02:24:41 +0100
Subject: [PATCH] FlightTaskManualAltitude: Follow altitude limit given by the
 estimator

---
 .../ManualAltitude/FlightTaskManualAltitude.cpp  | 16 +++++++++++-----
 .../ManualAltitude/FlightTaskManualAltitude.hpp  |  1 +
 2 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index 3880cc00ca..ea1cca3f33 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -61,6 +61,16 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
 
 	_constraints.tilt = math::radians(_param_mpc_man_tilt_max.get());
 
+	_updateConstraintsFromEstimator();
+
+	_max_speed_up = _constraints.speed_up;
+	_min_speed_down = _constraints.speed_down;
+
+	return ret;
+}
+
+void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
+{
 	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) {
 		_constraints.min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
 
@@ -74,11 +84,6 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
 	} else {
 		_constraints.max_distance_to_ground = INFINITY;
 	}
-
-	_max_speed_up = _constraints.speed_up;
-	_min_speed_down = _constraints.speed_down;
-
-	return ret;
 }
 
 void FlightTaskManualAltitude::_scaleSticks()
@@ -357,6 +362,7 @@ bool FlightTaskManualAltitude::_checkTakeoff()
 
 bool FlightTaskManualAltitude::update()
 {
+	_updateConstraintsFromEstimator();
 	_scaleSticks();
 	_updateSetpoints();
 	_constraints.want_takeoff = _checkTakeoff();
diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
index 512c181117..e4d2dcb19b 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -56,6 +56,7 @@ protected:
 	virtual void _updateSetpoints(); /**< updates all setpoints */
 	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
 	bool _checkTakeoff() override;
+	void _updateConstraintsFromEstimator();
 
 	/**
 	 * rotates vector into local frame