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_client: more cleanup

Browse Source
This commit is contained in:
TSC21
2019-11-23 17:37:36 +00:00
committed by Nuno Marques
parent ec0803815e
commit d80da97ef5
4 changed files with 29 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/uorb_microcdr/microRTPS_client.cpp.em
+9 -12
View File
@@ -95,7 +95,7 @@ void* send(void* /*unused*/)
// ucdrBuffer to serialize using the user defined buffer // ucdrBuffer to serialize using the user defined buffer
ucdrBuffer writer; ucdrBuffer writer;
header_length = transport_node->get_header_length(); header_length = transport_node->get_header_length();
ucdr_init_buffer(&writer, (uint8_t*)&data_buffer[header_length], BUFFER_SIZE - header_length); ucdr_init_buffer(&writer, reinterpret_cast<uint8_t*>(&data_buffer[header_length]), BUFFER_SIZE - header_length);
struct timespec begin; struct timespec begin;
px4_clock_gettime(CLOCK_REALTIME, &begin); px4_clock_gettime(CLOCK_REALTIME, &begin);
@@ -105,27 +105,24 @@ void* send(void* /*unused*/)
@[for idx, topic in enumerate(send_topics)]@ @[for idx, topic in enumerate(send_topics)]@
@(send_base_types[idx])_s @(topic)_data; @(send_base_types[idx])_s @(topic)_data;
if (@(topic)_sub.update(&@(topic)_data)) { if (@(topic)_sub.update(&@(topic)_data)) {
// copy raw data into local buffer // copy raw data into local buffer. Payload is shifted by header length to make room for header
// payload is shifted by header length to make room for header
serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length); serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length);
if (0 < (read = transport_node->write(static_cast<char>(@(rtps_message_id(ids, topic))), data_buffer, length)))
if (0 < (read = transport_node->write((char)@(rtps_message_id(ids, topic)), data_buffer, length)))
{ {
total_sent += read; total_sent += read;
++sent; ++sent;
} }
} }
@[end for]@ @[end for]@
px4_usleep(_options.sleep_ms * 1e3);
px4_usleep(_options.sleep_ms*1000);
++loop; ++loop;
} }
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 = static_cast<double>(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", 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<double>(total_sent / (1e3 * elapsed_secs)));
return nullptr; return nullptr;
} }
@@ -146,7 +143,7 @@ static int launch_send_thread(pthread_t &sender_thread)
} }
@[end if]@ @[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]@ @[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 to deserialize using the user defined buffer
ucdrBuffer reader; ucdrBuffer reader;
ucdr_init_buffer(&reader, (uint8_t*)data_buffer, BUFFER_SIZE); ucdr_init_buffer(&reader, reinterpret_cast<uint8_t*>(data_buffer), BUFFER_SIZE);
@[end if]@ @[end if]@
px4_clock_gettime(CLOCK_REALTIME, &begin); 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 // loop forever if informed loop number is negative
if (_options.loops >= 0 && loop >= _options.loops) break; if (_options.loops >= 0 && loop >= _options.loops) break;
px4_usleep(_options.sleep_ms*1000); px4_usleep(_options.sleep_ms * 1e3);
++loop; ++loop;
} }
@[if send_topics]@ @[if send_topics]@
msg/templates/urtps/microRTPS_transport.cpp
+3 -7
View File
@@ -161,10 +161,7 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer
return -1; 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]; 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; 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; static uint8_t seq = 0;
// [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] // [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end]
uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length); uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length);
header.topic_ID = topic_ID; header.topic_ID = topic_ID;
@@ -380,11 +376,11 @@ int UART_node::init()
* According to px4_time.h, px4_usleep() is only defined when lockstep is set * According to px4_time.h, px4_usleep() is only defined when lockstep is set
* to be used * to be used
*/ */
#ifndef ENABLE_LOCKSTEP_SCHEDULER #ifndef px4_usleep
usleep(1000); usleep(1000);
#else #else
px4_usleep(1000); px4_usleep(1000);
#endif /* ENABLE_LOCKSTEP_SCHEDULER */ #endif /* px4_usleep */
} }
if (flush) { if (flush) {
src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h
+6 -6
View File
@@ -60,7 +60,7 @@
#define DEFAULT_SEND_PORT 2020 #define DEFAULT_SEND_PORT 2020
void *send(void *data); 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 { struct baudtype {
speed_t code; speed_t code;
@@ -74,14 +74,14 @@ struct options {
}; };
eTransports transport = options::eTransports::UART; eTransports transport = options::eTransports::UART;
char device[64] = DEVICE; 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; char ip[16] = IP;
uint16_t recv_port = DEFAULT_RECV_PORT; uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_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; extern struct options _options;
src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
+9 -12
View File
@@ -153,23 +153,22 @@ static int micrortps_start(int argc, char *argv[])
return -1; return -1;
} }
struct timespec begin; struct timespec begin;
int total_read = 0, loop = 0;
uint32_t received = 0;
micrortps_start_topics(begin, total_read, received, loop);
struct timespec end; 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); 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<double>(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", PX4_INFO("RECEIVED: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s",
(unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs)); received, loop, total_read, elapsed_secs, static_cast<double>(total_read / (1e3 * elapsed_secs)));
delete transport_node; delete transport_node;
@@ -235,11 +234,9 @@ int micrortps_client_main(int argc, char *argv[])
if (nullptr != transport_node) { transport_node->close(); } if (nullptr != transport_node) { transport_node->close(); }
_rtps_task = -1; _rtps_task = -1;
return 0; return 0;
} }
usage(argv[0]); usage(argv[0]);
return -1; return -1;
} }