forked from Archive/PX4-Autopilot
orb status: print information about lost messages
This commit is contained in:
parent
7280f71cef
commit
b86cf2b017
|
@ -139,6 +139,11 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
Node *top() const
|
||||
{
|
||||
return _top;
|
||||
}
|
||||
|
||||
private:
|
||||
Node *_top;
|
||||
Node *_end;
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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");
|
||||
|
|
Loading…
Reference in New Issue