From b5ecf9824d9dd7db7e9709dac614af64906626a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 20 Nov 2017 21:27:32 +0100 Subject: [PATCH] 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 | 79 ++++++++++++------- src/lib/FlightTasks/tasks/FlightTask.hpp | 16 +--- .../FlightTasks/tasks/FlightTaskManual.cpp | 35 +++++--- .../FlightTasks/tasks/FlightTaskManual.hpp | 32 +------- src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp | 11 +-- src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp | 9 +-- 6 files changed, 86 insertions(+), 96 deletions(-) diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 7f56433f81..e56b08d52e 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -45,15 +45,21 @@ #include "tasks/FlightTaskManual.hpp" #include "tasks/FlightTaskOrbit.hpp" +#include + 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; }; diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 99ceaf32c2..7299850162 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/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 diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 14e0f9199b..030768a618 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/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() { diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index 4106fc5dd0..c704c438f3 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/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: diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp index 44683ee029..3b43cb878d 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp +++ b/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() diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index ed4345fe3a..24661719e6 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/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: