uORB: add bitset for faster orb_exists check and remove uORB::Subscription lazy subscribe hack/optimization

- add PX4 bitset and atomic_bitset with testing
 - add uORB::Subscription constructor to take ORB_ID enum
 - move orb test messages into msg/

src/modules/uORB/CMakeLists.txt

@@ -60,7 +60,6 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
 			uORBMain.cpp
 			uORBManager.cpp
 			uORBManager.hpp
-			uORBTopics.h
 			uORBUtils.cpp
 			uORBUtils.hpp
 		DEPENDS

src/modules/uORB/Subscription.cpp

@@ -42,48 +42,49 @@
 namespace uORB
 {
 
-bool
-Subscription::subscribe()
+bool Subscription::subscribe()
 {
-	// valid ORB_ID required
-	if (_meta == nullptr) {
-		return false;
-	}
-
 	// check if already subscribed
 	if (_node != nullptr) {
 		return true;
 	}
 
-	DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
+	if (_orb_id != ORB_ID::INVALID) {
 
-	if (device_master != nullptr) {
-		uORB::DeviceNode *node = device_master->getDeviceNode(_meta, _instance);
+		DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
 
-		if (node != nullptr) {
-			_node = node;
-			_node->add_internal_subscriber();
+		if (device_master != nullptr) {
 
-			// If there were any previous publications, allow the subscriber to read them
-			const unsigned curr_gen = _node->published_message_count();
-			const uint8_t q_size = _node->get_queue_size();
-
-			if (q_size < curr_gen) {
-				_last_generation = curr_gen - q_size;
-
-			} else {
-				_last_generation = 0;
+			if (!device_master->deviceNodeExists(_orb_id, _instance)) {
+				return false;
 			}
 
-			return true;
+			uORB::DeviceNode *node = device_master->getDeviceNode(get_topic(), _instance);
+
+			if (node != nullptr) {
+				_node = node;
+				_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 uint8_t q_size = _node->get_queue_size();
+
+				if (q_size < curr_gen) {
+					_last_generation = curr_gen - q_size;
+
+				} else {
+					_last_generation = 0;
+				}
+
+				return true;
+			}
 		}
 	}
 
 	return false;
 }
 
-void
-Subscription::unsubscribe()
+void Subscription::unsubscribe()
 {
 	if (_node != nullptr) {
 		_node->remove_internal_subscriber();
@@ -93,28 +94,7 @@ Subscription::unsubscribe()
 	_last_generation = 0;
 }
 
-bool
-Subscription::init()
-{
-	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 (_node == nullptr) {
-			// use generation to count attempts to subscribe
-			_last_generation++;
-		}
-	}
-
-	return false;
-}
-
-bool
-Subscription::update(uint64_t *time, void *dst)
+bool Subscription::update(uint64_t *time, void *dst)
 {
 	if ((time != nullptr) && (dst != nullptr) && advertised()) {
 		// always copy data to dst regardless of update

src/modules/uORB/Subscription.hpp

@@ -39,6 +39,8 @@
 #pragma once
 
 #include <uORB/uORB.h>
+#include <uORB/topics/uORBTopics.hpp>
+
 #include <px4_platform_common/defines.h>
 
 #include "uORBDeviceNode.hpp"
@@ -55,15 +57,28 @@ 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)
+	{
+	}
+
 	/**
 	 * 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) : _meta(meta), _instance(instance)
+	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();
 	}
 
 	~Subscription()
@@ -82,7 +97,7 @@ public:
 		}
 
 		// try to initialize
-		if (init()) {
+		if (subscribe()) {
 			// check again if valid
 			if (valid()) {
 				return _node->is_advertised();
@@ -121,7 +136,7 @@ public:
 	bool copy(void *dst) { return advertised() ? _node->copy(dst, _last_generation) : false; }
 
 	uint8_t		get_instance() const { return _instance; }
-	orb_id_t	get_topic() const { return _meta; }
+	orb_id_t	get_topic() const { return get_orb_meta(_orb_id); }
 
 protected:
 
@@ -129,18 +144,12 @@ protected:
 
 	DeviceNode		*get_node() { return _node; }
 
-	bool			init();
-
 	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};
+	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
@@ -148,6 +157,18 @@ 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
 	 *

src/modules/uORB/SubscriptionInterval.hpp

@@ -55,6 +55,18 @@ 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
 	 *

src/modules/uORB/uORB.h

@@ -52,6 +52,7 @@ struct orb_metadata {
 	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;
@@ -110,13 +111,15 @@ enum ORB_PRIO {
  * @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)		\
+#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					\
+		_fields,				\
+		_orb_id_enum				\
 	}; struct hack
 
 __BEGIN_DECLS

src/modules/uORB/uORBDeviceMaster.cpp

@@ -163,6 +163,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertise
 
 			// add to the node map.
 			_node_list.add(node);
+			_node_exists[node->get_instance()].set((uint8_t)node->id(), true);
 		}
 
 		group_tries++;
@@ -452,12 +453,25 @@ uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
 	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();

src/modules/uORB/uORBDeviceMaster.hpp

@@ -36,6 +36,8 @@
 #include <stdint.h>
 
 #include "uORBCommon.hpp"
+#include <uORB/topics/uORBTopics.hpp>
+
 #include <px4_platform_common/posix.h>
 
 namespace uORB
@@ -49,6 +51,9 @@ class Manager;
 #include <stdlib.h>
 
 #include <containers/List.hpp>
+#include <px4_platform_common/atomic_bitset.h>
+
+using px4::AtomicBitset;
 
 /**
  * Master control device for ObjDev.
@@ -69,6 +74,8 @@ public:
 	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.
 	 * @param reset if true, reset statistics afterwards
@@ -111,6 +118,7 @@ private:
 	uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance);
 
 	List<uORB::DeviceNode *> _node_list;
+	AtomicBitset<ORB_TOPICS_COUNT> _node_exists[ORB_MULTI_MAX_INSTANCES];
 
 	hrt_abstime       _last_statistics_output;
 

src/modules/uORB/uORBDeviceNode.hpp

@@ -192,6 +192,8 @@ public:
 
 	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; }

src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp

@@ -40,21 +40,6 @@
 #include <math.h>
 #include <lib/cdev/CDev.hpp>
 
-ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;");
-ORB_DEFINE(orb_multitest, struct orb_test, sizeof(orb_test), "ORB_MULTITEST:int val;hrt_abstime time;");
-
-ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium),
-	   "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;");
-ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium),
-	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
-ORB_DEFINE(orb_test_medium_queue, struct orb_test_medium, sizeof(orb_test_medium),
-	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
-ORB_DEFINE(orb_test_medium_queue_poll, struct orb_test_medium, sizeof(orb_test_medium),
-	   "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;");
-
-ORB_DEFINE(orb_test_large, struct orb_test_large, sizeof(orb_test_large),
-	   "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;");
-
 uORBTest::UnitTest &uORBTest::UnitTest::instance()
 {
 	static uORBTest::UnitTest t;
@@ -73,7 +58,7 @@ int uORBTest::UnitTest::pubsublatency_main()
 	int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0);
 	int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0);
 
-	orb_test_large t{};
+	orb_test_large_s t{};
 
 	/* clear all ready flags */
 	orb_copy(ORB_ID(orb_test), test_multi_sub, &t);
@@ -121,7 +106,7 @@ int uORBTest::UnitTest::pubsublatency_main()
 		num_missed += t.val - current_value - 1;
 		current_value = t.val;
 
-		unsigned elt = (unsigned)hrt_elapsed_time_atomic(&t.time);
+		unsigned elt = (unsigned)hrt_elapsed_time_atomic(&t.timestamp);
 		latency_integral += elt;
 		timings[i] = elt;
 
@@ -241,7 +226,7 @@ int uORBTest::UnitTest::test_unadvertise()
 
 	//try to advertise and see whether we get the right instance
 	int instance_test[4] {};
-	orb_test t{};
+	orb_test_s t{};
 
 	for (int i = 0; i < 4; ++i) {
 		_pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX);
@@ -268,8 +253,8 @@ int uORBTest::UnitTest::test_single()
 {
 	test_note("try single-topic support");
 
-	orb_test t{};
-	orb_test u{};
+	orb_test_s t{};
+	orb_test_s u{};
 	int sfd = -1;
 	orb_advert_t ptopic{};
 	bool updated{false};
@@ -346,8 +331,8 @@ int uORBTest::UnitTest::test_multi()
 	/* this routine tests the multi-topic support */
 	test_note("try multi-topic support");
 
-	orb_test t{};
-	orb_test u{};
+	orb_test_s t{};
+	orb_test_s u{};
 
 	int instance0;
 	_pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
@@ -419,7 +404,7 @@ int uORBTest::UnitTest::test_multi()
 		return test_fail("prio: %d", prio);
 	}
 
-	if (PX4_OK != latency_test<struct orb_test>(ORB_ID(orb_test), false)) {
+	if (PX4_OK != latency_test<orb_test_s>(ORB_ID(orb_test), false)) {
 		return test_fail("latency test failed");
 	}
 
@@ -440,7 +425,7 @@ int uORBTest::UnitTest::pub_test_multi2_main()
 	int data_next_idx = 0;
 	const int num_instances = 3;
 	orb_advert_t orb_pub[num_instances] {};
-	orb_test_medium data_topic{};
+	orb_test_medium_s data_topic{};
 
 	for (int i = 0; i < num_instances; ++i) {
 		orb_advert_t &pub = orb_pub[i];
@@ -464,7 +449,7 @@ int uORBTest::UnitTest::pub_test_multi2_main()
 		px4_usleep(2); //make sure the timestamps are different
 		orb_advert_t &pub = orb_pub[data_next_idx];
 
-		data_topic.time = hrt_absolute_time();
+		data_topic.timestamp = hrt_absolute_time();
 		data_topic.val = data_next_idx;
 
 		orb_publish(ORB_ID(orb_test_medium_multi), pub, &data_topic);
@@ -525,16 +510,16 @@ int uORBTest::UnitTest::test_multi2()
 		orb_check(orb_data_cur_fd, &updated);
 
 		if (updated) {
-			orb_test_medium msg{};
+			orb_test_medium_s msg{};
 			orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg);
 
-			if (last_time >= msg.time && last_time != 0) {
-				return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time);
+			if (last_time >= msg.timestamp && last_time != 0) {
+				return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.timestamp);
 			}
 
-			last_time = msg.time;
+			last_time = msg.timestamp;
 
-			PX4_DEBUG("got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time);
+			PX4_DEBUG("got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.timestamp);
 			orb_data_next = (orb_data_next + 1) % num_instances;
 		}
 	}
@@ -559,7 +544,8 @@ int uORBTest::UnitTest::test_multi_reversed()
 		return test_fail("sub. id2: ret: %d", sfd2);
 	}
 
-	struct orb_test t {}, u {};
+	orb_test_s t{};
+	orb_test_s u{};
 
 	t.val = 0;
 
@@ -621,18 +607,17 @@ int uORBTest::UnitTest::test_queue()
 {
 	test_note("Testing orb queuing");
 
-	struct orb_test_medium t, u;
-	int sfd;
-	orb_advert_t ptopic;
-	bool updated;
+	orb_test_medium_s t{};
+	orb_test_medium_s u{};
+	orb_advert_t ptopic{nullptr};
+	bool updated{false};
 
-	sfd = orb_subscribe(ORB_ID(orb_test_medium_queue));
+	int sfd = orb_subscribe(ORB_ID(orb_test_medium_queue));
 
 	if (sfd < 0) {
 		return test_fail("subscribe failed: %d", errno);
 	}
 
-
 	const int queue_size = 11;
 	t.val = 0;
 	ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
@@ -738,17 +723,18 @@ int uORBTest::UnitTest::pub_test_queue_entry(int argc, char *argv[])
 
 int uORBTest::UnitTest::pub_test_queue_main()
 {
-	struct orb_test_medium t;
-	orb_advert_t ptopic;
+	orb_test_medium_s t{};
+	orb_advert_t ptopic{nullptr};
 	const int queue_size = 50;
-	t.val = 0;
 
 	if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
 		_thread_should_exit = true;
 		return test_fail("advertise failed: %d", errno);
 	}
 
-	int message_counter = 0, num_messages = 20 * queue_size;
+	int message_counter = 0;
+	int num_messages = 20 * queue_size;
+
 	++t.val;
 
 	while (message_counter < num_messages) {
@@ -777,7 +763,7 @@ int uORBTest::UnitTest::test_queue_poll_notify()
 {
 	test_note("Testing orb queuing (poll & notify)");
 
-	orb_test_medium t{};
+	orb_test_medium_s t{};
 	int sfd = -1;
 
 	if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) {

src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp

@@ -33,38 +33,22 @@
 
 #ifndef _uORBTest_UnitTest_hpp_
 #define _uORBTest_UnitTest_hpp_
-#include "../uORBCommon.hpp"
-#include "../uORB.h"
+
+#include <uORB/uORB.h>
+#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>
 
-struct orb_test {
-	int val;
-	hrt_abstime time;
-};
-ORB_DECLARE(orb_test);
-ORB_DECLARE(orb_multitest);
-
-
-struct orb_test_medium {
-	int val;
-	hrt_abstime time;
-	char junk[64];
-};
-ORB_DECLARE(orb_test_medium);
-ORB_DECLARE(orb_test_medium_multi);
-ORB_DECLARE(orb_test_medium_queue);
-ORB_DECLARE(orb_test_medium_queue_poll);
-
-struct orb_test_large {
-	int val;
-	hrt_abstime time;
-	char junk[512];
-};
-ORB_DECLARE(orb_test_large);
-
-
 namespace uORBTest
 {
 class UnitTest;
@@ -125,9 +109,9 @@ template<typename S>
 int uORBTest::UnitTest::latency_test(orb_id_t T, bool print)
 {
 	test_note("---------------- LATENCY TEST ------------------");
-	S t;
+	S t{};
 	t.val = 308;
-	t.time = hrt_absolute_time();
+	t.timestamp = hrt_absolute_time();
 
 	orb_advert_t pfd0 = orb_advertise(T, &t);
 
@@ -155,7 +139,7 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print)
 	/* give the test task some data */
 	while (!pubsubtest_passed) {
 		++t.val;
-		t.time = hrt_absolute_time();
+		t.timestamp = hrt_absolute_time();
 
 		if (PX4_OK != orb_publish(T, pfd0, &t)) {
 			return test_fail("mult. pub0 timing fail");

src/modules/uORB/uORB_tests/uORB_tests_main.cpp

@@ -32,9 +32,6 @@
  ****************************************************************************/
 
 #include <string.h>
-#include "../uORBDeviceNode.hpp"
-#include "../uORB.h"
-#include "../uORBCommon.hpp"
 
 #include "uORBTest_UnitTest.hpp"
 
@@ -73,13 +70,13 @@ uorb_tests_main(int argc, char *argv[])
 		uORBTest::UnitTest &t = uORBTest::UnitTest::instance();
 
 		if (argc > 2 && !strcmp(argv[2], "medium")) {
-			return t.latency_test<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
+			return t.latency_test<orb_test_medium_s>(ORB_ID(orb_test_medium), true);
 
 		} else if (argc > 2 && !strcmp(argv[2], "large")) {
-			return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
+			return t.latency_test<orb_test_large_s>(ORB_ID(orb_test_large), true);
 
 		} else {
-			return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
+			return t.latency_test<orb_test_s>(ORB_ID(orb_test), true);
 		}
 	}