fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

microRTPS: client: show diagnostic of current bandwidth usage on the 'status' option

Browse Source
This commit is contained in:
TSC21 2021-05-06 17:36:18 +02:00 committed by Nuno Marques
parent d6ee15596d
commit 0cc79f3e48
3 changed files with 83 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
msg/templates/uorb_microcdr/microRTPS_client.cpp.em
View File

@ -79,6 +79,12 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] =
uint8_t last_remote_msg_seq = 0;
uint8_t last_msg_seq = 0;
uint64_t tx_last_sec_read = 0;
uint64_t rx_last_sec_read = 0;
pthread_mutex_t tx_lock;
pthread_mutex_t rx_lock;
@[if recv_topics]@
// Publishers for received messages
struct RcvTopicsPubs {
@ -101,10 +107,12 @@ struct SendTopicsSubs {
struct SendThreadArgs {
uint64_t &total_sent;
uint64_t &sent_last_sec;
uint64_t &sent;
int &sent_loop;
SendThreadArgs(uint64_t &total_sent_, uint64_t &sent_, int &sent_loop_)
SendThreadArgs(uint64_t &total_sent_, uint64_t &sent_last_sec_, uint64_t &sent_, int &sent_loop_)
: total_sent(total_sent_),
sent_last_sec(sent_last_sec_),
sent(sent_),
sent_loop(sent_loop_) {}
};
@ -123,6 +131,14 @@ void *send(void *args)
header_length = transport_node->get_header_length();
ucdr_init_buffer(&writer, reinterpret_cast<uint8_t *>(&data_buffer[header_length]), BUFFER_SIZE - header_length);
pthread_t tx_per_second_thread;
int rc = pthread_create(&tx_per_second_thread, nullptr, &tx_per_second, (void *)&data->sent_last_sec);
if (rc != 0) {
errno = rc;
PX4_ERR("Could not create tx counter thread (%d)", errno);
return nullptr;
}
while (!_should_exit_task) {
@[ for idx, topic in enumerate(send_topics)]@
{
@ -147,6 +163,9 @@ void *send(void *args)
if (0 < (read = transport_node->write(static_cast<char>(@(rtps_message_id(ids, topic))), data_buffer, length))) {
data->total_sent += read;
pthread_mutex_lock(&tx_lock);
tx_last_sec_read += read;
pthread_mutex_unlock(&tx_lock);
++data->sent;
}
@ -161,11 +180,36 @@ void *send(void *args)
++data->sent_loop;
}
pthread_join(tx_per_second_thread, nullptr);
delete subs;
return nullptr;
}
void *tx_per_second(void *sent_last_sec)
{
while (!_should_exit_task) {
pthread_mutex_lock(&tx_lock);
*((uint64_t *) sent_last_sec) = tx_last_sec_read;
tx_last_sec_read = 0;
pthread_mutex_unlock(&tx_lock);
px4_sleep(1);
}
return nullptr;
}
void *rx_per_second(void *rcvd_last_sec)
{
while (!_should_exit_task) {
pthread_mutex_lock(&rx_lock);
*((uint64_t *) rcvd_last_sec) = rx_last_sec_read;
rx_last_sec_read = 0;
pthread_mutex_unlock(&rx_lock);
px4_sleep(1);
}
return nullptr;
}
static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &args)
{
pthread_attr_t sender_thread_attr;
@ -192,8 +236,8 @@ static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &a
}
@[end if]@
void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &total_sent, uint64_t &received,
uint64_t &sent, int &rcvd_loop, int &sent_loop)
void micrortps_start_topics(struct timespec &begin, uint64_t &total_rcvd, uint64_t &total_sent, uint64_t &sent_last_sec,
uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop)
{
@[if recv_topics]@
char data_buffer[BUFFER_SIZE] = {};
@ -208,13 +252,21 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64
// ucdrBuffer to deserialize using the user defined buffer
ucdrBuffer reader;
ucdr_init_buffer(&reader, reinterpret_cast<uint8_t *>(data_buffer), BUFFER_SIZE);
pthread_t rx_per_second_thread;
int rc = pthread_create(&rx_per_second_thread, nullptr, &rx_per_second, (void *)&rcvd_last_sec);
if (rc != 0) {
errno = rc;
PX4_ERR("Could not create rx counter thread (%d)", errno);
return;
}
@[end if]@
px4_clock_gettime(CLOCK_REALTIME, &begin);
_should_exit_task = false;
@[if send_topics]@
// var struct to be updated on the thread
SendThreadArgs *sender_thread_args = new SendThreadArgs(total_sent, sent, sent_loop);
SendThreadArgs *sender_thread_args = new SendThreadArgs(total_sent, sent_last_sec, sent, sent_loop);
// create a thread for sending data
pthread_t sender_thread;
@ -224,7 +276,11 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64
while (!_should_exit_task) {
@[if recv_topics]@
while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) {
total_read += read;
total_rcvd += read;
pthread_mutex_lock(&rx_lock);
rcvd_last_sec += read;
pthread_mutex_unlock(&rx_lock);
uint64_t read_time = hrt_absolute_time();
switch (topic_ID) {
@ -258,12 +314,13 @@ void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64
++rcvd_loop;
}
@[if recv_topics]@
delete pubs;
@[end if]@
@[if send_topics]@
delete sender_thread_args;
_should_exit_task = true;
pthread_join(sender_thread, nullptr);
@[end if]@
@[if recv_topics]@
pthread_join(rx_per_second_thread, nullptr);
delete pubs;
@[end if]@
}

10
src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
@ -58,9 +58,13 @@
#define DEFAULT_RECV_PORT 2019
#define DEFAULT_SEND_PORT 2020
void *send(void *args);
void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &total_sent, uint64_t &received,
uint64_t &sent, int &rcvd_loop, int &sent_loop);
void *tx_per_second(void *sent_last_sec);
void *rx_per_second(void *rcvd_last_sec);
void micrortps_start_topics(struct timespec &begin, uint64_t &total_rcvd, uint64_t &total_sent, uint64_t &sent_last_sec,
uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop);
struct baudtype {
speed_t code;

17
src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
@ -58,8 +58,10 @@ struct options _options;
struct timespec begin;
struct timespec end;
uint64_t total_read{0};
uint64_t total_rcvd{0};
uint64_t total_sent{0};
uint64_t rcvd_last_sec{0};
uint64_t sent_last_sec{0};
uint64_t received{0};
uint64_t sent{0};
int rcv_loop{0};
@ -189,14 +191,15 @@ static int micrortps_start(int argc, char *argv[])
return -1;
}
micrortps_start_topics(begin, total_read, total_sent, received, sent, rcv_loop, send_loop);
micrortps_start_topics(begin, total_rcvd, total_sent, sent_last_sec, rcvd_last_sec, received, sent, rcv_loop,
send_loop);
px4_clock_gettime(CLOCK_REALTIME, &end);
const double elapsed_secs = static_cast<double>(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9);
PX4_INFO("RECEIVED: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s",
received, rcv_loop, total_read, elapsed_secs, static_cast<double>(total_read / (1e3 * elapsed_secs)));
received, rcv_loop, total_rcvd, elapsed_secs, static_cast<double>(total_rcvd / (1e3 * elapsed_secs)));
PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - avg %.02fKB/s",
sent, send_loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs));
@ -254,10 +257,12 @@ int micrortps_client_main(int argc, char *argv[])
printf("\tup and running for %.03f seconds\n", elapsed_secs);
printf("\tnr. of messages received: %" PRIu64 "\n", received);
printf("\tnr. of messages sent: %" PRIu64 "\n", sent);
printf("\ttotal data read: %" PRIu64 " bytes\n", total_read);
printf("\ttotal data read: %" PRIu64 " bytes\n", total_rcvd);
printf("\ttotal data sent: %" PRIu64 " bytes\n", total_sent);
printf("\trates:\n");
printf("\t avg rx: %.3f kB/s\n", static_cast<double>(total_read / (1e3 * elapsed_secs)));
printf("\t rx: %.3f kB/s\n", rcvd_last_sec / 1E3);
printf("\t tx: %.3f kB/s\n", sent_last_sec / 1E3);
printf("\t avg rx: %.3f kB/s\n", static_cast<double>(total_rcvd / (1e3 * elapsed_secs)));
printf("\t avg tx: %.3f kB/s\n", static_cast<double>(total_sent / (1e3 * elapsed_secs)));
}