uORB move to PX4 platform layer

platforms/common/uORB/CMakeLists.txt

@@ -0,0 +1,65 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+# this includes the generated topics directory
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+
+px4_add_library(uORB
+	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
+	uORBManager.cpp
+	uORBManager.hpp
+	uORBUtils.cpp
+	uORBUtils.hpp
+)
+
+target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_link_libraries(uORB PRIVATE cdev uorb_msgs)
+
+if(PX4_TESTING)
+	add_subdirectory(uORB_tests)
+endif()

platforms/common/uORB/ORBSet.hpp

@@ -0,0 +1,145 @@
+/****************************************************************************
+ *
+ *   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;
+};

platforms/common/uORB/Publication.hpp

@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ *   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

platforms/common/uORB/PublicationMulti.hpp

@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ *   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

platforms/common/uORB/Subscription.cpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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

platforms/common/uORB/Subscription.hpp

@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   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

platforms/common/uORB/SubscriptionBlocking.hpp

@@ -0,0 +1,171 @@
+/****************************************************************************
+ *
+ *   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

platforms/common/uORB/SubscriptionCallback.hpp

@@ -0,0 +1,191 @@
+/****************************************************************************
+ *
+ *   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

platforms/common/uORB/SubscriptionInterval.hpp

@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   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

platforms/common/uORB/SubscriptionMultiArray.hpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   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

platforms/common/uORB/uORB.cpp

@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   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"
+
+static uORB::DeviceMaster *g_dev = nullptr;
+
+int uorb_start(void)
+{
+	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;
+	}
+
+	return OK;
+}
+
+int uorb_status(void)
+{
+	if (g_dev != nullptr) {
+		g_dev->printStatistics();
+
+	} else {
+		PX4_INFO("uorb is not running");
+	}
+
+	return OK;
+}
+
+int uorb_top(char **topic_filter, int num_filters)
+{
+	if (g_dev != nullptr) {
+		g_dev->showTop(topic_filter, num_filters);
+
+	} else {
+		PX4_INFO("uorb is not running");
+	}
+
+	return OK;
+}
+
+
+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);
+}

platforms/common/uORB/uORB.h

@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ *   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
+
+int uorb_start(void);
+int uorb_status(void);
+int uorb_top(char **topic_filter, int num_filters);
+
+/**
+ * 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 */

platforms/common/uORB/uORBCommon.hpp

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ *   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_

platforms/common/uORB/uORBCommunicator.hpp

@@ -0,0 +1,221 @@
+/****************************************************************************
+ *
+ *   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_ */

platforms/common/uORB/uORBDeviceMaster.cpp

@@ -0,0 +1,491 @@
+/****************************************************************************
+ *
+ *   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;
+}

platforms/common/uORB/uORBDeviceMaster.hpp

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   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); }
+};

platforms/common/uORB/uORBDeviceNode.cpp

@@ -0,0 +1,587 @@
+/****************************************************************************
+ *
+ *   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;
+}

platforms/common/uORB/uORBDeviceNode.hpp

@@ -0,0 +1,256 @@
+/****************************************************************************
+ *
+ *   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};
+};

platforms/common/uORB/uORBManager.cpp

@@ -0,0 +1,649 @@
+/****************************************************************************
+ *
+ *   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 */

platforms/common/uORB/uORBManager.hpp

@@ -0,0 +1,487 @@
+/****************************************************************************
+ *
+ *   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_ */

platforms/common/uORB/uORBTopics.h

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   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_ */

platforms/common/uORB/uORBUtils.cpp

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   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;
+}

platforms/common/uORB/uORBUtils.hpp

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#ifndef _uORBUtils_hpp_
+#define _uORBUtils_hpp_
+
+#include "uORBCommon.hpp"
+
+namespace uORB
+{
+class Utils;
+}
+
+class uORB::Utils
+{
+public:
+	static int node_mkpath(char *buf, 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_

platforms/common/uORB/uORB_tests/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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
+	)

platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp

@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef _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_

platforms/common/uORB/uORB_tests/uORB_tests_main.cpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   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;
+}