From ab1ae1fb4e1b8644f25502b5b7190639acf67cef Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Wed, 11 Jul 2018 09:01:02 +0200
Subject: [PATCH] mc_pos_control_main: trigger failsafe if no setpoint along
 D-direction is valid

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 5619abe75c..c01764d79e 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -561,6 +561,12 @@ MulticopterPositionControl::task_main()
 				    !(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) {
 					failsafe(setpoint, _states);
 				}
+
+				// Check if altitude, climbrate or thrust in D-direction are valid -> trigger failsafe if none
+				// of these setpoints are valid
+				if (!PX4_ISFINITE(setpoint.z) && !PX4_ISFINITE(setpoint.vz) && !PX4_ISFINITE(setpoint.thrust[2])) {
+					failsafe(setpoint, _states);
+				}
 			}
 
 			vehicle_constraints_s constraints = _flight_tasks.getConstraints();