uORB::Subscription subscribe directly to uORB device node object

src/modules/uORB/CMakeLists.txt

@@ -32,10 +32,10 @@
 ############################################################################
 
 if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to platform layer)
-	
+
 	# this includes the generated topics directory
 	include_directories(${CMAKE_CURRENT_BINARY_DIR})
-	
+
 	px4_add_module(
 		MODULE modules__uORB
 		MAIN uorb
@@ -43,6 +43,7 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
 		SRCS
 			Publication.cpp
 			Subscription.cpp
+			SubscriptionPollable.cpp
 			uORB.cpp
 			uORBDeviceMaster.cpp
 			uORBDeviceNode.cpp
@@ -53,7 +54,7 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
 			cdev
 			uorb_msgs
 		)
-	
+
 	if(PX4_TESTING)
 		add_subdirectory(uORB_tests)
 	endif()

src/modules/uORB/Subscription.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   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
@@ -42,67 +42,78 @@
 namespace uORB
 {
 
-SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta, unsigned interval, unsigned instance) :
-	_meta(meta),
-	_instance(instance)
+bool Subscription::subscribe()
 {
-	if (instance > 0) {
-		_handle = orb_subscribe_multi(_meta, instance);
+	DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
+	_node = device_master->getDeviceNode(_meta, _instance);
 
-	} else {
-		_handle = orb_subscribe(_meta);
+	if (_node != nullptr) {
+		_node->add_internal_subscriber();
+
+		// If there were any previous publications, allow the subscriber to read them
+		const unsigned curr_gen = _node->published_message_count();
+		const unsigned q_size = _node->get_queue_size();
+
+		_last_generation = curr_gen - (q_size < curr_gen ? q_size : curr_gen);
+
+		return true;
 	}
 
-	if (_handle < 0) {
-		PX4_ERR("%s sub failed", _meta->o_name);
-	}
-
-	if (interval > 0) {
-		orb_set_interval(_handle, interval);
-	}
+	return false;
 }
 
-bool SubscriptionBase::updated()
+void Subscription::unsubscribe()
 {
-	bool isUpdated = false;
-
-	if (orb_check(_handle, &isUpdated) != PX4_OK) {
-		PX4_ERR("%s check failed", _meta->o_name);
+	if (_node != nullptr) {
+		_node->remove_internal_subscriber();
 	}
 
-	return isUpdated;
+	_last_generation = 0;
 }
 
-bool SubscriptionBase::update(void *data)
+bool Subscription::init()
 {
-	bool orb_updated = false;
+	if (_meta != nullptr) {
+		// this throttles the relatively expensive calls to getDeviceNode()
+		if ((_last_generation == 0) || (_last_generation < 1000) || (_last_generation % 100 == 0))  {
+			if (subscribe()) {
+				return true;
+			}
+		}
 
-	if (updated()) {
-		if (orb_copy(_meta, _handle, data) != PX4_OK) {
-			PX4_ERR("%s copy failed", _meta->o_name);
-
-		} else {
-			orb_updated = true;
+		if (_node == nullptr) {
+			// use generation to count attempts to subscribe
+			_last_generation++;
 		}
 	}
 
-	return orb_updated;
+	return false;
 }
 
-SubscriptionBase::~SubscriptionBase()
+bool Subscription::forceInit()
 {
-	if (orb_unsubscribe(_handle) != PX4_OK) {
-		PX4_ERR("%s unsubscribe failed", _meta->o_name);
+	if (_node == nullptr) {
+		// reset generation to force subscription attempt
+		_last_generation = 0;
+		return subscribe();
 	}
+
+	return false;
 }
 
-SubscriptionNode::SubscriptionNode(const struct orb_metadata *meta, unsigned interval, unsigned instance,
-				   List<SubscriptionNode *> *list)
-	: SubscriptionBase(meta, interval, instance)
+bool Subscription::update(uint64_t *time, void *dst)
 {
-	if (list != nullptr) {
-		list->add(this);
+	if ((time != nullptr) && (dst != nullptr) && published()) {
+		// always copy data to dst regardless of update
+		const uint64_t t = _node->copy_and_get_timestamp(dst, _last_generation);
+
+		if (*time == 0 || *time != t) {
+			*time = t;
+			return true;
+		}
 	}
+
+	return false;
 }
 
 } // namespace uORB

src/modules/uORB/Subscription.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   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
@@ -39,159 +39,204 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include <containers/List.hpp>
-#include <systemlib/err.h>
 #include <px4_defines.h>
 
+#include "uORBDeviceNode.hpp"
+#include "uORBManager.hpp"
+#include "uORBUtils.hpp"
+
 namespace uORB
 {
 
-/**
- * Base subscription wrapper class, used in list traversal
- * of various subscriptions.
- */
-class __EXPORT SubscriptionBase
+// Base subscription wrapper class
+class Subscription
 {
 public:
+
 	/**
 	 * Constructor
 	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID()
-	 * 	macro) for the topic.
-	 * @param interval  The minimum interval in milliseconds
-	 * 	between updates
+	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
 	 * @param instance The instance for multi sub.
 	 */
-	SubscriptionBase(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0);
-	virtual ~SubscriptionBase();
+	Subscription(const orb_metadata *meta, uint8_t instance = 0) : _meta(meta), _instance(instance)
+	{
+		init();
+	}
 
-	// no copy, assignment, move, move assignment
-	SubscriptionBase(const SubscriptionBase &) = delete;
-	SubscriptionBase &operator=(const SubscriptionBase &) = delete;
-	SubscriptionBase(SubscriptionBase &&) = delete;
-	SubscriptionBase &operator=(SubscriptionBase &&) = delete;
+	virtual ~Subscription() { unsubscribe(); }
+
+	bool init();
+	bool forceInit();
+
+	bool valid() const { return _node != nullptr; }
+	bool published() { return valid() ? _node->is_published() : init(); }
 
 	/**
 	 * Check if there is a new update.
 	 * */
-	bool updated();
+	virtual bool updated() { return published() ? (_node->published_message_count() != _last_generation) : false; }
 
 	/**
 	 * Update the struct
 	 * @param data The uORB message struct we are updating.
 	 */
-	bool update(void *data);
+	virtual bool update(void *dst) { return updated() ? copy(dst) : false; }
 
-	int getHandle() const { return _handle; }
+	/**
+	 * Check if subscription updated based on timestamp.
+	 *
+	 * @return true only if topic was updated based on a timestamp and
+	 * copied to buffer successfully.
+	 * If topic was not updated since last check it will return false but
+	 * still copy the data.
+	 * If no data available data buffer will be filled with zeros.
+	 */
+	bool update(uint64_t *time, void *dst);
 
-	const orb_metadata *getMeta() const { return _meta; }
+	/**
+	 * Copy the struct
+	 * @param data The uORB message struct we are updating.
+	 */
+	bool copy(void *dst) { return published() ? _node->copy(dst, _last_generation) : false; }
 
-	unsigned getInstance() const { return _instance; }
+	hrt_abstime	last_update() { return published() ? _node->last_update() : 0; }
+
+	uint8_t		get_instance() const { return _instance; }
+	orb_id_t	get_topic() const { return _meta; }
 
 protected:
-	const struct orb_metadata *_meta;
 
-	unsigned _instance;
+	bool subscribe();
+	void unsubscribe();
 
-	int _handle{-1};
+	DeviceNode		*_node{nullptr};
+	const orb_metadata	*_meta{nullptr};
+
+	/**
+	 * Subscription's latest data generation.
+	 * Also used to track (and rate limit) subscription
+	 * attempts if the topic has not yet been published.
+	 */
+	unsigned		_last_generation{0};
+	uint8_t			_instance{0};
 };
 
-/**
- * alias class name so it is clear that the base class
- */
-typedef SubscriptionBase SubscriptionTiny;
-
-/**
- * The subscription base class as a list node.
- */
-class __EXPORT SubscriptionNode : public SubscriptionBase, public ListNode<SubscriptionNode *>
+// Subscription wrapper class with configured interval
+class SubscriptionInterval : public Subscription
 {
 public:
 	/**
 	 * Constructor
 	 *
-	 * @param meta The uORB metadata (usually from the ORB_ID()
-	 * 	macro) for the topic.
-	 * @param interval  The minimum interval in milliseconds
-	 * 	between updates
+	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds between updates
 	 * @param instance The instance for multi sub.
-	 * @param list 	A pointer to a list of subscriptions
-	 * 	that this should be appended to.
 	 */
-	SubscriptionNode(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
-			 List<SubscriptionNode *> *list = nullptr);
+	SubscriptionInterval(const orb_metadata *meta, unsigned interval = 0, uint8_t instance = 0) :
+		Subscription(meta, instance),
+		_interval(interval)
+	{}
 
-	virtual ~SubscriptionNode() override = default;
+	virtual ~SubscriptionInterval() = default;
 
-	/**
-	 * This function is the callback for list traversal
-	 * updates, a child class must implement it.
-	 */
-	virtual bool update() = 0;
-
-	/**
-	 * Like update(), but does not check first if there is data available
-	 */
-	virtual bool forcedUpdate() = 0;
-
-};
-
-/**
- * Subscription wrapper class
- */
-template<class T>
-class __EXPORT Subscription : public SubscriptionNode
-{
-public:
-	/**
-	 * Constructor
-	 *
-	 * @param meta The uORB metadata (usually from
-	 * 	the ORB_ID() macro) for the topic.
-	 * @param interval  The minimum interval in milliseconds
-	 * 	between updates
-	 * @param list A list interface for adding to
-	 * 	list during construction
-	 */
-	Subscription(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
-		     List<SubscriptionNode *> *list = nullptr):
-		SubscriptionNode(meta, interval, instance, list),
-		_data() // initialize data structure to zero
+	bool updated() override
 	{
-		forcedUpdate();
+		if (hrt_elapsed_time(&_last_update) >= (_interval * 1000)) {
+			return Subscription::updated();
+		}
+
+		return false;
 	}
 
-	~Subscription() override = default;
+	bool update(void *dst) override
+	{
+		if (updated()) {
+			if (copy(dst)) {
+				_last_update = hrt_absolute_time();
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	int get_interval() const { return _interval; }
+	void set_interval(unsigned interval) { _interval = interval; }
+
+protected:
+	uint64_t _last_update{0};	// last update in microseconds
+	unsigned _interval{0};		// interval in milliseconds
+};
+
+// Subscription wrapper class with data
+template<class T>
+class SubscriptionData : public Subscription
+{
+public:
+	/**
+	 * 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);
+	}
+
+	virtual ~SubscriptionData() = default;
 
 	// no copy, assignment, move, move assignment
-	Subscription(const Subscription &) = delete;
-	Subscription &operator=(const Subscription &) = delete;
-	Subscription(Subscription &&) = delete;
-	Subscription &operator=(Subscription &&) = delete;
+	SubscriptionData(const SubscriptionData &) = delete;
+	SubscriptionData &operator=(const SubscriptionData &) = delete;
+	SubscriptionData(SubscriptionData &&) = delete;
+	SubscriptionData &operator=(SubscriptionData &&) = delete;
 
-	/**
-	 * Create an update function that uses the embedded struct.
-	 */
-	bool update() override
-	{
-		return SubscriptionBase::update((void *)(&_data));
-	}
+	// update the embedded struct.
+	bool update() { return Subscription::update((void *)(&_data)); }
 
-	bool forcedUpdate() override
-	{
-		return orb_copy(_meta, _handle, &_data) == PX4_OK;
-	}
-
-	/*
-	 * This function gets the T struct data
-	 * */
-	const T &get() const
-	{
-		return _data;
-	}
+	const T &get() const { return _data; }
 
 private:
-	T _data;
+
+	T _data{};
+};
+
+// Subscription wrapper class with data and configured interval
+template<class T>
+class SubscriptionIntervalData : public SubscriptionInterval
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds between updates
+	 * @param instance The instance for multi sub.
+	 */
+	SubscriptionIntervalData(const orb_metadata *meta, unsigned interval = 0, uint8_t instance = 0) :
+		SubscriptionInterval(meta, interval, instance)
+	{
+		copy(&_data);
+	}
+
+	~SubscriptionIntervalData() override = default;
+
+	// no copy, assignment, move, move assignment
+	SubscriptionIntervalData(const SubscriptionIntervalData &) = delete;
+	SubscriptionIntervalData &operator=(const SubscriptionIntervalData &) = delete;
+	SubscriptionIntervalData(SubscriptionIntervalData &&) = delete;
+	SubscriptionIntervalData &operator=(SubscriptionIntervalData &&) = delete;
+
+	// update the embedded struct.
+	bool update() { return SubscriptionInterval::update((void *)(&_data)); }
+
+	const T &get() const { return _data; }
+
+private:
+	T _data{};
 };
 
 } // namespace uORB

src/modules/uORB/SubscriptionPollable.cpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2017 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 SubscriptionPollable.cpp
+ *
+ */
+
+#include "SubscriptionPollable.hpp"
+#include <px4_defines.h>
+
+namespace uORB
+{
+
+SubscriptionPollableBase::SubscriptionPollableBase(const struct orb_metadata *meta, unsigned interval,
+		unsigned instance) :
+	_meta(meta),
+	_instance(instance)
+{
+	if (instance > 0) {
+		_handle = orb_subscribe_multi(_meta, instance);
+
+	} else {
+		_handle = orb_subscribe(_meta);
+	}
+
+	if (_handle < 0) {
+		PX4_ERR("%s sub failed", _meta->o_name);
+	}
+
+	if (interval > 0) {
+		orb_set_interval(_handle, interval);
+	}
+}
+
+bool SubscriptionPollableBase::updated()
+{
+	bool isUpdated = false;
+
+	if (orb_check(_handle, &isUpdated) != PX4_OK) {
+		PX4_ERR("%s check failed", _meta->o_name);
+	}
+
+	return isUpdated;
+}
+
+bool SubscriptionPollableBase::update(void *data)
+{
+	bool orb_updated = false;
+
+	if (updated()) {
+		if (orb_copy(_meta, _handle, data) != PX4_OK) {
+			PX4_ERR("%s copy failed", _meta->o_name);
+
+		} else {
+			orb_updated = true;
+		}
+	}
+
+	return orb_updated;
+}
+
+SubscriptionPollableBase::~SubscriptionPollableBase()
+{
+	if (orb_unsubscribe(_handle) != PX4_OK) {
+		PX4_ERR("%s unsubscribe failed", _meta->o_name);
+	}
+}
+
+SubscriptionPollableNode::SubscriptionPollableNode(const struct orb_metadata *meta, unsigned interval,
+		unsigned instance,
+		List<SubscriptionPollableNode *> *list)
+	: SubscriptionPollableBase(meta, interval, instance)
+{
+	if (list != nullptr) {
+		list->add(this);
+	}
+}
+
+} // namespace uORB

src/modules/uORB/SubscriptionPollable.hpp

@@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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 SubscriptionPollable.hpp
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include <containers/List.hpp>
+#include <systemlib/err.h>
+#include <px4_defines.h>
+
+namespace uORB
+{
+
+/**
+ * Base subscription wrapper class, used in list traversal
+ * of various subscriptions.
+ */
+class __EXPORT SubscriptionPollableBase
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID()
+	 * 	macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds
+	 * 	between updates
+	 * @param instance The instance for multi sub.
+	 */
+	SubscriptionPollableBase(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0);
+	virtual ~SubscriptionPollableBase();
+
+	// no copy, assignment, move, move assignment
+	SubscriptionPollableBase(const SubscriptionPollableBase &) = delete;
+	SubscriptionPollableBase &operator=(const SubscriptionPollableBase &) = delete;
+	SubscriptionPollableBase(SubscriptionPollableBase &&) = delete;
+	SubscriptionPollableBase &operator=(SubscriptionPollableBase &&) = delete;
+
+	/**
+	 * Check if there is a new update.
+	 * */
+	bool updated();
+
+	/**
+	 * Update the struct
+	 * @param data The uORB message struct we are updating.
+	 */
+	bool update(void *data);
+
+	int getHandle() const { return _handle; }
+
+	const orb_metadata *getMeta() const { return _meta; }
+
+	unsigned getInstance() const { return _instance; }
+
+protected:
+	const struct orb_metadata *_meta;
+
+	unsigned _instance;
+
+	int _handle{-1};
+};
+
+/**
+ * alias class name so it is clear that the base class
+ */
+typedef SubscriptionPollableBase SubscriptionPollableTiny;
+
+/**
+ * The subscription base class as a list node.
+ */
+class __EXPORT SubscriptionPollableNode : public SubscriptionPollableBase, public ListNode<SubscriptionPollableNode *>
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID()
+	 * 	macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds
+	 * 	between updates
+	 * @param instance The instance for multi sub.
+	 * @param list 	A pointer to a list of subscriptions
+	 * 	that this should be appended to.
+	 */
+	SubscriptionPollableNode(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
+				 List<SubscriptionPollableNode *> *list = nullptr);
+
+	virtual ~SubscriptionPollableNode() override = default;
+
+	/**
+	 * This function is the callback for list traversal
+	 * updates, a child class must implement it.
+	 */
+	virtual bool update() = 0;
+
+	/**
+	 * Like update(), but does not check first if there is data available
+	 */
+	virtual bool forcedUpdate() = 0;
+
+};
+
+/**
+ * SubscriptionPollable wrapper class
+ */
+template<class T>
+class __EXPORT SubscriptionPollable : public SubscriptionPollableNode
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from
+	 * 	the ORB_ID() macro) for the topic.
+	 * @param interval  The minimum interval in milliseconds
+	 * 	between updates
+	 * @param list A list interface for adding to
+	 * 	list during construction
+	 */
+	SubscriptionPollable(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
+			     List<SubscriptionPollableNode *> *list = nullptr):
+		SubscriptionPollableNode(meta, interval, instance, list),
+		_data() // initialize data structure to zero
+	{
+		forcedUpdate();
+	}
+
+	~SubscriptionPollable() override = default;
+
+	// no copy, assignment, move, move assignment
+	SubscriptionPollable(const SubscriptionPollable &) = delete;
+	SubscriptionPollable &operator=(const SubscriptionPollable &) = delete;
+	SubscriptionPollable(SubscriptionPollable &&) = delete;
+	SubscriptionPollable &operator=(SubscriptionPollable &&) = delete;
+
+	/**
+	 * Create an update function that uses the embedded struct.
+	 */
+	bool update() override
+	{
+		return SubscriptionPollableBase::update((void *)(&_data));
+	}
+
+	bool forcedUpdate() override
+	{
+		return orb_copy(_meta, _handle, &_data) == PX4_OK;
+	}
+
+	/*
+	 * This function gets the T struct data
+	 * */
+	const T &get() const
+	{
+		return _data;
+	}
+
+private:
+	T _data;
+};
+
+} // namespace uORB

src/modules/uORB/uORBDeviceNode.cpp

@@ -161,6 +161,75 @@ uORB::DeviceNode::close(cdev::file_t *filp)
 	return CDev::close(filp);
 }
 
+bool
+uORB::DeviceNode::copy_locked(void *dst, unsigned &generation)
+{
+	bool updated = false;
+
+	if ((dst != nullptr) && (_data != nullptr)) {
+
+		if (_generation > generation + _queue_size) {
+			// Reader is too far behind: some messages are lost
+			_lost_messages += _generation - (generation + _queue_size);
+			generation = _generation - _queue_size;
+		}
+
+		if ((_generation == generation) && (generation > 0)) {
+			/* The subscriber already read the latest message, but nothing new was published yet.
+			 * Return the previous message
+			 */
+			--generation;
+		}
+
+		memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
+
+		if (generation < _generation) {
+			++generation;
+		}
+
+		updated = true;
+	}
+
+	return updated;
+}
+
+bool
+uORB::DeviceNode::copy(void *dst, unsigned &generation)
+{
+	ATOMIC_ENTER;
+
+	bool updated = copy_locked(dst, generation);
+
+	ATOMIC_LEAVE;
+
+	return updated;
+}
+
+uint64_t
+uORB::DeviceNode::copy_and_get_timestamp(void *dst, unsigned &generation)
+{
+	ATOMIC_ENTER;
+
+	const hrt_abstime update_time = _last_update;
+	copy_locked(dst, generation);
+
+	ATOMIC_LEAVE;
+
+	return update_time;
+}
+
+hrt_abstime
+uORB::DeviceNode::last_update()
+{
+	ATOMIC_ENTER;
+
+	const hrt_abstime update_time = _last_update;
+
+	ATOMIC_LEAVE;
+
+	return update_time;
+}
+
 ssize_t
 uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
 {
@@ -181,29 +250,7 @@ uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
 	 */
 	ATOMIC_ENTER;
 
-	const unsigned gen = published_message_count();
-
-	if (gen > sd->generation + _queue_size) {
-		/* Reader is too far behind: some messages are lost */
-		_lost_messages += gen - (sd->generation + _queue_size);
-		sd->generation = gen - _queue_size;
-	}
-
-	if (gen == sd->generation && sd->generation > 0) {
-		/* The subscriber already read the latest message, but nothing new was published yet.
-		 * Return the previous message
-		 */
-		--sd->generation;
-	}
-
-	/* if the caller doesn't want the data, don't give it to them */
-	if (nullptr != buffer) {
-		memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size);
-	}
-
-	if (sd->generation < gen) {
-		++sd->generation;
-	}
+	copy_locked(buffer, sd->generation);
 
 	// if subscriber has an interval track the last update time
 	if (sd->update_interval) {

src/modules/uORB/uORBDeviceNode.hpp

@@ -195,6 +195,42 @@ public:
 	int get_priority() const { return _priority; }
 	void set_priority(uint8_t priority) { _priority = priority; }
 
+	/**
+	 * Copies the timestamp of the last update atomically.
+	 *
+	 * @return uint64_t
+	 *   Returns the timestamp of the most recent data.
+	 */
+	hrt_abstime last_update();
+
+	/**
+	 * 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);
+
+	/**
+	 * Copies data and the corresponding generation
+	 * from a node to the buffer provided.
+	 *
+	 * @param dst
+	 *   The buffer into which the data is copied.
+	 *   If topic was not updated since last check it will return false but
+	 *   still copy the data.
+	 * @param generation
+	 *   The generation that was copied.
+	 * @return uint64_t
+	 *   Returns the timestamp of the copied data.
+	 */
+	uint64_t copy_and_get_timestamp(void *dst, unsigned &generation);
+
 protected:
 
 	pollevent_t poll_state(cdev::file_t *filp) override;
@@ -203,6 +239,19 @@ protected:
 
 private:
 
+	/**
+	 * Copies data and the corresponding generation
+	 * from a node to the buffer provided. Caller handles locking.
+	 *
+	 * @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_locked(void *dst, unsigned &generation);
+
 	struct UpdateIntervalData {
 		uint64_t last_update{0}; /**< time at which the last update was provided, used when update_interval is nonzero */
 		unsigned interval{0}; /**< if nonzero minimum interval between updates */