Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

microRTPS: improve verbosity with the usage of px4_log types

Browse Source
This commit is contained in:
TSC21
2019-01-01 23:39:15 +00:00
committed by Nuno Marques
parent adbec90018
commit 3c246ce170
3 changed files with 36 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/uorb_microcdr/microRTPS_client.cpp.template
+2 -2
View File
@@ -130,7 +130,7 @@ void* send(void* /*unused*/)
struct timespec end; struct timespec end;
px4_clock_gettime(CLOCK_REALTIME, &end); px4_clock_gettime(CLOCK_REALTIME, &end);
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000); double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000);
printf("\nSENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s\n", PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s",
sent, loop, total_sent, elapsed_secs, (double)total_sent/(1000*elapsed_secs)); sent, loop, total_sent, elapsed_secs, (double)total_sent/(1000*elapsed_secs));
return nullptr; return nullptr;
@@ -203,7 +203,7 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r
break; break;
@[end for]@ @[end for]@
default: default:
printf("Unexpected topic ID\n"); PX4_WARN("Unexpected topic ID\n");
break; break;
} }
} }
msg/templates/urtps/microRTPS_transport.cpp
+24 -23
View File
@@ -36,6 +36,7 @@
#include <errno.h> #include <errno.h>
#include <sys/socket.h> #include <sys/socket.h>
#include <cstdlib> #include <cstdlib>
#include <px4_log.h>
#include <px4_time.h> #include <px4_time.h>
#include <microRTPS_transport.h> #include <microRTPS_transport.h>
@@ -116,7 +117,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
int errsv = errno; int errsv = errno;
if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) { if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) {
printf("Read fail %d\n", errsv); PX4_ERR("Read fail %d", errsv);
} }
return len; return len;
@@ -142,7 +143,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
// Start not found // Start not found
if (msg_start_pos > rx_buff_pos - header_size) { if (msg_start_pos > rx_buff_pos - header_size) {
printf(" (↓↓ %u)\n", msg_start_pos); PX4_INFO(" (↓↓ %u)", msg_start_pos);
// All we've checked so far is garbage, drop it - but save unchecked bytes // All we've checked so far is garbage, drop it - but save unchecked bytes
memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos);
rx_buff_pos = rx_buff_pos - msg_start_pos; rx_buff_pos = rx_buff_pos - msg_start_pos;
@@ -165,7 +166,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
if (msg_start_pos + header_size + payload_len > rx_buff_pos) { if (msg_start_pos + header_size + payload_len > rx_buff_pos) {
// If there's garbage at the beginning, drop it // If there's garbage at the beginning, drop it
if (msg_start_pos > 0) { if (msg_start_pos > 0) {
printf(" (↓ %u)\n", msg_start_pos); PX4_INFO(" (↓ %u)", msg_start_pos);
memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos);
rx_buff_pos -= msg_start_pos; rx_buff_pos -= msg_start_pos;
} }
@@ -177,8 +178,8 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
uint16_t calc_crc = crc16((uint8_t *)rx_buffer + msg_start_pos + header_size, payload_len); uint16_t calc_crc = crc16((uint8_t *)rx_buffer + msg_start_pos + header_size, payload_len);
if (read_crc != calc_crc) { if (read_crc != calc_crc) {
printf("BAD CRC %u != %u\n", read_crc, calc_crc); PX4_ERR("Bad CRC %u != %u", read_crc, calc_crc);
printf(" (↓ %lu)\n", (unsigned long)(header_size + payload_len)); PX4_ERR(" (↓ %lu)", (unsigned long)(header_size + payload_len));
len = -1; len = -1;
} else { } else {
@@ -241,8 +242,8 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng
err: err:
//int errsv = errno; //int errsv = errno;
//if (len == -1 ) printf(" => Writing error '%d'\n", errsv); //if (len == -1 ) PX4_ERR(" => Writing error '%d'", errsv);
//else printf(" => Wrote '%ld' != length(%lu) error '%d'\n", (long)len, (unsigned long)length, errsv); //else PX4_ERR(" => Wrote '%ld' != length(%lu) error '%d'", (long)len, (unsigned long)length, errsv);
return len; return len;
} }
@@ -269,7 +270,7 @@ int UART_node::init()
uart_fd = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); uart_fd = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (uart_fd < 0) { if (uart_fd < 0) {
printf("failed to open device: %s (%d)\n", uart_name, errno); PX4_ERR("Failed to open device: %s (%d)", uart_name, errno);
return -errno; return -errno;
} }
@@ -285,7 +286,7 @@ int UART_node::init()
// Back up the original uart configuration to restore it after exit // Back up the original uart configuration to restore it after exit
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
int errno_bkp = errno; int errno_bkp = errno;
printf("ERR GET CONF %s: %d (%d)\n", uart_name, termios_state, errno); PX4_ERR("ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno);
close(); close();
return -errno_bkp; return -errno_bkp;
} }
@@ -308,7 +309,7 @@ int UART_node::init()
// Set baud rate // Set baud rate
if (cfsetispeed(&uart_config, baudrate) < 0 || cfsetospeed(&uart_config, baudrate) < 0) { if (cfsetispeed(&uart_config, baudrate) < 0 || cfsetospeed(&uart_config, baudrate) < 0) {
int errno_bkp = errno; int errno_bkp = errno;
printf("ERR SET BAUD %s: %d (%d)\n", uart_name, termios_state, errno); PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno);
close(); close();
return -errno_bkp; return -errno_bkp;
} }
@@ -316,7 +317,7 @@ int UART_node::init()
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
int errno_bkp = errno; int errno_bkp = errno;
printf("ERR SET CONF %s (%d)\n", uart_name, errno); PX4_ERR("ERR SET CONF %s (%d)", uart_name, errno);
close(); close();
return -errno_bkp; return -errno_bkp;
} }
@@ -325,16 +326,16 @@ int UART_node::init()
bool flush = false; bool flush = false;
while (0 < ::read(uart_fd, (void *)&aux, 64)) { while (0 < ::read(uart_fd, (void *)&aux, 64)) {
//printf("%s ", aux); //PX4_INFO("%s ", aux);
flush = true; flush = true;
px4_usleep(1000); px4_usleep(1000);
} }
if (flush) { if (flush) {
printf("flush\n"); PX4_INFO("Flush");
} else { } else {
printf("no flush\n"); PX4_INFO("No flush");
} }
poll_fd[0].fd = uart_fd; poll_fd[0].fd = uart_fd;
@@ -351,7 +352,7 @@ bool UART_node::fds_OK()
uint8_t UART_node::close() uint8_t UART_node::close()
{ {
if (-1 != uart_fd) { if (-1 != uart_fd) {
printf("Close UART\n"); PX4_WARN("Closed UART...");
::close(uart_fd); ::close(uart_fd);
uart_fd = -1; uart_fd = -1;
memset(&poll_fd, 0, sizeof(poll_fd)); memset(&poll_fd, 0, sizeof(poll_fd));
@@ -423,18 +424,18 @@ int UDP_node::init_receiver(uint16_t udp_port)
receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY); receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY);
if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
printf("create socket failed\n"); PX4_ERR("Create socket failed");
return -1; return -1;
} }
printf("Trying to connect...\n"); PX4_INFO("Trying to connect...");
if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) { if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) {
printf("bind failed\n"); PX4_ERR("Bind failed");
return -1; return -1;
} }
printf("connected to server!\n"); PX4_INFO("Connected to server!");
#endif /* __PX4_NUTTX */ #endif /* __PX4_NUTTX */
return 0; return 0;
@@ -445,7 +446,7 @@ int UDP_node::init_sender(uint16_t udp_port)
#ifndef __PX4_NUTTX #ifndef __PX4_NUTTX
if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
printf("create socket failed\n"); PX4_ERR("Create socket failed");
return -1; return -1;
} }
@@ -454,7 +455,7 @@ int UDP_node::init_sender(uint16_t udp_port)
sender_outaddr.sin_port = htons(udp_port); sender_outaddr.sin_port = htons(udp_port);
if (inet_aton("127.0.0.1", &sender_outaddr.sin_addr) == 0) { if (inet_aton("127.0.0.1", &sender_outaddr.sin_addr) == 0) {
printf("inet_aton() failed\n"); PX4_ERR("inet_aton() failed");
return -1; return -1;
} }
@@ -468,14 +469,14 @@ uint8_t UDP_node::close()
#ifndef __PX4_NUTTX #ifndef __PX4_NUTTX
if (sender_fd != -1) { if (sender_fd != -1) {
printf("Close sender socket\n"); PX4_WARN("Closed sender socket!");
shutdown(sender_fd, SHUT_RDWR); shutdown(sender_fd, SHUT_RDWR);
::close(sender_fd); ::close(sender_fd);
sender_fd = -1; sender_fd = -1;
} }
if (receiver_fd != -1) { if (receiver_fd != -1) {
printf("Close receiver socket\n"); PX4_WARN("Closed receiver socket!");
shutdown(receiver_fd, SHUT_RDWR); shutdown(receiver_fd, SHUT_RDWR);
::close(receiver_fd); ::close(receiver_fd);
receiver_fd = -1; receiver_fd = -1;
src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
+10 -10
View File
@@ -152,7 +152,7 @@ static int parse_options(int argc, char *argv[])
static int micrortps_start(int argc, char *argv[]) static int micrortps_start(int argc, char *argv[])
{ {
if (0 > parse_options(argc, argv)) { if (0 > parse_options(argc, argv)) {
printf("EXITING...\n"); PX4_INFO("EXITING...");
_rtps_task = -1; _rtps_task = -1;
return -1; return -1;
} }
@@ -160,26 +160,26 @@ static int micrortps_start(int argc, char *argv[])
switch (_options.transport) { switch (_options.transport) {
case options::eTransports::UART: { case options::eTransports::UART: {
transport_node = new UART_node(_options.device, _options.baudrate.code, _options.poll_ms); transport_node = new UART_node(_options.device, _options.baudrate.code, _options.poll_ms);
printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n", PX4_INFO("UART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms",
_options.device, _options.baudrate.val, _options.sleep_ms, _options.poll_ms); _options.device, _options.baudrate.val, _options.sleep_ms, _options.poll_ms);
} }
break; break;
case options::eTransports::UDP: { case options::eTransports::UDP: {
transport_node = new UDP_node(_options.recv_port, _options.send_port); transport_node = new UDP_node(_options.recv_port, _options.send_port);
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n", PX4_INFO("UDP transport: recv port: %u; send port: %u; sleep: %dms",
_options.recv_port, _options.send_port, _options.sleep_ms); _options.recv_port, _options.send_port, _options.sleep_ms);
} }
break; break;
default: default:
_rtps_task = -1; _rtps_task = -1;
printf("EXITING...\n"); PX4_INFO("EXITING...");
return -1; return -1;
} }
if (0 > transport_node->init()) { if (0 > transport_node->init()) {
printf("EXITING...\n"); PX4_INFO("EXITING...");
_rtps_task = -1; _rtps_task = -1;
return -1; return -1;
} }
@@ -199,14 +199,14 @@ static int micrortps_start(int argc, char *argv[])
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec) / double(1000000000); double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec) / double(1000000000);
printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n", PX4_INFO("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s",
(unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs)); (unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs));
delete transport_node; delete transport_node;
transport_node = nullptr; transport_node = nullptr;
PX4_INFO("exiting"); PX4_INFO("Stopped!");
fflush(stdout); fflush(stdout);