From 5ffcb6af01c16677a20fc5822fd435c38a446fb0 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Fri, 25 May 2018 17:19:43 +0200
Subject: [PATCH] PositionControl: set vel_dot to 0 if NAN

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

diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index 6fffac900f..8474159b1d 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -173,6 +173,15 @@ void PositionControl::_interfaceMapping()
 		}
 	}
 
+	// ensure that vel_dot is finite, otherwise set to 0
+	if (!PX4_ISFINITE(_vel_dot(0)) || !PX4_ISFINITE(_vel_dot(1))) {
+		_vel_dot(0) = _vel_dot(1) = 0.0f;
+	}
+
+	if (!PX4_ISFINITE(_vel_dot(2))) {
+		_vel_dot(2) = 0.0f;
+	}
+
 	if (!PX4_ISFINITE(_yawspeed_sp)) {
 		// Set the yawspeed to 0 since not used.
 		_yawspeed_sp = 0.0f;