forked from Archive/PX4-Autopilot
247 lines
8.3 KiB
Python
247 lines
8.3 KiB
Python
#!/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, 'files/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)
|