From d9c7e6321f473105e9ec4693ff0f376b703bf8cf Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Fri, 15 Dec 2017 01:59:55 +0100
Subject: [PATCH] FlightTasks: Introduce the empty setpoint to reset the
 setpoint for every loop iteration and return it in case of no task running

---
 src/lib/FlightTasks/FlightTasks.cpp           | 10 ++++++++++
 src/lib/FlightTasks/FlightTasks.hpp           |  2 +-
 src/lib/FlightTasks/tasks/FlightTask.cpp      |  2 ++
 src/lib/FlightTasks/tasks/FlightTask.hpp      |  6 +++++-
 src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp |  4 ++--
 5 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index 746b46ed6e..25a4ff9dfe 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -15,6 +15,16 @@ bool FlightTasks::update()
 	return false;
 }
 
+const vehicle_local_position_setpoint_s &FlightTasks::getPositionSetpoint()
+{
+	if (isAnyTaskActive()) {
+		return _current_task->getPositionSetpoint();
+
+	} else {
+		return FlightTask::empty_setpoint;
+	}
+}
+
 int FlightTasks::switchTask(int task_number)
 {
 	/* switch to the running task, nothing to do */
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 0cdf7f4e61..09c28bf5d1 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -73,7 +73,7 @@ public:
 	 * Get the output data from the current task
 	 * Only call when task is active!
 	 */
-	const vehicle_local_position_setpoint_s &getPositionSetpoint() { return _current_task->getPositionSetpoint(); }
+	const vehicle_local_position_setpoint_s &getPositionSetpoint();
 
 	/**
 	 * Convenient operator to get the output data from the current task
diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp
index 0678a14a25..aae21f2507 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.cpp
@@ -2,6 +2,7 @@
 #include <mathlib/mathlib.h>
 
 constexpr uint64_t FlightTask::_timeout;
+constexpr vehicle_local_position_setpoint_s FlightTask::empty_setpoint;
 
 
 bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
@@ -21,6 +22,7 @@ bool FlightTask::activate()
 
 bool FlightTask::updateInitialize()
 {
+	_resetSetpoint();
 	_time_stamp_current = hrt_absolute_time();
 	_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
 	_deltatime  = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index 0d7803161d..7f3c151cf5 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -56,7 +56,7 @@ class FlightTask : public control::Block
 public:
 	FlightTask(control::SuperBlock *parent, const char *name) :
 		Block(parent, name)
-	{ }
+	{ _resetSetpoint(); }
 
 	virtual ~FlightTask() = default;
 
@@ -99,6 +99,8 @@ public:
 		return _vehicle_local_position_setpoint;
 	}
 
+	static constexpr vehicle_local_position_setpoint_s empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN};
+
 protected:
 	/* Time abstraction */
 	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
@@ -134,5 +136,7 @@ private:
 
 	vehicle_local_position_setpoint_s _vehicle_local_position_setpoint; /**< Output position setpoint that every task has */
 
+	void _resetSetpoint() { _vehicle_local_position_setpoint = empty_setpoint; }
+
 	bool _evaluate_vehicle_position();
 };
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
index 3869863218..97d45e970d 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
@@ -53,8 +53,8 @@ FlightTaskOrbit::FlightTaskOrbit(control::SuperBlock *parent, const char *name)
 
 bool FlightTaskOrbit::applyCommandParameters(vehicle_command_s command)
 {
-	float &r = command.param3;
-	float &v = command.param4;
+	float &r = command.param3; /**< commanded radius */
+	float &v = command.param4; /**< commanded velocity */
 
 	if (math::isInRange(r, 5.f, 50.f) &&
 	    fabs(v) < 10.f) {