forked from Archive/PX4-Autopilot
uORB: Major refactoring
uORB was refactored in order to support the MuORB changes required for QURT. Those changes wil be added in a subsequent commit. The files are split out by posix and nuttx so the changes are visible. When this has been tested, the files can be re-merged and re-tested. Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
parent
e24405d374
commit
e33a164ddb
File diff suppressed because it is too large
Load Diff
|
|
@ -40,12 +40,19 @@ MODULE_COMMAND = uorb
|
|||
MODULE_STACKSIZE = 2048
|
||||
|
||||
ifeq ($(PX4_TARGET_OS),nuttx)
|
||||
SRCS = uORB.cpp
|
||||
SRCS = uORBDevices_nuttx.cpp \
|
||||
uORBManager_nuttx.cpp
|
||||
|
||||
else
|
||||
SRCS = MuORB.cpp
|
||||
SRCS = uORBDevices_posix.cpp \
|
||||
uORBTest_UnitTest.cpp \
|
||||
uORBManager_posix.cpp
|
||||
endif
|
||||
SRCS += objects_common.cpp \
|
||||
Publication.cpp \
|
||||
Subscription.cpp
|
||||
Subscription.cpp \
|
||||
uORBUtils.cpp \
|
||||
uORB.cpp \
|
||||
uORBMain.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,65 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef _uORBCommon_hpp_
|
||||
#define _uORBCommon_hpp_
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_orb_dev.h>
|
||||
#include <systemlib/err.h>
|
||||
#include "uORB.h"
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
static const unsigned orb_maxpath = 64;
|
||||
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
/* ERROR is not defined for c++ */
|
||||
const int ERROR = -1;
|
||||
|
||||
enum Flavor {
|
||||
PUBSUB,
|
||||
PARAM
|
||||
};
|
||||
|
||||
struct orb_advertdata {
|
||||
const struct orb_metadata *meta;
|
||||
int *instance;
|
||||
int priority;
|
||||
};
|
||||
}
|
||||
#endif // _uORBCommon_hpp_
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#include "uORBDevices_nuttx.hpp"
|
||||
#elif defined (__PX4_POSIX)
|
||||
#include "uORBDevices_posix.hpp"
|
||||
#endif
|
||||
|
|
@ -0,0 +1,531 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 <stdint.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include "uORBDevices_nuttx.hpp"
|
||||
#include "uORBUtils.hpp"
|
||||
#include <stdlib.h>
|
||||
|
||||
uORB::DeviceNode::DeviceNode
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
const char *name,
|
||||
const char *path,
|
||||
int priority
|
||||
) :
|
||||
CDev(name, path),
|
||||
_meta(meta),
|
||||
_data(nullptr),
|
||||
_last_update(0),
|
||||
_generation(0),
|
||||
_publisher(0),
|
||||
_priority(priority)
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
}
|
||||
|
||||
uORB::DeviceNode::~DeviceNode()
|
||||
{
|
||||
if (_data != nullptr)
|
||||
delete[] _data;
|
||||
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::open(struct file *filp)
|
||||
{
|
||||
int ret;
|
||||
|
||||
/* is this a publisher? */
|
||||
if (filp->f_oflags == O_WRONLY) {
|
||||
|
||||
/* become the publisher if we can */
|
||||
lock();
|
||||
|
||||
if (_publisher == 0) {
|
||||
_publisher = getpid();
|
||||
ret = OK;
|
||||
|
||||
} else {
|
||||
ret = -EBUSY;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
/* now complete the open */
|
||||
if (ret == OK) {
|
||||
ret = CDev::open(filp);
|
||||
|
||||
/* open failed - not the publisher anymore */
|
||||
if (ret != OK)
|
||||
_publisher = 0;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* is this a new subscriber? */
|
||||
if (filp->f_oflags == O_RDONLY) {
|
||||
|
||||
/* allocate subscriber data */
|
||||
SubscriberData *sd = new SubscriberData;
|
||||
|
||||
if (nullptr == sd)
|
||||
return -ENOMEM;
|
||||
|
||||
memset(sd, 0, sizeof(*sd));
|
||||
|
||||
/* default to no pending update */
|
||||
sd->generation = _generation;
|
||||
|
||||
/* set priority */
|
||||
sd->priority = _priority;
|
||||
|
||||
filp->f_priv = (void *)sd;
|
||||
|
||||
ret = CDev::open(filp);
|
||||
|
||||
if (ret != OK)
|
||||
delete sd;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* can only be pub or sub, not both */
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::close(struct file *filp)
|
||||
{
|
||||
/* is this the publisher closing? */
|
||||
if (getpid() == _publisher) {
|
||||
_publisher = 0;
|
||||
|
||||
} else {
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
if (sd != nullptr) {
|
||||
hrt_cancel(&sd->update_call);
|
||||
delete sd;
|
||||
}
|
||||
}
|
||||
|
||||
return CDev::close(filp);
|
||||
}
|
||||
|
||||
ssize_t
|
||||
uORB::DeviceNode::read(struct file *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
|
||||
*/
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
/* if the caller doesn't want the data, don't give it to them */
|
||||
if (nullptr != buffer)
|
||||
memcpy(buffer, _data, _meta->o_size);
|
||||
|
||||
/* track the last generation that the file has seen */
|
||||
sd->generation = _generation;
|
||||
|
||||
/* set priority */
|
||||
sd->priority = _priority;
|
||||
|
||||
/*
|
||||
* Clear the flag that indicates that an update has been reported, as
|
||||
* we have just collected it.
|
||||
*/
|
||||
sd->update_reported = false;
|
||||
|
||||
irqrestore(flags);
|
||||
|
||||
return _meta->o_size;
|
||||
}
|
||||
|
||||
ssize_t
|
||||
uORB::DeviceNode::write(struct file *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) {
|
||||
if (!up_interrupt_context()) {
|
||||
|
||||
lock();
|
||||
|
||||
/* re-check size */
|
||||
if (nullptr == _data)
|
||||
_data = new uint8_t[_meta->o_size];
|
||||
|
||||
unlock();
|
||||
}
|
||||
|
||||
/* 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. */
|
||||
irqstate_t flags = irqsave();
|
||||
memcpy(_data, buffer, _meta->o_size);
|
||||
irqrestore(flags);
|
||||
|
||||
/* update the timestamp and generation count */
|
||||
_last_update = hrt_absolute_time();
|
||||
_generation++;
|
||||
|
||||
/* notify any poll waiters */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
return _meta->o_size;
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
switch (cmd) {
|
||||
case ORBIOCLASTUPDATE:
|
||||
*(hrt_abstime *)arg = _last_update;
|
||||
return OK;
|
||||
|
||||
case ORBIOCUPDATED:
|
||||
*(bool *)arg = appears_updated(sd);
|
||||
return OK;
|
||||
|
||||
case ORBIOCSETINTERVAL:
|
||||
sd->update_interval = arg;
|
||||
return OK;
|
||||
|
||||
case ORBIOCGADVERTISER:
|
||||
*(uintptr_t *)arg = (uintptr_t)this;
|
||||
return OK;
|
||||
|
||||
case ORBIOCGPRIORITY:
|
||||
*(int *)arg = sd->priority;
|
||||
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 *DeviceNode = (uORB::DeviceNode *)handle;
|
||||
int ret;
|
||||
|
||||
/* this is a bit risky, since we are trusting the handle in order to deref it */
|
||||
if (DeviceNode->_meta != meta) {
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* call the DeviceNode write method with no file pointer */
|
||||
ret = DeviceNode->write(nullptr, (const char *)data, meta->o_size);
|
||||
|
||||
if (ret < 0)
|
||||
return ERROR;
|
||||
|
||||
if (ret != (int)meta->o_size) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
pollevent_t
|
||||
uORB::DeviceNode::poll_state(struct file *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(struct pollfd *fds, pollevent_t events)
|
||||
{
|
||||
SubscriberData *sd = filp_to_sd((struct file *)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);
|
||||
}
|
||||
|
||||
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 = irqsave();
|
||||
|
||||
/* 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 == 0) {
|
||||
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_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_call,
|
||||
sd->update_interval,
|
||||
&uORB::DeviceNode::update_deferred_trampoline,
|
||||
(void *)this);
|
||||
|
||||
/*
|
||||
* Remember that we have told the subscriber that there is data.
|
||||
*/
|
||||
sd->update_reported = true;
|
||||
ret = true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
irqrestore(state);
|
||||
|
||||
/* consider it updated */
|
||||
return ret;
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
uORB::DeviceMaster::DeviceMaster(Flavor f) :
|
||||
CDev((f == PUBSUB) ? "obj_master" : "param_master",
|
||||
(f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH),
|
||||
_flavor(f)
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
||||
}
|
||||
|
||||
uORB::DeviceMaster::~DeviceMaster()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret;
|
||||
|
||||
switch (cmd) {
|
||||
case ORBIOCADVERTISE: {
|
||||
const struct orb_advertdata *adv = (const struct orb_advertdata *)arg;
|
||||
const struct orb_metadata *meta = adv->meta;
|
||||
const char *objname;
|
||||
const char *devpath;
|
||||
char nodepath[orb_maxpath];
|
||||
uORB::DeviceNode *node;
|
||||
|
||||
/* set instance to zero - we could allow selective multi-pubs later based on value */
|
||||
if (adv->instance != nullptr) {
|
||||
*(adv->instance) = 0;
|
||||
}
|
||||
|
||||
/* construct a path to the node - this also checks the node name */
|
||||
ret = uORB::Utils::node_mkpath( nodepath, _flavor, meta, adv->instance);
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ensure that only one advertiser runs through this critical section */
|
||||
lock();
|
||||
|
||||
ret = ERROR;
|
||||
|
||||
/* try for topic groups */
|
||||
const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
|
||||
unsigned group_tries = 0;
|
||||
do {
|
||||
/* if path is modifyable change try index */
|
||||
if (adv->instance != nullptr) {
|
||||
/* replace the number at the end of the string */
|
||||
nodepath[strlen(nodepath) - 1] = '0' + group_tries;
|
||||
*(adv->instance) = group_tries;
|
||||
}
|
||||
|
||||
/* driver wants a permanent copy of the node name, so make one here */
|
||||
objname = strdup(meta->o_name);
|
||||
|
||||
if (objname == nullptr) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* driver wants a permanent copy of the path, so make one here */
|
||||
devpath = strdup(nodepath);
|
||||
|
||||
if (devpath == nullptr) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* construct the new node */
|
||||
node = new uORB::DeviceNode(meta, objname, devpath, adv->priority);
|
||||
|
||||
/* if we didn't get a device, that's bad */
|
||||
if (node == nullptr) {
|
||||
unlock();
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* initialise the node - this may fail if e.g. a node with this name already exists */
|
||||
ret = node->init();
|
||||
|
||||
/* if init failed, discard the node and its name */
|
||||
if (ret != OK) {
|
||||
delete node;
|
||||
free((void *)objname);
|
||||
free((void *)devpath);
|
||||
}
|
||||
|
||||
group_tries++;
|
||||
|
||||
} while (ret != OK && (group_tries < max_group_tries));
|
||||
|
||||
if (group_tries > max_group_tries) {
|
||||
ret = -ENOMEM;
|
||||
}
|
||||
|
||||
/* the file handle for the driver has been created, unlock */
|
||||
unlock();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,190 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef _uORBDevices_nuttx_hpp_
|
||||
#define _uORBDevices_nuttx_hpp_
|
||||
|
||||
#include <stdint.h>
|
||||
#include "uORBCommon.hpp"
|
||||
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
class DeviceNode;
|
||||
class DeviceMaster;
|
||||
}
|
||||
|
||||
/**
|
||||
* Per-object device instance.
|
||||
*/
|
||||
class uORB::DeviceNode : public device::CDev
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
DeviceNode
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
const char *name,
|
||||
const char *path,
|
||||
int priority
|
||||
);
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
~DeviceNode();
|
||||
|
||||
/**
|
||||
* Method to create a subscriber instance and return the struct
|
||||
* pointing to the subscriber as a file pointer.
|
||||
*/
|
||||
virtual int open(struct file *filp);
|
||||
|
||||
/**
|
||||
* Method to close a subscriber for this topic.
|
||||
*/
|
||||
virtual int close(struct file *filp);
|
||||
|
||||
/**
|
||||
* reads data from a subscriber node to the buffer provided.
|
||||
* @param filp
|
||||
* The subscriber from which the data needs to be read from.
|
||||
* @param buffer
|
||||
* The buffer into which the data is read into.
|
||||
* @param buflen
|
||||
* the length of the buffer
|
||||
* @return
|
||||
* ssize_t the number of bytes read.
|
||||
*/
|
||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
|
||||
/**
|
||||
* writes the published data to the internal buffer to be read by
|
||||
* subscribers later.
|
||||
* @param filp
|
||||
* the subscriber; this is not used.
|
||||
* @param buffer
|
||||
* The buffer for the input data
|
||||
* @param buflen
|
||||
* the length of the buffer.
|
||||
* @return ssize_t
|
||||
* The number of bytes that are written
|
||||
*/
|
||||
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen);
|
||||
|
||||
/**
|
||||
* IOCTL control for the subscriber.
|
||||
*/
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Method to publish a data to this node.
|
||||
*/
|
||||
static ssize_t publish
|
||||
(
|
||||
const orb_metadata *meta,
|
||||
orb_advert_t handle,
|
||||
const void *data
|
||||
);
|
||||
|
||||
protected:
|
||||
virtual pollevent_t poll_state(struct file *filp);
|
||||
virtual void poll_notify_one(struct pollfd *fds, pollevent_t events);
|
||||
|
||||
private:
|
||||
struct SubscriberData {
|
||||
unsigned generation; /**< last generation the subscriber has seen */
|
||||
unsigned update_interval; /**< if nonzero minimum interval between updates */
|
||||
struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */
|
||||
void *poll_priv; /**< saved copy of fds->f_priv while poll is active */
|
||||
bool update_reported; /**< true if we have reported the update via poll/check */
|
||||
int priority; /**< priority of publisher */
|
||||
};
|
||||
|
||||
const struct orb_metadata *_meta; /**< object metadata information */
|
||||
uint8_t *_data; /**< allocated object buffer */
|
||||
hrt_abstime _last_update; /**< time the object was last updated */
|
||||
volatile unsigned _generation; /**< object generation count */
|
||||
pid_t _publisher; /**< if nonzero, current publisher */
|
||||
const int _priority; /**< priority of topic */
|
||||
|
||||
private: // private class methods.
|
||||
|
||||
SubscriberData *filp_to_sd(struct file *filp) {
|
||||
SubscriberData *sd = (SubscriberData *)(filp->f_priv);
|
||||
return sd;
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform a deferred update for a rate-limited subscriber.
|
||||
*/
|
||||
void update_deferred();
|
||||
|
||||
/**
|
||||
* Bridge from hrt_call to update_deferred
|
||||
*
|
||||
* void *arg ORBDevNode pointer for which the deferred update is performed.
|
||||
*/
|
||||
static void update_deferred_trampoline(void *arg);
|
||||
|
||||
/**
|
||||
* Check whether a topic appears updated to a subscriber.
|
||||
*
|
||||
* @param sd The subscriber for whom to check.
|
||||
* @return True if the topic should appear updated to the subscriber
|
||||
*/
|
||||
bool appears_updated(SubscriberData *sd);
|
||||
};
|
||||
|
||||
/**
|
||||
* Master control device for ObjDev.
|
||||
*
|
||||
* Used primarily to create new objects via the ORBIOCCREATE
|
||||
* ioctl.
|
||||
*/
|
||||
class uORB::DeviceMaster : public device::CDev
|
||||
{
|
||||
public:
|
||||
DeviceMaster(Flavor f);
|
||||
virtual ~DeviceMaster();
|
||||
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
private:
|
||||
Flavor _flavor;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,551 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 <stdint.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "uORBDevices_posix.hpp"
|
||||
#include "uORBUtils.hpp"
|
||||
#include <stdlib.h>
|
||||
|
||||
|
||||
uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(device::file_t *filp)
|
||||
{
|
||||
uORB::DeviceNode::SubscriberData *sd;
|
||||
if (filp) {
|
||||
sd = (uORB::DeviceNode::SubscriberData *)(filp->priv);
|
||||
}
|
||||
else {
|
||||
sd = 0;
|
||||
}
|
||||
return sd;
|
||||
}
|
||||
|
||||
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) :
|
||||
VDev(name, path),
|
||||
_meta(meta),
|
||||
_data(nullptr),
|
||||
_last_update(0),
|
||||
_generation(0),
|
||||
_publisher(0),
|
||||
_priority(priority)
|
||||
{
|
||||
// enable debug() calls
|
||||
//_debug_enabled = true;
|
||||
}
|
||||
|
||||
uORB::DeviceNode::~DeviceNode()
|
||||
{
|
||||
if (_data != nullptr)
|
||||
delete[] _data;
|
||||
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::open(device::file_t *filp)
|
||||
{
|
||||
int ret;
|
||||
|
||||
/* is this a publisher? */
|
||||
if (filp->flags == PX4_F_WRONLY) {
|
||||
|
||||
/* become the publisher if we can */
|
||||
lock();
|
||||
|
||||
if (_publisher == 0) {
|
||||
_publisher = getpid();
|
||||
ret = PX4_OK;
|
||||
|
||||
} else {
|
||||
ret = -EBUSY;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
/* now complete the open */
|
||||
if (ret == PX4_OK) {
|
||||
ret = VDev::open(filp);
|
||||
|
||||
/* open failed - not the publisher anymore */
|
||||
if (ret != PX4_OK)
|
||||
_publisher = 0;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* is this a new subscriber? */
|
||||
if (filp->flags == PX4_F_RDONLY) {
|
||||
|
||||
/* allocate subscriber data */
|
||||
SubscriberData *sd = new SubscriberData;
|
||||
|
||||
if (nullptr == sd)
|
||||
return -ENOMEM;
|
||||
|
||||
memset(sd, 0, sizeof(*sd));
|
||||
|
||||
/* default to no pending update */
|
||||
sd->generation = _generation;
|
||||
|
||||
/* set priority */
|
||||
sd->priority = _priority;
|
||||
|
||||
filp->priv = (void *)sd;
|
||||
|
||||
ret = VDev::open(filp);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
warnx("ERROR: VDev::open failed\n");
|
||||
delete sd;
|
||||
}
|
||||
|
||||
//warnx("uORB::DeviceNode::Open: fd = %d flags = %d, priv = %p cdev = %p\n", filp->fd, filp->flags, filp->priv, filp->cdev);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* can only be pub or sub, not both */
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::close(device::file_t *filp)
|
||||
{
|
||||
//warnx("uORB::DeviceNode::close fd = %d", filp->fd);
|
||||
/* is this the publisher closing? */
|
||||
if (getpid() == _publisher) {
|
||||
_publisher = 0;
|
||||
|
||||
} else {
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
if (sd != nullptr) {
|
||||
hrt_cancel(&sd->update_call);
|
||||
delete sd;
|
||||
}
|
||||
}
|
||||
|
||||
return VDev::close(filp);
|
||||
}
|
||||
|
||||
ssize_t
|
||||
uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
//warnx("uORB::DeviceNode::read fd = %d\n", filp->fd);
|
||||
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
|
||||
*/
|
||||
lock();
|
||||
|
||||
/* if the caller doesn't want the data, don't give it to them */
|
||||
if (nullptr != buffer)
|
||||
memcpy(buffer, _data, _meta->o_size);
|
||||
|
||||
/* track the last generation that the file has seen */
|
||||
sd->generation = _generation;
|
||||
|
||||
/* set priority */
|
||||
sd->priority = _priority;
|
||||
|
||||
/*
|
||||
* Clear the flag that indicates that an update has been reported, as
|
||||
* we have just collected it.
|
||||
*/
|
||||
sd->update_reported = false;
|
||||
|
||||
unlock();
|
||||
|
||||
return _meta->o_size;
|
||||
}
|
||||
|
||||
ssize_t
|
||||
uORB::DeviceNode::write(device::file_t *filp, const char *buffer, size_t buflen)
|
||||
{
|
||||
//warnx("uORB::DeviceNode::write filp = %p (null is normal)", filp);
|
||||
/*
|
||||
* 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) {
|
||||
lock();
|
||||
|
||||
/* re-check size */
|
||||
if (nullptr == _data)
|
||||
_data = new uint8_t[_meta->o_size];
|
||||
|
||||
unlock();
|
||||
|
||||
/* 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. */
|
||||
lock();
|
||||
memcpy(_data, buffer, _meta->o_size);
|
||||
unlock();
|
||||
|
||||
/* update the timestamp and generation count */
|
||||
_last_update = hrt_absolute_time();
|
||||
_generation++;
|
||||
|
||||
/* notify any poll waiters */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
return _meta->o_size;
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceNode::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
//warnx("uORB::DeviceNode::ioctl fd = %d cmd = %d", filp->fd, cmd);
|
||||
SubscriberData *sd = filp_to_sd(filp);
|
||||
|
||||
switch (cmd) {
|
||||
case ORBIOCLASTUPDATE:
|
||||
*(hrt_abstime *)arg = _last_update;
|
||||
return PX4_OK;
|
||||
|
||||
case ORBIOCUPDATED:
|
||||
*(bool *)arg = appears_updated(sd);
|
||||
return PX4_OK;
|
||||
|
||||
case ORBIOCSETINTERVAL:
|
||||
sd->update_interval = arg;
|
||||
return PX4_OK;
|
||||
|
||||
case ORBIOCGADVERTISER:
|
||||
*(uintptr_t *)arg = (uintptr_t)this;
|
||||
return PX4_OK;
|
||||
|
||||
case ORBIOCGPRIORITY:
|
||||
*(int *)arg = sd->priority;
|
||||
return PX4_OK;
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return VDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t
|
||||
uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data)
|
||||
{
|
||||
//warnx("uORB::DeviceNode::publish meta = %p", meta);
|
||||
|
||||
if (handle < 0) {
|
||||
warnx("uORB::DeviceNode::publish called with invalid handle");
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
|
||||
int ret;
|
||||
|
||||
/* this is a bit risky, since we are trusting the handle in order to deref it */
|
||||
if (devnode->_meta != meta) {
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* call the devnode write method with no file pointer */
|
||||
ret = devnode->write(nullptr, (const char *)data, meta->o_size);
|
||||
|
||||
if (ret < 0)
|
||||
return ERROR;
|
||||
|
||||
if (ret != (int)meta->o_size) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
pollevent_t
|
||||
uORB::DeviceNode::poll_state(device::file_t *filp)
|
||||
{
|
||||
//warnx("uORB::DeviceNode::poll_state fd = %d", filp->fd);
|
||||
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)
|
||||
{
|
||||
//warnx("uORB::DeviceNode::poll_notify_one fds = %p fds->priv = %p", fds, fds->priv);
|
||||
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))
|
||||
VDev::poll_notify_one(fds, events);
|
||||
}
|
||||
|
||||
bool
|
||||
uORB::DeviceNode::appears_updated(SubscriberData *sd)
|
||||
{
|
||||
//warnx("uORB::DeviceNode::appears_updated sd = %p", sd);
|
||||
/* assume it doesn't look updated */
|
||||
bool ret = false;
|
||||
|
||||
/* 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 == 0) {
|
||||
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;
|
||||
}
|
||||
|
||||
// FIXME - the calls to hrt_called and hrt_call_after seem not to work in the
|
||||
// POSIX build
|
||||
#ifndef __PX4_POSIX
|
||||
/*
|
||||
* 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_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_call,
|
||||
sd->update_interval,
|
||||
&uORB::DeviceNode::update_deferred_trampoline,
|
||||
(void *)this);
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Remember that we have told the subscriber that there is data.
|
||||
*/
|
||||
sd->update_reported = true;
|
||||
ret = true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
/* consider it updated */
|
||||
return ret;
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
uORB::DeviceMaster::DeviceMaster(Flavor f) :
|
||||
VDev((f == PUBSUB) ? "obj_master" : "param_master",
|
||||
(f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH),
|
||||
_flavor(f)
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
||||
}
|
||||
|
||||
uORB::DeviceMaster::~DeviceMaster()
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret;
|
||||
|
||||
switch (cmd) {
|
||||
case ORBIOCADVERTISE: {
|
||||
const struct orb_advertdata *adv = (const struct orb_advertdata *)arg;
|
||||
const struct orb_metadata *meta = adv->meta;
|
||||
const char *objname;
|
||||
const char *devpath;
|
||||
char nodepath[orb_maxpath];
|
||||
uORB::DeviceNode *node;
|
||||
|
||||
/* set instance to zero - we could allow selective multi-pubs later based on value */
|
||||
if (adv->instance != nullptr) {
|
||||
*(adv->instance) = 0;
|
||||
}
|
||||
|
||||
/* construct a path to the node - this also checks the node name */
|
||||
ret = uORB::Utils::node_mkpath(nodepath, _flavor, meta, adv->instance);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ensure that only one advertiser runs through this critical section */
|
||||
lock();
|
||||
|
||||
ret = ERROR;
|
||||
|
||||
/* try for topic groups */
|
||||
const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
|
||||
unsigned group_tries = 0;
|
||||
do {
|
||||
/* if path is modifyable change try index */
|
||||
if (adv->instance != nullptr) {
|
||||
/* replace the number at the end of the string */
|
||||
nodepath[strlen(nodepath) - 1] = '0' + group_tries;
|
||||
*(adv->instance) = group_tries;
|
||||
}
|
||||
|
||||
/* driver wants a permanent copy of the node name, so make one here */
|
||||
objname = strdup(meta->o_name);
|
||||
|
||||
if (objname == nullptr) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* driver wants a permanent copy of the path, so make one here */
|
||||
devpath = strdup(nodepath);
|
||||
|
||||
if (devpath == nullptr) {
|
||||
// FIXME - looks like we leaked memory here for objname
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* construct the new node */
|
||||
node = new uORB::DeviceNode(meta, objname, devpath, adv->priority);
|
||||
|
||||
/* if we didn't get a device, that's bad */
|
||||
if (node == nullptr) {
|
||||
unlock();
|
||||
|
||||
// FIXME - looks like we leaked memory here for devpath and objname
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* initialise the node - this may fail if e.g. a node with this name already exists */
|
||||
ret = node->init();
|
||||
|
||||
/* if init failed, discard the node and its name */
|
||||
if (ret != PX4_OK) {
|
||||
delete node;
|
||||
free((void *)objname);
|
||||
free((void *)devpath);
|
||||
}
|
||||
|
||||
group_tries++;
|
||||
|
||||
} while (ret != PX4_OK && (group_tries < max_group_tries));
|
||||
|
||||
if (group_tries > max_group_tries) {
|
||||
ret = -ENOMEM;
|
||||
}
|
||||
|
||||
/* the file handle for the driver has been created, unlock */
|
||||
unlock();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return VDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,122 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef _uORBDevices_posix_hpp_
|
||||
#define _uORBDevices_posix_hpp_
|
||||
|
||||
#include <stdint.h>
|
||||
#include "uORBCommon.hpp"
|
||||
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
class DeviceNode;
|
||||
class DeviceMaster;
|
||||
}
|
||||
|
||||
class uORB::DeviceNode : public device::VDev
|
||||
{
|
||||
public:
|
||||
DeviceNode(const struct orb_metadata *meta, const char *name, const char *path, int priority);
|
||||
~DeviceNode();
|
||||
|
||||
virtual int open(device::file_t *filp);
|
||||
virtual int close(device::file_t *filp);
|
||||
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
|
||||
virtual ssize_t write(device::file_t *filp, const char *buffer, size_t buflen);
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
|
||||
|
||||
static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data);
|
||||
|
||||
protected:
|
||||
virtual pollevent_t poll_state(device::file_t *filp);
|
||||
virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events);
|
||||
|
||||
private:
|
||||
struct SubscriberData {
|
||||
unsigned generation; /**< last generation the subscriber has seen */
|
||||
unsigned update_interval; /**< if nonzero minimum interval between updates */
|
||||
struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */
|
||||
void *poll_priv; /**< saved copy of fds->f_priv while poll is active */
|
||||
bool update_reported; /**< true if we have reported the update via poll/check */
|
||||
int priority; /**< priority of publisher */
|
||||
};
|
||||
|
||||
const struct orb_metadata *_meta; /**< object metadata information */
|
||||
uint8_t *_data; /**< allocated object buffer */
|
||||
hrt_abstime _last_update; /**< time the object was last updated */
|
||||
volatile unsigned _generation; /**< object generation count */
|
||||
pid_t _publisher; /**< if nonzero, current publisher */
|
||||
const int _priority; /**< priority of topic */
|
||||
|
||||
SubscriberData *filp_to_sd(device::file_t *filp);
|
||||
|
||||
/**
|
||||
* Perform a deferred update for a rate-limited subscriber.
|
||||
*/
|
||||
void update_deferred();
|
||||
|
||||
/**
|
||||
* Bridge from hrt_call to update_deferred
|
||||
*
|
||||
* void *arg ORBDevNode pointer for which the deferred update is performed.
|
||||
*/
|
||||
static void update_deferred_trampoline(void *arg);
|
||||
|
||||
/**
|
||||
* Check whether a topic appears updated to a subscriber.
|
||||
*
|
||||
* @param sd The subscriber for whom to check.
|
||||
* @return True if the topic should appear updated to the subscriber
|
||||
*/
|
||||
bool appears_updated(SubscriberData *sd);
|
||||
};
|
||||
|
||||
/**
|
||||
* Master control device for ObjDev.
|
||||
*
|
||||
* Used primarily to create new objects via the ORBIOCCREATE
|
||||
* ioctl.
|
||||
*/
|
||||
class uORB::DeviceMaster : public device::VDev
|
||||
{
|
||||
public:
|
||||
DeviceMaster(Flavor f);
|
||||
~DeviceMaster();
|
||||
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
|
||||
private:
|
||||
Flavor _flavor;
|
||||
};
|
||||
|
||||
#endif /* _uORBDeviceNode_posix.hpp */
|
||||
|
|
@ -0,0 +1,124 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 <string.h>
|
||||
#include "uORBDevices.hpp"
|
||||
#include "uORB.h"
|
||||
#include "uORBTest_UnitTest.hpp"
|
||||
#include "uORBCommon.hpp"
|
||||
|
||||
extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); }
|
||||
|
||||
static uORB::DeviceMaster *g_dev = nullptr;
|
||||
static void usage()
|
||||
{
|
||||
warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'");
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
uorb_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
usage();
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*
|
||||
* XXX it would be nice to have a wrapper for this...
|
||||
*/
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
warnx("already loaded");
|
||||
/* user wanted to start uorb, its already running, no error */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new uORB::DeviceMaster(uORB::PUBSUB);
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
warnx("driver alloc failed");
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
warnx("driver init failed");
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test"))
|
||||
{
|
||||
uORBTest::UnitTest t;
|
||||
return t.test();
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the latency.
|
||||
*/
|
||||
if (!strcmp(argv[1], "latency_test")) {
|
||||
|
||||
uORBTest::UnitTest t;
|
||||
if (argc > 2 && !strcmp(argv[2], "medium")) {
|
||||
return t.latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
|
||||
} else if (argc > 2 && !strcmp(argv[2], "large")) {
|
||||
return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
|
||||
} else {
|
||||
return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[1], "status"))
|
||||
{
|
||||
return OK;
|
||||
}
|
||||
|
||||
usage();
|
||||
errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'");
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,325 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef _uORBManager_hpp_
|
||||
#define _uORBManager_hpp_
|
||||
|
||||
#include "uORBCommon.hpp"
|
||||
#include <stdint.h>
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
class Manager;
|
||||
}
|
||||
|
||||
/**
|
||||
* This is implemented as a singleton. This class manages creating the
|
||||
* uORB nodes for each uORB topics and also implements the behavor of the
|
||||
* uORB Api's.
|
||||
*/
|
||||
class uORB::Manager
|
||||
{
|
||||
public:
|
||||
// public interfaces for this class.
|
||||
|
||||
/**
|
||||
* Method to get the singleton instance for the uORB::Manager.
|
||||
* @return uORB::Manager*
|
||||
*/
|
||||
static uORB::Manager* get_instance();
|
||||
|
||||
// ==== uORB interface methods ====
|
||||
/**
|
||||
* Advertise as the publisher of a topic.
|
||||
*
|
||||
* This performs the initial advertisement of a topic; it creates the topic
|
||||
* node in /obj if required and publishes the initial data.
|
||||
*
|
||||
* Any number of advertisers may publish to a topic; publications are atomic
|
||||
* but co-ordination between publishers is not provided by the ORB.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param data A pointer to the initial data to be published.
|
||||
* For topics updated by interrupt handlers, the advertisement
|
||||
* must be performed from non-interrupt context.
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to publish to the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data);
|
||||
|
||||
/**
|
||||
* Advertise as the publisher of a topic.
|
||||
*
|
||||
* This performs the initial advertisement of a topic; it creates the topic
|
||||
* node in /obj if required and publishes the initial data.
|
||||
*
|
||||
* Any number of advertisers may publish to a topic; publications are atomic
|
||||
* but co-ordination between publishers is not provided by the ORB.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param data A pointer to the initial data to be published.
|
||||
* For topics updated by interrupt handlers, the advertisement
|
||||
* must be performed from non-interrupt context.
|
||||
* @param instance Pointer to an integer which will yield the instance ID (0-based)
|
||||
* of the publication.
|
||||
* @param priority The priority of the instance. If a subscriber subscribes multiple
|
||||
* instances, the priority allows the subscriber to prioritize the best
|
||||
* data source as long as its available.
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to publish to the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
|
||||
int priority) ;
|
||||
|
||||
|
||||
/**
|
||||
* Publish new data to a topic.
|
||||
*
|
||||
* The data is atomically published to the topic and any waiting subscribers
|
||||
* will be notified. Subscribers that are not waiting can check the topic
|
||||
* for updates using orb_check and/or orb_stat.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @handle The handle returned from orb_advertise.
|
||||
* @param data A pointer to the data to be published.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) ;
|
||||
|
||||
/**
|
||||
* Subscribe to a topic.
|
||||
*
|
||||
* The returned value is a file descriptor that can be passed to poll()
|
||||
* in order to wait for updates to a topic, as well as topic_read,
|
||||
* orb_check and orb_stat.
|
||||
*
|
||||
* Subscription will succeed even if the topic has not been advertised;
|
||||
* in this case the topic will have a timestamp of zero, it will never
|
||||
* signal a poll() event, checking will always return false and it cannot
|
||||
* be copied. When the topic is subsequently advertised, poll, check,
|
||||
* stat and copy calls will react to the initial publication that is
|
||||
* performed as part of the advertisement.
|
||||
*
|
||||
* Subscription will fail if the topic is not known to the system, i.e.
|
||||
* there is nothing in the system that has declared the topic and thus it
|
||||
* can never be published.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to read and update the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
int orb_subscribe(const struct orb_metadata *meta) ;
|
||||
|
||||
/**
|
||||
* Subscribe to a multi-instance of a topic.
|
||||
*
|
||||
* The returned value is a file descriptor that can be passed to poll()
|
||||
* in order to wait for updates to a topic, as well as topic_read,
|
||||
* orb_check and orb_stat.
|
||||
*
|
||||
* Subscription will succeed even if the topic has not been advertised;
|
||||
* in this case the topic will have a timestamp of zero, it will never
|
||||
* signal a poll() event, checking will always return false and it cannot
|
||||
* be copied. When the topic is subsequently advertised, poll, check,
|
||||
* stat and copy calls will react to the initial publication that is
|
||||
* performed as part of the advertisement.
|
||||
*
|
||||
* Subscription will fail if the topic is not known to the system, i.e.
|
||||
* there is nothing in the system that has declared the topic and thus it
|
||||
* can never be published.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param instance The instance of the topic. Instance 0 matches the
|
||||
* topic of the orb_subscribe() call, higher indices
|
||||
* are for topics created with orb_publish_multi().
|
||||
* @return ERROR on error, otherwise returns a handle
|
||||
* that can be used to read and update the topic.
|
||||
* If the topic in question is not known (due to an
|
||||
* ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
|
||||
* this function will return -1 and set errno to ENOENT.
|
||||
*/
|
||||
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) ;
|
||||
|
||||
/**
|
||||
* Unsubscribe from a topic.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_unsubscribe(int handle) ;
|
||||
|
||||
/**
|
||||
* Fetch data from a topic.
|
||||
*
|
||||
* This is the only operation that will reset the internal marker that
|
||||
* indicates that a topic has been updated for a subscriber. Once poll
|
||||
* or check return indicating that an updaet is available, this call
|
||||
* must be used to update the subscription.
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param buffer Pointer to the buffer receiving the data, or NULL
|
||||
* if the caller wants to clear the updated flag without
|
||||
* using the data.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) ;
|
||||
|
||||
/**
|
||||
* Check whether a topic has been published to since the last orb_copy.
|
||||
*
|
||||
* This check can be used to determine whether to copy the topic when
|
||||
* not using poll(), or to avoid the overhead of calling poll() when the
|
||||
* topic is likely to have updated.
|
||||
*
|
||||
* Updates are tracked on a per-handle basis; this call will continue to
|
||||
* return true until orb_copy is called using the same handle. This interface
|
||||
* should be preferred over calling orb_stat due to the race window between
|
||||
* stat and copy that can lead to missed updates.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param updated Set to true if the topic has been updated since the
|
||||
* last time it was copied using this handle.
|
||||
* @return OK if the check was successful, ERROR otherwise with
|
||||
* errno set accordingly.
|
||||
*/
|
||||
int orb_check(int handle, bool *updated) ;
|
||||
|
||||
/**
|
||||
* Return the last time that the topic was updated.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param time Returns the absolute time that the topic was updated, or zero if it has
|
||||
* never been updated. Time is measured in microseconds.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_stat(int handle, uint64_t *time) ;
|
||||
|
||||
/**
|
||||
* Check if a topic has already been created.
|
||||
*
|
||||
* @param meta ORB topic metadata.
|
||||
* @param instance ORB instance
|
||||
* @return OK if the topic exists, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_exists(const struct orb_metadata *meta, int instance) ;
|
||||
|
||||
/**
|
||||
* Return the priority of the topic
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param priority Returns the priority of this topic. This is only relevant for
|
||||
* topics which are published by multiple publishers (e.g. mag0, mag1, etc.)
|
||||
* and allows a subscriber to automatically pick the topic with the highest
|
||||
* priority, independent of the startup order of the associated publishers.
|
||||
* @return OK on success, ERROR otherwise with errno set accordingly.
|
||||
*/
|
||||
int orb_priority(int handle, int *priority) ;
|
||||
|
||||
/**
|
||||
* Set the minimum interval between which updates are seen for a subscription.
|
||||
*
|
||||
* If this interval is set, the subscriber will not see more than one update
|
||||
* within the period.
|
||||
*
|
||||
* Specifically, the first time an update is reported to the subscriber a timer
|
||||
* is started. The update will continue to be reported via poll and orb_check, but
|
||||
* once fetched via orb_copy another update will not be reported until the timer
|
||||
* expires.
|
||||
*
|
||||
* This feature can be used to pace a subscriber that is watching a topic that
|
||||
* would otherwise update too quickly.
|
||||
*
|
||||
* @param handle A handle returned from orb_subscribe.
|
||||
* @param interval An interval period in milliseconds.
|
||||
* @return OK on success, ERROR otherwise with ERRNO set accordingly.
|
||||
*/
|
||||
int orb_set_interval(int handle, unsigned interval) ;
|
||||
|
||||
private: // class methods
|
||||
/**
|
||||
* Advertise a node; don't consider it an error if the node has
|
||||
* already been advertised.
|
||||
*
|
||||
* @todo verify that the existing node is the same as the one
|
||||
* we tried to advertise.
|
||||
*/
|
||||
int
|
||||
node_advertise
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
int *instance = nullptr,
|
||||
int priority = ORB_PRIO_DEFAULT
|
||||
);
|
||||
|
||||
/**
|
||||
* Common implementation for orb_advertise and orb_subscribe.
|
||||
*
|
||||
* Handles creation of the object and the initial publication for
|
||||
* advertisers.
|
||||
*/
|
||||
int
|
||||
node_open
|
||||
(
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance = nullptr,
|
||||
int priority = ORB_PRIO_DEFAULT
|
||||
);
|
||||
|
||||
private: // data members
|
||||
static Manager _Instance;
|
||||
|
||||
private: //class methods
|
||||
Manager();
|
||||
|
||||
};
|
||||
|
||||
#endif /* _uORBManager_hpp_ */
|
||||
|
|
@ -0,0 +1,282 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <stdarg.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include "uORBUtils.hpp"
|
||||
#include "uORBManager.hpp"
|
||||
#include "uORBDevices.hpp"
|
||||
|
||||
|
||||
//========================= Static initializations =================
|
||||
uORB::Manager uORB::Manager::_Instance;
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
uORB::Manager* uORB::Manager::get_instance()
|
||||
{
|
||||
return &_Instance;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
uORB::Manager::Manager()
|
||||
{
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
|
||||
{
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
char path[orb_maxpath];
|
||||
int inst = instance;
|
||||
int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst);
|
||||
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return uORB::ERROR;
|
||||
}
|
||||
|
||||
struct stat buffer;
|
||||
return stat(path, &buffer);
|
||||
}
|
||||
|
||||
orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data)
|
||||
{
|
||||
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
|
||||
{
|
||||
int result, fd;
|
||||
orb_advert_t advertiser;
|
||||
|
||||
/* open the node as an advertiser */
|
||||
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
||||
if (fd == ERROR)
|
||||
return ERROR;
|
||||
|
||||
/* get the advertiser handle and close the node */
|
||||
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||
close(fd);
|
||||
if (result == ERROR)
|
||||
return ERROR;
|
||||
|
||||
/* the advertiser must perform an initial publish to initialise the object */
|
||||
result = orb_publish(meta, advertiser, data);
|
||||
if (result == ERROR)
|
||||
return ERROR;
|
||||
|
||||
return advertiser;
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
|
||||
{
|
||||
return node_open(PUBSUB, meta, nullptr, false);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
|
||||
{
|
||||
int inst = instance;
|
||||
return node_open(PUBSUB, meta, nullptr, false, &inst);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_unsubscribe(int handle)
|
||||
{
|
||||
return close(handle);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
|
||||
{
|
||||
return uORB::DeviceNode::publish(meta, handle, data);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = read(handle, buffer, meta->o_size);
|
||||
|
||||
if (ret < 0)
|
||||
return ERROR;
|
||||
|
||||
if (ret != (int)meta->o_size) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_check(int handle, bool *updated)
|
||||
{
|
||||
return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_stat(int handle, uint64_t *time)
|
||||
{
|
||||
return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_priority(int handle, int *priority)
|
||||
{
|
||||
return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_set_interval(int handle, unsigned interval)
|
||||
{
|
||||
return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
|
||||
}
|
||||
|
||||
|
||||
int uORB::Manager::node_advertise
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
int *instance,
|
||||
int priority
|
||||
)
|
||||
{
|
||||
int fd = -1;
|
||||
int ret = ERROR;
|
||||
|
||||
/* fill advertiser data */
|
||||
const struct orb_advertdata adv = { meta, instance, priority };
|
||||
|
||||
/* open the control device */
|
||||
fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0)
|
||||
goto out;
|
||||
|
||||
/* advertise the object */
|
||||
ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
|
||||
|
||||
/* it's OK if it already exists */
|
||||
if ((OK != ret) && (EEXIST == errno)) {
|
||||
ret = OK;
|
||||
}
|
||||
|
||||
out:
|
||||
|
||||
if (fd >= 0)
|
||||
close(fd);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int uORB::Manager::node_open
|
||||
(
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance,
|
||||
int priority
|
||||
)
|
||||
{
|
||||
char path[orb_maxpath];
|
||||
int fd, ret;
|
||||
|
||||
/*
|
||||
* If meta is null, the object was not defined, i.e. it is not
|
||||
* known to the system. We can't advertise/subscribe such a thing.
|
||||
*/
|
||||
if (nullptr == meta) {
|
||||
errno = ENOENT;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Advertiser must publish an initial value.
|
||||
*/
|
||||
if (advertiser && (data == nullptr)) {
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
ret = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* open the path as either the advertiser or the subscriber */
|
||||
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
|
||||
|
||||
/* if we want to advertise and the node existed, we have to re-try again */
|
||||
if ((fd >= 0) && (instance != nullptr) && (advertiser)) {
|
||||
/* close the fd, we want a new one */
|
||||
close(fd);
|
||||
/* the node_advertise call will automatically go for the next free entry */
|
||||
fd = -1;
|
||||
}
|
||||
|
||||
/* we may need to advertise the node... */
|
||||
if (fd < 0) {
|
||||
|
||||
/* try to create the node */
|
||||
ret = node_advertise(meta, instance, priority);
|
||||
|
||||
if (ret == OK) {
|
||||
/* update the path, as it might have been updated during the node_advertise call */
|
||||
ret = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
/* on success, try the open again */
|
||||
if (ret == OK) {
|
||||
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
|
||||
}
|
||||
}
|
||||
|
||||
if (fd < 0) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* everything has been OK, we can return the handle now */
|
||||
return fd;
|
||||
}
|
||||
|
|
@ -0,0 +1,296 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <stdarg.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include "uORBUtils.hpp"
|
||||
#include "uORBManager.hpp"
|
||||
#include "uORBDevices.hpp"
|
||||
#include "px4_config.h"
|
||||
|
||||
|
||||
//========================= Static initializations =================
|
||||
uORB::Manager uORB::Manager::_Instance;
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
uORB::Manager* uORB::Manager::get_instance()
|
||||
{
|
||||
return &_Instance;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
//-----------------------------------------------------------------------------
|
||||
uORB::Manager::Manager()
|
||||
{
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
|
||||
{
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
char path[orb_maxpath];
|
||||
int inst = instance;
|
||||
int ret = uORB::Utils::node_mkpath(path, PUBSUB, meta, &inst);
|
||||
|
||||
if (ret != OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
struct stat buffer;
|
||||
return stat(path, &buffer);
|
||||
}
|
||||
|
||||
orb_advert_t uORB::Manager::orb_advertise(const struct orb_metadata *meta, const void *data)
|
||||
{
|
||||
//warnx("orb_advertise meta = %p", meta);
|
||||
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
|
||||
{
|
||||
int result, fd;
|
||||
orb_advert_t advertiser;
|
||||
|
||||
//warnx("orb_advertise_multi meta = %p\n", meta);
|
||||
|
||||
/* open the node as an advertiser */
|
||||
fd = node_open(PUBSUB, meta, data, true, instance, priority);
|
||||
if (fd == ERROR) {
|
||||
warnx("node_open as advertiser failed.");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* get the advertiser handle and close the node */
|
||||
result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||
px4_close(fd);
|
||||
if (result == ERROR) {
|
||||
warnx("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* the advertiser must perform an initial publish to initialise the object */
|
||||
result = orb_publish(meta, advertiser, data);
|
||||
if (result == ERROR) {
|
||||
warnx("orb_publish failed");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return advertiser;
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
|
||||
{
|
||||
return node_open(PUBSUB, meta, nullptr, false);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
|
||||
{
|
||||
int inst = instance;
|
||||
return node_open(PUBSUB, meta, nullptr, false, &inst);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_unsubscribe(int fd)
|
||||
{
|
||||
return px4_close(fd);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
|
||||
{
|
||||
return uORB::DeviceNode::publish(meta, handle, data);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = px4_read(handle, buffer, meta->o_size);
|
||||
|
||||
if (ret < 0)
|
||||
return ERROR;
|
||||
|
||||
if (ret != (int)meta->o_size) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_check(int handle, bool *updated)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_stat(int handle, uint64_t *time)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_priority(int handle, int *priority)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
|
||||
}
|
||||
|
||||
int uORB::Manager::orb_set_interval(int handle, unsigned interval)
|
||||
{
|
||||
return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
|
||||
}
|
||||
|
||||
|
||||
int uORB::Manager::node_advertise
|
||||
(
|
||||
const struct orb_metadata *meta,
|
||||
int *instance,
|
||||
int priority
|
||||
)
|
||||
{
|
||||
int fd = -1;
|
||||
int ret = ERROR;
|
||||
|
||||
/* fill advertiser data */
|
||||
const struct orb_advertdata adv = { meta, instance, priority };
|
||||
|
||||
/* open the control device */
|
||||
fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0);
|
||||
|
||||
if (fd < 0)
|
||||
goto out;
|
||||
|
||||
/* advertise the object */
|
||||
ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
|
||||
|
||||
/* it's PX4_OK if it already exists */
|
||||
if ((PX4_OK != ret) && (EEXIST == errno)) {
|
||||
ret = PX4_OK;
|
||||
}
|
||||
|
||||
out:
|
||||
|
||||
if (fd >= 0)
|
||||
px4_close(fd);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int uORB::Manager::node_open
|
||||
(
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
const void *data,
|
||||
bool advertiser,
|
||||
int *instance,
|
||||
int priority
|
||||
)
|
||||
{
|
||||
char path[orb_maxpath];
|
||||
int fd, ret;
|
||||
|
||||
/*
|
||||
* If meta is null, the object was not defined, i.e. it is not
|
||||
* known to the system. We can't advertise/subscribe such a thing.
|
||||
*/
|
||||
if (nullptr == meta) {
|
||||
errno = ENOENT;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Advertiser must publish an initial value.
|
||||
*/
|
||||
if (advertiser && (data == nullptr)) {
|
||||
errno = EINVAL;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/*
|
||||
* Generate the path to the node and try to open it.
|
||||
*/
|
||||
|
||||
// FIXME - if *instance is uninitialized, why is this being called? Seems risky and
|
||||
// its definiately a waste. This is the case in muli-topic test.
|
||||
ret = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* open the path as either the advertiser or the subscriber */
|
||||
fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
|
||||
|
||||
/* if we want to advertise and the node existed, we have to re-try again */
|
||||
if ((fd >= 0) && (instance != nullptr) && (advertiser)) {
|
||||
/* close the fd, we want a new one */
|
||||
px4_close(fd);
|
||||
|
||||
/* the node_advertise call will automatically go for the next free entry */
|
||||
fd = -1;
|
||||
}
|
||||
|
||||
/* we may need to advertise the node... */
|
||||
if (fd < 0) {
|
||||
|
||||
/* try to create the node */
|
||||
ret = node_advertise(meta, instance, priority);
|
||||
|
||||
if (ret == PX4_OK) {
|
||||
/* update the path, as it might have been updated during the node_advertise call */
|
||||
ret = uORB::Utils::node_mkpath(path, f, meta, instance);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
errno = -ret;
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
/* on success, try the open again */
|
||||
if (ret == PX4_OK) {
|
||||
fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
|
||||
}
|
||||
}
|
||||
|
||||
if (fd < 0) {
|
||||
errno = EIO;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* everything has been OK, we can return the handle now */
|
||||
return fd;
|
||||
}
|
||||
|
|
@ -0,0 +1,287 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 "uORBTest_UnitTest.hpp"
|
||||
#include "uORBCommon.hpp"
|
||||
#include <px4_config.h>
|
||||
#include <px4_time.h>
|
||||
|
||||
int uORBTest::UnitTest::pubsublatency_main(void)
|
||||
{
|
||||
/* poll on test topic and output latency */
|
||||
float latency_integral = 0.0f;
|
||||
|
||||
/* wakeup source(s) */
|
||||
px4_pollfd_struct_t fds[3];
|
||||
|
||||
int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test), 0);
|
||||
int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0);
|
||||
int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0);
|
||||
|
||||
struct orb_test_large t;
|
||||
|
||||
/* clear all ready flags */
|
||||
orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
|
||||
orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
|
||||
orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
|
||||
|
||||
fds[0].fd = test_multi_sub;
|
||||
fds[0].events = POLLIN;
|
||||
fds[1].fd = test_multi_sub_medium;
|
||||
fds[1].events = POLLIN;
|
||||
fds[2].fd = test_multi_sub_large;
|
||||
fds[2].events = POLLIN;
|
||||
|
||||
const unsigned maxruns = 1000;
|
||||
unsigned timingsgroup = 0;
|
||||
|
||||
unsigned *timings = new unsigned[maxruns];
|
||||
|
||||
for (unsigned i = 0; i < maxruns; i++) {
|
||||
/* wait for up to 500ms for data */
|
||||
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
|
||||
if (fds[0].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
|
||||
timingsgroup = 0;
|
||||
} else if (fds[1].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(orb_test_medium), test_multi_sub_medium, &t);
|
||||
timingsgroup = 1;
|
||||
} else if (fds[2].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(orb_test_large), test_multi_sub_large, &t);
|
||||
timingsgroup = 2;
|
||||
}
|
||||
|
||||
if (pret < 0) {
|
||||
warn("poll error %d, %d", pret, errno);
|
||||
continue;
|
||||
}
|
||||
|
||||
hrt_abstime elt = hrt_elapsed_time(&t.time);
|
||||
latency_integral += elt;
|
||||
timings[i] = elt;
|
||||
}
|
||||
|
||||
orb_unsubscribe(test_multi_sub);
|
||||
orb_unsubscribe(test_multi_sub_medium);
|
||||
orb_unsubscribe(test_multi_sub_large);
|
||||
|
||||
if (pubsubtest_print) {
|
||||
char fname[32];
|
||||
//sprintf(fname, "/fs/microsd/timings%u.txt", timingsgroup);
|
||||
sprintf(fname, "/tmp/timings%u.txt", timingsgroup);
|
||||
FILE *f = fopen(fname, "w");
|
||||
if (f == NULL) {
|
||||
warnx("Error opening file!\n");
|
||||
return uORB::ERROR;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < maxruns; i++) {
|
||||
fprintf(f, "%u\n", timings[i]);
|
||||
}
|
||||
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
delete[] timings;
|
||||
|
||||
warnx("mean: %8.4f", static_cast<double>(latency_integral / maxruns));
|
||||
|
||||
pubsubtest_passed = true;
|
||||
|
||||
if (static_cast<float>(latency_integral / maxruns) > 30.0f) {
|
||||
pubsubtest_res = uORB::ERROR;
|
||||
} else {
|
||||
pubsubtest_res = PX4_OK;
|
||||
}
|
||||
|
||||
return pubsubtest_res;
|
||||
}
|
||||
|
||||
int uORBTest::UnitTest::test()
|
||||
{
|
||||
struct orb_test t, u;
|
||||
int pfd, sfd;
|
||||
bool updated;
|
||||
|
||||
t.val = 0;
|
||||
pfd = orb_advertise(ORB_ID(orb_test), &t);
|
||||
|
||||
if (pfd < 0)
|
||||
return test_fail("advertise failed: %d", errno);
|
||||
|
||||
test_note("publish handle 0x%08x", pfd);
|
||||
sfd = orb_subscribe(ORB_ID(orb_test));
|
||||
|
||||
if (sfd < 0)
|
||||
return test_fail("subscribe failed: %d", errno);
|
||||
|
||||
test_note("subscribe fd %d", sfd);
|
||||
u.val = 1;
|
||||
|
||||
if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u))
|
||||
return test_fail("copy(1) failed: %d", errno);
|
||||
|
||||
if (u.val != t.val)
|
||||
return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
|
||||
|
||||
if (PX4_OK != orb_check(sfd, &updated))
|
||||
return test_fail("check(1) failed");
|
||||
|
||||
if (updated)
|
||||
return test_fail("spurious updated flag");
|
||||
|
||||
t.val = 2;
|
||||
test_note("try publish");
|
||||
|
||||
if (PX4_OK != orb_publish(ORB_ID(orb_test), pfd, &t))
|
||||
return test_fail("publish failed");
|
||||
|
||||
if (PX4_OK != orb_check(sfd, &updated))
|
||||
return test_fail("check(2) failed");
|
||||
|
||||
if (!updated)
|
||||
return test_fail("missing updated flag");
|
||||
|
||||
if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u))
|
||||
return test_fail("copy(2) failed: %d", errno);
|
||||
|
||||
if (u.val != t.val)
|
||||
return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val);
|
||||
|
||||
orb_unsubscribe(sfd);
|
||||
close(pfd);
|
||||
|
||||
/* this routine tests the multi-topic support */
|
||||
test_note("try multi-topic support");
|
||||
|
||||
int instance0;
|
||||
int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
|
||||
|
||||
test_note("advertised");
|
||||
|
||||
int instance1;
|
||||
int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
|
||||
|
||||
if (instance0 != 0)
|
||||
return test_fail("mult. id0: %d", instance0);
|
||||
|
||||
if (instance1 != 1)
|
||||
return test_fail("mult. id1: %d", instance1);
|
||||
|
||||
t.val = 103;
|
||||
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
|
||||
return test_fail("mult. pub0 fail");
|
||||
|
||||
test_note("published");
|
||||
|
||||
t.val = 203;
|
||||
if (PX4_OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t))
|
||||
return test_fail("mult. pub1 fail");
|
||||
|
||||
/* subscribe to both topics and ensure valid data is received */
|
||||
int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
|
||||
|
||||
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u))
|
||||
return test_fail("sub #0 copy failed: %d", errno);
|
||||
|
||||
if (u.val != 103)
|
||||
return test_fail("sub #0 val. mismatch: %d", u.val);
|
||||
|
||||
int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
|
||||
|
||||
if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u))
|
||||
return test_fail("sub #1 copy failed: %d", errno);
|
||||
|
||||
if (u.val != 203)
|
||||
return test_fail("sub #1 val. mismatch: %d", u.val);
|
||||
|
||||
/* test priorities */
|
||||
int prio;
|
||||
if (PX4_OK != orb_priority(sfd0, &prio))
|
||||
return test_fail("prio #0");
|
||||
|
||||
if (prio != ORB_PRIO_MAX)
|
||||
return test_fail("prio: %d", prio);
|
||||
|
||||
if (PX4_OK != orb_priority(sfd1, &prio))
|
||||
return test_fail("prio #1");
|
||||
|
||||
if (prio != ORB_PRIO_MIN)
|
||||
return test_fail("prio: %d", prio);
|
||||
|
||||
if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false))
|
||||
return test_fail("latency test failed");
|
||||
|
||||
return test_note("PASS");
|
||||
}
|
||||
|
||||
int uORBTest::UnitTest::info()
|
||||
{
|
||||
return OK;
|
||||
}
|
||||
|
||||
int uORBTest::UnitTest::test_fail(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
fprintf(stderr, "FAIL: ");
|
||||
va_start(ap, fmt);
|
||||
vfprintf(stderr, fmt, ap);
|
||||
va_end(ap);
|
||||
fprintf(stderr, "\n");
|
||||
fflush(stderr);
|
||||
return uORB::ERROR;
|
||||
}
|
||||
|
||||
int uORBTest::UnitTest::test_note(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
fprintf(stderr, "note: ");
|
||||
va_start(ap, fmt);
|
||||
vfprintf(stderr, fmt, ap);
|
||||
va_end(ap);
|
||||
fprintf(stderr, "\n");
|
||||
fflush(stderr);
|
||||
return OK;
|
||||
}
|
||||
|
||||
int uORBTest::UnitTest::pubsubtest_threadEntry( char* const* data )
|
||||
{
|
||||
uORBTest::UnitTest* t = (uORBTest::UnitTest*) data;
|
||||
if( data != nullptr )
|
||||
{
|
||||
return t->pubsublatency_main();
|
||||
}
|
||||
return uORB::ERROR;
|
||||
}
|
||||
|
|
@ -0,0 +1,128 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef _uORBTest_UnitTest_hpp_
|
||||
#define _uORBTest_UnitTest_hpp_
|
||||
#include "uORBCommon.hpp"
|
||||
#include "uORB.h"
|
||||
|
||||
struct orb_test {
|
||||
int val;
|
||||
hrt_abstime time;
|
||||
};
|
||||
ORB_DEFINE(orb_test, struct orb_test);
|
||||
ORB_DEFINE(orb_multitest, struct orb_test);
|
||||
|
||||
struct orb_test_medium {
|
||||
int val;
|
||||
hrt_abstime time;
|
||||
char junk[64];
|
||||
};
|
||||
ORB_DEFINE(orb_test_medium, struct orb_test_medium);
|
||||
|
||||
struct orb_test_large {
|
||||
int val;
|
||||
hrt_abstime time;
|
||||
char junk[512];
|
||||
};
|
||||
ORB_DEFINE(orb_test_large, struct orb_test_large);
|
||||
|
||||
|
||||
namespace uORBTest
|
||||
{
|
||||
class UnitTest;
|
||||
}
|
||||
|
||||
class uORBTest::UnitTest
|
||||
{
|
||||
public:
|
||||
|
||||
int test();
|
||||
template<typename S> int latency_test(orb_id_t T, bool print);
|
||||
int info();
|
||||
|
||||
private:
|
||||
static int pubsubtest_threadEntry( char* const* argv );
|
||||
int pubsublatency_main(void);
|
||||
bool pubsubtest_passed = false;
|
||||
bool pubsubtest_print = false;
|
||||
int pubsubtest_res = OK;
|
||||
|
||||
int test_fail(const char *fmt, ...);
|
||||
int test_note(const char *fmt, ...);
|
||||
};
|
||||
|
||||
template<typename S>
|
||||
int uORBTest::UnitTest::latency_test(orb_id_t T, bool print)
|
||||
{
|
||||
test_note("---------------- LATENCY TEST ------------------");
|
||||
S t;
|
||||
t.val = 308;
|
||||
t.time = hrt_absolute_time();
|
||||
|
||||
int pfd0 = orb_advertise(T, &t);
|
||||
|
||||
pubsubtest_print = print;
|
||||
|
||||
pubsubtest_passed = false;
|
||||
|
||||
/* test pub / sub latency */
|
||||
|
||||
int pubsub_task = px4_task_spawn_cmd("uorb_latency",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
1500,
|
||||
(px4_main_t)&uORBTest::UnitTest::pubsubtest_threadEntry,
|
||||
(char* const*) this);
|
||||
|
||||
/* give the test task some data */
|
||||
while (!pubsubtest_passed) {
|
||||
t.val = 308;
|
||||
t.time = hrt_absolute_time();
|
||||
if (PX4_OK != orb_publish(T, pfd0, &t))
|
||||
return test_fail("mult. pub0 timing fail");
|
||||
|
||||
/* simulate >800 Hz system operation */
|
||||
usleep(1000);
|
||||
}
|
||||
|
||||
close(pfd0);
|
||||
|
||||
if (pubsub_task < 0) {
|
||||
return test_fail("failed launching task");
|
||||
}
|
||||
|
||||
return pubsubtest_res;
|
||||
}
|
||||
|
||||
#endif // _uORBTest_UnitTest_hpp_
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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 "uORBUtils.hpp"
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
|
||||
int uORB::Utils::node_mkpath
|
||||
(
|
||||
char *buf,
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
int *instance
|
||||
)
|
||||
{
|
||||
unsigned len;
|
||||
|
||||
unsigned index = 0;
|
||||
|
||||
if (instance != nullptr) {
|
||||
index = *instance;
|
||||
}
|
||||
|
||||
len = snprintf(buf, orb_maxpath, "/%s/%s%d",
|
||||
(f == PUBSUB) ? "obj" : "param",
|
||||
meta->o_name, index);
|
||||
|
||||
if (len >= orb_maxpath) {
|
||||
return -ENAMETOOLONG;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#ifndef _uORBUtils_hpp_
|
||||
#define _uORBUtils_hpp_
|
||||
|
||||
#include "uORBCommon.hpp"
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
class Utils;
|
||||
}
|
||||
|
||||
class uORB::Utils
|
||||
{
|
||||
public:
|
||||
static int node_mkpath
|
||||
(
|
||||
char *buf,
|
||||
Flavor f,
|
||||
const struct orb_metadata *meta,
|
||||
int *instance = nullptr
|
||||
);
|
||||
};
|
||||
|
||||
#endif // _uORBUtils_hpp_
|
||||
Loading…
Reference in New Issue