diff --git a/src/modules/uORB/MuORB.cpp b/src/modules/uORB/MuORB.cpp deleted file mode 100644 index 932af53c8f..0000000000 --- a/src/modules/uORB/MuORB.cpp +++ /dev/null @@ -1,1366 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file uORB.cpp - * A lightweight object broker. - */ - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include - -#include "uORB.h" - -/** - * Utility functions. - */ -namespace -{ - -/* internal use only */ -static const unsigned orb_maxpath = 64; - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -enum Flavor { - PUBSUB, - PARAM -}; - -struct orb_advertdata { - const struct orb_metadata *meta; - int *instance; - int priority; -}; - -int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) -{ - 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 PX4_OK; -} - -} - -/** - * Per-object device instance. - */ -class ORBDevNode : public device::VDev -{ -public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); - ~ORBDevNode(); - - 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); -}; - -ORBDevNode::SubscriberData *ORBDevNode::filp_to_sd(device::file_t *filp) -{ - ORBDevNode::SubscriberData *sd; - if (filp) { - sd = (ORBDevNode::SubscriberData *)(filp->priv); - } - else { - sd = 0; - } - return sd; -} - -ORBDevNode::ORBDevNode(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; -} - -ORBDevNode::~ORBDevNode() -{ - if (_data != nullptr) - delete[] _data; - -} - -int -ORBDevNode::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("ORBDevNode::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 -ORBDevNode::close(device::file_t *filp) -{ - //warnx("ORBDevNode::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 -ORBDevNode::read(device::file_t *filp, char *buffer, size_t buflen) -{ - //warnx("ORBDevNode::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 -ORBDevNode::write(device::file_t *filp, const char *buffer, size_t buflen) -{ - //warnx("ORBDevNode::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 -ORBDevNode::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - //warnx("ORBDevNode::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 -ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) -{ - //warnx("ORBDevNode::publish meta = %p", meta); - - if (handle < 0) { - warnx("ORBDevNode::publish called with invalid handle"); - errno = EINVAL; - return ERROR; - } - - ORBDevNode *devnode = (ORBDevNode *)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 -ORBDevNode::poll_state(device::file_t *filp) -{ - //warnx("ORBDevNode::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 -ORBDevNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) -{ - //warnx("ORBDevNode::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 -ORBDevNode::appears_updated(SubscriberData *sd) -{ - //warnx("ORBDevNode::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, - &ORBDevNode::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 -ORBDevNode::update_deferred() -{ - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); -} - -void -ORBDevNode::update_deferred_trampoline(void *arg) -{ - ORBDevNode *node = (ORBDevNode *)arg; - - node->update_deferred(); -} - -/** - * Master control device for ObjDev. - * - * Used primarily to create new objects via the ORBIOCCREATE - * ioctl. - */ -class ORBDevMaster : public device::VDev -{ -public: - ORBDevMaster(Flavor f); - ~ORBDevMaster(); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); -private: - Flavor _flavor; -}; - -ORBDevMaster::ORBDevMaster(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; - -} - -ORBDevMaster::~ORBDevMaster() -{ -} - -int -ORBDevMaster::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]; - ORBDevNode *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 = 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 ORBDevNode(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); - } -} - - -/** - * Local functions in support of the shell command. - */ - -namespace -{ - -ORBDevMaster *g_dev; -bool pubsubtest_passed = false; -bool pubsubtest_print = true; -int pubsubtest_res = PX4_OK; - -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); - -int -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 ERROR; -} - -int -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 PX4_OK; -} - -int 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 ERROR; - } - - for (unsigned i = 0; i < maxruns; i++) { - fprintf(f, "%u\n", timings[i]); - } - - fclose(f); - } - - delete[] timings; - - warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); - - pubsubtest_passed = true; - - if (static_cast(latency_integral / maxruns) > 30.0f) { - pubsubtest_res = ERROR; - } else { - pubsubtest_res = PX4_OK; - } - - return pubsubtest_res; -} - -template int latency_test(orb_id_t T, bool print); - -int -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(ORB_ID(orb_test), false)) - return test_fail("latency test failed"); - - return test_note("PASS"); -} - -template int -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)&pubsublatency_main, - nullptr); - - /* 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; -} - -int -info() -{ - return PX4_OK; -} - - -} // namespace - -static void usage() -{ - warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); -} - -/* - * uORB server 'main'. - */ -extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } - -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 ORBDevMaster(PUBSUB); - - if (g_dev == nullptr) { - warnx("driver alloc failed"); - return -ENOMEM; - } - - if (PX4_OK != g_dev->init()) { - warnx("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -EIO; - } - - return PX4_OK; - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - return test(); - - /* - * Test the latency. - */ - if (!strcmp(argv[1], "latency_test")) { - - if (argc > 2 && !strcmp(argv[2], "medium")) { - return latency_test(ORB_ID(orb_test_medium), true); - } else if (argc > 2 && !strcmp(argv[2], "large")) { - return latency_test(ORB_ID(orb_test_large), true); - } else { - return latency_test(ORB_ID(orb_test), true); - } - } - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) - return info(); - - usage(); - return -EINVAL; -} - -/* - * Library functions. - */ -namespace -{ - -/** - * 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) -{ - 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; -} - -/** - * 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) -{ - 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 = 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 = 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 PX4_OK, we can return the handle now */ - return fd; -} - -} // namespace - -int -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 = node_mkpath(path, PUBSUB, meta, &inst); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - - struct stat buffer; - return stat(path, &buffer); -} - -orb_advert_t -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 -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 -orb_subscribe(const struct orb_metadata *meta) -{ - return node_open(PUBSUB, meta, nullptr, false); -} - -int -orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) -{ - int inst = instance; - return node_open(PUBSUB, meta, nullptr, false, &inst); -} - -int -orb_unsubscribe(int fd) -{ - return px4_close(fd); -} - -int -orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) -{ - return ORBDevNode::publish(meta, handle, data); -} - -int -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 -orb_check(int handle, bool *updated) -{ - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); -} - -int -orb_stat(int handle, uint64_t *time) -{ - return px4_ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); -} - -int -orb_priority(int handle, int *priority) -{ - return px4_ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); -} - -int -orb_set_interval(int handle, unsigned interval) -{ - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); -} - diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 82fb9a2c65..88f1d6807a 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -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 diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index a878853dff..c1266552fe 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -36,1281 +36,265 @@ * A lightweight object broker. */ -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - #include "uORB.h" +#include "uORBManager.hpp" +#include "uORBCommon.hpp" /** - * Utility functions. - */ -namespace -{ - -/* internal use only */ -static const unsigned orb_maxpath = 64; - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -enum Flavor { - PUBSUB, - PARAM -}; - -struct orb_advertdata { - const struct orb_metadata *meta; - int *instance; - int priority; -}; - -int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) -{ - 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; -} - -} - -/** - * Per-object device instance. - */ -class ORBDevNode : public device::CDev -{ -public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); - ~ORBDevNode(); - - virtual int open(struct file *filp); - virtual int close(struct file *filp); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); - virtual int ioctl(struct file *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(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 */ - - 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); -}; - -ORBDevNode::ORBDevNode(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; -} - -ORBDevNode::~ORBDevNode() -{ - if (_data != nullptr) - delete[] _data; - -} - -int -ORBDevNode::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 -ORBDevNode::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 -ORBDevNode::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 -ORBDevNode::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 -ORBDevNode::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 -ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) -{ - ORBDevNode *devnode = (ORBDevNode *)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 OK; -} - -pollevent_t -ORBDevNode::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 -ORBDevNode::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 -ORBDevNode::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, - &ORBDevNode::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 -ORBDevNode::update_deferred() -{ - /* - * Instigate a poll notification; any subscribers whose intervals have - * expired will be woken. - */ - poll_notify(POLLIN); -} - -void -ORBDevNode::update_deferred_trampoline(void *arg) -{ - ORBDevNode *node = (ORBDevNode *)arg; - - node->update_deferred(); -} - -/** - * Master control device for ObjDev. + * Advertise as the publisher of a topic. * - * Used primarily to create new objects via the ORBIOCCREATE - * ioctl. - */ -class ORBDevMaster : public device::CDev -{ -public: - ORBDevMaster(Flavor f); - ~ORBDevMaster(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); -private: - Flavor _flavor; -}; - -ORBDevMaster::ORBDevMaster(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; - -} - -ORBDevMaster::~ORBDevMaster() -{ -} - -int -ORBDevMaster::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]; - ORBDevNode *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 = 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 ORBDevNode(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); - } -} - - -/** - * Local functions in support of the shell command. - */ - -namespace -{ - -ORBDevMaster *g_dev; -bool pubsubtest_passed = false; -bool pubsubtest_print = false; -int pubsubtest_res = OK; - -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); - -int -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 ERROR; -} - -int -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 pubsublatency_main(void) -{ - /* poll on test topic and output latency */ - float latency_integral = 0.0f; - - /* wakeup source(s) */ - struct pollfd 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 = 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); - FILE *f = fopen(fname, "w"); - if (f == NULL) { - warnx("Error opening file!\n"); - return ERROR; - } - - for (unsigned i = 0; i < maxruns; i++) { - fprintf(f, "%u\n", timings[i]); - } - - fclose(f); - } - - delete[] timings; - - warnx("mean: %8.4f", static_cast(latency_integral / maxruns)); - - pubsubtest_passed = true; - - if (static_cast(latency_integral / maxruns) > 30.0f) { - pubsubtest_res = ERROR; - } else { - pubsubtest_res = OK; - } - - return pubsubtest_res; -} - -template int latency_test(orb_id_t T, bool print); - -int -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 (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 (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 (OK != orb_publish(ORB_ID(orb_test), pfd, &t)) - return test_fail("publish failed"); - - if (OK != orb_check(sfd, &updated)) - return test_fail("check(2) failed"); - - if (!updated) - return test_fail("missing updated flag"); - - if (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 (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) - return test_fail("mult. pub0 fail"); - - test_note("published"); - - t.val = 203; - if (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 (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 (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 (OK != orb_priority(sfd0, &prio)) - return test_fail("prio #0"); - - if (prio != ORB_PRIO_MAX) - return test_fail("prio: %d", prio); - - if (OK != orb_priority(sfd1, &prio)) - return test_fail("prio #1"); - - if (prio != ORB_PRIO_MIN) - return test_fail("prio: %d", prio); - - if (OK != latency_test(ORB_ID(orb_test), false)) - return test_fail("latency test failed"); - - return test_note("PASS"); -} - -template int -latency_test(orb_id_t T, bool print) -{ - 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 = task_spawn_cmd("uorb_latency", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (main_t)&pubsublatency_main, - nullptr); - - /* give the test task some data */ - while (!pubsubtest_passed) { - t.val = 308; - t.time = hrt_absolute_time(); - if (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; -} - -int -info() -{ - return OK; -} - - -} // namespace - -/* - * uORB server 'main'. - */ -extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } - -int -uorb_main(int argc, char *argv[]) -{ - /* - * 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 ORBDevMaster(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")) - return test(); - - /* - * Test the latency. - */ - if (!strcmp(argv[1], "latency_test")) { - - if (argc > 2 && !strcmp(argv[2], "medium")) { - return latency_test(ORB_ID(orb_test_medium), true); - } else if (argc > 2 && !strcmp(argv[2], "large")) { - return latency_test(ORB_ID(orb_test_large), true); - } else { - return latency_test(ORB_ID(orb_test), true); - } - } - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) - return info(); - - errx(-EINVAL, "unrecognized command, try 'start', 'test', 'latency_test' or 'status'"); -} - -/* - * Library functions. - */ -namespace -{ - -/** - * Advertise a node; don't consider it an error if the node has - * already been advertised. + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. * - * @todo verify that the existing node is the same as the one - * we tried to advertise. + * 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. */ -int -node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - 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; + return uORB::Manager::get_instance()->orb_advertise( meta, data ); } /** - * Common implementation for orb_advertise and orb_subscribe. + * Advertise as the publisher of a topic. * - * Handles creation of the object and the initial publication for - * advertisers. + * 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. */ -int -node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) +orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, 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 = 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 = 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; + return uORB::Manager::get_instance()->orb_advertise_multi( meta, data, instance, priority ); } -} // namespace -int -orb_exists(const struct orb_metadata *meta, int instance) +/** + * 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) { - /* - * Generate the path to the node and try to open it. - */ - char path[orb_maxpath]; - int inst = instance; - int ret = node_mkpath(path, PUBSUB, meta, &inst); - - if (ret != OK) { - errno = -ret; - return ERROR; - } - - struct stat buffer; - return stat(path, &buffer); + return uORB::Manager::get_instance()->orb_publish( meta, handle, data ); } -orb_advert_t -orb_advertise(const struct orb_metadata *meta, 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) { - return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); + return uORB::Manager::get_instance()->orb_subscribe( meta ); } -orb_advert_t -orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) +/** + * 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) { - 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; + return uORB::Manager::get_instance()->orb_subscribe_multi( meta, instance ); } -int -orb_subscribe(const struct orb_metadata *meta) +/** + * 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) { - return node_open(PUBSUB, meta, nullptr, false); + return uORB::Manager::get_instance()->orb_unsubscribe( handle ); } -int -orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +/** + * 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) { - int inst = instance; - return node_open(PUBSUB, meta, nullptr, false, &inst); + return uORB::Manager::get_instance()->orb_copy( meta, handle, buffer ); } -int -orb_unsubscribe(int handle) +/** + * 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 close(handle); + return uORB::Manager::get_instance()->orb_check( handle, updated ); } -int -orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +/** + * 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) { - return ORBDevNode::publish(meta, handle, data); + return uORB::Manager::get_instance()->orb_stat( handle, time ); } -int -orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +/** + * 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) { - 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; + return uORB::Manager::get_instance()->orb_exists( meta, instance ); } -int -orb_check(int handle, bool *updated) +/** + * 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) { - return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + return uORB::Manager::get_instance()->orb_priority( handle, priority ); } -int -orb_stat(int handle, uint64_t *time) +/** + * 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) { - return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); -} - -int -orb_priority(int handle, int *priority) -{ - return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); -} - -int -orb_set_interval(int handle, unsigned interval) -{ - return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); + return uORB::Manager::get_instance()->orb_set_interval( handle, interval ); } diff --git a/src/modules/uORB/uORBCommon.hpp b/src/modules/uORB/uORBCommon.hpp new file mode 100644 index 0000000000..06f731e823 --- /dev/null +++ b/src/modules/uORB/uORBCommon.hpp @@ -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 +#include +#include +#include "uORB.h" +#include + + +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_ diff --git a/src/modules/uORB/uORBDevices.hpp b/src/modules/uORB/uORBDevices.hpp new file mode 100644 index 0000000000..52f4ac6e8f --- /dev/null +++ b/src/modules/uORB/uORBDevices.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 diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp new file mode 100644 index 0000000000..d3c30f0508 --- /dev/null +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -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 +#include +#include +#include +#include +#include "uORBDevices_nuttx.hpp" +#include "uORBUtils.hpp" +#include + +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); + } +} diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp new file mode 100644 index 0000000000..3d8c0f90ad --- /dev/null +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -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 +#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 diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp new file mode 100644 index 0000000000..e3530926e1 --- /dev/null +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -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 +#include +#include +#include + +#include "uORBDevices_posix.hpp" +#include "uORBUtils.hpp" +#include + + +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); + } +} + diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp new file mode 100644 index 0000000000..ce22b84321 --- /dev/null +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -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 +#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 */ diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp new file mode 100644 index 0000000000..f144ac8e27 --- /dev/null +++ b/src/modules/uORB/uORBMain.cpp @@ -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 +#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(ORB_ID(orb_test_medium), true); + } else if (argc > 2 && !strcmp(argv[2], "large")) { + return t.latency_test(ORB_ID(orb_test_large), true); + } else { + return t.latency_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'"); +} + + diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp new file mode 100644 index 0000000000..e1a94a5e0f --- /dev/null +++ b/src/modules/uORB/uORBManager.hpp @@ -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 + +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_ */ diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp new file mode 100644 index 0000000000..85746c0c27 --- /dev/null +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -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 +#include +#include +#include +#include +#include +#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; +} diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp new file mode 100644 index 0000000000..a9dad24dcd --- /dev/null +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -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 +#include +#include +#include +#include +#include +#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; +} diff --git a/src/modules/uORB/uORBTest_UnitTest.cpp b/src/modules/uORB/uORBTest_UnitTest.cpp new file mode 100644 index 0000000000..6fd960352f --- /dev/null +++ b/src/modules/uORB/uORBTest_UnitTest.cpp @@ -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 +#include + +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(latency_integral / maxruns)); + + pubsubtest_passed = true; + + if (static_cast(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(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; +} diff --git a/src/modules/uORB/uORBTest_UnitTest.hpp b/src/modules/uORB/uORBTest_UnitTest.hpp new file mode 100644 index 0000000000..bbfa895fa6 --- /dev/null +++ b/src/modules/uORB/uORBTest_UnitTest.hpp @@ -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 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 +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_ diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp new file mode 100644 index 0000000000..c9d49222bc --- /dev/null +++ b/src/modules/uORB/uORBUtils.cpp @@ -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 +#include + +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; +} diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp new file mode 100644 index 0000000000..dc2fb9b78e --- /dev/null +++ b/src/modules/uORB/uORBUtils.hpp @@ -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_