def __init__(self, mpstate):
super(KmlReadModule, self).__init__(mpstate, "kmlread",
"Add kml or kmz layers to map")
self.add_command(
'kml', self.cmd_param, "kml map handling",
["<clear|snapwp|snapfence>", "<load> (FILENAME)", '<layers>'])
#allayers is all the loaded layers (slimap objects)
#curlayers is only the layers displayed (checked) on the map (text list of layer names)
self.allayers = []
self.curlayers = []
self.alltextlayers = []
self.curtextlayers = []
self.menu_added_map = False
self.menu_needs_refreshing = True
#the fence manager
self.fenceloader = mavwp.MAVFenceLoader()
self.snap_points = []
#make the initial map menu
if mp_util.has_wxpython:
self.menu_fence = MPMenuSubMenu('Set Geofence', items=[])
self.menu = MPMenuSubMenu(
'KML Layers',
items=[MPMenuItem('Clear', 'Clear', '# kml clear')])
def __init__(self, mpstate):
super(FenceModule, self).__init__(mpstate,
"fence",
"geo-fence management",
public=True)
self.fenceloader = mavwp.MAVFenceLoader()
self.last_fence_breach = 0
self.last_fence_status = 0
self.present = False
self.enabled = False
self.healthy = True
self.add_command('fence', self.cmd_fence, "geo-fence management", [
"<draw|list|clear|enable|disable|move|remove>",
"<load|save> (FILENAME)"
])
self.have_list = False
if self.continue_mode and self.logdir != None:
fencetxt = os.path.join(self.logdir, 'fence.txt')
if os.path.exists(fencetxt):
self.fenceloader.load(fencetxt)
self.have_list = True
print("Loaded fence from %s" % fencetxt)
if mp_util.has_wxpython:
self.menu_added_console = False
self.menu_added_mdlink = False
self.menu_added_map = False
self.menu = MPMenuSubMenu(
'Fence',
items=[
MPMenuItem('Clear', 'Clear', '# fence clear'),
MPMenuItem('List', 'List', '# fence list'),
MPMenuItem('Load',
'Load',
'# fence load ',
handler=MPMenuCallFileDialog(flags=wx.FD_OPEN,
title='Fence Load',
wildcard='*.fen')),
MPMenuItem('Save',
'Save',
'# fence save ',
handler=MPMenuCallFileDialog(
flags=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT,
title='Fence Save',
wildcard='*.fen')),
MPMenuItem('Draw', 'Draw', '# fence draw')
])
def __init__(self, master, target_system, target_component, console):
#super(FenceModule, self).__init__(mpstate, "fence", "geo-fence management", public = True)
self.fenceloader = mavwp.MAVFenceLoader()
self.last_fence_breach = 0
self.last_fence_status = 0
self.present = False
self.enabled = False
self.healthy = True
self.master = master
self.target_system = target_system
self.target_component = target_component
self.console = console
#self.add_command('fence', self.cmd_fence,
# "geo-fence management",
# ["<draw|list|clear|enable|disable|move|remove>",
# "<load|save> (FILENAME)"])
self.have_list = False
self.menu_added_console = False
self.menu_added_map = False
def fenceloader(self):
'''fence loader by sysid'''
if not self.target_system in self.fenceloader_by_sysid:
self.fenceloader_by_sysid[
self.target_system] = mavwp.MAVFenceLoader()
return self.fenceloader_by_sysid[self.target_system]
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(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 exportToMAVProxy(self,
loiterInSearchArea=1,
basealt=100,
outname="way.txt"):
'''Exports the airfield home, entry points, search pattern and exit points to MAVProxy'''
#make a fake waypoint loader for testing purposes, if we're not
#running within MAVProxy
if self.MAVpointLoader is None:
print "No loader - creating one"
self.MAVpointLoader = mavwp.MAVWPLoader()
MAVpointLoader = self.MAVpointLoader
entryjump = []
exitjump = []
#clear out all the old waypoints
MAVpointLoader.clear()
TargetSys = MAVpointLoader.target_system
TargetComp = MAVpointLoader.target_component
#WP0 - add landingPt as waypoint 0. This gets replaced when GPS gets lock
w = self.waypoint(*self.landingPt,
alt=basealt,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL)
MAVpointLoader.add(w, comment='Airfield home')
#WP2 - takeoff, then jump to entry lanes
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt,
bearing=self.landingHeading +
175,
distance=300),
alt=40,
cmd=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
param=[12, 0, 0, 0])
MAVpointLoader.add(w, comment="Takeoff")
entryjump.append(MAVpointLoader.count())
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to entry lane')
# landing approach 1
landing_approach_wpnum = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt,
bearing=self.landingHeading +
180,
distance=1000),
alt=80)
MAVpointLoader.add(w, comment='Landing approach')
# drop our speed
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
param=[0, 23, 20, 0])
MAVpointLoader.add(w, comment='Change to 23 m/s')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
param=[1, 23, 20, 0])
MAVpointLoader.add(w, comment='Change throttle to 20%%')
# landing approach 1
landing_approach_wpnum = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt,
bearing=self.landingHeading +
180,
distance=500),
alt=50)
MAVpointLoader.add(w, comment='Landing approach 2')
# landing
w = self.waypoint(self.landingPt[0],
self.landingPt[1],
0,
cmd=mavutil.mavlink.MAV_CMD_NAV_LAND)
MAVpointLoader.add(w, comment='Landing')
# comms Failure. Loiter at EL-4 for 2 minutes then fly to airfield home and loiter
point = self.exitPoints[1]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Comms Failure')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
param=[120, 0, 0, 0])
MAVpointLoader.add(w, comment='loiter 2 minutes')
w = self.waypoint(self.airfieldHome[0], self.airfieldHome[1],
self.airportHeight)
MAVpointLoader.add(w, comment='Airfield home')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)
MAVpointLoader.add(w, comment='loiter')
# GPS failure. Loiter in place for 30s then direct to airfield home
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
param=[30, 0, 0, 0])
MAVpointLoader.add(w, comment='GPS fail - loiter 30 secs')
w = self.waypoint(self.airfieldHome[0], self.airfieldHome[1],
self.airportHeight)
MAVpointLoader.add(w, comment='Airfield home')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)
MAVpointLoader.add(w, comment='loiter')
dropalt = self.joePos[2]
joe = (self.joePos[0], self.joePos[1])
crosswp = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=60,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='cross 1')
w = self.waypoint(*cuav_util.gps_newpos(*joe,
bearing=240,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='cross 2')
w = self.waypoint(*cuav_util.gps_newpos(*joe,
bearing=300,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='cross 3')
w = self.waypoint(*cuav_util.gps_newpos(*joe,
bearing=120,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='cross 4')
w = self.jump(crosswp)
MAVpointLoader.add(w, comment='Jump to cross')
# joe approach point
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=0, distance=350),
alt=dropalt)
MAVpointLoader.add(w, comment='Joe approach')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
param=[0, 20, 50, 0])
MAVpointLoader.add(w, comment='Change to 20 m/s')
# joe location. We use a default acceptance radius of 35 meters. Will be adjusted with 'wp param'
# command
w = self.waypoint(*joe, alt=dropalt, param=[0, 70, 0, 0])
MAVpointLoader.add(w, comment='Joe Location')
w = self.command(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, [8, 1500, 0, 0])
MAVpointLoader.add(w, comment='Drop bottle 1')
w = self.command(mavutil.mavlink.MAV_CMD_CONDITION_DELAY,
[2.0, 0, 0, 0])
MAVpointLoader.add(w, comment='Drop delay')
w = self.command(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, [6, 1050, 0, 0])
MAVpointLoader.add(w, comment='Drop bottle 2')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
param=[0, 28, 50, 0])
MAVpointLoader.add(w, comment='Change to 28 m/s')
w = self.waypoint(*cuav_util.gps_newpos(*joe,
bearing=180,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='Joe after')
w = self.jump(crosswp)
MAVpointLoader.add(w, comment='Jump to cross')
#print "Done AF home"
#WP12 - WPn - and add in the rest of the waypoints - Entry lane, search area, exit lane
entry_wpnum = MAVpointLoader.count()
for i in range(1):
point = self.entryPoints[i]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Entry %u' % (i + 1))
endentry_wpnum = MAVpointLoader.count()
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to search mission')
# exit points
exit_wpnum = MAVpointLoader.count()
for i in range(len(self.exitPoints)):
point = self.exitPoints[i]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Exit point %u' % (i + 1))
w = self.jump(landing_approach_wpnum)
MAVpointLoader.add(w, comment='Jump to landing approach')
# search pattern
MAVpointLoader.wp(endentry_wpnum).param1 = MAVpointLoader.count()
for i in range(len(self.SearchPattern)):
point = self.SearchPattern[i]
w = self.waypoint(point[0], point[1], point[2])
MAVpointLoader.add(w, comment='Search %u' % (i + 1))
#if desired, loiter in the search area for a bit
if loiterInSearchArea == 1:
meanPoint = tuple(numpy.average(self.SearchPattern, axis=0))
w = self.waypoint(meanPoint[0],
meanPoint[1],
meanPoint[2],
cmd=mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
param=[600, 0, 0, 0])
MAVpointLoader.add(w,
comment='Loiter in search area for 10 minutes')
exitjump.append(MAVpointLoader.count())
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to exit lane')
# fixup jump waypoint numbers
for wnum in entryjump:
MAVpointLoader.wp(wnum).param1 = entry_wpnum
for wnum in exitjump:
MAVpointLoader.wp(wnum).param1 = exit_wpnum
#export the waypoints to a MAVLink compatible format/file
waytxt = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'..', 'data', outname)
MAVpointLoader.save(waytxt)
print "Waypoints exported to %s" % waytxt
# create fence.txt
fenceloader = mavwp.MAVFenceLoader()
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1])
fenceloader.add(fp)
for p in gen.missionBounds:
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, float(p[0]), float(p[1]))
fenceloader.add(fp)
# close the polygon
p = gen.missionBounds[0]
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, float(p[0]), float(p[1]))
fenceloader.add(fp)
fencetxt = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'..', 'data', "fence.txt")
fenceloader.save(fencetxt)
print "Fence exported to %s" % fencetxt
def __init__(self):
self.fenceloader = mavwp.MAVFenceLoader()
self.fence_enabled = False
self.last_fence_breach = 0
self.last_fence_status = 0
return
def exportToMAVProxy(self, MAVpointLoader=None, loiterInSearchArea=1):
'''Exports the airfield home, entry points, search pattern and exit points to MAVProxy'''
#make a fake waypoint loader for testing purposes, if we're not
#running within MAVProxy
if MAVpointLoader is None:
print "No loader - creating one"
MAVpointLoader = mavwp.MAVWPLoader()
entryjump = []
exitjump = []
#clear out all the old waypoints
MAVpointLoader.clear()
TargetSys = MAVpointLoader.target_system
TargetComp = MAVpointLoader.target_component
#Check the MAVLink version and handle appropriately
if mavutil.mavlink10():
fn = mavutil.mavlink.MAVLink_mission_item_message
else:
fn = mavutil.mavlink.MAVLink_waypoint_message
# a dummy loiter waypoint
dummyw = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, 0, 0, 0)
#WP0 - add "airfield home" as waypoint 0. This gets replaced when GPS gets lock
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL,
MAV_CMD_NAV_WAYPOINT, 1, 1, 0, 0, 0, 0, self.airfieldHome[0],
self.airfieldHome[1], opts.basealt)
MAVpointLoader.add(w, comment='Airfield home')
# form is fn(target_system=0, target_component=0, seq, frame=0/3, command=16, current=1/0, autocontinue=1, param1=0, param2=0, param3=0, param4=0, x, y, z)
#WP1 - add in a jmp to entry lanes
entryjump.append(MAVpointLoader.count())
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Jump to entry lane')
MAVpointLoader.add(dummyw, 'jump dummy')
#WP2 - takeoff, then jump to entry lanes
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_TAKEOFF, 0, 1, 12, 0, 0, 0, self.takeoffPt[0],
self.takeoffPt[1], self.takeoffPt[2])
MAVpointLoader.add(w, comment="Takeoff")
entryjump.append(MAVpointLoader.count())
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Jump to entry lane')
MAVpointLoader.add(dummyw, 'jump dummy')
# MAVpointLoader.add(dummyw, 'takeoff2')
# MAVpointLoader.add(dummyw, 'takeoff3')
# MAVpointLoader.add(dummyw, 'takeoff4')
# landing approach
landing_approach_wpnum = MAVpointLoader.count()
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.landingApproach[0],
self.landingApproach[1], 80)
MAVpointLoader.add(w, comment='Landing approach')
# drop our speed
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_CHANGE_SPEED, 0, 1, 0, 25, 20, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Change to 25 m/s')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_CHANGE_SPEED, 0, 1, 1, 25, 20, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Change throttle to 20%%')
# landing approach
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0,
self.landingApproach2[0], self.landingApproach2[1], 30)
MAVpointLoader.add(w, comment='Landing approach 2')
# drop our speed again
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_CHANGE_SPEED, 0, 1, 0, 20, 12, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Change to 20 m/s')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_CHANGE_SPEED, 0, 1, 1, 20, 12, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Change throttle to 12%%')
# landing
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_LAND, 0, 1, 0, 0, 0, 0, self.landingPt[0],
self.landingPt[1], 0)
MAVpointLoader.add(w, comment='Landing')
MAVpointLoader.add(dummyw, 'landing dummy')
# comms Failure. Loiter at EL-1 for 2 minutes then fly to airfield home and loiter
point = self.entryPoints[0]
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1],
int(point[2] - self.airportHeight))
MAVpointLoader.add(w, comment='Comms Failure')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_LOITER_TIME, 0, 1, 120, 0, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='loiter 2 minutes')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.airfieldHome[0],
self.airfieldHome[1], 90)
MAVpointLoader.add(w, comment='Airfield home')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_LOITER_UNLIM, 0, 1, 0, 0, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='loiter')
# GPS failure. Loiter in place for 30s then direct to airfield home
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_LOITER_TIME, 0, 1, 30, 0, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='GPS fail - loiter 30 secs')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.airfieldHome[0],
self.airfieldHome[1], 90)
MAVpointLoader.add(w, comment='Airfield home')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_LOITER_UNLIM, 0, 1, 0, 0, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='loiter')
# joe drop approach
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.joeApproach[0],
self.joeApproach[1], int(self.joeApproach[2]))
MAVpointLoader.add(w, comment='Joe approach')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, self.joeDrop[0],
self.joeDrop[1], int(self.joeDrop[2]))
MAVpointLoader.add(w, comment='Joe Drop location')
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_SET_SERVO, 0, 1, 7, 1430, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Drop bottle')
# after drop, jump to exit lane
exitjump.append(MAVpointLoader.count())
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Jump to exit lane')
MAVpointLoader.add(dummyw, 'jump dummy')
#print "Done AF home"
#WP12 - WPn - and add in the rest of the waypoints - Entry lane, search area, exit lane
entry_wpnum = MAVpointLoader.count()
for i in range(1):
point = self.entryPoints[i]
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1],
int(point[2] - self.airportHeight))
MAVpointLoader.add(w, comment='Entry %u' % (i + 1))
endentry_wpnum = MAVpointLoader.count()
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Jump to search mission')
MAVpointLoader.add(dummyw, 'jump dummy')
# exit points
exit_wpnum = MAVpointLoader.count()
for i in range(len(self.exitPoints)):
point = self.exitPoints[i]
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1],
int(point[2] - self.airportHeight))
MAVpointLoader.add(w, comment='Exit point %u' % (i + 1))
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_JUMP, 0, 1, landing_approach_wpnum, -1, 0, 0, 0, 0,
0)
MAVpointLoader.add(w, comment='Jump to landing approach')
MAVpointLoader.add(dummyw, 'jump dummy')
# search pattern
MAVpointLoader.wp(endentry_wpnum).param1 = MAVpointLoader.count()
for i in range(len(self.SearchPattern)):
point = self.SearchPattern[i]
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, point[0], point[1],
int(point[2] - self.airportHeight))
MAVpointLoader.add(w, comment='Search %u' % (i + 1))
#if desired, loiter in the search area for a bit
if loiterInSearchArea == 1:
meanPoint = tuple(numpy.average(self.SearchPattern, axis=0))
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_NAV_LOITER_TIME, 0, 1, 600, 0, 0, 0, meanPoint[0],
meanPoint[1], int(meanPoint[2] - self.airportHeight))
MAVpointLoader.add(w,
comment='Loiter in search area for 10 minutes')
exitjump.append(MAVpointLoader.count())
w = fn(TargetSys, TargetComp, 0, MAV_FRAME_GLOBAL_RELATIVE_ALT,
MAV_CMD_DO_JUMP, 0, 1, 0, -1, 0, 0, 0, 0, 0)
MAVpointLoader.add(w, comment='Jump to exit lane')
MAVpointLoader.add(dummyw, 'jump dummy')
# fixup jump waypoint numbers
for wnum in entryjump:
MAVpointLoader.wp(wnum).param1 = entry_wpnum
for wnum in exitjump:
MAVpointLoader.wp(wnum).param1 = exit_wpnum
#export the waypoints to a MAVLink compatible format/file
waytxt = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'..', 'data', opts.outname)
MAVpointLoader.save(waytxt)
print "Waypoints exported to %s" % waytxt
# create fence.txt
fenceloader = mavwp.MAVFenceLoader()
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1])
fenceloader.add(fp)
for p in gen.missionBounds:
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, float(p[0]), float(p[1]))
fenceloader.add(fp)
# close the polygon
p = gen.missionBounds[0]
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, float(p[0]), float(p[1]))
fenceloader.add(fp)
fencetxt = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'..', 'data', "fence.txt")
fenceloader.save(fencetxt)
print "Fence exported to %s" % fencetxt
