diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index 6e157af3c1f..85c86aa311b 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -42,10 +42,12 @@ set(SRCS) set(SRCS_COMMON ORBSet.hpp + Publication.cpp Publication.hpp PublicationMulti.hpp Subscription.cpp Subscription.hpp + SubscriptionCallback.cpp SubscriptionCallback.hpp SubscriptionInterval.cpp SubscriptionInterval.hpp diff --git a/platforms/common/uORB/Publication.cpp b/platforms/common/uORB/Publication.cpp new file mode 100644 index 00000000000..0fca7fbe469 --- /dev/null +++ b/platforms/common/uORB/Publication.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 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 Publication.cpp + * + * Out-of-line definitions of the type-independent PublicationBase machinery. + * Keeping advertise()/publish() out of the header emits them once instead of + * once per translation unit (PX4 links with bfd ld, no ICF, and no LTO). + */ + +#include "Publication.hpp" + +namespace uORB +{ + +bool PublicationBase::advertise() +{ + if (!advertised()) { + _handle = orb_advertise(get_topic(), nullptr); + } + + return advertised(); +} + +bool PublicationBase::publish(const void *data) +{ + if (!advertised()) { + advertise(); + } + + return (Manager::orb_publish(get_topic(), _handle, data) == PX4_OK); +} + +} // namespace uORB diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp index d8415c43140..03d274a0351 100644 --- a/platforms/common/uORB/Publication.hpp +++ b/platforms/common/uORB/Publication.hpp @@ -54,14 +54,7 @@ public: bool advertised() const { return _handle != nullptr; } - bool advertise() - { - if (!advertised()) { - _handle = orb_advertise(get_topic(), nullptr); - } - - return advertised(); - } + bool advertise(); bool unadvertise() { return (Manager::orb_unadvertise(_handle) == PX4_OK); } @@ -82,14 +75,7 @@ protected: } // type-independent publish; data points to a message of the topic's type - bool publish(const void *data) - { - if (!advertised()) { - advertise(); - } - - return (Manager::orb_publish(get_topic(), _handle, data) == PX4_OK); - } + bool publish(const void *data); orb_advert_t _handle{nullptr}; const ORB_ID _orb_id; diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 473c5548666..d85457605e9 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -73,6 +73,24 @@ void Subscription::unsubscribe() _last_generation = 0; } +bool Subscription::update(void *dst) +{ + if (subscribe()) { + return Manager::orb_data_copy(_node, dst, _last_generation, true); + } + + return false; +} + +bool Subscription::copy(void *dst) +{ + if (subscribe()) { + return Manager::orb_data_copy(_node, dst, _last_generation, false); + } + + return false; +} + bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { @@ -92,4 +110,66 @@ bool Subscription::ChangeInstance(uint8_t instance) return false; } +Subscription::Subscription(ORB_ID id, uint8_t instance) : + _orb_id(id), + _instance(instance) +{ + subscribe(); +} + +Subscription::Subscription(const orb_metadata *meta, uint8_t instance) : + _orb_id((meta == nullptr) ? ORB_ID::INVALID : static_cast(meta->o_id)), + _instance(instance) +{ + subscribe(); +} + +Subscription::Subscription(const Subscription &other) : _orb_id(other._orb_id), _instance(other._instance) {} + +Subscription::Subscription(const Subscription &&other) noexcept : _orb_id(other._orb_id), _instance(other._instance) {} + +Subscription &Subscription::operator=(const Subscription &other) +{ + // Check for self-assignment + if (this == &other) { + return *this; + } + + unsubscribe(); + _orb_id = other._orb_id; + _instance = other._instance; + return *this; +} + +Subscription &Subscription::operator=(Subscription &&other) noexcept +{ + unsubscribe(); + _orb_id = other._orb_id; + _instance = other._instance; + return *this; +} + +Subscription::~Subscription() +{ + unsubscribe(); +} + +bool Subscription::advertised() +{ + if (subscribe()) { + return Manager::is_advertised(_node); + } + + return false; +} + +bool Subscription::updated() +{ + if (subscribe()) { + return Manager::updates_available(_node, _last_generation); + } + + return false; +} + } // namespace uORB diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index e8997627513..596d81cfeb0 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -63,12 +63,7 @@ public: * @param id The uORB ORB_ID enum for the topic. * @param instance The instance for multi sub. */ - Subscription(ORB_ID id, uint8_t instance = 0) : - _orb_id(id), - _instance(instance) - { - subscribe(); - } + Subscription(ORB_ID id, uint8_t instance = 0); /** * Constructor @@ -76,97 +71,44 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. * @param instance The instance for multi sub. */ - Subscription(const orb_metadata *meta = nullptr, uint8_t instance = 0) : - _orb_id((meta == nullptr) ? ORB_ID::INVALID : static_cast(meta->o_id)), - _instance(instance) - { - subscribe(); - } + Subscription(const orb_metadata *meta = nullptr, uint8_t instance = 0); // Copy constructor - Subscription(const Subscription &other) : _orb_id(other._orb_id), _instance(other._instance) {} + Subscription(const Subscription &other); // Move constructor - Subscription(const Subscription &&other) noexcept : _orb_id(other._orb_id), _instance(other._instance) {} + Subscription(const Subscription &&other) noexcept; // copy assignment - Subscription &operator=(const Subscription &other) - { - // Check for self-assignment - if (this == &other) { - return *this; - } - - unsubscribe(); - _orb_id = other._orb_id; - _instance = other._instance; - return *this; - } + Subscription &operator=(const Subscription &other); // move assignment - Subscription &operator=(Subscription &&other) noexcept - { - unsubscribe(); - _orb_id = other._orb_id; - _instance = other._instance; - return *this; - } + Subscription &operator=(Subscription &&other) noexcept; - ~Subscription() - { - unsubscribe(); - } + ~Subscription(); bool subscribe(); void unsubscribe(); bool valid() const { return _node != nullptr; } - bool advertised() - { - if (subscribe()) { - return Manager::is_advertised(_node); - } - - return false; - } + bool advertised(); /** * Check if there is a new update. */ - bool updated() - { - if (subscribe()) { - return Manager::updates_available(_node, _last_generation); - } - - return false; - } + bool updated(); /** * Update the struct * @param dst The uORB message struct we are updating. */ - bool update(void *dst) - { - if (subscribe()) { - return Manager::orb_data_copy(_node, dst, _last_generation, true); - } - - return false; - } + bool update(void *dst); /** * Copy the struct * @param dst The uORB message struct we are updating. */ - bool copy(void *dst) - { - if (subscribe()) { - return Manager::orb_data_copy(_node, dst, _last_generation, false); - } - - return false; - } + bool copy(void *dst); /** * Change subscription instance diff --git a/platforms/common/uORB/SubscriptionCallback.cpp b/platforms/common/uORB/SubscriptionCallback.cpp new file mode 100644 index 00000000000..908498f3091 --- /dev/null +++ b/platforms/common/uORB/SubscriptionCallback.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 SubscriptionCallback.cpp + * + * Out-of-line register/unregister machinery. Keeping these out of the header + * emits them once instead of once per translation unit (PX4 links with bfd + * ld, no ICF, and no LTO). + */ + +#include "SubscriptionCallback.hpp" + +namespace uORB +{ + +bool SubscriptionCallback::registerCallback() +{ + if (!_registered) { + if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) { + // registered + _registered = true; + + } else { + // force topic creation by subscribing with old API + int fd = orb_subscribe_multi(_subscription.get_topic(), _subscription.get_instance()); + + // try to register callback again + if (_subscription.subscribe()) { + if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) { + _registered = true; + } + } + + orb_unsubscribe(fd); + } + } + + return _registered; +} + +void SubscriptionCallback::unregisterCallback() +{ + if (_subscription.get_node()) { + Manager::unregister_callback(_subscription.get_node(), this); + } + + _registered = false; +} + +} // namespace uORB diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp index 942a9fbe17d..7a68fbb9114 100644 --- a/platforms/common/uORB/SubscriptionCallback.hpp +++ b/platforms/common/uORB/SubscriptionCallback.hpp @@ -66,39 +66,9 @@ public: unregisterCallback(); }; - bool registerCallback() - { - if (!_registered) { - if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) { - // registered - _registered = true; + bool registerCallback(); - } else { - // force topic creation by subscribing with old API - int fd = orb_subscribe_multi(_subscription.get_topic(), _subscription.get_instance()); - - // try to register callback again - if (_subscription.subscribe()) { - if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) { - _registered = true; - } - } - - orb_unsubscribe(fd); - } - } - - return _registered; - } - - void unregisterCallback() - { - if (_subscription.get_node()) { - Manager::unregister_callback(_subscription.get_node(), this); - } - - _registered = false; - } + void unregisterCallback(); /** * Change subscription instance