mavgpslag.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 calculate GPS lag from DF log
5 
6 The DF log file needs to be generated with ATT, GPS and IMU bits:
7 - On copter (bit0, bit2 and bit18) set on the LOG_BITMASK parameters
8 - On rover (bit0, bit2 and bit7) set on the LOG_BITMASK parameters
9 - On plane (bit0, bit2 and bit7) set on the LOG_BITMASK parameters
10 Make sure IMU_RAW (bit 19) is not set
11 
12 For this to work, the vehicle must move at speeds above the --minspeed parameter (defaults to 6m/s)
13 The code really only works when there is significant acceleration as well.
14 You'll need to fly quite aggressively on a copter to get a result.
15 
16 '''
17 from __future__ import print_function
18 from builtins import range
19 
20 import os
21 
22 from argparse import ArgumentParser
23 parser = ArgumentParser(description=__doc__)
24 parser.add_argument("--plot", action='store_true', default=False, help="plot errors")
25 parser.add_argument("--minspeed", type=float, default=6, help="minimum speed")
26 parser.add_argument("--gps", default='', help="GPS number")
27 parser.add_argument("logs", metavar="LOG", nargs="+")
28 
29 args = parser.parse_args()
30 
31 from pymavlink import mavutil
32 from pymavlink.mavextra import *
33 from pymavlink.rotmat import Vector3
34 
35 '''
36 Support having a $HOME/.pymavlink/mavextra.py for extra graphing functions
37 '''
38 home = os.getenv('HOME')
39 if home is not None:
40  extra = os.path.join(home, '.pymavlink', 'mavextra.py')
41  if os.path.exists(extra):
42  import imp
43  mavuser = imp.load_source('pymavlink.mavuser', extra)
44  from pymavlink.mavuser import *
45 
46 
47 def velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, shift=0):
48  '''return summed velocity error'''
49  sum = 0
50  count = 0
51  for i in range(0, len(vel)-1):
52  dv = vel[i+1] - vel[i]
53  da = Vector3()
54  for idx in range(1+accel_indexes[i]-shift, 1+accel_indexes[i+1]-shift):
55  da += gaccel[idx]
56  dt1 = timestamps[i+1] - timestamps[i]
57  dt2 = (accel_indexes[i+1] - accel_indexes[i]) * imu_dt
58  da *= imu_dt
59  da *= dt1/dt2
60  #print(accel_indexes[i+1] - accel_indexes[i])
61  ex = abs(dv.x - da.x)
62  ey = abs(dv.y - da.y)
63  sum += 0.5*(ex+ey)
64  count += 1
65  if count == 0:
66  return None
67  return sum/count
68 
69 def gps_lag(logfile):
70  '''work out gps velocity lag times for a log file'''
71  print("Processing log %s" % filename)
72  mlog = mavutil.mavlink_connection(filename)
73 
74  timestamps = []
75  vel = []
76  gaccel = []
77  accel_indexes = []
78  ATT = None
79  IMU = None
80  GPS = None
81 
82  dtsum = 0
83  dtcount = 0
84 
85  while True:
86  m = mlog.recv_match(type=['GPS'+args.gps,'IMU','ATT','GPA'+args.gps])
87  if m is None:
88  break
89  t = m.get_type()
90  if t.startswith('GPS') and m.Status >= 3 and m.Spd>args.minspeed:
91  GPS = m;
92  elif t.startswith('GPA') and GPS is not None and GPS.TimeUS == m.TimeUS:
93  v = Vector3(GPS.Spd*cos(radians(GPS.GCrs)), GPS.Spd*sin(radians(GPS.GCrs)), GPS.VZ)
94  vel.append(v)
95  timestamps.append(m.SMS*0.001)
96  accel_indexes.append(max(len(gaccel)-1,0))
97  elif t == 'ATT':
98  ATT = m
99  elif t == 'IMU':
100  if ATT is not None:
101  ga = earth_accel_df(m, ATT)
102  ga.z += 9.80665
103  gaccel.append(ga)
104  if IMU is not None:
105  dt = (m.TimeUS - IMU.TimeUS) * 1.0e-6
106  dtsum += dt
107  dtcount += 1
108  IMU = m
109 
110  imu_dt = dtsum / dtcount
111 
112  print("Loaded %u samples imu_dt=%.3f" % (len(vel), imu_dt))
113  besti = -1
114  besterr = 0
115  delays = []
116  errors = []
117  for i in range(0,100):
118  err = velocity_error(timestamps, vel, gaccel, accel_indexes, imu_dt, shift=i)
119  if err is None:
120  break
121  errors.append(err)
122  delays.append(i*imu_dt)
123  if besti == -1 or err < besterr:
124  besti = i
125  besterr = err
126  print("Best %u (%.3fs) %f" % (besti, besti*imu_dt, besterr))
127 
128  if args.plot:
129  import matplotlib.pyplot as plt
130  plt.plot(delays, errors, 'bo-')
131  x1,x2,y1,y2 = plt.axis()
132  plt.axis((x1,x2,0,y2))
133  plt.ylabel('Error')
134  plt.xlabel('Delay(s)')
135  plt.show()
136 
137 
138 for filename in args.logs:
139  gps_lag(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Apr 7 2019 02:06:02