GeezerVison is showing github.com — junkcode/python/find_aliasing.py at master · tridge/junkcode

Expand file tree

find_aliasing.py

Copy path

More file actions

Latest commit

History

executable file

107 lines (91 loc) · 2.98 KB

find_aliasing.py

Copy path

File metadata and controls

executable file

107 lines (91 loc) · 2.98 KB

Raw

Copy raw file

Download raw file

Open symbols panel

Edit and raw actions

100

101

102

103

104

105

106

107

#!/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)