From be6f3a5b48dde0e7538f52fb217dcb4b80724461 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Thu, 24 May 2018 08:53:18 +0200
Subject: [PATCH] FlightTaskOffboard: valid position and velocity is required

---
 .../FlightTasks/tasks/FlightTaskOffboard.cpp  | 19 ++++++++++---------
 .../FlightTasks/tasks/FlightTaskOffboard.hpp  |  1 +
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
index 043f3979a6..a6a982d775 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
@@ -52,21 +52,22 @@ bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription
 	return true;
 }
 
+bool FlightTaskOffboard::updateInitialize()
+{
+	bool ret = FlightTask::updateInitialize();
+	// require valid position / velocity in xy
+	return ret && PX4_ISFINITE(_position(0))
+	       && PX4_ISFINITE(_position(1))
+	       && PX4_ISFINITE(_velocity(0))
+	       && PX4_ISFINITE(_velocity(1));
+}
+
 bool FlightTaskOffboard::activate()
 {
 	bool ret = FlightTask::activate();
 	_position_setpoint = _position;
 	_velocity_setpoint *= 0.0f;
 	_position_lock *= NAN;
-
-	// need a valid position and velocity
-	ret = ret && PX4_ISFINITE(_position(0))
-	      && PX4_ISFINITE(_position(1))
-	      && PX4_ISFINITE(_position(2))
-	      && PX4_ISFINITE(_velocity(0))
-	      && PX4_ISFINITE(_velocity(1))
-	      && PX4_ISFINITE(_velocity(2));
-
 	return ret;
 }
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp b/src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp
index b67f3f71c4..01fd8ff157 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp
@@ -50,6 +50,7 @@ public:
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 	bool update() override;
 	bool activate() override;
+	bool updateInitialize() override;
 
 protected:
 	uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};