From 4f8f0d864527ce5a3ce6de049c5c187fdba50fd1 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Tue, 5 Dec 2017 11:13:50 +0100
Subject: [PATCH] mc_pos_control: set triplets to NAN if not in auto mode

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

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 75b39155f9..f395c48a4d 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1811,7 +1811,7 @@ void MulticopterPositionControl::control_auto(float dt)
 			diff = fabsf(_curr_pos_sp(2) - curr_pos_sp(2));
 		}
 
-		if (diff > FLT_EPSILON) {
+		if (diff > FLT_EPSILON || !PX4_ISFINITE(diff)) {
 			triplet_updated = true;
 		}
 
@@ -2373,6 +2373,7 @@ MulticopterPositionControl::do_control(float dt)
 		 * have been updated */
 		_pos_sp_triplet.current.valid = false;
 		_pos_sp_triplet.previous.valid = false;
+		_curr_pos_sp = math::Vector<3>(NAN, NAN, NAN);
 
 		_hold_offboard_xy = false;
 		_hold_offboard_z = false;