mavgpslag.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 calculate GPS lag from DF log
00005 '''
00006 
00007 import sys, time, os
00008 
00009 from argparse import ArgumentParser
00010 parser = ArgumentParser(description=__doc__)
00011 parser.add_argument("--plot", action='store_true', default=False, help="plot errors")
00012 parser.add_argument("--minspeed", type=float, default=6, help="minimum speed")
00013 parser.add_argument("logs", metavar="LOG", nargs="+")
00014 
00015 args = parser.parse_args()
00016 
00017 from pymavlink import mavutil
00018 from pymavlink.mavextra import *
00019 from pymavlink.rotmat import Vector3, Matrix3
00020 
00021 '''
00022 Support having a $HOME/.pymavlink/mavextra.py for extra graphing functions
00023 '''
00024 home = os.getenv('HOME')
00025 if home is not None:
00026     extra = os.path.join(home, '.pymavlink', 'mavextra.py')
00027     if os.path.exists(extra):
00028         import imp
00029         mavuser = imp.load_source('pymavlink.mavuser', extra)
00030         from pymavlink.mavuser import *
00031 
00032 
00033 def velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, shift=0):
00034     '''return summed velocity error'''
00035     sum = 0
00036     count = 0
00037     for i in range(0, len(vel)-1):
00038         dv = vel[i+1] - vel[i]
00039         da = Vector3()
00040         for idx in range(1+accel_indexes[i]-shift, 1+accel_indexes[i+1]-shift):
00041             da += gaccel[idx]
00042         dt1 = timestamps[i+1] - timestamps[i]
00043         dt2 = (accel_indexes[i+1] - accel_indexes[i]) * imu_dt
00044         da *= imu_dt
00045         da *= dt1/dt2
00046         #print(accel_indexes[i+1] - accel_indexes[i])
00047         ex = abs(dv.x - da.x)
00048         ey = abs(dv.y - da.y)
00049         sum += 0.5*(ex+ey)
00050         count += 1
00051     if count == 0:
00052         return None
00053     return sum/count
00054 
00055 def gps_lag(logfile):
00056     '''work out gps velocity lag times for a log file'''
00057     print("Processing log %s" % filename)
00058     mlog = mavutil.mavlink_connection(filename)
00059 
00060     timestamps = []
00061     vel = []
00062     gaccel = []
00063     accel_indexes = []
00064     ATT = None
00065     IMU = None
00066 
00067     dtsum = 0
00068     dtcount = 0
00069 
00070     while True:
00071         m = mlog.recv_match(type=['GPS','IMU','ATT'])
00072         if m is None:
00073             break
00074         t = m.get_type()
00075         if t == 'GPS' and m.Status==3 and m.Spd>args.minspeed:
00076             v = Vector3(m.Spd*cos(radians(m.GCrs)), m.Spd*sin(radians(m.GCrs)), m.VZ)
00077             vel.append(v)
00078             timestamps.append(m._timestamp)
00079             accel_indexes.append(max(len(gaccel)-1,0))
00080         elif t == 'ATT':
00081             ATT = m
00082         elif t == 'IMU':
00083             if ATT is not None:
00084                 gaccel.append(earth_accel_df(m, ATT))
00085                 if IMU is not None:
00086                     dt = m._timestamp - IMU._timestamp
00087                     dtsum += dt
00088                     dtcount += 1
00089                 IMU = m
00090 
00091     imu_dt = dtsum / dtcount
00092 
00093     print("Loaded %u samples imu_dt=%.3f" % (len(vel), imu_dt))
00094     besti = -1
00095     besterr = 0
00096     delays = []
00097     errors = []
00098     for i in range(0,100):
00099         err = velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, shift=i)
00100         if err is None:
00101             break
00102         errors.append(err)
00103         delays.append(i*imu_dt)
00104         if besti == -1 or err < besterr:
00105             besti = i
00106             besterr = err
00107     print("Best %u (%.3fs) %f" % (besti, besti*imu_dt, besterr))
00108 
00109     if args.plot:
00110         import matplotlib.pyplot as plt
00111         plt.plot(delays, errors, 'bo-')
00112         x1,x2,y1,y2 = plt.axis()
00113         plt.axis((x1,x2,0,y2))
00114         plt.ylabel('Error')
00115         plt.xlabel('Delay(s)')
00116         plt.show()
00117 
00118 
00119 for filename in args.logs:
00120     gps_lag(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57