mavplayback.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 '''
4 play back a mavlink log as a FlightGear FG NET stream, and as a
5 realtime mavlink stream
6 
7 Useful for visualising flights
8 '''
9 from __future__ import print_function
10 from future import standard_library
11 standard_library.install_aliases()
12 
13 from builtins import object
14 
15 import os
16 import sys
17 import time
18 import tkinter
19 
20 from pymavlink import fgFDM
21 
22 from argparse import ArgumentParser
23 parser = ArgumentParser(description=__doc__)
24 
25 parser.add_argument("--planner", action='store_true', help="use planner file format")
26 parser.add_argument("--condition", default=None, help="select packets by condition")
27 parser.add_argument("--gpsalt", action='store_true', default=False, help="Use GPS altitude")
28 parser.add_argument("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
29 parser.add_argument("--out", help="MAVLink output port (IP:port)",
30  action='append', default=['127.0.0.1:14550'])
31 parser.add_argument("--fgout", action='append', default=['127.0.0.1:5503'],
32  help="flightgear FDM NET output (IP:port)")
33 parser.add_argument("--baudrate", type=int, default=57600, help='baud rate')
34 parser.add_argument("log", metavar="LOG")
35 args = parser.parse_args()
36 
37 if args.mav10:
38  os.environ['MAVLINK10'] = '1'
39 from pymavlink import mavutil
40 
41 filename = args.log
42 
43 
44 def LoadImage(filename):
45  '''return an image from the images/ directory'''
46  app_dir = os.path.dirname(os.path.realpath(__file__))
47  path = os.path.join(app_dir, 'images', filename)
48  return tkinter.PhotoImage(file=path)
49 
50 
51 class App(object):
52  def __init__(self, filename):
53  self.root = tkinter.Tk()
54 
55  self.filesize = os.path.getsize(filename)
56  self.filepos = 0.0
57 
58  self.mlog = mavutil.mavlink_connection(filename, planner_format=args.planner,
59  robust_parsing=True)
60  self.mout = []
61  for m in args.out:
62  self.mout.append(mavutil.mavlink_connection(m, input=False, baud=args.baudrate))
63 
64  self.fgout = []
65  for f in args.fgout:
66  self.fgout.append(mavutil.mavudp(f, input=False))
67 
68  self.fdm = fgFDM.fgFDM()
69 
70  self.msg = self.mlog.recv_match(condition=args.condition)
71  if self.msg is None:
72  sys.exit(1)
73  self.last_timestamp = getattr(self.msg, '_timestamp')
74 
75  self.paused = False
76 
77  self.topframe = tkinter.Frame(self.root)
78  self.topframe.pack(side=tkinter.TOP)
79 
80  self.frame = tkinter.Frame(self.root)
81  self.frame.pack(side=tkinter.LEFT)
82 
83  self.slider = tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01,
84  orient=tkinter.HORIZONTAL, command=self.slew)
85  self.slider.pack(side=tkinter.LEFT)
86 
87  self.clock = tkinter.Label(self.topframe,text="")
88  self.clock.pack(side=tkinter.RIGHT)
89 
90  self.playback = tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3)
91  self.playback.pack(side=tkinter.BOTTOM)
92  self.playback.delete(0, "end")
93  self.playback.insert(0, 1)
94 
95  self.buttons = {}
96  self.button('quit', 'gtk-quit.gif', self.frame.quit)
97  self.button('pause', 'media-playback-pause.gif', self.pause)
98  self.button('rewind', 'media-seek-backward.gif', self.rewind)
99  self.button('forward', 'media-seek-forward.gif', self.forward)
100  self.button('status', 'Status', self.status)
101  self.flightmode = tkinter.Label(self.frame,text="")
102  self.flightmode.pack(side=tkinter.RIGHT)
103 
104  self.next_message()
105  self.root.mainloop()
106 
107  def button(self, name, filename, command):
108  '''add a button'''
109  try:
110  img = LoadImage(filename)
111  b = tkinter.Button(self.frame, image=img, command=command)
112  b.image = img
113  except Exception:
114  b = tkinter.Button(self.frame, text=filename, command=command)
115  b.pack(side=tkinter.LEFT)
116  self.buttons[name] = b
117 
118 
119  def pause(self):
120  '''pause playback'''
121  self.paused = not self.paused
122 
123  def rewind(self):
124  '''rewind 10%'''
125  pos = int(self.mlog.f.tell() - 0.1*self.filesize)
126  if pos < 0:
127  pos = 0
128  self.mlog.f.seek(pos)
129  self.find_message()
130 
131  def forward(self):
132  '''forward 10%'''
133  pos = int(self.mlog.f.tell() + 0.1*self.filesize)
134  if pos > self.filesize:
135  pos = self.filesize - 2048
136  self.mlog.f.seek(pos)
137  self.find_message()
138 
139  def status(self):
140  '''show status'''
141  for m in sorted(self.mlog.messages.keys()):
142  print(str(self.mlog.messages[m]))
143 
144 
145 
146  def find_message(self):
147  '''find the next valid message'''
148  while True:
149  self.msg = self.mlog.recv_match(condition=args.condition)
150  if self.msg is not None and self.msg.get_type() != 'BAD_DATA':
151  break
152  if self.mlog.f.tell() > self.filesize - 10:
153  self.paused = True
154  break
155  self.last_timestamp = getattr(self.msg, '_timestamp')
156 
157  def slew(self, value):
158  '''move to a given position in the file'''
159  if float(value) != self.filepos:
160  pos = float(value) * self.filesize
161  self.mlog.f.seek(int(pos))
162  self.find_message()
163 
164 
165  def next_message(self):
166  '''called as each msg is ready'''
167 
168  msg = self.msg
169  if msg is None:
170  self.paused = True
171 
172  if self.paused:
173  self.root.after(100, self.next_message)
174  return
175 
176  try:
177  speed = float(self.playback.get())
178  except:
179  speed = 0.0
180  timestamp = getattr(msg, '_timestamp')
181 
182  now = time.strftime("%H:%M:%S", time.localtime(timestamp))
183  self.clock.configure(text=now)
184 
185  if speed == 0.0:
186  self.root.after(200, self.next_message)
187  else:
188  self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message)
189  self.last_timestamp = timestamp
190 
191  while True:
192  self.msg = self.mlog.recv_match(condition=args.condition)
193  if self.msg is None and self.mlog.f.tell() > self.filesize - 10:
194  self.paused = True
195  return
196  if self.msg is not None and self.msg.get_type() != "BAD_DATA":
197  break
198 
199  pos = float(self.mlog.f.tell()) / self.filesize
200  self.slider.set(pos)
201  self.filepos = self.slider.get()
202 
203  if msg.get_type() != "BAD_DATA":
204  for m in self.mout:
205  m.write(msg.get_msgbuf())
206 
207  if msg.get_type() == "GPS_RAW":
208  self.fdm.set('latitude', msg.lat, units='degrees')
209  self.fdm.set('longitude', msg.lon, units='degrees')
210  if args.gpsalt:
211  self.fdm.set('altitude', msg.alt, units='meters')
212 
213  if msg.get_type() == "GPS_RAW_INT":
214  self.fdm.set('latitude', msg.lat/1.0e7, units='degrees')
215  self.fdm.set('longitude', msg.lon/1.0e7, units='degrees')
216  if args.gpsalt:
217  self.fdm.set('altitude', msg.alt/1.0e3, units='meters')
218 
219  if msg.get_type() == "VFR_HUD":
220  if not args.gpsalt:
221  self.fdm.set('altitude', msg.alt, units='meters')
222  self.fdm.set('num_engines', 1)
223  self.fdm.set('vcas', msg.airspeed, units='mps')
224 
225  if msg.get_type() == "ATTITUDE":
226  self.fdm.set('phi', msg.roll, units='radians')
227  self.fdm.set('theta', msg.pitch, units='radians')
228  self.fdm.set('psi', msg.yaw, units='radians')
229  self.fdm.set('phidot', msg.rollspeed, units='rps')
230  self.fdm.set('thetadot', msg.pitchspeed, units='rps')
231  self.fdm.set('psidot', msg.yawspeed, units='rps')
232 
233  if msg.get_type() == "RC_CHANNELS_SCALED":
234  self.fdm.set("right_aileron", msg.chan1_scaled*0.0001)
235  self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001)
236  self.fdm.set("rudder", msg.chan4_scaled*0.0001)
237  self.fdm.set("elevator", msg.chan2_scaled*0.0001)
238  self.fdm.set('rpm', msg.chan3_scaled*0.01)
239 
240  if msg.get_type() == 'STATUSTEXT':
241  print("APM: %s" % msg.text)
242 
243  if msg.get_type() == 'SYS_STATUS':
244  self.flightmode.configure(text=self.mlog.flightmode)
245 
246  if msg.get_type() == "BAD_DATA":
247  if mavutil.all_printable(msg.data):
248  sys.stdout.write(msg.data)
249  sys.stdout.flush()
250 
251  if self.fdm.get('latitude') != 0:
252  for f in self.fgout:
253  f.write(self.fdm.pack())
254 
255 
256 app=App(filename)

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