diff options
Diffstat (limited to 'mavlink/share/pyshared/pymavlink/tools/mavplayback.py')
| -rw-r--r-- | mavlink/share/pyshared/pymavlink/tools/mavplayback.py | 246 |
1 files changed, 246 insertions, 0 deletions
diff --git a/mavlink/share/pyshared/pymavlink/tools/mavplayback.py b/mavlink/share/pyshared/pymavlink/tools/mavplayback.py new file mode 100644 index 000000000..50a6fd45d --- /dev/null +++ b/mavlink/share/pyshared/pymavlink/tools/mavplayback.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python + +''' +play back a mavlink log as a FlightGear FG NET stream, and as a +realtime mavlink stream + +Useful for visualising flights +''' + +import sys, time, os, struct +import Tkinter + +# allow import from the parent directory, where mavlink.py is +sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) + +import fgFDM + +from optparse import OptionParser +parser = OptionParser("mavplayback.py [options]") + +parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") +parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") +parser.add_option("--gpsalt", action='store_true', default=False, help="Use GPS altitude") +parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") +parser.add_option("--out", help="MAVLink output port (IP:port)", + action='append', default=['127.0.0.1:14550']) +parser.add_option("--fgout", action='append', default=['127.0.0.1:5503'], + help="flightgear FDM NET output (IP:port)") +parser.add_option("--baudrate", type='int', default=57600, help='baud rate') +(opts, args) = parser.parse_args() + +if opts.mav10: + os.environ['MAVLINK10'] = '1' +import mavutil + +if len(args) < 1: + parser.print_help() + sys.exit(1) + +filename = args[0] + + +def LoadImage(filename): + '''return an image from the images/ directory''' + app_dir = os.path.dirname(os.path.realpath(__file__)) + path = os.path.join(app_dir, 'images', filename) + return Tkinter.PhotoImage(file=path) + + +class App(): + def __init__(self, filename): + self.root = Tkinter.Tk() + + self.filesize = os.path.getsize(filename) + self.filepos = 0.0 + + self.mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner, + robust_parsing=True) + self.mout = [] + for m in opts.out: + self.mout.append(mavutil.mavlink_connection(m, input=False, baud=opts.baudrate)) + + self.fgout = [] + for f in opts.fgout: + self.fgout.append(mavutil.mavudp(f, input=False)) + + self.fdm = fgFDM.fgFDM() + + self.msg = self.mlog.recv_match(condition=opts.condition) + if self.msg is None: + sys.exit(1) + self.last_timestamp = getattr(self.msg, '_timestamp') + + self.paused = False + + self.topframe = Tkinter.Frame(self.root) + self.topframe.pack(side=Tkinter.TOP) + + self.frame = Tkinter.Frame(self.root) + self.frame.pack(side=Tkinter.LEFT) + + self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01, + orient=Tkinter.HORIZONTAL, command=self.slew) + self.slider.pack(side=Tkinter.LEFT) + + self.clock = Tkinter.Label(self.topframe,text="") + self.clock.pack(side=Tkinter.RIGHT) + + self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3) + self.playback.pack(side=Tkinter.BOTTOM) + self.playback.delete(0, "end") + self.playback.insert(0, 1) + + self.buttons = {} + self.button('quit', 'gtk-quit.gif', self.frame.quit) + self.button('pause', 'media-playback-pause.gif', self.pause) + self.button('rewind', 'media-seek-backward.gif', self.rewind) + self.button('forward', 'media-seek-forward.gif', self.forward) + self.button('status', 'Status', self.status) + self.flightmode = Tkinter.Label(self.frame,text="") + self.flightmode.pack(side=Tkinter.RIGHT) + + self.next_message() + self.root.mainloop() + + def button(self, name, filename, command): + '''add a button''' + try: + img = LoadImage(filename) + b = Tkinter.Button(self.frame, image=img, command=command) + b.image = img + except Exception: + b = Tkinter.Button(self.frame, text=filename, command=command) + b.pack(side=Tkinter.LEFT) + self.buttons[name] = b + + + def pause(self): + '''pause playback''' + self.paused = not self.paused + + def rewind(self): + '''rewind 10%''' + pos = int(self.mlog.f.tell() - 0.1*self.filesize) + if pos < 0: + pos = 0 + self.mlog.f.seek(pos) + self.find_message() + + def forward(self): + '''forward 10%''' + pos = int(self.mlog.f.tell() + 0.1*self.filesize) + if pos > self.filesize: + pos = self.filesize - 2048 + self.mlog.f.seek(pos) + self.find_message() + + def status(self): + '''show status''' + for m in sorted(self.mlog.messages.keys()): + print(str(self.mlog.messages[m])) + + + + def find_message(self): + '''find the next valid message''' + while True: + self.msg = self.mlog.recv_match(condition=opts.condition) + if self.msg is not None and self.msg.get_type() != 'BAD_DATA': + break + if self.mlog.f.tell() > self.filesize - 10: + self.paused = True + break + self.last_timestamp = getattr(self.msg, '_timestamp') + + def slew(self, value): + '''move to a given position in the file''' + if float(value) != self.filepos: + pos = float(value) * self.filesize + self.mlog.f.seek(int(pos)) + self.find_message() + + + def next_message(self): + '''called as each msg is ready''' + + msg = self.msg + if msg is None: + self.paused = True + + if self.paused: + self.root.after(100, self.next_message) + return + + speed = float(self.playback.get()) + timestamp = getattr(msg, '_timestamp') + + now = time.strftime("%H:%M:%S", time.localtime(timestamp)) + self.clock.configure(text=now) + + if speed == 0.0: + self.root.after(200, self.next_message) + else: + self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message) + self.last_timestamp = timestamp + + while True: + self.msg = self.mlog.recv_match(condition=opts.condition) + if self.msg is None and self.mlog.f.tell() > self.filesize - 10: + self.paused = True + return + if self.msg is not None and self.msg.get_type() != "BAD_DATA": + break + + pos = float(self.mlog.f.tell()) / self.filesize + self.slider.set(pos) + self.filepos = self.slider.get() + + if msg.get_type() != "BAD_DATA": + for m in self.mout: + m.write(msg.get_msgbuf().tostring()) + + if msg.get_type() == "GPS_RAW": + self.fdm.set('latitude', msg.lat, units='degrees') + self.fdm.set('longitude', msg.lon, units='degrees') + if opts.gpsalt: + self.fdm.set('altitude', msg.alt, units='meters') + + if msg.get_type() == "VFR_HUD": + if not opts.gpsalt: + self.fdm.set('altitude', msg.alt, units='meters') + self.fdm.set('num_engines', 1) + self.fdm.set('vcas', msg.airspeed, units='mps') + + if msg.get_type() == "ATTITUDE": + self.fdm.set('phi', msg.roll, units='radians') + self.fdm.set('theta', msg.pitch, units='radians') + self.fdm.set('psi', msg.yaw, units='radians') + self.fdm.set('phidot', msg.rollspeed, units='rps') + self.fdm.set('thetadot', msg.pitchspeed, units='rps') + self.fdm.set('psidot', msg.yawspeed, units='rps') + + if msg.get_type() == "RC_CHANNELS_SCALED": + self.fdm.set("right_aileron", msg.chan1_scaled*0.0001) + self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001) + self.fdm.set("rudder", msg.chan4_scaled*0.0001) + self.fdm.set("elevator", msg.chan2_scaled*0.0001) + self.fdm.set('rpm', msg.chan3_scaled*0.01) + + if msg.get_type() == 'STATUSTEXT': + print("APM: %s" % msg.text) + + if msg.get_type() == 'SYS_STATUS': + self.flightmode.configure(text=self.mlog.flightmode) + + if msg.get_type() == "BAD_DATA": + if mavutil.all_printable(msg.data): + sys.stdout.write(msg.data) + sys.stdout.flush() + + if self.fdm.get('latitude') != 0: + for f in self.fgout: + f.write(self.fdm.pack()) + + +app=App(filename) |