diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-08 18:51:35 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-08 18:51:35 +0200 |
commit | a103fef948b7f239afef21a8d0f848151891b409 (patch) | |
tree | 4e2bec2b8a9a4490555c1e322a4c2f9da015beb2 | |
parent | f84e18f27a84254c9760a771f8ad91f864ba25fe (diff) | |
download | px4-firmware-a103fef948b7f239afef21a8d0f848151891b409.tar.gz px4-firmware-a103fef948b7f239afef21a8d0f848151891b409.tar.bz2 px4-firmware-a103fef948b7f239afef21a8d0f848151891b409.zip |
Fixed threading and transmission issues for FTP
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.cpp | 4 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.h | 33 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 62 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 2 |
4 files changed, 80 insertions, 21 deletions
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index d4d659d91..8c29043e0 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -64,7 +64,7 @@ MavlinkFTP::MavlinkFTP() } void -MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel) +MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg) { // get a free request auto req = _dqFree(); @@ -73,7 +73,7 @@ MavlinkFTP::handle_message(mavlink_message_t *msg, mavlink_channel_t channel) if (req != nullptr) { // decode the request - if (req->decode(msg, channel)) { + if (req->decode(mavlink, msg)) { // and queue it for the worker work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index f68dab98d..0869a5fdb 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -64,8 +64,8 @@ public: static MavlinkFTP *getServer(); // static interface - void handle_message(mavlink_message_t *msg, - mavlink_channel_t channel); + void handle_message(Mavlink* mavlink, + mavlink_message_t *msg); private: @@ -145,9 +145,9 @@ private: work_s work; }; - bool decode(mavlink_message_t *fromMessage, mavlink_channel_t fromChannel) { + bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { - _channel = fromChannel; + _mavlink = mavlink; mavlink_msg_encapsulated_data_decode(fromMessage, &_message); return true; } @@ -155,7 +155,26 @@ private: } void reply() { - mavlink_msg_encapsulated_data_send(_channel, sequence(), rawData()); + + // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the + // flat memory architecture, as we're operating between threads here. + mavlink_message_t msg; + msg.checksum = 0; + unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), + _mavlink->get_channel(), &msg, sequence(), rawData()); + // unsigned len = mavlink_msg_system_time_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), + // _mavlink->get_channel(), &msg, 255, 255); + + if (!_mavlink->message_buffer_write(&msg, len+2)) { + warnx("FTP TX ERR"); + } else { + warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + _mavlink->get_system_id(), + _mavlink->get_component_id(), + _mavlink->get_channel(), + len, + msg.checksum); + } } uint8_t *rawData() { return &_message.data[0]; } @@ -163,12 +182,12 @@ private: uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } uint16_t sequence() const { return _message.seqnr; } - mavlink_channel_t &channel() { return _channel; } + mavlink_channel_t channel() { return _mavlink->get_channel(); } char *dataAsCString(); private: - mavlink_channel_t _channel; + Mavlink *_mavlink; mavlink_encapsulated_data_t _message; }; 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[]) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f2c4ca85..4a244815a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -154,7 +154,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - MavlinkFTP::getServer()->handle_message(msg, _mavlink->get_channel()); + MavlinkFTP::getServer()->handle_message(_mavlink, msg); break; default: |