mavplayback.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 play back a mavlink log as a FlightGear FG NET stream, and as a
00005 realtime mavlink stream
00006 
00007 Useful for visualising flights
00008 '''
00009 
00010 import sys, time, os, struct
00011 import Tkinter
00012 
00013 from pymavlink import fgFDM
00014 
00015 from argparse import ArgumentParser
00016 parser = ArgumentParser(description=__doc__)
00017 
00018 parser.add_argument("--planner", action='store_true', help="use planner file format")
00019 parser.add_argument("--condition", default=None, help="select packets by condition")
00020 parser.add_argument("--gpsalt", action='store_true', default=False, help="Use GPS altitude")
00021 parser.add_argument("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
00022 parser.add_argument("--out", help="MAVLink output port (IP:port)",
00023                   action='append', default=['127.0.0.1:14550'])
00024 parser.add_argument("--fgout", action='append', default=['127.0.0.1:5503'],
00025                   help="flightgear FDM NET output (IP:port)")
00026 parser.add_argument("--baudrate", type=int, default=57600, help='baud rate')
00027 parser.add_argument("log", metavar="LOG")
00028 args = parser.parse_args()
00029 
00030 if args.mav10:
00031     os.environ['MAVLINK10'] = '1'
00032 from pymavlink import mavutil
00033 
00034 filename = args.log
00035 
00036 
00037 def LoadImage(filename):
00038     '''return an image from the images/ directory'''
00039     app_dir = os.path.dirname(os.path.realpath(__file__))
00040     path = os.path.join(app_dir, 'images', filename)
00041     return Tkinter.PhotoImage(file=path)
00042 
00043 
00044 class App():
00045     def __init__(self, filename):
00046         self.root = Tkinter.Tk()
00047 
00048         self.filesize = os.path.getsize(filename)
00049         self.filepos = 0.0
00050 
00051         self.mlog = mavutil.mavlink_connection(filename, planner_format=args.planner,
00052                                                robust_parsing=True)
00053         self.mout = []
00054         for m in args.out:
00055             self.mout.append(mavutil.mavlink_connection(m, input=False, baud=args.baudrate))
00056 
00057         self.fgout = []
00058         for f in args.fgout:
00059             self.fgout.append(mavutil.mavudp(f, input=False))
00060 
00061         self.fdm = fgFDM.fgFDM()
00062 
00063         self.msg = self.mlog.recv_match(condition=args.condition)
00064         if self.msg is None:
00065             sys.exit(1)
00066         self.last_timestamp = getattr(self.msg, '_timestamp')
00067 
00068         self.paused = False
00069 
00070         self.topframe = Tkinter.Frame(self.root)
00071         self.topframe.pack(side=Tkinter.TOP)
00072 
00073         self.frame = Tkinter.Frame(self.root)
00074         self.frame.pack(side=Tkinter.LEFT)
00075 
00076         self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01,
00077                                     orient=Tkinter.HORIZONTAL, command=self.slew)
00078         self.slider.pack(side=Tkinter.LEFT)
00079 
00080         self.clock = Tkinter.Label(self.topframe,text="")
00081         self.clock.pack(side=Tkinter.RIGHT)
00082 
00083         self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3)
00084         self.playback.pack(side=Tkinter.BOTTOM)
00085         self.playback.delete(0, "end")
00086         self.playback.insert(0, 1)
00087 
00088         self.buttons = {}
00089         self.button('quit', 'gtk-quit.gif', self.frame.quit)
00090         self.button('pause', 'media-playback-pause.gif', self.pause)
00091         self.button('rewind', 'media-seek-backward.gif', self.rewind)
00092         self.button('forward', 'media-seek-forward.gif', self.forward)
00093         self.button('status', 'Status', self.status)
00094         self.flightmode = Tkinter.Label(self.frame,text="")
00095         self.flightmode.pack(side=Tkinter.RIGHT)
00096 
00097         self.next_message()
00098         self.root.mainloop()
00099 
00100     def button(self, name, filename, command):
00101         '''add a button'''
00102         try:
00103             img = LoadImage(filename)
00104             b = Tkinter.Button(self.frame, image=img, command=command)
00105             b.image = img
00106         except Exception:
00107             b = Tkinter.Button(self.frame, text=filename, command=command)
00108         b.pack(side=Tkinter.LEFT)
00109         self.buttons[name] = b
00110 
00111 
00112     def pause(self):
00113         '''pause playback'''
00114         self.paused = not self.paused
00115 
00116     def rewind(self):
00117         '''rewind 10%'''
00118         pos = int(self.mlog.f.tell() - 0.1*self.filesize)
00119         if pos < 0:
00120             pos = 0
00121         self.mlog.f.seek(pos)
00122         self.find_message()
00123 
00124     def forward(self):
00125         '''forward 10%'''
00126         pos = int(self.mlog.f.tell() + 0.1*self.filesize)
00127         if pos > self.filesize:
00128             pos = self.filesize - 2048
00129         self.mlog.f.seek(pos)
00130         self.find_message()
00131 
00132     def status(self):
00133         '''show status'''
00134         for m in sorted(self.mlog.messages.keys()):
00135             print(str(self.mlog.messages[m]))
00136 
00137 
00138 
00139     def find_message(self):
00140         '''find the next valid message'''
00141         while True:
00142             self.msg = self.mlog.recv_match(condition=args.condition)
00143             if self.msg is not None and self.msg.get_type() != 'BAD_DATA':
00144                 break
00145             if self.mlog.f.tell() > self.filesize - 10:
00146                 self.paused = True
00147                 break
00148         self.last_timestamp = getattr(self.msg, '_timestamp')
00149 
00150     def slew(self, value):
00151         '''move to a given position in the file'''
00152         if float(value) != self.filepos:
00153             pos = float(value) * self.filesize
00154             self.mlog.f.seek(int(pos))
00155             self.find_message()
00156 
00157 
00158     def next_message(self):
00159         '''called as each msg is ready'''
00160 
00161         msg = self.msg
00162         if msg is None:
00163             self.paused = True
00164 
00165         if self.paused:
00166             self.root.after(100, self.next_message)
00167             return
00168 
00169         try:
00170             speed = float(self.playback.get())
00171         except:
00172             speed = 0.0
00173         timestamp = getattr(msg, '_timestamp')
00174 
00175         now = time.strftime("%H:%M:%S", time.localtime(timestamp))
00176         self.clock.configure(text=now)
00177 
00178         if speed == 0.0:
00179             self.root.after(200, self.next_message)
00180         else:
00181             self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message)
00182         self.last_timestamp = timestamp
00183 
00184         while True:
00185             self.msg = self.mlog.recv_match(condition=args.condition)
00186             if self.msg is None and self.mlog.f.tell() > self.filesize - 10:
00187                 self.paused = True
00188                 return
00189             if self.msg is not None and self.msg.get_type() != "BAD_DATA":
00190                 break
00191 
00192         pos = float(self.mlog.f.tell()) / self.filesize
00193         self.slider.set(pos)
00194         self.filepos = self.slider.get()
00195 
00196         if msg.get_type() != "BAD_DATA":
00197             for m in self.mout:
00198                 m.write(msg.get_msgbuf())
00199 
00200         if msg.get_type() == "GPS_RAW":
00201             self.fdm.set('latitude', msg.lat, units='degrees')
00202             self.fdm.set('longitude', msg.lon, units='degrees')
00203             if args.gpsalt:
00204                 self.fdm.set('altitude', msg.alt, units='meters')
00205 
00206         if msg.get_type() == "GPS_RAW_INT":
00207             self.fdm.set('latitude', msg.lat/1.0e7, units='degrees')
00208             self.fdm.set('longitude', msg.lon/1.0e7, units='degrees')
00209             if args.gpsalt:
00210                 self.fdm.set('altitude', msg.alt/1.0e3, units='meters')
00211 
00212         if msg.get_type() == "VFR_HUD":
00213             if not args.gpsalt:
00214                 self.fdm.set('altitude', msg.alt, units='meters')
00215             self.fdm.set('num_engines', 1)
00216             self.fdm.set('vcas', msg.airspeed, units='mps')
00217 
00218         if msg.get_type() == "ATTITUDE":
00219             self.fdm.set('phi', msg.roll, units='radians')
00220             self.fdm.set('theta', msg.pitch, units='radians')
00221             self.fdm.set('psi', msg.yaw, units='radians')
00222             self.fdm.set('phidot', msg.rollspeed, units='rps')
00223             self.fdm.set('thetadot', msg.pitchspeed, units='rps')
00224             self.fdm.set('psidot', msg.yawspeed, units='rps')
00225 
00226         if msg.get_type() == "RC_CHANNELS_SCALED":
00227             self.fdm.set("right_aileron", msg.chan1_scaled*0.0001)
00228             self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001)
00229             self.fdm.set("rudder",        msg.chan4_scaled*0.0001)
00230             self.fdm.set("elevator",      msg.chan2_scaled*0.0001)
00231             self.fdm.set('rpm',           msg.chan3_scaled*0.01)
00232 
00233         if msg.get_type() == 'STATUSTEXT':
00234             print("APM: %s" % msg.text)
00235 
00236         if msg.get_type() == 'SYS_STATUS':
00237             self.flightmode.configure(text=self.mlog.flightmode)
00238 
00239         if msg.get_type() == "BAD_DATA":
00240             if mavutil.all_printable(msg.data):
00241                 sys.stdout.write(msg.data)
00242                 sys.stdout.flush()
00243 
00244         if self.fdm.get('latitude') != 0:
00245             for f in self.fgout:
00246                 f.write(self.fdm.pack())
00247 
00248 
00249 app=App(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43