fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 10:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

uORB move to PX4 platform layer

Browse Source
This commit is contained in:
Daniel Agar
2021-02-17 11:25:56 -05:00
committed by GitHub
parent 6bbb2faf8a
commit ab0d0fd0be
156 changed files with 261 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/events/CMakeLists.txt
-2
View File
@@ -40,6 +40,4 @@ px4_add_module(
send_event.cpp
set_leds.cpp
status_display.cpp
DEPENDS
modules__uORB
)
src/modules/muorb/adsp/CMakeLists.txt
+1 -1
View File
@@ -35,4 +35,4 @@ px4_add_library(modules__muorb__adsp
px4muorb.cpp
uORBFastRpcChannel.cpp
)
target_include_directories(modules__muorb__adsp PRIVATE ${PX4_SOURCE_DIR}/src/modules/uORB)
target_include_directories(modules__muorb__adsp PRIVATE ${PX4_SOURCE_DIR}/platforms/common/uORB)
src/modules/uORB/CMakeLists.txt
-74
View File
@@ -1,74 +0,0 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
if(NOT PX4_BOARD MATCHES "px4_io") # TODO: fix this hack (move uORB to platform layer)
# this includes the generated topics directory
include_directories(${CMAKE_CURRENT_BINARY_DIR})
px4_add_module(
MODULE modules__uORB
MAIN uorb
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
ORBSet.hpp
Publication.hpp
PublicationMulti.hpp
Subscription.cpp
Subscription.hpp
SubscriptionCallback.hpp
SubscriptionInterval.hpp
SubscriptionMultiArray.hpp
uORB.cpp
uORB.h
uORBCommon.hpp
uORBCommunicator.hpp
uORBDeviceMaster.cpp
uORBDeviceMaster.hpp
uORBDeviceNode.cpp
uORBDeviceNode.hpp
uORBMain.cpp
uORBManager.cpp
uORBManager.hpp
uORBUtils.cpp
uORBUtils.hpp
DEPENDS
cdev
uorb_msgs
)
if(PX4_TESTING)
add_subdirectory(uORB_tests)
endif()
endif()
src/modules/uORB/ORBSet.hpp
-146
View File
@@ -1,146 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. 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
class ORBSet
{
public:
struct Node {
struct Node *next;
const char *node_name;
};
ORBSet() :
_top(nullptr),
_end(nullptr)
{ }
~ORBSet()
{
while (_top != nullptr) {
unlinkNext(_top);
if (_top->next == nullptr) {
free((void *)_top->node_name);
free(_top);
_top = nullptr;
}
}
}
void insert(const char *node_name)
{
Node **p;
if (_top == nullptr) {
p = &_top;
} else {
p = &_end->next;
}
*p = (Node *)malloc(sizeof(Node));
if (_end) {
_end = _end->next;
} else {
_end = _top;
}
_end->next = nullptr;
_end->node_name = strdup(node_name);
}
bool find(const char *node_name)
{
Node *p = _top;
while (p) {
if (strcmp(p->node_name, node_name) == 0) {
return true;
}
p = p->next;
}
return false;
}
bool erase(const char *node_name)
{
Node *p = _top;
if (_top && (strcmp(_top->node_name, node_name) == 0)) {
p = _top->next;
free((void *)_top->node_name);
free(_top);
_top = p;
if (_top == nullptr) {
_end = nullptr;
}
return true;
}
while (p->next) {
if (strcmp(p->next->node_name, node_name) == 0) {
unlinkNext(p);
return true;
}
}
return false;
}
private:
void unlinkNext(Node *a)
{
Node *b = a->next;
if (b != nullptr) {
if (_end == b) {
_end = a;
}
a->next = b->next;
free((void *)b->node_name);
free(b);
}
}
Node *_top;
Node *_end;
};
src/modules/uORB/Publication.hpp
-166
View File
@@ -1,166 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Publication.hpp
*
*/
#pragma once
#include <px4_platform_common/defines.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include "uORBDeviceNode.hpp"
#include <uORB/topics/uORBTopics.hpp>
namespace uORB
{
template <typename U> class DefaultQueueSize
{
private:
template <typename T>
static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *)
{
return T::ORB_QUEUE_LENGTH;
}
template <typename T> static constexpr uint8_t get_queue_size(...)
{
return 1;
}
public:
static constexpr unsigned value = get_queue_size<U>(nullptr);
};
class PublicationBase
{
public:
bool advertised() const { return _handle != nullptr; }
bool unadvertise() { return (DeviceNode::unadvertise(_handle) == PX4_OK); }
orb_id_t get_topic() const { return get_orb_meta(_orb_id); }
protected:
PublicationBase(ORB_ID id) : _orb_id(id) {}
~PublicationBase()
{
if (_handle != nullptr) {
// don't automatically unadvertise queued publications (eg vehicle_command)
if (static_cast<DeviceNode *>(_handle)->get_queue_size() == 1) {
unadvertise();
}
}
}
orb_advert_t _handle{nullptr};
const ORB_ID _orb_id;
};
/**
* uORB publication wrapper class
*/
template<typename T, uint8_t ORB_QSIZE = DefaultQueueSize<T>::value>
class Publication : public PublicationBase
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
*/
Publication(ORB_ID id) : PublicationBase(id) {}
Publication(const orb_metadata *meta) : PublicationBase(static_cast<ORB_ID>(meta->o_id)) {}
bool advertise()
{
if (!advertised()) {
_handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE);
}
return advertised();
}
/**
* Publish the struct
* @param data The uORB message struct we are updating.
*/
bool publish(const T &data)
{
if (!advertised()) {
advertise();
}
return (DeviceNode::publish(get_topic(), _handle, &data) == PX4_OK);
}
};
/**
* The publication class with data embedded.
*/
template<typename T>
class PublicationData : public Publication<T>
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
*/
PublicationData(ORB_ID id) : Publication<T>(id) {}
PublicationData(const orb_metadata *meta) : Publication<T>(meta) {}
T &get() { return _data; }
void set(const T &data) { _data = data; }
// Publishes the embedded struct.
bool update() { return Publication<T>::publish(_data); }
bool update(const T &data)
{
_data = data;
return Publication<T>::publish(_data);
}
private:
T _data{};
};
} // namespace uORB
src/modules/uORB/PublicationMulti.hpp
-135
View File
@@ -1,135 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Publication.hpp
*
*/
#pragma once
#include <px4_platform_common/defines.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include "uORBDeviceNode.hpp"
#include "Publication.hpp"
namespace uORB
{
/**
* Base publication multi wrapper class
*/
template<typename T, uint8_t QSIZE = DefaultQueueSize<T>::value>
class PublicationMulti : public PublicationBase
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
*/
PublicationMulti(ORB_ID id) :
PublicationBase(id)
{}
PublicationMulti(const orb_metadata *meta) :
PublicationBase(static_cast<ORB_ID>(meta->o_id))
{}
bool advertise()
{
if (!advertised()) {
int instance = 0;
_handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE);
}
return advertised();
}
/**
* Publish the struct
* @param data The uORB message struct we are updating.
*/
bool publish(const T &data)
{
if (!advertised()) {
advertise();
}
return (orb_publish(get_topic(), _handle, &data) == PX4_OK);
}
int get_instance() const
{
if (_handle) {
return static_cast<uORB::DeviceNode *>(_handle)->get_instance();
}
return -1;
}
};
/**
* The publication multi class with data embedded.
*/
template<typename T>
class PublicationMultiData : public PublicationMulti<T>
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
*/
PublicationMultiData(ORB_ID id) : PublicationMulti<T>(id) {}
PublicationMultiData(const orb_metadata *meta) : PublicationMulti<T>(meta) {}
T &get() { return _data; }
void set(const T &data) { _data = data; }
// Publishes the embedded struct.
bool update() { return PublicationMulti<T>::publish(_data); }
bool update(const T &data)
{
_data = data;
return PublicationMulti<T>::publish(_data);
}
private:
T _data{};
};
} // namespace uORB
src/modules/uORB/Subscription.cpp
-113
View File
@@ -1,113 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Subscription.cpp
*
*/
#include "Subscription.hpp"
#include <px4_platform_common/defines.h>
namespace uORB
{
bool Subscription::subscribe()
{
// check if already subscribed
if (_node != nullptr) {
return true;
}
if (_orb_id != ORB_ID::INVALID) {
DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
if (device_master != nullptr) {
if (!device_master->deviceNodeExists(_orb_id, _instance)) {
return false;
}
uORB::DeviceNode *node = device_master->getDeviceNode(get_topic(), _instance);
if (node != nullptr) {
_node = node;
_node->add_internal_subscriber();
_last_generation = _node->get_initial_generation();
return true;
}
}
}
return false;
}
void Subscription::unsubscribe()
{
if (_node != nullptr) {
_node->remove_internal_subscriber();
}
_node = nullptr;
_last_generation = 0;
}
bool Subscription::ChangeInstance(uint8_t instance)
{
if (instance != _instance) {
DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
if (device_master != nullptr) {
if (!device_master->deviceNodeExists(_orb_id, _instance)) {
return false;
}
// if desired new instance exists, unsubscribe from current
unsubscribe();
_instance = instance;
subscribe();
return true;
}
} else {
// already on desired index
return true;
}
return false;
}
} // namespace uORB
src/modules/uORB/Subscription.hpp
-226
View File
@@ -1,226 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Subscription.hpp
*
*/
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/uORBTopics.hpp>
#include <px4_platform_common/defines.h>
#include <lib/mathlib/mathlib.h>
#include "uORBDeviceNode.hpp"
#include "uORBManager.hpp"
#include "uORBUtils.hpp"
namespace uORB
{
class SubscriptionCallback;
// Base subscription wrapper class
class Subscription
{
public:
/**
* Constructor
*
* @param id The uORB ORB_ID enum for the topic.
* @param instance The instance for multi sub.
*/
Subscription(ORB_ID id, uint8_t instance = 0) :
_orb_id(id),
_instance(instance)
{
subscribe();
}
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param instance The instance for multi sub.
*/
Subscription(const orb_metadata *meta, uint8_t instance = 0) :
_orb_id((meta == nullptr) ? ORB_ID::INVALID : static_cast<ORB_ID>(meta->o_id)),
_instance(instance)
{
subscribe();
}
// Copy constructor
Subscription(const Subscription &other) : _orb_id(other._orb_id), _instance(other._instance) {}
// Move constructor
Subscription(const Subscription &&other) noexcept : _orb_id(other._orb_id), _instance(other._instance) {}
// copy assignment
Subscription &operator=(const Subscription &other)
{
unsubscribe();
_orb_id = other._orb_id;
_instance = other._instance;
return *this;
}
// move assignment
Subscription &operator=(Subscription &&other) noexcept
{
unsubscribe();
_orb_id = other._orb_id;
_instance = other._instance;
return *this;
}
~Subscription()
{
unsubscribe();
}
bool subscribe();
void unsubscribe();
bool valid() const { return _node != nullptr; }
bool advertised()
{
if (valid()) {
return _node->is_advertised();
}
// try to initialize
if (subscribe()) {
// check again if valid
if (valid()) {
return _node->is_advertised();
}
}
return false;
}
/**
* Check if there is a new update.
*/
bool updated() { return advertised() && _node->updates_available(_last_generation); }
/**
* Update the struct
* @param dst The uORB message struct we are updating.
*/
bool update(void *dst) { return updated() && _node->copy(dst, _last_generation); }
/**
* Copy the struct
* @param dst The uORB message struct we are updating.
*/
bool copy(void *dst) { return advertised() && _node->copy(dst, _last_generation); }
/**
* Change subscription instance
* @param instance The new multi-Subscription instance
*/
bool ChangeInstance(uint8_t instance);
uint8_t get_instance() const { return _instance; }
unsigned get_last_generation() const { return _last_generation; }
orb_id_t get_topic() const { return get_orb_meta(_orb_id); }
protected:
friend class SubscriptionCallback;
friend class SubscriptionCallbackWorkItem;
DeviceNode *get_node() { return _node; }
DeviceNode *_node{nullptr};
unsigned _last_generation{0}; /**< last generation the subscriber has seen */
ORB_ID _orb_id{ORB_ID::INVALID};
uint8_t _instance{0};
};
// Subscription wrapper class with data
template<class T>
class SubscriptionData : public Subscription
{
public:
/**
* Constructor
*
* @param id The uORB metadata ORB_ID enum for the topic.
* @param instance The instance for multi sub.
*/
SubscriptionData(ORB_ID id, uint8_t instance = 0) :
Subscription(id, instance)
{
copy(&_data);
}
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param instance The instance for multi sub.
*/
SubscriptionData(const orb_metadata *meta, uint8_t instance = 0) :
Subscription(meta, instance)
{
copy(&_data);
}
~SubscriptionData() = default;
// no copy, assignment, move, move assignment
SubscriptionData(const SubscriptionData &) = delete;
SubscriptionData &operator=(const SubscriptionData &) = delete;
SubscriptionData(SubscriptionData &&) = delete;
SubscriptionData &operator=(SubscriptionData &&) = delete;
// update the embedded struct.
bool update() { return Subscription::update((void *)(&_data)); }
const T &get() const { return _data; }
private:
T _data{};
};
} // namespace uORB
src/modules/uORB/SubscriptionBlocking.hpp
-171
View File
@@ -1,171 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "SubscriptionCallback.hpp"
#include <containers/LockGuard.hpp>
#include <px4_time.h>
namespace uORB
{
// Subscription with callback that schedules a WorkItem
template<typename T>
class SubscriptionBlocking : public SubscriptionCallback
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param interval_us The requested maximum update interval in microseconds.
* @param instance The instance for multi sub.
*/
SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
SubscriptionCallback(meta, interval_us, instance)
{
// pthread_mutexattr_init
pthread_mutexattr_t attr;
int ret_attr_init = pthread_mutexattr_init(&attr);
if (ret_attr_init != 0) {
PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
}
#if defined(PTHREAD_PRIO_NONE)
// pthread_mutexattr_settype
// PTHREAD_PRIO_NONE not available on cygwin
int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
if (ret_mutexattr_settype != 0) {
PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
}
#endif // PTHREAD_PRIO_NONE
// pthread_mutex_init
int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
if (ret_mutex_init != 0) {
PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init);
}
}
virtual ~SubscriptionBlocking()
{
pthread_mutex_destroy(&_mutex);
pthread_cond_destroy(&_cv);
}
void call() override
{
// signal immediately if no interval, otherwise only if interval has elapsed
if ((_interval_us == 0) || (hrt_elapsed_time(&_last_update) >= _interval_us)) {
pthread_cond_signal(&_cv);
}
}
/**
* Block until updated.
* @param timeout_us The requested timeout in microseconds, or 0 to wait indefinitely.
*
* @return true only if topic was updated
*/
bool updatedBlocking(uint32_t timeout_us = 0)
{
if (!_registered) {
registerCallback();
}
if (updated()) {
// return immediately if updated
return true;
} else {
// otherwise wait
LockGuard lg{_mutex};
if (timeout_us == 0) {
// wait with no timeout
if (pthread_cond_wait(&_cv, &_mutex) == 0) {
return updated();
}
} else {
// otherwise wait with timeout based on interval
// Calculate an absolute time in the future
struct timespec ts;
px4_clock_gettime(CLOCK_REALTIME, &ts);
uint64_t nsecs = ts.tv_nsec + (timeout_us * 1000);
static constexpr unsigned billion = (1000 * 1000 * 1000);
ts.tv_sec += nsecs / billion;
nsecs -= (nsecs / billion) * billion;
ts.tv_nsec = nsecs;
if (px4_pthread_cond_timedwait(&_cv, &_mutex, &ts) == 0) {
return updated();
}
}
}
return false;
}
/**
* Copy the struct if updated.
* @param data The data reference where the struct will be copied.
* @param timeout_us The requested timeout in microseconds, or 0 to wait indefinitely.
*
* @return true only if topic was updated and copied successfully.
*/
bool updateBlocking(T &data, uint32_t timeout_us = 0)
{
if (updatedBlocking(timeout_us)) {
return copy(&data);
}
return false;
}
private:
pthread_mutex_t _mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t _cv = PTHREAD_COND_INITIALIZER;
};
} // namespace uORB
src/modules/uORB/SubscriptionCallback.hpp
-191
View File
@@ -1,191 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file SubscriptionCallback.hpp
*
*/
#pragma once
#include <uORB/SubscriptionInterval.hpp>
#include <containers/List.hpp>
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
namespace uORB
{
// Subscription wrapper class with callbacks on new publications
class SubscriptionCallback : public SubscriptionInterval, public ListNode<SubscriptionCallback *>
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param interval_us The requested maximum update interval in microseconds.
* @param instance The instance for multi sub.
*/
SubscriptionCallback(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
SubscriptionInterval(meta, interval_us, instance)
{
}
virtual ~SubscriptionCallback()
{
unregisterCallback();
};
bool registerCallback()
{
if (!_registered) {
if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
// registered
_registered = true;
} else {
// force topic creation by subscribing with old API
int fd = orb_subscribe_multi(_subscription.get_topic(), _subscription.get_instance());
// try to register callback again
if (_subscription.subscribe()) {
if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
_registered = true;
}
}
orb_unsubscribe(fd);
}
}
return _registered;
}
void unregisterCallback()
{
if (_subscription.get_node()) {
_subscription.get_node()->unregister_callback(this);
}
_registered = false;
}
/**
* Change subscription instance
* @param instance The new multi-Subscription instance
*/
bool ChangeInstance(uint8_t instance)
{
bool ret = false;
if (instance != get_instance()) {
const bool registered = _registered;
if (registered) {
unregisterCallback();
}
if (_subscription.ChangeInstance(instance)) {
ret = true;
}
if (registered) {
registerCallback();
}
} else {
// already on desired index
return true;
}
return ret;
}
virtual void call() = 0;
bool registered() const { return _registered; }
protected:
bool _registered{false};
};
// Subscription with callback that schedules a WorkItem
class SubscriptionCallbackWorkItem : public SubscriptionCallback
{
public:
/**
* Constructor
*
* @param work_item The WorkItem that will be scheduled immediately on new publications.
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param instance The instance for multi sub.
*/
SubscriptionCallbackWorkItem(px4::WorkItem *work_item, const orb_metadata *meta, uint8_t instance = 0) :
SubscriptionCallback(meta, 0, instance), // interval 0
_work_item(work_item)
{
}
virtual ~SubscriptionCallbackWorkItem() = default;
void call() override
{
// schedule immediately if updated (queue depth or subscription interval)
if ((_required_updates == 0)
|| (_subscription.get_node()->updates_available(_subscription.get_last_generation()) >= _required_updates)) {
if (updated()) {
_work_item->ScheduleNow();
}
}
}
/**
* Optionally limit callback until more samples are available.
*
* @param required_updates Number of queued updates required before a callback can be called.
*/
void set_required_updates(uint8_t required_updates)
{
// TODO: constrain to queue depth?
_required_updates = required_updates;
}
private:
px4::WorkItem *_work_item;
uint8_t _required_updates{0};
};
} // namespace uORB
src/modules/uORB/SubscriptionInterval.hpp
-163
View File
@@ -1,163 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file SubscriptionInterval.hpp
*
*/
#pragma once
#include <uORB/uORB.h>
#include <px4_platform_common/defines.h>
#include "uORBDeviceNode.hpp"
#include "uORBManager.hpp"
#include "uORBUtils.hpp"
#include "Subscription.hpp"
#include <mathlib/mathlib.h>
namespace uORB
{
// Base subscription wrapper class
class SubscriptionInterval
{
public:
/**
* Constructor
*
* @param id The uORB ORB_ID enum for the topic.
* @param interval The requested maximum update interval in microseconds.
* @param instance The instance for multi sub.
*/
SubscriptionInterval(ORB_ID id, uint32_t interval_us = 0, uint8_t instance = 0) :
_subscription{id, instance},
_interval_us(interval_us)
{}
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param interval The requested maximum update interval in microseconds.
* @param instance The instance for multi sub.
*/
SubscriptionInterval(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
_subscription{meta, instance},
_interval_us(interval_us)
{}
SubscriptionInterval() : _subscription{nullptr} {}
~SubscriptionInterval() = default;
bool subscribe() { return _subscription.subscribe(); }
void unsubscribe() { _subscription.unsubscribe(); }
bool advertised() { return _subscription.advertised(); }
/**
* Check if there is a new update.
* */
bool updated()
{
if (advertised() && (hrt_elapsed_time(&_last_update) >= _interval_us)) {
return _subscription.updated();
}
return false;
}
/**
* Copy the struct if updated.
* @param dst The destination pointer where the struct will be copied.
* @return true only if topic was updated and copied successfully.
*/
bool update(void *dst)
{
if (updated()) {
return copy(dst);
}
return false;
}
/**
* Copy the struct
* @param dst The destination pointer where the struct will be copied.
* @return true only if topic was copied successfully.
*/
bool copy(void *dst)
{
if (_subscription.copy(dst)) {
const hrt_abstime now = hrt_absolute_time();
// shift last update time forward, but don't let it get further behind than the interval
_last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now);
return true;
}
return false;
}
bool valid() const { return _subscription.valid(); }
uint8_t get_instance() const { return _subscription.get_instance(); }
uint32_t get_interval_us() const { return _interval_us; }
unsigned get_last_generation() const { return _subscription.get_last_generation(); }
orb_id_t get_topic() const { return _subscription.get_topic(); }
/**
* Set the interval in microseconds
* @param interval The interval in microseconds.
*/
void set_interval_us(uint32_t interval) { _interval_us = interval; }
/**
* Set the interval in milliseconds
* @param interval The interval in milliseconds.
*/
void set_interval_ms(uint32_t interval) { _interval_us = interval * 1000; }
protected:
Subscription _subscription;
uint64_t _last_update{0}; // last update in microseconds
uint32_t _interval_us{0}; // maximum update interval in microseconds
};
} // namespace uORB
src/modules/uORB/SubscriptionMultiArray.hpp
-128
View File
@@ -1,128 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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 SubscriptionMultiArray.hpp
*
*/
#pragma once
#include <uORB/uORB.h>
#include <px4_platform_common/defines.h>
#include <lib/mathlib/mathlib.h>
#include "SubscriptionInterval.hpp"
namespace uORB
{
/**
* An array of uORB::Subscriptions of the same topic
*/
template<typename T, uint8_t SIZE = ORB_MULTI_MAX_INSTANCES>
class SubscriptionMultiArray
{
public:
static_assert(SIZE <= ORB_MULTI_MAX_INSTANCES, "size must be <= uORB max instances");
static constexpr uint8_t size() { return SIZE; }
/**
* Constructor
*
* @param id The uORB ORB_ID enum for the topic.
*/
explicit SubscriptionMultiArray(ORB_ID id)
{
for (uint8_t i = 0; i < SIZE; i++) {
_subscriptions[i] = SubscriptionInterval{id, 0, i};
_subscriptions[i].subscribe();
}
}
~SubscriptionMultiArray() = default;
SubscriptionInterval &operator [](int i) { return _subscriptions[i]; }
const SubscriptionInterval &operator [](int i) const { return _subscriptions[i]; }
SubscriptionInterval *begin() { return _subscriptions; }
SubscriptionInterval *end() { return _subscriptions + SIZE; }
const SubscriptionInterval *begin() const { return _subscriptions; }
const SubscriptionInterval *end() const { return _subscriptions + SIZE; }
// true if any instance is advertised
bool advertised()
{
for (auto &s : _subscriptions) {
if (s.advertised()) {
return true;
}
}
return false;
}
// return the number of instances currently advertised
uint8_t advertised_count()
{
uint8_t count = 0;
for (auto &s : _subscriptions) {
if (s.advertised()) {
count++;
}
}
return count;
}
// true if any instance is updated
bool updated()
{
for (auto &s : _subscriptions) {
if (s.updated()) {
return true;
}
}
return false;
}
private:
SubscriptionInterval _subscriptions[SIZE];
};
} // namespace uORB
src/modules/uORB/uORB.cpp
-123
View File
@@ -1,123 +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 "uORB.h"
#include "uORBManager.hpp"
#include "uORBCommon.hpp"
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
{
return uORB::Manager::get_instance()->orb_advertise(meta, data);
}
orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
{
return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
}
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
{
return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance);
}
orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
unsigned int queue_size)
{
return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size);
}
int orb_unadvertise(orb_advert_t handle)
{
return uORB::Manager::get_instance()->orb_unadvertise(handle);
}
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
{
return uORB::Manager::get_instance()->orb_publish(meta, handle, data);
}
int orb_subscribe(const struct orb_metadata *meta)
{
return uORB::Manager::get_instance()->orb_subscribe(meta);
}
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
{
return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance);
}
int orb_unsubscribe(int handle)
{
return uORB::Manager::get_instance()->orb_unsubscribe(handle);
}
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
{
return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer);
}
int orb_check(int handle, bool *updated)
{
return uORB::Manager::get_instance()->orb_check(handle, updated);
}
int orb_exists(const struct orb_metadata *meta, int instance)
{
return uORB::Manager::get_instance()->orb_exists(meta, instance);
}
int orb_group_count(const struct orb_metadata *meta)
{
unsigned instance = 0;
while (uORB::Manager::get_instance()->orb_exists(meta, instance) == OK) {
++instance;
};
return instance;
}
int orb_set_interval(int handle, unsigned interval)
{
return uORB::Manager::get_instance()->orb_set_interval(handle, interval);
}
int orb_get_interval(int handle, unsigned *interval)
{
return uORB::Manager::get_instance()->orb_get_interval(handle, interval);
}
src/modules/uORB/uORB.h
-244
View File
@@ -1,244 +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.
*
****************************************************************************/
#ifndef _UORB_UORB_H
#define _UORB_UORB_H
/**
* @file uORB.h
* API for the uORB lightweight object broker.
*/
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
/**
* Object metadata.
*/
struct orb_metadata {
const char *o_name; /**< unique object name */
const uint16_t o_size; /**< object size */
const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
const char *o_fields; /**< semicolon separated list of fields (with type) */
uint8_t o_id; /**< ORB_ID enum */
};
typedef const struct orb_metadata *orb_id_t;
/**
* Maximum number of multi topic instances. This must be <= 10 (because it's the last char of the node path)
*/
#if defined(CONSTRAINED_MEMORY)
# define ORB_MULTI_MAX_INSTANCES 4
#else
# define ORB_MULTI_MAX_INSTANCES 10
#endif
/**
* Generates a pointer to the uORB metadata structure for
* a given topic.
*
* The topic must have been declared previously in scope
* with ORB_DECLARE().
*
* @param _name The name of the topic.
*/
#define ORB_ID(_name) &__orb_##_name
/**
* Declare (prototype) the uORB metadata for a topic (used by code generators).
*
* @param _name The name of the topic.
*/
#if defined(__cplusplus)
# define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT
#else
# define ORB_DECLARE(_name) extern const struct orb_metadata __orb_##_name __EXPORT
#endif
/**
* Define (instantiate) the uORB metadata for a topic.
*
* The uORB metadata is used to help ensure that updates and
* copies are accessing the right data.
*
* Note that there must be no more than one instance of this macro
* for each topic.
*
* @param _name The name of the topic.
* @param _struct The structure the topic provides.
* @param _size_no_padding Struct size w/o padding at the end
* @param _fields All fields in a semicolon separated list e.g: "float[3] position;bool armed"
* @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status
*/
#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum) \
const struct orb_metadata __orb_##_name = { \
#_name, \
sizeof(_struct), \
_size_no_padding, \
_fields, \
_orb_id_enum \
}; struct hack
__BEGIN_DECLS
/**
* ORB topic advertiser handle.
*
* Advertiser handles are global; once obtained they can be shared freely
* and do not need to be closed or released.
*
* This permits publication from interrupt context and other contexts where
* a file-descriptor-based handle would not otherwise be in scope for the
* publisher.
*/
typedef void *orb_advert_t;
/**
* @see uORB::Manager::orb_advertise()
*/
extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
/**
* @see uORB::Manager::orb_advertise()
*/
extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data,
unsigned int queue_size) __EXPORT;
/**
* @see uORB::Manager::orb_advertise_multi()
*/
extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT;
/**
* @see uORB::Manager::orb_advertise_multi()
*/
extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
unsigned int queue_size) __EXPORT;
/**
* @see uORB::Manager::orb_unadvertise()
*/
extern int orb_unadvertise(orb_advert_t handle) __EXPORT;
/**
* @see uORB::Manager::orb_publish()
*/
extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
/**
* 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.
*
* @see uORB::Manager::orb_advertise_multi() for meaning of the individual parameters
*/
static inline int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data,
int *instance)
{
if (!*handle) {
*handle = orb_advertise_multi(meta, data, instance);
if (*handle) {
return 0;
}
} else {
return orb_publish(meta, *handle, data);
}
return -1;
}
/**
* @see uORB::Manager::orb_subscribe()
*/
extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT;
/**
* @see uORB::Manager::orb_subscribe_multi()
*/
extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT;
/**
* @see uORB::Manager::orb_unsubscribe()
*/
extern int orb_unsubscribe(int handle) __EXPORT;
/**
* @see uORB::Manager::orb_copy()
*/
extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) __EXPORT;
/**
* @see uORB::Manager::orb_check()
*/
extern int orb_check(int handle, bool *updated) __EXPORT;
/**
* @see uORB::Manager::orb_exists()
*/
extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT;
/**
* Get the number of published instances of a topic group
*
* @param meta ORB topic metadata.
* @return The number of published instances of this topic
*/
extern int orb_group_count(const struct orb_metadata *meta) __EXPORT;
/**
* @see uORB::Manager::orb_set_interval()
*/
extern int orb_set_interval(int handle, unsigned interval) __EXPORT;
/**
* @see uORB::Manager::orb_get_interval()
*/
extern int orb_get_interval(int handle, unsigned *interval) __EXPORT;
__END_DECLS
/* Diverse uORB header defines */ //XXX: move to better location
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
typedef uint8_t arming_state_t;
typedef uint8_t main_state_t;
typedef uint8_t hil_state_t;
typedef uint8_t navigation_state_t;
typedef uint8_t switch_pos_t;
#endif /* _UORB_UORB_H */
src/modules/uORB/uORBCommon.hpp
-53
View File
@@ -1,53 +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.
*
****************************************************************************/
#ifndef _uORBCommon_hpp_
#define _uORBCommon_hpp_
#include <drivers/drv_orb_dev.h>
#include <systemlib/err.h>
#include "uORB.h"
#include <drivers/drv_hrt.h>
namespace uORB
{
static constexpr unsigned orb_maxpath = 64;
struct orb_advertdata {
const struct orb_metadata *meta;
int *instance;
};
}
#endif // _uORBCommon_hpp_
src/modules/uORB/uORBCommunicator.hpp
-221
View File
@@ -1,221 +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.
*
****************************************************************************/
#ifndef _uORBCommunicator_hpp_
#define _uORBCommunicator_hpp_
#include <stdint.h>
namespace uORBCommunicator
{
class IChannel;
class IChannelRxHandler;
}
/**
* Interface to enable remote subscriptions. The implementor of this interface
* shall manage the communication channel. It can be fastRPC or tcp or ip.
*/
class uORBCommunicator::IChannel
{
public:
//=========================================================================
// INTERFACES FOR Control messages over a channel.
//=========================================================================
/**
* @brief Interface to notify the remote entity of a topic being advertised.
*
* @param messageName
* This represents the uORB message name(aka topic); This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t topic_advertised(const char *messageName) = 0;
/**
* @brief Interface to notify the remote entity of a topic being unadvertised
* and is no longer publishing messages.
*
* @param messageName
* This represents the uORB message name(aka topic); This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
//virtual int16_t topic_unadvertised(const char *messageName) = 0;
/**
* @brief Interface to notify the remote entity of interest of a
* subscription for a message.
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz) = 0;
/**
* @brief Interface to notify the remote entity of removal of a subscription
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not necessarily mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t remove_subscription(const char *messageName) = 0;
/**
* Register Message Handler. This is internal for the IChannel implementer*
*/
virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler) = 0;
//=========================================================================
// INTERFACES FOR Data messages
//=========================================================================
/**
* @brief Sends the data message over the communication link.
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data) = 0;
};
/**
* Class passed to the communication link implement to provide callback for received
* messages over a channel.
*/
class uORBCommunicator::IChannelRxHandler
{
public:
/**
* Interface to process a received topic from remote.
* @param topic_name
* This represents the uORB message Name (topic); This message Name should be
* globally unique.
* @param isAdvertisement
* Represents if the topic has been advertised or is no longer avialable.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_remote_topic(const char *topic_name, bool isAdvertisement) = 0;
/**
* Interface to process a received AddSubscription from remote.
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_add_subscription(const char *messageName, int32_t msgRateInHz) = 0;
/**
* Interface to process a received control msg to remove subscription
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_remove_subscription(const char *messageName) = 0;
/**
* Interface to process the received data message.
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data) = 0;
};
#endif /* _uORBCommunicator_hpp_ */
src/modules/uORB/uORBDeviceMaster.cpp
-491
View File
@@ -1,491 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "uORBDeviceMaster.hpp"
#include "uORBDeviceNode.hpp"
#include "uORBManager.hpp"
#include "uORBUtils.hpp"
#ifdef ORB_COMMUNICATOR
#include "uORBCommunicator.hpp"
#endif /* ORB_COMMUNICATOR */
#include <px4_platform_common/sem.hpp>
#include <systemlib/px4_macros.h>
#include <math.h>
#ifndef __PX4_QURT // QuRT has no poll()
#include <poll.h>
#endif // PX4_QURT
uORB::DeviceMaster::DeviceMaster()
{
px4_sem_init(&_lock, 0, 1);
}
uORB::DeviceMaster::~DeviceMaster()
{
px4_sem_destroy(&_lock);
}
int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance)
{
int ret = PX4_ERROR;
char nodepath[orb_maxpath];
/* construct a path to the node - this also checks the node name */
ret = uORB::Utils::node_mkpath(nodepath, meta, instance);
if (ret != PX4_OK) {
return ret;
}
ret = PX4_ERROR;
/* try for topic groups */
const unsigned max_group_tries = (instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
unsigned group_tries = 0;
if (instance) {
/* for an advertiser, this will be 0, but a for subscriber that requests a certain instance,
* we do not want to start with 0, but with the instance the subscriber actually requests.
*/
group_tries = *instance;
if (group_tries >= max_group_tries) {
return -ENOMEM;
}
}
SmartLock smart_lock(_lock);
do {
/* if path is modifyable change try index */
if (instance != nullptr) {
/* replace the number at the end of the string */
nodepath[strlen(nodepath) - 1] = '0' + group_tries;
*instance = group_tries;
}
/* driver wants a permanent copy of the path, so make one here */
const char *devpath = strdup(nodepath);
if (devpath == nullptr) {
return -ENOMEM;
}
/* construct the new node, passing the ownership of path to it */
uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath);
/* if we didn't get a device, that's bad, free the path too */
if (node == nullptr) {
free((void *)devpath);
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;
if (ret == -EEXIST) {
/* if the node exists already, get the existing one and check if it's advertised. */
uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries);
/*
* We can claim an existing node in these cases:
* - The node is not advertised (yet). It means there is already one or more subscribers or it was
* unadvertised.
* - We are a single-instance advertiser requesting the first instance.
* (Usually we don't end up here, but we might in case of a race condition between 2
* advertisers).
* - We are a subscriber requesting a certain instance.
* (Also we usually don't end up in that case, but we might in case of a race condtion
* between an advertiser and subscriber).
*/
bool is_single_instance_advertiser = is_advertiser && !instance;
if (existing_node != nullptr &&
(!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) {
if (is_advertiser) {
/* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers
* could get the same instance).
*/
existing_node->mark_as_advertised();
}
ret = PX4_OK;
} else {
/* otherwise: already advertised, keep looking */
}
}
} else {
if (is_advertiser) {
node->mark_as_advertised();
}
// add to the node map.
_node_list.add(node);
_node_exists[node->get_instance()].set((uint8_t)node->id(), true);
}
group_tries++;
} while (ret != PX4_OK && (group_tries < max_group_tries));
if (ret != PX4_OK && group_tries >= max_group_tries) {
ret = -ENOMEM;
}
return ret;
}
void uORB::DeviceMaster::printStatistics()
{
/* Add all nodes to a list while locked, and then print them in unlocked state, to avoid potential
* dead-locks (where printing blocks) */
lock();
DeviceNodeStatisticsData *first_node = nullptr;
DeviceNodeStatisticsData *cur_node = nullptr;
size_t max_topic_name_length = 0;
int num_topics = 0;
int ret = addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, nullptr, 0);
unlock();
if (ret != 0) {
PX4_ERR("addNewDeviceNodes failed (%i)", ret);
return;
}
PX4_INFO_RAW("%-*s INST #SUB #Q SIZE PATH\n", (int)max_topic_name_length - 2, "TOPIC NAME");
cur_node = first_node;
while (cur_node) {
cur_node->node->print_statistics(max_topic_name_length);
DeviceNodeStatisticsData *prev = cur_node;
cur_node = cur_node->next;
delete prev;
}
}
int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics,
size_t &max_topic_name_length, char **topic_filter, int num_filters)
{
DeviceNodeStatisticsData *cur_node = nullptr;
num_topics = 0;
DeviceNodeStatisticsData *last_node = *first_node;
if (last_node) {
while (last_node->next) {
last_node = last_node->next;
}
}
for (const auto &node : _node_list) {
++num_topics;
//check if already added
cur_node = *first_node;
while (cur_node && cur_node->node != node) {
cur_node = cur_node->next;
}
if (cur_node) {
continue;
}
if (num_filters > 0 && topic_filter) {
bool matched = false;
for (int i = 0; i < num_filters; ++i) {
if (strstr(node->get_meta()->o_name, topic_filter[i])) {
matched = true;
}
}
if (!matched) {
continue;
}
}
if (last_node) {
last_node->next = new DeviceNodeStatisticsData();
last_node = last_node->next;
} else {
*first_node = last_node = new DeviceNodeStatisticsData();
}
if (!last_node) {
return -ENOMEM;
}
last_node->node = node;
size_t name_length = strlen(last_node->node->get_meta()->o_name);
if (name_length > max_topic_name_length) {
max_topic_name_length = name_length;
}
// Pass in 0 to get the index of the latest published data
last_node->last_pub_msg_count = last_node->node->updates_available(0);
}
return 0;
}
#define CLEAR_LINE "\033[K"
void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
{
bool print_active_only = true;
bool only_once = false; // if true, run only once, then exit
if (topic_filter && num_filters > 0) {
bool show_all = false;
for (int i = 0; i < num_filters; ++i) {
if (!strcmp("-a", topic_filter[i])) {
show_all = true;
} else if (!strcmp("-1", topic_filter[i])) {
only_once = true;
}
}
print_active_only = only_once ? (num_filters == 1) : false; // print non-active if -a or some filter given
if (show_all || print_active_only) {
num_filters = 0;
}
}
PX4_INFO_RAW("\033[2J\n"); //clear screen
lock();
if (_node_list.empty()) {
unlock();
PX4_INFO("no active topics");
return;
}
DeviceNodeStatisticsData *first_node = nullptr;
DeviceNodeStatisticsData *cur_node = nullptr;
size_t max_topic_name_length = 0;
int num_topics = 0;
int ret = addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters);
/* a DeviceNode is never deleted, so it's save to unlock here and still access the DeviceNodes */
unlock();
if (ret != 0) {
PX4_ERR("addNewDeviceNodes failed (%i)", ret);
}
#ifdef __PX4_QURT // QuRT has no poll()
only_once = true;
#else
const int stdin_fileno = 0;
struct pollfd fds;
fds.fd = stdin_fileno;
fds.events = POLLIN;
#endif
bool quit = false;
hrt_abstime start_time = hrt_absolute_time();
while (!quit) {
#ifndef __PX4_QURT
if (!only_once) {
/* Sleep 200 ms waiting for user input five times ~ 1.4s */
for (int k = 0; k < 7; k++) {
char c;
ret = ::poll(&fds, 1, 0); //just want to check if there is new data available
if (ret > 0) {
ret = ::read(stdin_fileno, &c, 1);
if (ret) {
quit = true;
break;
}
}
px4_usleep(200000);
}
} else {
px4_usleep(2000000); // 2 seconds
}
#endif
if (!quit) {
//update the stats
hrt_abstime current_time = hrt_absolute_time();
float dt = (current_time - start_time) / 1.e6f;
cur_node = first_node;
while (cur_node) {
unsigned int num_msgs = cur_node->node->updates_available(cur_node->last_pub_msg_count);
cur_node->pub_msg_delta = roundf(num_msgs / dt);
cur_node->last_pub_msg_count += num_msgs;
cur_node = cur_node->next;
}
start_time = current_time;
if (!only_once) {
PX4_INFO_RAW("\033[H"); // move cursor to top left corner
}
PX4_INFO_RAW(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics);
PX4_INFO_RAW(CLEAR_LINE "%-*s INST #SUB RATE #Q SIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME");
cur_node = first_node;
while (cur_node) {
if (!print_active_only || (cur_node->pub_msg_delta > 0 && cur_node->node->subscriber_count() > 0)) {
PX4_INFO_RAW(CLEAR_LINE "%-*s %2i %4i %4i %2i %4i \n", (int)max_topic_name_length,
cur_node->node->get_meta()->o_name, (int)cur_node->node->get_instance(),
(int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta,
cur_node->node->get_queue_size(), cur_node->node->get_meta()->o_size);
}
cur_node = cur_node->next;
}
if (!only_once) {
PX4_INFO_RAW("\033[0J"); // clear the rest of the screen
}
lock();
ret = addNewDeviceNodes(&first_node, num_topics, max_topic_name_length, topic_filter, num_filters);
unlock();
if (ret != 0) {
PX4_ERR("addNewDeviceNodes failed (%i)", ret);
}
}
if (only_once) {
quit = true;
}
}
//cleanup
cur_node = first_node;
while (cur_node) {
DeviceNodeStatisticsData *next_node = cur_node->next;
delete cur_node;
cur_node = next_node;
}
}
#undef CLEAR_LINE
uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
{
lock();
for (uORB::DeviceNode *node : _node_list) {
if (strcmp(node->get_devname(), nodepath) == 0) {
unlock();
return node;
}
}
unlock();
return nullptr;
}
bool uORB::DeviceMaster::deviceNodeExists(ORB_ID id, const uint8_t instance)
{
if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) {
return false;
}
return _node_exists[instance][(uint8_t)id];
}
uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance)
{
if (meta == nullptr) {
return nullptr;
}
if (!deviceNodeExists(static_cast<ORB_ID>(meta->o_id), instance)) {
return nullptr;
}
lock();
uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance);
unlock();
//We can safely return the node that can be used by any thread, because
//a DeviceNode never gets deleted.
return node;
}
uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance)
{
for (uORB::DeviceNode *node : _node_list) {
if ((strcmp(node->get_name(), meta->o_name) == 0) && (node->get_instance() == instance)) {
return node;
}
}
return nullptr;
}
src/modules/uORB/uORBDeviceMaster.hpp
-124
View File
@@ -1,124 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <stdint.h>
#include "uORBCommon.hpp"
#include <uORB/topics/uORBTopics.hpp>
#include <px4_platform_common/posix.h>
namespace uORB
{
class DeviceNode;
class DeviceMaster;
class Manager;
}
#include <string.h>
#include <stdlib.h>
#include <containers/IntrusiveSortedList.hpp>
#include <px4_platform_common/atomic_bitset.h>
using px4::AtomicBitset;
/**
* Master control device for ObjDev.
*
* Used primarily to create new objects via the ORBIOCCREATE
* ioctl.
*/
class uORB::DeviceMaster
{
public:
int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance);
/**
* Public interface for getDeviceNodeLocked(). Takes care of synchronization.
* @return node if exists, nullptr otherwise
*/
uORB::DeviceNode *getDeviceNode(const char *node_name);
uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance);
bool deviceNodeExists(ORB_ID id, const uint8_t instance);
/**
* Print statistics for each existing topic.
*/
void printStatistics();
/**
* Continuously print statistics, like the unix top command for processes.
* Exited when the user presses the enter key.
* @param topic_filter list of topic filters: if set, each string can be a substring for topics to match.
* Or it can be '-a', which means to print all topics instead of only ones currently publishing with subscribers.
* @param num_filters
*/
void showTop(char **topic_filter, int num_filters);
private:
// Private constructor, uORB::Manager takes care of its creation
DeviceMaster();
~DeviceMaster();
struct DeviceNodeStatisticsData {
DeviceNode *node;
unsigned int last_pub_msg_count;
unsigned int pub_msg_delta;
DeviceNodeStatisticsData *next = nullptr;
};
int addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, size_t &max_topic_name_length,
char **topic_filter, int num_filters);
friend class uORB::Manager;
/**
* Find a node give its name.
* _lock must already be held when calling this.
* @return node if exists, nullptr otherwise
*/
uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance);
IntrusiveSortedList<uORB::DeviceNode *> _node_list;
AtomicBitset<ORB_TOPICS_COUNT> _node_exists[ORB_MULTI_MAX_INSTANCES];
px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */
void lock() { do {} while (px4_sem_wait(&_lock) != 0); }
void unlock() { px4_sem_post(&_lock); }
};
src/modules/uORB/uORBDeviceNode.cpp
-587
View File
@@ -1,587 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "uORBDeviceNode.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
#include "SubscriptionCallback.hpp"
#ifdef ORB_COMMUNICATOR
#include "uORBCommunicator.hpp"
#endif /* ORB_COMMUNICATOR */
static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
// Determine the data range
static inline bool is_in_range(unsigned left, unsigned value, unsigned right)
{
if (right > left) {
return (left <= value) && (value <= right);
} else { // Maybe the data overflowed and a wraparound occurred
return (left <= value) || (value <= right);
}
}
// round up to nearest power of two
// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
// Note: When the input value > 128, the output is always 128
static inline uint8_t round_pow_of_two_8(uint8_t n)
{
if (n == 0) {
return 1;
}
// Avoid is already a power of 2
uint8_t value = n - 1;
// Fill 1
value |= value >> 1U;
value |= value >> 2U;
value |= value >> 4U;
// Unable to round-up, take the value of round-down
if (value == UINT8_MAX) {
value >>= 1U;
}
return value + 1;
}
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path,
uint8_t queue_size) :
CDev(path),
_meta(meta),
_instance(instance),
_queue_size(round_pow_of_two_8(queue_size))
{
}
uORB::DeviceNode::~DeviceNode()
{
delete[] _data;
CDev::unregister_driver_and_memory();
}
int
uORB::DeviceNode::open(cdev::file_t *filp)
{
/* is this a publisher? */
if (filp->f_oflags == PX4_F_WRONLY) {
lock();
mark_as_advertised();
unlock();
/* now complete the open */
return CDev::open(filp);
}
/* is this a new subscriber? */
if (filp->f_oflags == PX4_F_RDONLY) {
/* allocate subscriber data */
SubscriptionInterval *sd = new SubscriptionInterval(_meta, 0, _instance);
if (nullptr == sd) {
return -ENOMEM;
}
filp->f_priv = (void *)sd;
int ret = CDev::open(filp);
if (ret != PX4_OK) {
PX4_ERR("CDev::open failed");
delete sd;
}
return ret;
}
if (filp->f_oflags == 0) {
return CDev::open(filp);
}
/* can only be pub or sub, not both */
return -EINVAL;
}
int
uORB::DeviceNode::close(cdev::file_t *filp)
{
if (filp->f_oflags == PX4_F_RDONLY) { /* subscriber */
SubscriptionInterval *sd = filp_to_subscription(filp);
delete sd;
}
return CDev::close(filp);
}
bool
uORB::DeviceNode::copy(void *dst, unsigned &generation)
{
if ((dst != nullptr) && (_data != nullptr)) {
if (_queue_size == 1) {
ATOMIC_ENTER;
memcpy(dst, _data, _meta->o_size);
generation = _generation.load();
ATOMIC_LEAVE;
return true;
} else {
ATOMIC_ENTER;
const unsigned current_generation = _generation.load();
if (current_generation == generation) {
/* The subscriber already read the latest message, but nothing new was published yet.
* Return the previous message
*/
--generation;
}
// Compatible with normal and overflow conditions
if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
// Reader is too far behind: some messages are lost
generation = current_generation - _queue_size;
}
memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
ATOMIC_LEAVE;
++generation;
return true;
}
}
return false;
}
ssize_t
uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
{
/* if the caller's buffer is the wrong size, that's an error */
if (buflen != _meta->o_size) {
return -EIO;
}
return filp_to_subscription(filp)->copy(buffer) ? _meta->o_size : 0;
}
ssize_t
uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
{
/*
* Writes are legal from interrupt context as long as the
* object has already been initialised from thread context.
*
* Writes outside interrupt context will allocate the object
* if it has not yet been allocated.
*
* Note that filp will usually be NULL.
*/
if (nullptr == _data) {
#ifdef __PX4_NUTTX
if (!up_interrupt_context()) {
#endif /* __PX4_NUTTX */
lock();
/* re-check size */
if (nullptr == _data) {
_data = new uint8_t[_meta->o_size * _queue_size];
}
unlock();
#ifdef __PX4_NUTTX
}
#endif /* __PX4_NUTTX */
/* failed or could not allocate */
if (nullptr == _data) {
return -ENOMEM;
}
}
/* If write size does not match, that is an error */
if (_meta->o_size != buflen) {
return -EIO;
}
/* Perform an atomic copy. */
ATOMIC_ENTER;
/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
unsigned generation = _generation.fetch_add(1);
memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size);
// callbacks
for (auto item : _callbacks) {
item->call();
}
/* Mark at least one data has been published */
_data_valid = true;
ATOMIC_LEAVE;
/* notify any poll waiters */
poll_notify(POLLIN);
return _meta->o_size;
}
int
uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case ORBIOCUPDATED: {
ATOMIC_ENTER;
*(bool *)arg = filp_to_subscription(filp)->updated();
ATOMIC_LEAVE;
return PX4_OK;
}
case ORBIOCSETINTERVAL:
filp_to_subscription(filp)->set_interval_us(arg);
return PX4_OK;
case ORBIOCGADVERTISER:
*(uintptr_t *)arg = (uintptr_t)this;
return PX4_OK;
case ORBIOCSETQUEUESIZE: {
lock();
int ret = update_queue_size(arg);
unlock();
return ret;
}
case ORBIOCGETINTERVAL:
*(unsigned *)arg = filp_to_subscription(filp)->get_interval_us();
return PX4_OK;
case ORBIOCISADVERTISED:
*(unsigned long *)arg = _advertised;
return PX4_OK;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
ssize_t
uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data)
{
uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
int ret;
/* check if the device handle is initialized and data is valid */
if ((devnode == nullptr) || (meta == nullptr) || (data == nullptr)) {
errno = EFAULT;
return PX4_ERROR;
}
/* check if the orb meta data matches the publication */
if (devnode->_meta != meta) {
errno = EINVAL;
return PX4_ERROR;
}
/* call the devnode write method with no file pointer */
ret = devnode->write(nullptr, (const char *)data, meta->o_size);
if (ret < 0) {
errno = -ret;
return PX4_ERROR;
}
if (ret != (int)meta->o_size) {
errno = EIO;
return PX4_ERROR;
}
#ifdef ORB_COMMUNICATOR
/*
* if the write is successful, send the data over the Multi-ORB link
*/
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr) {
if (ch->send_message(meta->o_name, meta->o_size, (uint8_t *)data) != 0) {
PX4_ERR("Error Sending [%s] topic data over comm_channel", meta->o_name);
return PX4_ERROR;
}
}
#endif /* ORB_COMMUNICATOR */
return PX4_OK;
}
int uORB::DeviceNode::unadvertise(orb_advert_t handle)
{
if (handle == nullptr) {
return -EINVAL;
}
uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
/*
* We are cheating a bit here. First, with the current implementation, we can only
* have multiple publishers for instance 0. In this case the caller will have
* instance=nullptr and _published has no effect at all. Thus no unadvertise is
* necessary.
* In case of multiple instances, we have at most 1 publisher per instance and
* we can signal an instance as 'free' by setting _published to false.
* We never really free the DeviceNode, for this we would need reference counting
* of subscribers and publishers. But we also do not have a leak since future
* publishers reuse the same DeviceNode object.
*/
devnode->_advertised = false;
return PX4_OK;
}
#ifdef ORB_COMMUNICATOR
int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta)
{
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr && meta != nullptr) {
return ch->topic_advertised(meta->o_name);
}
return -1;
}
/*
//TODO: Check if we need this since we only unadvertise when things all shutdown and it doesn't actually remove the device
int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta)
{
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr && meta != nullptr) {
return ch->topic_unadvertised(meta->o_name);
}
return -1;
}
*/
#endif /* ORB_COMMUNICATOR */
px4_pollevent_t
uORB::DeviceNode::poll_state(cdev::file_t *filp)
{
// If the topic appears updated to the subscriber, say so.
return filp_to_subscription(filp)->updated() ? POLLIN : 0;
}
void
uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events)
{
// If the topic looks updated to the subscriber, go ahead and notify them.
if (filp_to_subscription((cdev::file_t *)fds->priv)->updated()) {
CDev::poll_notify_one(fds, events);
}
}
bool
uORB::DeviceNode::print_statistics(int max_topic_length)
{
if (!_advertised) {
return false;
}
lock();
const uint8_t instance = get_instance();
const int8_t sub_count = subscriber_count();
const uint8_t queue_size = get_queue_size();
unlock();
PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
queue_size, get_meta()->o_size, get_devname());
return true;
}
void uORB::DeviceNode::add_internal_subscriber()
{
lock();
_subscriber_count++;
#ifdef ORB_COMMUNICATOR
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr && _subscriber_count > 0) {
unlock(); //make sure we cannot deadlock if add_subscription calls back into DeviceNode
ch->add_subscription(_meta->o_name, 1);
} else
#endif /* ORB_COMMUNICATOR */
{
unlock();
}
}
void uORB::DeviceNode::remove_internal_subscriber()
{
lock();
_subscriber_count--;
#ifdef ORB_COMMUNICATOR
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (ch != nullptr && _subscriber_count == 0) {
unlock(); //make sure we cannot deadlock if remove_subscription calls back into DeviceNode
ch->remove_subscription(_meta->o_name);
} else
#endif /* ORB_COMMUNICATOR */
{
unlock();
}
}
#ifdef ORB_COMMUNICATOR
int16_t uORB::DeviceNode::process_add_subscription(int32_t rateInHz)
{
// if there is already data in the node, send this out to
// the remote entity.
// send the data to the remote entity.
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
ch->send_message(_meta->o_name, _meta->o_size, _data);
}
return PX4_OK;
}
int16_t uORB::DeviceNode::process_remove_subscription()
{
return PX4_OK;
}
int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data)
{
int16_t ret = -1;
if (length != (int32_t)(_meta->o_size)) {
PX4_ERR("Received '%s' with DataLength[%d] != ExpectedLen[%d]", _meta->o_name, (int)length, (int)_meta->o_size);
return PX4_ERROR;
}
/* call the devnode write method with no file pointer */
ret = write(nullptr, (const char *)data, _meta->o_size);
if (ret < 0) {
return PX4_ERROR;
}
if (ret != (int)_meta->o_size) {
errno = EIO;
return PX4_ERROR;
}
return PX4_OK;
}
#endif /* ORB_COMMUNICATOR */
int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
{
if (_queue_size == queue_size) {
return PX4_OK;
}
//queue size is limited to 255 for the single reason that we use uint8 to store it
if (_data || _queue_size > queue_size || queue_size > 255) {
return PX4_ERROR;
}
_queue_size = round_pow_of_two_8(queue_size);
return PX4_OK;
}
unsigned uORB::DeviceNode::get_initial_generation()
{
ATOMIC_ENTER;
// If there any previous publications allow the subscriber to read them
unsigned generation = _generation.load() - (_data_valid ? 1 : 0);
ATOMIC_LEAVE;
return generation;
}
bool
uORB::DeviceNode::register_callback(uORB::SubscriptionCallback *callback_sub)
{
if (callback_sub != nullptr) {
ATOMIC_ENTER;
// prevent duplicate registrations
for (auto existing_callbacks : _callbacks) {
if (callback_sub == existing_callbacks) {
ATOMIC_LEAVE;
return true;
}
}
_callbacks.add(callback_sub);
ATOMIC_LEAVE;
return true;
}
return false;
}
void
uORB::DeviceNode::unregister_callback(uORB::SubscriptionCallback *callback_sub)
{
ATOMIC_ENTER;
_callbacks.remove(callback_sub);
ATOMIC_LEAVE;
}
src/modules/uORB/uORBDeviceNode.hpp
-256
View File
@@ -1,256 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "uORBCommon.hpp"
#include "uORBDeviceMaster.hpp"
#include <lib/cdev/CDev.hpp>
#include <containers/IntrusiveSortedList.hpp>
#include <containers/List.hpp>
#include <px4_platform_common/atomic.h>
namespace uORB
{
class DeviceNode;
class DeviceMaster;
class Manager;
class SubscriptionCallback;
}
namespace uORBTest
{
class UnitTest;
}
/**
* Per-object device instance.
*/
class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode<uORB::DeviceNode *>
{
public:
DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1);
virtual ~DeviceNode();
// no copy, assignment, move, move assignment
DeviceNode(const DeviceNode &) = delete;
DeviceNode &operator=(const DeviceNode &) = delete;
DeviceNode(DeviceNode &&) = delete;
DeviceNode &operator=(DeviceNode &&) = delete;
bool operator<=(const DeviceNode &rhs) const { return (strcmp(get_devname(), rhs.get_devname()) <= 0); }
/**
* Method to create a subscriber instance and return the struct
* pointing to the subscriber as a file pointer.
*/
int open(cdev::file_t *filp) override;
/**
* Method to close a subscriber for this topic.
*/
int close(cdev::file_t *filp) override;
/**
* 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.
*/
ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen) override;
/**
* 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
*/
ssize_t write(cdev::file_t *filp, const char *buffer, size_t buflen) override;
/**
* IOCTL control for the subscriber.
*/
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
/**
* Method to publish a data to this node.
*/
static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data);
static int unadvertise(orb_advert_t handle);
#ifdef ORB_COMMUNICATOR
static int16_t topic_advertised(const orb_metadata *meta);
//static int16_t topic_unadvertised(const orb_metadata *meta);
/**
* processes a request for add subscription from remote
* @param rateInHz
* Specifies the desired rate for the message.
* @return
* 0 = success
* otherwise failure.
*/
int16_t process_add_subscription(int32_t rateInHz);
/**
* processes a request to remove a subscription from remote.
*/
int16_t process_remove_subscription();
/**
* processed the received data message from remote.
*/
int16_t process_received_message(int32_t length, uint8_t *data);
#endif /* ORB_COMMUNICATOR */
/**
* Add the subscriber to the node's list of subscriber. If there is
* remote proxy to which this subscription needs to be sent, it will
* done via uORBCommunicator::IChannel interface.
* @param sd
* the subscriber to be added.
*/
void add_internal_subscriber();
/**
* Removes the subscriber from the list. Also notifies the remote
* if there a uORBCommunicator::IChannel instance.
* @param sd
* the Subscriber to be removed.
*/
void remove_internal_subscriber();
/**
* Return true if this topic has been advertised.
*
* This is used in the case of multi_pub/sub to check if it's valid to advertise
* and publish to this node or if another node should be tried. */
bool is_advertised() const { return _advertised; }
void mark_as_advertised() { _advertised = true; }
/**
* Try to change the size of the queue. This can only be done as long as nobody published yet.
* This is the case, for example when orb_subscribe was called before an orb_advertise.
* The queue size can only be increased.
* @param queue_size new size of the queue
* @return PX4_OK if queue size successfully set
*/
int update_queue_size(unsigned int queue_size);
/**
* Print statistics
* @param max_topic_length max topic name length for printing
* @return true if printed something, false otherwise
*/
bool print_statistics(int max_topic_length);
uint8_t get_queue_size() const { return _queue_size; }
int8_t subscriber_count() const { return _subscriber_count; }
/**
* Returns the number of updated data relative to the parameter 'generation'
* We can get the correct value regardless of wrap-around or not.
* @param generation The generation of subscriber
*/
unsigned updates_available(unsigned generation) const { return _generation.load() - generation; }
/**
* Return the initial generation to the subscriber
* @return The initial generation.
*/
unsigned get_initial_generation();
const orb_metadata *get_meta() const { return _meta; }
ORB_ID id() const { return static_cast<ORB_ID>(_meta->o_id); }
const char *get_name() const { return _meta->o_name; }
uint8_t get_instance() const { return _instance; }
/**
* Copies data and the corresponding generation
* from a node to the buffer provided.
*
* @param dst
* The buffer into which the data is copied.
* @param generation
* The generation that was copied.
* @return bool
* Returns true if the data was copied.
*/
bool copy(void *dst, unsigned &generation);
// add item to list of work items to schedule on node update
bool register_callback(SubscriptionCallback *callback_sub);
// remove item from list of work items
void unregister_callback(SubscriptionCallback *callback_sub);
protected:
px4_pollevent_t poll_state(cdev::file_t *filp) override;
void poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) override;
private:
friend uORBTest::UnitTest;
const orb_metadata *_meta; /**< object metadata information */
uint8_t *_data{nullptr}; /**< allocated object buffer */
bool _data_valid{false}; /**< At least one valid data */
px4::atomic<unsigned> _generation{0}; /**< object generation count */
List<uORB::SubscriptionCallback *> _callbacks;
const uint8_t _instance; /**< orb multi instance identifier */
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
uint8_t _queue_size; /**< maximum number of elements in the queue */
int8_t _subscriber_count{0};
};
src/modules/uORB/uORBMain.cpp
-153
View File
@@ -1,153 +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.
*
****************************************************************************/
#include <string.h>
#include "uORBManager.hpp"
#include "uORB.h"
#include "uORBCommon.hpp"
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); }
static uORB::DeviceMaster *g_dev = nullptr;
static void usage()
{
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
uORB is the internal pub-sub messaging system, used for communication between modules.
It is typically started as one of the very first modules and most other modules depend on it.
### Implementation
No thread or work queue is needed, the module start only makes sure to initialize the shared global state.
Communication is done via shared memory.
The implementation is asynchronous and lock-free, ie. a publisher does not wait for a subscriber and vice versa.
This is achieved by having a separate buffer between a publisher and a subscriber.
The code is optimized to minimize the memory footprint and the latency to exchange messages.
The interface is based on file descriptors: internally it uses `read`, `write` and `ioctl`. Except for the
publications, which use `orb_advert_t` handles, so that they can be used from interrupts as well (on NuttX).
Messages are defined in the `/msg` directory. They are converted into C/C++ code at build-time.
If compiled with ORB_USE_PUBLISHER_RULES, a file with uORB publication rules can be used to configure which
modules are allowed to publish which topics. This is used for system-wide replay.
### Examples
Monitor topic publication rates. Besides `top`, this is an important command for general system inspection:
$ uorb top
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("uorb", "communication");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print topic statistics");
PRINT_MODULE_USAGE_COMMAND_DESCR("top", "Monitor topic publication rates");
PRINT_MODULE_USAGE_PARAM_FLAG('a', "print all instead of only currently publishing topics with subscribers", true);
PRINT_MODULE_USAGE_PARAM_FLAG('1', "run only once, then exit", true);
PRINT_MODULE_USAGE_ARG("<filter1> [<filter2>]", "topic(s) to match (implies -a)", true);
}
int
uorb_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return -EINVAL;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
PX4_WARN("already loaded");
/* user wanted to start uorb, its already running, no error */
return 0;
}
if (!uORB::Manager::initialize()) {
PX4_ERR("uorb manager alloc failed");
return -ENOMEM;
}
/* create the driver */
g_dev = uORB::Manager::get_instance()->get_device_master();
if (g_dev == nullptr) {
return -errno;
}
#if !defined(__PX4_QURT)
/* FIXME: this fails on Snapdragon (see https://github.com/PX4/Firmware/issues/5406),
* so we disable logging messages to the ulog for now. This needs further investigations.
*/
px4_log_initialize();
#endif
return OK;
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "status")) {
if (g_dev != nullptr) {
g_dev->printStatistics();
} else {
PX4_INFO("uorb is not running");
}
return OK;
}
if (!strcmp(argv[1], "top")) {
if (g_dev != nullptr) {
g_dev->showTop(argv + 2, argc - 2);
} else {
PX4_INFO("uorb is not running");
}
return OK;
}
usage();
return -EINVAL;
}
src/modules/uORB/uORBManager.cpp
-649
View File
@@ -1,649 +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.
*
****************************************************************************/
#include <sys/types.h>
#include <sys/stat.h>
#include <stdarg.h>
#include <fcntl.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include "uORBDeviceNode.hpp"
#include "uORBUtils.hpp"
#include "uORBManager.hpp"
uORB::Manager *uORB::Manager::_Instance = nullptr;
bool uORB::Manager::initialize()
{
if (_Instance == nullptr) {
_Instance = new uORB::Manager();
}
return _Instance != nullptr;
}
bool uORB::Manager::terminate()
{
if (_Instance != nullptr) {
delete _Instance;
_Instance = nullptr;
return true;
}
return false;
}
uORB::Manager::Manager()
{
#ifdef ORB_USE_PUBLISHER_RULES
const char *file_name = PX4_STORAGEDIR"/orb_publisher.rules";
int ret = readPublisherRulesFromFile(file_name, _publisher_rule);
if (ret == PX4_OK) {
_has_publisher_rules = true;
PX4_INFO("Using orb rules from %s", file_name);
} else {
PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret));
}
#endif /* ORB_USE_PUBLISHER_RULES */
}
uORB::Manager::~Manager()
{
delete _device_master;
}
uORB::DeviceMaster *uORB::Manager::get_device_master()
{
if (!_device_master) {
_device_master = new DeviceMaster();
if (_device_master == nullptr) {
PX4_ERR("Failed to allocate DeviceMaster");
errno = ENOMEM;
}
}
return _device_master;
}
int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
{
int ret = PX4_ERROR;
// instance valid range: [0, ORB_MULTI_MAX_INSTANCES)
if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) {
return ret;
}
if (get_device_master()) {
uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance);
if (node != nullptr) {
if (node->is_advertised()) {
return PX4_OK;
}
}
}
#ifdef ORB_COMMUNICATOR
/*
* Generate the path to the node and try to open it.
*/
char path[orb_maxpath];
int inst = instance;
ret = uORB::Utils::node_mkpath(path, meta, &inst);
if (ret != OK) {
errno = -ret;
return PX4_ERROR;
}
ret = px4_access(path, F_OK);
if (ret == -1 && meta != nullptr && !_remote_topics.empty()) {
ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : PX4_ERROR;
}
if (ret == 0) {
// we know the topic exists, but it's not necessarily advertised/published yet (for example
// if there is only a subscriber)
// The open() will not lead to memory allocations.
int fd = px4_open(path, 0);
if (fd >= 0) {
unsigned long is_advertised;
if (px4_ioctl(fd, ORBIOCISADVERTISED, (unsigned long)&is_advertised) == 0) {
if (!is_advertised) {
ret = PX4_ERROR;
}
}
px4_close(fd);
}
}
#endif /* ORB_COMMUNICATOR */
return ret;
}
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
unsigned int queue_size)
{
#ifdef ORB_USE_PUBLISHER_RULES
// check publisher rule
if (_has_publisher_rules) {
const char *prog_name = px4_get_taskname();
if (strcmp(_publisher_rule.module_name, prog_name) == 0) {
if (_publisher_rule.ignore_other_topics) {
if (!findTopic(_publisher_rule, meta->o_name)) {
PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
return (orb_advert_t)_Instance;
}
}
} else {
if (findTopic(_publisher_rule, meta->o_name)) {
PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name);
return (orb_advert_t)_Instance;
}
}
}
#endif /* ORB_USE_PUBLISHER_RULES */
/* open the node as an advertiser */
int fd = node_open(meta, true, instance);
if (fd == PX4_ERROR) {
PX4_ERR("%s advertise failed (%i)", meta->o_name, errno);
return nullptr;
}
/* Set the queue size. This must be done before the first publication; thus it fails if
* this is not the first advertiser.
*/
int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
if (result < 0 && queue_size > 1) {
PX4_WARN("orb_advertise_multi: failed to set queue size");
}
/* get the advertiser handle and close the node */
orb_advert_t advertiser;
result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
px4_close(fd);
if (result == PX4_ERROR) {
PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd);
return nullptr;
}
#ifdef ORB_COMMUNICATOR
// For remote systems call over and inform them
uORB::DeviceNode::topic_advertised(meta);
#endif /* ORB_COMMUNICATOR */
/* the advertiser may perform an initial publish to initialise the object */
if (data != nullptr) {
result = orb_publish(meta, advertiser, data);
if (result == PX4_ERROR) {
PX4_ERR("orb_publish failed %s", meta->o_name);
return nullptr;
}
}
return advertiser;
}
int uORB::Manager::orb_unadvertise(orb_advert_t handle)
{
#ifdef ORB_USE_PUBLISHER_RULES
if (handle == _Instance) {
return PX4_OK; //pretend success
}
#endif /* ORB_USE_PUBLISHER_RULES */
return uORB::DeviceNode::unadvertise(handle);
}
int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
{
return node_open(meta, false);
}
int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
{
int inst = instance;
return node_open(meta, 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)
{
#ifdef ORB_USE_PUBLISHER_RULES
if (handle == _Instance) {
return PX4_OK; //pretend success
}
#endif /* ORB_USE_PUBLISHER_RULES */
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 PX4_ERROR;
}
if (ret != (int)meta->o_size) {
errno = EIO;
return PX4_ERROR;
}
return PX4_OK;
}
int uORB::Manager::orb_check(int handle, bool *updated)
{
/* Set to false here so that if `px4_ioctl` fails to false. */
*updated = false;
return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
}
int uORB::Manager::orb_set_interval(int handle, unsigned interval)
{
return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
}
int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
{
int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval);
*interval /= 1000;
return ret;
}
int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
{
char path[orb_maxpath];
int fd = -1;
int ret = PX4_ERROR;
/*
* 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 PX4_ERROR;
}
/* if we have an instance and are an advertiser, we will generate a new node and set the instance,
* so we do not need to open here */
if (!instance || !advertiser) {
/*
* Generate the path to the node and try to open it.
*/
ret = uORB::Utils::node_mkpath(path, meta, instance);
if (ret != OK) {
errno = -ret;
return PX4_ERROR;
}
/* open the path as either the advertiser or the subscriber */
fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY);
} else {
*instance = 0;
}
/* we may need to advertise the node... */
if (fd < 0) {
ret = PX4_ERROR;
if (get_device_master()) {
ret = _device_master->advertise(meta, advertiser, instance);
}
/* it's OK if it already exists */
if ((ret != PX4_OK) && (EEXIST == errno)) {
ret = PX4_OK;
}
if (ret == PX4_OK) {
/* update the path, as it might have been updated during the node advertise call */
ret = uORB::Utils::node_mkpath(path, meta, instance);
/* on success, try to open again */
if (ret == PX4_OK) {
fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
} else {
errno = -ret;
return PX4_ERROR;
}
}
}
if (fd < 0) {
errno = EIO;
return PX4_ERROR;
}
/* everything has been OK, we can return the handle now */
return fd;
}
#ifdef ORB_COMMUNICATOR
void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel)
{
_comm_channel = channel;
if (_comm_channel != nullptr) {
_comm_channel->register_handler(this);
}
}
uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator()
{
return _comm_channel;
}
int16_t uORB::Manager::process_remote_topic(const char *topic_name, bool isAdvertisement)
{
int16_t rc = 0;
if (isAdvertisement) {
_remote_topics.insert(topic_name);
} else {
_remote_topics.erase(topic_name);
}
return rc;
}
int16_t uORB::Manager::process_add_subscription(const char *messageName, int32_t msgRateInHz)
{
PX4_DEBUG("entering Manager_process_add_subscription: name: %s", messageName);
int16_t rc = 0;
_remote_subscriber_topics.insert(messageName);
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, messageName);
DeviceMaster *device_master = get_device_master();
if (ret == OK && device_master) {
uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
if (node == nullptr) {
PX4_DEBUG("DeviceNode(%s) not created yet", messageName);
} else {
// node is present.
node->process_add_subscription(msgRateInHz);
}
} else {
rc = -1;
}
return rc;
}
int16_t uORB::Manager::process_remove_subscription(const char *messageName)
{
int16_t rc = -1;
_remote_subscriber_topics.erase(messageName);
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, messageName);
DeviceMaster *device_master = get_device_master();
if (ret == OK && device_master) {
uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
// get the node name.
if (node == nullptr) {
PX4_DEBUG("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]",
__LINE__, messageName);
} else {
// node is present.
node->process_remove_subscription();
rc = 0;
}
}
return rc;
}
int16_t uORB::Manager::process_received_message(const char *messageName, int32_t length, uint8_t *data)
{
int16_t rc = -1;
char nodepath[orb_maxpath];
int ret = uORB::Utils::node_mkpath(nodepath, messageName);
DeviceMaster *device_master = get_device_master();
if (ret == OK && device_master) {
uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
// get the node name.
if (node == nullptr) {
PX4_DEBUG("No existing subscriber found for message: [%s] nodepath:[%s]", messageName, nodepath);
} else {
// node is present.
node->process_received_message(length, data);
rc = 0;
}
}
return rc;
}
bool uORB::Manager::is_remote_subscriber_present(const char *messageName)
{
#ifdef __PX4_NUTTX
return _remote_subscriber_topics.find(messageName);
#else
return (_remote_subscriber_topics.find(messageName) != _remote_subscriber_topics.end());
#endif
}
#endif /* ORB_COMMUNICATOR */
#ifdef ORB_USE_PUBLISHER_RULES
bool uORB::Manager::startsWith(const char *pre, const char *str)
{
size_t lenpre = strlen(pre),
lenstr = strlen(str);
return lenstr < lenpre ? false : strncmp(pre, str, lenpre) == 0;
}
bool uORB::Manager::findTopic(const PublisherRule &rule, const char *topic_name)
{
const char **topics_ptr = rule.topics;
while (*topics_ptr) {
if (strcmp(*topics_ptr, topic_name) == 0) {
return true;
}
++topics_ptr;
}
return false;
}
void uORB::Manager::strTrim(const char **str)
{
while (**str == ' ' || **str == '\t') { ++(*str); }
}
int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRule &rule)
{
FILE *fp;
static const int line_len = 1024;
int ret = PX4_OK;
char *line = new char[line_len];
if (!line) {
return -ENOMEM;
}
fp = fopen(file_name, "r");
if (fp == NULL) {
delete[](line);
return -errno;
}
const char *restrict_topics_str = "restrict_topics:";
const char *module_str = "module:";
const char *ignore_others = "ignore_others:";
rule.ignore_other_topics = false;
rule.module_name = nullptr;
rule.topics = nullptr;
while (fgets(line, line_len, fp) && ret == PX4_OK) {
if (strlen(line) < 2 || line[0] == '#') {
continue;
}
if (startsWith(restrict_topics_str, line)) {
//read topics list
char *start = line + strlen(restrict_topics_str);
strTrim((const char **)&start);
char *topics = strdup(start);
int topic_len = 0, num_topics = 0;
for (int i = 0; topics[i]; ++i) {
if (topics[i] == ',' || topics[i] == '\n') {
if (topic_len > 0) {
topics[i] = 0;
++num_topics;
}
topic_len = 0;
} else {
++topic_len;
}
}
if (num_topics > 0) {
rule.topics = new const char *[num_topics + 1];
int topic = 0;
strTrim((const char **)&topics);
rule.topics[topic++] = topics;
while (topic < num_topics) {
if (*topics == 0) {
++topics;
strTrim((const char **)&topics);
rule.topics[topic++] = topics;
} else {
++topics;
}
}
rule.topics[num_topics] = nullptr;
}
} else if (startsWith(module_str, line)) {
//read module name
char *start = line + strlen(module_str);
strTrim((const char **)&start);
int len = strlen(start);
if (len > 0 && start[len - 1] == '\n') {
start[len - 1] = 0;
}
rule.module_name = strdup(start);
} else if (startsWith(ignore_others, line)) {
const char *start = line + strlen(ignore_others);
strTrim(&start);
if (startsWith("true", start)) {
rule.ignore_other_topics = true;
}
} else {
PX4_ERR("orb rules file: wrong format: %s", line);
ret = -EINVAL;
}
}
if (ret == PX4_OK && (!rule.module_name || !rule.topics)) {
PX4_ERR("Wrong format in orb publisher rules file");
ret = -EINVAL;
}
delete[](line);
fclose(fp);
return ret;
}
#endif /* ORB_USE_PUBLISHER_RULES */
src/modules/uORB/uORBManager.hpp
-487
View File
@@ -1,487 +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.
*
****************************************************************************/
#ifndef _uORBManager_hpp_
#define _uORBManager_hpp_
#include "uORBCommon.hpp"
#include "uORBDeviceMaster.hpp"
#include <stdint.h>
#ifdef __PX4_NUTTX
#include "ORBSet.hpp"
#else
#include <string>
#include <set>
#define ORBSet std::set<std::string>
#endif
#ifdef ORB_COMMUNICATOR
#include "uORBCommunicator.hpp"
#endif /* ORB_COMMUNICATOR */
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
#ifdef ORB_COMMUNICATOR
: public uORBCommunicator::IChannelRxHandler
#endif /* ORB_COMMUNICATOR */
{
public:
// public interfaces for this class.
/**
* Initialize the singleton. Call this before everything else.
* @return true on success
*/
static bool initialize();
/**
* Terminate the singleton. Call this after everything else.
* @return true on success
*/
static bool terminate();
/**
* Method to get the singleton instance for the uORB::Manager.
* Make sure initialize() is called first.
* @return uORB::Manager*
*/
static uORB::Manager *get_instance() { return _Instance; }
/**
* Get the DeviceMaster. If it does not exist,
* it will be created and initialized.
* Note: the first call to this is not thread-safe.
* @return nullptr if initialization failed (and errno will be set)
*/
uORB::DeviceMaster *get_device_master();
// ==== 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.
*
* Internally this will call orb_advertise_multi with an instance of 0.
*
* @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 queue_size Maximum number of buffered elements. If this is 1, no queuing is
* used.
* @return nullptr on error, otherwise returns an object pointer
* 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 nullptr and set errno to ENOENT.
*/
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
{
return orb_advertise_multi(meta, data, nullptr, queue_size);
}
/**
* 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.
*
* The multi can be used to create multiple independent instances of the same topic
* (each instance has its own buffer).
* This is useful for multiple publishers who publish the same topic. The subscriber
* then subscribes to all instances and chooses which source he wants to use.
*
* @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. This is an output parameter and will be set to the newly
* created instance, ie. 0 for the first advertiser, 1 for the next and so on.
* @param queue_size Maximum number of buffered elements. If this is 1, no queuing is
* used.
* @return PX4_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,
unsigned int queue_size = 1);
/**
* Unadvertise a topic.
*
* @param handle handle returned by orb_advertise or orb_advertise_multi.
* @return 0 on success
*/
int orb_unadvertise(orb_advert_t handle);
/**
* 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.
*
* @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, PX4_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.
*
* If there were any publications of the topic prior to the subscription,
* an orb_check right after orb_subscribe will return true.
*
* 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.
*
* Internally this will call orb_subscribe_multi with instance 0.
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @return PX4_ERROR on error, otherwise returns a handle
* that can be used to read and update the topic.
*/
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.
*
* If there were any publications of the topic prior to the subscription,
* an orb_check right after orb_subscribe_multi will return true.
*
* 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.
*
* If a publisher publishes multiple instances the subscriber should
* subscribe to each instance with orb_subscribe_multi
* (@see orb_advertise_multi()).
*
* @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_advertise_multi().
* @return PX4_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, PX4_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, PX4_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.
*
* @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, PX4_ERROR otherwise with
* errno set accordingly.
*/
int orb_check(int handle, bool *updated);
/**
* Check if a topic has already been created and published (advertised)
*
* @param meta ORB topic metadata.
* @param instance ORB instance
* @return OK if the topic exists, PX4_ERROR otherwise.
*/
int orb_exists(const struct orb_metadata *meta, int instance);
/**
* 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, PX4_ERROR otherwise with ERRNO set accordingly.
*/
int orb_set_interval(int handle, unsigned interval);
/**
* Get the minimum interval between which updates are seen for a subscription.
*
* @see orb_set_interval()
*
* @param handle A handle returned from orb_subscribe.
* @param interval The returned interval period in milliseconds.
* @return OK on success, PX4_ERROR otherwise with ERRNO set accordingly.
*/
int orb_get_interval(int handle, unsigned *interval);
#ifdef ORB_COMMUNICATOR
/**
* Method to set the uORBCommunicator::IChannel instance.
* @param comm_channel
* The IChannel instance to talk to remote proxies.
* @note:
* Currently this call only supports the use of one IChannel
* Future extensions may include more than one IChannel's.
*/
void set_uorb_communicator(uORBCommunicator::IChannel *comm_channel);
/**
* Gets the uORB Communicator instance.
*/
uORBCommunicator::IChannel *get_uorb_communicator();
/**
* Utility method to check if there is a remote subscriber present
* for a given topic
*/
bool is_remote_subscriber_present(const char *messageName);
#endif /* ORB_COMMUNICATOR */
private: // class methods
/**
* Common implementation for orb_advertise and orb_subscribe.
*
* Handles creation of the object and the initial publication for
* advertisers.
*/
int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr);
private: // data members
static Manager *_Instance;
#ifdef ORB_COMMUNICATOR
// the communicator channel instance.
uORBCommunicator::IChannel *_comm_channel{nullptr};
ORBSet _remote_subscriber_topics;
ORBSet _remote_topics;
#endif /* ORB_COMMUNICATOR */
DeviceMaster *_device_master{nullptr};
private: //class methods
Manager();
virtual ~Manager();
#ifdef ORB_COMMUNICATOR
/**
* Interface to process a received topic from remote.
* @param topic_name
* This represents the uORB message Name (topic); This message Name should be
* globally unique.
* @param isAdvertisement
* Represents if the topic has been advertised or is no longer avialable.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_remote_topic(const char *topic_name, bool isAdvertisement);
/**
* Interface to process a received AddSubscription from remote.
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_add_subscription(const char *messageName, int32_t msgRateInHz);
/**
* Interface to process a received control msg to remove subscription
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_remove_subscription(const char *messageName);
/**
* Interface to process the received data message.
* @param messageName
* This represents the uORB message Name; This message Name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully handled in the
* handler.
* otherwise = failure.
*/
virtual int16_t process_received_message(const char *messageName, int32_t length, uint8_t *data);
#endif /* ORB_COMMUNICATOR */
#ifdef ORB_USE_PUBLISHER_RULES
struct PublisherRule {
const char **topics; //null-terminated list of topic names
const char *module_name; //only this module is allowed to publish one of the topics
bool ignore_other_topics;
};
/**
* test if str starts with pre
*/
bool startsWith(const char *pre, const char *str);
/**
* find a topic in a rule
*/
bool findTopic(const PublisherRule &rule, const char *topic_name);
/**
* trim whitespace from the beginning of a string
*/
void strTrim(const char **str);
/**
* Read publisher rules from a file. It has the format:
*
* restrict_topics: <topic1>, <topic2>, <topic3>
* module: <module_name>
* [ignore_others:true]
*
* @return 0 on success, <0 otherwise
*/
int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule);
PublisherRule _publisher_rule;
bool _has_publisher_rules = false;
#endif /* ORB_USE_PUBLISHER_RULES */
};
#endif /* _uORBManager_hpp_ */
src/modules/uORB/uORBTopics.h
-50
View File
@@ -1,50 +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.
*
****************************************************************************/
#ifndef MODULES_UORB_UORBTOPICS_H_
#define MODULES_UORB_UORBTOPICS_H_
#include <uORB/uORB.h>
/*
* Returns count of all declared topics.
* It is equal to size of array from orb_get_topics()
*/
extern size_t orb_topics_count() __EXPORT;
/*
* Returns array of topics metadata
*/
extern const struct orb_metadata *const *orb_get_topics() __EXPORT;
#endif /* MODULES_UORB_UORBTOPICS_H_ */
src/modules/uORB/uORBUtils.cpp
-72
View File
@@ -1,72 +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.
*
****************************************************************************/
#include "uORBUtils.hpp"
#include <stdio.h>
#include <errno.h>
int uORB::Utils::node_mkpath(char *buf, 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", "obj", meta->o_name, index);
if (len >= orb_maxpath) {
return -ENAMETOOLONG;
}
return OK;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
int uORB::Utils::node_mkpath(char *buf, const char *orbMsgName)
{
unsigned len;
unsigned index = 0;
len = snprintf(buf, orb_maxpath, "/%s/%s%d", "obj", orbMsgName, index);
if (len >= orb_maxpath) {
return -ENAMETOOLONG;
}
return OK;
}
src/modules/uORB/uORBUtils.hpp
-55
View File
@@ -1,55 +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.
*
****************************************************************************/
#ifndef _uORBUtils_hpp_
#define _uORBUtils_hpp_
#include "uORBCommon.hpp"
namespace uORB
{
class Utils;
}
class uORB::Utils
{
public:
static int node_mkpath(char *buf, const struct orb_metadata *meta, int *instance = nullptr);
/**
* same as above except this generators the path based on the string.
*/
static int node_mkpath(char *buf, const char *orbMsgName);
};
#endif // _uORBUtils_hpp_
src/modules/uORB/uORB_tests/CMakeLists.txt
-41
View File
@@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
px4_add_module(
MODULE modules__uORB__uORB_tests
MAIN uorb_tests
PRIORITY "SCHED_PRIORITY_MAX"
SRCS
uORB_tests_main.cpp
uORBTest_UnitTest.cpp
)
src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
-1120
View File
File diff suppressed because it is too large Load Diff
src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
-122
View File
@@ -1,122 +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.
*
****************************************************************************/
#ifndef _uORBTest_UnitTest_hpp_
#define _uORBTest_UnitTest_hpp_
#include <uORB/uORB.h>
#include <uORB/uORBDeviceMaster.hpp>
#include <uORB/uORBDeviceNode.hpp>
#include <uORB/uORBManager.hpp>
#include <uORB/topics/orb_test.h>
#include <uORB/topics/orb_test_medium.h>
#include <uORB/topics/orb_test_large.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <px4_platform_common/tasks.h>
#include <drivers/drv_hrt.h>
#include <errno.h>
#include <unistd.h>
namespace uORBTest
{
class UnitTest;
}
class uORBTest::UnitTest
{
public:
// Singleton pattern
static uORBTest::UnitTest &instance();
~UnitTest() = default;
int test();
int latency_test(bool print);
int info();
// Disallow copy
UnitTest(const uORBTest::UnitTest & /*unused*/) = delete;
// Assist in testing the wrap-around situation
static void set_generation(uORB::DeviceNode &node, unsigned generation)
{
node._generation.store(generation);
}
private:
UnitTest() = default;
static int pubsubtest_threadEntry(int argc, char *argv[]);
int pubsublatency_main();
static int pub_test_multi2_entry(int argc, char *argv[]);
int pub_test_multi2_main();
volatile bool _thread_should_exit;
bool pubsubtest_passed{false};
bool pubsubtest_print{false};
int pubsubtest_res = OK;
orb_advert_t _pfd[4] {}; ///< used for test_multi and test_multi_reversed
int test_single();
/* These 3 depend on each other and must be called in this order */
int test_multi();
int test_multi_reversed();
int test_unadvertise();
int test_multi2();
int test_wrap_around();
int test_SubscriptionMulti();
/* queuing tests */
int test_queue();
static int pub_test_queue_entry(int argc, char *argv[]);
int pub_test_queue_main();
int test_queue_poll_notify();
volatile int _num_messages_sent = 0;
int test_fail(const char *fmt, ...);
int test_note(const char *fmt, ...);
};
#endif // _uORBTest_UnitTest_hpp_
src/modules/uORB/uORB_tests/uORB_tests_main.cpp
-75
View File
@@ -1,75 +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.
*
****************************************************************************/
#include <string.h>
#include "uORBTest_UnitTest.hpp"
extern "C" { __EXPORT int uorb_tests_main(int argc, char *argv[]); }
static void usage()
{
PX4_INFO("Usage: uorb_tests [latency_test]");
}
int
uorb_tests_main(int argc, char *argv[])
{
/*
* Test the driver/device.
*/
if (argc == 1) {
uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
int rc = t.test();
if (rc == OK) {
PX4_INFO("PASS");
return 0;
} else {
PX4_ERR("FAIL");
return -1;
}
}
/*
* Test the latency.
*/
if (argc > 1 && !strcmp(argv[1], "latency_test")) {
uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
return t.latency_test(true);
}
usage();
return -EINVAL;
}