From 6c0e7654ed5e1dec4fdb422e3118368882061e7e Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 26 Sep 2017 10:53:20 +0200
Subject: [PATCH] FlightTasks: added handling for switching to the already
 active task and success feedback removed and added comments

---
 src/lib/FlightTasks/FlightTasks.hpp            | 15 +++++++++++----
 src/lib/FlightTasks/tasks/FlightTask.hpp       |  4 ++--
 src/lib/FlightTasks/tasks/FlightTaskManual.hpp |  5 +----
 3 files changed, 14 insertions(+), 10 deletions(-)

diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index c512005083..a3d908a616 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -93,6 +93,7 @@ public:
 
 	/**
 	 * Switch to the next task in the available list (for testing)
+	 * @return 0 on success, >0 on error
 	 */
 	void switch_task()
 	{
@@ -102,9 +103,14 @@ public:
 	/**
 	 * Switch to a specific task (for normal usage)
 	 * @param task number to switch to
+	 * @return 0 on success, >0 on error
 	 */
-	void switch_task(int task_number)
+	int switch_task(int task_number)
 	{
+		if (task_number == _current_task) {
+			return 0;
+		}
+
 		if (is_any_task_active()) {
 			_tasks[_current_task]->disable();
 		}
@@ -113,10 +119,11 @@ public:
 
 		if (is_any_task_active()) {
 			_tasks[_current_task]->activate();
-
-		} else {
-			_current_task = -1;
+			return 0;
 		}
+
+		_current_task = -1;
+		return 1;
 	};
 
 	/**
diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index 0fcbe76fe0..e5e9e45d08 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -111,8 +111,8 @@ protected:
 
 	/* Prepared general inputs for every task */
 	matrix::Vector<float, 4> _sticks;
-	matrix::Vector3f _position;
-	matrix::Vector3f _velocity;
+	matrix::Vector3f _position; /*< current vehicle position */
+	matrix::Vector3f _velocity; /*< current vehicle velocity */
 	float _yaw;
 
 	/**
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
index ab670a7a6d..8a48aeedcf 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp
@@ -114,8 +114,6 @@ public:
 		velocity_setpoint = velocity_setpoint.emult(vel_scale);
 
 		_set_velocity_setpoint(velocity_setpoint);
-		//printf("------");
-		//velocity_setpoint.print();
 
 		/* handle position and altitude hold */
 		const bool stick_xy_zero = stick_xy_norm <= FLT_EPSILON;
@@ -142,7 +140,6 @@ public:
 		}
 
 		_set_position_setpoint(_hold_position);
-		//_hold_position.print();
 		return 0;
 	};
 
@@ -164,5 +161,5 @@ private:
 
 	uORB::Subscription<control_state_s> _sub_control_state;
 
-	matrix::Vector3f _hold_position;
+	matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */
 };