/**************************************************************************** * * 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 Subscription.cpp * */ #include "Subscription.hpp" #include "SubscriptionMultiArray.hpp" #include namespace uORB { bool Subscription::subscribe() { // check if already subscribed if (_node != nullptr) { return true; } if (_orb_id != ORB_ID::INVALID && uORB::Manager::get_instance()) { // While unsubscribed, _last_generation holds the node generation of the last failed lookup: // nodes are never removed, so the lookup cannot succeed before a node is added. const unsigned node_generation = uORB::Manager::orb_node_generation(); if (node_generation != 0 && node_generation == _last_generation) { return false; } unsigned initial_generation; void *node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &initial_generation); if (node) { _node = node; _last_generation = initial_generation; return true; } _last_generation = node_generation; } return false; } unsigned Subscription::get_last_generation() const { // while unsubscribed, _last_generation holds a node generation: report 0 as before return (_node != nullptr) ? _last_generation : 0; } void Subscription::unsubscribe() { if (_node != nullptr) { uORB::Manager::orb_remove_internal_subscriber(_node); } _node = nullptr; _last_generation = 0; } bool Subscription::update(void *dst) { if (subscribe()) { unsigned gen = _last_generation; bool ret = Manager::orb_data_copy(_node, dst, gen, true); _last_generation = gen; return ret; } return false; } bool Subscription::copy(void *dst) { if (subscribe()) { unsigned gen = _last_generation; bool ret = Manager::orb_data_copy(_node, dst, gen, false); _last_generation = gen; return ret; } return false; } bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { if (uORB::Manager::orb_device_node_exists(_orb_id, instance)) { // if desired new instance exists, unsubscribe from current unsubscribe(); _instance = instance; subscribe(); return true; } } else { // already on desired index return true; } 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; } void subscribeMultiArray(Subscription *subscriptions, uint8_t count, ORB_ID id) { for (uint8_t i = 0; i < count; i++) { subscriptions[i] = Subscription{id, i}; subscriptions[i].subscribe(); } } } // namespace uORB