00001
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 speed = float(self.playback.get())
00170 timestamp = getattr(msg, '_timestamp')
00171
00172 now = time.strftime("%H:%M:%S", time.localtime(timestamp))
00173 self.clock.configure(text=now)
00174
00175 if speed == 0.0:
00176 self.root.after(200, self.next_message)
00177 else:
00178 self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message)
00179 self.last_timestamp = timestamp
00180
00181 while True:
00182 self.msg = self.mlog.recv_match(condition=args.condition)
00183 if self.msg is None and self.mlog.f.tell() > self.filesize - 10:
00184 self.paused = True
00185 return
00186 if self.msg is not None and self.msg.get_type() != "BAD_DATA":
00187 break
00188
00189 pos = float(self.mlog.f.tell()) / self.filesize
00190 self.slider.set(pos)
00191 self.filepos = self.slider.get()
00192
00193 if msg.get_type() != "BAD_DATA":
00194 for m in self.mout:
00195 m.write(msg.get_msgbuf())
00196
00197 if msg.get_type() == "GPS_RAW":
00198 self.fdm.set('latitude', msg.lat, units='degrees')
00199 self.fdm.set('longitude', msg.lon, units='degrees')
00200 if args.gpsalt:
00201 self.fdm.set('altitude', msg.alt, units='meters')
00202
00203 if msg.get_type() == "GPS_RAW_INT":
00204 self.fdm.set('latitude', msg.lat/1.0e7, units='degrees')
00205 self.fdm.set('longitude', msg.lon/1.0e7, units='degrees')
00206 if args.gpsalt:
00207 self.fdm.set('altitude', msg.alt/1.0e3, units='meters')
00208
00209 if msg.get_type() == "VFR_HUD":
00210 if not args.gpsalt:
00211 self.fdm.set('altitude', msg.alt, units='meters')
00212 self.fdm.set('num_engines', 1)
00213 self.fdm.set('vcas', msg.airspeed, units='mps')
00214
00215 if msg.get_type() == "ATTITUDE":
00216 self.fdm.set('phi', msg.roll, units='radians')
00217 self.fdm.set('theta', msg.pitch, units='radians')
00218 self.fdm.set('psi', msg.yaw, units='radians')
00219 self.fdm.set('phidot', msg.rollspeed, units='rps')
00220 self.fdm.set('thetadot', msg.pitchspeed, units='rps')
00221 self.fdm.set('psidot', msg.yawspeed, units='rps')
00222
00223 if msg.get_type() == "RC_CHANNELS_SCALED":
00224 self.fdm.set("right_aileron", msg.chan1_scaled*0.0001)
00225 self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001)
00226 self.fdm.set("rudder", msg.chan4_scaled*0.0001)
00227 self.fdm.set("elevator", msg.chan2_scaled*0.0001)
00228 self.fdm.set('rpm', msg.chan3_scaled*0.01)
00229
00230 if msg.get_type() == 'STATUSTEXT':
00231 print("APM: %s" % msg.text)
00232
00233 if msg.get_type() == 'SYS_STATUS':
00234 self.flightmode.configure(text=self.mlog.flightmode)
00235
00236 if msg.get_type() == "BAD_DATA":
00237 if mavutil.all_printable(msg.data):
00238 sys.stdout.write(msg.data)
00239 sys.stdout.flush()
00240
00241 if self.fdm.get('latitude') != 0:
00242 for f in self.fgout:
00243 f.write(self.fdm.pack())
00244
00245
00246 app=App(filename)