def mavflightview_mav(mlog, options=None, flightmode_selections=[]):
'''create a map for a log file'''
wp = mavwp.MAVWPLoader()
if options.mission is not None:
wp.load(options.mission)
fen = mavwp.MAVFenceLoader()
if options.fence is not None:
fen.load(options.fence)
all_false = True
for s in flightmode_selections:
if s:
all_false = False
idx = 0
path = [[]]
instances = {}
ekf_counter = 0
nkf_counter = 0
types = ['MISSION_ITEM', 'MISSION_ITEM_INT', 'CMD']
if options.types is not None:
types.extend(options.types.split(','))
else:
types.extend(['POS','GLOBAL_POSITION_INT'])
if options.rawgps or options.dualgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if options.rawgps2 or options.dualgps:
types.extend(['GPS2_RAW','GPS2'])
if options.ekf:
types.extend(['EKF1', 'GPS'])
if options.nkf:
types.extend(['NKF1', 'GPS'])
if options.ahr2:
types.extend(['AHR2', 'AHRS2', 'GPS'])
# handle forms like GPS[0], mapping to GPS for recv_match_types
for i in range(len(types)):
bracket = types[i].find('[')
if bracket != -1:
types[i] = types[i][:bracket]
recv_match_types = types[:]
colour_source = getattr(options, "colour_source")
re_caps = re.compile('[A-Z_][A-Z0-9_]+')
if colour_source is not None:
# stolen from mavgraph.py
caps = set(re.findall(re_caps, colour_source))
recv_match_types.extend(caps)
print("Looking for types %s" % str(recv_match_types))
last_timestamps = {}
used_flightmodes = {}
mlog.rewind()
while True:
try:
m = mlog.recv_match(type=recv_match_types)
if m is None:
break
except Exception:
break
type = m.get_type()
if type in ['MISSION_ITEM', 'MISSION_ITEM_INT']:
try:
new_m = m
if type == 'MISSION_ITEM_INT':
# create a MISSION_ITEM from MISSION_ITEM_INT
new_m = mavutil.mavlink.MAVLink_mission_item_message(
0,
0,
m.seq,
m.frame,
m.command,
m.current,
m.autocontinue,
m.param1,
m.param2,
m.param3,
m.param4,
m.x / 1.0e7,
m.y / 1.0e7,
m.z
)
while new_m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(new_m, wp.count())
wp.set(new_m, m.seq)
except Exception as e:
print("Exception: %s" % str(e))
pass
continue
elif type == 'CMD':
if options.mission is None:
m = mavutil.mavlink.MAVLink_mission_item_message(0,
0,
m.CNum,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId,
0, 1,
m.Prm1, m.Prm2, m.Prm3, m.Prm4,
m.Lat, m.Lng, m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(options.condition):
continue
if options.mode is not None and mlog.flightmode.lower() != options.mode.lower():
continue
if not type in types:
# may only be present for colour-source expressions to work
continue
if type == 'GPS' and hasattr(m,'I'):
type = 'GPS[%u]' % m.I
if not all_false and len(flightmode_selections) > 0 and idx < len(options._flightmodes) and m._timestamp >= options._flightmodes[idx][2]:
idx += 1
elif (idx < len(flightmode_selections) and flightmode_selections[idx]) or all_false or len(flightmode_selections) == 0:
used_flightmodes[mlog.flightmode] = 1
(lat, lng) = (None,None)
if type in ['GPS','GPS2']:
status = getattr(m, 'Status', None)
nsats = getattr(m, 'NSats', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2 and nsats < 5:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif type in ['EKF1', 'ANU1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
ekf_counter += 1
if ekf_counter % options.ekf_sample != 0:
continue
(lat, lng) = pos
elif type in ['NKF1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
nkf_counter += 1
if nkf_counter % options.nkf_sample != 0:
continue
(lat, lng) = pos
elif type in ['ANU5']:
(lat, lng) = (m.Alat*1.0e-7, m.Alng*1.0e-7)
elif type in ['AHR2', 'POS', 'CHEK']:
(lat, lng) = (m.Lat, m.Lng)
elif type == 'AHRS2':
(lat, lng) = (m.lat*1.0e-7, m.lng*1.0e-7)
elif type == 'ORGN':
(lat, lng) = (m.Lat, m.Lng)
elif type == 'SIM':
(lat, lng) = (m.Lat, m.Lng)
elif type == 'GUID':
if (m.Type == 0):
(lat, lng) = (m.pX*1.0e-7, m.pY*1.0e-7)
else:
if hasattr(m,'Lat'):
lat = m.Lat
if hasattr(m,'Lon'):
lng = m.Lon
if hasattr(m,'Lng'):
lng = m.Lng
if hasattr(m,'lat'):
lat = m.lat * 1.0e-7
if hasattr(m,'lon'):
lng = m.lon * 1.0e-7
if hasattr(m,'latitude'):
lat = m.latitude * 1.0e-7
if hasattr(m,'longitude'):
lng = m.longitude * 1.0e-7
if lat is None or lng is None:
continue
# automatically add new types to instances
if type not in instances:
instances[type] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[type]
# only plot thing we have a valid-looking location for:
if abs(lat)<=0.01 and abs(lng)<=0.01:
continue
colour = colour_for_point(mlog, (lat, lng), instance, options)
if colour is None:
continue
tdays = grapher.timestamp_to_days(m._timestamp)
point = (lat, lng, colour, tdays)
if options.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[type] > 1.0/options.rate:
last_timestamps[type] = m._timestamp
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return None
return [path, wp, fen, used_flightmodes, getattr(mlog, 'mav_type',None), instances]
def mavflightview_mav(mlog, options=None, flightmode_selections=[]):
'''create a map for a log file'''
wp = mavwp.MAVWPLoader()
if options.mission is not None:
wp.load(options.mission)
fen = mavwp.MAVFenceLoader()
if options.fence is not None:
fen.load(options.fence)
all_false = True
for s in flightmode_selections:
if s:
all_false = False
idx = 0
path = [[]]
instances = {}
ekf_counter = 0
nkf_counter = 0
types = ['MISSION_ITEM', 'CMD']
if options.types is not None:
types.extend(options.types.split(','))
else:
types.extend(['GPS', 'GLOBAL_POSITION_INT'])
if options.rawgps or options.dualgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if options.rawgps2 or options.dualgps:
types.extend(['GPS2_RAW', 'GPS2'])
if options.ekf:
types.extend(['EKF1', 'GPS'])
if options.nkf:
types.extend(['NKF1', 'GPS'])
if options.ahr2:
types.extend(['AHR2', 'AHRS2', 'GPS'])
print("Looking for types %s" % str(types))
last_timestamps = {}
used_flightmodes = {}
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
type = m.get_type()
if type == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if type == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(options.condition):
continue
if options.mode is not None and mlog.flightmode.lower(
) != options.mode.lower():
continue
if not all_false and len(flightmode_selections) > 0 and idx < len(
options._flightmodes
) and m._timestamp >= options._flightmodes[idx][2]:
idx += 1
elif (idx < len(flightmode_selections) and flightmode_selections[idx]
) or all_false or len(flightmode_selections) == 0:
used_flightmodes[mlog.flightmode] = 1
if type in ['GPS', 'GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif type in ['EKF1', 'ANU1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
ekf_counter += 1
if ekf_counter % options.ekf_sample != 0:
continue
(lat, lng) = pos
elif type in ['NKF1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
nkf_counter += 1
if nkf_counter % options.nkf_sample != 0:
continue
(lat, lng) = pos
elif type in ['ANU5']:
(lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7)
elif type in ['AHR2', 'POS', 'CHEK']:
(lat, lng) = (m.Lat, m.Lng)
elif type == 'AHRS2':
(lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7)
elif type == 'ORGN':
(lat, lng) = (m.Lat, m.Lng)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if type not in instances:
instances[type] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[type]
if abs(lat) > 0.01 or abs(lng) > 0.01:
colour = colour_for_point(mlog, (lat, lng), instance, options)
point = (lat, lng, colour)
if options.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[
type] > 1.0 / options.rate:
last_timestamps[type] = m._timestamp
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return None
return [path, wp, fen, used_flightmodes, getattr(mlog, 'mav_type', None)]
def mavflightview_mav(mlog, options=None, flightmode_selections=[]):
'''create a map for a log file'''
wp = mavwp.MAVWPLoader()
if options.mission is not None:
wp.load(options.mission)
fen = mavwp.MAVFenceLoader()
if options.fence is not None:
fen.load(options.fence)
all_false = True
for s in flightmode_selections:
if s:
all_false = False
idx = 0
path = [[]]
instances = {}
ekf_counter = 0
nkf_counter = 0
types = ['MISSION_ITEM','CMD']
if options.types is not None:
types.extend(options.types.split(','))
else:
types.extend(['GPS','GLOBAL_POSITION_INT'])
if options.rawgps or options.dualgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if options.rawgps2 or options.dualgps:
types.extend(['GPS2_RAW','GPS2'])
if options.ekf:
types.extend(['EKF1', 'GPS'])
if options.nkf:
types.extend(['NKF1', 'GPS'])
if options.ahr2:
types.extend(['AHR2', 'AHRS2', 'GPS'])
print("Looking for types %s" % str(types))
last_timestamps = {}
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
type = m.get_type()
if type == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if type == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(0,
0,
m.CNum,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId,
0, 1,
m.Prm1, m.Prm2, m.Prm3, m.Prm4,
m.Lat, m.Lng, m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(options.condition):
continue
if options.mode is not None and mlog.flightmode.lower() != options.mode.lower():
continue
if not all_false and len(flightmode_selections) > 0 and idx < len(options._flightmodes) and m._timestamp >= options._flightmodes[idx][2]:
idx += 1
elif (idx < len(flightmode_selections) and flightmode_selections[idx]) or all_false or len(flightmode_selections) == 0:
if type in ['GPS','GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif type in ['EKF1', 'ANU1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
ekf_counter += 1
if ekf_counter % options.ekf_sample != 0:
continue
(lat, lng) = pos
elif type in ['NKF1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
nkf_counter += 1
if nkf_counter % options.nkf_sample != 0:
continue
(lat, lng) = pos
elif type in ['ANU5']:
(lat, lng) = (m.Alat*1.0e-7, m.Alng*1.0e-7)
elif type in ['AHR2', 'POS', 'CHEK']:
(lat, lng) = (m.Lat, m.Lng)
elif type == 'AHRS2':
(lat, lng) = (m.lat*1.0e-7, m.lng*1.0e-7)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if type not in instances:
instances[type] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[type]
if abs(lat)>0.01 or abs(lng)>0.01:
colour = colour_for_point(mlog, (lat, lng), instance, options)
point = (lat, lng, colour)
if options.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[type] > 1.0/options.rate:
last_timestamps[type] = m._timestamp
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
return [path, wp, fen]
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
instances = {}
ekf_counter = 0
types = ["MISSION_ITEM", "CMD"]
if opts.types is not None:
types.extend(opts.types.split(","))
else:
types.extend(["GPS", "GLOBAL_POSITION_INT"])
if opts.rawgps or opts.dualgps:
types.extend(["GPS", "GPS_RAW_INT"])
if opts.rawgps2 or opts.dualgps:
types.extend(["GPS2_RAW", "GPS2"])
if opts.ekf:
types.extend(["EKF1", "GPS"])
if opts.ahr2:
types.extend(["AHR2", "AHRS2", "GPS"])
print("Looking for types %s" % str(types))
last_timestamps = {}
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
type = m.get_type()
if type == "MISSION_ITEM":
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if type == "CMD":
m = mavutil.mavlink.MAVLink_mission_item_message(
0,
0,
m.CNum,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId,
0,
1,
m.Prm1,
m.Prm2,
m.Prm3,
m.Prm4,
m.Lat,
m.Lng,
m.Alt,
)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
if type in ["GPS", "GPS2"]:
status = getattr(m, "Status", None)
if status is None:
status = getattr(m, "FixType", None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, "Lng", None)
if lng is None:
lng = getattr(m, "Lon", None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif type in ["EKF1", "ANU1"]:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
ekf_counter += 1
if ekf_counter % opts.ekf_sample != 0:
continue
(lat, lng) = pos
elif type in ["ANU5"]:
(lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7)
elif type in ["AHR2", "POS", "CHEK"]:
(lat, lng) = (m.Lat, m.Lng)
elif type == "AHRS2":
(lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if type not in instances:
instances[type] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[type]
if abs(lat) > 0.01 or abs(lng) > 0.01:
fmode = getattr(mlog, "flightmode", "")
if fmode in colourmap:
colour = colourmap[fmode]
else:
colour = colourmap["UNKNOWN"]
(r, g, b) = colour
(r, g, b) = (r + instance * 80, g + instance * 50, b + instance * 70)
if r > 255:
r = 205
if g > 255:
g = 205
if g < 0:
g = 0
if b > 255:
b = 205
colour = (r, g, b)
point = (lat, lng, colour)
if opts.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[type] > 1.0 / opts.rate:
last_timestamps[type] = m._timestamp
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2], lon + bounds[3])
while (
mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width - 20
or mp_util.gps_distance(lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20
):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(
mp_slipmap.SlipPolygon(
"FlightPath[%u]-%s" % (i, filename), path[i], layer="FlightPath", linewidth=2, colour=(255, 0, 180)
)
)
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(
mp_slipmap.SlipPolygon(
"Mission-%s-%u" % (filename, i), plist[i], layer="Mission", linewidth=2, colour=(255, 255, 255)
)
)
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon(
"Fence-%s" % filename, fen.polygon(), layer="Fence", linewidth=2, colour=(0, 255, 0)
)
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat, lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(
title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=opts.debug,
)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(",")
lat = a[0]
lon = a[1]
icon = "flag.png"
if len(a) > 2:
icon = a[2] + ".png"
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon(
"icon - %s" % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False
)
)
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
types = ['MISSION_ITEM', 'CMD']
if opts.rawgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2:
types.extend(['GPS2_RAW', 'GPS2'])
if opts.dualgps:
types.extend(['GPS2_RAW', 'GPS2', 'GPS_RAW_INT', 'GPS'])
if opts.ekf:
types.extend(['EKF1', 'GPS'])
if opts.ahr2:
types.extend(['AHR2', 'GPS'])
if len(types) == 2:
types.extend(['GPS', 'GLOBAL_POSITION_INT'])
print("Looking for types %s" % str(types))
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
if m.get_type() == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if m.get_type() == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower(
) != opts.mode.lower():
continue
if m.get_type() in ['GPS', 'GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif m.get_type() == 'EKF1':
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
(lat, lng) = pos
elif m.get_type() == 'AHR2':
(lat, lng) = (m.Lat, m.Lng)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
instance = 0
if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']:
instance = 1
if m.get_type() == 'EKF1':
if opts.dualgps:
instance = 2
else:
instance = 1
if m.get_type() == 'AHR2':
if opts.dualgps:
instance = 2
else:
instance = 1
if abs(lat) > 0.01 or abs(lng) > 0.01:
if getattr(mlog, 'flightmode', '') in colourmap:
colour = colourmap[mlog.flightmode]
(r, g, b) = colour
(r, g, b) = (r + instance * 50, g + instance * 50,
b + instance * 50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r, g, b)
point = (lat, lng, colour)
else:
point = (lat, lng)
while instance >= len(path):
path.append([])
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(
mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, filename),
path[i],
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(
mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename, i),
plist[i],
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename,
fen.polygon(),
layer='Fence',
linewidth=2,
colour=(0, 255, 0))
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat, lon), ground_width, path_objs,
mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
def mavflightview_mav(mlog, options=None, title=None):
'''create a map for a log file'''
if not title:
title = 'MAVFlightView'
wp = mavwp.MAVWPLoader()
if options.mission is not None:
wp.load(options.mission)
fen = mavwp.MAVFenceLoader()
if options.fence is not None:
fen.load(options.fence)
path = [[]]
instances = {}
ekf_counter = 0
nkf_counter = 0
types = ['MISSION_ITEM', 'CMD']
if options.types is not None:
types.extend(options.types.split(','))
else:
types.extend(['GPS', 'GLOBAL_POSITION_INT'])
if options.rawgps or options.dualgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if options.rawgps2 or options.dualgps:
types.extend(['GPS2_RAW', 'GPS2'])
if options.ekf:
types.extend(['EKF1', 'GPS'])
if options.nkf:
types.extend(['NKF1', 'GPS'])
if options.ahr2:
types.extend(['AHR2', 'AHRS2', 'GPS'])
print("Looking for types %s" % str(types))
last_timestamps = {}
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
type = m.get_type()
if type == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if type == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(options.condition):
continue
if options.mode is not None and mlog.flightmode.lower(
) != options.mode.lower():
continue
if type in ['GPS', 'GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif type in ['EKF1', 'ANU1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
ekf_counter += 1
if ekf_counter % options.ekf_sample != 0:
continue
(lat, lng) = pos
elif type in ['NKF1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
nkf_counter += 1
if nkf_counter % options.nkf_sample != 0:
continue
(lat, lng) = pos
elif type in ['ANU5']:
(lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7)
elif type in ['AHR2', 'POS', 'CHEK']:
(lat, lng) = (m.Lat, m.Lng)
elif type == 'AHRS2':
(lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if type not in instances:
instances[type] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[type]
if abs(lat) > 0.01 or abs(lng) > 0.01:
colour = colour_for_point(mlog, (lat, lng), instance, options)
point = (lat, lng, colour)
if options.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[
type] > 1.0 / options.rate:
last_timestamps[type] = m._timestamp
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(
mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, title),
path[i],
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(
mp_slipmap.SlipPolygon('Mission-%s-%u' % (title, i),
plist[i],
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title,
fen.polygon(),
layer='Fence',
linewidth=2,
colour=(0, 255, 0))
else:
fence_obj = None
if options.imagefile:
create_imagefile(options, options.imagefile, (lat, lon), ground_width,
path_objs, mission_obj, fence_obj)
else:
global multi_map
if options.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=title,
service=options.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=options.debug)
if options.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in options.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
source = getattr(options, "colour_source", "flightmode")
if source != "flightmode":
print("colour-source: min=%f max=%f" %
(colour_source_min, colour_source_max))
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
types = ['MISSION_ITEM']
if opts.rawgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2:
types.extend(['GPS2_RAW','GPS2'])
if opts.dualgps:
types.extend(['GPS2_RAW','GPS2', 'GPS_RAW_INT', 'GPS'])
if opts.ekf:
types.extend(['EKF1'])
if len(types) == 1:
types.extend(['GPS','GLOBAL_POSITION_INT'])
print("Looking for types %s" % str(types))
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
if m.get_type() == 'MISSION_ITEM':
wp.set(m, m.seq)
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
if m.get_type() in ['GPS','GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif m.get_type() == 'EKF1':
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
(lat, lng) = pos
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
instance = 0
if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']:
instance = 1
if m.get_type() == 'EKF1':
if opts.dualgps:
instance = 2
else:
instance = 1
if lat != 0 or lng != 0:
if getattr(mlog, 'flightmode','') in colourmap:
colour = colourmap[mlog.flightmode]
(r,g,b) = colour
(r,g,b) = (r+instance*50,g+instance*50,b+instance*50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r,g,b)
point = (lat, lng, colour)
else:
point = (lat, lng)
while instance >= len(path):
path.append([])
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,filename), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
mission = wp.polygon()
if len(mission) > 1:
mission_obj = mp_slipmap.SlipPolygon('Mission-%s' % filename, wp.polygon(), layer='Mission',
linewidth=2, colour=(255,255,255))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
map.add_object(mission_obj)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
instances = {}
types = ['MISSION_ITEM','CMD']
if opts.types is not None:
types.extend(opts.types.split(','))
else:
types.extend(['GPS','GLOBAL_POSITION_INT'])
if opts.rawgps or opts.dualgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2 or opts.dualgps:
types.extend(['GPS2_RAW','GPS2'])
if opts.ekf:
types.extend(['EKF1', 'GPS'])
if opts.ahr2:
types.extend(['AHR2', 'AHRS2', 'GPS'])
print("Looking for types %s" % str(types))
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
if m.get_type() == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if m.get_type() == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(0,
0,
m.CNum,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId,
0, 1,
m.Prm1, m.Prm2, m.Prm3, m.Prm4,
m.Lat, m.Lng, m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
if m.get_type() in ['GPS','GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif m.get_type() == 'EKF1':
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
(lat, lng) = pos
elif m.get_type() == 'AHR2':
(lat, lng) = (m.Lat, m.Lng)
elif m.get_type() == 'AHRS2':
(lat, lng) = (m.lat*1.0e-7, m.lng*1.0e-7)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if m.get_type() not in instances:
instances[m.get_type()] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[m.get_type()]
if abs(lat)>0.01 or abs(lng)>0.01:
if getattr(mlog, 'flightmode','') in colourmap:
colour = colourmap[mlog.flightmode]
(r,g,b) = colour
(r,g,b) = (r+instance*50,g+instance*50,b+instance*50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r,g,b)
point = (lat, lng, colour)
else:
point = (lat, lng)
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,filename), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))