Modified to use new FILE_TRANSFER_PROTOCOL message

- Also corrected system/component id transmit/check

1 files changed, 27 insertions, 22 deletions

diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 43de89de9..cf9ea0c07 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,18 @@ private:
 
 	static MavlinkFTP	*_server;
 
+	/// @brief This structure is sent in the payload portion of the file transfer protocol mavlink message.
+	/// This structure is layed out such that it should not require any special compiler packing to not consume extra space.
 	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
+		unsigned int    session:4;  ///< Session id for read and write commands
+		unsigned int    opcode:4;   ///< Command opcode
+		uint8_t         size;       ///< Size of data
+		uint32_t        crc32;      ///< CRC for entire Request structure, with crc32 set to 0
+		uint32_t        offset;     ///< Offsets for List and Read commands
 		uint8_t		data[];
-	};
+        };
 
 	enum Opcode : uint8_t
 	{
@@ -131,10 +130,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 +145,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 +173,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.