From 63a6ab34f7e37c6eac48fae85fb84c84c5d77b97 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 25 Feb 2019 15:43:25 +0100
Subject: [PATCH] FlightTaskManualAltitude: slow down when landing manually

---
 .../FlightTaskManualAltitude.cpp              | 35 +++++++++++++++++++
 .../FlightTaskManualAltitude.hpp              | 14 +++++++-
 2 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index 5cbc4636b7..cb2f28deb6 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -41,6 +41,19 @@
 
 using namespace matrix;
 
+bool FlightTaskManualAltitude::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!FlightTaskManual::initializeSubscriptions(subscription_array)) {
+		return false;
+	}
+
+	if (!subscription_array.get(ORB_ID(home_position), _sub_home_position)) {
+		return false;
+	}
+
+	return true;
+}
+
 bool FlightTaskManualAltitude::updateInitialize()
 {
 	bool ret = FlightTaskManual::updateInitialize();
@@ -252,6 +265,27 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 	}
 }
 
+void FlightTaskManualAltitude::_respectGroundSlowdown()
+{
+	float dist_to_ground = NAN;
+
+	// if there is a valid distance to bottom or vertical distance to home
+	if (PX4_ISFINITE(_dist_to_bottom)) {
+		dist_to_ground = _dist_to_bottom;
+
+	} else if (_sub_home_position->get().valid_alt) {
+		dist_to_ground = -(_position(2) - _sub_home_position->get().z);
+	}
+
+	// limit downwards speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
+	if (PX4_ISFINITE(dist_to_ground)) {
+		const float slowdown_limit = math::gradual(dist_to_ground,
+					     MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
+					     MPC_LAND_SPEED.get(), _constraints.speed_down);
+		_velocity_setpoint(2) = math::min(_velocity_setpoint(2), slowdown_limit);
+	}
+}
+
 void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
 {
 	float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
@@ -305,6 +339,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	_thrust_setpoint(2) = NAN;
 
 	_updateAltitudeLock();
+	_respectGroundSlowdown();
 }
 
 bool FlightTaskManualAltitude::update()
diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
index 474c0cce6b..e3c4bed999 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -40,12 +40,14 @@
 #pragma once
 
 #include "FlightTaskManual.hpp"
+#include <uORB/topics/home_position.h>
 
 class FlightTaskManualAltitude : public FlightTaskManual
 {
 public:
 	FlightTaskManualAltitude() = default;
 	virtual ~FlightTaskManualAltitude() = default;
+	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool activate() override;
 	bool updateInitialize() override;
 	bool update() override;
@@ -73,7 +75,10 @@ protected:
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
 					(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P, /**< position controller altitude propotional gain */
 					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */
-					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX /**< maximum tilt allowed for manual flight */
+					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX, /**< maximum tilt allowed for manual flight */
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which to start downwards slowdown
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude below wich to land with land speed
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED
 				       )
 private:
 	/**
@@ -95,6 +100,13 @@ private:
 
 	void _respectMaxAltitude();
 
+	/**
+	 * Sets downwards velocity constraint based on the distance to ground.
+	 * To ensure a slowdown to land speed before hitting the ground.
+	 */
+	void _respectGroundSlowdown();
+
+	uORB::Subscription<home_position_s> *_sub_home_position{nullptr};
 
 	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
 	float _max_speed_up = 10.0f;