From 888a63c00151385c316584975636951862d1461b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 28 Nov 2017 22:00:46 +0100 Subject: [PATCH] FlightTasks: bool return values for clarity & introduce updateInitialize() to have input data fetching separated from the update() --- src/lib/FlightTasks/FlightTasks.hpp | 26 +++++++------------ src/lib/FlightTasks/tasks/FlightTask.cpp | 13 +++++----- src/lib/FlightTasks/tasks/FlightTask.hpp | 19 +++++++++----- .../FlightTasks/tasks/FlightTaskManual.cpp | 16 +++++++----- .../FlightTasks/tasks/FlightTaskManual.hpp | 6 +++-- src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp | 10 +++---- src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp | 4 +-- 7 files changed, 48 insertions(+), 46 deletions(-) diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 87bd1e01a1..62147c9c2c 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -65,13 +65,13 @@ public: /** * Call regularly in the control loop cycle to execute the task - * @return 0 on success, <0 on error + * @return true on success, false on error */ - int update() + bool update() { if (is_any_task_active()) { _subscription_array.update(); - return _current_task->update(); + return _current_task->updateInitialize() && _current_task->update(); } return 1; @@ -95,7 +95,7 @@ public: /** * Switch to the next task in the available list (for testing) - * @return 0 on success, <0 on error + * @return true on success, false on error */ int switch_task() { @@ -105,13 +105,13 @@ public: /** * Switch to a specific task (for normal usage) * @param task number to switch to - * @return 0 on success, <0 on error + * @return true on success, false on error */ int switch_task(int task_number) { /* switch to the running task, nothing to do */ if (task_number == _current_task_index) { - return 0; + return true; } /* disable the old task if there is any */ @@ -135,6 +135,7 @@ public: return -1; } + /* subscription failed */ if (!_current_task->initializeSubscriptions(_subscription_array)) { _current_task->~FlightTask(); _current_task = nullptr; @@ -144,7 +145,8 @@ public: _subscription_array.forcedUpdate(); // make sure data is available for all new subscriptions - if (_current_task->activate()) { + /* activation failed */ + if (!_current_task->updateInitialize() || !_current_task->activate()) { _current_task->~FlightTask(); _current_task = nullptr; _current_task_index = -1; @@ -167,14 +169,7 @@ public: */ bool is_any_task_active() const { return _current_task; } - /** - * Check if the task number exists - * @return true if yes, false if not - */ - bool is_task_number_valid(int task_number) const { return task_number > -1 && task_number < _task_count; } - private: - static constexpr int _task_count = 2; /** 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. @@ -185,8 +180,7 @@ private: FlightTaskManual manual; FlightTaskOrbit orbit; - }; - TaskUnion _task_union; ///< storage for the currently active task + } _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.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp index ab57f8720b..9104aa1890 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask.cpp @@ -13,14 +13,13 @@ bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array) return true; } -int FlightTask::activate() +bool FlightTask::activate() { _time_stamp_activate = hrt_absolute_time(); - update(); /* to get subscriptions and evaluate them */ - return 0; + return true; } -int FlightTask::update() +bool FlightTask::updateInitialize() { _time_stamp_current = hrt_absolute_time(); _time = (_time_stamp_current - _time_stamp_activate) / 1e6f; @@ -29,16 +28,16 @@ int FlightTask::update() return _evaluate_vehicle_position(); } -int FlightTask::_evaluate_vehicle_position() +bool FlightTask::_evaluate_vehicle_position() { if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) { _position = matrix::Vector3f(&_sub_vehicle_local_position->get().x); _velocity = matrix::Vector3f(&_sub_vehicle_local_position->get().vx); _yaw = _sub_vehicle_local_position->get().yaw; - return 0; + return true; } else { _velocity.zero(); /* default velocity is all zero */ - return -1; + return false; } } diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index ae46d38ee5..b790cd98a8 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -57,6 +57,8 @@ public: Block(parent, name) { } + virtual ~FlightTask() = default; + /** * initialize the uORB subscriptions using an array * @return true on success, false on error @@ -65,17 +67,22 @@ public: /** * Call once on the event where you switch to the task - * @return 0 on success, <0 on error + * @return true on success, false on error */ - virtual int activate(); + virtual bool activate(); - virtual ~FlightTask() = default; + /** + * Call before activate() or update() + * to initialize time and input data + * @return true on success, false on error + */ + virtual bool updateInitialize(); /** * To be called regularly in the control loop cycle to execute the task - * @return 0 on success, <0 on error + * @return true on success, false on error */ - virtual int update(); + virtual bool update() = 0; /** * Get the output data @@ -119,5 +126,5 @@ private: vehicle_local_position_setpoint_s _vehicle_local_position_setpoint; /**< Output position setpoint that every task has */ - int _evaluate_vehicle_position(); + bool _evaluate_vehicle_position(); }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 30738aa127..59b48d95cc 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -88,11 +88,13 @@ bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_a return true; } -int FlightTaskManual::update() +bool FlightTaskManual::updateInitialize() { - int ret = FlightTask::update(); - ret += _evaluate_sticks(); + return FlightTask::updateInitialize() && _evaluate_sticks(); +} +bool FlightTaskManual::update() +{ /* prepare stick input */ Vector2f stick_xy(_sticks.data()); /**< horizontal two dimensional stick input within a unit circle */ @@ -142,10 +144,10 @@ int FlightTaskManual::update() } _set_position_setpoint(_hold_position); - return ret; + return true; } -int FlightTaskManual::_evaluate_sticks() +bool FlightTaskManual::_evaluate_sticks() { if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < _timeout) { /* get data and scale correctly */ @@ -159,11 +161,11 @@ int FlightTaskManual::_evaluate_sticks() _sticks(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get()); _sticks(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get()); - return 0; + return true; } else { _sticks.zero(); /* default is all zero */ - return -1; + return false; } } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index 63fe71c2f5..41f688013b 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -56,7 +56,9 @@ public: bool initializeSubscriptions(SubscriptionArray &subscription_array) override; - int update() override; + bool updateInitialize() override; + + bool update() override; protected: matrix::Vector _sticks; @@ -77,7 +79,7 @@ private: matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */ - int _evaluate_sticks(); + bool _evaluate_sticks(); /* --- Acceleration Smoothing --- */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp index cbaeb08d6b..54aa96958b 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp @@ -49,9 +49,9 @@ FlightTaskOrbit::FlightTaskOrbit(control::SuperBlock *parent, const char *name) FlightTaskManual(parent, name) {} -int FlightTaskOrbit::activate() +bool FlightTaskOrbit::activate() { - int ret = FlightTaskManual::activate(); + bool ret = FlightTaskManual::activate(); _r = 1.f; _v = 0.5f; _z = _position(2); @@ -60,10 +60,8 @@ int FlightTaskOrbit::activate() return ret; } -int FlightTaskOrbit::update() +bool FlightTaskOrbit::update() { - int ret = FlightTaskManual::update(); - _r += _sticks(0) * _deltatime; _r = math::constrain(_r, 1.f, 20.f); _v -= _sticks(1) * _deltatime; @@ -85,5 +83,5 @@ int FlightTaskOrbit::update() _set_position_setpoint(Vector3f(NAN, NAN, _z)); _set_velocity_setpoint(Vector3f(velocity_xy(0), velocity_xy(1), 0.f)); _set_yaw_setpoint(yaw); - return ret; + return true; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp index 682989c709..eb4012ce99 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp @@ -50,9 +50,9 @@ public: virtual ~FlightTaskOrbit() = default; - int activate() override; + bool activate() override; - int update() override; + bool update() override; private: