66 lines
2.0 KiB
Python
66 lines
2.0 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
|
||
|
|
'''
|
||
|
|
extract ISBH and ISBD messages from AP_Logging files into a bin GYR and ACC log
|
||
|
|
'''
|
||
|
|
from pymavlink import DFReader
|
||
|
|
from argparse import ArgumentParser
|
||
|
|
|
||
|
|
parser = ArgumentParser('Parse bin log into GYR+ACC bin log')
|
||
|
|
parser.add_argument("inlog")
|
||
|
|
parser.add_argument("outlog")
|
||
|
|
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
from pymavlink import mavutil
|
||
|
|
|
||
|
|
class DF_logger:
|
||
|
|
'''write to a DF format log'''
|
||
|
|
def __init__(self, filename):
|
||
|
|
self.outf = open(filename,'wb')
|
||
|
|
self.outf.write(bytes([0]))
|
||
|
|
self.outf.flush()
|
||
|
|
self.mlog = DFReader.DFReader_binary(filename)
|
||
|
|
self.outf.seek(0)
|
||
|
|
self.formats = {}
|
||
|
|
|
||
|
|
def write(self, name, fmt, fields, *args):
|
||
|
|
if not name in self.formats:
|
||
|
|
self.formats[name] = self.mlog.add_format(DFReader.DFFormat(0, name, 0, fmt, fields))
|
||
|
|
self.outf.write(self.mlog.make_format_msgbuf(self.formats[name]))
|
||
|
|
self.outf.write(self.mlog.make_msgbuf(self.formats[name], args))
|
||
|
|
self.outf.flush()
|
||
|
|
|
||
|
|
def extract_batch(inlog, outlog):
|
||
|
|
print("Processing log %s" % inlog)
|
||
|
|
mlog = mavutil.mavlink_connection(inlog)
|
||
|
|
dflog = DF_logger(outlog)
|
||
|
|
|
||
|
|
ISBH = None
|
||
|
|
count = 0
|
||
|
|
|
||
|
|
while True:
|
||
|
|
m = mlog.recv_match(type=['ISBH','ISBD'])
|
||
|
|
if m is None:
|
||
|
|
break
|
||
|
|
msg_type = m.get_type()
|
||
|
|
if msg_type == "ISBH":
|
||
|
|
ISBH = m
|
||
|
|
count += 1
|
||
|
|
elif msg_type == "ISBD" and ISBH is not None:
|
||
|
|
if m.N != ISBH.N:
|
||
|
|
continue
|
||
|
|
stype = 'ACC' if ISBH.type == 0 else 'GYR'
|
||
|
|
stype += '%u' % ISBH.instance
|
||
|
|
sample_us = int(1.0e6 / ISBH.smp_rate)
|
||
|
|
tbase = ISBH.SampleUS + (m.seqno * 32) * sample_us
|
||
|
|
mul = 1.0/ISBH.mul
|
||
|
|
for i in range(32):
|
||
|
|
dflog.write(stype, 'Qfff', 'TimeUS,X,Y,Z',
|
||
|
|
tbase+i*sample_us,
|
||
|
|
m.x[i]*mul, m.y[i]*mul, m.z[i]*mul)
|
||
|
|
|
||
|
|
print("Extracted %u frames" % count)
|
||
|
|
extract_batch(args.inlog, args.outlog)
|
||
|
|
|