From 80867343ab93a61b2b5f31935da9d13f874ed6e9 Mon Sep 17 00:00:00 2001
From: ChristophTobler <christoph@px4.io>
Date: Tue, 10 Jul 2018 11:27:29 +0200
Subject: [PATCH] mc_pos_control: trigger failsafe if no pos, vel or thrust
 pair is valid

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 +++++++
 1 file changed, 7 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 1b2359006d..5619abe75c 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -554,6 +554,13 @@ MulticopterPositionControl::task_main()
 
 			} else {
 				setpoint = _flight_tasks.getPositionSetpoint();
+
+				// Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid
+				if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) &&
+				    !(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) &&
+				    !(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) {
+					failsafe(setpoint, _states);
+				}
 			}
 
 			vehicle_constraints_s constraints = _flight_tasks.getConstraints();