From a37e539647df2b3cfd335b33e4d186c4d39316b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 22:26:12 +0100 Subject: Correctly initialize mission count --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5bdf6c262..667580452 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -178,6 +178,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { _wpm = &_wpm_s; + mission.count = 0; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); -- cgit v1.2.3 From f66b0ad8ac926a79608cf872c8e49ea7ace92288 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:04:30 +0100 Subject: Added hardware flow control to mavlink app. Auto-disables if nothing can be written to the port --- src/modules/mavlink/mavlink_main.cpp | 46 ++++++++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 7 ++++++ 2 files changed, 53 insertions(+) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 667580452..ed3b265f9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -104,6 +104,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +static uint64_t last_write_times[6] = {0}; + /* * Internal function to send the bytes through the right serial port */ @@ -149,10 +151,28 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t desired = (sizeof(uint8_t) * length); + + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + + if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0 && + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } + ssize_t ret = write(uart, ch, desired); if (ret != desired) { warn("write err"); + } else { + last_write_times[(unsigned)channel] = hrt_absolute_time(); } } @@ -541,9 +561,35 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -1; } + /* + * Setup hardware flow control. If the port has no RTS pin this call will fail, + * which is not an issue, but requires a separate call so we can fail silently. + */ + (void)tcgetattr(_uart_fd, &uart_config); + uart_config.c_cflag |= CRTS_IFLOW; + (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); + + /* setup output flow control */ + (void)enable_flow_control(true); + return _uart_fd; } +int +Mavlink::enable_flow_control(bool enabled) +{ + struct termios uart_config; + int ret = tcgetattr(_uart_fd, &uart_config); + if (enabled) { + uart_config.c_cflag |= CRTSCTS; + } else { + uart_config.c_cflag &= ~CRTSCTS; + } + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + + return ret; +} + int Mavlink::set_hil_enabled(bool hil_enabled) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cc4c896b9..2a7bd57cf 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -184,6 +184,13 @@ public: int get_instance_id(); + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int enable_flow_control(bool enabled); + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ -- cgit v1.2.3 From 023c7069c4a340aa1b7c9f05963a066cee368b2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:36:12 +0100 Subject: Fix FIONWRITE usage --- src/modules/mavlink/mavlink_main.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed3b265f9..9a6e1130e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -158,13 +158,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length */ int buf_free = 0; - if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); - warnx("DISABLING HARDWARE FLOW CONTROL"); + if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + + if (buf_free == 0 && last_write_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } } ssize_t ret = write(uart, ch, desired); -- cgit v1.2.3 From 76af0970f5220d3a842c4daa353a45ef51df1082 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:46:02 +0100 Subject: Increased param rate, fixed wrong usage of MAVLink chan. --- src/modules/mavlink/mavlink_main.cpp | 5 ++--- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 667580452..1044984bf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -661,7 +661,7 @@ int Mavlink::mavlink_pm_send_param(param_t param) mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, - MAVLINK_COMM_0, + _channel, &tx_msg, name_buf, val_buf, @@ -1561,7 +1561,6 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; _datarate = 0; - _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1776,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_param_queue_index = param_count(); MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); - MavlinkRateLimiter fast_rate_limiter(100000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(20000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fa63e06c5..c222a3ddf 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -912,7 +912,7 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); } } } -- cgit v1.2.3 From 2d2ecbad00c4f21d0c3ded2faa4f9bdb7adefddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:56:26 +0100 Subject: Fixed more wrong usages of encoding channel --- src/modules/mavlink/mavlink_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1044984bf..d722bec46 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -856,7 +856,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); + mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } @@ -879,7 +879,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) wpc.seq = seq; - mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); } else if (seq == 0 && _wpm->size == 0) { @@ -902,7 +902,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_component = compid; wpc.count = mission.count; - mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } @@ -933,7 +933,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; - mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); + mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } @@ -953,7 +953,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); + mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } @@ -979,7 +979,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } @@ -1448,7 +1448,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) statustext.text[i] = '\0'; mavlink_message_t msg; - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + mavlink_msg_statustext_encode_chan(mavlink_system.sysid, mavlink_system.compid, _channel, &msg, &statustext); mavlink_missionlib_send_message(&msg); return OK; -- cgit v1.2.3 From 7604518db479e7cc9618e19b224c5d5c94013142 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 10:29:39 +0100 Subject: Fixed HW flow control enable / disable scheme, console output cleanup on start --- src/modules/mavlink/mavlink_main.cpp | 45 ++++++++++++++++++------------------ 1 file changed, 23 insertions(+), 22 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7e1af8824..4420e04fd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -160,23 +160,30 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0 && last_write_times[(unsigned)channel] != 0 && + if (buf_free == 0) { + + if (last_write_times[(unsigned)channel] != 0 && hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); - warnx("DISABLING HARDWARE FLOW CONTROL"); + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } + + } else { + + /* apparently there is space left, although we might be + * partially overflooding the buffer already */ + last_write_times[(unsigned)channel] = hrt_absolute_time(); } } ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warn("write err"); - } else { - last_write_times[(unsigned)channel] = hrt_absolute_time(); + // XXX do something here, but change to using FIONWRITE and OS buf size for detection } } @@ -536,7 +543,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { - warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); close(_uart_fd); return -1; } @@ -552,7 +559,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); close(_uart_fd); return -1; } @@ -560,7 +567,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR SET CONF %s\n", uart_name); close(_uart_fd); return -1; } @@ -574,7 +581,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); /* setup output flow control */ - (void)enable_flow_control(true); + if (enable_flow_control(true)) { + warnx("ERR FLOW CTRL EN"); + } return _uart_fd; } @@ -1604,10 +1613,6 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::task_main(int argc, char *argv[]) { - /* inform about start */ - warnx("start"); - fflush(stdout); - int ch; _baudrate = 57600; _datarate = 0; @@ -1736,8 +1741,7 @@ Mavlink::task_main(int argc, char *argv[]) break; } - warnx("data rate: %d bytes/s", _datarate); - warnx("port: %s, baudrate: %d", _device_name, _baudrate); + warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1782,9 +1786,6 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); - warnx("started"); - mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; -- cgit v1.2.3 From 45a18587fc5e96697da586b9bf51ee65c10e3753 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 17:53:25 +0100 Subject: Fine tuning for flow control disable to stop firing after change is effective --- src/modules/mavlink/mavlink_main.cpp | 35 ++++++++++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 4 ++++ 2 files changed, 26 insertions(+), 13 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4420e04fd..b699b86a5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -112,44 +112,50 @@ static uint64_t last_write_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { - int uart = -1; + Mavlink *instance; switch (channel) { case MAVLINK_COMM_0: - uart = Mavlink::get_uart_fd(0); + instance = Mavlink::get_instance(0); break; case MAVLINK_COMM_1: - uart = Mavlink::get_uart_fd(1); + instance = Mavlink::get_instance(1); break; case MAVLINK_COMM_2: - uart = Mavlink::get_uart_fd(2); + instance = Mavlink::get_instance(2); break; case MAVLINK_COMM_3: - uart = Mavlink::get_uart_fd(3); + instance = Mavlink::get_instance(3); break; #ifdef MAVLINK_COMM_4 case MAVLINK_COMM_4: - uart = Mavlink::get_uart_fd(4); + instance = Mavlink::get_instance(4); break; #endif #ifdef MAVLINK_COMM_5 case MAVLINK_COMM_5: - uart = Mavlink::get_uart_fd(5); + instance = Mavlink::get_instance(5); break; #endif #ifdef MAVLINK_COMM_6 case MAVLINK_COMM_6: - uart = Mavlink::get_uart_fd(6); + instance = Mavlink::get_instance(6); break; #endif } + /* no valid instance, bail */ + if (!instance) + return; + + int uart = instance->get_uart_fd(); + ssize_t desired = (sizeof(uint8_t) * length); /* @@ -158,18 +164,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length */ int buf_free = 0; - if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + if (instance->get_flow_control_enabled() + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } } else { @@ -204,6 +208,7 @@ Mavlink::Mavlink() : _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -600,6 +605,10 @@ Mavlink::enable_flow_control(bool enabled) } ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + if (!ret) { + _flow_control_enabled = enabled; + } + return ret; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2a7bd57cf..94b2c9dbc 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -156,6 +156,8 @@ public: bool get_hil_enabled() { return _mavlink_hil_enabled; }; + bool get_flow_control_enabled() { return _flow_control_enabled; } + /** * Handle waypoint related messages. */ @@ -248,6 +250,8 @@ private: char *_subscribe_to_stream; float _subscribe_to_stream_rate; + bool _flow_control_enabled; + /** * Send one parameter. * -- cgit v1.2.3 From 6b79f533388ab78ddbbb8cc316b38c6e7403e46a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 14 Mar 2014 14:40:54 +0100 Subject: mavlink stream: do not use getopt as it leads to problems with the global optarg variable --- src/modules/mavlink/mavlink_main.cpp | 37 +++++++++++++++++------------------- 1 file changed, 17 insertions(+), 20 deletions(-) (limited to 'src/modules/mavlink/mavlink_main.cpp') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d722bec46..f1a1f9568 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1538,7 +1538,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) /* copy stream name */ unsigned n = strlen(stream_name) + 1; char *s = new char[n]; - memcpy(s, stream_name, n); + strcpy(s, stream_name); /* set subscription task */ _subscribe_to_stream_rate = rate; @@ -1959,36 +1959,33 @@ Mavlink::stream(int argc, char *argv[]) const char *stream_name = nullptr; int ch; - argc -= 1; - argv += 1; + argc -= 2; + argv += 2; /* don't exit from getopt loop to leave getopt global variables in consistent state, * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "r:d:s:")) != EOF) { - switch (ch) { - case 'r': - rate = strtod(optarg, nullptr); + int i = 0; + while (i < argc) { + if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) { + rate = strtod(argv[i+1], nullptr); if (rate < 0.0f) { err_flag = true; } - - break; - - case 'd': - device_name = optarg; - break; - - case 's': - stream_name = optarg; - break; - - default: + i++; + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) { + device_name = argv[i+1]; + i++; + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) { + stream_name = argv[i+1]; + i++; + } else { err_flag = true; - break; } + + i++; } if (!err_flag && rate >= 0.0 && stream_name != nullptr) { -- cgit v1.2.3