diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9a63c8e65e..2a6332cb20 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2144,9 +2144,14 @@ Commander::run() /* handle commands last, as the system needs to be updated to handle them */ if (_cmd_sub.updated()) { /* got command */ + const unsigned last_generation = _cmd_sub.get_last_generation(); vehicle_command_s cmd; if (_cmd_sub.copy(&cmd)) { + if (_cmd_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _cmd_sub.get_last_generation()); + } + if (handle_command(&status, cmd, &armed, _command_ack_pub)) { _status_changed = true; } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f180b82387..c2673543e5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2271,9 +2271,17 @@ Mavlink::task_main(int argc, char *argv[]) } } + + // vehicle_command + const unsigned last_generation = cmd_sub.get_last_generation(); vehicle_command_s vehicle_cmd; if (cmd_sub.update(&vehicle_cmd)) { + + if (cmd_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, cmd_sub.get_last_generation()); + } + if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && (_mode == MAVLINK_MODE_IRIDIUM)) { if (vehicle_cmd.param1 > 0.5f) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3eef314180..9b163884dc 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -217,9 +217,14 @@ Navigator::run() _home_pos_sub.update(&_home_pos); if (_vehicle_command_sub.updated()) { + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); vehicle_command_s cmd{}; _vehicle_command_sub.copy(&cmd); + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _vehicle_command_sub.get_last_generation()); + } + if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { // DO_GO_AROUND is currently handled by the position controller (unacknowledged) diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index c5c6de1564..e2506dcef0 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -52,7 +52,6 @@ uORB::DeviceMaster::DeviceMaster() { px4_sem_init(&_lock, 0, 1); - _last_statistics_output = hrt_absolute_time(); } uORB::DeviceMaster::~DeviceMaster() @@ -178,15 +177,8 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver return ret; } -void uORB::DeviceMaster::printStatistics(bool reset) +void uORB::DeviceMaster::printStatistics() { - 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; - /* Add all nodes to a list while locked, and then print them in unlocked state, to avoid potential * dead-locks (where printing blocks) */ lock(); @@ -199,23 +191,20 @@ void uORB::DeviceMaster::printStatistics(bool reset) if (ret != 0) { PX4_ERR("addNewDeviceNodes failed (%i)", ret); + return; } + PX4_INFO_RAW("%-*s INST #SUB #Q SIZE PRIO PATH\n", (int)max_topic_name_length - 2, "TOPIC NAME"); + cur_node = first_node; while (cur_node) { - if (cur_node->node->print_statistics(reset)) { - had_print = true; - } + cur_node->node->print_statistics(max_topic_name_length); DeviceNodeStatisticsData *prev = cur_node; cur_node = cur_node->next; delete prev; } - - if (!had_print) { - PX4_INFO("No lost messages"); - } } int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics, @@ -280,7 +269,6 @@ int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, max_topic_name_length = name_length; } - last_node->last_lost_msg_count = last_node->node->lost_message_count(); last_node->last_pub_msg_count = last_node->node->published_message_count(); } @@ -387,11 +375,8 @@ 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(); cur_node->pub_msg_delta = roundf((num_msgs - cur_node->last_pub_msg_count) / dt); - cur_node->lost_msg_delta = roundf((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; } @@ -404,16 +389,16 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) } PX4_INFO_RAW(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics); - PX4_INFO_RAW(CLEAR_LINE "%-*s INST #SUB RATE #LOST #Q SIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME"); + PX4_INFO_RAW(CLEAR_LINE "%-*s INST #SUB RATE #Q SIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME"); cur_node = first_node; while (cur_node) { if (!print_active_only || (cur_node->pub_msg_delta > 0 && cur_node->node->subscriber_count() > 0)) { - PX4_INFO_RAW(CLEAR_LINE "%-*s %2i %4i %4i %5i %2i %4i \n", (int)max_topic_name_length, + PX4_INFO_RAW(CLEAR_LINE "%-*s %2i %4i %4i %2i %4i \n", (int)max_topic_name_length, cur_node->node->get_meta()->o_name, (int)cur_node->node->get_instance(), (int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta, - (int)cur_node->lost_msg_delta, cur_node->node->get_queue_size(), cur_node->node->get_meta()->o_size); + cur_node->node->get_queue_size(), cur_node->node->get_meta()->o_size); } cur_node = cur_node->next; diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index 90e9c456f0..5484e38ffd 100644 --- a/src/modules/uORB/uORBDeviceMaster.hpp +++ b/src/modules/uORB/uORBDeviceMaster.hpp @@ -78,9 +78,8 @@ public: /** * Print statistics for each existing topic. - * @param reset if true, reset statistics afterwards */ - void printStatistics(bool reset); + void printStatistics(); /** * Continuously print statistics, like the unix top command for processes. @@ -98,9 +97,7 @@ private: struct DeviceNodeStatisticsData { DeviceNode *node; - 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; }; @@ -120,8 +117,6 @@ private: List _node_list; AtomicBitset _node_exists[ORB_MULTI_MAX_INSTANCES]; - hrt_abstime _last_statistics_output; - px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */ void lock() { do {} while (px4_sem_wait(&_lock) != 0); } diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index f65f272835..4557a14276 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -127,16 +127,15 @@ uORB::DeviceNode::close(cdev::file_t *filp) } bool -uORB::DeviceNode::copy_locked(void *dst, unsigned &generation) +uORB::DeviceNode::copy_locked(void *dst, unsigned &generation) const { bool updated = false; if ((dst != nullptr) && (_data != nullptr)) { - unsigned current_generation = _generation.load(); + const unsigned current_generation = _generation.load(); if (current_generation > generation + _queue_size) { // Reader is too far behind: some messages are lost - _lost_messages += current_generation - (generation + _queue_size); generation = current_generation - _queue_size; } @@ -489,23 +488,24 @@ uORB::DeviceNode::appears_updated(cdev::file_t *filp) } bool -uORB::DeviceNode::print_statistics(bool reset) +uORB::DeviceNode::print_statistics(int max_topic_length) { - if (!_lost_messages) { + if (!_advertised) { 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; - } + const uint8_t instance = get_instance(); + const uint8_t priority = get_priority(); + const int8_t sub_count = subscriber_count(); + const uint8_t queue_size = get_queue_size(); unlock(); - PX4_INFO("%s: %i", _meta->o_name, lost_messages); + PX4_INFO_RAW("%-*s %2i %4i %2i %4i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count, + queue_size, get_meta()->o_size, priority, get_devname()); + return true; } diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index adf65758ff..5707d1a202 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -176,18 +176,16 @@ 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) + * Print statistics + * @param max_topic_length max topic name length for printing + * @return true if printed something, false otherwise */ - bool print_statistics(bool reset); + bool print_statistics(int max_topic_length); uint8_t get_queue_size() const { return _queue_size; } int8_t subscriber_count() const { return _subscriber_count; } - uint32_t lost_message_count() const { return _lost_messages; } - unsigned published_message_count() const { return _generation.load(); } const orb_metadata *get_meta() const { return _meta; } @@ -239,7 +237,7 @@ private: * @return bool * Returns true if the data was copied. */ - bool copy_locked(void *dst, unsigned &generation); + bool copy_locked(void *dst, unsigned &generation) const; struct UpdateIntervalData { uint64_t last_update{0}; /**< time at which the last update was provided, used when update_interval is nonzero */ @@ -259,10 +257,6 @@ private: px4::atomic _generation{0}; /**< object generation count */ List _callbacks; - // 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. */ - ORB_PRIO _priority; /**< priority of the topic */ const uint8_t _instance; /**< orb multi instance identifier */ bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */ diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index 2f241471f0..84e05f34ff 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -128,7 +128,7 @@ uorb_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "status")) { if (g_dev != nullptr) { - g_dev->printStatistics(true); + g_dev->printStatistics(); } else { PX4_INFO("uorb is not running");