From d1850f5112497dfe05bb1238d884c99c1d3f49fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 8 Sep 2016 18:48:32 +0200 Subject: [PATCH] uorb top: measure the elapsed time to give more accurate results --- src/modules/uORB/uORBDevices.cpp | 33 ++++++++++++++++++++++++-------- src/modules/uORB/uORBDevices.hpp | 2 ++ 2 files changed, 27 insertions(+), 8 deletions(-) diff --git a/src/modules/uORB/uORBDevices.cpp b/src/modules/uORB/uORBDevices.cpp index 3b71a780f1..f5157a8620 100644 --- a/src/modules/uORB/uORBDevices.cpp +++ b/src/modules/uORB/uORBDevices.cpp @@ -1071,7 +1071,10 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) fds.events = POLLIN; bool quit = false; + hrt_abstime start_time = hrt_absolute_time(); + while (!quit) { + /* Sleep 200 ms waiting for user input five times ~ 1s */ for (int k = 0; k < 5; k++) { char c; @@ -1092,6 +1095,25 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) } 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) { + uint32_t num_lost = cur_node->node->lost_message_count(); + unsigned int num_msgs = cur_node->node->published_message_count(); + cur_node->pub_msg_delta = (num_msgs - cur_node->last_pub_msg_count) / dt; + cur_node->lost_msg_delta = (num_lost - cur_node->last_lost_msg_count) / dt; + cur_node->last_lost_msg_count = num_lost; + cur_node->last_pub_msg_count = num_msgs; + cur_node = cur_node->next; + } + + start_time = current_time; + + printf("\033[H"); // move cursor home and clear screen printf(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics); #ifdef __PX4_NUTTX @@ -1102,23 +1124,18 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) cur_node = first_node; while (cur_node) { - uint32_t num_lost = cur_node->node->lost_message_count(); - unsigned int num_msgs = cur_node->node->published_message_count(); - if (!print_active_only || num_msgs - cur_node->last_pub_msg_count > 0) { + if (!print_active_only || cur_node->pub_msg_delta > 0) { #ifdef __PX4_NUTTX printf(CLEAR_LINE "%*-s %2i %4i %4i %5i %i\n", (int)max_topic_name_length, #else printf(CLEAR_LINE "%*s %2i %4i %4i %5i %i\n", -(int)max_topic_name_length, #endif cur_node->node->meta()->o_name, (int)cur_node->instance, - cur_node->node->subscriber_count(), num_msgs - cur_node->last_pub_msg_count, - num_lost - cur_node->last_lost_msg_count, cur_node->node->queue_size()); + cur_node->node->subscriber_count(), cur_node->pub_msg_delta, + cur_node->lost_msg_delta, cur_node->node->queue_size()); } - cur_node->last_lost_msg_count = num_lost; - cur_node->last_pub_msg_count = num_msgs; - cur_node = cur_node->next; } diff --git a/src/modules/uORB/uORBDevices.hpp b/src/modules/uORB/uORBDevices.hpp index cf88e0f9b1..c807473e7d 100644 --- a/src/modules/uORB/uORBDevices.hpp +++ b/src/modules/uORB/uORBDevices.hpp @@ -311,6 +311,8 @@ private: uint8_t instance; uint32_t last_lost_msg_count; unsigned int last_pub_msg_count; + uint32_t lost_msg_delta; + unsigned int pub_msg_delta; DeviceNodeStatisticsData *next = nullptr; }; void addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, size_t &max_topic_name_length,