diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 95b356f97e..0164d11a5c 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_module( tasks/FlightTask.cpp tasks/FlightTaskManual.cpp tasks/FlightTaskOrbit.cpp + SubscriptionArray.cpp DEPENDS platforms__common modules__uORB diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index e56b08d52e..693dc52e39 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -45,6 +45,8 @@ #include "tasks/FlightTaskManual.hpp" #include "tasks/FlightTaskOrbit.hpp" +#include "SubscriptionArray.hpp" + #include class FlightTasks : control::SuperBlock @@ -68,6 +70,7 @@ public: int update() { if (is_any_task_active()) { + _subscription_array.update(); return _current_task->update(); } @@ -132,6 +135,13 @@ public: return 1; } + if (!_current_task->initializeSubscriptions(_subscription_array)) { + _current_task->~FlightTask(); + _current_task = nullptr; + _current_task_index = -1; + return 1; + } + _current_task_index = task_number; _current_task->update(); return 0; @@ -172,4 +182,6 @@ private: FlightTask *_current_task = nullptr; int _current_task_index = -1; + + SubscriptionArray _subscription_array; }; diff --git a/src/lib/FlightTasks/SubscriptionArray.cpp b/src/lib/FlightTasks/SubscriptionArray.cpp new file mode 100644 index 0000000000..6a42cbde1d --- /dev/null +++ b/src/lib/FlightTasks/SubscriptionArray.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "SubscriptionArray.hpp" + +#include + +SubscriptionArray::~SubscriptionArray() +{ + cleanup(); +} + +void SubscriptionArray::cleanup() +{ + for (int i = 0; i < _subscriptions_count; ++i) { + delete _subscriptions[i]; + } + + delete[] _subscriptions; + _subscriptions = nullptr; +} + +bool SubscriptionArray::resizeSubscriptions() +{ + const int new_size = _subscriptions_size == 0 ? 4 : _subscriptions_size * 2; + uORB::SubscriptionNode **new_array = new uORB::SubscriptionNode*[new_size]; + + if (!new_array) { + return false; + } + + if (_subscriptions) { + memcpy(new_array, _subscriptions, sizeof(uORB::SubscriptionNode *)*_subscriptions_count); + delete[] _subscriptions; + } + + _subscriptions = new_array; + _subscriptions_size = new_size; + + return true; +} + +void SubscriptionArray::update() +{ + for (int i = 0; i < _subscriptions_count; ++i) { + _subscriptions[i]->update(); + } +} diff --git a/src/lib/FlightTasks/SubscriptionArray.hpp b/src/lib/FlightTasks/SubscriptionArray.hpp new file mode 100644 index 0000000000..a5f7121844 --- /dev/null +++ b/src/lib/FlightTasks/SubscriptionArray.hpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file SubscriptionArray.hpp + * + * Simple array that contains a dynamic amount of Subscription instances + * + * @author Beat Küng + */ + +#pragma once + +#include + +class SubscriptionArray +{ +public: + SubscriptionArray() = default; + ~SubscriptionArray(); + + /** + * Get a subscription + * @param meta ORB_ID(topic) + * @param subscription returned subscription (output parameter) + * @param instance topic instance + * @return true on success, false otherwise (subscription set to nullptr) + */ + template + bool get(const struct orb_metadata *meta, uORB::Subscription *&subscription, unsigned instance = 0); + + /** + * update all subscriptions + */ + void update(); + +private: + void cleanup(); + + bool resizeSubscriptions(); + + uORB::SubscriptionNode **_subscriptions{nullptr}; + int _subscriptions_count{0}; ///< number of valid subscriptions + int _subscriptions_size{0}; ///< actual size of the _subscriptions array +}; + + +template +bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::Subscription *&subscription, unsigned instance) +{ + // does it already exist? + for (int i = 0; i < _subscriptions_count; ++i) { + if (_subscriptions[i]->meta() == meta && _subscriptions[i]->instance() == instance) { + // we know the type must be correct, so we can use reinterpret_cast (dynamic_cast is not available) + subscription = reinterpret_cast*>(_subscriptions[i]); + return true; + } + } + + // resize if needed + if (_subscriptions_count >= _subscriptions_size) { + if (!resizeSubscriptions()) { + subscription = nullptr; + return false; + } + } + + subscription = new uORB::Subscription(meta, 0, instance); + + if (!subscription) { + return false; + } + + _subscriptions[_subscriptions_count++] = subscription; + return true; +} diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp index 4adf7500bb..195fb26d5b 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask.cpp @@ -3,23 +3,32 @@ constexpr uint64_t FlightTask::_timeout; + +bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!subscription_array.get(ORB_ID(vehicle_local_position), _sub_vehicle_local_position)) { + return false; + } + + return true; +} + int FlightTask::update() { _time_stamp_current = hrt_absolute_time(); _time = (_time_stamp_current - _time_stamp_activate) / 1e6f; _deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f; _time_stamp_last = _time_stamp_current; - updateSubscriptions(); int ret = _evaluate_vehicle_position(); return ret; } int 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; + 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; } else { diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 7299850162..22f1dd614b 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -47,17 +47,24 @@ #include #include +#include "../SubscriptionArray.hpp" + class FlightTask : public control::SuperBlock { public: FlightTask(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _sub_vehicle_local_position(ORB_ID(vehicle_local_position), 0, 0, &getSubscriptions()) + SuperBlock(parent, name) { _time_stamp_activate = hrt_absolute_time(); }; + /** + * initialize the uORB subscriptions using an array + * @return true on success, false on error + */ + virtual bool initializeSubscriptions(SubscriptionArray &subscription_array); + virtual ~FlightTask() = default; /** @@ -104,7 +111,7 @@ protected: void _set_yawspeed_setpoint(const float &yawspeed) { _vehicle_local_position_setpoint.yawspeed = yawspeed; }; private: - uORB::Subscription _sub_vehicle_local_position; + uORB::Subscription *_sub_vehicle_local_position{nullptr}; vehicle_local_position_setpoint_s _vehicle_local_position_setpoint; /**< Output position setpoint that every task has */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 030768a618..9241349b2a 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -48,7 +48,6 @@ using namespace matrix; 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), @@ -76,6 +75,18 @@ FlightTaskManual::FlightTaskManual(SuperBlock *parent, const char *name) : _hold_position = Vector3f(NAN, NAN, NAN); }; +bool FlightTaskManual::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!FlightTask::initializeSubscriptions(subscription_array)) { + return false; + } + + if (!subscription_array.get(ORB_ID(manual_control_setpoint), _sub_manual_control_setpoint)) { + return false; + } + + return true; +} int FlightTaskManual::update() { int ret = FlightTask::update(); @@ -135,12 +146,12 @@ int FlightTaskManual::update() int FlightTaskManual::_evaluate_sticks() { - if ((_time_stamp_current - _sub_manual_control_setpoint.get().timestamp) < _timeout) { + if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < _timeout) { /* get data and scale correctly */ - _sticks(0) = _sub_manual_control_setpoint.get().x; /* NED x, "pitch" [-1,1] */ - _sticks(1) = _sub_manual_control_setpoint.get().y; /* NED y, "roll" [-1,1] */ - _sticks(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ - _sticks(3) = _sub_manual_control_setpoint.get().r; /* "yaw" [-1,1] */ + _sticks(0) = _sub_manual_control_setpoint->get().x; /* NED x, "pitch" [-1,1] */ + _sticks(1) = _sub_manual_control_setpoint->get().y; /* NED y, "roll" [-1,1] */ + _sticks(2) = -(_sub_manual_control_setpoint->get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ + _sticks(3) = _sub_manual_control_setpoint->get().r; /* "yaw" [-1,1] */ /* apply expo and deadzone */ _sticks(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get()); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index c704c438f3..ea6e1c7053 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -54,6 +54,8 @@ public: virtual ~FlightTaskManual() = default; + bool initializeSubscriptions(SubscriptionArray &subscription_array) override; + int update() override; protected: @@ -62,7 +64,7 @@ protected: float get_input_frame_yaw() { return _yaw; }; private: - uORB::Subscription _sub_manual_control_setpoint; + uORB::Subscription *_sub_manual_control_setpoint{nullptr}; control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 3c5f58e7c2..70ffad289f 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -83,6 +83,10 @@ public: int getHandle() const { return _handle; } + const orb_metadata *meta() const { return _meta; } + + unsigned instance() const { return _instance; } + protected: const struct orb_metadata *_meta; int _handle;