radio/util/crossfire-parse.py

   1 #!/usr/bin/env python
   2 # -*- coding: utf-8 -*-
   3
   4 # This program parses sport.log files
   5
   6 from __future__ import division, print_function
   7
   8 import sys, struct
   9
  10 lineNumber = 0
  11 crossfireDataBuff = []
  12
  13 crossfire_types = [
  14     "UINT8",
  15     "INT8",
  16     "UINT16",
  17     "INT16",
  18     "UINT32",
  19     "INT32",
  20     "UINT64",
  21     "INT64",
  22     "FLOAT",
  23     "TEXT_SELECTION",
  24     "STRING",
  25     "FOLDER",
  26     "INFO",
  27     "COMMAND",
  28     "VTX",
  29 ]
  30
  31 def dump(data, maxLen=None):
  32     if maxLen and len(data) > maxLen:
  33         data = data[:maxLen]
  34     return " ".join("{:02x}".format(c) for c in data)
  35
  36
  37 # CRC8 implementation with polynom = x^8+x^7+x^6+x^4+x^2+1 (0xD5)
  38 crc8tab = [
  39     0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54,
  40     0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D,
  41     0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06,
  42     0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F,
  43     0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0,
  44     0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9,
  45     0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2,
  46     0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B,
  47     0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9,
  48     0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0,
  49     0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B,
  50     0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2,
  51     0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D,
  52     0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44,
  53     0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F,
  54     0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16,
  55     0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB,
  56     0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92,
  57     0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9,
  58     0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0,
  59     0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F,
  60     0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36,
  61     0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D,
  62     0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64,
  63     0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26,
  64     0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F,
  65     0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74,
  66     0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D,
  67     0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82,
  68     0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB,
  69     0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0,
  70     0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9
  71 ]
  72
  73 def crc8(buffer):
  74     crc = 0
  75     for c in buffer:
  76         crc = crc8tab[crc ^ c]
  77     return crc
  78
  79 def ParseGPS(payload):
  80     lat, long, speed, head, alt, numsat = struct.unpack('>iiHHHB', bytes(bytearray(payload)))          # bytes(bytearray) casting is required for python 2.7.3 compatibility
  81     return "[GPS] lat:%f long:%f speed:%d heading:%d alt:%d numsat:%d" % (lat / 1e7, long / 1e7, speed / 100, head / 100, alt - 1000, numsat)
  82
  83 def ParseBattery(payload):
  84     voltage = float((payload[0] << 8) + payload[1]) / 10
  85     current = float((payload[2] << 8) + payload[3]) / 10
  86     consumption = (payload[4] << 16) + (payload[5] << 8) + payload[6]
  87     return "[Battery] %.1fV %.1fA %dmAh" % (voltage, current, consumption)
  88
  89 def ParseLinkStatistics(payload):
  90     return "[Link Statistics] "
  91
  92 def ParseAttitude(payload):
  93     pitch = float((payload[0] << 8) + payload[1]) / 1000
  94     roll = float((payload[2] << 8) + payload[3]) / 1000
  95     yaw = float((payload[4] << 8) + payload[5]) / 1000
  96     return "[Attitude] pitch=%.3f roll=%.3f yaw=%.3f" % (pitch, roll, yaw)
  97
  98 def ParseFlightMode(payload):
  99     return '[Flight Mode] "%s"' % "".join([chr(c) for c in payload[:-1]])
 100
 101 def ParsePingDevices(_):
 102     return '[Ping Devices]'
 103
 104 def ParseDevice(payload):
 105     return '[Device] 0x%02x "%s" %d parameters' % (payload[1], "".join([chr(c) for c in payload[2:-14]]), payload[-1])
 106
 107 def ParseFieldsRequest(payload):
 108     return '[Fields request]'
 109
 110 def ParseFieldRequest(payload):
 111     return '[Field request] device=0x%02x field=%d' % (payload[1], payload[2])
 112
 113 def ParseField(payload):
 114     name = ""
 115     i = 6
 116     try:
 117         while payload[i] != 0:
 118             name += chr(payload[i])
 119             i += 1
 120         i += 1
 121         return '[Field] %s device=0x%02x field=%d parent=%d type=%s' % (name, payload[1], payload[2], payload[4], crossfire_types[payload[5] & 0x7f])
 122     except:
 123         return '[Exception]'
 124
 125 parsers = (
 126     (0x02, ParseGPS),
 127     (0x08, ParseBattery),
 128     (0x14, ParseLinkStatistics),
 129     (0x1E, ParseAttitude),
 130     (0x21, ParseFlightMode),
 131     (0x28, ParsePingDevices),
 132     (0x29, ParseDevice),
 133     (0x2a, ParseFieldsRequest),
 134     (0x2b, ParseField),
 135     (0x2c, ParseFieldRequest),
 136 )
 137
 138 def ParsePacket(packet):
 139     length = packet[1]
 140     command = packet[2]
 141     payload = packet[3:-1]
 142     crc = packet[-1]
 143     if crc != crc8(packet[2:-1]):
 144         print("[%s]" % timeData, dump(packet), "[CRC error]")
 145         return
 146     for id, parser in parsers:
 147         if id == command:
 148             print("[%s]" % timeData, dump(packet), parser(payload))
 149             return
 150     print("(%d)" % lineNumber, dump(packet))
 151
 152 def ParseData(data):
 153     global crossfireDataBuff
 154     # convert from hex
 155     parts = data.split(' ')
 156     binData = [int(hex, 16) for hex in parts]
 157     # print binData
 158     crossfireDataBuff += binData
 159     # process whole packets
 160     while len(crossfireDataBuff) > 4:
 161         if crossfireDataBuff[0] != 0x00 and crossfireDataBuff[0] != 0xee and crossfireDataBuff[0] != 0xea:
 162             print("Skipped 1 byte", dump(crossfireDataBuff[:1]))
 163             crossfireDataBuff = crossfireDataBuff[1:]
 164             continue
 165         length = crossfireDataBuff[1]
 166         if length < 2 or length > 0x40:
 167             print("Skipped 1 bytex", dump(crossfireDataBuff[:1]))
 168             crossfireDataBuff = crossfireDataBuff[1:]
 169             continue
 170         if len(crossfireDataBuff) < length+2:
 171             break
 172         ParsePacket(crossfireDataBuff[:length+2])
 173         crossfireDataBuff = crossfireDataBuff[length+2:]
 174
 175 inputFile = None
 176
 177 if len(sys.argv) > 1:
 178     inputFile = sys.argv[1]
 179
 180 # open input
 181 if inputFile:
 182     inp = open(inputFile, 'r')
 183 else:
 184     inp = sys.stdin
 185
 186 while True:
 187     line = inp.readline()
 188     lineNumber += 1
 189     if len(line) == 0:
 190         break
 191     line = line.strip('\r\n')
 192     if len(line) == 0:
 193         continue
 194     # print line
 195     parts = line.split(': ')
 196     if len(parts) < 2:
 197         print("weird data: \"%s\" at line %d" % (line, lineNumber))
 198         continue
 199     timeData = parts[0].strip()
 200     crossfireData = parts[1].strip()
 201     # print "sd: %s" % sportData
 202     ParseData(crossfireData)