orb status: print information about lost messages

src/modules/uORB/ORBMap.hpp

@@ -139,6 +139,11 @@ public:
 		}
 	}
 
+	Node *top() const
+	{
+		return _top;
+	}
+
 private:
 	Node *_top;
 	Node *_end;

src/modules/uORB/uORBDevices_nuttx.cpp

@@ -190,6 +190,7 @@ uORB::DeviceNode::read(struct file *filp, char *buffer, size_t buflen)
 
 	if (_generation > sd->generation + _queue_size) {
 		/* Reader is too far behind: some messages are lost */
+		_lost_messages += _generation - (sd->generation + _queue_size);
 		sd->generation = _generation - _queue_size;
 	}
 
@@ -549,6 +550,27 @@ uORB::DeviceNode::update_deferred_trampoline(void *arg)
 	node->update_deferred();
 }
 
+bool
+uORB::DeviceNode::print_statistics(bool reset)
+{
+	if (!_lost_messages) {
+		return false;
+	}
+
+	lock();
+	//This can be wrong: if a reader never reads, _lost_messages will not be increased either
+	uint32_t lost_messages = _lost_messages;
+
+	if (reset) {
+		_lost_messages = 0;
+	}
+
+	unlock();
+
+	PX4_INFO("%s: %i", _meta->o_name, lost_messages);
+	return true;
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 void uORB::DeviceNode::add_internal_subscriber()
@@ -652,7 +674,7 @@ uORB::DeviceMaster::DeviceMaster(Flavor f) :
 {
 	// enable debug() calls
 	_debug_enabled = true;
-
+	_last_statistics_output = hrt_absolute_time();
 }
 
 uORB::DeviceMaster::~DeviceMaster()
@@ -771,6 +793,35 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
 	}
 }
 
+void uORB::DeviceMaster::printStatistics(bool reset)
+{
+	hrt_abstime current_time = hrt_absolute_time();
+	PX4_INFO("Statistics, since last output (%i ms):",
+		 (int)((current_time - _last_statistics_output) / 1000));
+	_last_statistics_output = current_time;
+
+	PX4_INFO("TOPIC, NR LOST MSGS");
+	bool had_print = false;
+
+	lock();
+	ORBMap::Node *node = _node_map.top();
+
+	while (node) {
+		if (node->node->print_statistics(reset)) {
+			had_print = true;
+		}
+
+		node = node->next;
+	}
+
+	unlock();
+
+	if (!had_print) {
+		PX4_INFO("No lost messages");
+	}
+}
+
+
 uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
 {
 	lock();

src/modules/uORB/uORBDevices_nuttx.hpp

@@ -179,6 +179,13 @@ public:
 	 */
 	int update_queue_size(unsigned int queue_size);
 
+	/**
+	 * Print statistics (nr of lost messages)
+	 * @param reset if true, reset statistics afterwards
+	 * @return true if printed something, false otherwise (if no lost messages)
+	 */
+	bool print_statistics(bool reset);
+
 protected:
 	virtual pollevent_t poll_state(struct file *filp);
 	virtual void poll_notify_one(struct pollfd *fds, pollevent_t events);
@@ -212,8 +219,6 @@ private:
 	bool _published;  /**< has ever data been published */
 	unsigned int _queue_size; /**< maximum number of elements in the queue */
 
-private: // private class methods.
-
 	static SubscriberData    *filp_to_sd(struct file *filp)
 	{
 		SubscriberData *sd = (SubscriberData *)(filp->f_priv);
@@ -223,6 +228,10 @@ private: // private class methods.
 	bool    _IsRemoteSubscriberPresent;
 	int32_t _subscriber_count;
 
+	//statistics
+	uint32_t _lost_messages = 0; ///< nr of lost messages for all subscribers. If two subscribers lose the same
+	///message, it is counted as two.
+
 	/**
 	 * Perform a deferred update for a rate-limited subscriber.
 	 */
@@ -265,6 +274,12 @@ public:
 	 */
 	uORB::DeviceNode *getDeviceNode(const char *node_name);
 
+	/**
+	 * Print statistics for each existing topic.
+	 * @param reset if true, reset statistics afterwards
+	 */
+	void printStatistics(bool reset);
+
 private:
 	// Private constructor, uORB::Manager takes care of its creation
 	DeviceMaster(Flavor f);
@@ -281,6 +296,7 @@ private:
 
 	const Flavor  _flavor;
 	ORBMap _node_map;
+	hrt_abstime       _last_statistics_output;
 };
 
 

src/modules/uORB/uORBDevices_posix.cpp

@@ -203,6 +203,7 @@ uORB::DeviceNode::read(device::file_t *filp, char *buffer, size_t buflen)
 
 	if (_generation > sd->generation + _queue_size) {
 		/* Reader is too far behind: some messages are lost */
+		_lost_messages += _generation - (sd->generation + _queue_size);
 		sd->generation = _generation - _queue_size;
 	}
 
@@ -553,6 +554,27 @@ uORB::DeviceNode::update_deferred_trampoline(void *arg)
 	node->update_deferred();
 }
 
+bool
+uORB::DeviceNode::print_statistics(bool reset)
+{
+	if (!_lost_messages) {
+		return false;
+	}
+
+	lock();
+	//This can be wrong: if a reader never reads, _lost_messages will not be increased either
+	uint32_t lost_messages = _lost_messages;
+
+	if (reset) {
+		_lost_messages = 0;
+	}
+
+	unlock();
+
+	PX4_INFO("%s: %i", _meta->o_name, lost_messages);
+	return true;
+}
+
 //-----------------------------------------------------------------------------
 //-----------------------------------------------------------------------------
 void uORB::DeviceNode::add_internal_subscriber()
@@ -657,7 +679,7 @@ uORB::DeviceMaster::DeviceMaster(Flavor f) :
 {
 	// enable debug() calls
 	//_debug_enabled = true;
-
+	_last_statistics_output = hrt_absolute_time();
 }
 
 uORB::DeviceMaster::~DeviceMaster()
@@ -777,6 +799,31 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 	}
 }
 
+void uORB::DeviceMaster::printStatistics(bool reset)
+{
+	hrt_abstime current_time = hrt_absolute_time();
+	PX4_INFO("Statistics, since last output (%i ms):",
+		 (int)((current_time - _last_statistics_output) / 1000));
+	_last_statistics_output = current_time;
+
+	PX4_INFO("TOPIC, NR LOST MSGS");
+
+	lock();
+	bool had_print = false;
+
+	for (const auto &node : _node_map) {
+		if (node.second->print_statistics(reset)) {
+			had_print = true;
+		}
+	}
+
+	unlock();
+
+	if (!had_print) {
+		PX4_INFO("No lost messages");
+	}
+}
+
 uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
 {
 	lock();

src/modules/uORB/uORBDevices_posix.hpp

@@ -116,6 +116,13 @@ public:
 	 */
 	int update_queue_size(unsigned int queue_size);
 
+	/**
+	 * Print statistics (nr of lost messages)
+	 * @param reset if true, reset statistics afterwards
+	 * @return true if printed something, false otherwise (if no lost messages)
+	 */
+	bool print_statistics(bool reset);
+
 protected:
 	virtual pollevent_t poll_state(device::file_t *filp);
 	virtual void    poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events);
@@ -154,6 +161,10 @@ private:
 
 	int32_t _subscriber_count;
 
+	//statistics
+	uint32_t _lost_messages = 0; ///< nr of lost messages for all subscribers. If two subscribers lose the same
+	///message, it is counted as two.
+
 	/**
 	 * Perform a deferred update for a rate-limited subscriber.
 	 */
@@ -199,6 +210,12 @@ public:
 	 */
 	uORB::DeviceNode *getDeviceNode(const char *node_name);
 
+	/**
+	 * Print statistics for each existing topic.
+	 * @param reset if true, reset statistics afterwards
+	 */
+	void printStatistics(bool reset);
+
 private:
 	// Private constructor, uORB::Manager takes care of its creation
 	DeviceMaster(Flavor f);
@@ -215,6 +232,7 @@ private:
 
 	const Flavor      _flavor;
 	std::map<std::string, uORB::DeviceNode *> _node_map;
+	hrt_abstime       _last_statistics_output;
 };
 
 #endif /* _uORBDeviceNode_posix.hpp */

src/modules/uORB/uORBMain.cpp

@@ -86,7 +86,7 @@ uorb_main(int argc, char *argv[])
 	 */
 	if (!strcmp(argv[1], "status")) {
 		if (g_dev != nullptr) {
-			PX4_INFO("uorb is running");
+			g_dev->printStatistics(true);
 
 		} else {
 			PX4_INFO("uorb is not running");