[Crossfire] Telemetry logs parser
This commit is contained in:
@@ -0,0 +1,115 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
|
||||||
|
# This program parses sport.log files
|
||||||
|
|
||||||
|
from __future__ import division, print_function
|
||||||
|
|
||||||
|
import sys
|
||||||
|
|
||||||
|
lineNumber = 0
|
||||||
|
crossfireDataBuff = []
|
||||||
|
|
||||||
|
def dump(data, maxLen=None):
|
||||||
|
if maxLen and len(data) > maxLen:
|
||||||
|
data = data[:maxLen]
|
||||||
|
return " ".join("{:02x}".format(c) for c in data)
|
||||||
|
|
||||||
|
def CheckCrc(packet):
|
||||||
|
return True
|
||||||
|
|
||||||
|
def ParseGPS(payload):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def ParseBattery(payload):
|
||||||
|
voltage = float((payload[0] << 8) + payload[1]) / 10
|
||||||
|
current = float((payload[2] << 8) + payload[3]) / 10
|
||||||
|
consumption = (payload[4] << 16) + (payload[5] << 8) + payload[6]
|
||||||
|
return "[Battery] %.1fV %.1fA %dmAh" % (voltage, current, consumption)
|
||||||
|
|
||||||
|
def ParseLinkStatistics(payload):
|
||||||
|
return "[Link Statistics] "
|
||||||
|
|
||||||
|
def ParseAttitude(payload):
|
||||||
|
pitch = float((payload[0] << 8) + payload[1]) / 1000
|
||||||
|
roll = float((payload[2] << 8) + payload[3]) / 1000
|
||||||
|
yaw = float((payload[4] << 8) + payload[5]) / 1000
|
||||||
|
return "[Attitude] pitch=%.3f roll=%.3f yaw=%.3f" % (pitch, roll, yaw)
|
||||||
|
|
||||||
|
def ParseFlightMode(payload):
|
||||||
|
return '[Flight Mode] "%s"' % "".join([chr(c) for c in payload[:-1]])
|
||||||
|
|
||||||
|
def ParseDevice(payload):
|
||||||
|
return '[Device] "%s" %d parameters' % ("".join([chr(c) for c in payload[2:-14]]), payload[-1])
|
||||||
|
|
||||||
|
parsers = (
|
||||||
|
(0x02, ParseGPS),
|
||||||
|
(0x08, ParseBattery),
|
||||||
|
(0x14, ParseLinkStatistics),
|
||||||
|
(0x1E, ParseAttitude),
|
||||||
|
(0x21, ParseFlightMode),
|
||||||
|
(0x29, ParseDevice)
|
||||||
|
)
|
||||||
|
|
||||||
|
def ParsePacket(packet):
|
||||||
|
len = packet[1]
|
||||||
|
command = packet[2]
|
||||||
|
payload = packet[3:-1]
|
||||||
|
crc = packet[-1]
|
||||||
|
for id, parser in parsers:
|
||||||
|
if id == command:
|
||||||
|
print("(%d)" % lineNumber, dump(packet), parser(payload))
|
||||||
|
return
|
||||||
|
print("(%d)" % lineNumber, dump(packet))
|
||||||
|
|
||||||
|
def ParseData(data):
|
||||||
|
global crossfireDataBuff
|
||||||
|
# convert from hex
|
||||||
|
parts = data.split(' ')
|
||||||
|
binData = [int(hex, 16) for hex in parts]
|
||||||
|
# print binData
|
||||||
|
crossfireDataBuff += binData
|
||||||
|
# process whole packets
|
||||||
|
while len(crossfireDataBuff) > 4:
|
||||||
|
if crossfireDataBuff[0] != 0x00:
|
||||||
|
print("Skipped 1 byte", dump(crossfireDataBuff[:1]))
|
||||||
|
crossfireDataBuff = crossfireDataBuff[1:]
|
||||||
|
continue
|
||||||
|
length = crossfireDataBuff[1]
|
||||||
|
if length < 2 or length > 100-2:
|
||||||
|
print("Skipped 2 bytes", dump(crossfireDataBuff[:2]))
|
||||||
|
crossfireDataBuff = crossfireDataBuff[2:]
|
||||||
|
continue
|
||||||
|
if len(crossfireDataBuff) < length+2:
|
||||||
|
break
|
||||||
|
ParsePacket(crossfireDataBuff[:length+2])
|
||||||
|
crossfireDataBuff = crossfireDataBuff[length+2:]
|
||||||
|
|
||||||
|
|
||||||
|
inputFile = None
|
||||||
|
|
||||||
|
if len(sys.argv) > 1:
|
||||||
|
inputFile = sys.argv[1]
|
||||||
|
|
||||||
|
# open input
|
||||||
|
if inputFile:
|
||||||
|
inp = open(inputFile, 'r')
|
||||||
|
else:
|
||||||
|
inp = sys.stdin
|
||||||
|
|
||||||
|
while True:
|
||||||
|
line = inp.readline()
|
||||||
|
lineNumber += 1
|
||||||
|
if len(line) == 0:
|
||||||
|
break
|
||||||
|
line = line.strip('\r\n')
|
||||||
|
if len(line) == 0:
|
||||||
|
continue
|
||||||
|
# print line
|
||||||
|
parts = line.split(': ')
|
||||||
|
if len(parts) < 2:
|
||||||
|
print("weird data: \"%s\" at line %d" % (line, lineNumber))
|
||||||
|
continue
|
||||||
|
crossfireData = parts[1].strip()
|
||||||
|
# print "sd: %s" % sportData
|
||||||
|
ParseData(crossfireData)
|
||||||
Reference in New Issue
Block a user