diff --git a/tools/mavftpdecode.py b/tools/mavftpdecode.py new file mode 100755 index 0000000000000000000000000000000000000000..385370a3f91a34ce555440d12c39a3cf6535b059 --- /dev/null +++ b/tools/mavftpdecode.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python + +''' +decode FTP file transfers from tlog +''' + + +import time +import struct +import sys + +from argparse import ArgumentParser +parser = ArgumentParser(description=__doc__) +parser.add_argument("logs", metavar="LOG", nargs="+") + +args = parser.parse_args() + +from pymavlink import mavutil + +class Block(object): + def __init__(self, offset, size, data): + self.offset = offset + self.size = size + self.data = data + +class Transfer(object): + def __init__(self, filename): + self.filename = filename + self.blocks = [] + + def extract(self): + self.blocks.sort(key = lambda x: x.offset) + data = bytes() + for b in self.blocks: + if b.offset != len(data): + print("gap at %u" % len(data)) + return None + data += bytes(b.data) + return data + +def param_decode(data): + '''decode param packed data''' + magic = 0x671b + magic2,num_params,total_params = struct.unpack("<HHH", data[0:6]) + if magic != magic2: + print("paramftp: bad magic 0x%x expected 0x%x" % (magic2, magic)) + return + data = data[6:] + + # mapping of data type to type length and format + data_types = { + 1: (1, 'b'), + 2: (2, 'h'), + 3: (4, 'i'), + 4: (4, 'f'), + } + + count = 0 + params = [] + + if sys.version_info.major < 3: + pad_byte = chr(0) + last_name = '' + else: + pad_byte = 0 + last_name = bytes() + ret = [] + + while True: + while len(data) > 0 and data[0] == pad_byte: + # skip pad bytes + data = data[1:] + + if len(data) == 0: + break + + ptype, plen = struct.unpack("<BB", data[0:2]) + flags = (ptype>>4) & 0x0F + ptype &= 0x0F + + if not ptype in data_types: + print("paramftp: bad type 0x%x" % ptype) + return + + (type_len, type_format) = data_types[ptype] + + 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] + last_name = name + data = data[2+name_len+type_len:] + v, = struct.unpack("<" + type_format, vdata) + ret.append((name, v, ptype)) + count += 1 + return ret + + + +# transfers indexed by session ID +ftp_transfers = {} +ftp_block_size = 0 + +FTP_OpenFileRO = 4 +FTP_ReadFile = 5 +FTP_BurstReadFile = 15 +FTP_Ack = 128 +FTP_NAck = 129 + +def ftp_add(m): + session = m.payload[2] + opcode = m.payload[3] + size = m.payload[4] + req_opcode = m.payload[5] + burst_complete = m.payload[6] + data = m.payload[12:12+size] + if opcode == FTP_OpenFileRO: + filename = data + ftp_transfers[session] = Transfer(bytes(filename)) + if req_opcode in [FTP_ReadFile, FTP_BurstReadFile] and opcode == FTP_Ack: + if not session in ftp_transfers: + print("No session %u" % session) + return + offset, = struct.unpack("<I", bytes(m.payload[8:12])) + ftp_transfers[session].blocks.append(Block(offset,size,data)) + +def mavparse(logfile): + '''extract FTP transfers''' + mlog = mavutil.mavlink_connection(filename) + + while True: + try: + m = mlog.recv_match(type=['FILE_TRANSFER_PROTOCOL']) + if m is None: + return + except Exception: + return + if m.get_type() == 'FILE_TRANSFER_PROTOCOL': + ftp_add(m) + +for filename in args.logs: + mavparse(filename) + +for session in ftp_transfers: + f = ftp_transfers[session] + print('# %s' % f.filename.decode()) + if f.filename.decode() == '@PARAM/param.pck': + plist = param_decode(f.extract()) + for p in plist: + print(p[0].decode(), p[1])