Merge pull request #1332 from PX4/FTP

Modified to use new FILE_TRANSFER_PROTOCOL message

3 files changed, 37 insertions, 25 deletions

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 00c8df18c..5b65dc369 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -114,7 +114,7 @@ MavlinkFTP::_worker(Request *req)
 	uint32_t messageCRC;
 
 	// basic sanity checks; must validate length before use
-	if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) {
+	if (hdr->size > kMaxDataLength) {
 		errorCode = kErrNoRequest;
 		goto out;
 	}
@@ -122,6 +122,9 @@ MavlinkFTP::_worker(Request *req)
 	// check request CRC to make sure this is one of ours
 	messageCRC = hdr->crc32;
 	hdr->crc32 = 0;
+	hdr->padding[0] = 0;
+	hdr->padding[1] = 0;
+	hdr->padding[2] = 0;
 	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
 		errorCode = kErrNoRequest;
 		goto out;
@@ -199,10 +202,13 @@ MavlinkFTP::_reply(Request *req)
 {
 	auto hdr = req->header();
 	
-	hdr->magic = kProtocolMagic;
+	hdr->seqNumber = req->header()->seqNumber + 1;
 
 	// message is assumed to be already constructed in the request buffer, so generate the CRC
 	hdr->crc32 = 0;
+	hdr->padding[0] = 0;
+	hdr->padding[1] = 0;
+	hdr->padding[2] = 0;
 	hdr->crc32 = crc32(req->rawData(), req->dataSize());
 
 	// then pack and send the reply back to the request source
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 43de89de9..1dd8f102e 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -38,9 +38,6 @@
  *
  * MAVLink remote file server.
  *
- * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes
- * a session ID and sequence number.
- *
  * A limited number of requests (currently 2) may be outstanding at a time.
  * Additional messages will be discarded.
  *
@@ -74,16 +71,19 @@ private:
 
 	static MavlinkFTP	*_server;
 
+	/// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the
+	/// structure ourselves to 32 bit alignment which should get us what we want.
 	struct RequestHeader
-	{
-		uint8_t		magic;
-		uint8_t		session;
-		uint8_t		opcode;
-		uint8_t		size;
-		uint32_t	crc32;
-		uint32_t	offset;
+        {
+		uint16_t	seqNumber;  ///< sequence number for message
+		uint8_t		session;    ///< Session id for read and write commands
+		uint8_t		opcode;     ///< Command opcode
+		uint8_t		size;       ///< Size of data
+		uint8_t		padding[3];
+		uint32_t	crc32;      ///< CRC for entire Request structure, with crc32 and padding set to 0
+		uint32_t	offset;     ///< Offsets for List and Read commands
 		uint8_t		data[];
-	};
+        };
 
 	enum Opcode : uint8_t
 	{
@@ -131,10 +131,11 @@ private:
 		};
 
 		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
-			if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) {
+			if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
+				_systemId = fromMessage->sysid;
 				_mavlink = mavlink;
-				mavlink_msg_encapsulated_data_decode(fromMessage, &_message);
-				return true;
+				mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
+				return _message.target_system == _mavlink->get_system_id();
 			}
 			return false;
 		}
@@ -145,8 +146,14 @@ private:
 			// 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()+1, rawData());
+			unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(),		// Sender system id
+										    _mavlink->get_component_id(),	// Sender component id
+										    _mavlink->get_channel(),		// Channel to send on
+										    &msg,				// Message to pack payload into
+										    0,					// Target network
+										    _systemId,				// Target system id
+										    0,					// Target component id
+										    rawData());				// Payload to pack into message
 
 			_mavlink->lockMessageBufferMutex();
 			bool success = _mavlink->message_buffer_write(&msg, len);
@@ -167,26 +174,25 @@ private:
 #endif
 		}
 
-		uint8_t		*rawData() { return &_message.data[0]; }
-		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.data[0]); }
+		uint8_t		*rawData() { return &_message.payload[0]; }
+		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
 		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 _mavlink->get_channel(); }
 
 		char		*dataAsCString();
 
 	private:
 		Mavlink			*_mavlink;
-		mavlink_encapsulated_data_t _message;
+		mavlink_file_transfer_protocol_t _message;
+		uint8_t _systemId;
 
 	};
 
-	static const uint8_t	kProtocolMagic = 'f';
 	static const char	kDirentFile = 'F';
 	static const char	kDirentDir = 'D';
 	static const char	kDirentUnknown = 'U';
-	static const uint8_t	kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader);
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
 
 	/// Request worker; runs on the low-priority work queue to service
 	/// remote requests.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index a602344fd..bed1bd789 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -172,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_request_data_stream(msg);
 		break;
 
-	case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
+	case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
 		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
 		break;