diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em index 34b5566101..696927e2da 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em @@ -94,8 +94,8 @@ void* send(void* /*unused*/) // ucdrBuffer to serialize using the user defined buffer ucdrBuffer writer; - header_length=transport_node->get_header_length(); - ucdr_init_buffer(&writer, (uint8_t*)&data_buffer[header_length], BUFFER_SIZE - header_length); + header_length = transport_node->get_header_length(); + ucdr_init_buffer(&writer, reinterpret_cast(&data_buffer[header_length]), BUFFER_SIZE - header_length); struct timespec begin; px4_clock_gettime(CLOCK_REALTIME, &begin); @@ -105,27 +105,24 @@ void* send(void* /*unused*/) @[for idx, topic in enumerate(send_topics)]@ @(send_base_types[idx])_s @(topic)_data; if (@(topic)_sub.update(&@(topic)_data)) { - // copy raw data into local buffer - // payload is shifted by header length to make room for header + // copy raw data into local buffer. Payload is shifted by header length to make room for header serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length); - - if (0 < (read = transport_node->write((char)@(rtps_message_id(ids, topic)), data_buffer, length))) + if (0 < (read = transport_node->write(static_cast(@(rtps_message_id(ids, topic))), data_buffer, length))) { total_sent += read; ++sent; } } @[end for]@ - - px4_usleep(_options.sleep_ms*1000); + px4_usleep(_options.sleep_ms * 1e3); ++loop; } struct timespec 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 = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); 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, static_cast(total_sent / (1e3 * elapsed_secs))); return nullptr; } @@ -146,7 +143,7 @@ static int launch_send_thread(pthread_t &sender_thread) } @[end if]@ -void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &received, int &loop) +void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &received, int &loop) { @[if recv_topics]@ @@ -162,7 +159,7 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r // ucdrBuffer to deserialize using the user defined buffer ucdrBuffer reader; - ucdr_init_buffer(&reader, (uint8_t*)data_buffer, BUFFER_SIZE); + ucdr_init_buffer(&reader, reinterpret_cast(data_buffer), BUFFER_SIZE); @[end if]@ px4_clock_gettime(CLOCK_REALTIME, &begin); @@ -201,7 +198,7 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r // loop forever if informed loop number is negative if (_options.loops >= 0 && loop >= _options.loops) break; - px4_usleep(_options.sleep_ms*1000); + px4_usleep(_options.sleep_ms * 1e3); ++loop; } @[if send_topics]@ diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp index 30d418114a..3a80f2b1e8 100644 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -161,10 +161,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer return -1; } - /* - * [>,>,>,topic_ID,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd] - */ - + // [>,>,>,topic_ID,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd] struct Header *header = (struct Header *)&rx_buffer[msg_start_pos]; uint32_t payload_len = ((uint32_t)header->payload_len_h << 8) | header->payload_len_l; @@ -241,7 +238,6 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng static uint8_t seq = 0; // [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] - uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length); header.topic_ID = topic_ID; @@ -252,7 +248,7 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng header.crc_l = crc & 0xff; /* Headroom for header is created in client */ - /*Fill in the header in the same payload buffer to call a single node_write */ + /* Fill in the header in the same payload buffer to call a single node_write */ memcpy(buffer, &header, sizeof(header)); ssize_t len = node_write(buffer, length + sizeof(header)); if (len != ssize_t(length + sizeof(header))) { @@ -380,11 +376,11 @@ int UART_node::init() * According to px4_time.h, px4_usleep() is only defined when lockstep is set * to be used */ -#ifndef ENABLE_LOCKSTEP_SCHEDULER +#ifndef px4_usleep usleep(1000); #else px4_usleep(1000); -#endif /* ENABLE_LOCKSTEP_SCHEDULER */ +#endif /* px4_usleep */ } if (flush) { diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h index 71a45cdc85..d841fe5fb0 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -60,7 +60,7 @@ #define DEFAULT_SEND_PORT 2020 void *send(void *data); -void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &received, int &loop); +void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &received, int &loop); struct baudtype { speed_t code; @@ -74,14 +74,14 @@ struct options { }; eTransports transport = options::eTransports::UART; char device[64] = DEVICE; - uint32_t update_time_ms = UPDATE_TIME_MS; - int loops = LOOPS; - uint32_t sleep_ms = SLEEP_MS; - uint32_t baudrate = BAUDRATE; - uint32_t poll_ms = POLL_MS; char ip[16] = IP; uint16_t recv_port = DEFAULT_RECV_PORT; uint16_t send_port = DEFAULT_SEND_PORT; + uint32_t update_time_ms = UPDATE_TIME_MS; + uint32_t sleep_ms = SLEEP_MS; + uint32_t baudrate = BAUDRATE; + uint32_t poll_ms = POLL_MS; + int loops = LOOPS; }; extern struct options _options; diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp index 2dd58a505b..9c2ac35b18 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -153,23 +153,22 @@ static int micrortps_start(int argc, char *argv[]) return -1; } - struct timespec begin; - int total_read = 0, loop = 0; - - uint32_t received = 0; - - micrortps_start_topics(begin, total_read, received, loop); - struct timespec end; + uint64_t total_read = 0, received = 0; + + int loop = 0; + + micrortps_start_topics(begin, total_read, received, loop); + 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 = static_cast(end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9); - 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)); + PX4_INFO("RECEIVED: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s", + received, loop, total_read, elapsed_secs, static_cast(total_read / (1e3 * elapsed_secs))); delete transport_node; @@ -235,11 +234,9 @@ int micrortps_client_main(int argc, char *argv[]) if (nullptr != transport_node) { transport_node->close(); } _rtps_task = -1; - return 0; } usage(argv[0]); - return -1; }