00001
00002
00003 '''
00004 simple kml export for logfiles
00005 Thomas Gubler <thomasgubler@gmail.com>
00006 '''
00007
00008 from argparse import ArgumentParser
00009 import simplekml
00010 from pymavlink.mavextra import *
00011 from pymavlink import mavutil
00012 import time
00013 import re
00014
00015 mainstate_field = 'STAT.MainState'
00016 position_field_types = [
00017 ['Lon', 'Lat', 'Alt'],
00018 ['Lng', 'Lat', 'Alt']
00019 ]
00020
00021 colors = [simplekml.Color.red, simplekml.Color.green, simplekml.Color.blue,
00022 simplekml.Color.violet, simplekml.Color.yellow, simplekml.Color.orange,
00023 simplekml.Color.burlywood, simplekml.Color.azure, simplekml.Color.lightblue,
00024 simplekml.Color.lawngreen, simplekml.Color.indianred, simplekml.Color.hotpink]
00025
00026 kml = simplekml.Kml()
00027 kml_linestrings = []
00028
00029
00030 def add_to_linestring(position_data, kml_linestring):
00031 '''add a point to the kml file'''
00032 global kml
00033
00034
00035 position_data[2] += float(args.aoff)
00036 kml_linestring.coords.addcoordinates([position_data])
00037
00038
00039 def save_kml(filename):
00040 '''saves the kml file'''
00041 global kml
00042 kml.save(filename)
00043 print("KML written to %s" % filename)
00044
00045
00046 def add_data(t, msg, msg_types, vars, fields, field_types, position_field_type):
00047 '''add some data'''
00048
00049 mtype = msg.get_type()
00050 if mtype not in msg_types:
00051 return
00052
00053 for i in range(0, len(fields)):
00054 if mtype not in field_types[i]:
00055 continue
00056 f = fields[i]
00057 v = mavutil.evaluate_expression(f, vars)
00058 if v is None:
00059 continue
00060
00061
00062 if f == mainstate_field:
00063
00064
00065
00066 if (v != add_data.mainstate_current and
00067 add_data.mainstate_current >= 0):
00068 add_data.new_linestring = True
00069 add_data.mainstate_current = v
00070 else:
00071
00072
00073
00074 add_data.position_data[i] = v
00075
00076
00077 gps_measurement_ready = True
00078 for v in add_data.position_data:
00079 if v is None:
00080 gps_measurement_ready = False
00081 if not gps_measurement_ready:
00082 return
00083
00084
00085
00086
00087
00088 if add_data.new_linestring:
00089 if add_data.current_kml_linestring is not None:
00090 kml_linestrings.append(add_data.current_kml_linestring)
00091
00092 name = "".join([args.source, ":", str(add_data.mainstate_current)])
00093 add_data.current_kml_linestring = \
00094 kml.newlinestring(name=name, altitudemode='absolute')
00095
00096
00097 if args.extrude:
00098 add_data.current_kml_linestring.extrude = 1
00099 add_data.current_kml_linestring.style.linestyle.color = \
00100 colors[max([add_data.mainstate_current, 0])]
00101
00102 add_data.new_linestring = False
00103
00104 add_to_linestring(add_data.position_data,
00105 add_data.current_kml_linestring)
00106 add_data.last_time = msg._timestamp
00107
00108 else:
00109 if (msg._timestamp - add_data.last_time) > 0.1:
00110 add_to_linestring(add_data.position_data,
00111 add_data.current_kml_linestring)
00112 add_data.last_time = msg._timestamp
00113
00114
00115 add_data.position_data = [None for n in position_field_type]
00116
00117
00118 def process_file(filename, source):
00119 '''process one file'''
00120 print("Processing %s" % filename)
00121 mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
00122
00123 position_field_type = sniff_field_spelling(mlog, source)
00124
00125
00126 fields = [args.source + "." + s for s in position_field_type]
00127 fields.append(mainstate_field)
00128 field_types = []
00129
00130 msg_types = set()
00131 re_caps = re.compile('[A-Z_][A-Z0-9_]+')
00132
00133 for f in fields:
00134 caps = set(re.findall(re_caps, f))
00135 msg_types = msg_types.union(caps)
00136 field_types.append(caps)
00137
00138 add_data.new_linestring = True
00139 add_data.mainstate_current = -1
00140 add_data.current_kml_linestring = None
00141 add_data.position_data = [None for n in position_field_type]
00142 add_data.last_time = 0
00143
00144 while True:
00145 msg = mlog.recv_match(args.condition)
00146 if msg is None:
00147 break
00148 tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60)
00149 tdays += 719163
00150 add_data(tdays, msg, msg_types, mlog.messages, fields, field_types, position_field_type)
00151
00152
00153 def sniff_field_spelling(mlog, source):
00154 '''attempt to detect whether APM or PX4 attributes names are in use'''
00155 position_field_type_default = position_field_types[0]
00156
00157 msg = mlog.recv_match(source)
00158 mlog._rewind()
00159
00160 position_field_selection = [spelling for spelling in position_field_types if hasattr(msg, spelling[0])]
00161
00162 return position_field_selection[0] if position_field_selection else position_field_type_default
00163
00164
00165 if __name__ == '__main__':
00166 parser = ArgumentParser(description=__doc__)
00167 parser.add_argument("--no-timestamps", dest="notimestamps",
00168 action='store_true', help="Log doesn't have timestamps")
00169 parser.add_argument("--condition", default=None,
00170 help="select packets by a condition [default: %(default)s]")
00171 parser.add_argument("--aoff", default=0.,
00172 help="Altitude offset for paths that go through the"
00173 "ground in google earth [default: %(default)s]")
00174 parser.add_argument("-o", "--output", dest="filename_out", default="mav.kml",
00175 help="Output filename [default: %(default)s] ")
00176 parser.add_argument("-s", "--source", default="GPOS",
00177 help="Select position data source"
00178 "(GPOS or GPS) [default: %(default)s]")
00179 parser.add_argument("-e", "--extrude", default=False,
00180 action='store_true',
00181 help="Extrude paths to ground [default: %(default)s]")
00182 parser.add_argument("logs", metavar="LOG", nargs="+")
00183
00184 args = parser.parse_args()
00185
00186 filenames = []
00187 for f in args.logs:
00188 if os.path.exists(f):
00189 filenames.append(f)
00190
00191 if len(filenames) == 0:
00192 print("No files to process")
00193 sys.exit(1)
00194
00195 for fi in range(0, len(filenames)):
00196 f = filenames[fi]
00197 process_file(f, args.source)
00198
00199 save_kml(args.filename_out)
00200