diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em index 572a4c259f..5b325fd18f 100644 --- a/msg/templates/urtps/microRTPS_agent.cpp.em +++ b/msg/templates/urtps/microRTPS_agent.cpp.em @@ -76,8 +76,7 @@ recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumer // Default values #define DEVICE "/dev/ttyACM0" #define SLEEP_US 1 -#define BAUDRATE B460800 -#define BAUDRATE_VAL 460800 +#define BAUDRATE 460800 #define POLL_MS 0 #define WAIT_CNST 2 #define DEFAULT_RECV_PORT 2020 @@ -92,23 +91,6 @@ Transport_node *transport_node = nullptr; RtpsTopics topics; uint32_t total_sent = 0, sent = 0; -struct baudtype { - speed_t code; - uint32_t val; -}; - -const baudtype baudlist[] = { - [0] = {.code = B0, .val = 0}, - [1] = {.code = B9600, .val = 9600}, - [2] = {.code = B19200, .val = 19200}, - [3] = {.code = B38400, .val = 38400}, - [4] = {.code = B57600, .val = 57600}, - [5] = {.code = B115200, .val = 115200}, - [6] = {.code = B230400, .val = 230400}, - [7] = {.code = B460800, .val = 460800}, - [8] = {.code = B921600, .val = 921600}, -}; - struct options { enum class eTransports { @@ -118,7 +100,7 @@ struct options { eTransports transport = options::eTransports::UART; char device[64] = DEVICE; int sleep_us = SLEEP_US; - baudtype baudrate = {.code=BAUDRATE,.val=BAUDRATE_VAL}; + uint32_t baudrate = BAUDRATE; int poll_ms = POLL_MS; uint16_t recv_port = DEFAULT_RECV_PORT; uint16_t send_port = DEFAULT_SEND_PORT; @@ -139,15 +121,6 @@ static void usage(const char *name) name); } -baudtype getbaudrate(char *valstr) -{ - uint32_t baudval = strtoul(valstr, nullptr, 10); - for (unsigned int i=1; i exit_sender_thread(false); std::condition_variable t_send_queue_cv; -std::mutex t_send_queue_mutex; +std::mutex t_send_queue_mutex; std::queue t_send_queue; void t_send(void *data) { char data_buffer[BUFFER_SIZE] = {}; - int length = 0; + int length = 0; while (running && !exit_sender_thread.load()) { @@ -209,7 +182,7 @@ void t_send(void *data) uint8_t topic_ID = t_send_queue.front(); t_send_queue.pop(); lk.unlock(); - + uint16_t header_length = transport_node->get_header_length(); /* make room for the header to fill in later */ eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer)-header_length); @@ -242,9 +215,9 @@ int main(int argc, char** argv) { 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, _options.poll_ms); printf("\nUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms\n\n", - _options.device, _options.baudrate.val, _options.sleep_us, _options.poll_ms); + _options.device, _options.baudrate, _options.sleep_us, _options.poll_ms); } break; case options::eTransports::UDP: diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h index 7b06bda337..11391345f4 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -52,14 +52,7 @@ #define UPDATE_TIME_MS 0 #define LOOPS -1 #define SLEEP_MS 1 -#define BAUDRATE B460800 -#define BAUDRATE_VAL 460800 -#ifndef B460800 -#define B460800 460800 -#endif -#ifndef B921600 -#define B921600 921600 -#endif +#define BAUDRATE 460800 #define DEVICE "/dev/ttyACM0" #define POLL_MS 1 #define IP "127.0.0.1" @@ -84,7 +77,7 @@ struct options { int update_time_ms = UPDATE_TIME_MS; int loops = LOOPS; int sleep_ms = SLEEP_MS; - struct baudtype baudrate = {.code = BAUDRATE, .val = BAUDRATE_VAL}; + uint32_t baudrate = BAUDRATE; int poll_ms = POLL_MS; char ip[16] = IP; uint16_t recv_port = DEFAULT_RECV_PORT; 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 ae33dc35ca..b70fa5b3c6 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -54,20 +54,6 @@ bool _should_exit_task = false; Transport_node *transport_node = nullptr; struct options _options; -const baudtype baudlist[] = { - [0] = {.code = B0, .val = 0}, - [1] = {.code = B9600, .val = 9600}, - [2] = {.code = B19200, .val = 19200}, - [3] = {.code = B38400, .val = 38400}, - [4] = {.code = B57600, .val = 57600}, - [5] = {.code = B115200, .val = 115200}, - [6] = {.code = B230400, .val = 230400}, - [7] = {.code = B460800, .val = 460800}, - [8] = {.code = B921600, .val = 921600} -}; - -baudtype getbaudrate(const char *valstr); - static void usage(const char *name) { PRINT_MODULE_USAGE_NAME("micrortps_client", "communication"); @@ -90,19 +76,6 @@ static void usage(const char *name) PRINT_MODULE_USAGE_COMMAND("status"); } -baudtype getbaudrate(const char *valstr) -{ - int baudval; - - if (px4_get_parameter_value(valstr, baudval) == 0) { - for (unsigned int i = 1; i < sizeof(baudlist) / sizeof(baudtype); i++) { - if (baudlist[i].val == (unsigned)baudval) { return baudlist[i]; } - } - } - - return baudlist[0]; -} - static int parse_options(int argc, char *argv[]) { int ch; @@ -123,7 +96,7 @@ static int parse_options(int argc, char *argv[]) case 'w': _options.sleep_ms = strtol(myoptarg, nullptr, 10); break; - case 'b': _options.baudrate = getbaudrate(myoptarg); break; + case 'b': _options.baudrate = strtoul(myoptarg, nullptr, 10); break; case 'p': _options.poll_ms = strtol(myoptarg, nullptr, 10); break; @@ -162,9 +135,9 @@ static int micrortps_start(int argc, char *argv[]) switch (_options.transport) { 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, _options.poll_ms); 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, _options.sleep_ms, _options.poll_ms); } break;