1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
|
#!/usr/bin/env python
'''
example program to extract GPS data from a waypoint file, and create a GPX
file, for loading into google earth
'''
import sys, struct, time, os
# allow import from the parent directory, where mavlink.py is
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
from optparse import OptionParser
parser = OptionParser("wptogpx.py [options]")
(opts, args) = parser.parse_args()
import mavutil, mavwp
if len(args) < 1:
print("Usage: wptogpx.py <WPFILE>")
sys.exit(1)
def wp_to_gpx(infilename, outfilename):
'''convert a wp file to a GPX file'''
wp = mavwp.MAVWPLoader()
wp.load(infilename)
outf = open(outfilename, mode='w')
def process_wp(w, i):
t = time.localtime(i)
outf.write('''<wpt lat="%s" lon="%s">
<ele>%s</ele>
<cmt>WP %u</cmt>
</wpt>
''' % (w.x, w.y, w.z, i))
def add_header():
outf.write('''<?xml version="1.0" encoding="UTF-8"?>
<gpx
version="1.0"
creator="pymavlink"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xmlns="http://www.topografix.com/GPX/1/0"
xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
''')
def add_footer():
outf.write('''
</gpx>
''')
add_header()
count = 0
for i in range(wp.count()):
w = wp.wp(i)
if w.frame == 3:
w.z += wp.wp(0).z
if w.command == 16:
process_wp(w, i)
count += 1
add_footer()
print("Created %s with %u points" % (outfilename, count))
for infilename in args:
outfilename = infilename + '.gpx'
wp_to_gpx(infilename, outfilename)
|
