From 071b09c65d3923f573a9c40d214f57362d1d860c Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Mon, 27 Nov 2017 21:31:47 +0100
Subject: [PATCH] FlightTasks: initialize task state based on subscriptions
 after the SubscriptionArray was initialized

---
 src/lib/FlightTasks/FlightTasks.hpp            | 14 +++++++++++---
 src/lib/FlightTasks/tasks/FlightTask.cpp       |  7 +++++++
 src/lib/FlightTasks/tasks/FlightTask.hpp       | 12 ++++++++----
 src/lib/FlightTasks/tasks/FlightTaskManual.cpp |  5 +++--
 src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp  |  5 +++++
 src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp  |  2 ++
 6 files changed, 36 insertions(+), 9 deletions(-)

diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 693dc52e39..7b464c21c5 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -132,18 +132,26 @@ public:
 
 		default:
 			/* invalid task */
-			return 1;
+			return -1;
 		}
 
 		if (!_current_task->initializeSubscriptions(_subscription_array)) {
 			_current_task->~FlightTask();
 			_current_task = nullptr;
 			_current_task_index = -1;
-			return 1;
+			return -2;
+		}
+
+		_subscription_array.update();
+
+		if (_current_task->activate()) {
+			_current_task->~FlightTask();
+			_current_task = nullptr;
+			_current_task_index = -1;
+			return -3;
 		}
 
 		_current_task_index = task_number;
-		_current_task->update();
 		return 0;
 	}
 
diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp
index 195fb26d5b..1c11d3386c 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.cpp
@@ -13,6 +13,13 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
 	return true;
 }
 
+int FlightTask::activate()
+{
+	_time_stamp_activate = hrt_absolute_time();
+	update(); /* to get subscriptions and evaluate them */
+	return 0;
+}
+
 int FlightTask::update()
 {
 	_time_stamp_current = hrt_absolute_time();
diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp
index 40cb3ed03d..6eb0746431 100644
--- a/src/lib/FlightTasks/tasks/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask.hpp
@@ -55,9 +55,7 @@ class FlightTask : public control::Block
 public:
 	FlightTask(control::SuperBlock *parent, const char *name) :
 		Block(parent, name)
-	{
-		_time_stamp_activate = hrt_absolute_time();
-	};
+	{ }
 
 	/**
 	 * initialize the uORB subscriptions using an array
@@ -65,6 +63,12 @@ public:
 	 */
 	virtual bool initializeSubscriptions(SubscriptionArray &subscription_array);
 
+	/**
+	* Call once on the event where you switch to the task
+	* @return 0 on success, <0 on error
+	*/
+	virtual int activate();
+
 	virtual ~FlightTask() = default;
 
 	/**
@@ -79,7 +83,7 @@ public:
 	const vehicle_local_position_setpoint_s &get_position_setpoint()
 	{
 		return _vehicle_local_position_setpoint;
-	};
+	}
 
 protected:
 	/* time abstraction */
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp
index b6a1d8cc0c..30738aa127 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp
@@ -60,7 +60,7 @@ FlightTaskManual::FlightTaskManual(control::SuperBlock *parent, const char *name
 	_jerk_hor_min(parent, "MPC_JERK_MIN", false),
 	_deceleration_hor_slow(parent, "MPC_DEC_HOR_SLOW", false),
 	_acceleration_hor_max(this, "MPC_ACC_HOR_MAX", false),
-	_acceleration_hor_manual(this, "MPC_ACC_HOR_MAN", false),
+	_acceleration_hor_manual(this, "MPC_ACC_HOR", false),
 	_acceleration_z_max_up(this, "MPC_ACC_UP_MAX", false),
 	_acceleration_z_max_down(this, "MPC_ACC_DOWN_MAX", false),
 	_rc_flt_smp_rate(parent, "RC_FLT_SMP_RATE", false),
@@ -73,7 +73,7 @@ FlightTaskManual::FlightTaskManual(control::SuperBlock *parent, const char *name
 	_filter_roll_stick.set_cutoff_frequency(_rc_flt_smp_rate.get(), _rc_flt_cutoff.get());
 	_filter_pitch_stick.set_cutoff_frequency(_rc_flt_smp_rate.get(), _rc_flt_cutoff.get());
 	_hold_position = Vector3f(NAN, NAN, NAN);
-};
+}
 
 bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_array)
 {
@@ -87,6 +87,7 @@ bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_a
 
 	return true;
 }
+
 int FlightTaskManual::update()
 {
 	int ret = FlightTask::update();
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
index 3852a2f287..cbaeb08d6b 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
@@ -47,12 +47,17 @@ using namespace matrix;
 
 FlightTaskOrbit::FlightTaskOrbit(control::SuperBlock *parent, const char *name) :
 	FlightTaskManual(parent, name)
+{}
+
+int FlightTaskOrbit::activate()
 {
+	int ret = FlightTaskManual::activate();
 	_r = 1.f;
 	_v =  0.5f;
 	_z = _position(2);
 	_center = Vector2f(_position.data());
 	_center(0) -= _r;
+	return ret;
 }
 
 int FlightTaskOrbit::update()
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
index 1bc81e7378..682989c709 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
@@ -50,6 +50,8 @@ public:
 
 	virtual ~FlightTaskOrbit() = default;
 
+	int activate() override;
+
 	int update() override;
 
 private: