dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

micrortps_client: more cleanup

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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<uint8_t*>(&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<char>(@(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<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",
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;
}
@ -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<uint8_t*>(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]@

12
msg/templates/urtps/microRTPS_transport.cpp
View File

@ -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) {

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

@ -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;

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

@ -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<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",
(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<double>(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;
}