From 09b398120e20b2791c70f96530117c5d942bc3ec Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 7 Aug 2024 09:32:04 +0200 Subject: [PATCH 1/2] mavftp: initial library file copied from MAVProxy/MAVProxy/modules/mavproxy_ftp.py --- mavftp.py | 850 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 850 insertions(+) create mode 100644 mavftp.py diff --git a/mavftp.py b/mavftp.py new file mode 100644 index 000000000..9c5d2a43d --- /dev/null +++ b/mavftp.py @@ -0,0 +1,850 @@ +#!/usr/bin/env python +'''mavlink file transfer support''' + +import io +import time, os, sys +import struct +import random +from pymavlink import mavutil + +try: + # py2 + from StringIO import StringIO as SIO +except ImportError: + # py3 + from io import BytesIO as SIO + +from MAVProxy.modules.lib import mp_module +from MAVProxy.modules.lib import mp_settings + +# opcodes +OP_None = 0 +OP_TerminateSession = 1 +OP_ResetSessions = 2 +OP_ListDirectory = 3 +OP_OpenFileRO = 4 +OP_ReadFile = 5 +OP_CreateFile = 6 +OP_WriteFile = 7 +OP_RemoveFile = 8 +OP_CreateDirectory = 9 +OP_RemoveDirectory = 10 +OP_OpenFileWO = 11 +OP_TruncateFile = 12 +OP_Rename = 13 +OP_CalcFileCRC32 = 14 +OP_BurstReadFile = 15 +OP_Ack = 128 +OP_Nack = 129 + +# error codes +ERR_None = 0 +ERR_Fail = 1 +ERR_FailErrno = 2 +ERR_InvalidDataSize = 3 +ERR_InvalidSession = 4 +ERR_NoSessionsAvailable = 5 +ERR_EndOfFile = 6 +ERR_UnknownCommand = 7 +ERR_FileExists = 8 +ERR_FileProtected = 9 +ERR_FileNotFound = 10 + +HDR_Len = 12 +MAX_Payload = 239 + +class FTP_OP: + def __init__(self, seq, session, opcode, size, req_opcode, burst_complete, offset, payload): + self.seq = seq + self.session = session + self.opcode = opcode + self.size = size + self.req_opcode = req_opcode + self.burst_complete = burst_complete + self.offset = offset + self.payload = payload + + def pack(self): + '''pack message''' + ret = struct.pack(" 0: + ret += " [%u]" % self.payload[0] + return ret + +class WriteQueue: + def __init__(self, ofs, size): + self.ofs = ofs + self.size = size + self.last_send = 0 + +class FTPModule(mp_module.MPModule): + def __init__(self, mpstate): + super(FTPModule, self).__init__(mpstate, "ftp", public=True) + self.add_command('ftp', self.cmd_ftp, "file transfer", + ["", + "set (FTPSETTING)", + "put (FILENAME) (FILENAME)"]) + self.ftp_settings = mp_settings.MPSettings( + [('debug', int, 0), + ('pkt_loss_tx', int, 0), + ('pkt_loss_rx', int, 0), + ('max_backlog', int, 5), + ('burst_read_size', int, 80), + ('write_size', int, 80), + ('write_qsize', int, 5), + ('retry_time', float, 0.5)]) + self.add_completion_function('(FTPSETTING)', + self.ftp_settings.completion) + self.seq = 0 + self.session = 0 + self.network = 0 + self.last_op = None + self.fh = None + self.filename = None + self.callback = None + self.callback_progress = None + self.put_callback = None + self.put_callback_progress = None + self.total_size = 0 + self.read_gaps = [] + self.read_gap_times = {} + self.last_gap_send = 0 + self.read_retries = 0 + self.read_total = 0 + self.duplicates = 0 + self.last_read = None + self.last_burst_read = None + self.op_start = None + self.dir_offset = 0 + self.last_op_time = time.time() + self.rtt = 0.5 + self.reached_eof = False + self.backlog = 0 + self.burst_size = self.ftp_settings.burst_read_size + self.write_list = None + self.write_block_size = 0 + self.write_acks = 0 + self.write_total = 0 + self.write_file_size = 0 + self.write_idx = 0 + self.write_recv_idx = -1 + self.write_pending = 0 + self.write_last_send = None + self.warned_component = False + + def cmd_ftp(self, args): + '''FTP operations''' + usage = "Usage: ftp " + if len(args) < 1: + print(usage) + return + if args[0] == 'list': + self.cmd_list(args[1:]) + elif args[0] == "set": + self.ftp_settings.command(args[1:]) + elif args[0] == 'get': + self.cmd_get(args[1:]) + elif args[0] == 'put': + self.cmd_put(args[1:]) + elif args[0] == 'rm': + self.cmd_rm(args[1:]) + elif args[0] == 'rmdir': + self.cmd_rmdir(args[1:]) + elif args[0] == 'rename': + self.cmd_rename(args[1:]) + elif args[0] == 'mkdir': + self.cmd_mkdir(args[1:]) + elif args[0] == 'crc': + self.cmd_crc(args[1:]) + elif args[0] == 'status': + self.cmd_status() + elif args[0] == 'cancel': + self.cmd_cancel() + else: + print(usage) + + def send(self, op): + '''send a request''' + op.seq = self.seq + payload = op.pack() + plen = len(payload) + if plen < MAX_Payload + HDR_Len: + payload.extend(bytearray([0]*((HDR_Len+MAX_Payload)-plen))) + self.master.mav.file_transfer_protocol_send(self.network, self.target_system, self.target_component, payload) + self.seq = (self.seq + 1) % 256 + self.last_op = op + now = time.time() + if self.ftp_settings.debug > 1: + print("> %s dt=%.2f" % (op, now - self.last_op_time)) + self.last_op_time = time.time() + + def terminate_session(self): + '''terminate current session''' + self.send(FTP_OP(self.seq, self.session, OP_TerminateSession, 0, 0, 0, 0, None)) + self.fh = None + self.filename = None + self.write_list = None + if self.callback is not None: + # tell caller that the transfer failed + self.callback(None) + self.callback = None + if self.put_callback is not None: + # tell caller that the transfer failed + self.put_callback(None) + self.put_callback = None + if self.put_callback_progress is not None: + self.put_callback_progress(None) + self.put_callback_progress = None + self.read_gaps = [] + self.read_total = 0 + self.read_gap_times = {} + self.last_read = None + self.last_burst_read = None + self.session = (self.session + 1) % 256 + self.reached_eof = False + self.backlog = 0 + self.duplicates = 0 + if self.ftp_settings.debug > 0: + print("Terminated session") + + def cmd_list(self, args): + '''list files''' + if len(args) > 0: + dname = args[0] + else: + dname = '/' + print("Listing %s" % dname) + enc_dname = bytearray(dname, 'ascii') + self.total_size = 0 + self.dir_offset = 0 + op = FTP_OP(self.seq, self.session, OP_ListDirectory, len(enc_dname), 0, 0, self.dir_offset, enc_dname) + self.send(op) + + def handle_list_reply(self, op, m): + '''handle OP_ListDirectory reply''' + if op.opcode == OP_Ack: + dentries = sorted(op.payload.split(b'\x00')) + #print(dentries) + for d in dentries: + if len(d) == 0: + continue + self.dir_offset += 1 + try: + if sys.version_info.major >= 3: + d = str(d, 'ascii') + else: + d = str(d) + except Exception: + continue + if d[0] == 'D': + print(" D %s" % d[1:]) + elif d[0] == 'F': + (name, size) = d[1:].split('\t') + size = int(size) + self.total_size += size + print(" %s\t%u" % (name, size)) + else: + print(d) + # ask for more + more = self.last_op + more.offset = self.dir_offset + self.send(more) + elif op.opcode == OP_Nack and len(op.payload) == 1 and op.payload[0] == ERR_EndOfFile: + print("Total size %.2f kByte" % (self.total_size / 1024.0)) + self.total_size = 0 + else: + print('LIST: %s' % op) + + def cmd_get(self, args, callback=None, callback_progress=None): + '''get file''' + if len(args) == 0: + print("Usage: get FILENAME ") + return + self.terminate_session() + fname = args[0] + if len(args) > 1: + self.filename = args[1] + else: + self.filename = os.path.basename(fname) + if callback is None or self.ftp_settings.debug > 1: + print("Getting %s as %s" % (fname, self.filename)) + self.op_start = time.time() + self.callback = callback + self.callback_progress = callback_progress + self.read_retries = 0 + self.duplicates = 0 + self.reached_eof = False + self.burst_size = self.ftp_settings.burst_read_size + if self.burst_size < 1: + self.burst_size = 239 + elif self.burst_size > 239: + self.burst_size = 239 + enc_fname = bytearray(fname, 'ascii') + self.open_retries = 0 + op = FTP_OP(self.seq, self.session, OP_OpenFileRO, len(enc_fname), 0, 0, 0, enc_fname) + self.send(op) + + def handle_open_RO_reply(self, op, m): + '''handle OP_OpenFileRO reply''' + if op.opcode == OP_Ack: + if self.filename is None: + return + try: + if self.callback is not None or self.filename == '-': + self.fh = SIO() + else: + self.fh = open(self.filename, 'wb') + except Exception as ex: + print("Failed to open %s: %s" % (self.filename, ex)) + self.terminate_session() + return + read = FTP_OP(self.seq, self.session, OP_BurstReadFile, self.burst_size, 0, 0, 0, None) + self.last_burst_read = time.time() + self.send(read) + else: + if self.callback is None or self.ftp_settings.debug > 0: + print("ftp open failed") + self.terminate_session() + + def check_read_finished(self): + '''check if download has completed''' + if self.reached_eof and len(self.read_gaps) == 0: + ofs = self.fh.tell() + dt = time.time() - self.op_start + rate = (ofs / dt) / 1024.0 + if self.callback is not None: + self.fh.seek(0) + self.callback(self.fh) + self.callback = None + elif self.filename == "-": + self.fh.seek(0) + if sys.version_info.major < 3: + print(self.fh.read()) + else: + print(self.fh.read().decode('utf-8')) + else: + print("Wrote %u bytes to %s in %.2fs %.1fkByte/s" % (ofs, self.filename, dt, rate)) + self.terminate_session() + return True + return False + + def write_payload(self, op): + '''write payload from a read op''' + self.fh.seek(op.offset) + self.fh.write(op.payload) + self.read_total += len(op.payload) + if self.callback_progress is not None: + self.callback_progress(self.fh, self.read_total) + + def handle_burst_read(self, op, m): + '''handle OP_BurstReadFile reply''' + if self.ftp_settings.pkt_loss_tx > 0: + if random.uniform(0,100) < self.ftp_settings.pkt_loss_tx: + if self.ftp_settings.debug > 0: + print("FTP: dropping TX") + return + if self.fh is None or self.filename is None: + if op.session != self.session: + # old session + return + print("FTP Unexpected burst read reply") + print(op) + return + self.last_burst_read = time.time() + size = len(op.payload) + if size > self.burst_size: + # this server doesn't handle the burst size argument + self.burst_size = MAX_Payload + if self.ftp_settings.debug > 0: + print("Setting burst size to %u" % self.burst_size) + if op.opcode == OP_Ack and self.fh is not None: + ofs = self.fh.tell() + if op.offset < ofs: + # writing an earlier portion, possibly remove a gap + gap = (op.offset, len(op.payload)) + if gap in self.read_gaps: + self.read_gaps.remove(gap) + self.read_gap_times.pop(gap) + if self.ftp_settings.debug > 0: + print("FTP: removed gap", gap, self.reached_eof, len(self.read_gaps)) + else: + if self.ftp_settings.debug > 0: + print("FTP: dup read reply at %u of len %u ofs=%u" % (op.offset, op.size, self.fh.tell())) + self.duplicates += 1 + return + self.write_payload(op) + self.fh.seek(ofs) + if self.check_read_finished(): + return + elif op.offset > ofs: + # we have a gap + gap = (ofs, op.offset-ofs) + max_read = self.burst_size + while True: + if gap[1] <= max_read: + self.read_gaps.append(gap) + self.read_gap_times[gap] = 0 + break + g = (gap[0], max_read) + self.read_gaps.append(g) + self.read_gap_times[g] = 0 + gap = (gap[0] + max_read, gap[1] - max_read) + self.write_payload(op) + else: + self.write_payload(op) + if op.burst_complete: + if op.size > 0 and op.size < self.burst_size: + # a burst complete with non-zero size and less than burst packet size + # means EOF + if not self.reached_eof and self.ftp_settings.debug > 0: + print("EOF at %u with %u gaps t=%.2f" % (self.fh.tell(), len(self.read_gaps), time.time() - self.op_start)) + self.reached_eof = True + if self.check_read_finished(): + return + self.check_read_send() + return + more = self.last_op + more.offset = op.offset + op.size + if self.ftp_settings.debug > 0: + print("FTP: burst continue at %u %u" % (more.offset, self.fh.tell())) + self.send(more) + elif op.opcode == OP_Nack: + ecode = op.payload[0] + if self.ftp_settings.debug > 0: + print("FTP: burst nack: ", op) + if ecode == ERR_EndOfFile or ecode == 0: + if not self.reached_eof and op.offset > self.fh.tell(): + # we lost the last part of the burst + if self.ftp_settings.debug > 0: + print("burst lost EOF %u %u" % (self.fh.tell(), op.offset)) + return + if not self.reached_eof and self.ftp_settings.debug > 0: + print("EOF at %u with %u gaps t=%.2f" % (self.fh.tell(), len(self.read_gaps), time.time() - self.op_start)) + self.reached_eof = True + if self.check_read_finished(): + return + self.check_read_send() + elif self.ftp_settings.debug > 0: + print("FTP: burst Nack (ecode:%u): %s" % (ecode, op)) + else: + print("FTP: burst error: %s" % op) + + def handle_reply_read(self, op, m): + '''handle OP_ReadFile reply''' + if self.fh is None or self.filename is None: + if self.ftp_settings.debug > 0: + print("FTP Unexpected read reply") + print(op) + return + if self.backlog > 0: + self.backlog -= 1 + if op.opcode == OP_Ack and self.fh is not None: + gap = (op.offset, op.size) + if gap in self.read_gaps: + self.read_gaps.remove(gap) + self.read_gap_times.pop(gap) + ofs = self.fh.tell() + self.write_payload(op) + self.fh.seek(ofs) + if self.ftp_settings.debug > 0: + print("FTP: removed gap", gap, self.reached_eof, len(self.read_gaps)) + if self.check_read_finished(): + return + elif op.size < self.burst_size: + print("FTP: file size changed to %u" % op.offset+op.size) + self.terminate_session() + else: + self.duplicates += 1 + if self.ftp_settings.debug > 0: + print("FTP: no gap read", gap, len(self.read_gaps)) + elif op.opcode == OP_Nack: + print("Read failed with %u gaps" % len(self.read_gaps), str(op)) + self.terminate_session() + self.check_read_send() + + def cmd_put(self, args, fh=None, callback=None, progress_callback=None): + '''put file''' + if len(args) == 0: + print("Usage: put FILENAME ") + return + if self.write_list is not None: + print("put already in progress") + return + fname = args[0] + self.fh = fh + if self.fh is None: + try: + self.fh = open(fname, 'rb') + except Exception as ex: + print("Failed to open %s: %s" % (fname, ex)) + return + if len(args) > 1: + self.filename = args[1] + else: + self.filename = os.path.basename(fname) + if self.filename.endswith("/"): + self.filename += os.path.basename(fname) + if callback is None: + print("Putting %s as %s" % (fname, self.filename)) + self.fh.seek(0,2) + file_size = self.fh.tell() + self.fh.seek(0) + + # setup write list + self.write_block_size = self.ftp_settings.write_size + self.write_file_size = file_size + + write_blockcount = file_size // self.write_block_size + if file_size % self.write_block_size != 0: + write_blockcount += 1 + + self.write_list = set(range(write_blockcount)) + self.write_acks = 0 + self.write_total = write_blockcount + self.write_idx = 0 + self.write_recv_idx = -1 + self.write_pending = 0 + self.write_last_send = None + + self.put_callback = callback + self.put_callback_progress = progress_callback + self.read_retries = 0 + self.op_start = time.time() + enc_fname = bytearray(self.filename, 'ascii') + op = FTP_OP(self.seq, self.session, OP_CreateFile, len(enc_fname), 0, 0, 0, enc_fname) + self.send(op) + + def put_finished(self, flen): + '''finish a put''' + if self.put_callback_progress: + self.put_callback_progress(1.0) + self.put_callback_progress = None + if self.put_callback is not None: + self.put_callback(flen) + self.put_callback = None + else: + print("Sent file of length ", flen) + + def handle_create_file_reply(self, op, m): + '''handle OP_CreateFile reply''' + if self.fh is None: + self.terminate_session() + return + if op.opcode == OP_Ack: + self.send_more_writes() + else: + print("Create failed") + self.terminate_session() + + def send_more_writes(self): + '''send some more writes''' + if len(self.write_list) == 0: + # all done + self.put_finished(self.write_file_size) + self.terminate_session() + return + + now = time.time() + if self.write_last_send is not None: + if now - self.write_last_send > max(min(10*self.rtt, 1),0.2): + # we seem to have lost a block of replies + self.write_pending = max(0, self.write_pending-1) + + n = min(self.ftp_settings.write_qsize-self.write_pending, len(self.write_list)) + for i in range(n): + # send in round-robin, skipping any that have been acked + idx = self.write_idx + while idx not in self.write_list: + idx = (idx + 1) % self.write_total + ofs = idx * self.write_block_size + self.fh.seek(ofs) + data = self.fh.read(self.write_block_size) + write = FTP_OP(self.seq, self.session, OP_WriteFile, len(data), 0, 0, ofs, bytearray(data)) + self.send(write) + self.write_idx = (idx + 1) % self.write_total + self.write_pending += 1 + self.write_last_send = now + + def handle_write_reply(self, op, m): + '''handle OP_WriteFile reply''' + if self.fh is None: + self.terminate_session() + return + if op.opcode != OP_Ack: + print("Write failed") + self.terminate_session() + return + + # assume the FTP server processes the blocks sequentially. This means + # when we receive an ack that any blocks between the last ack and this + # one have been lost + idx = op.offset // self.write_block_size + count = (idx - self.write_recv_idx) % self.write_total + + self.write_pending = max(0, self.write_pending - count) + self.write_recv_idx = idx + self.write_list.discard(idx) + self.write_acks += 1 + if self.put_callback_progress: + self.put_callback_progress(self.write_acks/float(self.write_total)) + self.send_more_writes() + + def cmd_rm(self, args): + '''remove file''' + if len(args) == 0: + print("Usage: rm FILENAME") + return + fname = args[0] + print("Removing %s" % fname) + enc_fname = bytearray(fname, 'ascii') + op = FTP_OP(self.seq, self.session, OP_RemoveFile, len(enc_fname), 0, 0, 0, enc_fname) + self.send(op) + + def cmd_rmdir(self, args): + '''remove directory''' + if len(args) == 0: + print("Usage: rmdir FILENAME") + return + dname = args[0] + print("Removing %s" % dname) + enc_dname = bytearray(dname, 'ascii') + op = FTP_OP(self.seq, self.session, OP_RemoveDirectory, len(enc_dname), 0, 0, 0, enc_dname) + self.send(op) + + def handle_remove_reply(self, op, m): + '''handle remove reply''' + if op.opcode != OP_Ack: + print("Remove failed %s" % op) + + def cmd_rename(self, args): + '''rename file''' + if len(args) < 2: + print("Usage: rename OLDNAME NEWNAME") + return + name1 = args[0] + name2 = args[1] + print("Renaming %s to %s" % (name1, name2)) + enc_name1 = bytearray(name1, 'ascii') + enc_name2 = bytearray(name2, 'ascii') + enc_both = enc_name1 + b'\x00' + enc_name2 + op = FTP_OP(self.seq, self.session, OP_Rename, len(enc_both), 0, 0, 0, enc_both) + self.send(op) + + def handle_rename_reply(self, op, m): + '''handle rename reply''' + if op.opcode != OP_Ack: + print("Rename failed %s" % op) + + def cmd_mkdir(self, args): + '''make directory''' + if len(args) < 1: + print("Usage: mkdir NAME") + return + name = args[0] + print("Creating directory %s" % name) + enc_name = bytearray(name, 'ascii') + op = FTP_OP(self.seq, self.session, OP_CreateDirectory, len(enc_name), 0, 0, 0, enc_name) + self.send(op) + + def handle_mkdir_reply(self, op, m): + '''handle mkdir reply''' + if op.opcode != OP_Ack: + print("Create directory failed %s" % op) + + def cmd_crc(self, args): + '''get crc''' + if len(args) < 1: + print("Usage: crc NAME") + return + name = args[0] + self.filename = name + self.op_start = time.time() + print("Getting CRC for %s" % name) + enc_name = bytearray(name, 'ascii') + op = FTP_OP(self.seq, self.session, OP_CalcFileCRC32, len(enc_name), 0, 0, 0, bytearray(enc_name)) + self.send(op) + + def handle_crc_reply(self, op, m): + '''handle crc reply''' + if op.opcode == OP_Ack and op.size == 4: + crc, = struct.unpack(" 1: + print("< %s dt=%.2f" % (op, dt)) + self.last_op_time = now + if self.ftp_settings.pkt_loss_rx > 0: + if random.uniform(0,100) < self.ftp_settings.pkt_loss_rx: + if self.ftp_settings.debug > 1: + print("FTP: dropping packet RX") + return + + if op.req_opcode == self.last_op.opcode and op.seq == (self.last_op.seq + 1) % 256: + self.rtt = max(min(self.rtt, dt), 0.01) + if op.req_opcode == OP_ListDirectory: + self.handle_list_reply(op, m) + elif op.req_opcode == OP_OpenFileRO: + self.handle_open_RO_reply(op, m) + elif op.req_opcode == OP_BurstReadFile: + self.handle_burst_read(op, m) + elif op.req_opcode == OP_TerminateSession: + pass + elif op.req_opcode == OP_CreateFile: + self.handle_create_file_reply(op, m) + elif op.req_opcode == OP_WriteFile: + self.handle_write_reply(op, m) + elif op.req_opcode in [OP_RemoveFile, OP_RemoveDirectory]: + self.handle_remove_reply(op, m) + elif op.req_opcode == OP_Rename: + self.handle_rename_reply(op, m) + elif op.req_opcode == OP_CreateDirectory: + self.handle_mkdir_reply(op, m) + elif op.req_opcode == OP_ReadFile: + self.handle_reply_read(op, m) + elif op.req_opcode == OP_CalcFileCRC32: + self.handle_crc_reply(op, m) + else: + print('FTP Unknown %s' % str(op)) + + def send_gap_read(self, g): + '''send a read for a gap''' + (offset, length) = g + if self.ftp_settings.debug > 0: + print("Gap read of %u at %u rem=%u blog=%u" % (length, offset, len(self.read_gaps), self.backlog)) + read = FTP_OP(self.seq, self.session, OP_ReadFile, length, 0, 0, offset, None) + self.send(read) + self.read_gaps.remove(g) + self.read_gaps.append(g) + self.last_gap_send = time.time() + self.read_gap_times[g] = self.last_gap_send + self.backlog += 1 + + def check_read_send(self): + '''see if we should send another gap read''' + if len(self.read_gaps) == 0: + return + g = self.read_gaps[0] + now = time.time() + dt = now - self.read_gap_times[g] + if not self.reached_eof: + # send gap reads once + for g in self.read_gap_times.keys(): + if self.read_gap_times[g] == 0: + self.send_gap_read(g) + return + if self.read_gap_times[g] > 0 and dt > self.ftp_settings.retry_time: + if self.backlog > 0: + self.backlog -= 1 + self.read_gap_times[g] = 0 + + if self.read_gap_times[g] != 0: + # still pending + return + if not self.reached_eof and self.backlog >= self.ftp_settings.max_backlog: + # don't fill queue too far until we have got past the burst + return + if now - self.last_gap_send < 0.05: + # don't send too fast + return + self.send_gap_read(g) + + def idle_task(self): + '''check for file gaps and lost requests''' + now = time.time() + + # see if we lost an open reply + if self.op_start is not None and now - self.op_start > 1.0 and self.last_op.opcode == OP_OpenFileRO: + self.op_start = now + self.open_retries += 1 + if self.open_retries > 2: + # fail the get + self.op_start = None + self.terminate_session() + return + if self.ftp_settings.debug > 0: + print("FTP: retry open") + send_op = self.last_op + self.send(FTP_OP(self.seq, self.session, OP_TerminateSession, 0, 0, 0, 0, None)) + self.session = (self.session + 1) % 256 + send_op.session = self.session + self.send(send_op) + + if len(self.read_gaps) == 0 and self.last_burst_read is None and self.write_list is None: + return + + if self.fh is None: + return + + # see if burst read has stalled + if not self.reached_eof and self.last_burst_read is not None and now - self.last_burst_read > self.ftp_settings.retry_time: + dt = now - self.last_burst_read + self.last_burst_read = now + if self.ftp_settings.debug > 0: + print("Retry read at %u rtt=%.2f dt=%.2f" % (self.fh.tell(), self.rtt, dt)) + self.send(FTP_OP(self.seq, self.session, OP_BurstReadFile, self.burst_size, 0, 0, self.fh.tell(), None)) + self.read_retries += 1 + + # see if we can fill gaps + self.check_read_send() + + if self.write_list is not None: + self.send_more_writes() + +def init(mpstate): + '''initialise module''' + return FTPModule(mpstate) From 6098497e22c6aa71a5c46c6cb15b895be177327d Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Wed, 7 Aug 2024 09:46:08 +0200 Subject: [PATCH 2/2] examples: change mavftp class to be a python3 library with a simple API. The library can be called directly and it is a mavftp application the application contains documented argparse arguments Add a example that uses the library --- examples/mavftp_example.py | 280 +++++++ mavftp.py | 1459 +++++++++++++++++++++++++++--------- tests/test_mavftp.py | 249 ++++++ 3 files changed, 1644 insertions(+), 344 deletions(-) create mode 100755 examples/mavftp_example.py create mode 100644 tests/test_mavftp.py diff --git a/examples/mavftp_example.py b/examples/mavftp_example.py new file mode 100755 index 000000000..0a4ced6ca --- /dev/null +++ b/examples/mavftp_example.py @@ -0,0 +1,280 @@ +#!/usr/bin/env python3 + +''' +MAVLink File Transfer Protocol support example + +SPDX-FileCopyrightText: 2024 Amilcar Lucas + +SPDX-License-Identifier: GPL-3.0-or-later +''' + +from argparse import ArgumentParser + +from logging import basicConfig as logging_basicConfig +from logging import getLevelName as logging_getLevelName + +from logging import debug as logging_debug +from logging import info as logging_info +from logging import error as logging_error + +import os +import sys +#import time + +import requests + + +from pymavlink import mavutil +from pymavlink import mavftp + +old_mavftp_member_variable_values = {} + + +# pylint: disable=duplicate-code +def argument_parser(): + """ + Parses command-line arguments for the script. + """ + parser = ArgumentParser(description='This main is just an example, adapt it to your needs') + parser.add_argument("--baudrate", type=int, default=115200, + help="master port baud rate. Defaults to %(default)s") + parser.add_argument("--device", type=str, default='', + help="serial device. For windows use COMx where x is the port number. " + "For Unix use /dev/ttyUSBx where x is the port number. Defaults to autodetection") + parser.add_argument("--source-system", type=int, default=250, + help='MAVLink source system for this GCS. Defaults to %(default)s') + parser.add_argument("--loglevel", default="INFO", + help="log level. Defaults to %(default)s") + + # MAVFTP settings + parser.add_argument("--debug", type=int, default=0, choices=[0, 1, 2], + help="Debug level 0 for none, 2 for max verbosity. Defaults to %(default)s") + + return parser.parse_args() + +def auto_detect_serial(): + preferred_ports = [ + '*FTDI*', + "*3D*", + "*USB_to_UART*", + '*Ardu*', + '*PX4*', + '*Hex_*', + '*Holybro_*', + '*mRo*', + '*FMU*', + '*Swift-Flyer*', + '*Serial*', + '*CubePilot*', + '*Qiotek*', + ] + serial_list = mavutil.auto_detect_serial(preferred_list=preferred_ports) + serial_list.sort(key=lambda x: x.device) + + # remove OTG2 ports for dual CDC + if len(serial_list) == 2 and serial_list[0].device.startswith("/dev/serial/by-id"): + if serial_list[0].device[:-1] == serial_list[1].device[0:-1]: + serial_list.pop(1) + + return serial_list + + +def auto_connect(device): + comport = None + if device: + comport = mavutil.SerialPort(device=device, description=device) + else: + autodetect_serial = auto_detect_serial() + if autodetect_serial: + # Resolve the soft link if it's a Linux system + if os.name == 'posix': + try: + dev = autodetect_serial[0].device + logging_debug("Auto-detected device %s", dev) + # Get the directory part of the soft link + softlink_dir = os.path.dirname(dev) + # Resolve the soft link and join it with the directory part + resolved_path = os.path.abspath(os.path.join(softlink_dir, os.readlink(dev))) + autodetect_serial[0].device = resolved_path + logging_debug("Resolved soft link %s to %s", dev, resolved_path) + except OSError: + pass # Not a soft link, proceed with the original device path + comport = autodetect_serial[0] + else: + logging_error("No serial ports found. Please connect a flight controller and try again.") + sys.exit(1) + return comport + + +def wait_heartbeat(m): + '''wait for a heartbeat so we know the target system IDs''' + logging_info("Waiting for flight controller heartbeat") + m.wait_heartbeat() + logging_info("Got heartbeat from system %u, component %u", m.target_system, m.target_system) +# pylint: enable=duplicate-code + + +def delete_local_file_if_exists(filename): + if os.path.exists(filename): + os.remove(filename) + + +def get_list_dir(mav_ftp, directory): + ret = mav_ftp.cmd_list([directory]) + ret.display_message() + debug_class_member_variable_changes(mav_ftp) + + +def get_file(mav_ftp, remote_filename, local_filename, timeout=5): + #session = mav_ftp.session # save the session to restore it after the file transfer + mav_ftp.cmd_get([remote_filename, local_filename]) + ret = mav_ftp.process_ftp_reply('OpenFileRO', timeout=timeout) + ret.display_message() + #mav_ftp.session = session # FIXME: this is a huge workaround hack # pylint: disable=fixme + debug_class_member_variable_changes(mav_ftp) + #time.sleep(0.2) + + +def get_last_log(mav_ftp): + try: + with open('LASTLOG.TXT', 'r', encoding='UTF-8') as file: + file_contents = file.readline() + remote_filenumber = int(file_contents.strip()) + except FileNotFoundError: + logging_error("File LASTLOG.TXT not found.") + return + except ValueError: + logging_error("Could not extract last log file number from LASTLOG.TXT contants %s", file_contents) + return + remote_filenumber = remote_filenumber - 1 # we do not want the very last log + remote_filename = f'/APM/LOGS/{remote_filenumber:08}.BIN' + get_file(mav_ftp, remote_filename, 'LASTLOG.BIN', 0) + + +def download_script(url, local_filename): + # Download the script from the internet to the PC + response = requests.get(url, timeout=5) + + if response.status_code == 200: + with open(local_filename, "wb") as file: + file.write(response.content) + else: + logging_error("Failed to download the file") + + +def create_directory(mav_ftp, remote_directory): + ret = mav_ftp.cmd_mkdir([remote_directory]) + ret.display_message() + debug_class_member_variable_changes(mav_ftp) + + +def remove_directory(mav_ftp, remote_directory): + ret = mav_ftp.cmd_rmdir([remote_directory]) + ret.display_message() + debug_class_member_variable_changes(mav_ftp) + + +def upload_script(mav_ftp, remote_directory, local_filename, timeout): + # Upload it from the PC to the flight controller + mav_ftp.cmd_put([local_filename, remote_directory + '/' + local_filename]) + ret = mav_ftp.process_ftp_reply('CreateFile', timeout=timeout) + ret.display_message() + debug_class_member_variable_changes(mav_ftp) + + +def debug_class_member_variable_changes(instance): + return + global old_mavftp_member_variable_values # pylint: disable=global-statement, unreachable + new_mavftp_member_variable_values = instance.__dict__ + if old_mavftp_member_variable_values and instance.ftp_settings.debug > 1: # pylint: disable=too-many-nested-blocks + logging_info(f"{instance.__class__.__name__} member variable changes:") + for key, value in new_mavftp_member_variable_values.items(): + if old_mavftp_member_variable_values[key] != value: + old_value = old_mavftp_member_variable_values[key] + if old_value and isinstance(value, mavftp.FTP_OP): + # Convert both new and old FTP_OP instances to dictionaries for comparison + new_op_dict = dict(value.items()) + old_op_dict = dict(old_value.items()) if isinstance(old_value, mavftp.FTP_OP) else {} + for op_key, op_value in new_op_dict.items(): + old_op_value = old_op_dict.get(op_key) + if old_op_value != op_value: + logging_info(f"CHANGED {key}.{op_key}: {old_op_value} -> {op_value}") + else: + logging_info(f"CHANGED {key}: {old_mavftp_member_variable_values[key]} -> {value}") + old_mavftp_member_variable_values = new_mavftp_member_variable_values.copy() + +def main(): + '''for testing/example purposes only''' + args = argument_parser() + + logging_basicConfig(level=logging_getLevelName(args.loglevel), format='%(levelname)s - %(message)s') + + # create a mavlink serial instance + comport = auto_connect(args.device) + master = mavutil.mavlink_connection(comport.device, baud=args.baudrate, source_system=args.source_system) + + # wait for the heartbeat msg to find the system ID + wait_heartbeat(master) + + mav_ftp = mavftp.MAVFTP(master, + target_system=master.target_system, + target_component=master.target_component) + + mav_ftp.ftp_settings.debug = args.debug + + if args.loglevel == 'DEBUG': + mav_ftp.ftp_settings.debug = 2 + + debug_class_member_variable_changes(mav_ftp) + + get_list_dir(mav_ftp, '/APM/LOGS') + + delete_local_file_if_exists("params.param") + delete_local_file_if_exists("defaults.param") + mav_ftp.cmd_getparams(["params.param", "defaults.param"]) + ret = mav_ftp.process_ftp_reply('OpenFileRO', timeout=500) + ret.display_message() + + get_list_dir(mav_ftp, '/APM/LOGS') + + #delete_local_file_if_exists("LASTLOG.TXT") + delete_local_file_if_exists("LASTLOG.BIN") + + #get_file(mav_ftp, '/APM/LOGS/LASTLOG.TXT', 'LASTLOG.TXT') + + get_list_dir(mav_ftp, '/APM/LOGS') + + #get_file(mav_ftp, '/APM/LOGS/LASTLOG.TXT', 'LASTLOG2.TXT') + + get_last_log(mav_ftp) + + remove_directory(mav_ftp, "test_dir") + create_directory(mav_ftp, "test_dir") + remove_directory(mav_ftp, "test_dir") + create_directory(mav_ftp, "test_dir2") + + remote_directory = '/APM/Scripts' + #create_directory(mav_ftp, remote_directory) + + url = "https://discuss.ardupilot.org/uploads/short-url/4pyrl7PcfqiMEaRItUhljuAqLSs.lua" + local_filename = "copter-magfit-helper.lua" + + if not os.path.exists(local_filename): + download_script(url, local_filename) + + upload_script(mav_ftp, remote_directory, local_filename, 5) + + url = "https://raw.githubusercontent.com/ArduPilot/ardupilot/Copter-4.5/libraries/AP_Scripting/applets/" \ + "VTOL-quicktune.lua" + local_filename = "VTOL-quicktune.lua" + + if not os.path.exists(local_filename): + download_script(url, local_filename) + + upload_script(mav_ftp, remote_directory, local_filename, 5) + + master.close() + + +if __name__ == "__main__": + main() diff --git a/mavftp.py b/mavftp.py index 9c5d2a43d..d30980546 100644 --- a/mavftp.py +++ b/mavftp.py @@ -1,22 +1,37 @@ -#!/usr/bin/env python -'''mavlink file transfer support''' +#!/usr/bin/env python3 + +''' +MAVLink File Transfer Protocol support - https://mavlink.io/en/services/ftp.html +Original from MAVProxy/MAVProxy/modules/mavproxy_ftp.py + +SPDX-FileCopyrightText: 2011-2024 Andrew Tridgell, 2024 Amilcar Lucas + +SPDX-License-Identifier: GPL-3.0-or-later +''' + +from argparse import ArgumentParser + +import logging -import io -import time, os, sys import struct +import time import random -from pymavlink import mavutil +import os -try: - # py2 - from StringIO import StringIO as SIO -except ImportError: - # py3 - from io import BytesIO as SIO +from io import BytesIO as SIO -from MAVProxy.modules.lib import mp_module -from MAVProxy.modules.lib import mp_settings +import sys +from typing import Dict +from typing import Tuple + +from datetime import datetime + +from pymavlink import mavutil + + +# pylint: disable=too-many-lines +# pylint: disable=invalid-name # opcodes OP_None = 0 OP_TerminateSession = 1 @@ -50,23 +65,43 @@ ERR_FileProtected = 9 ERR_FileNotFound = 10 +ERR_NoErrorCodeInPayload = 64 +ERR_NoErrorCodeInNack = 65 +ERR_NoFilesystemErrorInPayload = 66 +ERR_InvalidErrorCode = 67 +ERR_PayloadTooLarge = 68 +ERR_InvalidOpcode = 69 +ERR_InvalidArguments = 70 +ERR_PutAlreadyInProgress = 71 +ERR_FailToOpenLocalFile = 72 +ERR_RemoteReplyTimeout = 73 + HDR_Len = 12 MAX_Payload = 239 +# pylint: enable=invalid-name -class FTP_OP: - def __init__(self, seq, session, opcode, size, req_opcode, burst_complete, offset, payload): - self.seq = seq - self.session = session - self.opcode = opcode - self.size = size - self.req_opcode = req_opcode - self.burst_complete = burst_complete - self.offset = offset - self.payload = payload +class FTP_OP: # pylint: disable=invalid-name, too-many-instance-attributes + """ + Represents an operation in the MAVLink File Transfer Protocol (FTP). + + This class encapsulates the details of a single FTP operation such as reading, writing, listing directories, etc., + including the necessary parameters and payload for the operation. + """ + def __init__(self, seq, session, opcode, size, # pylint: disable=too-many-arguments + req_opcode, burst_complete, offset, payload): + self.seq = seq # Sequence number for the operation. + self.session = session # Session identifier. + self.opcode = opcode # Operation code indicating the type of FTP operation. + self.size = size # Size of the operation. + self.req_opcode = req_opcode # Request operation code. + self.burst_complete = burst_complete # (bool) Flag indicating if the burst transfer is complete. + self.offset = offset # Offset for read/write operations. + self.payload = payload # (bytes) Payload for the operation. def pack(self): '''pack message''' - ret = struct.pack(" 0: - ret += " [%u]" % self.payload[0] + ret += f" [{self.payload[0]}]" return ret -class WriteQueue: - def __init__(self, ofs, size): - self.ofs = ofs - self.size = size - self.last_send = 0 - -class FTPModule(mp_module.MPModule): - def __init__(self, mpstate): - super(FTPModule, self).__init__(mpstate, "ftp", public=True) - self.add_command('ftp', self.cmd_ftp, "file transfer", - ["", - "set (FTPSETTING)", - "put (FILENAME) (FILENAME)"]) - self.ftp_settings = mp_settings.MPSettings( - [('debug', int, 0), - ('pkt_loss_tx', int, 0), - ('pkt_loss_rx', int, 0), - ('max_backlog', int, 5), - ('burst_read_size', int, 80), - ('write_size', int, 80), - ('write_qsize', int, 5), - ('retry_time', float, 0.5)]) - self.add_completion_function('(FTPSETTING)', - self.ftp_settings.completion) + def items(self): + """Yield each attribute and its value for the FTP_OP instance. For debugging purposes.""" + yield "seq", self.seq + yield "session", self.session + yield "opcode", self.opcode + yield "size", self.size + yield "req_opcode", self.req_opcode + yield "burst_complete", self.burst_complete + yield "offset", self.offset + yield "payload", self.payload + + +class WriteQueue: # pylint: disable=too-few-public-methods + """ + Manages a queue of write operations for the MAVFTP class. + + Keeps track of offsets and sizes for pending write operations to ensure orderly processing. + """ + def __init__(self, ofs: int, size: int): + self.ofs = ofs # Offset where the write operation starts. + self.size = size # Size of the data to be written. + self.last_send = 0 # Timestamp of the last send operation. + + +class ParamData: + """ + A class to manage parameter values and defaults for ArduPilot configuration. + """ + def __init__(self): + self.params = [] # params as (name, value, ptype) + self.defaults = None # defaults as (name, value, ptype) + + def add_param(self, name, value, ptype): + self.params.append((name, value, ptype)) + + def add_default(self, name, value, ptype): + if self.defaults is None: + self.defaults = [] + self.defaults.append((name, value, ptype)) + + +class MAVFTPSetting: # pylint: disable=too-few-public-methods + """A single MAVFTP setting with a name, type, value and default value.""" + def __init__(self, name, s_type, default): + self.name = name + self.type = s_type + self.default = default + self.value = default + + +class MAVFTPSettings: + """A collection of MAVFTP settings.""" + def __init__(self, s_vars): + self._vars = {} + for v in s_vars: + self.append(v) + + def append(self, v): + if isinstance(v, MAVFTPSetting): + setting = v + else: + (name, s_type, default) = v + setting = MAVFTPSetting(name, s_type, default) + self._vars[setting.name] = setting + + def __getattr__(self, name): + try: + return self._vars[name].value + except Exception as exc: + raise AttributeError from exc + + def __setattr__(self, name, value): + if name[0] == '_': + self.__dict__[name] = value + return + if name in self._vars: + self._vars[name].value = value + return + raise AttributeError + + +class MAVFTPReturn: + """The result of a MAVFTP operation.""" + def __init__(self, operation_name: str, error_code: int, system_error: int=0, # pylint: disable=too-many-arguments + invalid_error_code: int=0, invalid_opcode: int=0, invalid_payload_size: int=0): + self.operation_name = operation_name + self.error_code = error_code + self.system_error = system_error + self.invalid_error_code = invalid_error_code + self.invalid_opcode = invalid_opcode + self.invalid_payload_size = invalid_payload_size + + def display_message(self): # pylint: disable=too-many-branches + if self.error_code == ERR_None: + logging.info("%s succeeded", self.operation_name) + elif self.error_code == ERR_Fail: + logging.error("%s failed, generic error", self.operation_name) + elif self.error_code == ERR_FailErrno: + logging.error("%s failed, system error %u", self.operation_name, self.system_error) + elif self.error_code == ERR_InvalidDataSize: + logging.error("%s failed, invalid data size", self.operation_name) + elif self.error_code == ERR_InvalidSession: + logging.error("%s failed, session is not currently open", self.operation_name) + elif self.error_code == ERR_NoSessionsAvailable: + logging.error("%s failed, no sessions available", self.operation_name) + elif self.error_code == ERR_EndOfFile: + logging.error("%s failed, offset past end of file", self.operation_name) + elif self.error_code == ERR_UnknownCommand: + logging.error("%s failed, unknown command", self.operation_name) + elif self.error_code == ERR_FileExists: + logging.warning("%s failed, file/directory already exists", self.operation_name) + elif self.error_code == ERR_FileProtected: + logging.warning("%s failed, file/directory is protected", self.operation_name) + elif self.error_code == ERR_FileNotFound: + logging.warning("%s failed, file/directory not found", self.operation_name) + + elif self.error_code == ERR_NoErrorCodeInPayload: + logging.error("%s failed, payload contains no error code", self.operation_name) + elif self.error_code == ERR_NoErrorCodeInNack: + logging.error("%s failed, no error code", self.operation_name) + elif self.error_code == ERR_NoFilesystemErrorInPayload: + logging.error("%s failed, file-system error missing in payload", self.operation_name) + elif self.error_code == ERR_InvalidErrorCode: + logging.error("%s failed, invalid error code %u", self.operation_name, self.invalid_error_code) + elif self.error_code == ERR_PayloadTooLarge: + logging.error("%s failed, payload is too long %u", self.operation_name, self.invalid_payload_size) + elif self.error_code == ERR_InvalidOpcode: + logging.error("%s failed, invalid opcode %u", self.operation_name, self.invalid_opcode) + elif self.error_code == ERR_InvalidArguments: + logging.error("%s failed, invalid arguments", self.operation_name) + elif self.error_code == ERR_PutAlreadyInProgress: + logging.error("%s failed, put already in progress", self.operation_name) + elif self.error_code == ERR_FailToOpenLocalFile: + logging.error("%s failed, failed to open local file", self.operation_name) + elif self.error_code == ERR_RemoteReplyTimeout: + logging.error("%s failed, remote reply timeout", self.operation_name) + else: + logging.error("%s failed, unknown error %u in display_message()", self.operation_name, self.error_code) + + @property + def return_code(self): + return self.error_code + + +class MAVFTP: # pylint: disable=too-many-instance-attributes + """ + Implements the client-side logic for the MAVLink File Transfer Protocol (FTP) over MAVLink connections. + + Handles file operations such as reading, writing, listing directories, and managing sessions. + """ + def __init__(self, master, target_system, target_component, + settings=MAVFTPSettings( + [('debug', int, 0), + ('pkt_loss_tx', int, 0), + ('pkt_loss_rx', int, 0), + ('max_backlog', int, 5), + ('burst_read_size', int, 80), + ('write_size', int, 80), + ('write_qsize', int, 5), + ('idle_detection_time', float, 1.2), + ('read_retry_time', float, 1.0), + ('retry_time', float, 0.5)])): + self.ftp_settings = settings self.seq = 0 self.session = 0 self.network = 0 @@ -128,12 +296,14 @@ def __init__(self, mpstate): self.last_gap_send = 0 self.read_retries = 0 self.read_total = 0 + self.remote_file_size = None self.duplicates = 0 self.last_read = None self.last_burst_read = None self.op_start = None self.dir_offset = 0 self.last_op_time = time.time() + self.last_send_time = time.time() self.rtt = 0.5 self.reached_eof = False self.backlog = 0 @@ -147,40 +317,50 @@ def __init__(self, mpstate): self.write_recv_idx = -1 self.write_pending = 0 self.write_last_send = None - self.warned_component = False + self.open_retries = 0 + + self.master = master + self.target_system = target_system + self.target_component = target_component + + # Reset the flight controller FTP state-machine + self.__send(FTP_OP(self.seq, self.session, OP_ResetSessions, 0, 0, 0, 0, None)) + self.process_ftp_reply('ResetSessions') - def cmd_ftp(self, args): + def cmd_ftp(self, args) -> MAVFTPReturn: # pylint: disable=too-many-return-statements, too-many-branches '''FTP operations''' - usage = "Usage: ftp " + usage = "Usage: ftp " if len(args) < 1: - print(usage) - return + logging.error(usage) + return MAVFTPReturn("FTP command", ERR_InvalidArguments) if args[0] == 'list': - self.cmd_list(args[1:]) - elif args[0] == "set": - self.ftp_settings.command(args[1:]) - elif args[0] == 'get': - self.cmd_get(args[1:]) - elif args[0] == 'put': - self.cmd_put(args[1:]) - elif args[0] == 'rm': - self.cmd_rm(args[1:]) - elif args[0] == 'rmdir': - self.cmd_rmdir(args[1:]) - elif args[0] == 'rename': - self.cmd_rename(args[1:]) - elif args[0] == 'mkdir': - self.cmd_mkdir(args[1:]) - elif args[0] == 'crc': - self.cmd_crc(args[1:]) - elif args[0] == 'status': - self.cmd_status() - elif args[0] == 'cancel': - self.cmd_cancel() - else: - print(usage) + return self.cmd_list(args[1:]) + if args[0] == "set": + return self.ftp_settings.command(args[1:]) + if args[0] == 'get': + return self.cmd_get(args[1:]) + if args[0] == 'getparams': + return self.cmd_getparams(args[1:]) + if args[0] == 'put': + return self.cmd_put(args[1:]) + if args[0] == 'rm': + return self.cmd_rm(args[1:]) + if args[0] == 'rmdir': + return self.cmd_rmdir(args[1:]) + if args[0] == 'rename': + return self.cmd_rename(args[1:]) + if args[0] == 'mkdir': + return self.cmd_mkdir(args[1:]) + if args[0] == 'crc': + return self.cmd_crc(args[1:]) + if args[0] == 'status': + return self.cmd_status() + if args[0] == 'cancel': + return self.cmd_cancel() + logging.error(usage) + return MAVFTPReturn("FTP command", ERR_InvalidArguments) - def send(self, op): + def __send(self, op): '''send a request''' op.seq = self.seq payload = op.pack() @@ -192,12 +372,13 @@ def send(self, op): self.last_op = op now = time.time() if self.ftp_settings.debug > 1: - print("> %s dt=%.2f" % (op, now - self.last_op_time)) + logging.info("FTP: > %s dt=%.2f", op, now - self.last_op_time) self.last_op_time = time.time() + self.last_send_time = now - def terminate_session(self): + def __terminate_session(self): '''terminate current session''' - self.send(FTP_OP(self.seq, self.session, OP_TerminateSession, 0, 0, 0, 0, None)) + self.__send(FTP_OP(self.seq, self.session, OP_TerminateSession, 0, 0, 0, 0, None)) self.fh = None self.filename = None self.write_list = None @@ -205,6 +386,9 @@ def terminate_session(self): # tell caller that the transfer failed self.callback(None) self.callback = None + if self.callback_progress is not None: + self.callback_progress(None) + self.callback_progress = None if self.put_callback is not None: # tell caller that the transfer failed self.put_callback(None) @@ -217,77 +401,79 @@ def terminate_session(self): self.read_gap_times = {} self.last_read = None self.last_burst_read = None - self.session = (self.session + 1) % 256 self.reached_eof = False self.backlog = 0 self.duplicates = 0 if self.ftp_settings.debug > 0: - print("Terminated session") + logging.info("FTP: Terminated session") + self.process_ftp_reply('TerminateSession') + self.session = (self.session + 1) % 256 - def cmd_list(self, args): + def cmd_list(self, args) -> MAVFTPReturn: '''list files''' - if len(args) > 0: + if len(args) == 0: + dname = '/' + elif len(args) == 1: dname = args[0] else: - dname = '/' - print("Listing %s" % dname) + logging.error("Usage: list [directory]") + return MAVFTPReturn("ListDirectory", ERR_InvalidArguments) + logging.info("Listing %s", dname) enc_dname = bytearray(dname, 'ascii') self.total_size = 0 self.dir_offset = 0 op = FTP_OP(self.seq, self.session, OP_ListDirectory, len(enc_dname), 0, 0, self.dir_offset, enc_dname) - self.send(op) + self.__send(op) + return self.process_ftp_reply('ListDirectory') - def handle_list_reply(self, op, m): + def __handle_list_reply(self, op, _m) -> MAVFTPReturn: '''handle OP_ListDirectory reply''' if op.opcode == OP_Ack: dentries = sorted(op.payload.split(b'\x00')) - #print(dentries) + #logging.info(dentries) for d in dentries: if len(d) == 0: continue self.dir_offset += 1 try: - if sys.version_info.major >= 3: - d = str(d, 'ascii') - else: - d = str(d) - except Exception: + d = str(d, 'ascii') + except Exception: # pylint: disable=broad-exception-caught continue if d[0] == 'D': - print(" D %s" % d[1:]) + logging.info(" D %s", d[1:]) elif d[0] == 'F': (name, size) = d[1:].split('\t') size = int(size) self.total_size += size - print(" %s\t%u" % (name, size)) + logging.info(" %s\t%u", name, size) else: - print(d) + logging.info(d) # ask for more more = self.last_op more.offset = self.dir_offset - self.send(more) + self.__send(more) elif op.opcode == OP_Nack and len(op.payload) == 1 and op.payload[0] == ERR_EndOfFile: - print("Total size %.2f kByte" % (self.total_size / 1024.0)) + logging.info("Total size %.2f kByte", self.total_size / 1024.0) self.total_size = 0 else: - print('LIST: %s' % op) + return self.__decode_ftp_ack_and_nack(op) + return MAVFTPReturn("ListDirectory", ERR_None) - def cmd_get(self, args, callback=None, callback_progress=None): + def cmd_get(self, args, callback=None, progress_callback=None) -> MAVFTPReturn: '''get file''' - if len(args) == 0: - print("Usage: get FILENAME ") - return - self.terminate_session() + if len(args) == 0 or len(args) > 2: + logging.error("Usage: get [FILENAME ]") + return MAVFTPReturn("OpenFileRO", ERR_InvalidArguments) fname = args[0] if len(args) > 1: self.filename = args[1] else: self.filename = os.path.basename(fname) if callback is None or self.ftp_settings.debug > 1: - print("Getting %s as %s" % (fname, self.filename)) + logging.info("Getting %s to %s", fname, self.filename) self.op_start = time.time() self.callback = callback - self.callback_progress = callback_progress + self.callback_progress = progress_callback self.read_retries = 0 self.duplicates = 0 self.reached_eof = False @@ -296,35 +482,48 @@ def cmd_get(self, args, callback=None, callback_progress=None): self.burst_size = 239 elif self.burst_size > 239: self.burst_size = 239 + self.remote_file_size = None enc_fname = bytearray(fname, 'ascii') self.open_retries = 0 op = FTP_OP(self.seq, self.session, OP_OpenFileRO, len(enc_fname), 0, 0, 0, enc_fname) - self.send(op) + self.__send(op) + return MAVFTPReturn("OpenFileRO", ERR_None) - def handle_open_RO_reply(self, op, m): + def __handle_open_ro_reply(self, op, _m) -> MAVFTPReturn: '''handle OP_OpenFileRO reply''' if op.opcode == OP_Ack: if self.filename is None: - return + return MAVFTPReturn('OpenFileRO', ERR_FileNotFound) try: if self.callback is not None or self.filename == '-': self.fh = SIO() else: - self.fh = open(self.filename, 'wb') - except Exception as ex: - print("Failed to open %s: %s" % (self.filename, ex)) - self.terminate_session() - return + self.fh = open(self.filename, 'wb') # pylint: disable=consider-using-with + except Exception as ex: # pylint: disable=broad-except + logging.error("FTP: Failed to open local file %s: %s", self.filename, ex) + self.__terminate_session() + return MAVFTPReturn('OpenFileRO', ERR_FileNotFound) + if op.size == 4 and len(op.payload) >= 4: + self.remote_file_size = op.payload[0] + (op.payload[1] << 8) + (op.payload[2] << 16) + \ + (op.payload[3] << 24) + if self.ftp_settings.debug > 0: + logging.info("Remote file size: %u", self.remote_file_size) + else: + self.remote_file_size = None read = FTP_OP(self.seq, self.session, OP_BurstReadFile, self.burst_size, 0, 0, 0, None) self.last_burst_read = time.time() - self.send(read) - else: - if self.callback is None or self.ftp_settings.debug > 0: - print("ftp open failed") - self.terminate_session() + self.__send(read) + return MAVFTPReturn('OpenFileRO', ERR_None) + + ret = self.__decode_ftp_ack_and_nack(op) + if self.callback is None or self.ftp_settings.debug > 0: + ret.display_message() + self.__terminate_session() + return ret - def check_read_finished(self): + def __check_read_finished(self): '''check if download has completed''' + #logging.debug("FTP: check_read_finished: %s %s", self.reached_eof, self.read_gaps) if self.reached_eof and len(self.read_gaps) == 0: ofs = self.fh.tell() dt = time.time() - self.op_start @@ -335,45 +534,43 @@ def check_read_finished(self): self.callback = None elif self.filename == "-": self.fh.seek(0) - if sys.version_info.major < 3: - print(self.fh.read()) - else: - print(self.fh.read().decode('utf-8')) + print(self.fh.read().decode('utf-8')) else: - print("Wrote %u bytes to %s in %.2fs %.1fkByte/s" % (ofs, self.filename, dt, rate)) - self.terminate_session() + logging.info("Got %u bytes from %s in %.2fs %.1fkByte/s", ofs, self.filename, dt, rate) + self.remote_file_size = None + self.__terminate_session() return True return False - def write_payload(self, op): + def __write_payload(self, op): '''write payload from a read op''' self.fh.seek(op.offset) self.fh.write(op.payload) self.read_total += len(op.payload) - if self.callback_progress is not None: - self.callback_progress(self.fh, self.read_total) - - def handle_burst_read(self, op, m): + if self.callback_progress is not None and self.remote_file_size: + self.callback_progress(self.read_total/self.remote_file_size) + + def __handle_burst_read(self, op, _m) -> MAVFTPReturn: # pylint: disable=too-many-branches, too-many-statements, too-many-return-statements '''handle OP_BurstReadFile reply''' if self.ftp_settings.pkt_loss_tx > 0: - if random.uniform(0,100) < self.ftp_settings.pkt_loss_tx: + if random.uniform(0, 100) < self.ftp_settings.pkt_loss_tx: if self.ftp_settings.debug > 0: - print("FTP: dropping TX") - return + logging.warning("FTP: dropping TX") + return MAVFTPReturn('BurstReadFile', ERR_Fail) if self.fh is None or self.filename is None: if op.session != self.session: # old session - return - print("FTP Unexpected burst read reply") - print(op) - return + return MAVFTPReturn('BurstReadFile', ERR_InvalidSession) + logging.warning("FTP: Unexpected burst read reply. Will be discarded") + logging.info(op) + return MAVFTPReturn('BurstReadFile', ERR_Fail) self.last_burst_read = time.time() size = len(op.payload) if size > self.burst_size: # this server doesn't handle the burst size argument self.burst_size = MAX_Payload if self.ftp_settings.debug > 0: - print("Setting burst size to %u" % self.burst_size) + logging.info("FTP: Setting burst size to %u", self.burst_size) if op.opcode == OP_Ack and self.fh is not None: ofs = self.fh.tell() if op.offset < ofs: @@ -383,16 +580,16 @@ def handle_burst_read(self, op, m): self.read_gaps.remove(gap) self.read_gap_times.pop(gap) if self.ftp_settings.debug > 0: - print("FTP: removed gap", gap, self.reached_eof, len(self.read_gaps)) + logging.info("FTP: removed gap %u, %u, %u", gap, self.reached_eof, len(self.read_gaps)) else: if self.ftp_settings.debug > 0: - print("FTP: dup read reply at %u of len %u ofs=%u" % (op.offset, op.size, self.fh.tell())) + logging.info("FTP: dup read reply at %u of len %u ofs=%u", op.offset, op.size, self.fh.tell()) self.duplicates += 1 - return - self.write_payload(op) + return MAVFTPReturn('BurstReadFile', ERR_Fail) + self.__write_payload(op) self.fh.seek(ofs) - if self.check_read_finished(): - return + if self.__check_read_finished(): + return MAVFTPReturn('BurstReadFile', ERR_None) elif op.offset > ofs: # we have a gap gap = (ofs, op.offset-ofs) @@ -406,53 +603,58 @@ def handle_burst_read(self, op, m): self.read_gaps.append(g) self.read_gap_times[g] = 0 gap = (gap[0] + max_read, gap[1] - max_read) - self.write_payload(op) + self.__write_payload(op) else: - self.write_payload(op) + self.__write_payload(op) if op.burst_complete: if op.size > 0 and op.size < self.burst_size: # a burst complete with non-zero size and less than burst packet size # means EOF if not self.reached_eof and self.ftp_settings.debug > 0: - print("EOF at %u with %u gaps t=%.2f" % (self.fh.tell(), len(self.read_gaps), time.time() - self.op_start)) + logging.info("FTP: EOF at %u with %u gaps t=%.2f", self.fh.tell(), + len(self.read_gaps), time.time() - self.op_start) self.reached_eof = True - if self.check_read_finished(): - return - self.check_read_send() - return + if self.__check_read_finished(): + return MAVFTPReturn('BurstReadFile', ERR_None) + self.__check_read_send() + return MAVFTPReturn('BurstReadFile', ERR_None) more = self.last_op more.offset = op.offset + op.size if self.ftp_settings.debug > 0: - print("FTP: burst continue at %u %u" % (more.offset, self.fh.tell())) - self.send(more) + logging.info("FTP: burst continue at %u %u", more.offset, self.fh.tell()) + self.__send(more) elif op.opcode == OP_Nack: ecode = op.payload[0] - if self.ftp_settings.debug > 0: - print("FTP: burst nack: ", op) - if ecode == ERR_EndOfFile or ecode == 0: + if ecode in (ERR_EndOfFile, 0): if not self.reached_eof and op.offset > self.fh.tell(): # we lost the last part of the burst if self.ftp_settings.debug > 0: - print("burst lost EOF %u %u" % (self.fh.tell(), op.offset)) - return + logging.error("FTP: burst lost EOF %u %u", self.fh.tell(), op.offset) + return MAVFTPReturn('BurstReadFile', ERR_Fail) if not self.reached_eof and self.ftp_settings.debug > 0: - print("EOF at %u with %u gaps t=%.2f" % (self.fh.tell(), len(self.read_gaps), time.time() - self.op_start)) + logging.info("FTP: EOF at %u with %u gaps t=%.2f", self.fh.tell(), + len(self.read_gaps), time.time() - self.op_start) self.reached_eof = True - if self.check_read_finished(): - return - self.check_read_send() + if self.__check_read_finished(): + return MAVFTPReturn('BurstReadFile', ERR_None) + self.__check_read_send() elif self.ftp_settings.debug > 0: - print("FTP: burst Nack (ecode:%u): %s" % (ecode, op)) + logging.info("FTP: burst Nack (ecode:%u): %s", ecode, op) + return MAVFTPReturn('BurstReadFile', ERR_Fail) + if self.ftp_settings.debug > 0: + logging.error("FTP: burst nack: %s", op) + return MAVFTPReturn('BurstReadFile', ERR_Fail) else: - print("FTP: burst error: %s" % op) + logging.warning("FTP: burst error: %s", op) + return MAVFTPReturn('BurstReadFile', ERR_Fail) - def handle_reply_read(self, op, m): + def __handle_reply_read(self, op, _m) -> MAVFTPReturn: '''handle OP_ReadFile reply''' if self.fh is None or self.filename is None: if self.ftp_settings.debug > 0: - print("FTP Unexpected read reply") - print(op) - return + logging.warning("FTP: Unexpected read reply") + logging.warning(op) + return MAVFTPReturn('ReadFile', ERR_Fail) if self.backlog > 0: self.backlog -= 1 if op.opcode == OP_Ack and self.fh is not None: @@ -461,40 +663,41 @@ def handle_reply_read(self, op, m): self.read_gaps.remove(gap) self.read_gap_times.pop(gap) ofs = self.fh.tell() - self.write_payload(op) + self.__write_payload(op) self.fh.seek(ofs) if self.ftp_settings.debug > 0: - print("FTP: removed gap", gap, self.reached_eof, len(self.read_gaps)) - if self.check_read_finished(): - return + logging.info("FTP: removed gap %u, %u, %u", gap, self.reached_eof, len(self.read_gaps)) + if self.__check_read_finished(): + return MAVFTPReturn('ReadFile', ERR_None) elif op.size < self.burst_size: - print("FTP: file size changed to %u" % op.offset+op.size) - self.terminate_session() + logging.info("FTP: file size changed to %u", op.offset+op.size) + self.__terminate_session() else: self.duplicates += 1 if self.ftp_settings.debug > 0: - print("FTP: no gap read", gap, len(self.read_gaps)) + logging.info("FTP: no gap read %u, %u", gap, len(self.read_gaps)) elif op.opcode == OP_Nack: - print("Read failed with %u gaps" % len(self.read_gaps), str(op)) - self.terminate_session() - self.check_read_send() - - def cmd_put(self, args, fh=None, callback=None, progress_callback=None): + logging.info("FTP: Read failed with %u gaps %s", len(self.read_gaps), str(op)) + self.__terminate_session() + self.__check_read_send() + return MAVFTPReturn('ReadFile', ERR_None) + + def cmd_put(self, args, fh=None, callback=None, progress_callback=None) -> MAVFTPReturn: '''put file''' - if len(args) == 0: - print("Usage: put FILENAME ") - return + if len(args) == 0 or len(args) > 2: + logging.error("Usage: put [FILENAME ]") + return MAVFTPReturn("CreateFile", ERR_InvalidArguments) if self.write_list is not None: - print("put already in progress") - return + logging.error("FTP: put already in progress") + return MAVFTPReturn("CreateFile", ERR_PutAlreadyInProgress) fname = args[0] self.fh = fh if self.fh is None: try: - self.fh = open(fname, 'rb') - except Exception as ex: - print("Failed to open %s: %s" % (fname, ex)) - return + self.fh = open(fname, 'rb') # pylint: disable=consider-using-with + except Exception as ex: # pylint: disable=broad-exception-caught + logging.error("FTP: Failed to open %s: %s", fname, ex) + return MAVFTPReturn("CreateFile", ERR_FailToOpenLocalFile) if len(args) > 1: self.filename = args[1] else: @@ -502,7 +705,7 @@ def cmd_put(self, args, fh=None, callback=None, progress_callback=None): if self.filename.endswith("/"): self.filename += os.path.basename(fname) if callback is None: - print("Putting %s as %s" % (fname, self.filename)) + logging.info("Putting %s to %s", fname, self.filename) self.fh.seek(0,2) file_size = self.fh.tell() self.fh.seek(0) @@ -529,9 +732,10 @@ def cmd_put(self, args, fh=None, callback=None, progress_callback=None): self.op_start = time.time() enc_fname = bytearray(self.filename, 'ascii') op = FTP_OP(self.seq, self.session, OP_CreateFile, len(enc_fname), 0, 0, 0, enc_fname) - self.send(op) + self.__send(op) + return MAVFTPReturn("CreateFile", ERR_None) - def put_finished(self, flen): + def __put_finished(self, flen): '''finish a put''' if self.put_callback_progress: self.put_callback_progress(1.0) @@ -540,25 +744,29 @@ def put_finished(self, flen): self.put_callback(flen) self.put_callback = None else: - print("Sent file of length ", flen) - - def handle_create_file_reply(self, op, m): + dt = time.time() - self.op_start + rate = (flen / dt) / 1024.0 + logging.info("Put %u bytes to %s file in %.2fs %.1fkByte/s", flen, self.filename, dt, rate) + + def __handle_create_file_reply(self, op, _m) -> MAVFTPReturn: '''handle OP_CreateFile reply''' if self.fh is None: - self.terminate_session() - return + self.__terminate_session() + return MAVFTPReturn('CreateFile', ERR_FileNotFound) if op.opcode == OP_Ack: - self.send_more_writes() + self.__send_more_writes() else: - print("Create failed") - self.terminate_session() + ret = self.__decode_ftp_ack_and_nack(op) + self.__terminate_session() + return ret + return MAVFTPReturn('CreateFile', ERR_None) - def send_more_writes(self): + def __send_more_writes(self): '''send some more writes''' if len(self.write_list) == 0: # all done - self.put_finished(self.write_file_size) - self.terminate_session() + self.__put_finished(self.write_file_size) + self.__terminate_session() return now = time.time() @@ -568,7 +776,7 @@ def send_more_writes(self): self.write_pending = max(0, self.write_pending-1) n = min(self.ftp_settings.write_qsize-self.write_pending, len(self.write_list)) - for i in range(n): + for _i in range(n): # send in round-robin, skipping any that have been acked idx = self.write_idx while idx not in self.write_list: @@ -577,20 +785,20 @@ def send_more_writes(self): self.fh.seek(ofs) data = self.fh.read(self.write_block_size) write = FTP_OP(self.seq, self.session, OP_WriteFile, len(data), 0, 0, ofs, bytearray(data)) - self.send(write) + self.__send(write) self.write_idx = (idx + 1) % self.write_total self.write_pending += 1 self.write_last_send = now - def handle_write_reply(self, op, m): + def __handle_write_reply(self, op, _m) -> MAVFTPReturn: '''handle OP_WriteFile reply''' if self.fh is None: - self.terminate_session() - return + self.__terminate_session() + return MAVFTPReturn('WriteFile', ERR_FileNotFound) if op.opcode != OP_Ack: - print("Write failed") - self.terminate_session() - return + logging.error("FTP: Write failed") + self.__terminate_session() + return MAVFTPReturn('WriteFile', ERR_FileProtected) # assume the FTP server processes the blocks sequentially. This means # when we receive an ack that any blocks between the last ack and this @@ -604,177 +812,193 @@ def handle_write_reply(self, op, m): self.write_acks += 1 if self.put_callback_progress: self.put_callback_progress(self.write_acks/float(self.write_total)) - self.send_more_writes() + self.__send_more_writes() + return MAVFTPReturn('WriteFile', ERR_None) - def cmd_rm(self, args): + def cmd_rm(self, args) -> MAVFTPReturn: '''remove file''' - if len(args) == 0: - print("Usage: rm FILENAME") - return + if len(args) != 1: + logging.error("Usage: rm [FILENAME]") + return MAVFTPReturn("RemoveFile", ERR_InvalidArguments) fname = args[0] - print("Removing %s" % fname) + logging.info("Removing file %s", fname) enc_fname = bytearray(fname, 'ascii') op = FTP_OP(self.seq, self.session, OP_RemoveFile, len(enc_fname), 0, 0, 0, enc_fname) - self.send(op) + self.__send(op) + return self.process_ftp_reply('RemoveFile') def cmd_rmdir(self, args): '''remove directory''' - if len(args) == 0: - print("Usage: rmdir FILENAME") - return + if len(args) != 1: + logging.error("Usage: rmdir [DIRECTORYNAME]") + return MAVFTPReturn("RemoveDirectory", ERR_InvalidArguments) dname = args[0] - print("Removing %s" % dname) + logging.info("Removing directory %s", dname) enc_dname = bytearray(dname, 'ascii') op = FTP_OP(self.seq, self.session, OP_RemoveDirectory, len(enc_dname), 0, 0, 0, enc_dname) - self.send(op) + self.__send(op) + return self.process_ftp_reply('RemoveDirectory') - def handle_remove_reply(self, op, m): + def __handle_remove_reply(self, op, _m): '''handle remove reply''' - if op.opcode != OP_Ack: - print("Remove failed %s" % op) + return self.__decode_ftp_ack_and_nack(op) - def cmd_rename(self, args): - '''rename file''' + def cmd_rename(self, args) -> MAVFTPReturn: + '''rename file or directory''' if len(args) < 2: - print("Usage: rename OLDNAME NEWNAME") - return + logging.error("Usage: rename [OLDNAME NEWNAME]") + return MAVFTPReturn("Rename", ERR_InvalidArguments) name1 = args[0] name2 = args[1] - print("Renaming %s to %s" % (name1, name2)) + logging.info("Renaming %s to %s", name1, name2) enc_name1 = bytearray(name1, 'ascii') enc_name2 = bytearray(name2, 'ascii') enc_both = enc_name1 + b'\x00' + enc_name2 op = FTP_OP(self.seq, self.session, OP_Rename, len(enc_both), 0, 0, 0, enc_both) - self.send(op) + self.__send(op) + return self.process_ftp_reply('Rename') - def handle_rename_reply(self, op, m): + def __handle_rename_reply(self, op, _m): '''handle rename reply''' - if op.opcode != OP_Ack: - print("Rename failed %s" % op) + return self.__decode_ftp_ack_and_nack(op) - def cmd_mkdir(self, args): + def cmd_mkdir(self, args) -> MAVFTPReturn: '''make directory''' - if len(args) < 1: - print("Usage: mkdir NAME") - return + if len(args) != 1: + logging.error("Usage: mkdir NAME") + return MAVFTPReturn("CreateDirectory", ERR_InvalidArguments) name = args[0] - print("Creating directory %s" % name) + logging.info("Creating directory %s", name) enc_name = bytearray(name, 'ascii') op = FTP_OP(self.seq, self.session, OP_CreateDirectory, len(enc_name), 0, 0, 0, enc_name) - self.send(op) + self.__send(op) + return self.process_ftp_reply('CreateDirectory') - def handle_mkdir_reply(self, op, m): + def __handle_mkdir_reply(self, op, _m): '''handle mkdir reply''' - if op.opcode != OP_Ack: - print("Create directory failed %s" % op) + return self.__decode_ftp_ack_and_nack(op) - def cmd_crc(self, args): - '''get crc''' - if len(args) < 1: - print("Usage: crc NAME") - return + def cmd_crc(self, args) -> MAVFTPReturn: + '''get file crc''' + if len(args) != 1: + logging.error("Usage: crc [NAME]") + return MAVFTPReturn("CalcFileCRC32", ERR_InvalidArguments) name = args[0] self.filename = name self.op_start = time.time() - print("Getting CRC for %s" % name) + logging.info("Getting CRC for %s", name) enc_name = bytearray(name, 'ascii') op = FTP_OP(self.seq, self.session, OP_CalcFileCRC32, len(enc_name), 0, 0, 0, bytearray(enc_name)) - self.send(op) + self.__send(op) + return self.process_ftp_reply('CalcFileCRC32') - def handle_crc_reply(self, op, m): + def __handle_crc_reply(self, op, _m): '''handle crc reply''' if op.opcode == OP_Ack and op.size == 4: crc, = struct.unpack(" MAVFTPReturn: '''cancel any pending op''' - self.terminate_session() + self.__terminate_session() + return MAVFTPReturn("TerminateSession", ERR_None) - def cmd_status(self): + def cmd_status(self) -> MAVFTPReturn: '''show status''' if self.fh is None: - print("No transfer in progress") + logging.info("No transfer in progress") else: ofs = self.fh.tell() dt = time.time() - self.op_start rate = (ofs / dt) / 1024.0 - print("Transfer at offset %u with %u gaps %u retries %.1f kByte/sec" % (ofs, len(self.read_gaps), self.read_retries, rate)) + logging.info("Transfer at offset %u with %u gaps %u retries %.1f kByte/sec", + ofs, len(self.read_gaps), self.read_retries, rate) + return MAVFTPReturn("Status", ERR_None) - def op_parse(self, m): + def __op_parse(self, m): '''parse a FILE_TRANSFER_PROTOCOL msg''' hdr = bytearray(m.payload[0:12]) - (seq, session, opcode, size, req_opcode, burst_complete, pad, offset) = struct.unpack(" MAVFTPReturn: # pylint: disable=too-many-branches, too-many-return-statements '''handle a mavlink packet''' + operation_name = "mavlink_packet" mtype = m.get_type() - if mtype == "FILE_TRANSFER_PROTOCOL": - if (m.target_system != self.settings.source_system or - m.target_component != self.settings.source_component): - if m.target_system == self.settings.source_system and not self.warned_component: - self.warned_component = True - print("FTP reply for mavlink component %u" % m.target_component) - return + if mtype != "FILE_TRANSFER_PROTOCOL": + logging.error("FTP: Unexpected MAVLink message type %s", mtype) + return MAVFTPReturn(operation_name, ERR_Fail) - op = self.op_parse(m) - now = time.time() - dt = now - self.last_op_time - if self.ftp_settings.debug > 1: - print("< %s dt=%.2f" % (op, dt)) - self.last_op_time = now - if self.ftp_settings.pkt_loss_rx > 0: - if random.uniform(0,100) < self.ftp_settings.pkt_loss_rx: - if self.ftp_settings.debug > 1: - print("FTP: dropping packet RX") - return - - if op.req_opcode == self.last_op.opcode and op.seq == (self.last_op.seq + 1) % 256: - self.rtt = max(min(self.rtt, dt), 0.01) - if op.req_opcode == OP_ListDirectory: - self.handle_list_reply(op, m) - elif op.req_opcode == OP_OpenFileRO: - self.handle_open_RO_reply(op, m) - elif op.req_opcode == OP_BurstReadFile: - self.handle_burst_read(op, m) - elif op.req_opcode == OP_TerminateSession: - pass - elif op.req_opcode == OP_CreateFile: - self.handle_create_file_reply(op, m) - elif op.req_opcode == OP_WriteFile: - self.handle_write_reply(op, m) - elif op.req_opcode in [OP_RemoveFile, OP_RemoveDirectory]: - self.handle_remove_reply(op, m) - elif op.req_opcode == OP_Rename: - self.handle_rename_reply(op, m) - elif op.req_opcode == OP_CreateDirectory: - self.handle_mkdir_reply(op, m) - elif op.req_opcode == OP_ReadFile: - self.handle_reply_read(op, m) - elif op.req_opcode == OP_CalcFileCRC32: - self.handle_crc_reply(op, m) - else: - print('FTP Unknown %s' % str(op)) + if m.target_system != self.master.source_system or m.target_component != self.master.source_component: + logging.info("FTP: wrong MAVLink target %u component %u. Will discard message", + m.target_system, m.target_component) + return MAVFTPReturn(operation_name, ERR_Fail) - def send_gap_read(self, g): + op = self.__op_parse(m) + now = time.time() + dt = now - self.last_op_time + if self.ftp_settings.debug > 1: + logging.info("FTP: < %s dt=%.2f", op, dt) + if op.session != self.session: + if self.ftp_settings.debug > 0: + logging.warning("FTP: wrong session replied %u expected %u. Will discard message", + op.session, self.session) + return MAVFTPReturn(operation_name, ERR_InvalidSession) + self.last_op_time = now + if self.ftp_settings.pkt_loss_rx > 0: + if random.uniform(0, 100) < self.ftp_settings.pkt_loss_rx: + if self.ftp_settings.debug > 1: + logging.warning("FTP: dropping packet RX") + return MAVFTPReturn(operation_name, ERR_Fail) + + if op.req_opcode == self.last_op.opcode and op.seq == (self.last_op.seq + 1) % 256: + self.rtt = max(min(self.rtt, dt), 0.01) + + if op.req_opcode == OP_ListDirectory: + return self.__handle_list_reply(op, m) + if op.req_opcode == OP_OpenFileRO: + return self.__handle_open_ro_reply(op, m) + if op.req_opcode == OP_BurstReadFile: + return self.__handle_burst_read(op, m) + if op.req_opcode == OP_ResetSessions: + return self.__handle_reset_sessions_reply(op, m) + if op.req_opcode in [OP_None, OP_TerminateSession]: + return MAVFTPReturn(operation_name, ERR_None) # ignore reply + if op.req_opcode == OP_CreateFile: + return self.__handle_create_file_reply(op, m) + if op.req_opcode == OP_WriteFile: + return self.__handle_write_reply(op, m) + if op.req_opcode in [OP_RemoveFile, OP_RemoveDirectory]: + return self.__handle_remove_reply(op, m) + if op.req_opcode == OP_Rename: + return self.__handle_rename_reply(op, m) + if op.req_opcode == OP_CreateDirectory: + return self.__handle_mkdir_reply(op, m) + if op.req_opcode == OP_ReadFile: + return self.__handle_reply_read(op, m) + if op.req_opcode == OP_CalcFileCRC32: + return self.__handle_crc_reply(op, m) + + logging.info('FTP Unknown %s', str(op)) + return MAVFTPReturn(operation_name, ERR_InvalidOpcode) + + def __send_gap_read(self, g): '''send a read for a gap''' (offset, length) = g if self.ftp_settings.debug > 0: - print("Gap read of %u at %u rem=%u blog=%u" % (length, offset, len(self.read_gaps), self.backlog)) + logging.info("FTP: Gap read of %u at %u rem=%u blog=%u", length, offset, len(self.read_gaps), self.backlog) read = FTP_OP(self.seq, self.session, OP_ReadFile, length, 0, 0, offset, None) - self.send(read) + self.__send(read) self.read_gaps.remove(g) self.read_gaps.append(g) self.last_gap_send = time.time() self.read_gap_times[g] = self.last_gap_send self.backlog += 1 - def check_read_send(self): + def __check_read_send(self): '''see if we should send another gap read''' if len(self.read_gaps) == 0: return @@ -783,9 +1007,9 @@ def check_read_send(self): dt = now - self.read_gap_times[g] if not self.reached_eof: # send gap reads once - for g in self.read_gap_times.keys(): - if self.read_gap_times[g] == 0: - self.send_gap_read(g) + for g, gap_time in self.read_gap_times.items(): + if gap_time == 0: + self.__send_gap_read(g) return if self.read_gap_times[g] > 0 and dt > self.ftp_settings.retry_time: if self.backlog > 0: @@ -801,50 +1025,597 @@ def check_read_send(self): if now - self.last_gap_send < 0.05: # don't send too fast return - self.send_gap_read(g) + self.__send_gap_read(g) - def idle_task(self): + def __idle_task(self) -> bool: '''check for file gaps and lost requests''' now = time.time() + assert self.ftp_settings.idle_detection_time > self.ftp_settings.read_retry_time, \ + "settings.idle_detection_time must be > settings.read_retry_time" # see if we lost an open reply - if self.op_start is not None and now - self.op_start > 1.0 and self.last_op.opcode == OP_OpenFileRO: + if self.op_start is not None and now - self.op_start > self.ftp_settings.read_retry_time and \ + self.last_op.opcode == OP_OpenFileRO: self.op_start = now self.open_retries += 1 if self.open_retries > 2: # fail the get self.op_start = None - self.terminate_session() - return + self.__terminate_session() + return False # Not idle yet if self.ftp_settings.debug > 0: - print("FTP: retry open") + logging.info("FTP: retry open") send_op = self.last_op - self.send(FTP_OP(self.seq, self.session, OP_TerminateSession, 0, 0, 0, 0, None)) + self.__send(FTP_OP(self.seq, self.session, OP_TerminateSession, 0, 0, 0, 0, None)) self.session = (self.session + 1) % 256 send_op.session = self.session - self.send(send_op) + self.__send(send_op) if len(self.read_gaps) == 0 and self.last_burst_read is None and self.write_list is None: - return + return self.__last_send_time_was_more_than_idle_detection_time_ago(now) if self.fh is None: - return + return self.__last_send_time_was_more_than_idle_detection_time_ago(now) # see if burst read has stalled - if not self.reached_eof and self.last_burst_read is not None and now - self.last_burst_read > self.ftp_settings.retry_time: + if not self.reached_eof and self.last_burst_read is not None and \ + now - self.last_burst_read > self.ftp_settings.retry_time: dt = now - self.last_burst_read self.last_burst_read = now if self.ftp_settings.debug > 0: - print("Retry read at %u rtt=%.2f dt=%.2f" % (self.fh.tell(), self.rtt, dt)) - self.send(FTP_OP(self.seq, self.session, OP_BurstReadFile, self.burst_size, 0, 0, self.fh.tell(), None)) + logging.info("FTP: Retry read at %u rtt=%.2f dt=%.2f", self.fh.tell(), self.rtt, dt) + self.__send(FTP_OP(self.seq, self.session, OP_BurstReadFile, self.burst_size, 0, 0, self.fh.tell(), None)) self.read_retries += 1 # see if we can fill gaps - self.check_read_send() + self.__check_read_send() if self.write_list is not None: - self.send_more_writes() + self.__send_more_writes() + + return self.__last_send_time_was_more_than_idle_detection_time_ago(now) + + def __last_send_time_was_more_than_idle_detection_time_ago(self, now: float) -> bool: + return self.last_send_time is not None and now - self.last_send_time > self.ftp_settings.idle_detection_time + + def __handle_reset_sessions_reply(self, op, _m): + '''handle reset sessions reply''' + return self.__decode_ftp_ack_and_nack(op) + + def process_ftp_reply(self, operation_name, timeout=5) -> MAVFTPReturn: + ''' execute an FTP operation that requires processing a MAVLink response''' + start_time = time.time() + ret = MAVFTPReturn(operation_name, ERR_Fail) + recv_timeout = 0.1 + assert timeout == 0 or timeout > self.ftp_settings.idle_detection_time, \ + "timeout must be > settings.idle_detection_time" + assert recv_timeout < self.ftp_settings.retry_time, \ + "recv_timeout must be < settings.retry_time" + + while True: # an FTP operation can have multiple responses + m = self.master.recv_match(type=['FILE_TRANSFER_PROTOCOL'], timeout=recv_timeout) + if m is not None: + if operation_name == "TerminateSession": + #self.silently_discard_terminate_session_reply() + ret = MAVFTPReturn(operation_name, ERR_None) + else: + ret = self.__mavlink_packet(m) + if self.__idle_task(): + break + if timeout > 0 and time.time() - start_time > timeout: # pylint: disable=chained-comparison + logging.error("FTP: timed out after %f seconds", time.time() - start_time) + ret = MAVFTPReturn(operation_name, ERR_RemoteReplyTimeout) + break + return ret + + def __decode_ftp_ack_and_nack(self, op: FTP_OP, operation_name: str='') -> MAVFTPReturn: + '''decode FTP Acknowledge reply''' + system_error = 0 + invalid_error_code = 0 + operation_name_dict = { + OP_None: "None", + OP_TerminateSession: "TerminateSession", + OP_ResetSessions: "ResetSessions", + OP_ListDirectory: "ListDirectory", + OP_OpenFileRO: "OpenFileRO", + OP_ReadFile: "ReadFile", + OP_CreateFile: "CreateFile", + OP_WriteFile: "WriteFile", + OP_RemoveFile: "RemoveFile", + OP_CreateDirectory: "CreateDirectory", + OP_RemoveDirectory: "RemoveDirectory", + OP_OpenFileWO: "OpenFileWO", + OP_TruncateFile: "TruncateFile", + OP_Rename: "Rename", + OP_CalcFileCRC32: "CalcFileCRC32", + OP_BurstReadFile: "BurstReadFile", + } + if operation_name: + op_ret_name = operation_name + else: + op_ret_name = operation_name_dict.get(op.req_opcode, "Unknown") + len_payload = len(op.payload) if op.payload is not None else 0 + if op.opcode == OP_Ack: + error_code = ERR_None + elif op.opcode == OP_Nack: + if len_payload <= 0: + error_code = ERR_NoErrorCodeInPayload + elif len_payload == 1: + error_code = op.payload[0] + if error_code == ERR_None: + error_code = ERR_NoErrorCodeInNack + elif error_code == ERR_FailErrno: + error_code = ERR_NoFilesystemErrorInPayload + elif error_code not in [ERR_Fail, ERR_InvalidDataSize, ERR_InvalidSession, ERR_NoSessionsAvailable, + ERR_EndOfFile, ERR_UnknownCommand, ERR_FileExists, ERR_FileProtected, + ERR_FileNotFound]: + invalid_error_code = error_code + error_code = ERR_InvalidErrorCode + elif op.payload[0] == ERR_FailErrno and len_payload == 2: + system_error = op.payload[1] + error_code = ERR_FailErrno + else: + error_code = ERR_PayloadTooLarge + else: + error_code = ERR_InvalidOpcode + return MAVFTPReturn(op_ret_name, error_code, system_error=system_error, invalid_error_code=invalid_error_code, + invalid_payload_size=len_payload, invalid_opcode=op.opcode) + + @staticmethod + def ftp_param_decode(data): # pylint: disable=too-many-locals + '''decode parameter data, returning ParamData''' + pdata = ParamData() + + magic = 0x671b + magic_defaults = 0x671c + if len(data) < 6: + logging.error("paramftp: Not enough data do decode, only %u bytes", len(data)) + return None + magic2, _num_params, total_params = struct.unpack(" 0 and data[0] == pad_byte: + data = data[1:] # skip pad bytes + + if len(data) == 0: + break + + ptype, plen = struct.unpack("> 4) & 0x0F + has_default = with_defaults and (flags & 1) != 0 + ptype &= 0x0F + + if ptype not in data_types: + logging.error("paramftp: bad type 0x%x", ptype) + return None + + (type_len, type_format) = data_types[ptype] + default_len = type_len if has_default else 0 + + name_len = ((plen >> 4) & 0x0F) + 1 + common_len = plen & 0x0F + name = last_name[0:common_len] + data[2:2+name_len] + vdata = data[2+name_len:2+name_len+type_len+default_len] + last_name = name + data = data[2+name_len+type_len+default_len:] + if with_defaults: + if has_default: + v1, v2, = struct.unpack("<" + type_format + type_format, vdata) + pdata.add_param(name, v1, ptype) + pdata.add_default(name, v2, ptype) + else: + v, = struct.unpack("<" + type_format, vdata) + pdata.add_param(name, v, ptype) + pdata.add_default(name, v, ptype) + else: + v, = struct.unpack("<" + type_format, vdata) + pdata.add_param(name, v, ptype) + count += 1 + + if count != total_params: + logging.error("paramftp: bad count %u should be %u", count, total_params) + return None + + return pdata + + @staticmethod + def missionplanner_sort(item: str) -> Tuple[str, ...]: + """ + Sorts a parameter name according to the rules defined in the Mission Planner software. + """ + return tuple(item.split("_")) + + @staticmethod + def extract_params(pdata, sort_type) -> Dict[str, Tuple[float, str]]: + ''' extract parameter values to an optionally sorted dictionary of name->(value, type)''' + pdict = {} + if pdata: + for (name, value, ptype) in pdata: + pdict[name.decode('utf-8')] = (value, ptype) + + if sort_type == "missionplanner": + pdict = dict(sorted(pdict.items(), key=lambda x: MAVFTP.missionplanner_sort(x[0]))) # sort alphabetically + elif sort_type == "mavproxy": + pdict = dict(sorted(pdict.items())) # sort in ASCIIbetical order + elif sort_type == "none": + pass + return pdict + + @staticmethod + def save_params(pdict: Dict[str, Tuple[float, str]], filename: str, sort_type: str, + add_datatype_comments: bool, add_timestamp_comment: bool) -> None: + '''Save Ardupilot parameter information to a local file''' + if not pdict: + return + with open(filename, 'w', encoding='utf-8') as f: + parameter_data_types = { + 1: '8-bit', + 2: '16-bit', + 3: '32-bit integer', + 4: '32-bit float', + } + if add_timestamp_comment: + f.write(f"# Parameters saved at {datetime.now().strftime('%Y-%m-%d %H:%M:%S')}\n") + for name, (value, datatype) in pdict.items(): + if sort_type == "missionplanner": + f.write(f"{name},{format(value, '.6f').rstrip('0').rstrip('.')}") + elif sort_type in ["mavproxy", "none"]: + f.write(f"{name:<16} {value:<8.6f}") + + if add_datatype_comments: + f.write(f" # {parameter_data_types[datatype]}") + f.write("\n") + logging.info("Outputted %u parameters to %s", len(pdict), filename) + + + def cmd_getparams(self, args, progress_callback=None, sort_type: str="missionplanner", # pylint: disable=too-many-arguments + add_datatype_comments: bool=False, add_timestamp_comment: bool=False): + ''' Decode the parameter file and save the values and defaults to disk ''' + + def decode_and_save_params(fh): + if fh is None: + logging.error("FTP: no parameter file handler") + return + try: + data = fh.read() + except IOError as exp: + logging.error("FTP: Failed to read file param.pck: %s", exp) + sys.exit(1) + pdata = MAVFTP.ftp_param_decode(data) + if pdata is None: + sys.exit(1) + + param_values = MAVFTP.extract_params(pdata.params, sort_type) + param_defaults = MAVFTP.extract_params(pdata.defaults, sort_type) + + param_values_path = args[0] + param_defaults_path = args[1] if len(args) > 1 else None + + MAVFTP.save_params(param_values, param_values_path, sort_type, add_datatype_comments, add_timestamp_comment) + MAVFTP.save_params(param_defaults, param_defaults_path, sort_type, add_datatype_comments, add_timestamp_comment) + + if self.ftp_settings.debug > 0: + for name, (value, _param_type) in param_values.items(): + if name in param_defaults: + logging.info("%-16s %f (default %f)", name, value, param_defaults[name][0]) + else: + logging.info("%-16s %f", name, value) + + self.cmd_get(['@PARAM/param.pck?withdefaults=1' if len(args) > 1 else '@PARAM/param.pck'], + callback=decode_and_save_params, + progress_callback=progress_callback) + + +if __name__ == "__main__": + + def argument_parser(): + """ + Parses command-line arguments for the script. + + This function sets up an argument parser to handle the command-line arguments for the script. + """ + parser = ArgumentParser(description='MAVFTP - MAVLink File Transfer Protocol https://mavlink.io/en/services/ftp.html' + ' A tool to do file operations between a ground control station and a drone using the MAVLink' + ' protocol.') + parser.add_argument("--baudrate", type=int, default=115200, + help="master port baud rate. Defaults to %(default)s") + parser.add_argument("--device", type=str, default='', + help="serial device. For windows use COMx where x is the port number. " + "For Unix use /dev/ttyUSBx where x is the port number. Defaults to autodetection") + parser.add_argument("--source-system", type=int, default=250, + help='MAVLink source system for this GCS. Defaults to %(default)s') + parser.add_argument("--loglevel", default="INFO", + help="log level. Defaults to %(default)s") + + # MAVFTP settings + parser.add_argument("--debug", type=int, default=0, choices=[0, 1, 2], + help="Debug level 0 for none, 2 for max verbosity. Defaults to %(default)s") + parser.add_argument("--pkt_loss_tx", type=int, default=0, + help="Packet loss on TX. Defaults to %(default)s") + parser.add_argument("--pkt_loss_rx", type=int, default=0, + help="Packet loss on RX. Defaults to %(default)s") + parser.add_argument("--max_backlog", type=int, default=5, + help="Max backlog. Defaults to %(default)s") + parser.add_argument("--burst_read_size", type=int, default=80, + help="Burst read size. Defaults to %(default)s") + parser.add_argument("--write_size", type=int, default=80, + help="Write size. Defaults to %(default)s") + parser.add_argument("--write_qsize", type=int, default=5, + help="Write queue size. Defaults to %(default)s") + parser.add_argument("--idle_detection_time", type=float, default=1.2, + help="Idle detection time. Defaults to %(default)s") + parser.add_argument("--read_retry_time", type=float, default=1.0, + help="Read retry time. Defaults to %(default)s") + parser.add_argument("--retry_time", type=float, default=0.5, + help="Retry time. Defaults to %(default)s") + + subparsers = parser.add_subparsers(dest="command", required=True) + + # Get command + parser_get = subparsers.add_parser( + 'get', + help='Get a file from the remote flight controller.') + parser_get.add_argument( + 'arg1', + type=str, + metavar='remote_path', + help='Path to the file on the remote flight controller.') + parser_get.add_argument( + 'arg2', + nargs='?', + type=str, + metavar='local_path', + help='Optional local path to save the file.') + + # Getparams command + parser_getparams = subparsers.add_parser( + 'getparams', + help='Get and decode parameters from the remote flight controller.') + parser_getparams.add_argument( + 'arg1', + type=str, + metavar='param_values_path', + help='Local path to save the parameter values file to.') + parser_getparams.add_argument( + 'arg2', + nargs='?', + type=str, + metavar='param_defaults_path', + help='Optional local path to save the parameter defaults file to.') + parser_getparams.add_argument( + '-s', '--sort', + choices=['none', 'missionplanner', 'mavproxy'], + default='missionplanner', + help='Sort the parameters in the file. Defaults to %(default)s.', + ) + parser_getparams.add_argument( + '-dtc', '--add_datatype_comments', + action='store_true', + default=False, + help='Add parameter datatype type comments to the outputted parameter files. Defaults to %(default)s.', + ) + parser_getparams.add_argument( + '-t', '--add_timestamp_comment', + action='store_true', + default=False, + help='Add timestamp comment at the top of the file. Defaults to %(default)s.', + ) + + # Put command + parser_put = subparsers.add_parser( + 'put', + help='Put a file to the remote flight controller.') + parser_put.add_argument( + 'arg1', + type=str, + metavar='local_path', + help='Local path to the file to upload to the flight controller.') + parser_put.add_argument( + 'arg2', + nargs='?', + type=str, + metavar='remote_path', + help='Optional remote path where the file should be uploaded on the remote flight controller.') + + # List command + parser_list = subparsers.add_parser( + 'list', + help='List files in a directory on the remote flight controller.') + parser_list.add_argument( + 'arg1', + nargs='?', + type=str, + metavar='remote_path', + help='Optional path to list files from.') + + # Mkdir command + parser_mkdir = subparsers.add_parser( + 'mkdir', + help='Create a directory on the remote flight controller.') + parser_mkdir.add_argument( + 'arg1', + type=str, + metavar='remote_path', + help='Path to the directory to create.') + + # Rmdir command + parser_rmdir = subparsers.add_parser( + 'rmdir', + help='Remove a directory on the remote flight controller.') + parser_rmdir.add_argument( + 'arg1', + type=str, + metavar='remote_path', + help='Path to the directory to remove.') + + # Rm command + parser_rm = subparsers.add_parser( + 'rm', + help='Remove a file on the remote flight controller.') + parser_rm.add_argument( + 'arg1', + type=str, + metavar='remote_path', + help='Path to the file to remove.') + + # Rename command + parser_rename = subparsers.add_parser( + 'rename', + help='Rename a file or directory on the remote flight controller.') + parser_rename.add_argument( + 'arg1', + type=str, + metavar='old_remote_path', + help='Current path of the file/directory.') + parser_rename.add_argument( + 'new_remote_path', + type=str, + metavar='arg2', + help='New path for the file/directory.') + + # CRC command + parser_crc = subparsers.add_parser( + 'crc', + help='Calculate the CRC of a file on the remote flight controller.') + parser_crc.add_argument( + 'arg1', + type=str, + metavar='remote_path', + help='Path to the file to calculate the CRC of.') + + # Add other subparsers commands as needed + return parser.parse_args() + + + def auto_detect_serial(): + preferred_ports = [ + '*FTDI*', + "*3D*", + "*USB_to_UART*", + '*Ardu*', + '*PX4*', + '*Hex_*', + '*Holybro_*', + '*mRo*', + '*FMU*', + '*Swift-Flyer*', + '*Serial*', + '*CubePilot*', + '*Qiotek*', + ] + serial_list = mavutil.auto_detect_serial(preferred_list=preferred_ports) + serial_list.sort(key=lambda x: x.device) + + # remove OTG2 ports for dual CDC + if len(serial_list) == 2 and serial_list[0].device.startswith("/dev/serial/by-id"): + if serial_list[0].device[:-1] == serial_list[1].device[0:-1]: + serial_list.pop(1) + + return serial_list + + + def auto_connect(device): + comport = None + if device: + comport = mavutil.SerialPort(device=device, description=device) + else: + autodetect_serial = auto_detect_serial() + if autodetect_serial: + # Resolve the soft link if it's a Linux system + if os.name == 'posix': + try: + dev = autodetect_serial[0].device + logging.debug("Auto-detected device %s", dev) + # Get the directory part of the soft link + softlink_dir = os.path.dirname(dev) + # Resolve the soft link and join it with the directory part + resolved_path = os.path.abspath(os.path.join(softlink_dir, os.readlink(dev))) + autodetect_serial[0].device = resolved_path + logging.debug("Resolved soft link %s to %s", dev, resolved_path) + except OSError: + pass # Not a soft link, proceed with the original device path + comport = autodetect_serial[0] + else: + logging.error("No serial ports found. Please connect a flight controller and try again.") + sys.exit(1) + return comport + + + def wait_heartbeat(m): + '''wait for a heartbeat so we know the target system IDs''' + logging.info("Waiting for flight controller heartbeat") + m.wait_heartbeat(timeout=5) + logging.info("Heartbeat from system %u, component %u", m.target_system, m.target_system) + + + def main(): + '''for testing/example purposes only''' + args = argument_parser() + + logging.basicConfig(level=logging.getLevelName(args.loglevel), format='%(levelname)s - %(message)s') + + # create a mavlink serial instance + comport = auto_connect(args.device) + master = mavutil.mavlink_connection(comport.device, baud=args.baudrate, source_system=args.source_system) + + # wait for the heartbeat msg to find the system ID + wait_heartbeat(master) + + ftp_settings = MAVFTPSettings( + [('debug', int, args.debug), + ('pkt_loss_tx', int, args.pkt_loss_tx), + ('pkt_loss_rx', int, args.pkt_loss_rx), + ('max_backlog', int, args.max_backlog), + ('burst_read_size', int, args.burst_read_size), + ('write_size', int, args.write_size), + ('write_qsize', int, args.write_qsize), + ('idle_detection_time', float, args.idle_detection_time), + ('read_retry_time', float, args.read_retry_time), + ('retry_time', float, args.retry_time)]) + + mav_ftp = MAVFTP(master, + target_system=master.target_system, + target_component=master.target_component, + settings=ftp_settings) + + cmd_ftp_args = [args.command] + if 'arg1' in args and args.arg1: + cmd_ftp_args.append(args.arg1) + if 'arg2' in args and args.arg2: + cmd_ftp_args.append(args.arg2) + + ret = mav_ftp.cmd_ftp(cmd_ftp_args) + + if args.command in ['get', 'put', 'getparams']: + ret = mav_ftp.process_ftp_reply(args.command, timeout=500) + + if isinstance(ret, str): + logging.error("Command returned: %s, but it should return a MAVFTPReturn instead", ret) + elif isinstance(ret, MAVFTPReturn): + if ret.error_code: + ret.display_message() + elif ret is None: + logging.error("Command returned: None, but it should return a MAVFTPReturn instead") + else: + logging.error("Command returned: something strange, but it should return a MAVFTPReturn instead") + + master.close() + -def init(mpstate): - '''initialise module''' - return FTPModule(mpstate) + main() diff --git a/tests/test_mavftp.py b/tests/test_mavftp.py new file mode 100644 index 000000000..f7ab77d9a --- /dev/null +++ b/tests/test_mavftp.py @@ -0,0 +1,249 @@ +#!/usr/bin/env python3 + +''' +MAVLink File Transfer Protocol support test - https://mavlink.io/en/services/ftp.html + +SPDX-FileCopyrightText: 2024 Amilcar Lucas + +SPDX-License-Identifier: GPL-3.0-or-later +''' + +import unittest +#from unittest.mock import patch +from io import StringIO +import logging +from pymavlink import mavutil +from pymavlink.mavftp import FTP_OP, MAVFTP, MAVFTPReturn + +from pymavlink.mavftp import OP_ListDirectory +from pymavlink.mavftp import OP_ReadFile +from pymavlink.mavftp import OP_Ack +from pymavlink.mavftp import OP_Nack +from pymavlink.mavftp import ERR_None +from pymavlink.mavftp import ERR_Fail +from pymavlink.mavftp import ERR_FailErrno +from pymavlink.mavftp import ERR_InvalidDataSize +from pymavlink.mavftp import ERR_InvalidSession +from pymavlink.mavftp import ERR_NoSessionsAvailable +from pymavlink.mavftp import ERR_EndOfFile +from pymavlink.mavftp import ERR_UnknownCommand +from pymavlink.mavftp import ERR_FileExists +from pymavlink.mavftp import ERR_FileProtected +from pymavlink.mavftp import ERR_FileNotFound +#from pymavlink.mavftp import ERR_NoErrorCodeInPayload +#from pymavlink.mavftp import ERR_NoErrorCodeInNack +#from pymavlink.mavftp import ERR_NoFilesystemErrorInPayload +from pymavlink.mavftp import ERR_InvalidErrorCode +#from pymavlink.mavftp import ERR_PayloadTooLarge +#from pymavlink.mavftp import ERR_InvalidOpcode +from pymavlink.mavftp import ERR_InvalidArguments +from pymavlink.mavftp import ERR_PutAlreadyInProgress +from pymavlink.mavftp import ERR_FailToOpenLocalFile +from pymavlink.mavftp import ERR_RemoteReplyTimeout + + +class TestMAVFTPPayloadDecoding(unittest.TestCase): + """Test MAVFTP payload decoding""" + + def setUp(self): + self.log_stream = StringIO() + handler = logging.StreamHandler(self.log_stream) + formatter = logging.Formatter('%(levelname)s: %(message)s') + handler.setFormatter(formatter) + logger = logging.getLogger() + logger.addHandler(handler) + logger.setLevel(logging.DEBUG) + + # Mock mavutil.mavlink_connection to simulate a connection + self.mock_master = mavutil.mavlink_connection(device="udp:localhost:14550", source_system=1) + + # Initialize MAVFTP instance for testing + self.mav_ftp = MAVFTP(self.mock_master, target_system=1, target_component=1) + + def tearDown(self): + self.log_stream.seek(0) + self.log_stream.truncate(0) + + def test_logging(self): + # Code that triggers logging + logging.info("This is a test log message") + + # Flush and get log output + log_output = self.log_stream.getvalue() + + # Assert to check if the expected log is in log_output + self.assertIn("This is a test log message", log_output) + + @staticmethod + def ftp_operation(seq: int, opcode: int, req_opcode: int, payload: bytearray) -> FTP_OP: + return FTP_OP(seq=seq, session=1, opcode=opcode, size=0, req_opcode=req_opcode, burst_complete=0, offset=0, + payload=payload) + + def test_decode_ftp_ack_and_nack(self): + # Test cases grouped by expected outcome + # pylint: disable=line-too-long + test_cases = [ + { + "name": "Successful Operation", + "op": self.ftp_operation(seq=1, opcode=OP_Ack, req_opcode=OP_ListDirectory, payload=None), + "expected_message": "ListDirectory succeeded" + }, + { + "name": "Generic Failure", + "op": self.ftp_operation(seq=2, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_Fail])), + "expected_message": "ListDirectory failed, generic error" + }, + { + "name": "System Error", + "op": self.ftp_operation(seq=3, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_FailErrno, 1])), # System error 1 + "expected_message": "ListDirectory failed, system error 1" + }, + { + "name": "Invalid Data Size", + "op": self.ftp_operation(seq=4, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_InvalidDataSize])), + "expected_message": "ListDirectory failed, invalid data size" + }, + { + "name": "Invalid Session", + "op": self.ftp_operation(seq=5, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_InvalidSession])), + "expected_message": "ListDirectory failed, session is not currently open" + }, + { + "name": "No Sessions Available", + "op": self.ftp_operation(seq=6, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_NoSessionsAvailable])), + "expected_message": "ListDirectory failed, no sessions available" + }, + { + "name": "End of File", + "op": self.ftp_operation(seq=7, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_EndOfFile])), + "expected_message": "ListDirectory failed, offset past end of file" + }, + { + "name": "Unknown Command", + "op": self.ftp_operation(seq=8, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_UnknownCommand])), + "expected_message": "ListDirectory failed, unknown command" + }, + { + "name": "File Exists", + "op": self.ftp_operation(seq=9, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_FileExists])), + "expected_message": "ListDirectory failed, file/directory already exists" + }, + { + "name": "File Protected", + "op": self.ftp_operation(seq=10, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_FileProtected])), + "expected_message": "ListDirectory failed, file/directory is protected" + }, + { + "name": "File Not Found", + "op": self.ftp_operation(seq=11, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_FileNotFound])), + "expected_message": "ListDirectory failed, file/directory not found" + }, + { + "name": "No Error Code in Payload", + "op": self.ftp_operation(seq=12, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=None), + "expected_message": "ListDirectory failed, payload contains no error code" + }, + { + "name": "No Error Code in Nack", + "op": self.ftp_operation(seq=13, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_None])), + "expected_message": "ListDirectory failed, no error code" + }, + { + "name": "No Filesystem Error in Payload", + "op": self.ftp_operation(seq=14, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_FailErrno])), + "expected_message": "ListDirectory failed, file-system error missing in payload" + }, + { + "name": "Invalid Error Code", + "op": self.ftp_operation(seq=15, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_InvalidErrorCode])), + "expected_message": "ListDirectory failed, invalid error code" + }, + { + "name": "Payload Too Large", + "op": self.ftp_operation(seq=16, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([0, 0, 0])), + "expected_message": "ListDirectory failed, payload is too long" + }, + { + "name": "Invalid Opcode", + "op": self.ftp_operation(seq=17, opcode=126, req_opcode=OP_ListDirectory, payload=None), + "expected_message": "ListDirectory failed, invalid opcode 126" + }, + { + "name": "Unknown Opcode in Request", + "op": self.ftp_operation(seq=19, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_UnknownCommand])), # Assuming 100 is an unknown opcode + "expected_message": "ListDirectory failed, unknown command" + }, + { + "name": "Payload with System Error", + "op": self.ftp_operation(seq=20, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([ERR_FailErrno, 2])), # System error 2 + "expected_message": "ListDirectory failed, system error 2" + }, + { + "name": "Invalid Error Code in Payload", + "op": self.ftp_operation(seq=21, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([105])), # Assuming 105 is an invalid error code + "expected_message": "ListDirectory failed, invalid error code 105" + }, + { + "name": "Invalid Opcode with Payload", + "op": self.ftp_operation(seq=23, opcode=126, req_opcode=OP_ReadFile, payload=bytes([1, 1])), # Invalid opcode with payload + "expected_message": "ReadFile failed, invalid opcode 126" + }, + # Add more test cases as needed... + ] + # pylint: enable=line-too-long + + for case in test_cases: + ret = self.mav_ftp._MAVFTP__decode_ftp_ack_and_nack(case['op']) # pylint: disable=protected-access + ret.display_message() + log_output = self.log_stream.getvalue().strip() + self.assertIn(case["expected_message"], log_output, + f"Test {case['name']}: Expected {case['expected_message']} but got {log_output}") + self.log_stream.seek(0) + self.log_stream.truncate(0) + + # Invalid Arguments + ret = MAVFTPReturn("Command arguments", ERR_InvalidArguments) + ret.display_message() + log_output = self.log_stream.getvalue().strip() + self.assertIn("Command arguments failed, invalid arguments", log_output, "Expected invalid arguments message") + self.log_stream.seek(0) + self.log_stream.truncate(0) + + # Test for unknown error code in display_message + op = self.ftp_operation(seq=22, opcode=OP_Nack, req_opcode=OP_ListDirectory, payload=bytes([255])) + ret = self.mav_ftp._MAVFTP__decode_ftp_ack_and_nack(op, "ListDirectory") # pylint: disable=protected-access + ret.error_code = 125 # Set error code to 125 to trigger unknown error message + ret.display_message() + log_output = self.log_stream.getvalue().strip() + self.assertIn("ListDirectory failed, unknown error 125 in display_message()", log_output, + "Expected unknown error message for unknown error code") + self.log_stream.seek(0) + self.log_stream.truncate(0) + + # Put already in progress + ret = MAVFTPReturn("Put", ERR_PutAlreadyInProgress) + ret.display_message() + log_output = self.log_stream.getvalue().strip() + self.assertIn("Put failed, put already in progress", log_output, "Expected put already in progress message") + self.log_stream.seek(0) + self.log_stream.truncate(0) + + # Fail to open local file + ret = MAVFTPReturn("Put", ERR_FailToOpenLocalFile) + ret.display_message() + log_output = self.log_stream.getvalue().strip() + self.assertIn("Put failed, failed to open local file", log_output, "Expected fail to open local file message") + self.log_stream.seek(0) + self.log_stream.truncate(0) + + # Remote Reply Timeout + ret = MAVFTPReturn("Put", ERR_RemoteReplyTimeout) + ret.display_message() + log_output = self.log_stream.getvalue().strip() + self.assertIn("Put failed, remote reply timeout", log_output, "Expected remote reply timeout message") + self.log_stream.seek(0) + self.log_stream.truncate(0) + + +if __name__ == '__main__': + unittest.main()