Tools/scripts/param_unpack.py

   1 #!/usr/bin/env python3
   2 '''
   3 unpack a param.pck file from @PARAM/param.pck via mavlink FTP
   4 '''
   5
   6 import struct, sys
   7
   8 from argparse import ArgumentParser
   9 parser = ArgumentParser(description=__doc__)
  10 parser.add_argument("file", metavar="LOG")
  11
  12 args = parser.parse_args()
  13
  14 data = open(args.file,'rb').read()
  15 last_name = ""
  16
  17 magic = 0x671b
  18
  19 # header of 6 bytes
  20 magic2,num_params,total_params = struct.unpack("<HHH", data[0:6])
  21 if magic != magic2:
  22     print("Bad magic 0x%x expected 0x%x" % (magic2, magic))
  23     sys.exit(1)
  24
  25 data = data[6:]
  26
  27 # mapping of data type to type length and format
  28 data_types = {
  29     1: (1, 'b'),
  30     2: (2, 'h'),
  31     3: (4, 'i'),
  32     4: (4, 'f'),
  33 }
  34
  35
  36 # we discard leading pad bytes (zero) on all params
  37 # pad bytes are inserted to ensure that a value doesn't
  38 # cross a block boundary
  39 pad_byte = 0
  40 if sys.version_info.major < 3:
  41     pad_byte = chr(0)
  42
  43 count = 0
  44
  45 while True:
  46     # skip pad bytes
  47     while len(data) > 0 and data[0] == pad_byte:
  48         data = data[1:]
  49
  50     if len(data) == 0:
  51         break
  52
  53     ptype, plen = struct.unpack("<BB", data[0:2])
  54     flags = (ptype>>4) & 0x0F
  55     ptype &= 0x0F
  56
  57     if ptype not in data_types:
  58         raise Exception("bad type 0x%x" % ptype)
  59
  60     (type_len, type_format) = data_types[ptype]
  61
  62     name_len = ((plen>>4) & 0x0F) + 1
  63     common_len = (plen & 0x0F)
  64     name = last_name[0:common_len] + data[2:2+name_len].decode('utf-8')
  65     vdata = data[2+name_len:2+name_len+type_len]
  66     last_name = name
  67     data = data[2+name_len+type_len:]
  68     v, = struct.unpack("<" + type_format, vdata)
  69     count += 1
  70     print("%-16s %f" % (name, float(v)))
  71
  72 if count != num_params or count > total_params:
  73     print("Error: Got %u params expected %u/%u" % (count, num_params, total_params))
  74     sys.exit(1)
  75 sys.exit(0)