wptogpx.py 1.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566
  1. #!/usr/bin/env python
  2. '''
  3. example program to extract GPS data from a waypoint file, and create a GPX
  4. file, for loading into google earth
  5. '''
  6. from __future__ import print_function
  7. from builtins import range
  8. import time
  9. from argparse import ArgumentParser
  10. parser = ArgumentParser(description=__doc__)
  11. parser.add_argument("wpfiles", metavar="WP_FILE", nargs="+")
  12. args = parser.parse_args()
  13. from pymavlink import mavwp
  14. def wp_to_gpx(infilename, outfilename):
  15. '''convert a wp file to a GPX file'''
  16. wp = mavwp.MAVWPLoader()
  17. wp.load(infilename)
  18. outf = open(outfilename, mode='w')
  19. def process_wp(w, i):
  20. t = time.localtime(i)
  21. outf.write('''<wpt lat="%s" lon="%s">
  22. <ele>%s</ele>
  23. <cmt>WP %u</cmt>
  24. </wpt>
  25. ''' % (w.x, w.y, w.z, i))
  26. def add_header():
  27. outf.write('''<?xml version="1.0" encoding="UTF-8"?>
  28. <gpx
  29. version="1.0"
  30. creator="pymavlink"
  31. xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
  32. xmlns="http://www.topografix.com/GPX/1/0"
  33. xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">
  34. ''')
  35. def add_footer():
  36. outf.write('''
  37. </gpx>
  38. ''')
  39. add_header()
  40. count = 0
  41. for i in range(wp.count()):
  42. w = wp.wp(i)
  43. if w.frame == 3:
  44. w.z += wp.wp(0).z
  45. if w.command == 16:
  46. process_wp(w, i)
  47. count += 1
  48. add_footer()
  49. print("Created %s with %u points" % (outfilename, count))
  50. for infilename in args.wpfiles:
  51. outfilename = infilename + '.gpx'
  52. wp_to_gpx(infilename, outfilename)