108 lines
3.0 KiB
Python
108 lines
3.0 KiB
Python
#!/usr/bin/env python
|
|
'''
|
|
find signs of aliasing on IMU3
|
|
'''
|
|
|
|
from pymavlink import mavutil
|
|
from pymavlink import mavextra
|
|
import math, sys, multiprocessing, random, time
|
|
|
|
from argparse import ArgumentParser
|
|
parser = ArgumentParser(description=__doc__)
|
|
|
|
parser.add_argument("infile", default=None, nargs='+', help="input file")
|
|
parser.add_argument("--parallel", default=4, type=int)
|
|
parser.add_argument("--threshold", type=float, default=3.0)
|
|
args = parser.parse_args()
|
|
|
|
def process_log(fname):
|
|
'''process one log'''
|
|
mlog = mavutil.mavlink_connection(fname)
|
|
|
|
count_imu1 = 0
|
|
count_imu3 = 0
|
|
err_count = {}
|
|
lp1 = {}
|
|
lp3 = {}
|
|
last_err = {}
|
|
max_err = 0.0
|
|
max_err_axis = None
|
|
|
|
axes = ['AccX', 'AccY', 'AccZ']
|
|
for a in axes:
|
|
err_count[a] = 0
|
|
last_err[a] = 0.0
|
|
|
|
alpha = 0.9
|
|
msg_types = set(['IMU', 'IMU3'])
|
|
|
|
while True:
|
|
m = mlog.recv_match(type=msg_types)
|
|
if m is None:
|
|
break
|
|
|
|
mtype = m.get_type()
|
|
if mtype == 'IMU':
|
|
if count_imu1 == 0:
|
|
for a in axes:
|
|
lp1[a] = getattr(m,a)
|
|
for a in axes:
|
|
lp1[a] = alpha * lp1[a] + (1.0-alpha) * getattr(m,a)
|
|
count_imu1 += 1
|
|
|
|
if mtype == 'IMU3':
|
|
if count_imu3 == 0:
|
|
for a in axes:
|
|
lp3[a] = getattr(m,a)
|
|
for a in axes:
|
|
lp3[a] = alpha * lp3[a] + (1.0-alpha) * getattr(m,a)
|
|
count_imu3 += 1
|
|
|
|
if count_imu1 > 100 and count_imu3 > 100:
|
|
for a in axes:
|
|
err = lp1[a] - lp3[a]
|
|
# look for errors above threshold where it is not changing sign
|
|
if abs(err) > args.threshold and err * last_err[a] > 0 and abs(lp1[a]) < 60.0:
|
|
err_count[a] += 1
|
|
else:
|
|
err_count[a] = 0
|
|
last_err[a] = err
|
|
if err_count[a] > 30:
|
|
if abs(err) > max_err:
|
|
max_err = abs(err)
|
|
max_err_axis = a
|
|
|
|
if count_imu1 > 100 and count_imu3 == 0:
|
|
# we don't have both IMUs logged
|
|
break
|
|
|
|
if max_err_axis is not None:
|
|
print("%s aliasing on %s %.1f" % (fname, max_err_axis, max_err))
|
|
try:
|
|
open(fname + ".aliasing", "w").write("aliasing on %s %.1f : %s\n" % (max_err_axis, max_err, fname))
|
|
except Exception:
|
|
pass
|
|
|
|
files = args.infile
|
|
nfiles = len(args.infile)
|
|
|
|
procs = []
|
|
done = set()
|
|
|
|
while len(done) < nfiles:
|
|
r = random.randint(0, nfiles-1)
|
|
fname = files[r]
|
|
if fname in done:
|
|
continue
|
|
done.add(fname)
|
|
print("Processing %s (%u/%u)" % (fname, len(done), nfiles))
|
|
p = multiprocessing.Process(target=process_log, args=(fname,))
|
|
p.start()
|
|
procs.append(p)
|
|
while len(procs) >= args.parallel:
|
|
for p in procs:
|
|
if p.exitcode is not None:
|
|
p.join()
|
|
procs.remove(p)
|
|
time.sleep(0.1)
|