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 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)