Go to the documentation of this file.00001
00002
00003 '''
00004 search a set of log files for bad accel values
00005 '''
00006
00007 import sys, time, os, glob
00008 import zipfile
00009
00010 from pymavlink import mavutil
00011
00012
00013 import json
00014 from pymavlink.dialects.v10 import ardupilotmega
00015
00016 search_dirs = ['c:\Program Files\APM Planner',
00017 'c:\Program Files\Mission Planner',
00018 'c:\Program Files (x86)\APM Planner',
00019 'c:\Program Files (x86)\Mission Planner']
00020 results = 'SearchResults.zip'
00021 email = 'Craig Elder <craig@3drobotics.com>'
00022
00023 from argparse import ArgumentParser
00024 parser = ArgumentParser(description=__doc__)
00025 parser.add_argument("--directory", action='append', default=search_dirs, help="directories to search")
00026 parser.add_argument("--post-boot", action='store_true', help="post boot only")
00027 parser.add_argument("--init-only", action='store_true', help="init only")
00028 parser.add_argument("--single-axis", action='store_true', help="single axis only")
00029
00030 args = parser.parse_args()
00031
00032 logcount = 0
00033
00034 def AccelSearch(filename):
00035 global logcount
00036 mlog = mavutil.mavlink_connection(filename)
00037 badcount = 0
00038 badval = None
00039 have_ok = False
00040 last_t = 0
00041 while True:
00042 m = mlog.recv_match(type=['PARAM_VALUE','RAW_IMU'])
00043 if m is None:
00044 if last_t != 0:
00045 logcount += 1
00046 return False
00047 if m.get_type() == 'PARAM_VALUE':
00048 if m.param_id.startswith('INS_PRODUCT_ID'):
00049 if m.param_value not in [0.0, 5.0]:
00050 return False
00051 if m.get_type() == 'RAW_IMU':
00052 if m.time_usec < last_t:
00053 have_ok = False
00054 last_t = m.time_usec
00055 if abs(m.xacc) >= 3000 and abs(m.yacc) > 3000 and abs(m.zacc) > 3000 and not args.single_axis:
00056 if args.post_boot and not have_ok:
00057 continue
00058 if args.init_only and have_ok:
00059 continue
00060 print(have_ok, last_t, m)
00061 break
00062
00063 for axes in ['xacc', 'yacc', 'zacc']:
00064 value1 = getattr(m, axes)
00065 if abs(value1) > 2000:
00066 if badval is None:
00067 badcount = 1
00068 badval = m
00069 continue
00070 value2 = getattr(badval, axes)
00071 if abs(value1 - value2) < 30:
00072 badcount += 1
00073 badval = m
00074 if badcount > 5:
00075 logcount += 1
00076 if args.init_only and have_ok:
00077 continue
00078 print(have_ok, badcount, badval, m)
00079 return True
00080 else:
00081 badcount = 1
00082 badval = m
00083 if badcount == 0:
00084 have_ok = True
00085 if last_t != 0:
00086 logcount += 1
00087 return True
00088
00089 found = []
00090 directories = args.directory
00091
00092
00093 if len(sys.argv) > 1:
00094 directories = sys.argv[1:]
00095
00096 filelist = []
00097
00098 for d in directories:
00099 if not os.path.exists(d):
00100 continue
00101 if os.path.isdir(d):
00102 print("Searching in %s" % d)
00103 for (root, dirs, files) in os.walk(d):
00104 for f in files:
00105 if not f.endswith('.tlog'):
00106 continue
00107 path = os.path.join(root, f)
00108 filelist.append(path)
00109 elif d.endswith('.tlog'):
00110 filelist.append(d)
00111
00112 for i in range(len(filelist)):
00113 f = filelist[i]
00114 print("Checking %s ... [found=%u logcount=%u i=%u/%u]" % (f, len(found), logcount, i, len(filelist)))
00115 if AccelSearch(f):
00116 found.append(f)
00117
00118
00119 if len(found) == 0:
00120 print("No matching files found - all OK!")
00121 raw_input('Press enter to close')
00122 sys.exit(0)
00123
00124 print("Creating zip file %s" % results)
00125 try:
00126 zip = zipfile.ZipFile(results, 'w')
00127 except Exception:
00128 print("Unable to create zip file %s" % results)
00129 print("Please send matching files manually")
00130 for f in found:
00131 print('MATCHED: %s' % f)
00132 raw_input('Press enter to close')
00133 sys.exit(1)
00134
00135 for f in found:
00136 zip.write(f, arcname=os.path.basename(f))
00137 zip.close()
00138
00139 print('==============================================')
00140 print("Created %s with %u of %u matching logs" % (results, len(found), logcount))
00141 print("Please send this file to %s" % email)
00142 print('==============================================')
00143
00144 raw_input('Press enter to close')
00145 sys.exit(0)