FlightTasks: initialize task state based on subscriptions after the...

FlightTasks: initialize task state based on subscriptions after the SubscriptionArray was initialized

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;
 	}
 

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();

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 */

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();

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()

src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp

@@ -50,6 +50,8 @@ public:
 
 	virtual ~FlightTaskOrbit() = default;
 
+	int activate() override;
+
 	int update() override;
 
 private: