mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
The constructor loop that assigns and subscribes every instance was expanded into each of the ~55 SubscriptionMultiArray members in the firmware. Move it into a non-template function in Subscription.cpp, called with the array, its size and the topic. px4_fmu-v6x_default: 1918712 -> 1917160 bytes of text, -1552 B. Assisted-by: Claude:claude-fable-5-1 Co-authored-by: Balduin <balduin@auterion.com>
207 lines
5.2 KiB
C++
207 lines
5.2 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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 <px4_platform_common/defines.h>
|
|
|
|
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<ORB_ID>(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
|