mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-30 06:40:35 +08:00
uORB: split uORBDevices into uORBDeviceNode and uORBDeviceMaster
This commit is contained in:
Daniel Agar
@@ -0,0 +1,805 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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"
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#define FILE_FLAGS(filp) filp->f_oflags
|
||||
#define FILE_PRIV(filp) filp->f_priv
|
||||
#else
|
||||
#define FILE_FLAGS(filp) filp->flags
|
||||
#define FILE_PRIV(filp) filp->priv
|
||||
#endif
|
||||
|
||||
#include "uORBDeviceNode.hpp"
|
||||
#include "uORBUtils.hpp"
|
||||
#include "uORBManager.hpp"
|
||||
|
||||
#ifdef ORB_COMMUNICATOR
|
||||
#include "uORBCommunicator.hpp"
|
||||
#endif /* ORB_COMMUNICATOR */
|
||||
|
||||
using namespace device;
|
||||
|
||||
uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp)
|
||||
{
|
||||
#ifndef __PX4_NUTTX
|
||||
|
||||
if (!filp) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
#endif
|
||||
return (SubscriberData *)(FILE_PRIV(filp));
|
||||
}
|
||||
|
||||
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path,
|
||||
int priority, unsigned int queue_size) :
|
||||
CDev(name, path),
|
||||
_meta(meta),
|
||||
_priority((uint8_t)priority),
|
||||
_queue_size(queue_size)
|
||||
{
|
||||
}
|
||||
|
||||
uORB::DeviceNode::~DeviceNode()
|
||||
{
|
||||
if (_data != nullptr) {
|
||||
delete[] _data;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::open(device::file_t *filp)
|
||||
{
|
||||
int ret;
|
||||
|
||||
/* is this a publisher? */
|
||||
if (FILE_FLAGS(filp) == PX4_F_WRONLY) {
|
||||
|
||||
/* become the publisher if we can */
|
||||
lock();
|
||||
|
||||
if (_publisher == 0) {
|
||||
_publisher = px4_getpid();
|
||||
ret = PX4_OK;
|
||||
|
||||
} else {
|
||||
ret = -EBUSY;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
/* now complete the open */
|
||||
if (ret == PX4_OK) {
|
||||
ret = CDev::open(filp);
|
||||
|
||||
/* open failed - not the publisher anymore */
|
||||
if (ret != PX4_OK) {
|
||||
_publisher = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* is this a new subscriber? */
|
||||
if (FILE_FLAGS(filp) == PX4_F_RDONLY) {
|
||||
|
||||
/* allocate subscriber data */
|
||||
SubscriberData *sd = new SubscriberData{};
|
||||
|
||||
if (nullptr == sd) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* If there were any previous publications, allow the subscriber to read them */
|
||||
sd->generation = _generation - (_queue_size < _generation ? _queue_size : _generation);
|
||||
|
||||
/* set priority */
|
||||
sd->set_priority(_priority);
|
||||
|
||||
FILE_PRIV(filp) = (void *)sd;
|
||||
|
||||
ret = CDev::open(filp);
|
||||
|
||||
add_internal_subscriber();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
PX4_ERR("CDev::open failed");
|
||||
delete sd;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (FILE_FLAGS(filp) == 0) {
|
||||
return CDev::open(filp);
|
||||
}
|
||||
|
||||
/* can only be pub or sub, not both */
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::close(device::file_t *filp)
|
||||
{
|
||||
/* is this the publisher closing? */
|
||||
if (px4_getpid() == _publisher) {
|
||||
_publisher = 0;
|
||||
|
||||
} else {
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
if (sd != nullptr) {
|
||||
if (sd->update_interval) {
|
||||
hrt_cancel(&sd->update_interval->update_call);
|
||||
}
|
||||
|
||||
remove_internal_subscriber();
|
||||
|
||||
delete sd;
|
||||
sd = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
return CDev::close(filp);
|
||||
}
|
||||
|
||||
ssize_t
|
||||
uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
SubscriberData *sd = (SubscriberData *)filp_to_sd(filp);
|
||||
|
||||
/* if the object has not been written yet, return zero */
|
||||
if (_data == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* if the caller's buffer is the wrong size, that's an error */
|
||||
if (buflen != _meta->o_size) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
/*
|
||||
* Perform an atomic copy & state update
|
||||
*/
|
||||
ATOMIC_ENTER;
|
||||
|
||||
if (_generation > sd->generation + _queue_size) {
|
||||
/* Reader is too far behind: some messages are lost */
|
||||
_lost_messages += _generation - (sd->generation + _queue_size);
|
||||
sd->generation = _generation - _queue_size;
|
||||
}
|
||||
|
||||
if (_generation == sd->generation && sd->generation > 0) {
|
||||
/* The subscriber already read the latest message, but nothing new was published yet.
|
||||
* Return the previous message
|
||||
*/
|
||||
--sd->generation;
|
||||
}
|
||||
|
||||
/* if the caller doesn't want the data, don't give it to them */
|
||||
if (nullptr != buffer) {
|
||||
memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size);
|
||||
}
|
||||
|
||||
if (sd->generation < _generation) {
|
||||
++sd->generation;
|
||||
}
|
||||
|
||||
/* set priority */
|
||||
sd->set_priority(_priority);
|
||||
|
||||
/*
|
||||
* Clear the flag that indicates that an update has been reported, as
|
||||
* we have just collected it.
|
||||
*/
|
||||
sd->set_update_reported(false);
|
||||
|
||||
ATOMIC_LEAVE;
|
||||
|
||||
return _meta->o_size;
|
||||
}
|
||||
|
||||
ssize_t
|
||||
uORB::DeviceNode::write(device::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()) {
|
||||
|
||||
lock();
|
||||
|
||||
/* re-check size */
|
||||
if (nullptr == _data) {
|
||||
_data = new uint8_t[_meta->o_size * _queue_size];
|
||||
}
|
||||
|
||||
unlock();
|
||||
}
|
||||
|
||||
#else
|
||||
lock();
|
||||
|
||||
/* re-check size */
|
||||
if (nullptr == _data) {
|
||||
_data = new uint8_t[_meta->o_size * _queue_size];
|
||||
}
|
||||
|
||||
unlock();
|
||||
#endif
|
||||
|
||||
/* 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;
|
||||
memcpy(_data + (_meta->o_size * (_generation % _queue_size)), buffer, _meta->o_size);
|
||||
|
||||
/* update the timestamp and generation count */
|
||||
_last_update = hrt_absolute_time();
|
||||
/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
|
||||
_generation++;
|
||||
|
||||
_published = true;
|
||||
|
||||
ATOMIC_LEAVE;
|
||||
|
||||
/* notify any poll waiters */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
return _meta->o_size;
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
switch (cmd) {
|
||||
case ORBIOCLASTUPDATE: {
|
||||
ATOMIC_ENTER;
|
||||
*(hrt_abstime *)arg = _last_update;
|
||||
ATOMIC_LEAVE;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
case ORBIOCUPDATED:
|
||||
#ifndef __PX4_NUTTX
|
||||
lock();
|
||||
#endif
|
||||
*(bool *)arg = appears_updated(sd);
|
||||
#ifndef __PX4_NUTTX
|
||||
unlock();
|
||||
#endif
|
||||
return PX4_OK;
|
||||
|
||||
case ORBIOCSETINTERVAL: {
|
||||
int ret = PX4_OK;
|
||||
lock();
|
||||
|
||||
if (arg == 0) {
|
||||
if (sd->update_interval) {
|
||||
delete (sd->update_interval);
|
||||
sd->update_interval = nullptr;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (sd->update_interval) {
|
||||
sd->update_interval->interval = arg;
|
||||
#ifndef __PX4_NUTTX
|
||||
sd->update_interval->last_update = hrt_absolute_time();
|
||||
#endif
|
||||
|
||||
} else {
|
||||
sd->update_interval = new UpdateIntervalData();
|
||||
|
||||
if (sd->update_interval) {
|
||||
memset(&sd->update_interval->update_call, 0, sizeof(hrt_call));
|
||||
sd->update_interval->interval = arg;
|
||||
#ifndef __PX4_NUTTX
|
||||
sd->update_interval->last_update = hrt_absolute_time();
|
||||
#endif
|
||||
|
||||
} else {
|
||||
ret = -ENOMEM;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
unlock();
|
||||
return ret;
|
||||
}
|
||||
|
||||
case ORBIOCGADVERTISER:
|
||||
*(uintptr_t *)arg = (uintptr_t)this;
|
||||
return PX4_OK;
|
||||
|
||||
case ORBIOCGPRIORITY:
|
||||
*(int *)arg = sd->priority();
|
||||
return PX4_OK;
|
||||
|
||||
case ORBIOCSETQUEUESIZE:
|
||||
//no need for locking here, since this is used only during the advertisement call,
|
||||
//and only one advertiser is allowed to open the DeviceNode at the same time.
|
||||
return update_queue_size(arg);
|
||||
|
||||
case ORBIOCGETINTERVAL:
|
||||
if (sd->update_interval) {
|
||||
*(unsigned *)arg = sd->update_interval->interval;
|
||||
|
||||
} else {
|
||||
*(unsigned *)arg = 0;
|
||||
}
|
||||
|
||||
return OK;
|
||||
|
||||
case ORBIOCISPUBLISHED:
|
||||
*(unsigned long *)arg = _published;
|
||||
|
||||
return 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 */
|
||||
if ((devnode == nullptr) || (meta == nullptr)) {
|
||||
errno = EFAULT;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
/* check if the orb meta data matches the publication */
|
||||
if (devnode->_meta != meta) {
|
||||
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 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 /* ORB_COMMUNICATOR */
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int uORB::DeviceNode::unadvertise(orb_advert_t handle)
|
||||
{
|
||||
if (handle == nullptr) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
|
||||
|
||||
/*
|
||||
* We are cheating a bit here. First, with the current implementation, we can only
|
||||
* have multiple publishers for instance 0. In this case the caller will have
|
||||
* instance=nullptr and _published has no effect at all. Thus no unadvertise is
|
||||
* necessary.
|
||||
* In case of multiple instances, we have at most 1 publisher per instance and
|
||||
* we can signal an instance as 'free' by setting _published to false.
|
||||
* We never really free the DeviceNode, for this we would need reference counting
|
||||
* of subscribers and publishers. But we also do not have a leak since future
|
||||
* publishers reuse the same DeviceNode object.
|
||||
*/
|
||||
devnode->_published = false;
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
#ifdef ORB_COMMUNICATOR
|
||||
int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta, int priority)
|
||||
{
|
||||
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
|
||||
|
||||
if (ch != nullptr && meta != nullptr) {
|
||||
return ch->topic_advertised(meta->o_name);
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
//TODO: Check if we need this since we only unadvertise when things all shutdown and it doesn't actually remove the device
|
||||
int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta, int priority)
|
||||
{
|
||||
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
|
||||
if (ch != nullptr && meta != nullptr) {
|
||||
return ch->topic_unadvertised(meta->o_name);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
*/
|
||||
#endif /* ORB_COMMUNICATOR */
|
||||
|
||||
pollevent_t
|
||||
uORB::DeviceNode::poll_state(device::file_t *filp)
|
||||
{
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
/*
|
||||
* If the topic appears updated to the subscriber, say so.
|
||||
*/
|
||||
if (appears_updated(sd)) {
|
||||
return POLLIN;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events)
|
||||
{
|
||||
SubscriberData *sd = filp_to_sd((device::file_t *)fds->priv);
|
||||
|
||||
/*
|
||||
* If the topic looks updated to the subscriber, go ahead and notify them.
|
||||
*/
|
||||
if (appears_updated(sd)) {
|
||||
CDev::poll_notify_one(fds, events);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
bool
|
||||
uORB::DeviceNode::appears_updated(SubscriberData *sd)
|
||||
{
|
||||
/* assume it doesn't look updated */
|
||||
bool ret = false;
|
||||
|
||||
/* avoid racing between interrupt and non-interrupt context calls */
|
||||
irqstate_t state = px4_enter_critical_section();
|
||||
|
||||
/* check if this topic has been published yet, if not bail out */
|
||||
if (_data == nullptr) {
|
||||
ret = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
/*
|
||||
* If the subscriber's generation count matches the update generation
|
||||
* count, there has been no update from their perspective; if they
|
||||
* don't match then we might have a visible update.
|
||||
*/
|
||||
while (sd->generation != _generation) {
|
||||
|
||||
/*
|
||||
* Handle non-rate-limited subscribers.
|
||||
*/
|
||||
if (sd->update_interval == nullptr) {
|
||||
ret = true;
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* If we have previously told the subscriber that there is data,
|
||||
* and they have not yet collected it, continue to tell them
|
||||
* that there has been an update. This mimics the non-rate-limited
|
||||
* behaviour where checking / polling continues to report an update
|
||||
* until the topic is read.
|
||||
*/
|
||||
if (sd->update_reported()) {
|
||||
ret = true;
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* If the interval timer is still running, the topic should not
|
||||
* appear updated, even though at this point we know that it has.
|
||||
* We have previously been through here, so the subscriber
|
||||
* must have collected the update we reported, otherwise
|
||||
* update_reported would still be true.
|
||||
*/
|
||||
if (!hrt_called(&sd->update_interval->update_call)) {
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* Make sure that we don't consider the topic to be updated again
|
||||
* until the interval has passed once more by restarting the interval
|
||||
* timer and thereby re-scheduling a poll notification at that time.
|
||||
*/
|
||||
hrt_call_after(&sd->update_interval->update_call,
|
||||
sd->update_interval->interval,
|
||||
&uORB::DeviceNode::update_deferred_trampoline,
|
||||
(void *)this);
|
||||
|
||||
/*
|
||||
* Remember that we have told the subscriber that there is data.
|
||||
*/
|
||||
sd->set_update_reported(true);
|
||||
ret = true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
px4_leave_critical_section(state);
|
||||
|
||||
/* consider it updated */
|
||||
return ret;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
bool
|
||||
uORB::DeviceNode::appears_updated(SubscriberData *sd)
|
||||
{
|
||||
|
||||
/* block if in simulation mode */
|
||||
while (px4_sim_delay_enabled()) {
|
||||
usleep(100);
|
||||
}
|
||||
|
||||
/* assume it doesn't look updated */
|
||||
bool ret = false;
|
||||
|
||||
/* check if this topic has been published yet, if not bail out */
|
||||
if (_data == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* If the subscriber's generation count matches the update generation
|
||||
* count, there has been no update from their perspective; if they
|
||||
* don't match then we might have a visible update.
|
||||
*/
|
||||
while (sd->generation != _generation) {
|
||||
|
||||
/*
|
||||
* Handle non-rate-limited subscribers.
|
||||
*/
|
||||
if (sd->update_interval == nullptr) {
|
||||
ret = true;
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* If we have previously told the subscriber that there is data,
|
||||
* and they have not yet collected it, continue to tell them
|
||||
* that there has been an update. This mimics the non-rate-limited
|
||||
* behaviour where checking / polling continues to report an update
|
||||
* until the topic is read.
|
||||
*/
|
||||
if (sd->update_reported()) {
|
||||
ret = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// If we have not yet reached the deadline, then assume that we can ignore any
|
||||
// newly received data.
|
||||
if (sd->update_interval->last_update + sd->update_interval->interval > hrt_absolute_time()) {
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* Remember that we have told the subscriber that there is data.
|
||||
*/
|
||||
sd->set_update_reported(true);
|
||||
sd->update_interval->last_update = hrt_absolute_time();
|
||||
ret = true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
#endif /* ifdef __PX4_NUTTX */
|
||||
|
||||
void
|
||||
uORB::DeviceNode::update_deferred()
|
||||
{
|
||||
/*
|
||||
* Instigate a poll notification; any subscribers whose intervals have
|
||||
* expired will be woken.
|
||||
*/
|
||||
poll_notify(POLLIN);
|
||||
}
|
||||
|
||||
void
|
||||
uORB::DeviceNode::update_deferred_trampoline(void *arg)
|
||||
{
|
||||
uORB::DeviceNode *node = (uORB::DeviceNode *)arg;
|
||||
|
||||
node->update_deferred();
|
||||
}
|
||||
|
||||
bool
|
||||
uORB::DeviceNode::print_statistics(bool reset)
|
||||
{
|
||||
if (!_lost_messages) {
|
||||
return false;
|
||||
}
|
||||
|
||||
lock();
|
||||
//This can be wrong: if a reader never reads, _lost_messages will not be increased either
|
||||
uint32_t lost_messages = _lost_messages;
|
||||
|
||||
if (reset) {
|
||||
_lost_messages = 0;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
PX4_INFO("%s: %i", _meta->o_name, lost_messages);
|
||||
return true;
|
||||
}
|
||||
|
||||
void uORB::DeviceNode::add_internal_subscriber()
|
||||
{
|
||||
lock();
|
||||
_subscriber_count++;
|
||||
|
||||
#ifdef 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 /* ORB_COMMUNICATOR */
|
||||
|
||||
{
|
||||
unlock();
|
||||
}
|
||||
}
|
||||
|
||||
void uORB::DeviceNode::remove_internal_subscriber()
|
||||
{
|
||||
lock();
|
||||
_subscriber_count--;
|
||||
|
||||
#ifdef 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 /* ORB_COMMUNICATOR */
|
||||
{
|
||||
unlock();
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef ORB_COMMUNICATOR
|
||||
int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)
|
||||
{
|
||||
// if there is already data in the node, send this out to
|
||||
// the remote entity.
|
||||
// send the data to the remote entity.
|
||||
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
|
||||
|
||||
if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
|
||||
ch->send_message(_meta->o_name, _meta->o_size, _data);
|
||||
}
|
||||
|
||||
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 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 /* ORB_COMMUNICATOR */
|
||||
|
||||
int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
|
||||
{
|
||||
if (_queue_size == queue_size) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
//queue size is limited to 255 for the single reason that we use uint8 to store it
|
||||
if (_data || _queue_size > queue_size || queue_size > 255) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_queue_size = queue_size;
|
||||
return PX4_OK;
|
||||
}
|
||||
Reference in New Issue
Block a user