From 22c4bb498cf7a41551ac6ac5f35eb7ea6040ad44 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 15 Oct 2019 13:53:17 +0200
Subject: [PATCH] FlightTaskDescend: set no vertical thrust when commanding
 velocity

---
 .../FlightTasks/tasks/Descend/FlightTaskDescend.cpp | 13 +++++++------
 .../FlightTasks/tasks/Descend/FlightTaskDescend.hpp |  8 +++-----
 2 files changed, 10 insertions(+), 11 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp
index cce5497f2715..8e948cc3394f 100644
--- a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp
+++ b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.cpp
@@ -39,11 +39,10 @@
 bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
-	_position_setpoint = {NAN, NAN, NAN};
-	_velocity_setpoint = {NAN, NAN, NAN};
-	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f);
+	// stay level to minimize horizontal drift
+	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN);
+	// keep heading
 	_yaw_setpoint = _yaw;
-	_yawspeed_setpoint = 0.0f;
 	return ret;
 }
 
@@ -52,11 +51,13 @@ bool FlightTaskDescend::update()
 	if (PX4_ISFINITE(_velocity(2))) {
 		// land with landspeed
 		_velocity_setpoint(2) = _param_mpc_land_speed.get();
+		_thrust_setpoint(2) = NAN;
 
 	} else {
-		return false;
+		// descend with constant thrust (crash landing)
+		_velocity_setpoint(2) = NAN;
+		_thrust_setpoint(2) = -_param_mpc_thr_hover.get() * 0.7f;
 	}
 
 	return true;
-
 }
diff --git a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp
index ba9c9015c08c..7e6035d9ab62 100644
--- a/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp
+++ b/src/lib/FlightTasks/tasks/Descend/FlightTaskDescend.hpp
@@ -33,7 +33,6 @@
 
 /**
  * @file FlightTaskDescend.hpp
- *
  */
 
 #pragma once
@@ -44,15 +43,14 @@ class FlightTaskDescend : public FlightTask
 {
 public:
 	FlightTaskDescend() = default;
-
 	virtual ~FlightTaskDescend() = default;
+
 	bool update() override;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
-					(ParamFloat<px4::params::MPC_THR_HOVER>)
-					_param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */
+					(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover, ///< thrust at hover equilibrium
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed ///< velocity for controlled descend
 				       )
 };