flight tasks: use placement new to reduce memory overhead and the need for dynamic allocations

In addition, we will need some shared data structure for the uorb
subscriptions.

src/lib/FlightTasks/FlightTasks.hpp

@@ -45,15 +45,21 @@
 #include "tasks/FlightTaskManual.hpp"
 #include "tasks/FlightTaskOrbit.hpp"
 
+#include <new>
+
 class FlightTasks : control::SuperBlock
 {
 public:
 	FlightTasks() :
-		SuperBlock(nullptr, "TSK"),
-		Manual(this, "MAN"),
-		Orbit(this, "ORB")
+		SuperBlock(nullptr, "TSK")
 	{};
-	~FlightTasks() {};
+
+	~FlightTasks()
+	{
+		if (_current_task) {
+			_current_task->~FlightTask();
+		}
+	};
 
 	/**
 	 * Call regularly in the control loop cycle to execute the task
@@ -62,11 +68,10 @@ public:
 	int update()
 	{
 		if (is_any_task_active()) {
-			return _tasks[_current_task]->update();
-
-		} else {
-			return 1;
+			return _current_task->update();
 		}
+
+		return 1;
 	}
 
 	/**
@@ -74,7 +79,7 @@ public:
 	 */
 	const vehicle_local_position_setpoint_s &get_position_setpoint()
 	{
-		return _tasks[_current_task]->get_position_setpoint();
+		return _current_task->get_position_setpoint();
 	}
 
 	/**
@@ -89,9 +94,9 @@ public:
 	 * Switch to the next task in the available list (for testing)
 	 * @return 0 on success, <0 on error
 	 */
-	void switch_task()
+	int switch_task()
 	{
-		switch_task(_current_task + 1);
+		return switch_task(_current_task_index + 1);
 	}
 
 	/**
@@ -102,37 +107,47 @@ public:
 	int switch_task(int task_number)
 	{
 		/* switch to the running task, nothing to do */
-		if (task_number == _current_task) {
+		if (task_number == _current_task_index) {
 			return 0;
 		}
 
 		/* disable the old task if there is any */
-		if (is_any_task_active()) {
-			_tasks[_current_task]->disable();
+		if (_current_task) {
+			_current_task->~FlightTask();
+			_current_task = nullptr;
+			_current_task_index = -1;
 		}
 
-		/* if the new task exists and it activates succesfully we switched */
-		if (is_task_number_valid(task_number) && !_tasks[task_number]->activate()) {
-			_current_task = task_number;
-			return 0;
+		switch (task_number) {
+		case 0:
+			_current_task = new (&_task_union.manual) FlightTaskManual(this, "MAN");
+			break;
+
+		case 1:
+			_current_task = new (&_task_union.orbit) FlightTaskOrbit(this, "ORB");
+			break;
+
+		default:
+			/* invalid task */
+			return 1;
 		}
 
-		/* something went wrong, no task running anymore */
-		_current_task = -1;
-		return 1;
+		_current_task_index = task_number;
+		_current_task->update();
+		return 0;
 	}
 
 	/**
 	 * Get the number of the active task
 	 * @return number of active task, -1 if there is none
 	 */
-	int get_active_task() const { return _current_task; };
+	int get_active_task() const { return _current_task_index; };
 
 	/**
 	 * Check if any task is active
 	 * @return true if a task is active, false if not
 	 */
-	bool is_any_task_active() const { return is_task_number_valid(_current_task); };
+	bool is_any_task_active() const { return _current_task; };
 
 	/**
 	 * Check if the task number exists
@@ -142,9 +157,19 @@ public:
 
 private:
 	static constexpr int _task_count = 2;
-	FlightTask *_tasks[_task_count] = {&Manual, &Orbit};
-	FlightTaskManual Manual;
-	FlightTaskOrbit Orbit;
 
-	int _current_task = -1;
+	/** union with all existing tasks: we use it to make sure that only the memory of the largest existing
+	 * task is needed, and to avoid using dynamic memory allocations.
+	 */
+	union TaskUnion {
+		TaskUnion() {}
+		~TaskUnion() {}
+
+		FlightTaskManual manual;
+		FlightTaskOrbit orbit;
+	};
+	TaskUnion _task_union; ///< storage for the currently active task
+
+	FlightTask *_current_task = nullptr;
+	int _current_task_index = -1;
 };

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -54,25 +54,11 @@ public:
 	FlightTask(SuperBlock *parent, const char *name) :
 		SuperBlock(parent, name),
 		_sub_vehicle_local_position(ORB_ID(vehicle_local_position), 0, 0, &getSubscriptions())
-	{ };
-	virtual ~FlightTask() {};
-
-	/**
-	 * Call once on the event where you switch to the task
-	 * @return 0 on success, <0 on error
-	 */
-	virtual int activate()
 	{
 		_time_stamp_activate = hrt_absolute_time();
-		FlightTask::update();
-		return 0;
 	};
 
-	/**
-	 * Call once on the event of switching away from the task
-	 * @return 0 on success, <0 on error
-	 */
-	virtual int disable() { return 0; };
+	virtual ~FlightTask() = default;
 
 	/**
 	 * To be called regularly in the control loop cycle to execute the task

src/lib/FlightTasks/tasks/FlightTaskManual.cpp

@@ -46,20 +46,35 @@
 
 using namespace matrix;
 
-int FlightTaskManual::activate()
+FlightTaskManual::FlightTaskManual(SuperBlock *parent, const char *name) :
+	FlightTask(parent, name),
+	_sub_manual_control_setpoint(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()),
+	_xy_vel_man_expo(parent, "MPC_XY_MAN_EXPO", false),
+	_z_vel_man_expo(parent, "MPC_Z_MAN_EXPO", false),
+	_hold_dz(parent, "MPC_HOLD_DZ", false),
+	_velocity_hor_manual(parent, "MPC_VEL_MANUAL", false),
+	_z_vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false),
+	_z_vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false),
+	_hold_max_xy(parent, "MPC_HOLD_MAX_XY", false),
+	_hold_max_z(parent, "MPC_HOLD_MAX_Z", false),
+	_jerk_hor_max(parent, "MPC_JERK_MAX", false),
+	_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_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),
+	_rc_flt_cutoff(parent, "RC_FLT_CUTOFF", false),
+	_manual_direction_change_hysteresis(false),
+	_filter_roll_stick(50.0f, 10.0f),
+	_filter_pitch_stick(50.0f, 10.0f)
 {
-	FlightTask::activate();
+	_manual_direction_change_hysteresis.set_hysteresis_time_from(false, DIRECTION_CHANGE_TIME_US);
 	_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);
-	return 0;
-}
-
-int FlightTaskManual::disable()
-{
-	FlightTask::disable();
-	return 0;
-}
+};
 
 int FlightTaskManual::update()
 {

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -50,36 +50,10 @@
 class FlightTaskManual : public FlightTask
 {
 public:
-	FlightTaskManual(SuperBlock *parent, const char *name) :
-		FlightTask(parent, name),
-		_sub_manual_control_setpoint(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()),
-		_xy_vel_man_expo(parent, "MPC_XY_MAN_EXPO", false),
-		_z_vel_man_expo(parent, "MPC_Z_MAN_EXPO", false),
-		_hold_dz(parent, "MPC_HOLD_DZ", false),
-		_velocity_hor_manual(parent, "MPC_VEL_MANUAL", false),
-		_z_vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false),
-		_z_vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false),
-		_hold_max_xy(parent, "MPC_HOLD_MAX_XY", false),
-		_hold_max_z(parent, "MPC_HOLD_MAX_Z", false),
-		_jerk_hor_max(parent, "MPC_JERK_MAX", false),
-		_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_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),
-		_rc_flt_cutoff(parent, "RC_FLT_CUTOFF", false),
-		_manual_direction_change_hysteresis(false),
-		_filter_roll_stick(50.0f, 10.0f),
-		_filter_pitch_stick(50.0f, 10.0f)
-	{
-		_manual_direction_change_hysteresis.set_hysteresis_time_from(false, DIRECTION_CHANGE_TIME_US);
-	};
-	virtual ~FlightTaskManual() {};
+	FlightTaskManual(SuperBlock *parent, const char *name);
+
+	virtual ~FlightTaskManual() = default;
 
-	int activate() override;
-	int disable() override;
 	int update() override;
 
 protected:

src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp

@@ -45,21 +45,14 @@
 
 using namespace matrix;
 
-int FlightTaskOrbit::activate()
+FlightTaskOrbit::FlightTaskOrbit(SuperBlock *parent, const char *name) :
+	FlightTaskManual(parent, name)
 {
-	FlightTask::activate();
 	_r = 1.f;
 	_v =  0.5f;
 	_z = _position(2);
 	_center = Vector2f(_position.data());
 	_center(0) -= _r;
-	return 0;
-}
-
-int FlightTaskOrbit::disable()
-{
-	FlightTask::disable();
-	return 0;
 }
 
 int FlightTaskOrbit::update()

src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp

@@ -46,13 +46,10 @@
 class FlightTaskOrbit : public FlightTaskManual
 {
 public:
-	FlightTaskOrbit(SuperBlock *parent, const char *name) :
-		FlightTaskManual(parent, name)
-	{};
-	virtual ~FlightTaskOrbit() {};
+	FlightTaskOrbit(SuperBlock *parent, const char *name);
+
+	virtual ~FlightTaskOrbit() = default;
 
-	int activate() override;
-	int disable() override;
 	int update() override;
 
 private: