Go to the documentation of this file.00001
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
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
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)