diff --git a/src/modules/uORB/ORBMap.hpp b/src/modules/uORB/ORBMap.hpp index 3d26735809..8c7e1df2a5 100644 --- a/src/modules/uORB/ORBMap.hpp +++ b/src/modules/uORB/ORBMap.hpp @@ -139,6 +139,11 @@ public: } } + Node *top() const + { + return _top; + } + private: Node *_top; Node *_end; diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 9b2780fb25..5f9df9791a 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/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(); diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index 9d30be2a52..dd371f6716 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/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; }; diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index d61a887949..aa98aacb90 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/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(); diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index bcfbc41951..0d549f6fce 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/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 _node_map; + hrt_abstime _last_statistics_output; }; #endif /* _uORBDeviceNode_posix.hpp */ diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index c86d7caa26..0f0cd0d9af 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/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");