MPU6KSearch.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 search a set of log files for signs of inconsistent IMU data
00005 '''
00006 
00007 import sys, time, os, glob
00008 import zipfile
00009 
00010 from pymavlink import mavutil
00011 from math import degrees
00012 
00013 # extra imports for pyinstaller
00014 import json
00015 from pymavlink.dialects.v10 import ardupilotmega
00016 
00017 search_dirs = ['c:\Program Files\APM Planner',
00018                'c:\Program Files\Mission Planner',
00019                'c:\Program Files (x86)\APM Planner',
00020                'c:\Program Files (x86)\Mission Planner']
00021 results = 'SearchResults.zip'
00022 email = 'Craig Elder <craig@3drobotics.com>'
00023 
00024 def IMUCheckFail(filename):
00025     try:
00026         mlog = mavutil.mavlink_connection(filename)
00027     except Exception:
00028         return False
00029 
00030     accel1 = None
00031     accel2 = None
00032     gyro1 = None
00033     gyro2 = None
00034     t1 = 0
00035     t2 = 0
00036     ecount_accel = [0]*3
00037     ecount_gyro = [0]*3
00038     athreshold = 3.0
00039     gthreshold = 30.0
00040     count_threshold = 100
00041     imu1_count = 0
00042     imu2_count = 0
00043     
00044     while True:
00045         try:
00046             m = mlog.recv_match(type=['RAW_IMU','SCALED_IMU2','IMU','IMU2','PARM','PARAM_VALUE'])
00047         except Exception as e:
00048             print('Error: %s' % e)
00049             break
00050         if m is None:
00051             break
00052         mtype = m.get_type()
00053         gotimu2 = False
00054         if mtype == 'RAW_IMU':
00055             accel1 = [m.xacc*9.81*0.001, m.yacc*9.81*0.001, m.zacc*9.81*0.001]
00056             gyro1  = [degrees(m.xgyro*0.001), degrees(m.ygyro*0.001), degrees(m.zgyro*0.001)]
00057             t1 = m.time_usec/1000
00058             imu1_count += 1
00059         elif mtype == 'SCALED_IMU2':
00060             accel2 = [m.xacc*9.81*0.001, m.yacc*9.81*0.001, m.zacc*9.81*0.001]
00061             gyro2  = [degrees(m.xgyro*0.001), degrees(m.ygyro*0.001), degrees(m.zgyro*0.001)]
00062             gotimu2 = True
00063             t2 = m.time_boot_ms
00064             imu2_count += 1
00065         elif mtype == 'IMU':
00066             accel1 = [m.AccX, m.AccY, m.AccZ]
00067             gyro1  = [m.GyrX, m.GyrY, m.GyrZ]
00068             t1 = m.TimeMS
00069             imu1_count += 1
00070         elif mtype == 'IMU2':
00071             accel2 = [m.AccX, m.AccY, m.AccZ]
00072             gyro2  = [m.GyrX, m.GyrY, m.GyrZ]
00073             gotimu2 = True
00074             t2 = m.TimeMS
00075             imu2_count += 1
00076         elif mtype == 'PARM':
00077             if m.Name.startswith('INS_ACCOFFS_') or m.Name.startswith('INS_ACC2OFFS_'):
00078                 if m.Value == 0.0:
00079                     print('UNCALIBRATED: %s' % m)
00080                     return False
00081         elif mtype == 'PARAM_VALUE':
00082             if m.param_id.startswith('INS_ACCOFFS_') or m.param_id.startswith('INS_ACC2OFFS_'):
00083                 if m.param_value == 0.0:
00084                     print('UNCALIBRATED: %s' % m)
00085                     return False
00086 
00087         # skip out early if we don't have two IMUs
00088         if imu1_count > imu2_count + 100:
00089             return False
00090         
00091         if accel1 is not None and accel2 is not None and gotimu2 and t2 >= t1:
00092             for i in range(3):
00093                 adiff = accel1[i] - accel2[i]
00094                 if adiff > athreshold:
00095                     if ecount_accel[i] < 0:
00096                         ecount_accel[i] = 0
00097                     else:
00098                         ecount_accel[i] += 1
00099                 elif adiff < -athreshold:
00100                     if ecount_accel[i] > 0:
00101                         ecount_accel[i] = 0
00102                     else:
00103                         ecount_accel[i] -= 1
00104                 else:
00105                         ecount_accel[i] = 0                    
00106                 gdiff = gyro1[i] - gyro2[i]
00107                 if gdiff > gthreshold:
00108                     if ecount_gyro[i] < 0:
00109                         ecount_gyro[i] = 0
00110                     else:
00111                         ecount_gyro[i] += 1
00112                 elif gdiff < -gthreshold:
00113                     if ecount_gyro[i] > 0:
00114                         ecount_gyro[i] = 0
00115                     else:
00116                         ecount_gyro[i] -= 1
00117                 else:
00118                         ecount_gyro[i] = 0                    
00119                 if abs(ecount_accel[i]) > count_threshold:
00120                     print("acceldiff[%u] %.1f" % (i, adiff))
00121                     print(m)
00122                     return True
00123                 if abs(ecount_gyro[i]) > count_threshold:
00124                     print("gyrodiff[i] %.1f" % (i, adiff))
00125                     print(m)
00126                     return True
00127         
00128     return False
00129 
00130 found = []
00131 directories = sys.argv[1:]
00132 if not directories:
00133     directories = search_dirs
00134     
00135 filelist = []
00136 
00137 extensions = ['.tlog','.bin']
00138 
00139 def match_extension(f):
00140     '''see if the path matches a extension'''
00141     (root,ext) = os.path.splitext(f)
00142     return ext.lower() in extensions
00143 
00144 for d in directories:
00145     if not os.path.exists(d):
00146         continue
00147     if os.path.isdir(d):
00148         print("Searching in %s" % d)
00149         for (root, dirs, files) in os.walk(d):
00150             for f in files:
00151                 if not match_extension(f):
00152                     continue
00153                 path = os.path.join(root, f)
00154                 filelist.append(path)
00155     elif match_extension(d):
00156         filelist.append(d)
00157 
00158 for i in range(len(filelist)):
00159     f = filelist[i]
00160     print("Checking %s ... [found=%u i=%u/%u]" % (f, len(found), i, len(filelist)))
00161     try:
00162         if IMUCheckFail(f):
00163             found.append(f)
00164     except Exception as e:
00165         print("Failed - %s" % e)
00166         continue
00167     sys.stdout.flush()
00168 
00169 if len(found) == 0:
00170     print("No matching files found - all OK!")
00171     raw_input('Press enter to close')
00172     sys.exit(0)
00173 
00174 print("Creating zip file %s" % results)
00175 try:
00176     zip = zipfile.ZipFile(results, 'w')
00177 except Exception:
00178     print("Unable to create zip file %s" % results)
00179     print("Please send matching files manually")
00180     for f in found:
00181         print('MATCHED: %s' % f)
00182     raw_input('Press enter to close')
00183     sys.exit(1)
00184 
00185 for f in found:
00186     arcname=os.path.basename(f)
00187     if not arcname.startswith('201'):
00188         mtime = os.path.getmtime(f)
00189         arcname = "%s-%s" % (time.strftime("%Y-%m-%d-%H-%M-%S", time.localtime(mtime)), arcname)
00190     zip.write(f, arcname=arcname)
00191 zip.close()
00192 
00193 print('==============================================')
00194 print("Created %s with %u of %u matching logs" % (results, len(found), len(filelist)))
00195 print("Please send this file to %s" % email)
00196 print('==============================================')
00197 
00198 raw_input('Press enter to close')
00199 sys.exit(0)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43