radio/util/sport-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
   9 import struct
  10
  11
  12 START_STOP = '\x7e'
  13 BYTE_STUFF = '\x7d'
  14 STUFF_MASK = '\x20'
  15
  16 DATA_FRAME = 0x10
  17
  18
  19 lineNumber = 0
  20 sportDataBuff = ""
  21
  22 quiet = False
  23
  24
  25 def ParseFlVSS(packet, dataId, prim, appId, data, crc):
  26     # if dataId != 0xa1:
  27     #     return
  28     print("packet: %s (%4d)" % (dump(packet), lineNumber), end=' ')
  29     cells = (data & 0xF0) >> 4
  30     battnumber = data & 0xF
  31     voltage1 = ((data & 0x000FFF00) >> 8) // 5
  32     voltage2 = ((data & 0xFFF00000) >> 20) // 5
  33     print(" FLVSS: no cells: %d, cell: %d: voltages: %0.2f %0.2f" % (cells, battnumber, voltage1 / 100., voltage2 / 100.))
  34
  35
  36 def ParseASS(packet, dataId, prim, appId, data, crc):
  37     print("packet: %s (%4d)" % (dump(packet), lineNumber), end=' ')
  38     print(" ASS: %dkm/h" % (float(data) / 10))
  39
  40
  41 def ParseRSSI(packet, dataId, prim, appId, data, crc):
  42     print("packet: %s (%4d)" % (dump(packet), lineNumber), end=' ')
  43     print(" RSSI: %d" % (data & 0xFF))
  44
  45
  46 def ParseAdc(packet, dataId, prim, appId, data, crc):
  47     print("packet: %s (%4d)" % (dump(packet), lineNumber), end=' ')
  48     print(" A%d: %d" % (dataId - 0xf102, data & 0xFF))
  49
  50
  51 def ParseBatt(packet, dataId, prim, appId, data, crc):
  52     print("packet: %s (%4d)" % (dump(packet), lineNumber), end=' ')
  53     print(" Batt: %d" % (data & 0xFF))
  54
  55
  56 def ParseRAS(packet, dataId, prim, appId, data, crc):
  57     print("packet: %s (%4d)" % (dump(packet), lineNumber), end=' ')
  58     print(" RAS: %d" % (data & 0xFF))
  59
  60
  61 def ParseVersion(packet, dataId, prim, appId, data, crc):
  62     print("packet: %s (%4d)" % (dump(packet), lineNumber), end=' ')
  63     print(" VERSION: %d" % data)
  64
  65
  66 def ParseAirSpeed(packet, dataId, prim, appId, data, crc):
  67     print("packet: %s (%4d)" % (dump(packet), lineNumber), end=' ')
  68     print(" Aspd: %.1f km/h" % (data / 10.0))
  69
  70
  71 appIdParsers = (
  72     (0x0300, 0x030f, ParseFlVSS),
  73     (0x0a00, 0x0a0f, ParseASS),
  74     (0xf101, 0xf101, ParseRSSI),
  75     (0xf102, 0xf103, ParseAdc),
  76     (0xf104, 0xf104, ParseBatt),
  77     (0xf105, 0xf105, ParseRAS),
  78     (0xf106, 0xf106, ParseVersion),
  79 )
  80
  81
  82 def dump(data, maxLen=None):
  83     if maxLen and len(data) > maxLen:
  84         data = data[:maxLen]
  85     return ":".join("{:02x}".format(ord(c)) for c in data)
  86
  87
  88 def CheckSportCrc(packet):
  89     return True
  90
  91
  92 def ParseSportPacket(packet):
  93     global lineNumber
  94     (dataId, prim, appId, data, crc) = struct.unpack('<BBHIB', packet)
  95     # print "dataId:%02x, prim:%02x, appId:%04x, data:%08x, crc:%02x)\n" % (dataId, prim, appId, data, crc)
  96     if prim != DATA_FRAME:
  97         print("unknown prim: %02x for packet %s in line %s" % (prim, dump(packet), lineNumber))
  98     # process according to appId
  99     for firstId, lastId, parser in appIdParsers:
 100         if firstId <= appId <= lastId:
 101             parser(packet, dataId, prim, appId, data, crc)
 102             return
 103     # no parser found
 104     if not quiet:
 105         print("\tdataId:%02x, prim:%02x, appId:%04x, data:%08x, crc:%02x)" % (dataId, prim, appId, data, crc))
 106         print("\tparser for appId %02x not implemented" % appId)
 107
 108
 109 def ParsePacket(packet):
 110     global lineNumber
 111     # unstuff packet
 112     while True:
 113         pos = packet.find(BYTE_STUFF)
 114         if pos == -1:
 115             break
 116         # print "found stuff at %d in %s" % (pos, dump(packet, 20))
 117         # if we have nex char, then unstuff it
 118         if len(packet) > pos + 1:
 119             unstuffed = ord(packet[pos]) ^ ord(STUFF_MASK)
 120             # print "unstuffed data: %02x" % unstuffed
 121             # buffer[pos] = chr(unstuffed)
 122             packet = packet[:pos] + chr(unstuffed) + packet[pos + 2:]
 123             # print "buff after unstuff                 %s" % dump(packet, 20)
 124         else:
 125             # missing data, wait for more data
 126             print("unstuff missing data")
 127             return
 128
 129     # print "packet: %s" % dump(packet, 10)
 130     if len(packet) == 9:
 131         # valid sport packet length
 132         # print "\npacket: %s @%d" % (dump(packet), lineNumber)
 133         # check crc
 134         if not CheckSportCrc(packet):
 135             print("error: wrong CRC for packet %s at line %d" % (dump(packet), lineNumber))
 136         ParseSportPacket(packet)
 137     else:
 138         if len(packet) > 1:
 139             print("warning: wrong length %s for packet %s at line %d" % (len(packet), dump(packet, 10), lineNumber))
 140
 141
 142 def ParseSportData(data):
 143     global sportDataBuff
 144     # convert from hex
 145     parts = data.split(' ')
 146     binData = [chr(int(hex, 16)) for hex in parts]
 147     # print binData
 148     data = ''.join(binData)
 149     sportDataBuff += data
 150     # process whole packets
 151     while True:
 152         # find whole frame
 153         # remove all data before START_STOP
 154         # find next START_STOP
 155         # print "start: %s" % dump(sportDataBuff, 10)
 156         posStop = sportDataBuff.find(START_STOP)
 157         # process pacet between two START_STOPs
 158         if posStop > -1:
 159             # print "found stop at %d" % posStop
 160             ParsePacket(sportDataBuff[:posStop])
 161             # remove processed data
 162             sportDataBuff = sportDataBuff[posStop + 1:]
 163             # print "after: %s" % dump(sportDataBuff, 10)
 164         else:
 165             break
 166
 167
 168 inputFile = None
 169
 170 if len(sys.argv) > 1:
 171     inputFile = sys.argv[1]
 172
 173 # open input
 174 if inputFile:
 175     inp = open(inputFile, 'r')
 176 else:
 177     inp = sys.stdin
 178
 179 while True:
 180     line = inp.readline()
 181     lineNumber += 1
 182     if len(line) == 0:
 183         break
 184     line = line.strip('\r\n')
 185     if len(line) == 0:
 186         continue
 187     # print line
 188     parts = line.split(': ')
 189     if len(parts) < 2:
 190         print("weird data: \"%s\" at line %d" % (line, lineNumber))
 191         continue
 192     sportData = parts[1].strip()
 193     # print "sd: %s" % sportData
 194     ParseSportData(sportData)