diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-12 17:54:04 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-12 17:54:04 +0100 |
commit | 939b8464a2ccea462b1e85b55757e065328d084b (patch) | |
tree | f181c00db43dbb47c059f63c6025aba3daeebd80 | |
parent | 23258edfa132a6329e72f4e260d5fef8798999d8 (diff) | |
parent | 45a18587fc5e96697da586b9bf51ee65c10e3753 (diff) | |
download | px4-firmware-939b8464a2ccea462b1e85b55757e065328d084b.tar.gz px4-firmware-939b8464a2ccea462b1e85b55757e065328d084b.tar.bz2 px4-firmware-939b8464a2ccea462b1e85b55757e065328d084b.zip |
Merge branch 'beta_mavlink2_flow_control' into paul_estimator_mavlink2
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 35 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 4 |
2 files changed, 26 insertions, 13 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5bac73d29..1ead2f9d2 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 0804af753..e6e20eb93 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -154,6 +154,8 @@ public: bool get_hil_enabled() { return _hil_enabled; }; + bool get_flow_control_enabled() { return _flow_control_enabled; } + /** * Handle waypoint related messages. */ @@ -246,6 +248,8 @@ private: char *_subscribe_to_stream; float _subscribe_to_stream_rate; + bool _flow_control_enabled; + /** * Send one parameter. * |