MPU6KSearch.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 search a set of log files for signs of inconsistent IMU data
5 '''
6 from __future__ import print_function
7 from builtins import input
8 from builtins import range
9 
10 import sys, time, os
11 import zipfile
12 
13 from pymavlink import mavutil
14 from math import degrees
15 
16 # extra imports for pyinstaller
17 import json
18 from pymavlink.dialects.v10 import ardupilotmega
19 
20 search_dirs = ['c:\Program Files\APM Planner',
21  'c:\Program Files\Mission Planner',
22  'c:\Program Files (x86)\APM Planner',
23  'c:\Program Files (x86)\Mission Planner']
24 results = 'SearchResults.zip'
25 email = 'Craig Elder <craig@3drobotics.com>'
26 
27 def IMUCheckFail(filename):
28  try:
29  mlog = mavutil.mavlink_connection(filename)
30  except Exception:
31  return False
32 
33  accel1 = None
34  accel2 = None
35  gyro1 = None
36  gyro2 = None
37  t1 = 0
38  t2 = 0
39  ecount_accel = [0]*3
40  ecount_gyro = [0]*3
41  athreshold = 3.0
42  gthreshold = 30.0
43  count_threshold = 100
44  imu1_count = 0
45  imu2_count = 0
46 
47  while True:
48  try:
49  m = mlog.recv_match(type=['RAW_IMU','SCALED_IMU2','IMU','IMU2','PARM','PARAM_VALUE'])
50  except Exception as e:
51  print('Error: %s' % e)
52  break
53  if m is None:
54  break
55  mtype = m.get_type()
56  gotimu2 = False
57  if mtype == 'RAW_IMU':
58  accel1 = [m.xacc*9.81*0.001, m.yacc*9.81*0.001, m.zacc*9.81*0.001]
59  gyro1 = [degrees(m.xgyro*0.001), degrees(m.ygyro*0.001), degrees(m.zgyro*0.001)]
60  t1 = m.time_usec/1000
61  imu1_count += 1
62  elif mtype == 'SCALED_IMU2':
63  accel2 = [m.xacc*9.81*0.001, m.yacc*9.81*0.001, m.zacc*9.81*0.001]
64  gyro2 = [degrees(m.xgyro*0.001), degrees(m.ygyro*0.001), degrees(m.zgyro*0.001)]
65  gotimu2 = True
66  t2 = m.time_boot_ms
67  imu2_count += 1
68  elif mtype == 'IMU':
69  accel1 = [m.AccX, m.AccY, m.AccZ]
70  gyro1 = [m.GyrX, m.GyrY, m.GyrZ]
71  t1 = m.TimeMS
72  imu1_count += 1
73  elif mtype == 'IMU2':
74  accel2 = [m.AccX, m.AccY, m.AccZ]
75  gyro2 = [m.GyrX, m.GyrY, m.GyrZ]
76  gotimu2 = True
77  t2 = m.TimeMS
78  imu2_count += 1
79  elif mtype == 'PARM':
80  if m.Name.startswith('INS_ACCOFFS_') or m.Name.startswith('INS_ACC2OFFS_'):
81  if m.Value == 0.0:
82  print('UNCALIBRATED: %s' % m)
83  return False
84  elif mtype == 'PARAM_VALUE':
85  if m.param_id.startswith('INS_ACCOFFS_') or m.param_id.startswith('INS_ACC2OFFS_'):
86  if m.param_value == 0.0:
87  print('UNCALIBRATED: %s' % m)
88  return False
89 
90  # skip out early if we don't have two IMUs
91  if imu1_count > imu2_count + 100:
92  return False
93 
94  if accel1 is not None and accel2 is not None and gotimu2 and t2 >= t1:
95  for i in range(3):
96  adiff = accel1[i] - accel2[i]
97  if adiff > athreshold:
98  if ecount_accel[i] < 0:
99  ecount_accel[i] = 0
100  else:
101  ecount_accel[i] += 1
102  elif adiff < -athreshold:
103  if ecount_accel[i] > 0:
104  ecount_accel[i] = 0
105  else:
106  ecount_accel[i] -= 1
107  else:
108  ecount_accel[i] = 0
109  gdiff = gyro1[i] - gyro2[i]
110  if gdiff > gthreshold:
111  if ecount_gyro[i] < 0:
112  ecount_gyro[i] = 0
113  else:
114  ecount_gyro[i] += 1
115  elif gdiff < -gthreshold:
116  if ecount_gyro[i] > 0:
117  ecount_gyro[i] = 0
118  else:
119  ecount_gyro[i] -= 1
120  else:
121  ecount_gyro[i] = 0
122  if abs(ecount_accel[i]) > count_threshold:
123  print("acceldiff[%u] %.1f" % (i, adiff))
124  print(m)
125  return True
126  if abs(ecount_gyro[i]) > count_threshold:
127  print("gyrodiff[i] %.1f" % (i, adiff))
128  print(m)
129  return True
130 
131  return False
132 
133 found = []
134 directories = sys.argv[1:]
135 if not directories:
136  directories = search_dirs
137 
138 filelist = []
139 
140 extensions = ['.tlog','.bin']
141 
143  '''see if the path matches a extension'''
144  (root,ext) = os.path.splitext(f)
145  return ext.lower() in extensions
146 
147 for d in directories:
148  if not os.path.exists(d):
149  continue
150  if os.path.isdir(d):
151  print("Searching in %s" % d)
152  for (root, dirs, files) in os.walk(d):
153  for f in files:
154  if not match_extension(f):
155  continue
156  path = os.path.join(root, f)
157  filelist.append(path)
158  elif match_extension(d):
159  filelist.append(d)
160 
161 for i in range(len(filelist)):
162  f = filelist[i]
163  print("Checking %s ... [found=%u i=%u/%u]" % (f, len(found), i, len(filelist)))
164  try:
165  if IMUCheckFail(f):
166  found.append(f)
167  except Exception as e:
168  print("Failed - %s" % e)
169  continue
170  sys.stdout.flush()
171 
172 if len(found) == 0:
173  print("No matching files found - all OK!")
174  input('Press enter to close')
175  sys.exit(0)
176 
177 print("Creating zip file %s" % results)
178 try:
179  zip = zipfile.ZipFile(results, 'w')
180 except Exception:
181  print("Unable to create zip file %s" % results)
182  print("Please send matching files manually")
183  for f in found:
184  print('MATCHED: %s' % f)
185  input('Press enter to close')
186  sys.exit(1)
187 
188 for f in found:
189  arcname=os.path.basename(f)
190  if not arcname.startswith('201'):
191  mtime = os.path.getmtime(f)
192  arcname = "%s-%s" % (time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(mtime)), arcname)
193  zip.write(f, arcname=arcname)
194 zip.close()
195 
196 print('==============================================')
197 print("Created %s with %u of %u matching logs" % (results, len(found), len(filelist)))
198 print("Please send this file to %s" % email)
199 print('==============================================')
200 
201 input('Press enter to close')
202 sys.exit(0)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:07