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 /src/modules/mavlink/mavlink_ftp.h | |
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
Diffstat (limited to 'src/modules/mavlink/mavlink_ftp.h')
-rw-r--r-- | src/modules/mavlink/mavlink_ftp.h | 33 |
1 files changed, 26 insertions, 7 deletions
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; }; |