Go to the documentation of this file.00001
00002
00003 '''
00004 example program to extract GPS data from a mavlink log, and create a GPX
00005 file, for loading into google earth
00006 '''
00007
00008 import sys, struct, time, os
00009
00010 from argparse import ArgumentParser
00011 parser = ArgumentParser(description=__doc__)
00012 parser.add_argument("--condition", default=None, help="select packets by a condition")
00013 parser.add_argument("--nofixcheck", default=False, action='store_true', help="don't check for GPS fix")
00014 parser.add_argument("logs", metavar="LOG", nargs="+")
00015 args = parser.parse_args()
00016
00017 from pymavlink import mavutil
00018
00019
00020 def mav_to_gpx(infilename, outfilename):
00021 '''convert a mavlink log file to a GPX file'''
00022
00023 mlog = mavutil.mavlink_connection(infilename)
00024 outf = open(outfilename, mode='w')
00025
00026 def process_packet(timestamp, lat, lon, alt, hdg, v):
00027 t = time.localtime(timestamp)
00028 outf.write('''<trkpt lat="%s" lon="%s">
00029 <ele>%s</ele>
00030 <time>%s</time>
00031 <course>%s</course>
00032 <speed>%s</speed>
00033 <fix>3d</fix>
00034 </trkpt>
00035 ''' % (lat, lon, alt,
00036 time.strftime("%Y-%m-%dT%H:%M:%SZ", t),
00037 hdg, v))
00038
00039 def add_header():
00040 outf.write('''<?xml version="1.0" encoding="UTF-8"?>
00041 <gpx
00042 version="1.0"
00043 creator="pymavlink"
00044 xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
00045 xmlns="http://www.topografix.com/GPX/1/0"
00046 xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
00047 <trk>
00048 <trkseg>
00049 ''')
00050
00051 def add_footer():
00052 outf.write('''</trkseg>
00053 </trk>
00054 </gpx>
00055 ''')
00056
00057 add_header()
00058
00059 count=0
00060 lat=0
00061 lon=0
00062 fix=0
00063 while True:
00064 m = mlog.recv_match(type=['GPS_RAW', 'GPS_RAW_INT', 'GPS', 'GPS2'], condition=args.condition)
00065 if m is None:
00066 break
00067 if m.get_type() == 'GPS_RAW_INT':
00068 lat = m.lat/1.0e7
00069 lon = m.lon/1.0e7
00070 alt = m.alt/1.0e3
00071 v = m.vel/100.0
00072 hdg = m.cog/100.0
00073 timestamp = m._timestamp
00074 fix = m.fix_type
00075 elif m.get_type() == 'GPS_RAW':
00076 lat = m.lat
00077 lon = m.lon
00078 alt = m.alt
00079 v = m.v
00080 hdg = m.hdg
00081 timestamp = m._timestamp
00082 fix = m.fix_type
00083 elif m.get_type() == 'GPS' or m.get_type() == 'GPS2':
00084 lat = m.Lat
00085 lon = m.Lng
00086 alt = m.Alt
00087 v = m.Spd
00088 hdg = m.GCrs
00089 timestamp = m._timestamp
00090 fix = m.Status
00091 else:
00092 pass
00093
00094 if fix < 2 and not args.nofixcheck:
00095 continue
00096 if lat == 0.0 or lon == 0.0:
00097 continue
00098 process_packet(timestamp, lat, lon, alt, hdg, v)
00099 count += 1
00100 add_footer()
00101 print("Created %s with %u points" % (outfilename, count))
00102
00103
00104 for infilename in args.logs:
00105 outfilename = infilename + '.gpx'
00106 mav_to_gpx(infilename, outfilename)