Files
PX4-Autopilot/platforms/common/uORB/uORBDeviceNode.cpp
T
e146fe4cc6 fix(uorb): keep shared topics advertised on publisher teardown (#29176)
* fix(dataman_client): keep request topic advertised on client teardown

* fix(uorb): keep shared topics advertised on publisher teardown

Single-instance publications share one advertised flag, so destroying one
publisher must not hide updates from the others.

Keep automatic unadvertising in PublicationMultiBase and remove the
DatamanClient workaround. Cover surviving publishers and instance reuse,
and reset retained input samples between test cases.

Tested: make tests (215/215), make format.
Signed-off-by: jonas <jonas.perolini@rigi.tech>
Assisted-by: Codex

* docs(uorb): correct unadvertise comments and document teardown

The unadvertise comment claimed it has no effect on single-instance
topics, which is how one publisher's teardown came to hide the others.
State what clearing the shared flag does, and document that only
PublicationMulti releases its instance on destruction.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* test(uorb): keep test cases independent of retained samples

A destroyed single-instance publication now leaves its last sample
visible, so ManualControlTest, test_prec_takeoff and mixer_module_tests
passed only in declaration order and failed under --gtest_shuffle.
Reset with an inert sample where one exists, otherwise unadvertise the
test's own publications, and drop module outputs left over from a
previous case.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: jonas <jonas.perolini@rigi.tech>

* fix(boards): drop the PublicationBase destructor from the ITCM lists

The destructor is now trivial and no longer emitted, so itcm_check.py
fails on the missing section.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

---------

Signed-off-by: jonas <jonas.perolini@rigi.tech>
Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
Co-authored-by: jonas <jonas.perolini@rigi.tech>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
2026-10-14 16:53:20 -06:00

518 lines
13 KiB
C++

/****************************************************************************
*
* Copyright (c) 2012-2016 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 "uORBDeviceNode.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
#include "SubscriptionCallback.hpp"
#ifdef CONFIG_ORB_COMMUNICATOR
#include "uORBCommunicator.hpp"
#endif /* CONFIG_ORB_COMMUNICATOR */
#if defined(__PX4_NUTTX)
#include <nuttx/mm/mm.h>
#endif
static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) :
CDev(strdup(path)), // success is checked in CDev::init
_meta(meta),
_instance(instance)
{
}
uORB::DeviceNode::~DeviceNode()
{
free(_data);
const char *devname = get_devname();
if (devname) {
#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT)
kmm_free((void *)devname);
#else
free((void *)devname);
#endif
}
}
int
uORB::DeviceNode::open(cdev::file_t *filp)
{
/* is this a publisher? */
if (filp->f_oflags == PX4_F_WRONLY) {
// _advertised is atomic, so no lock is needed around this single store
mark_as_advertised();
/* now complete the open */
return CDev::open(filp);
}
/* is this a new subscriber? */
if (filp->f_oflags == PX4_F_RDONLY) {
/* allocate subscriber data */
SubscriptionInterval *sd = new SubscriptionInterval(_meta, 0, _instance);
if (nullptr == sd) {
return -ENOMEM;
}
filp->f_priv = (void *)sd;
int ret = CDev::open(filp);
if (ret != PX4_OK) {
PX4_ERR("CDev::open failed");
delete sd;
}
return ret;
}
if (filp->f_oflags == 0) {
return CDev::open(filp);
}
/* can only be pub or sub, not both */
return -EINVAL;
}
int
uORB::DeviceNode::close(cdev::file_t *filp)
{
if (filp->f_oflags == PX4_F_RDONLY) { /* subscriber */
SubscriptionInterval *sd = filp_to_subscription(filp);
delete sd;
}
return CDev::close(filp);
}
ssize_t
uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
{
/* if the caller's buffer is the wrong size, that's an error */
if (buflen != _meta->o_size) {
return -EIO;
}
return filp_to_subscription(filp)->copy(buffer) ? _meta->o_size : 0;
}
ssize_t
uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
{
/*
* Writes are legal from interrupt context as long as the
* object has already been initialised from thread context.
*
* Writes outside interrupt context will allocate the object
* if it has not yet been allocated.
*
* Note that filp will usually be NULL.
*/
if (nullptr == _data) {
#ifdef __PX4_NUTTX
if (!up_interrupt_context()) {
#endif /* __PX4_NUTTX */
// Allocate the buffer under the node lock. It cannot be allocated from
// ATOMIC_ENTER (an interrupts-off critical section on NuttX), so the lock
// is what serializes the lazy allocation against concurrent readers.
lock();
/* re-check size */
if (nullptr == _data) {
const size_t data_size = _meta->o_size * _meta->o_queue;
uint8_t *data = (uint8_t *) px4_cache_aligned_alloc(data_size);
if (data) {
memset(data, 0, data_size);
}
_data = data;
}
unlock();
#ifdef __PX4_NUTTX
}
#endif /* __PX4_NUTTX */
/* failed or could not allocate */
if (nullptr == _data) {
return -ENOMEM;
}
}
/* If write size does not match, that is an error */
if (_meta->o_size != buflen) {
return -EIO;
}
/* Perform an atomic copy. */
ATOMIC_ENTER;
/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
unsigned generation = _generation.fetch_add(1);
memcpy(_data + (_meta->o_size * (generation % _meta->o_queue)), buffer, _meta->o_size);
// callbacks
for (auto item : _callbacks) {
item->call(generation + 1);
}
/* Mark at least one data has been published */
_data_valid = true;
ATOMIC_LEAVE;
/* notify any poll waiters */
poll_notify(POLLIN);
return _meta->o_size;
}
int
uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case ORBIOCUPDATED: {
ATOMIC_ENTER;
*(bool *)arg = filp_to_subscription(filp)->updated();
ATOMIC_LEAVE;
return PX4_OK;
}
case ORBIOCSETINTERVAL:
filp_to_subscription(filp)->set_interval_us(arg);
return PX4_OK;
case ORBIOCGADVERTISER:
*(uintptr_t *)arg = (uintptr_t)this;
return PX4_OK;
case ORBIOCGETINTERVAL:
*(unsigned *)arg = filp_to_subscription(filp)->get_interval_us();
return PX4_OK;
case ORBIOCISADVERTISED:
*(unsigned long *)arg = _advertised.load();
return PX4_OK;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
ssize_t
uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data)
{
uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
int ret;
/* check if the device handle is initialized and data is valid */
if ((devnode == nullptr) || (meta == nullptr) || (data == nullptr)) {
errno = EFAULT;
return PX4_ERROR;
}
/* check if the orb meta data matches the publication */
if (devnode->_meta->o_id != meta->o_id) {
errno = EINVAL;
return PX4_ERROR;
}
/* call the devnode write method with no file pointer */
ret = devnode->write(nullptr, (const char *)data, meta->o_size);
if (ret < 0) {
errno = -ret;
return PX4_ERROR;
}
if (ret != (int)meta->o_size) {
errno = EIO;
return PX4_ERROR;
}
#ifdef CONFIG_ORB_COMMUNICATOR
/*
* if the write is successful, send the data over the Multi-ORB link
*/
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr) {
if (ch->send_message(meta->o_name, meta->o_size, (uint8_t *)data) != 0) {
PX4_ERR("Error Sending [%s] topic data over comm_channel", meta->o_name);
return PX4_ERROR;
}
}
#endif /* CONFIG_ORB_COMMUNICATOR */
return PX4_OK;
}
int uORB::DeviceNode::unadvertise(orb_advert_t handle)
{
if (handle == nullptr) {
return -EINVAL;
}
uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
/*
* Frees a multi-instance node for the next advertiser and hides it from all subscribers.
* Publishers of a single-instance topic share this flag, so unadvertising hides all of them.
* The DeviceNode is never freed; later publishers reuse it.
*/
devnode->_advertised.store(false);
return PX4_OK;
}
#ifdef CONFIG_ORB_COMMUNICATOR
int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)
{
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr && meta != nullptr) {
return ch->topic_advertised(meta->o_name);
}
return -1;
}
#endif /* CONFIG_ORB_COMMUNICATOR */
px4_pollevent_t
uORB::DeviceNode::poll_state(cdev::file_t *filp)
{
// If the topic appears updated to the subscriber, say so.
return filp_to_subscription(filp)->updated() ? POLLIN : 0;
}
void
uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events)
{
// If the topic looks updated to the subscriber, go ahead and notify them.
if (filp_to_subscription((cdev::file_t *)fds->priv)->updated()) {
CDev::poll_notify_one(fds, events);
}
}
bool
uORB::DeviceNode::print_statistics(int max_topic_length)
{
if (!_advertised.load()) {
return false;
}
lock();
const uint8_t instance = get_instance();
const int8_t sub_count = subscriber_count();
unlock();
PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
get_meta()->o_queue, get_meta()->o_size, get_devname());
return true;
}
void uORB::DeviceNode::add_internal_subscriber()
{
lock();
_subscriber_count++;
#ifdef CONFIG_ORB_COMMUNICATOR
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr && _subscriber_count > 0) {
unlock(); //make sure we cannot deadlock if add_subscription calls back into DeviceNode
ch->add_subscription(_meta->o_name, 1);
} else
#endif /* CONFIG_ORB_COMMUNICATOR */
{
unlock();
}
}
void uORB::DeviceNode::remove_internal_subscriber()
{
lock();
_subscriber_count--;
#ifdef CONFIG_ORB_COMMUNICATOR
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr && _subscriber_count == 0) {
unlock(); //make sure we cannot deadlock if remove_subscription calls back into DeviceNode
ch->remove_subscription(_meta->o_name);
} else
#endif /* CONFIG_ORB_COMMUNICATOR */
{
unlock();
}
}
#ifdef CONFIG_ORB_COMMUNICATOR
int16_t uORB::DeviceNode::process_add_subscription()
{
// If there is already data in the node, send this out to the remote entity to
// initialize its end.
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch == nullptr) {
return PX4_OK;
}
// Snapshot the most recent sample under the node lock, then send it outside the
// critical section. Reading _data / _data_valid and the buffer contents while
// sending would race with a concurrent write(), which memcpy's into _data and
// sets _data_valid under ATOMIC_ENTER. send_message() must also not run under
// ATOMIC_ENTER: it can be slow and may call back into DeviceNode. This mirrors the
// snapshot-under-lock done in copy().
uint8_t *buffer = (uint8_t *)px4_cache_aligned_alloc(_meta->o_size);
if (buffer == nullptr) {
return PX4_ERROR;
}
bool have_data = false;
ATOMIC_ENTER;
if (_data != nullptr && _data_valid) { // _data will not be null if there is a publisher.
memcpy(buffer, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue)), _meta->o_size);
have_data = true;
}
ATOMIC_LEAVE;
if (have_data) {
ch->send_message(_meta->o_name, _meta->o_size, buffer);
}
free(buffer);
return PX4_OK;
}
int16_t uORB::DeviceNode::process_remove_subscription()
{
return PX4_OK;
}
int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data)
{
int16_t ret = -1;
if (length != (int32_t)(_meta->o_size)) {
PX4_ERR("Received '%s' with DataLength[%d] != ExpectedLen[%d]", _meta->o_name, (int)length, (int)_meta->o_size);
return PX4_ERROR;
}
/* call the devnode write method with no file pointer */
ret = write(nullptr, (const char *)data, _meta->o_size);
if (ret < 0) {
return PX4_ERROR;
}
if (ret != (int)_meta->o_size) {
errno = EIO;
return PX4_ERROR;
}
return PX4_OK;
}
#endif /* CONFIG_ORB_COMMUNICATOR */
unsigned uORB::DeviceNode::get_initial_generation()
{
ATOMIC_ENTER;
// If there any previous publications allow the subscriber to read them
unsigned generation = _generation.load() - (_data_valid ? 1 : 0);
ATOMIC_LEAVE;
return generation;
}
bool
uORB::DeviceNode::register_callback(uORB::SubscriptionCallback *callback_sub)
{
if (callback_sub != nullptr) {
ATOMIC_ENTER;
// prevent duplicate registrations
for (auto existing_callbacks : _callbacks) {
if (callback_sub == existing_callbacks) {
ATOMIC_LEAVE;
return true;
}
}
_callbacks.add(callback_sub);
ATOMIC_LEAVE;
return true;
}
return false;
}
void
uORB::DeviceNode::unregister_callback(uORB::SubscriptionCallback *callback_sub)
{
ATOMIC_ENTER;
_callbacks.remove(callback_sub);
ATOMIC_LEAVE;
}