From a103fef948b7f239afef21a8d0f848151891b409 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Jun 2014 18:51:35 +0200 Subject: Fixed threading and transmission issues for FTP --- src/modules/mavlink/mavlink_main.cpp | 62 +++++++++++++++++++++++++++++------- 1 file changed, 51 insertions(+), 11 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 e300be074..066d25bf6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -83,6 +83,10 @@ #include "mavlink_rate_limiter.h" #include "mavlink_commands.h" +#ifndef MAVLINK_CRC_EXTRA + #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { + Mavlink *instance; switch (channel) { @@ -198,7 +203,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warnx("TX FAIL"); + // XXX overflow perf } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -230,6 +235,7 @@ Mavlink::Mavlink() : _verbose(false), _forwarding_on(false), _passing_on(false), + _ftp_on(false), _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), @@ -453,7 +459,7 @@ Mavlink::get_instance_id() return _instance_id; } -mavlink_channel_t +const mavlink_channel_t Mavlink::get_channel() { return _channel; @@ -536,6 +542,16 @@ void Mavlink::mavlink_update_system(void) _use_hil_gps = (bool)use_hil_gps; } +int Mavlink::get_system_id() +{ + return mavlink_system.sysid; +} + +int Mavlink::get_component_id() +{ + return mavlink_system.compid; +} + int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -1649,11 +1665,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::message_buffer_init(int size) { + _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; _message_buffer.data = (char*)malloc(_message_buffer.size); - return (_message_buffer.data == 0) ? ERROR : OK; + + int ret; + if (_message_buffer.data == 0) { + ret = ERROR; + _message_buffer.size = 0; + } else { + ret = OK; + } + + return ret; } void @@ -1781,7 +1807,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1837,6 +1863,10 @@ Mavlink::task_main(int argc, char *argv[]) _wait_to_transmit = true; break; + case 'x': + _ftp_on = true; + break; + default: err_flag = true; break; @@ -1902,9 +1932,9 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on) { + if (_passing_on || _ftp_on) { /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(500)) { + if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN)) { errx(1, "can't allocate message buffer, exiting"); } @@ -2064,8 +2094,8 @@ Mavlink::task_main(int argc, char *argv[]) } } - /* pass messages from other UARTs */ - if (_passing_on) { + /* pass messages from other UARTs or FTP worker */ + if (_passing_on || _ftp_on) { bool is_part; void *read_ptr; @@ -2076,11 +2106,21 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { + + // int oldseq = mavlink_get_channel_status(get_channel())->current_tx_seq; + + const mavlink_message_t* msg = (const mavlink_message_t*)read_ptr; /* write first part of buffer */ - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + _mavlink_resend_uart(_channel, msg); + + // mavlink_get_channel_status(get_channel())->current_tx_seq = oldseq; + // mavlink_msg_system_time_send(get_channel(), 255, 255); + message_buffer_mark_read(available); + /* write second part of buffer if there is some */ + // XXX this doesn't quite work, as the resend UART call assumes a continous block if (is_part) { /* guard get ptr by mutex */ pthread_mutex_lock(&_message_buffer_mutex); @@ -2139,7 +2179,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); - if (_passing_on) { + if (_passing_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } @@ -2281,7 +2321,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) -- cgit v1.2.3