From 88bf40e3cb536b6834f438af3fc73bac939d6919 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 30 May 2017 14:25:54 +0200
Subject: [PATCH] FlightTasks: added simple task switching with possibility do
 disable FlightTasks completely

---
 src/lib/FlightTasks/FlightTasks.hpp      | 41 +++++++++++++++++++++---
 src/lib/FlightTasks/tasks/FlightTask.hpp |  6 ++--
 2 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 122df8e990..995d87f122 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -52,10 +52,21 @@ public:
 
 	/**
 	 * Call regularly in the control loop cycle to execute the task
-	 * @return 0 on success, >0 on error otherwise
+	 * @return 0 on success, >0 on error
 	 */
-	int update() { return _tasks[_current_task]->update(); };
+	int update()
+	{
+		if (is_any_task_active()) {
+			return _tasks[_current_task]->update();
+
+		} else {
+			return 1;
+		}
+	};
 
+	/**
+	 * Call this function initially to point all tasks to the general input data
+	 */
 	void set_input_pointers(vehicle_local_position_s *vehicle_local_position,
 				manual_control_setpoint_s *manual_control_setpoint)
 	{
@@ -65,15 +76,37 @@ public:
 		}
 	};
 
+	/**
+	 * Switch to a different task
+	 */
+	void switch_task(int task_number)
+	{
+		if (is_any_task_active()) {
+			_tasks[_current_task]->disable();
+		}
+
+		_current_task = task_number;
+
+		if (is_any_task_active()) {
+			_tasks[_current_task]->activate();
+		}
+	};
+
 	/**
 	 * Call to get result of the task execution
-	 * @return pointer to
+	 * @return pointer to the setpoint for the position controller
 	 */
 	const vehicle_local_position_setpoint_s *get_position_setpoint() const { return Orbit.get_position_setpoint(); };
 
+	/**
+	 * Call to get result of the task execution
+	 * @return pointer to
+	 */
+	bool is_any_task_active() const { return _current_task > -1 && _current_task < _task_count; };
+
 private:
 	static const int _task_count = 1;
-	int _current_task = 0;
+	int _current_task = -1;
 
 	FlightTaskOrbit Orbit;
 	FlightTask *_tasks[_task_count] = {&Orbit};
diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index d84b77d8d9..977248db0b 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -55,7 +55,7 @@ public:
 	/**
 	 * Call once on the event where you switch to the task
 	 * Note: Set the necessary input pointers first!
-	 * @return 0 on success, >0 on error otherwise
+	 * @return 0 on success, >0 on error
 	 */
 	virtual int activate()
 	{
@@ -65,13 +65,13 @@ public:
 
 	/**
 	 * Call once on the event of switching away from the task
-	 * 	@return 0 on success, >0 on error otherwise
+	 * 	@return 0 on success, >0 on error
 	 */
 	virtual int disable() = 0;
 
 	/**
 	 * To be called regularly in the control loop cycle to execute the task
-	 * @return 0 on success, >0 on error otherwise
+	 * @return 0 on success, >0 on error
 	 */
 	virtual int update()
 	{