diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h index 65a2ba3c65..aec5ded101 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client.h @@ -56,24 +56,23 @@ #define DEFAULT_RECV_PORT 2019 #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); struct options { - enum class eTransports - { - UART, - UDP - }; - eTransports transport = options::eTransports::UART; - char device[64] = DEVICE; - int update_time_ms = UPDATE_TIME_MS; - int loops = LOOPS; - int sleep_ms = SLEEP_MS; - uint32_t baudrate = BAUDRATE; - int poll_ms = POLL_MS; - uint16_t recv_port = DEFAULT_RECV_PORT; - uint16_t send_port = DEFAULT_SEND_PORT; + enum class eTransports { + UART, + UDP + }; + eTransports transport = options::eTransports::UART; + char device[64] = DEVICE; + int update_time_ms = UPDATE_TIME_MS; + int loops = LOOPS; + int sleep_ms = SLEEP_MS; + uint32_t baudrate = BAUDRATE; + int poll_ms = POLL_MS; + uint16_t recv_port = DEFAULT_RECV_PORT; + uint16_t send_port = DEFAULT_SEND_PORT; }; extern struct options _options; diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_dummy.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_dummy.cpp index f65a7b58c0..881ff4fc53 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_dummy.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_dummy.cpp @@ -39,30 +39,27 @@ extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]); static void usage(const char *name) { - PX4_INFO("usage: %s start|stop\n\n", name); + PX4_INFO("usage: %s start|stop\n\n", name); } int micrortps_client_main(int argc, char *argv[]) { - if (argc < 2) - { - usage(argv[0]); - return -1; - } + if (argc < 2) { + usage(argv[0]); + return -1; + } - if (!strcmp(argv[1], "start")) - { - PX4_WARN("PX4 built without RTPS bridge support, EXITING...\n"); - return -1; - } + if (!strcmp(argv[1], "start")) { + PX4_WARN("PX4 built without RTPS bridge support, EXITING...\n"); + return -1; + } - if (!strcmp(argv[1], "stop")) - { - PX4_INFO("Not running"); - return -1; - } + if (!strcmp(argv[1], "stop")) { + PX4_INFO("Not running"); + return -1; + } - usage(argv[0]); + usage(argv[0]); - return -1; + return -1; } 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 593d6df3dd..544f432e2e 100644 --- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp +++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp @@ -53,158 +53,158 @@ struct options _options; static void usage(const char *name) { - PX4_INFO("usage: %s start|stop [options]\n\n" - " -t [UART|UDP] Default UART\n" - " -d UART device. Default /dev/ttyACM0\n" - " -u Time in ms for uORB subscribed topics update. Default 0\n" - " -l How many iterations will this program have. -1 for infinite. Default 10000\n" - " -w Time in ms for which each iteration sleep. Default 1ms\n" - " -b UART device baudrate. Default 460800\n" - " -p Time in ms to poll over UART. Default 1ms\n" - " -r UDP port for receiving. Default 2019\n" - " -s UDP port for sending. Default 2020\n", - name); + PX4_INFO("usage: %s start|stop [options]\n\n" + " -t [UART|UDP] Default UART\n" + " -d UART device. Default /dev/ttyACM0\n" + " -u Time in ms for uORB subscribed topics update. Default 0\n" + " -l How many iterations will this program have. -1 for infinite. Default 10000\n" + " -w Time in ms for which each iteration sleep. Default 1ms\n" + " -b UART device baudrate. Default 460800\n" + " -p Time in ms to poll over UART. Default 1ms\n" + " -r UDP port for receiving. Default 2019\n" + " -s UDP port for sending. Default 2020\n", + name); } static int parse_options(int argc, char *argv[]) { - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:", &myoptind, &myoptarg)) != EOF) - { - switch (ch) - { - case 't': _options.transport = strcmp(myoptarg, "UDP") == 0? - options::eTransports::UDP - :options::eTransports::UART; break; - case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; - case 'u': _options.update_time_ms = strtol(myoptarg, nullptr, 10); break; - case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; - case 'w': _options.sleep_ms = strtol(myoptarg, nullptr, 10); break; - case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break; - case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break; - case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; - case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; - default: - usage(argv[1]); - return -1; - } - } + while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 't': _options.transport = strcmp(myoptarg, "UDP") == 0 ? + options::eTransports::UDP + : options::eTransports::UART; break; - return 0; + case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break; + + case 'u': _options.update_time_ms = strtol(myoptarg, nullptr, 10); break; + + case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break; + + case 'w': _options.sleep_ms = strtol(myoptarg, nullptr, 10); break; + + case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break; + + case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break; + + case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; + + case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; + + default: + usage(argv[1]); + return -1; + } + } + + return 0; } static int micrortps_start(int argc, char *argv[]) { - if (0 > parse_options(argc, argv)) - { - printf("EXITING...\n"); - _rtps_task = -1; - return -1; - } + if (0 > parse_options(argc, argv)) { + printf("EXITING...\n"); + _rtps_task = -1; + return -1; + } - switch (_options.transport) - { - case options::eTransports::UART: - { - transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms); - printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n", - _options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms); - } - break; - case options::eTransports::UDP: - { - transport_node = new UDP_node(_options.recv_port, _options.send_port); - printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n", - _options.recv_port, _options.send_port, _options.sleep_ms); - } - break; - default: - _rtps_task = -1; - printf("EXITING...\n"); - return -1; - } + switch (_options.transport) { + case options::eTransports::UART: { + transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms); + printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n", + _options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms); + } + break; - if (0 > transport_node->init()) - { - printf("EXITING...\n"); - _rtps_task = -1; - return -1; - } + case options::eTransports::UDP: { + transport_node = new UDP_node(_options.recv_port, _options.send_port); + printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n", + _options.recv_port, _options.send_port, _options.sleep_ms); + } + break; - sleep(1); + default: + _rtps_task = -1; + printf("EXITING...\n"); + return -1; + } - struct timespec begin; - int total_read = 0; - uint32_t received = 0; - int loop = 0; - micrortps_start_topics(begin, total_read, received, loop); + if (0 > transport_node->init()) { + printf("EXITING...\n"); + _rtps_task = -1; + return -1; + } - 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); - printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n", - (unsigned long)received, loop, total_read, elapsed_secs, (double)total_read/(1000*elapsed_secs)); + sleep(1); - delete transport_node; - transport_node = nullptr; - PX4_INFO("exiting"); - fflush(stdout); + struct timespec begin; + int total_read = 0; + uint32_t received = 0; + int loop = 0; + micrortps_start_topics(begin, total_read, received, loop); - _rtps_task = -1; + 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); + printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n", + (unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs)); - return 0; + delete transport_node; + transport_node = nullptr; + PX4_INFO("exiting"); + fflush(stdout); + + _rtps_task = -1; + + return 0; } int micrortps_client_main(int argc, char *argv[]) { - if (argc < 2) - { - usage(argv[0]); - return -1; - } + if (argc < 2) { + usage(argv[0]); + return -1; + } - if (!strcmp(argv[1], "start")) - { - if (_rtps_task != -1) - { - PX4_INFO("Already running"); - return -1; - } + if (!strcmp(argv[1], "start")) { + if (_rtps_task != -1) { + PX4_INFO("Already running"); + return -1; + } - _rtps_task = px4_task_spawn_cmd("rtps", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 4096, - (px4_main_t) micrortps_start, - (char *const *)argv); + _rtps_task = px4_task_spawn_cmd("rtps", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + (px4_main_t) micrortps_start, + (char *const *)argv); - if (_rtps_task < 0) - { - PX4_WARN("Could not start task"); - _rtps_task = -1; - return -1; - } + if (_rtps_task < 0) { + PX4_WARN("Could not start task"); + _rtps_task = -1; + return -1; + } - return 0; - } + return 0; + } - if (!strcmp(argv[1], "stop")) - { - if (_rtps_task == -1) - { - PX4_INFO("Not running"); - return -1; - } + if (!strcmp(argv[1], "stop")) { + if (_rtps_task == -1) { + PX4_INFO("Not running"); + return -1; + } - _should_exit_task = true; - if (nullptr != transport_node) transport_node->close(); - return 0; - } + _should_exit_task = true; - usage(argv[0]); + if (nullptr != transport_node) { transport_node->close(); } - return -1; + return 0; + } + + usage(argv[0]); + + return -1; }