mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
The publisher fires callbacks via SubscriptionCallbackWorkItem::call(), which read the subscriber's _last_generation (and called updated(), which reads it again) to decide whether to schedule the work item. That made the subscriber's read cursor a cross-thread field - publisher reads it while the subscriber writes it in copy() - which TSan flagged and which we had wrapped in an atomic. Instead, hand the publishing node's generation into call() and let the callback keep its own _last_scheduled_generation cursor for the count gate (_required_updates). call() runs under the node lock, so that cursor is only ever touched there and is serialized - no atomic needed, and using the real generation (not a plain counter) means a coalesced/missed call() does not drift the batch. The redundant updated() generation check is dropped (call() only runs right after a publish, so it is always true); the interval gate (_last_update / _interval_us) is kept unchanged. With the publisher no longer reading it, _last_generation reverts to a plain unsigned (subscriber-thread only). Behaviour is unchanged: the count and interval throttles fire at the same rates (verified via work_queue status - gyro_fft 62.5 Hz, flight_mode_manager 50 Hz) and TSan stays clean.
182 lines
4.4 KiB
C++
182 lines
4.4 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 <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()) {
|
|
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;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
} // namespace uORB
|