class FieldCheck(object):
def __init__(self):
self.is_armed = False
self.last_fence_fetch = 0
self.last_mission_fetch = 0
self.last_rally_fetch = 0
self.done_heartbeat_check = 0
# an altitude should always be within a few metres of when disarmed:
self.disarmed_alt = 584
self.rate_period = mavutil.periodic_event(1.0 / 15)
self.done_map_menu = False
def close_to(self, loc1):
ret = self.get_distance(loc1, self.location)
print("Distance to %s: %um" % (self.lc_name, ret))
return ret < 100
# swiped from ArduPilot's common.py:
def get_distance(self, loc1, loc2):
"""Get ground distance between two locations."""
dlat = loc2.lat - loc1.lat
try:
dlong = loc2.lng - loc1.lng
except AttributeError:
dlong = loc2.lon - loc1.lon
return math.sqrt((dlat * dlat) + (dlong * dlong)) * 1.113195e5
def flightdata_filepath(self, filename):
if os.path.exists(filename):
return filename
return pkg_resources.resource_filename(__name__, filename)
def loadRally(self):
filepath = self.flightdata_filepath(self.fc_settings.rally_filename)
rallymod = self.module('rally')
rallymod.cmd_rally(["load", filepath])
def loadFoamyFence(self):
filepath = self.flightdata_filepath(self.fc_settings.fence_filename)
fencemod = self.module('fence')
fencemod.cmd_fence(["load", filepath])
def loadFoamyMission(self, filename):
filepath = self.flightdata_filepath(filename)
wpmod = self.module('wp')
wpmod.cmd_wp(["load", filepath])
def loadFoamyMissionCW(self):
self.loadFoamyMission(self.fc_settings.mission_filename_cw)
def loadFoamyMissionCCW(self):
self.loadFoamyMission(self.fc_settings.mission_filename_ccw)
def fixMissionRallyFence(self):
self.loadFoamyMissionCW()
self.loadFoamyFence()
self.loadRally()
def fixEVERYTHING(self):
self.loadFoamyMissionCW()
self.loadFoamyFence()
self.loadRally()
self.check_parameters(fix=True)
def whinge(self, message):
self.console.writeln("FC:%s %s" % (
self.lc_name,
message,
))
def check_parameters(self, fix=False):
'''check key parameters'''
want_values = {
"FENCE_ACTION": 4,
"FENCE_MAXALT": self.fc_settings.param_fence_maxalt,
"THR_FAILSAFE": 1,
"FS_SHORT_ACTN": 0,
"FS_LONG_ACTN": 1,
}
ret = True
for key in want_values.keys():
want = want_values[key]
got = self.mav_param.get(key, None)
if got is None:
self.whinge("No param %s" % key)
ret = False
if got != want:
self.whinge('%s should be %f (not %s)' % (key, want, got))
ret = False
if fix:
self.whinge('Setting %s to %f' % (key, want))
for m, _, _ in self.master:
self.mav_param.mavset(m, key, want, retries=3)
return ret
def check_fence_location(self):
fencemod = self.module('fence')
if fencemod is None:
self.whinge("Fence module not loaded")
return False
if not fencemod.have_list:
self.whinge("No fence list")
if self.is_armed:
return False
now = time.time()
if now - self.last_fence_fetch > 10:
self.last_fence_fetch = now
self.whinge("Running 'fence list'")
fencemod.list_fence(None)
return False
count = fencemod.fenceloader.count()
if count < 6:
self.whinge("Too few fence points")
return False
ret = True
for i in range(fencemod.fenceloader.count()):
p = fencemod.fenceloader.point(i)
loc = mavutil.location(p.lat, p.lng)
dist = self.get_distance(self.location, loc)
if dist > self.fc_settings.fence_maxdist:
self.whinge("Fencepoint %i too far away (%fm)" % (i, dist))
ret = False
return ret
def check_rally(self):
rallymod = self.module('rally')
if rallymod is None:
self.whinge("No rally module")
return False
if not rallymod.have_list:
self.whinge("No rally list")
if self.is_armed:
return False
now = time.time()
if now - self.last_rally_fetch > 10:
self.last_rally_fetch = now
self.whinge("Running 'rally list'")
rallymod.cmd_rally(["list"])
return False
count = rallymod.rally_count()
if count < 1:
self.whinge("Too few rally points")
return False
rtl_loiter_radius = self.mav_param.get("RTL_RADIUS", None)
if rtl_loiter_radius is None or rtl_loiter_radius == 0:
rtl_loiter_radius = self.mav_param.get("WP_LOITER_RAD")
if rtl_loiter_radius is None:
self.whinge("No RTL loiter radius available")
return False
ret = True
for i in range(count):
r = rallymod.rally_point(i)
loc = mavutil.location(r.lat / 10000000.0, r.lng / 10000000.0)
dist = self.get_distance(self.location, loc)
if dist > self.fc_settings.rally_maxdist:
self.whinge("Rally Point %i too far away (%fm)" % (i, dist))
ret = False
# ensure we won't loiter over the runway when doing
# rally loitering:
# print("dist=%f rtl_loiter_radius=%f", dist, rtl_loiter_radius)
if dist < rtl_loiter_radius + 30: # add a few metres of slop
self.whinge("Rally Point %i too close (%fm)" % (i, dist))
ret = False
return ret
def check_fence_health(self):
try:
sys_status = self.master.messages['SYS_STATUS']
except Exception:
return False
bits = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
present = ((sys_status.onboard_control_sensors_present & bits) == bits)
enabled = ((sys_status.onboard_control_sensors_enabled & bits) == bits)
healthy = ((sys_status.onboard_control_sensors_health & bits) == bits)
if not present or not enabled:
self.console.writeln('Fence should be enabled', fg='blue')
return False
if not healthy:
self.console.writeln('Fence unhealthy', fg='blue')
return False
return True
def check_fence(self):
ret = True
if not self.check_fence_health():
ret = False
if not self.check_fence_location():
ret = False
return ret
def check_altitude(self):
if self.is_armed:
return True
try:
gpi = self.master.messages['GLOBAL_POSITION_INT']
except Exception:
return False
max_delta = 10
current_alt = gpi.alt / 1000
if abs(current_alt - self.disarmed_alt) > max_delta:
self.whinge("Altitude (%f) not within %fm of %fm" %
(current_alt, max_delta, self.disarmed_alt))
return False
return True
def check_mission(self):
wpmod = self.module('wp')
if wpmod is None:
self.whinge("No waypoint module")
return False
count = wpmod.wploader.count()
if count == 0:
self.whinge("No waypoints")
if self.is_armed:
return False
now = time.time()
if now - self.last_mission_fetch > 10:
self.whinge("Requesting waypoints")
self.last_mission_fetch = now
wpmod.wp_op = "list"
wpmod.master.waypoint_request_list_send()
return False
if count < 2:
self.whinge("Too few waypoints")
return False
ret = True
for i in range(count):
if i == 0:
# skip home
continue
w = wpmod.wploader.wp(i)
if w.command not in [
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
mavutil.mavlink.MAV_CMD_NAV_LAND
]:
continue
loc = mavutil.location(w.x, w.y)
dist = self.get_distance(self.location, loc)
if dist > self.fc_settings.wp_maxdist:
self.whinge("Waypoint %i too far away (%fm)" % (i, dist))
ret = False
return ret
def check_status(self):
try:
hb = self.master.messages['HEARTBEAT']
mc = self.master.messages['MISSION_CURRENT']
except Exception:
return False
self.is_armed = (hb.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
if not self.is_armed and hb.custom_mode == 0:
# disarmed in MANUAL we should be at WP 0
if mc.seq > 1:
self.whinge('Incorrect WP %u' % mc.seq)
return False
return True
def check(self):
success = True
if self.master.messages.get('HEARTBEAT') is None:
self.whinge("Waiting for heartbeat")
success = False
return
if not self.check_status():
success = False
if not self.check_parameters():
success = False
if not self.check_fence():
success = False
if not self.check_mission():
success = False
if not self.check_rally():
success = False
if not self.check_altitude():
success = False
if not success:
self.whinge("CHECKS BAD")
return
if not self.is_armed:
self.whinge("CHECKS GOOD")
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if not self.done_heartbeat_check:
if self.master.messages.get('HEARTBEAT') is not None:
self.check()
self.done_heartbeat_check = True
if self.rate_period.trigger():
self.check()
def check_map_menu(self):
# make the initial map menu
if not mp_util.has_wxpython:
return
if self.done_map_menu:
if not self.module('map'):
self.done_map_menu = False
return
if self.module('map'):
self.menu = MPMenuSubMenu(
'FieldCheck',
items=[
MPMenuItem('Load foamy mission CW',
'Load foamy mission CW',
'# fieldcheck loadFoamyMissionCW'),
MPMenuItem('Load foamy mission CCW',
'Load foamy mission CCW',
'# fieldcheck loadFoamyMissionCCW'),
MPMenuItem('Load rally points', 'Load rally points',
'# fieldcheck loadRally'),
MPMenuItem('Load foamy fence', 'Load foamy fence',
'# fieldcheck loadFoamyFence'),
MPMenuItem('Fix Mission+Rally+Fence',
'Fix Mission+Rally+Fence',
'# fieldcheck fixMissionRallyFence'),
MPMenuItem('Fix EVERYTHING', 'Fix EVERYTHING',
'# fieldcheck fixEVERYTHING'),
])
self.module('map').add_menu(self.menu)
self.done_map_menu = True
def idle_task(self):
self.check_map_menu()
def FC_MPSetting(self, name, atype, default, description):
xname = "fc_%s_%s" % (self.lc_name, name)
return MPSetting(name, atype, default, description)
def select(self):
self.fc_settings = MPSettings([
self.FC_MPSetting('fence_maxdist', float, 1000,
'Max FencePoint Distance from location'),
self.FC_MPSetting('wp_maxdist', float, 500,
'Max WayPoint Distance from location'),
self.FC_MPSetting('rally_maxdist', float, 200,
'Max Rally Distance from location'),
self.FC_MPSetting('param_fence_maxalt', float, 120,
'Value parameter FENCE_MAXALT should have'),
self.FC_MPSetting('rally_filename', str,
"%s-foamy-rally.txt" % self.lc_name,
"%s Rally Point File" % self.lc_name),
self.FC_MPSetting('fence_filename', str,
"%s-foamy-fence.txt" % self.lc_name,
"%s Fence File" % self.lc_name),
self.FC_MPSetting('mission_filename_cw', str,
"%s-foamy-mission-cw.txt" % self.lc_name,
"%s Mission (CW) File" % self.lc_name),
self.FC_MPSetting('mission_filename_ccw', str,
"%s-foamy-mission-ccw.txt" % self.lc_name,
"%s Mission (CCW) File" % self.lc_name),
])
self.x.add_completion_function('(FIELDCHECKCHECKSETTING)',
self.fc_settings.completion)
self.x.add_command('fieldcheck', self.cmd_fieldcheck,
'field check control',
['check', 'set (FIELDCHECKSETTING)'])
def cmd_fieldcheck(self, args):
'''handle fieldcheck commands'''
usage = 'Usage: fieldcheck <set>'
if len(args) == 0:
print(usage)
return
if args[0] == "set":
self.fc_settings.command(args[1:])
elif args[0] == "loadFoamyMissionCW":
self.loadFoamyMissionCW()
elif args[0] == "loadFoamyMissionCCW":
self.loadFoamyMissionCCW()
elif args[0] == "loadFoamyFence":
self.loadFoamyFence()
elif args[0] == "loadRally":
self.loadRally()
elif args[0] == "fixMissionRallyFence":
self.fixMissionRallyFence()
elif args[0] == "fixEVERYTHING":
self.fixEVERYTHING()
elif args[0] == "check":
self.check()
else:
print(usage)
return
class CameraGroundModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraGroundModule, self).__init__(mpstate,
"camera_ground",
"cuav camera control (ground)",
public=True)
self.unload_event = threading.Event()
self.unload_event.clear()
self.view_thread = None
self.handled_timestamps = {}
self.camera_settings = MPSettings(
[MPSetting('air_address',
str,
"",
'Air Addresses in RemIP:RemPort:LocalPort:Bandwidth\
format (127.0.0.1:1440:1234:45, ...)',
tab='GCS'),
MPSetting('brightness', float, 1.0, 'Display Brightness',
range=(0.1, 10), increment=0.1,
digits=2, tab='Display'),
MPSetting('debug', bool, False, 'debug enable'),
MPSetting('camparms', str, None, 'camera parameters file (json) in cuav package'),
MPSetting('mosaic_thumbsize', int, 35, 'Mosaic Thumbnail Size',
range=(10, 200), increment=1),
MPSetting('maxqueue', int, 100, 'Maximum images queue'),
MPSetting('target_latitude', float, 0, 'filter detected images to latitude', tab='Filter to Location'),
MPSetting('target_longitude', float, 0, 'filter detected images to longitude', tab='Filter to Location'),
MPSetting('target_radius', float, 0, 'filter detected images to radius', tab='Filter to Location'),
],
title='Camera (ground) Settings'
)
self.viewing = False
self.boundary = None
self.boundary_polygon = None
#just make a temp dir for the downloaded images and thumbs
#this folder is deleted when the module is unloaded
#self.camera_dir = tempfile.mkdtemp()
self.camera_dir = self.mpstate.status.logdir
self.bsend = []
self.msend = None
self.msocket = None
self.mcount = [0,0,0]
self.last_msg_stamp = [0,0,0]
self.last_status_update = 0
#self.last_minscore = None
self.mosaic = None
self.last_heartbeat = time.time()
self.joelog = None
self.c_params = None
self.preview_window = None
self.add_command('camera', self.cmd_camera,
'camera control',
['<status|view|boundary>',
'set (CAMERASETTING)'])
self.add_command('remote', self.cmd_remote, "remote command", ['(COMMAND)'])
self.add_command('remotem', self.cmd_remotem, "remote command over mavlink", ['(COMMAND)'])
self.add_completion_function('(CAMERASETTING)', self.settings.completion)
self.add_completion_function('(CAMERASETTING)', self.camera_settings.completion)
for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
self.module('link').no_fwd_types.add(mtype)
print("camera (ground) initialised")
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <status|view|boundary|set>"
if len(args) == 0:
print(usage)
return
elif args[0] == "status":
#print("Cap imgs: regions:%u" % (self.region_count))
#request status update from air module
pkt = cuav_command.CommandPacket('status')
self.send_packet(pkt)
pkt = cuav_command.CommandPacket('queue')
self.send_packet(pkt)
elif args[0] == "view":
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params")
return
if self.mpstate.map is None:
print("Error - load map module first")
return
if not self.viewing:
print("Starting image viewer")
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir,
'joe_ground.log'),
append=self.continue_mode)
if self.view_thread is None:
self.view_thread = self.start_thread(self.view_threadfunc)
self.viewing = True
elif args[0] == "set":
self.camera_settings.command(args[1:])
elif args[0] == "boundary":
if len(args) != 2:
print("boundary=%s" % self.boundary)
else:
self.boundary = args[1]
self.boundary_polygon = cuav_util.polygon_load(self.boundary)
if self.mpstate.map is not None:
self.mpstate.map.add_object(mp_slipmap.SlipPolygon('boundary',
self.boundary_polygon,
layer=1, linewidth=2,
colour=(0, 0, 255)))
def cmd_remote(self, args):
'''camera remove commands over UDP'''
cmd = " ".join(args)
pkt = cuav_command.CommandPacket(cmd)
self.send_packet(pkt)
def cmd_remotem(self, args):
'''camera remote commands over mavlink'''
cmd = " ".join(args)
pkt = cuav_command.CommandPacket(cmd)
if self.msend is not None:
self.send_packet(pkt, bsnd=self.msend)
def check_camera_parms(self):
'''check for change in camera parameters'''
#dir is rel to this python file:
if self.camera_settings.camparms is None:
return False
camfiletxt = pkg_resources.resource_string("cuav", self.camera_settings.camparms)
if sys.version_info.major >= 3:
camfiletxt = camfiletxt.decode('utf-8')
try:
self.c_params = CameraParams.fromstring(camfiletxt)
return True
except:
return False
def reload_mosaic(self, mosaic):
'''reload state into mosaic'''
regions = []
last_thumbfile = None
last_joe = None
joes = []
if os.path.isfile(self.joelog.filename):
joes = cuav_joe.JoeIterator(self.joelog.filename)
for joe in joes.getjoes():
if joe.thumb_filename == last_thumbfile or last_thumbfile is None:
regions.append(joe.r)
last_joe = joe
last_thumbfile = joe.thumb_filename
else:
try:
composite = cv.LoadImage(last_joe.thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, last_joe.image_filename, last_joe.pos)
except Exception:
pass
regions = []
last_joe = None
last_thumbfile = None
if last_joe:
try:
composite = cv.LoadImage(last_joe.thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, last_joe.image_filename, last_joe.pos)
except Exception:
pass
def start_gcs_bsend(self):
'''start up block senders for GCS side'''
if self.msend is None:
self.msocket = cuav_command.MavSocket(self.mpstate.mav_master[0])
self.msend = block_xmit.BlockSender(mss=96, sock=self.msocket, dest_ip='mavlink',
dest_port=0, backlog=5, debug=False)
self.msend.set_bandwidth(500)
if len(self.bsend) == 0:
for lnk in self.camera_settings.air_address.split(','):
try:
[remoteip, remoteport, localport, bw] = lnk.split(':')
newbsnd = block_xmit.BlockSender(bandwidth=int(bw), debug=False,
dest_ip=remoteip,
dest_port=int(remoteport),
port=int(localport))
self.bsend.append(newbsnd)
except:
print("Bad Air endpoint (must be remIP:remport:localport:bw): " + str(lnk))
pass
def view_threadfunc(self):
'''image viewing thread - this runs on the ground station'''
self.start_gcs_bsend()
self.image_count = 0
self.thumb_count = 0
self.image_total_bytes = 0
#jpeg_total_bytes = 0
self.thumb_total_bytes = 0
self.region_count = 0
self.mosaic = None
self.thumbs_received = set()
# the downloaded thumbs and views are stored in a temp dir
self.view_dir = os.path.join(self.camera_dir, "view")
#self.thumb_dir = os.path.join(self.camera_dir, "thumb")
cuav_util.mkdir_p(self.view_dir)
#cuav_util.mkdir_p(self.thumb_dir)
self.console.set_status('Images', 'Images %u' % self.image_count, row=6)
self.console.set_status('Regions', 'Regions %u' % self.region_count, row=6)
self.console.set_status('Thumbs', 'Thumbs %u' % self.thumb_count, row=7)
self.console.set_status('ThumbSize', 'ThumbSize %.0f' % 0.0, row=7)
self.console.set_status('BX1', 'BX1 --', row=7)
self.console.set_status('BX2', 'BX2 --', row=7)
self.console.set_status('BX3', 'BX3 --', row=7)
# give time for maps to init
time.sleep(3)
map2 = self.module("map2")
map3 = self.module("map3")
if map2:
search_map = map2.map
if map3:
lz_map = map3.map
self.mosaic = cuav_mosaic.Mosaic(slipmap=self.mpstate.map, C=self.c_params,
camera_settings=None,
image_settings=None,
thumb_size=self.camera_settings.mosaic_thumbsize,
search_map=search_map, lz_map=lz_map)
while not self.unload_event.wait(0.05):
if self.boundary_polygon is not None:
self.mosaic.set_boundary(self.boundary_polygon)
if self.continue_mode:
self.reload_mosaic(self.mosaic)
# check for keyboard events
self.mosaic.check_events()
self.check_requested_images(self.mosaic)
#check for any new packets
for bsnd in self.bsend:
bsnd.tick(packet_count=1000, max_queue=self.camera_settings.maxqueue)
self.check_commands(bsnd)
if self.msend is not None:
self.msend.tick(packet_count=1000, max_queue=self.camera_settings.maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
#ensure the mosiac is closed at end of thread
if self.mosaic.image_mosaic:
self.mosaic.image_mosaic.terminate()
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = pickle.loads(buf)
if obj is None:
return
except Exception as e:
print(e)
return
if isinstance(obj, cuav_command.StampedCommand):
bidx = None
for i in range(len(self.bsend)):
if bsend == self.bsend[i]:
bidx = i+1
if bidx is None and bsend == self.msend:
bidx = 0
if bidx is not None:
now = time.time()
self.mcount[bidx] += 1
self.last_msg_stamp[bidx] = now
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.HeartBeat):
self.image_count = obj.icount
self.console.set_status('Images', 'Images %u' % self.image_count, row=6)
if isinstance(obj, cuav_command.ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time not in self.thumbs_received:
self.thumbs_received.add(obj.frame_time)
self.thumb_total_bytes += len(buf)
# add the thumbnails to the mosaic
thumbdec = cv2.imdecode(obj.thumb, 1)
if thumbdec is None:
pass
thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
# log the joe positions
# note the filename is where the fullsize image would be downloaded
# to, if requested
filename = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
self.log_joe_position(obj.pos, obj.frame_time, obj.regions, filename, None)
# update the mosaic and map
self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos)
# update console display
self.region_count += len(obj.regions)
self.thumb_count += 1
self.console.set_status('Regions', 'Regions %u' % self.region_count, row=6)
self.console.set_status('Thumbs', 'Thumbs %u' % self.thumb_count, row=7)
self.console.set_status('ThumbSize', 'ThumbSize %.0f' %
(self.thumb_total_bytes/self.thumb_count), row=7)
if isinstance(obj, cuav_command.ImagePacket):
# we have an image from the plane
self.image_total_bytes += len(buf)
#save to file
imagedec = cv2.imdecode(obj.jpeg, 1)
ff = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99]
cv2.imwrite(ff, imagedec, write_param)
self.mosaic.tag_image(obj.frame_time)
if obj.pos is not None:
self.mosaic.add_image(obj.frame_time, ff, obj.pos)
# update console
self.image_count += 1
color = 'black'
self.console.set_status('Images', 'Images %u' % self.image_count, row=6, fg=color)
if isinstance(obj, cuav_command.CommandPacket):
# ground doesn't accept command packets from air
pass
if isinstance(obj, cuav_command.CommandResponse):
# reply to CommandPacket
print('REMOTE: %s' % obj.response)
if isinstance(obj, cuav_command.CameraMessage):
print('CUAV AIR REMOTE: %s' % obj.msg)
if isinstance(obj, cuav_landingregion.LandingZoneDisplay):
lzresult = obj
# display on all maps
for m in self.module_matching('map?'):
m.map.add_object(mp_slipmap.SlipCircle('LZ', 'LZ', lzresult.latlon, lzresult.maxrange,
linewidth=3, color=(0,255,0)))
m.map.add_object(mp_slipmap.SlipCircle('LZMid', 'LZMid', lzresult.latlon, 2.0,
linewidth=3, color=(0,255,0)))
lztext = 'LZ: %.6f %.6f E:%.1f AS:%.0f N:%u' % (
lzresult.latlon[0], lzresult.latlon[1], lzresult.maxrange, lzresult.avgscore, lzresult.numregions)
m.map.add_object(mp_slipmap.SlipInfoText('landingzone', lztext))
# assume map2 is the search map
map2 = self.module('map2')
if map2 is not None:
#map2.map.set_zoom(250)
map2.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map2.map.set_follow(0)
# assume map3 is the lz map
map3 = self.module('map3')
if map3 is not None:
map3.map.set_zoom(max(50, 2*lzresult.maxrange))
map3.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map3.map.set_follow(0)
try:
cuav = self.module('CUAV')
cuav.show_JoeZone()
except Exception as ex:
print("err: ", ex)
return
if isinstance(obj, cuav_command.FilePacket):
print("got file %s" % obj.filename)
try:
open(obj.filename,"w").write(obj.contents)
except Exception as ex:
print("file save failed", ex)
return
if obj.filename == "newwp.txt":
try:
wpmod = self.module('wp')
wpmod.wploader.load(obj.filename)
except Exception as ex:
print("wp load failed", ex)
if isinstance(obj, cuav_command.PreviewPacket):
# we have a preview image from the plane
img = cv2.imdecode(obj.jpeg, 1)
if self.preview_window is None or not self.preview_window.is_alive():
self.preview_window = mp_image.MPImage(title='Preview', width=410, height=308, auto_size=True)
if self.preview_window is not None:
self.preview_window.set_image(img, bgr=True)
self.preview_window.poll()
def log_joe_position(self, pos, frame_time, regions, filename=None, thumb_filename=None):
'''add to joe_ground.log if possible, returning a list of (lat,lon) tuples
for the positions of the identified image regions'''
return self.joelog.add_regions(frame_time, regions, pos, filename, thumb_filename)
def start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.unload_event.set()
if self.view_thread is not None:
self.view_thread.join(1.0)
#shutil.rmtree(self.camera_dir)
print('camera unload OK')
def idle_task(self):
'''called on idle'''
now = time.time()
if now - self.last_status_update > 0.9:
self.last_status_update = now
for i in range(3):
if now - self.last_msg_stamp[i] > 20:
color = 'red'
elif now - self.last_msg_stamp[i] > 5:
color = 'orange'
else:
color = 'green'
self.console.set_status('BX%u' % (i+1), 'BX%u %u' % (i+1, self.mcount[i]), row=7, fg=color)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if m.get_type() in [ 'DATA16', 'DATA32', 'DATA64', 'DATA96' ]:
if self.msocket is not None:
self.msocket.incoming.append(m)
def check_requested_images(self, mosaic):
'''check if the user has requested download of an image'''
requests = mosaic.get_image_requests()
for frame_time in requests.keys():
fullres = requests[frame_time]
pkt = cuav_command.ImageRequest(frame_time, fullres)
print("Requesting image %s" % frame_time)
self.send_object(pkt, priority=10000)
def send_packet(self, pkt, bsnd=None):
'''send a packet from GCS'''
self.send_object(pkt, priority=10000, bsnd=bsnd)
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat(self.image_count)
self.send_packet(pkt)
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
self.send_packet(pkt)
def send_object_complete(self, obj):
'''called on complete of an send_object, cancelling send on other link'''
if obj.blockid is not None:
for bsnd in self.bsend:
bsnd.cancel(obj.blockid)
if self.msend is not None:
self.msend.cancel(obj.blockid)
def send_object(self, obj, priority, bsnd=None):
buf = pickle.dumps(obj, pickle.HIGHEST_PROTOCOL)
#only send if the queue is not clogged
if bsnd is not None:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf, priority=priority,
callback=functools.partial(self.send_object_complete, obj))
return
for bsnd in self.bsend:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf, priority=priority,
callback=functools.partial(self.send_object_complete, obj))
class CameraGroundModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraGroundModule, self).__init__(mpstate,
"camera_ground",
"cuav camera control (ground)",
public=True)
self.unload_event = threading.Event()
self.unload_event.clear()
self.view_thread = None
self.handled_timestamps = {}
self.camera_settings = MPSettings(
[MPSetting('air_address',
str,
"",
'Air Addresses in RemIP:RemPort:LocalPort:Bandwidth\
format (127.0.0.1:1440:1234:45, ...)',
tab='GCS'),
MPSetting('brightness', float, 1.0, 'Display Brightness',
range=(0.1, 10), increment=0.1,
digits=2, tab='Display'),
MPSetting('debug', bool, False, 'debug enable'),
MPSetting('camparms', str, None, 'camera parameters file (json) in cuav package'),
MPSetting('mosaic_thumbsize', int, 35, 'Mosaic Thumbnail Size',
range=(10, 200), increment=1),
MPSetting('maxqueue', int, 100, 'Maximum images queue'),
MPSetting('target_latitude', float, 0, 'filter detected images to latitude', tab='Filter to Location'),
MPSetting('target_longitude', float, 0, 'filter detected images to longitude', tab='Filter to Location'),
MPSetting('target_radius', float, 0, 'filter detected images to radius', tab='Filter to Location'),
],
title='Camera (ground) Settings'
)
self.viewing = False
self.boundary = None
self.boundary_polygon = None
#just make a temp dir for the downloaded images and thumbs
#this folder is deleted when the module is unloaded
#self.camera_dir = tempfile.mkdtemp()
self.camera_dir = self.mpstate.status.logdir
self.bsend = []
#self.last_minscore = None
self.mosaic = None
self.last_heartbeat = time.time()
self.joelog = None
self.c_params = None
self.add_command('camera', self.cmd_camera,
'camera control',
['<status|view|boundary>',
'set (CAMERASETTING)'])
self.add_command('remote', self.cmd_remote, "remote command", ['(COMMAND)'])
self.add_completion_function('(CAMERASETTING)', self.settings.completion)
self.add_completion_function('(CAMERASETTING)', self.camera_settings.completion)
print("camera (ground) initialised")
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <status|view|boundary|set>"
if len(args) == 0:
print(usage)
return
elif args[0] == "status":
#print("Cap imgs: regions:%u" % (self.region_count))
#request status update from air module
pkt = cuav_command.CommandPacket('status')
self.send_packet(pkt)
pkt = cuav_command.CommandPacket('queue')
self.send_packet(pkt)
elif args[0] == "view":
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params")
return
if self.mpstate.map is None:
print("Error - load map module first")
return
if not self.viewing:
print("Starting image viewer")
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir,
'joe_ground.log'),
append=self.continue_mode)
if self.view_thread is None:
self.view_thread = self.start_thread(self.view_threadfunc)
self.viewing = True
elif args[0] == "set":
self.camera_settings.command(args[1:])
elif args[0] == "boundary":
if len(args) != 2:
print("boundary=%s" % self.boundary)
else:
self.boundary = args[1]
self.boundary_polygon = cuav_util.polygon_load(self.boundary)
if self.mpstate.map is not None:
self.mpstate.map.add_object(mp_slipmap.SlipPolygon('boundary',
self.boundary_polygon,
layer=1, linewidth=2,
colour=(0, 0, 255)))
def cmd_remote(self, args):
'''camera commands'''
cmd = " ".join(args)
pkt = cuav_command.CommandPacket(cmd)
self.send_packet(pkt)
def check_camera_parms(self):
'''check for change in camera parameters'''
#dir is rel to this python file:
if self.camera_settings.camparms is None:
return False
camfiletxt = pkg_resources.resource_string("cuav", self.camera_settings.camparms)
try:
self.c_params = CameraParams.fromstring(camfiletxt)
return True
except:
return False
def reload_mosaic(self, mosaic):
'''reload state into mosaic'''
regions = []
last_thumbfile = None
last_joe = None
joes = []
if os.path.isfile(self.joelog.filename):
joes = cuav_joe.JoeIterator(self.joelog.filename)
for joe in joes.getjoes():
if joe.thumb_filename == last_thumbfile or last_thumbfile is None:
regions.append(joe.r)
last_joe = joe
last_thumbfile = joe.thumb_filename
else:
try:
composite = cv.LoadImage(last_joe.thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, last_joe.image_filename, last_joe.pos)
except Exception:
pass
regions = []
last_joe = None
last_thumbfile = None
if last_joe:
try:
composite = cv.LoadImage(last_joe.thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, last_joe.image_filename, last_joe.pos)
except Exception:
pass
def start_gcs_bsend(self):
'''start up block senders for GCS side'''
if len(self.bsend) == 0:
for lnk in self.camera_settings.air_address.split(','):
try:
[remoteip, remoteport, localport, bw] = lnk.split(':')
newbsnd = block_xmit.BlockSender(bandwidth=int(bw), debug=False,
dest_ip=remoteip,
dest_port=int(remoteport),
port=int(localport))
self.bsend.append(newbsnd)
except:
print("Bad Air endpoint (must be remIP:remport:localport:bw): " + str(lnk))
pass
def view_threadfunc(self):
'''image viewing thread - this runs on the ground station'''
self.start_gcs_bsend()
self.image_count = 0
self.thumb_count = 0
self.image_total_bytes = 0
#jpeg_total_bytes = 0
self.thumb_total_bytes = 0
self.region_count = 0
self.mosaic = None
self.thumbs_received = set()
# the downloaded thumbs and views are stored in a temp dir
self.view_dir = os.path.join(self.camera_dir, "view")
#self.thumb_dir = os.path.join(self.camera_dir, "thumb")
cuav_util.mkdir_p(self.view_dir)
#cuav_util.mkdir_p(self.thumb_dir)
self.console.set_status('Images', 'Images %u' % self.image_count, row=6)
self.console.set_status('Regions', 'Regions %u' % self.region_count, row=6)
self.console.set_status('JPGSize', 'JPGSize %.0f' % 0.0, row=6)
self.console.set_status('Thumbs', 'Thumbs %u' % self.thumb_count, row=7)
self.console.set_status('ThumbSize', 'ThumbSize %.0f' % 0.0, row=7)
self.console.set_status('ImageSize', 'ImageSize %.0f' % 0.0, row=7)
self.mosaic = cuav_mosaic.Mosaic(slipmap=self.mpstate.map, C=self.c_params,
camera_settings=None,
image_settings=None,
thumb_size=self.camera_settings.mosaic_thumbsize)
while not self.unload_event.wait(0.05):
if self.boundary_polygon is not None:
self.mosaic.set_boundary(self.boundary_polygon)
if self.continue_mode:
self.reload_mosaic(self.mosaic)
# check for keyboard events
self.mosaic.check_events()
self.check_requested_images(self.mosaic)
#check for any new packets
for bsnd in self.bsend:
bsnd.tick(packet_count=1000, max_queue=self.camera_settings.maxqueue)
self.check_commands(bsnd)
self.send_heartbeats()
#ensure the mosiac is closed at end of thread
if self.mosaic.image_mosaic:
self.mosaic.image_mosaic.terminate()
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = cPickle.loads(str(buf))
if obj is None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time not in self.thumbs_received:
self.thumbs_received.add(obj.frame_time)
self.thumb_total_bytes += len(buf)
# add the thumbnails to the mosaic
thumbdec = cv2.imdecode(obj.thumb, 1)
if thumbdec is None:
pass
thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
# log the joe positions
# note the filename is where the fullsize image would be downloaded
# to, if requested
filename = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
self.log_joe_position(obj.pos, obj.frame_time, obj.regions, filename, None)
# update the mosaic and map
self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos)
# update console display
self.region_count += len(obj.regions)
self.thumb_count += 1
self.console.set_status('Regions', 'Regions %u' % self.region_count, row=6)
self.console.set_status('Thumbs', 'Thumbs %u' % self.thumb_count, row=7)
self.console.set_status('ThumbSize', 'ThumbSize %.0f' %
(self.thumb_total_bytes/self.thumb_count), row=7)
if isinstance(obj, cuav_command.ImagePacket):
# we have an image from the plane
self.image_total_bytes += len(buf)
#save to file
imagedec = cv2.imdecode(obj.jpeg, 1)
ff = os.path.join(self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99]
cv2.imwrite(ff, imagedec, write_param)
self.mosaic.tag_image(obj.frame_time)
if obj.pos is not None:
self.mosaic.add_image(obj.frame_time, ff, obj.pos)
# update console
self.image_count += 1
color = 'black'
self.console.set_status('Images', 'Images %u' % self.image_count, row=6, fg=color)
self.console.set_status('ImageSize', 'ImageSize %.0f' %
(self.image_total_bytes/self.image_count), row=7)
if isinstance(obj, cuav_command.CommandPacket):
# ground doesn't accept command packets from air
pass
if isinstance(obj, cuav_command.CommandResponse):
# reply to CommandPacket
print('REMOTE: %s' % obj.response)
if isinstance(obj, cuav_command.CameraMessage):
print('CUAV AIR REMOTE: %s' % obj.msg)
def log_joe_position(self, pos, frame_time, regions, filename=None, thumb_filename=None):
'''add to joe_ground.log if possible, returning a list of (lat,lon) tuples
for the positions of the identified image regions'''
return self.joelog.add_regions(frame_time, regions, pos, filename, thumb_filename)
def start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.unload_event.set()
if self.view_thread is not None:
self.view_thread.join(1.0)
#shutil.rmtree(self.camera_dir)
print('camera unload OK')
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
pass
def check_requested_images(self, mosaic):
'''check if the user has requested download of an image'''
requests = mosaic.get_image_requests()
for frame_time in requests.keys():
fullres = requests[frame_time]
pkt = cuav_command.ImageRequest(frame_time, fullres)
print("Requesting image %s" % frame_time)
self.send_object(pkt, priority=10000)
def send_packet(self, pkt):
'''send a packet from GCS'''
self.send_object(pkt, priority=10000)
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat()
self.send_packet(pkt)
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
self.send_packet(pkt)
def send_object_complete(self, obj):
'''called on complete of an send_object, cancelling send on other link'''
if obj.blockid is not None:
for bsnd in self.bsend:
bsnd.cancel(obj.blockid)
def send_object(self, obj, priority):
buf = cPickle.dumps(obj, cPickle.HIGHEST_PROTOCOL)
#only send if the queue is not clogged
for bsnd in self.bsend:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf, priority=priority,
callback=functools.partial(self.send_object_complete, obj))
class CameraGroundModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraGroundModule,
self).__init__(mpstate,
"camera_ground",
"cuav camera control (ground)",
public=True)
self.unload_event = threading.Event()
self.unload_event.clear()
self.view_thread = None
self.handled_timestamps = {}
self.camera_settings = MPSettings([
MPSetting('air_address',
str,
"",
'Air Addresses in RemIP:RemPort:LocalPort:Bandwidth\
format (127.0.0.1:1440:1234:45, ...)',
tab='GCS'),
MPSetting('brightness',
float,
1.0,
'Display Brightness',
range=(0.1, 10),
increment=0.1,
digits=2,
tab='Display'),
MPSetting('debug', bool, False, 'debug enable'),
MPSetting('camparms', str, None,
'camera parameters file (json) in cuav package'),
MPSetting('mosaic_thumbsize',
int,
35,
'Mosaic Thumbnail Size',
range=(10, 200),
increment=1),
MPSetting('maxqueue', int, 100, 'Maximum images queue'),
MPSetting('target_latitude',
float,
0,
'filter detected images to latitude',
tab='Filter to Location'),
MPSetting('target_longitude',
float,
0,
'filter detected images to longitude',
tab='Filter to Location'),
MPSetting('target_radius',
float,
0,
'filter detected images to radius',
tab='Filter to Location'),
],
title='Camera (ground) Settings')
self.viewing = False
self.boundary = None
self.boundary_polygon = None
#just make a temp dir for the downloaded images and thumbs
#this folder is deleted when the module is unloaded
#self.camera_dir = tempfile.mkdtemp()
self.camera_dir = self.mpstate.status.logdir
self.bsend = []
self.msend = None
self.msocket = None
self.mcount = [0, 0, 0]
self.last_msg_stamp = [0, 0, 0]
self.last_status_update = 0
#self.last_minscore = None
self.mosaic = None
self.last_heartbeat = time.time()
self.joelog = None
self.c_params = None
self.preview_window = None
self.add_command('camera', self.cmd_camera, 'camera control',
['<status|view|boundary>', 'set (CAMERASETTING)'])
self.add_command('remote', self.cmd_remote, "remote command",
['(COMMAND)'])
self.add_command('remotem', self.cmd_remotem,
"remote command over mavlink", ['(COMMAND)'])
self.add_completion_function('(CAMERASETTING)',
self.settings.completion)
self.add_completion_function('(CAMERASETTING)',
self.camera_settings.completion)
for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
self.module('link').no_fwd_types.add(mtype)
print("camera (ground) initialised")
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <status|view|boundary|set>"
if len(args) == 0:
print(usage)
return
elif args[0] == "status":
#print("Cap imgs: regions:%u" % (self.region_count))
#request status update from air module
pkt = cuav_command.CommandPacket('status')
self.send_packet(pkt)
pkt = cuav_command.CommandPacket('queue')
self.send_packet(pkt)
elif args[0] == "view":
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params")
return
if self.mpstate.map is None:
print("Error - load map module first")
return
if not self.viewing:
print("Starting image viewer")
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir,
'joe_ground.log'),
append=self.continue_mode)
if self.view_thread is None:
self.view_thread = self.start_thread(self.view_threadfunc)
self.viewing = True
elif args[0] == "set":
self.camera_settings.command(args[1:])
elif args[0] == "boundary":
if len(args) != 2:
print("boundary=%s" % self.boundary)
else:
self.boundary = args[1]
self.boundary_polygon = cuav_util.polygon_load(self.boundary)
if self.mpstate.map is not None:
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon('boundary',
self.boundary_polygon,
layer=1,
linewidth=2,
colour=(0, 0, 255)))
def cmd_remote(self, args):
'''camera remove commands over UDP'''
cmd = " ".join(args)
pkt = cuav_command.CommandPacket(cmd)
self.send_packet(pkt)
def cmd_remotem(self, args):
'''camera remote commands over mavlink'''
cmd = " ".join(args)
pkt = cuav_command.CommandPacket(cmd)
if self.msend is not None:
self.send_packet(pkt, bsnd=self.msend)
def check_camera_parms(self):
'''check for change in camera parameters'''
#dir is rel to this python file:
if self.camera_settings.camparms is None:
return False
camfiletxt = pkg_resources.resource_string(
"cuav", self.camera_settings.camparms)
try:
self.c_params = CameraParams.fromstring(camfiletxt)
return True
except:
return False
def reload_mosaic(self, mosaic):
'''reload state into mosaic'''
regions = []
last_thumbfile = None
last_joe = None
joes = []
if os.path.isfile(self.joelog.filename):
joes = cuav_joe.JoeIterator(self.joelog.filename)
for joe in joes.getjoes():
if joe.thumb_filename == last_thumbfile or last_thumbfile is None:
regions.append(joe.r)
last_joe = joe
last_thumbfile = joe.thumb_filename
else:
try:
composite = cv.LoadImage(last_joe.thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs,
last_joe.image_filename, last_joe.pos)
except Exception:
pass
regions = []
last_joe = None
last_thumbfile = None
if last_joe:
try:
composite = cv.LoadImage(last_joe.thumb_filename)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, last_joe.image_filename,
last_joe.pos)
except Exception:
pass
def start_gcs_bsend(self):
'''start up block senders for GCS side'''
if self.msend is None:
self.msocket = cuav_command.MavSocket(self.mpstate.mav_master[0])
self.msend = block_xmit.BlockSender(mss=96,
sock=self.msocket,
dest_ip='mavlink',
dest_port=0,
backlog=5,
debug=False)
self.msend.set_bandwidth(500)
if len(self.bsend) == 0:
for lnk in self.camera_settings.air_address.split(','):
try:
[remoteip, remoteport, localport, bw] = lnk.split(':')
newbsnd = block_xmit.BlockSender(bandwidth=int(bw),
debug=False,
dest_ip=remoteip,
dest_port=int(remoteport),
port=int(localport))
self.bsend.append(newbsnd)
except:
print(
"Bad Air endpoint (must be remIP:remport:localport:bw): "
+ str(lnk))
pass
def view_threadfunc(self):
'''image viewing thread - this runs on the ground station'''
self.start_gcs_bsend()
self.image_count = 0
self.thumb_count = 0
self.image_total_bytes = 0
#jpeg_total_bytes = 0
self.thumb_total_bytes = 0
self.region_count = 0
self.mosaic = None
self.thumbs_received = set()
# the downloaded thumbs and views are stored in a temp dir
self.view_dir = os.path.join(self.camera_dir, "view")
#self.thumb_dir = os.path.join(self.camera_dir, "thumb")
cuav_util.mkdir_p(self.view_dir)
#cuav_util.mkdir_p(self.thumb_dir)
self.console.set_status('Images',
'Images %u' % self.image_count,
row=6)
self.console.set_status('Regions',
'Regions %u' % self.region_count,
row=6)
self.console.set_status('Thumbs',
'Thumbs %u' % self.thumb_count,
row=7)
self.console.set_status('ThumbSize', 'ThumbSize %.0f' % 0.0, row=7)
self.console.set_status('BX1', 'BX1 --', row=7)
self.console.set_status('BX2', 'BX2 --', row=7)
self.console.set_status('BX3', 'BX3 --', row=7)
# give time for maps to init
time.sleep(3)
map2 = self.module("map2")
map3 = self.module("map3")
if map2:
search_map = map2.map
if map3:
lz_map = map3.map
self.mosaic = cuav_mosaic.Mosaic(
slipmap=self.mpstate.map,
C=self.c_params,
camera_settings=None,
image_settings=None,
thumb_size=self.camera_settings.mosaic_thumbsize,
search_map=search_map,
lz_map=lz_map)
while not self.unload_event.wait(0.05):
if self.boundary_polygon is not None:
self.mosaic.set_boundary(self.boundary_polygon)
if self.continue_mode:
self.reload_mosaic(self.mosaic)
# check for keyboard events
self.mosaic.check_events()
self.check_requested_images(self.mosaic)
#check for any new packets
for bsnd in self.bsend:
bsnd.tick(packet_count=1000,
max_queue=self.camera_settings.maxqueue)
self.check_commands(bsnd)
if self.msend is not None:
self.msend.tick(packet_count=1000,
max_queue=self.camera_settings.maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
#ensure the mosiac is closed at end of thread
if self.mosaic.image_mosaic:
self.mosaic.image_mosaic.terminate()
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = cPickle.loads(str(buf))
if obj is None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
bidx = None
for i in range(len(self.bsend)):
if bsend == self.bsend[i]:
bidx = i + 1
if bidx is None and bsend == self.msend:
bidx = 0
if bidx is not None:
now = time.time()
self.mcount[bidx] += 1
self.last_msg_stamp[bidx] = now
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.HeartBeat):
self.image_count = obj.icount
self.console.set_status('Images',
'Images %u' % self.image_count,
row=6)
if isinstance(obj, cuav_command.ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time not in self.thumbs_received:
self.thumbs_received.add(obj.frame_time)
self.thumb_total_bytes += len(buf)
# add the thumbnails to the mosaic
thumbdec = cv2.imdecode(obj.thumb, 1)
if thumbdec is None:
pass
thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
# log the joe positions
# note the filename is where the fullsize image would be downloaded
# to, if requested
filename = os.path.join(
self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
self.log_joe_position(obj.pos, obj.frame_time, obj.regions,
filename, None)
# update the mosaic and map
self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos)
# update console display
self.region_count += len(obj.regions)
self.thumb_count += 1
self.console.set_status('Regions',
'Regions %u' % self.region_count,
row=6)
self.console.set_status('Thumbs',
'Thumbs %u' % self.thumb_count,
row=7)
self.console.set_status(
'ThumbSize',
'ThumbSize %.0f' % (self.thumb_total_bytes / self.thumb_count),
row=7)
if isinstance(obj, cuav_command.ImagePacket):
# we have an image from the plane
self.image_total_bytes += len(buf)
#save to file
imagedec = cv2.imdecode(obj.jpeg, 1)
ff = os.path.join(self.view_dir,
cuav_util.frame_time(obj.frame_time)) + ".jpg"
write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99]
cv2.imwrite(ff, imagedec, write_param)
self.mosaic.tag_image(obj.frame_time)
if obj.pos is not None:
self.mosaic.add_image(obj.frame_time, ff, obj.pos)
# update console
self.image_count += 1
color = 'black'
self.console.set_status('Images',
'Images %u' % self.image_count,
row=6,
fg=color)
if isinstance(obj, cuav_command.CommandPacket):
# ground doesn't accept command packets from air
pass
if isinstance(obj, cuav_command.CommandResponse):
# reply to CommandPacket
print('REMOTE: %s' % obj.response)
if isinstance(obj, cuav_command.CameraMessage):
print('CUAV AIR REMOTE: %s' % obj.msg)
if isinstance(obj, cuav_landingregion.LandingZoneDisplay):
lzresult = obj
# display on all maps
for m in self.module_matching('map?'):
m.map.add_object(
mp_slipmap.SlipCircle('LZ',
'LZ',
lzresult.latlon,
lzresult.maxrange,
linewidth=3,
color=(0, 255, 0)))
m.map.add_object(
mp_slipmap.SlipCircle('LZMid',
'LZMid',
lzresult.latlon,
2.0,
linewidth=3,
color=(0, 255, 0)))
lztext = 'LZ: %.6f %.6f E:%.1f AS:%.0f N:%u' % (
lzresult.latlon[0], lzresult.latlon[1], lzresult.maxrange,
lzresult.avgscore, lzresult.numregions)
m.map.add_object(mp_slipmap.SlipInfoText(
'landingzone', lztext))
# assume map2 is the search map
map2 = self.module('map2')
if map2 is not None:
#map2.map.set_zoom(250)
map2.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map2.map.set_follow(0)
# assume map3 is the lz map
map3 = self.module('map3')
if map3 is not None:
map3.map.set_zoom(max(50, 2 * lzresult.maxrange))
map3.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map3.map.set_follow(0)
try:
cuav = self.module('CUAV')
cuav.show_JoeZone()
except Exception as ex:
print("err: ", ex)
return
if isinstance(obj, cuav_command.FilePacket):
print("got file %s" % obj.filename)
try:
open(obj.filename, "w").write(obj.contents)
except Exception as ex:
print("file save failed", ex)
return
if obj.filename == "newwp.txt":
try:
wpmod = self.module('wp')
wpmod.wploader.load(obj.filename)
except Exception as ex:
print("wp load failed", ex)
if isinstance(obj, cuav_command.PreviewPacket):
# we have a preview image from the plane
img = cv2.imdecode(obj.jpeg, 1)
if self.preview_window is None or not self.preview_window.is_alive(
):
self.preview_window = mp_image.MPImage(title='Preview',
width=410,
height=308,
auto_size=True)
if self.preview_window is not None:
self.preview_window.set_image(img, bgr=True)
self.preview_window.poll()
def log_joe_position(self,
pos,
frame_time,
regions,
filename=None,
thumb_filename=None):
'''add to joe_ground.log if possible, returning a list of (lat,lon) tuples
for the positions of the identified image regions'''
return self.joelog.add_regions(frame_time, regions, pos, filename,
thumb_filename)
def start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.unload_event.set()
if self.view_thread is not None:
self.view_thread.join(1.0)
#shutil.rmtree(self.camera_dir)
print('camera unload OK')
def idle_task(self):
'''called on idle'''
now = time.time()
if now - self.last_status_update > 0.9:
self.last_status_update = now
for i in range(3):
if now - self.last_msg_stamp[i] > 20:
color = 'red'
elif now - self.last_msg_stamp[i] > 5:
color = 'orange'
else:
color = 'green'
self.console.set_status('BX%u' % (i + 1),
'BX%u %u' % (i + 1, self.mcount[i]),
row=7,
fg=color)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if m.get_type() in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
if self.msocket is not None:
self.msocket.incoming.append(m)
def check_requested_images(self, mosaic):
'''check if the user has requested download of an image'''
requests = mosaic.get_image_requests()
for frame_time in requests.keys():
fullres = requests[frame_time]
pkt = cuav_command.ImageRequest(frame_time, fullres)
print("Requesting image %s" % frame_time)
self.send_object(pkt, priority=10000)
def send_packet(self, pkt, bsnd=None):
'''send a packet from GCS'''
self.send_object(pkt, priority=10000, bsnd=bsnd)
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat(self.image_count)
self.send_packet(pkt)
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
self.send_packet(pkt)
def send_object_complete(self, obj):
'''called on complete of an send_object, cancelling send on other link'''
if obj.blockid is not None:
for bsnd in self.bsend:
bsnd.cancel(obj.blockid)
if self.msend is not None:
self.msend.cancel(obj.blockid)
def send_object(self, obj, priority, bsnd=None):
buf = cPickle.dumps(obj, cPickle.HIGHEST_PROTOCOL)
#only send if the queue is not clogged
if bsnd is not None:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf,
priority=priority,
callback=functools.partial(
self.send_object_complete, obj))
return
for bsnd in self.bsend:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf,
priority=priority,
callback=functools.partial(
self.send_object_complete, obj))
class CameraAirModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraAirModule, self).__init__(mpstate, "camera_air", "cuav camera control (air)", public = True)
self.running = False
self.unload_event = threading.Event()
self.unload_event.clear()
self.capture_thread = None
self.scan_thread = None
self.transmit_thread = None
self.airstart_triggered = False
self.terrain_alt = None
self.handled_timestamps = {}
self.imagefilenamemapping = {}
self.posmapping = {}
self.is_armed = True
self.lz = cuav_landingregion.LandingZone()
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.camera_settings = MPSettings(
[ MPSetting('roll_stabilised', bool, False, 'Roll Stabilised'),
MPSetting('roll_limit', float, 0, 'Roll stabilisation limit'),
MPSetting('minspeed', int, 20, 'For airstart, minimum speed for capture to start'),
MPSetting('minalt', int, 30, 'MinAltitude of images', range=(0,10000), increment=1),
MPSetting('rotate180', bool, False, 'rotate images by 180', tab='Capture2'),
MPSetting('ignoretimestamps', bool, False, 'Ignore image timestamps', tab='Capture2'),
MPSetting('camparms', str, None, 'camera parameters file (json) in cuav package', tab='Imaging'),
MPSetting('imagefile', str, None, 'latest captured image', tab='Imaging'),
MPSetting('filter_type', str, 'simple', 'Filter Type',
choice=['simple'], tab='Imaging'),
MPSetting('blue_emphasis', bool, False, 'BlueEmphasis', tab='Imaging'),
MPSetting('use_capture_time', bool, True, 'Use Capture Time (false for sim)', tab='Simulation'),
MPSetting('target_latitude', float, 0, 'filter detected images to latitude', tab='Filter to Location'),
MPSetting('target_longitude', float, 0, 'filter detected images to longitude', tab='Filter to Location'),
MPSetting('target_radius', float, 0, 'filter detected images to radius', tab='Filter to Location'),
MPSetting('gcs_address', str, "", 'GCS Addresses in RemIP:RemPort:LocalPort:Bandwidth format (127.0.0.1:1440:1234:45, ...)', tab='GCS'),
MPSetting('qualitysend', int, 90, 'Compression Quality for send', range=(1,100), increment=1, tab='GCS'),
MPSetting('transmit', bool, True, 'Transmit Enable for thumbnails', tab='GCS'),
MPSetting('maxqueue', int, 50, 'Maximum images queue', tab='GCS'),
MPSetting('thumbsize', int, 60, 'Thumbnail Size', range=(10, 200), increment=1),
MPSetting('minscore', int, 1000, 'Min Score to pass detection', range=(0,100000), increment=1, tab='Imaging'),
MPSetting('clock_sync', bool, False, 'GPS Clock Sync'),
MPSetting('m_minscore', int, 20000, 'Min Score to pass detection on mavlink', range=(0,100000), increment=1, tab='Imaging'),
MPSetting('m_bandwidth', int, 500, 'max bandwidth on mavlink', increment=1, tab='GCS'),
MPSetting('m_maxqueue', int, 5, 'Maximum images queue for mavlink', tab='GCS'),
MPSetting('preview', bool, True, 'enable camera preview window', tab='Imaging'),
MPSetting('previewquality', int, 40, 'Compression Quality for preview', range=(1,100), increment=1, tab='Imaging'),
MPSetting('previewscale', int, 5, 'preview downscaling', range=(1,10), increment=1, tab='Imaging'),
MPSetting('previewfreq', int, 4, 'preview image frequency', range=(1,10), increment=1, tab='Imaging'),
],
title='Camera Settings'
)
self.image_settings = MPSettings(
[ MPSetting('MinRegionArea', float, 0.15, range=(0,100), increment=0.05, digits=2, tab='Image Processing'),
MPSetting('MaxRegionArea', float, 1.0, range=(0,100), increment=0.1, digits=1, tab='Image Processing'),
MPSetting('MinRegionSize', float, 0.2, range=(0,100), increment=0.05, digits=2, tab='Image Processing'),
MPSetting('MaxRegionSize', float, 1.0, range=(0,100), increment=0.1, digits=1, tab='Image Processing'),
MPSetting('MaxRarityPct', float, 0.02, range=(0,100), increment=0.01, digits=2, tab='Image Processing'),
MPSetting('RegionMergeSize', float, 1.0, range=(0,100), increment=0.1, digits=1, tab='Image Processing'),
],
title='Image Settings')
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.scan_fps = 0
self.scan_queue = multiproc.Queue()
self.transmit_queue = multiproc.Queue()
self.have_set_gps_time = False
self.c_params = None
self.jpeg_size = 0
self.xmit_queue = []
self.efficiency = []
self.last_watch = 0
self.boundary = None
self.boundary_polygon = None
self.bandwidth_used = []
self.rtt_estimate = []
self.bsend = [] #note this is an array of bsends
# msend is a BlockSender over MAVLink
self.msocket = None
self.msend = None
self.last_heartbeat = time.time()
self.mpos = mav_position.MavInterpolator(backlog=500, gps_lag=0.0)
self.joelog = None #cuav_joe.JoeLog(os.path.join(self.settings.imagefile, '..', 'joe.log'), append=self.continue_mode)
self.add_command('camera', self.cmd_camera,
'camera control',
['<start|stop|status|boundary|airstart>',
'set (CAMERASETTING)'])
self.add_completion_function('(CAMERASETTING)', self.camera_settings.completion)
# prevent loopback of messages
for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
self.module('link').no_fwd_types.add(mtype)
print("camera initialised")
def get_bsend(self, bsnd):
'''get a bsend object, given a tag name'''
if bsnd is None:
return None
if bsnd == 'msend':
return self.msend
return self.bsend[bsnd]
def get_bsend_index(self, bsnd):
'''get a bsend index from a bsend object. This avoids pickling a block xmit object'''
if bsnd is None or isinstance(bsnd, int) or bsnd == 'msend':
return bsnd
if bsnd == self.msend:
return 'msend'
return self.bsend.index(bsnd)
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <start|airstart|stop|status|queue|set>"
if len(args) == 0:
print(usage)
return
if args[0] == "start":
self.capture_count = 0
self.error_count = 0
self.error_msg = None
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params " + str(self.camera_settings.camparms))
return
if self.running == False:
self.running = True
self.joelog = cuav_joe.JoeLog(os.path.join(os.path.dirname(self.camera_settings.imagefile), 'joe_air.log'), append=self.continue_mode)
self.capture_thread = self.start_thread(self.capture_threadfunc)
self.scan_thread = self.start_thread(self.scan_threadfunc)
self.transmit_thread = self.start_thread(self.transmit_threadfunc)
time.sleep(0.1)
self.send_message("Started cuav running")
print("Started cuav running")
else:
self.send_message("cuav already running")
print("cuav already running")
elif args[0] == "stop":
self.running = False
self.airstart_triggered = False
print("Stopped cuav")
self.send_message("Stopped cuav")
elif args[0] == "status":
ret = "Cap imgs:%u err:%u scan:%u regions:%u jsize:%.0f xmitq:%s sq:%u eff:%s" % (
self.capture_count, self.error_count, self.scan_count,
self.region_count,
self.jpeg_size,
self.xmit_queue, self.scan_queue.qsize(),
self.efficiency)
print(ret)
self.send_message(ret)
elif args[0] == "queue":
ret = "scan %u transmit %u eff %s bw %s rtt %s" % (
self.scan_queue.qsize(),
self.transmit_queue.qsize(),
self.efficiency,
self.bandwidth_used,
self.rtt_estimate)
print(ret)
elif args[0] == "set":
self.camera_settings.command(args[1:])
elif args[0] == "airstart":
#just keep the block xmit going for now
self.capture_count = 0
self.error_count = 0
self.error_msg = None
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params " + str(self.camera_settings.camparms))
return
if self.airstart_triggered == False:
self.airstart_triggered = True
self.joelog = cuav_joe.JoeLog(os.path.join(os.path.dirname(self.camera_settings.imagefile), 'joe_air.log'), append=self.continue_mode)
self.transmit_thread = self.start_thread(self.transmit_threadfunc)
time.sleep(0.1)
self.send_message("cuav airstart ready")
print("cuav airstart ready")
else:
self.send_message("cuav airstart already running")
print("cuav airstart already running")
else:
print(usage)
def check_camera_parms(self):
'''check for change in camera parameters'''
#dir is rel to this python file:
if self.camera_settings.camparms is None:
return False
camfiletxt = pkg_resources.resource_string("cuav", self.camera_settings.camparms)
if sys.version_info.major >= 3:
camfiletxt = camfiletxt.decode('utf-8')
try:
self.c_params = CameraParams.fromstring(camfiletxt)
return True
except:
return False
def capture_threadfunc(self):
'''image capture thread, via monitoring the
link for changed linked filenames'''
prev_image = None
self.scan_queue = multiproc.Queue()
while not self.unload_event.wait(0.05):
try:
filename = os.path.realpath(self.camera_settings.imagefile)
if not self.camera_settings.ignoretimestamps:
filetime = cuav_util.parse_frame_time(filename)
else:
filetime = float(time.time())
except Exception:
filename = None
pass
#ensure all items are valid and the queue isn't overfilled > 100
if filename != None and prev_image != filename and filetime != None and self.scan_queue.qsize() < 100:
self.scan_queue.put((filetime, filename))
self.imagefilenamemapping[str(filetime)] = filename
self.capture_count += 1
prev_image = filename
if self.is_armed:
stopfile = self.camera_settings.imagefile + ".stop"
if os.path.exists(stopfile):
print("Removing stopfile")
os.unlink(self.camera_settings.imagefile + ".stop")
def scan_threadfunc(self):
'''image scanning thread'''
while not self.unload_event.wait(0.05):
if self.scan_queue.empty():
continue
(frame_time,im) = self.scan_queue.get()
scan_parms = {}
for name in self.image_settings.list():
scan_parms[name] = self.image_settings.get(name)
scan_parms['BlueEmphasis'] = float(self.camera_settings.blue_emphasis)
if self.terrain_alt is not None:
altitude = self.terrain_alt
if altitude < self.camera_settings.minalt:
altitude = self.camera_settings.minalt
scan_parms['MetersPerPixel'] = cuav_util.pixel_width(altitude,
self.c_params.xresolution,
self.c_params.lens,
self.c_params.sensorwidth)
t1 = time.time()
try:
img_scan = cv2.imread(im, -1)
except Exception:
continue
if img_scan is None:
continue
(h, w) = img_scan.shape[:2]
if self.camera_settings.rotate180:
M = cv2.getRotationMatrix2D(center, angle180, scale)
img_scan = cv2.warpAffine(img_scan, M, (w, h))
im_numpy = numpy.ascontiguousarray(img_scan)
regions = scanner.scan(im_numpy, scan_parms)
regions = cuav_region.RegionsConvert(regions,
cuav_util.image_shape(img_scan),
cuav_util.image_shape(img_scan))
t2 = time.time()
self.scan_fps = 1.0 / (t2-t1)
self.scan_count += 1
regions = cuav_region.filter_regions(img_scan, regions,
min_score=self.camera_settings.minscore,
filter_type=self.camera_settings.filter_type)
self.region_count += len(regions)
# possibly send a preview image
self.send_preview(img_scan)
if self.camera_settings.roll_stabilised:
roll=0
else:
roll=None
pos = self.get_plane_position(frame_time, roll=roll)
if pos is not None:
self.posmapping[str(frame_time)] = pos
# this adds the latlon field to the regions (georeferencing)
for r in regions:
r.latlon = cuav_util.gps_position_from_image_region(r, pos, w, h, altitude=None, C=self.c_params)
if self.joelog:
self.log_joe_position(pos, frame_time, regions)
# filter out any regions outside the target radius
if self.camera_settings.target_radius > 0 and pos is not None:
regions = cuav_region.filter_radius(regions,
(self.camera_settings.target_latitude,
self.camera_settings.target_longitude),
self.camera_settings.target_radius)
# filter out any regions outside the boundary
if self.boundary_polygon:
regions = cuav_region.filter_boundary(regions, self.boundary_polygon, pos)
#filter by minscore
regions = cuav_region.filter_regions(img_scan, regions, min_score=self.camera_settings.minscore,
filter_type=self.camera_settings.filter_type)
high_score = 1
for r in regions:
if r.score > high_score:
high_score = r.score
if len(regions) > 0 and pos is not None:
for r in regions:
self.lz.checkaddregion(r, pos)
try:
lzresult = self.lz.calclandingzone()
except Exception as ex:
print("calclandingzone failed: ", ex)
continue
if lzresult:
self.transmit_queue.put((lzresult, 100000, None))
if self.msend:
self.transmit_queue.put((lzresult, 100000, 'msend'))
if len(regions) > 0 and self.camera_settings.transmit:
# send a region message with thumbnails to the ground station
thumb_img = cuav_region.CompositeThumbnail(img_scan, regions,
thumb_size=self.camera_settings.thumbsize)
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90]
(result, thumb) = cv2.imencode('.jpg', thumb_img, encode_param)
pkt = cuav_command.ThumbPacket(frame_time, regions, thumb, pos)
if self.transmit_queue.qsize() < 100:
self.transmit_queue.put((pkt, None, None))
if self.msend is not None and high_score >= self.camera_settings.m_minscore:
self.transmit_queue.put((pkt, None, 'msend'))
else:
self.send_message("Warning: image Tx queue too long")
print("Warning: image Tx queue too long")
def get_plane_position(self, frame_time,roll=None):
'''get a MavPosition object for the planes position if possible'''
try:
pos = self.mpos.position(frame_time, 0, roll=roll, maxroll=self.camera_settings.roll_limit)
return pos
except mav_position.MavInterpolatorException as e:
print(str(e))
return None
def log_joe_position(self, pos, frame_time, regions, filename=None, thumb_filename=None):
'''add to joe.log if possible, returning a list of (lat,lon) tuples
for the positions of the identified image regions'''
return self.joelog.add_regions(frame_time, regions, pos, filename, thumb_filename)
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def transmit_threadfunc(self):
'''thread for image and message transmit to camera_ground
in addition to reading commands from the camera_ground'''
self.start_aircraft_bsend()
self.spacewarning = False
while (not self.unload_event.wait(0.05)) or self.airstart_triggered:
for bsnd in self.bsend:
bsnd.tick(packet_count=1000, max_queue=self.camera_settings.maxqueue)
try:
self.check_commands(bsnd)
except Exception as ex:
print("Failed command", ex)
if self.msend is not None:
self.msend.tick(packet_count=1000, max_queue=self.camera_settings.m_maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
#check remaining disk space and warn user if required
try:
stat = os.statvfs(os.path.dirname(self.camera_settings.imagefile))
if not self.spacewarning and stat.f_bfree*stat.f_bsize < 20971520:
self.send_message("Warning: <200Mb disk space left on cuav_air")
self.spacewarning = True
except OSError:
pass
while not self.transmit_queue.empty():
(pkt, priority, linktosend) = self.transmit_queue.get()
linktosend = self.get_bsend(linktosend)
self.send_object(pkt, priority, linktosend)
#update the stats
self.xmit_queue = []
self.efficiency = []
self.bandwidth_used = []
self.rtt_estimate = []
for bsnd in self.bsend:
self.xmit_queue.append(bsnd.sendq_size())
self.efficiency.append(bsnd.get_efficiency())
self.bandwidth_used.append(bsnd.get_bandwidth_used())
self.rtt_estimate.append(bsnd.get_rtt_estimate())
if self.msend is not None:
self.xmit_queue.append(self.msend.sendq_size())
self.efficiency.append(self.msend.get_efficiency())
self.bandwidth_used.append(self.msend.get_bandwidth_used())
self.rtt_estimate.append(self.msend.get_rtt_estimate())
def send_image(self, img, frame_time, priority, pos, linktosend):
'''send an image object to the GCS'''
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), self.camera_settings.qualitysend]
(result, jpeg) = cv2.imencode('.jpg', img, encode_param)
# keep filtered image size
self.jpeg_size = 0.95 * self.jpeg_size + 0.05 * len(jpeg)
pkt = cuav_command.ImagePacket(frame_time, jpeg, pos, priority)
self.transmit_queue.put((pkt, priority, self.get_bsend_index(linktosend)))
def send_preview(self, img):
'''send a preview image object to the GCS'''
if not self.camera_settings.preview or self.transmit_queue.qsize() > 3:
# only send when link is nearly idle
return
if self.scan_count % self.camera_settings.previewfreq != 0:
return
scale = 1.0 / self.camera_settings.previewscale
small_img = cv2.resize(img, (0,0), fx=scale, fy=scale)
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), self.camera_settings.previewquality]
(result, jpeg) = cv2.imencode('.jpg', small_img, encode_param)
pkt = cuav_command.PreviewPacket(jpeg)
self.transmit_queue.put((pkt, 1, None))
def send_file(self, filename):
'''send a file over all links'''
try:
contents = open(filename).read()
filename = os.path.basename(filename)
except Exception:
return
pkt = cuav_command.FilePacket(filename, contents)
# send over all links
self.transmit_queue.put((pkt, 20000, None))
if self.msend:
self.transmit_queue.put((pkt, 20000, 'msend'))
def start_aircraft_bsend(self):
'''start bsend for aircraft side'''
if len(self.bsend) == 0:
for lnk in self.camera_settings.gcs_address.split(','):
try:
[remoteip, remoteport, localport, bw] = lnk.split(':')
newbsnd = block_xmit.BlockSender(bandwidth=int(bw), debug=False,
dest_ip=remoteip, dest_port=int(remoteport), port=int(localport))
self.bsend.append(newbsnd)
except:
print("Bad GCS endpoint (must be remIP:remport:localport:bw): " + str(lnk))
pass
if self.msend is None:
self.msocket = cuav_command.MavSocket(self.mpstate.mav_master[0])
self.msend = block_xmit.BlockSender(mss=96, sock=self.msocket, dest_ip='mavlink', dest_port=0, backlog=5, debug=False)
self.msend.set_bandwidth(self.camera_settings.m_bandwidth)
def start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.running = False
self.unload_event.set()
if self.capture_thread is not None:
self.capture_thread.join(1.0)
self.scan_thread.join(1.0)
self.transmit_thread.join(1.0)
print('camera unload OK')
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = pickle.loads(buf)
if obj == None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.ImageRequest):
self.handle_image_request(obj, bsend)
if isinstance(obj, cuav_command.ChangeCameraSetting):
self.camera_settings.set(obj.name, obj.value)
self.camera_settings_callback(obj)
if isinstance(obj, cuav_command.ChangeImageSetting):
self.image_settings.set(obj.name, obj.value)
self.image_settings_callback(obj)
if isinstance(obj, cuav_command.CommandPacket):
self.handle_command_packet(obj, bsend)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if self.mpstate.status.watch in ["camera","queue"] and time.time() > self.last_watch+1:
self.last_watch = time.time()
self.cmd_camera(["status" if self.mpstate.status.watch == "camera" else "queue"])
# update position interpolator
self.mpos.add_msg(m)
if m.get_type() == 'SYSTEM_TIME' and self.camera_settings.clock_sync and self.capture_thread is not None:
# optionally sync system clock on the capture side
self.sync_gps_clock(m.time_unix_usec)
if m.get_type() == 'VFR_HUD' and self.airstart_triggered and not self.running:
#if the airstart is triggered and we're flying, then start capture
if m.airspeed > self.camera_settings.minspeed or m.groundspeed > self.camera_settings.minspeed:
self.running = True
self.joelog = cuav_joe.JoeLog(os.path.join(os.path.dirname(self.camera_settings.imagefile), 'joe_air.log'), append=self.continue_mode)
self.capture_thread = self.start_thread(self.capture_threadfunc)
self.scan_thread = self.start_thread(self.scan_threadfunc)
self.send_message("Started cuav running")
print("Started cuav running")
if m.get_type() == "TERRAIN_REPORT":
self.terrain_alt = m.current_height
if m.get_type() == "HEARTBEAT" and m.type != mavutil.mavlink.MAV_TYPE_GCS:
was_armed = self.is_armed
self.is_armed = (m.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
if not self.is_armed and was_armed:
stopfile = self.camera_settings.imagefile + ".stop"
if not os.path.exists(stopfile):
print("Creating stop file")
open(stopfile,"w").write("")
if m.get_type() in [ 'DATA16', 'DATA32', 'DATA64', 'DATA96' ]:
if self.msocket is not None:
self.msocket.incoming.append(m)
def sync_gps_clock(self, time_usec):
'''sync system clock with GPS time'''
if time_usec == 0:
# no GPS lock
return
if os.geteuid() != 0:
# can only do this as root
return
time_seconds = time_usec*1.0e-6
if self.have_set_gps_time and abs(time_seconds - time.time()) < 10:
# only change a 2nd time if time is off by 10 seconds
return
t1 = time.time()
cuav_util.set_system_clock(time_seconds)
t2 = time.time()
print("Changed system time by %.2f seconds" % (t2-t1))
self.have_set_gps_time = True
def handle_image_request(self, obj, bsend):
'''handle ImageRequest from GCS. Only sends to the requesting GCS'''
strname = str(obj.frame_time)
if not strname in self.imagefilenamemapping:
print("Unknown image %s" % strname)
return
filename = self.imagefilenamemapping[strname]
if not os.path.exists(filename):
print("No file: %s" % filename)
return
try:
img = cv2.imread(filename, -1)
except Exception:
return
if img is None:
print("Bad image: %s" % filename)
return
if not obj.fullres:
im_small = cv2.resize(img, (0,0), fx=0.5, fy=0.5)
img = im_small
print("Sending image %s" % filename)
pos = self.posmapping.get(str(obj.frame_time), None)
self.send_image(img, obj.frame_time, 10000, pos, bsend)
def camera_settings_callback(self, setting):
'''called on a changed camera setting'''
pkt = cuav_command.ChangeCameraSetting(setting.name, setting.value)
self.transmit_queue.put((pkt, None, None))
def image_settings_callback(self, setting):
'''called on a changed image setting'''
pkt = cuav_command.ChangeImageSetting(setting.name, setting.value)
self.transmit_queue.put((pkt, None, None))
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat(self.capture_count)
for bidx in range(len(self.bsend)):
self.transmit_queue.put((pkt, None, bidx))
if self.msend is not None:
self.transmit_queue.put((pkt, None, 'msend'))
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
self.transmit_queue.put((pkt, 100, None))
if self.msend is not None:
self.transmit_queue.put((pkt, 100, 'msend'))
def send_object_complete(self, obj, bsend):
'''called on complete of an send_object, cancelling send on other links'''
if obj.blockid is not None:
for bsnd in self.bsend:
if bsend != bsnd:
bsnd.cancel(obj.blockid)
if self.msend is not None:
self.msend.cancel(obj.blockid)
def send_object(self, obj, priority=None, linktosend=None):
'''send an object to all links if linktosend is none
otherwise just send to the specified link'''
try:
buf = pickle.dumps(obj, pickle.HIGHEST_PROTOCOL)
except Exception as ex:
print("dump failed: ", ex)
return
if priority is None:
priority = 10000
#only send if the queue is not clogged
if not linktosend:
for bsnd in self.bsend:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf, priority=priority, callback=functools.partial(self.send_object_complete, obj, bsnd))
else:
if linktosend == self.msend:
qsize = self.camera_settings.m_maxqueue
else:
qsize = self.camera_settings.maxqueue
if linktosend.sendq_size() < qsize:
obj.blockid = linktosend.send(buf, priority=priority, callback=functools.partial(self.send_object_complete, obj, linktosend))
def handle_command_packet(self, obj, bsend):
'''handle CommandPacket from other end'''
stdout_saved = sys.stdout
buf = StringIO()
sys.stdout = buf
self.mpstate.functions.process_stdin(obj.command, immediate=True)
sys.stdout = stdout_saved
if str(buf.getvalue().strip()):
pkt = cuav_command.CommandResponse(str(buf.getvalue()).strip())
self.transmit_queue.put((pkt, None, self.get_bsend_index(bsend)))
class CameraAirModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraAirModule, self).__init__(mpstate,
"camera_air",
"cuav camera control (air)",
public=True)
self.running = False
self.unload_event = threading.Event()
self.unload_event.clear()
self.capture_thread = None
self.scan_thread = None
self.transmit_thread = None
self.airstart_triggered = False
self.terrain_alt = None
self.handled_timestamps = {}
self.imagefilenamemapping = {}
self.posmapping = {}
self.is_armed = True
self.lz = cuav_landingregion.LandingZone()
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.camera_settings = MPSettings([
MPSetting('roll_stabilised', bool, False, 'Roll Stabilised'),
MPSetting('roll_limit', float, 0, 'Roll stabilisation limit'),
MPSetting('minspeed', int, 20,
'For airstart, minimum speed for capture to start'),
MPSetting('minalt',
int,
30,
'MinAltitude of images',
range=(0, 10000),
increment=1),
MPSetting('rotate180',
bool,
False,
'rotate images by 180',
tab='Capture2'),
MPSetting('ignoretimestamps',
bool,
False,
'Ignore image timestamps',
tab='Capture2'),
MPSetting('camparms',
str,
None,
'camera parameters file (json) in cuav package',
tab='Imaging'),
MPSetting(
'imagefile', str, None, 'latest captured image',
tab='Imaging'),
MPSetting('filter_type',
str,
'simple',
'Filter Type',
choice=['simple'],
tab='Imaging'),
MPSetting(
'blue_emphasis', bool, False, 'BlueEmphasis', tab='Imaging'),
MPSetting('use_capture_time',
bool,
True,
'Use Capture Time (false for sim)',
tab='Simulation'),
MPSetting('target_latitude',
float,
0,
'filter detected images to latitude',
tab='Filter to Location'),
MPSetting('target_longitude',
float,
0,
'filter detected images to longitude',
tab='Filter to Location'),
MPSetting('target_radius',
float,
0,
'filter detected images to radius',
tab='Filter to Location'),
MPSetting(
'gcs_address',
str,
"",
'GCS Addresses in RemIP:RemPort:LocalPort:Bandwidth format (127.0.0.1:1440:1234:45, ...)',
tab='GCS'),
MPSetting('qualitysend',
int,
90,
'Compression Quality for send',
range=(1, 100),
increment=1,
tab='GCS'),
MPSetting('transmit',
bool,
True,
'Transmit Enable for thumbnails',
tab='GCS'),
MPSetting('maxqueue', int, 50, 'Maximum images queue', tab='GCS'),
MPSetting('thumbsize',
int,
60,
'Thumbnail Size',
range=(10, 200),
increment=1),
MPSetting('minscore',
int,
1000,
'Min Score to pass detection',
range=(0, 100000),
increment=1,
tab='Imaging'),
MPSetting('clock_sync', bool, False, 'GPS Clock Sync'),
MPSetting('m_minscore',
int,
20000,
'Min Score to pass detection on mavlink',
range=(0, 100000),
increment=1,
tab='Imaging'),
MPSetting('m_bandwidth',
int,
500,
'max bandwidth on mavlink',
increment=1,
tab='GCS'),
MPSetting('m_maxqueue',
int,
5,
'Maximum images queue for mavlink',
tab='GCS'),
MPSetting('preview',
bool,
True,
'enable camera preview window',
tab='Imaging'),
MPSetting('previewquality',
int,
40,
'Compression Quality for preview',
range=(1, 100),
increment=1,
tab='Imaging'),
MPSetting('previewscale',
int,
5,
'preview downscaling',
range=(1, 10),
increment=1,
tab='Imaging'),
MPSetting('previewfreq',
int,
4,
'preview image frequency',
range=(1, 10),
increment=1,
tab='Imaging'),
],
title='Camera Settings')
self.image_settings = MPSettings([
MPSetting('MinRegionArea',
float,
0.15,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionArea',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1,
tab='Image Processing'),
MPSetting('MinRegionSize',
float,
0.2,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionSize',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1,
tab='Image Processing'),
MPSetting('MaxRarityPct',
float,
0.02,
range=(0, 100),
increment=0.01,
digits=2,
tab='Image Processing'),
MPSetting('RegionMergeSize',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1,
tab='Image Processing'),
],
title='Image Settings')
self.capture_count = 0
self.scan_count = 0
self.error_count = 0
self.error_msg = None
self.region_count = 0
self.scan_fps = 0
self.scan_queue = multiproc.Queue()
self.transmit_queue = multiproc.Queue()
self.have_set_gps_time = False
self.c_params = None
self.jpeg_size = 0
self.xmit_queue = []
self.efficiency = []
self.last_watch = 0
self.boundary = None
self.boundary_polygon = None
self.bandwidth_used = []
self.rtt_estimate = []
self.bsend = [] #note this is an array of bsends
# msend is a BlockSender over MAVLink
self.msocket = None
self.msend = None
self.last_heartbeat = time.time()
self.mpos = mav_position.MavInterpolator(backlog=500, gps_lag=0.0)
self.joelog = None #cuav_joe.JoeLog(os.path.join(self.settings.imagefile, '..', 'joe.log'), append=self.continue_mode)
self.add_command(
'camera', self.cmd_camera, 'camera control',
['<start|stop|status|boundary|airstart>', 'set (CAMERASETTING)'])
self.add_completion_function('(CAMERASETTING)',
self.camera_settings.completion)
# prevent loopback of messages
for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
self.module('link').no_fwd_types.add(mtype)
print("camera initialised")
def get_bsend(self, bsnd):
'''get a bsend object, given a tag name'''
if bsnd is None:
return None
if bsnd == 'msend':
return self.msend
return self.bsend[bsnd]
def get_bsend_index(self, bsnd):
'''get a bsend index from a bsend object. This avoids pickling a block xmit object'''
if bsnd is None or isinstance(bsnd, int) or bsnd == 'msend':
return bsnd
if bsnd == self.msend:
return 'msend'
return self.bsend.index(bsnd)
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <start|airstart|stop|status|queue|set>"
if len(args) == 0:
print(usage)
return
if args[0] == "start":
self.capture_count = 0
self.error_count = 0
self.error_msg = None
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params " +
str(self.camera_settings.camparms))
return
if self.running == False:
self.running = True
self.joelog = cuav_joe.JoeLog(os.path.join(
os.path.dirname(self.camera_settings.imagefile),
'joe_air.log'),
append=self.continue_mode)
self.capture_thread = self.start_thread(
self.capture_threadfunc)
self.scan_thread = self.start_thread(self.scan_threadfunc)
self.transmit_thread = self.start_thread(
self.transmit_threadfunc)
time.sleep(0.1)
self.send_message("Started cuav running")
print("Started cuav running")
else:
self.send_message("cuav already running")
print("cuav already running")
elif args[0] == "stop":
self.running = False
self.airstart_triggered = False
print("Stopped cuav")
self.send_message("Stopped cuav")
elif args[0] == "status":
ret = "Cap imgs:%u err:%u scan:%u regions:%u jsize:%.0f xmitq:%s sq:%u eff:%s" % (
self.capture_count, self.error_count, self.scan_count,
self.region_count, self.jpeg_size, self.xmit_queue,
self.scan_queue.qsize(), self.efficiency)
print(ret)
self.send_message(ret)
elif args[0] == "queue":
ret = "scan %u transmit %u eff %s bw %s rtt %s" % (
self.scan_queue.qsize(), self.transmit_queue.qsize(),
self.efficiency, self.bandwidth_used, self.rtt_estimate)
print(ret)
elif args[0] == "set":
self.camera_settings.command(args[1:])
elif args[0] == "airstart":
#just keep the block xmit going for now
self.capture_count = 0
self.error_count = 0
self.error_msg = None
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params " +
str(self.camera_settings.camparms))
return
if self.airstart_triggered == False:
self.airstart_triggered = True
self.joelog = cuav_joe.JoeLog(os.path.join(
os.path.dirname(self.camera_settings.imagefile),
'joe_air.log'),
append=self.continue_mode)
self.transmit_thread = self.start_thread(
self.transmit_threadfunc)
time.sleep(0.1)
self.send_message("cuav airstart ready")
print("cuav airstart ready")
else:
self.send_message("cuav airstart already running")
print("cuav airstart already running")
else:
print(usage)
def check_camera_parms(self):
'''check for change in camera parameters'''
#dir is rel to this python file:
if self.camera_settings.camparms is None:
return False
camfiletxt = pkg_resources.resource_string(
"cuav", self.camera_settings.camparms)
if sys.version_info.major >= 3:
camfiletxt = camfiletxt.decode('utf-8')
try:
self.c_params = CameraParams.fromstring(camfiletxt)
return True
except:
return False
def capture_threadfunc(self):
'''image capture thread, via monitoring the
link for changed linked filenames'''
prev_image = None
self.scan_queue = multiproc.Queue()
while not self.unload_event.wait(0.05):
try:
filename = os.path.realpath(self.camera_settings.imagefile)
if not self.camera_settings.ignoretimestamps:
filetime = cuav_util.parse_frame_time(filename)
else:
filetime = float(time.time())
except Exception:
filename = None
pass
#ensure all items are valid and the queue isn't overfilled > 100
if filename != None and prev_image != filename and filetime != None and self.scan_queue.qsize(
) < 100:
self.scan_queue.put((filetime, filename))
self.imagefilenamemapping[str(filetime)] = filename
self.capture_count += 1
prev_image = filename
if self.is_armed:
stopfile = self.camera_settings.imagefile + ".stop"
if os.path.exists(stopfile):
print("Removing stopfile")
os.unlink(self.camera_settings.imagefile + ".stop")
def scan_threadfunc(self):
'''image scanning thread'''
while not self.unload_event.wait(0.05):
if self.scan_queue.empty():
continue
(frame_time, im) = self.scan_queue.get()
scan_parms = {}
for name in self.image_settings.list():
scan_parms[name] = self.image_settings.get(name)
scan_parms['BlueEmphasis'] = float(
self.camera_settings.blue_emphasis)
if self.terrain_alt is not None:
altitude = self.terrain_alt
if altitude < self.camera_settings.minalt:
altitude = self.camera_settings.minalt
scan_parms['MetersPerPixel'] = cuav_util.pixel_width(
altitude, self.c_params.xresolution, self.c_params.lens,
self.c_params.sensorwidth)
t1 = time.time()
try:
img_scan = cv2.imread(im, -1)
except Exception:
continue
if img_scan is None:
continue
(h, w) = img_scan.shape[:2]
if self.camera_settings.rotate180:
M = cv2.getRotationMatrix2D(center, angle180, scale)
img_scan = cv2.warpAffine(img_scan, M, (w, h))
im_numpy = numpy.ascontiguousarray(img_scan)
regions = scanner.scan(im_numpy, scan_parms)
regions = cuav_region.RegionsConvert(
regions, cuav_util.image_shape(img_scan),
cuav_util.image_shape(img_scan))
t2 = time.time()
self.scan_fps = 1.0 / (t2 - t1)
self.scan_count += 1
regions = cuav_region.filter_regions(
img_scan,
regions,
min_score=self.camera_settings.minscore,
filter_type=self.camera_settings.filter_type)
self.region_count += len(regions)
# possibly send a preview image
self.send_preview(img_scan)
if self.camera_settings.roll_stabilised:
roll = 0
else:
roll = None
pos = self.get_plane_position(frame_time, roll=roll)
if pos is not None:
self.posmapping[str(frame_time)] = pos
# this adds the latlon field to the regions (georeferencing)
for r in regions:
r.latlon = cuav_util.gps_position_from_image_region(
r, pos, w, h, altitude=None, C=self.c_params)
if self.joelog:
self.log_joe_position(pos, frame_time, regions)
# filter out any regions outside the target radius
if self.camera_settings.target_radius > 0 and pos is not None:
regions = cuav_region.filter_radius(
regions, (self.camera_settings.target_latitude,
self.camera_settings.target_longitude),
self.camera_settings.target_radius)
# filter out any regions outside the boundary
if self.boundary_polygon:
regions = cuav_region.filter_boundary(regions,
self.boundary_polygon,
pos)
#filter by minscore
regions = cuav_region.filter_regions(
img_scan,
regions,
min_score=self.camera_settings.minscore,
filter_type=self.camera_settings.filter_type)
high_score = 1
for r in regions:
if r.score > high_score:
high_score = r.score
if len(regions) > 0 and pos is not None:
for r in regions:
self.lz.checkaddregion(r, pos)
try:
lzresult = self.lz.calclandingzone()
except Exception as ex:
print("calclandingzone failed: ", ex)
continue
if lzresult:
self.transmit_queue.put((lzresult, 100000, None))
if self.msend:
self.transmit_queue.put((lzresult, 100000, 'msend'))
if len(regions) > 0 and self.camera_settings.transmit:
# send a region message with thumbnails to the ground station
thumb_img = cuav_region.CompositeThumbnail(
img_scan,
regions,
thumb_size=self.camera_settings.thumbsize)
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90]
(result, thumb) = cv2.imencode('.jpg', thumb_img, encode_param)
pkt = cuav_command.ThumbPacket(frame_time, regions, thumb, pos)
if self.transmit_queue.qsize() < 100:
self.transmit_queue.put((pkt, None, None))
if self.msend is not None and high_score >= self.camera_settings.m_minscore:
self.transmit_queue.put((pkt, None, 'msend'))
else:
self.send_message("Warning: image Tx queue too long")
print("Warning: image Tx queue too long")
def get_plane_position(self, frame_time, roll=None):
'''get a MavPosition object for the planes position if possible'''
try:
pos = self.mpos.position(frame_time,
0,
roll=roll,
maxroll=self.camera_settings.roll_limit)
return pos
except mav_position.MavInterpolatorException as e:
print(str(e))
return None
def log_joe_position(self,
pos,
frame_time,
regions,
filename=None,
thumb_filename=None):
'''add to joe.log if possible, returning a list of (lat,lon) tuples
for the positions of the identified image regions'''
return self.joelog.add_regions(frame_time, regions, pos, filename,
thumb_filename)
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def transmit_threadfunc(self):
'''thread for image and message transmit to camera_ground
in addition to reading commands from the camera_ground'''
self.start_aircraft_bsend()
self.spacewarning = False
while (not self.unload_event.wait(0.05)) or self.airstart_triggered:
for bsnd in self.bsend:
bsnd.tick(packet_count=1000,
max_queue=self.camera_settings.maxqueue)
try:
self.check_commands(bsnd)
except Exception as ex:
print("Failed command", ex)
if self.msend is not None:
self.msend.tick(packet_count=1000,
max_queue=self.camera_settings.m_maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
#check remaining disk space and warn user if required
try:
stat = os.statvfs(
os.path.dirname(self.camera_settings.imagefile))
if not self.spacewarning and stat.f_bfree * stat.f_bsize < 20971520:
self.send_message(
"Warning: <200Mb disk space left on cuav_air")
self.spacewarning = True
except OSError:
pass
while not self.transmit_queue.empty():
(pkt, priority, linktosend) = self.transmit_queue.get()
linktosend = self.get_bsend(linktosend)
self.send_object(pkt, priority, linktosend)
#update the stats
self.xmit_queue = []
self.efficiency = []
self.bandwidth_used = []
self.rtt_estimate = []
for bsnd in self.bsend:
self.xmit_queue.append(bsnd.sendq_size())
self.efficiency.append(bsnd.get_efficiency())
self.bandwidth_used.append(bsnd.get_bandwidth_used())
self.rtt_estimate.append(bsnd.get_rtt_estimate())
if self.msend is not None:
self.xmit_queue.append(self.msend.sendq_size())
self.efficiency.append(self.msend.get_efficiency())
self.bandwidth_used.append(self.msend.get_bandwidth_used())
self.rtt_estimate.append(self.msend.get_rtt_estimate())
def send_image(self, img, frame_time, priority, pos, linktosend):
'''send an image object to the GCS'''
encode_param = [
int(cv2.IMWRITE_JPEG_QUALITY), self.camera_settings.qualitysend
]
(result, jpeg) = cv2.imencode('.jpg', img, encode_param)
# keep filtered image size
self.jpeg_size = 0.95 * self.jpeg_size + 0.05 * len(jpeg)
pkt = cuav_command.ImagePacket(frame_time, jpeg, pos, priority)
self.transmit_queue.put(
(pkt, priority, self.get_bsend_index(linktosend)))
def send_preview(self, img):
'''send a preview image object to the GCS'''
if not self.camera_settings.preview or self.transmit_queue.qsize() > 3:
# only send when link is nearly idle
return
if self.scan_count % self.camera_settings.previewfreq != 0:
return
scale = 1.0 / self.camera_settings.previewscale
small_img = cv2.resize(img, (0, 0), fx=scale, fy=scale)
encode_param = [
int(cv2.IMWRITE_JPEG_QUALITY), self.camera_settings.previewquality
]
(result, jpeg) = cv2.imencode('.jpg', small_img, encode_param)
pkt = cuav_command.PreviewPacket(jpeg)
self.transmit_queue.put((pkt, 1, None))
def send_file(self, filename):
'''send a file over all links'''
try:
contents = open(filename).read()
filename = os.path.basename(filename)
except Exception:
return
pkt = cuav_command.FilePacket(filename, contents)
# send over all links
self.transmit_queue.put((pkt, 20000, None))
if self.msend:
self.transmit_queue.put((pkt, 20000, 'msend'))
def start_aircraft_bsend(self):
'''start bsend for aircraft side'''
if len(self.bsend) == 0:
for lnk in self.camera_settings.gcs_address.split(','):
try:
[remoteip, remoteport, localport, bw] = lnk.split(':')
newbsnd = block_xmit.BlockSender(bandwidth=int(bw),
debug=False,
dest_ip=remoteip,
dest_port=int(remoteport),
port=int(localport))
self.bsend.append(newbsnd)
except:
print(
"Bad GCS endpoint (must be remIP:remport:localport:bw): "
+ str(lnk))
pass
if self.msend is None:
self.msocket = cuav_command.MavSocket(self.mpstate.mav_master[0])
self.msend = block_xmit.BlockSender(mss=96,
sock=self.msocket,
dest_ip='mavlink',
dest_port=0,
backlog=5,
debug=False)
self.msend.set_bandwidth(self.camera_settings.m_bandwidth)
def start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.running = False
self.unload_event.set()
if self.capture_thread is not None:
self.capture_thread.join(1.0)
self.scan_thread.join(1.0)
self.transmit_thread.join(1.0)
print('camera unload OK')
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = pickle.loads(buf)
if obj == None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.ImageRequest):
self.handle_image_request(obj, bsend)
if isinstance(obj, cuav_command.ChangeCameraSetting):
self.camera_settings.set(obj.name, obj.value)
self.camera_settings_callback(obj)
if isinstance(obj, cuav_command.ChangeImageSetting):
self.image_settings.set(obj.name, obj.value)
self.image_settings_callback(obj)
if isinstance(obj, cuav_command.CommandPacket):
self.handle_command_packet(obj, bsend)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if self.mpstate.status.watch in [
"camera", "queue"
] and time.time() > self.last_watch + 1:
self.last_watch = time.time()
self.cmd_camera([
"status" if self.mpstate.status.watch == "camera" else "queue"
])
# update position interpolator
self.mpos.add_msg(m)
if m.get_type(
) == 'SYSTEM_TIME' and self.camera_settings.clock_sync and self.capture_thread is not None:
# optionally sync system clock on the capture side
self.sync_gps_clock(m.time_unix_usec)
if m.get_type(
) == 'VFR_HUD' and self.airstart_triggered and not self.running:
#if the airstart is triggered and we're flying, then start capture
if m.airspeed > self.camera_settings.minspeed or m.groundspeed > self.camera_settings.minspeed:
self.running = True
self.joelog = cuav_joe.JoeLog(os.path.join(
os.path.dirname(self.camera_settings.imagefile),
'joe_air.log'),
append=self.continue_mode)
self.capture_thread = self.start_thread(
self.capture_threadfunc)
self.scan_thread = self.start_thread(self.scan_threadfunc)
self.send_message("Started cuav running")
print("Started cuav running")
if m.get_type() == "TERRAIN_REPORT":
self.terrain_alt = m.current_height
if m.get_type(
) == "HEARTBEAT" and m.type != mavutil.mavlink.MAV_TYPE_GCS:
was_armed = self.is_armed
self.is_armed = (m.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
if not self.is_armed and was_armed:
stopfile = self.camera_settings.imagefile + ".stop"
if not os.path.exists(stopfile):
print("Creating stop file")
open(stopfile, "w").write("")
if m.get_type() in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
if self.msocket is not None:
self.msocket.incoming.append(m)
def sync_gps_clock(self, time_usec):
'''sync system clock with GPS time'''
if time_usec == 0:
# no GPS lock
return
if os.geteuid() != 0:
# can only do this as root
return
time_seconds = time_usec * 1.0e-6
if self.have_set_gps_time and abs(time_seconds - time.time()) < 10:
# only change a 2nd time if time is off by 10 seconds
return
t1 = time.time()
cuav_util.set_system_clock(time_seconds)
t2 = time.time()
print("Changed system time by %.2f seconds" % (t2 - t1))
self.have_set_gps_time = True
def handle_image_request(self, obj, bsend):
'''handle ImageRequest from GCS. Only sends to the requesting GCS'''
strname = str(obj.frame_time)
if not strname in self.imagefilenamemapping:
print("Unknown image %s" % strname)
return
filename = self.imagefilenamemapping[strname]
if not os.path.exists(filename):
print("No file: %s" % filename)
return
try:
img = cv2.imread(filename, -1)
except Exception:
return
if img is None:
print("Bad image: %s" % filename)
return
if not obj.fullres:
im_small = cv2.resize(img, (0, 0), fx=0.5, fy=0.5)
img = im_small
print("Sending image %s" % filename)
pos = self.posmapping.get(str(obj.frame_time), None)
self.send_image(img, obj.frame_time, 10000, pos, bsend)
def camera_settings_callback(self, setting):
'''called on a changed camera setting'''
pkt = cuav_command.ChangeCameraSetting(setting.name, setting.value)
self.transmit_queue.put((pkt, None, None))
def image_settings_callback(self, setting):
'''called on a changed image setting'''
pkt = cuav_command.ChangeImageSetting(setting.name, setting.value)
self.transmit_queue.put((pkt, None, None))
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat(self.capture_count)
for bidx in range(len(self.bsend)):
self.transmit_queue.put((pkt, None, bidx))
if self.msend is not None:
self.transmit_queue.put((pkt, None, 'msend'))
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
self.transmit_queue.put((pkt, 100, None))
if self.msend is not None:
self.transmit_queue.put((pkt, 100, 'msend'))
def send_object_complete(self, obj, bsend):
'''called on complete of an send_object, cancelling send on other links'''
if obj.blockid is not None:
for bsnd in self.bsend:
if bsend != bsnd:
bsnd.cancel(obj.blockid)
if self.msend is not None:
self.msend.cancel(obj.blockid)
def send_object(self, obj, priority=None, linktosend=None):
'''send an object to all links if linktosend is none
otherwise just send to the specified link'''
try:
buf = pickle.dumps(obj, pickle.HIGHEST_PROTOCOL)
except Exception as ex:
print("dump failed: ", ex)
return
if priority is None:
priority = 10000
#only send if the queue is not clogged
if not linktosend:
for bsnd in self.bsend:
if bsnd.sendq_size() < self.camera_settings.maxqueue:
obj.blockid = bsnd.send(buf,
priority=priority,
callback=functools.partial(
self.send_object_complete, obj,
bsnd))
else:
if linktosend == self.msend:
qsize = self.camera_settings.m_maxqueue
else:
qsize = self.camera_settings.maxqueue
if linktosend.sendq_size() < qsize:
obj.blockid = linktosend.send(buf,
priority=priority,
callback=functools.partial(
self.send_object_complete,
obj, linktosend))
def handle_command_packet(self, obj, bsend):
'''handle CommandPacket from other end'''
stdout_saved = sys.stdout
buf = StringIO()
sys.stdout = buf
self.mpstate.functions.process_stdin(obj.command, immediate=True)
sys.stdout = stdout_saved
if str(buf.getvalue().strip()):
pkt = cuav_command.CommandResponse(str(buf.getvalue()).strip())
self.transmit_queue.put((pkt, None, self.get_bsend_index(bsend)))
class CameraAirModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraAirModule, self).__init__(mpstate,
"camera_air",
"cuav camera control (air)",
public=True,
multi_vehicle=True)
self.running = False
self.unload_event = threading.Event()
self.unload_event.clear()
self.capture_thread = None
self.is_armed = True
self.airstart_triggered = False
self.transmit_queue = Queue.Queue()
self.capture_count = 0
self.encoder = video_encode.VideoWriter()
self.camera = picamera.PiCamera()
self.start_time = None
self.handled_timestamps = {}
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.camera_settings = MPSettings([
MPSetting('quality',
int,
90,
'Compression Quality',
range=(1, 100),
increment=1,
tab='GCS'),
MPSetting('cropX',
int,
0,
'crop X position',
range=(0, 2000),
increment=1,
tab='GCS'),
MPSetting('cropY',
int,
0,
'crop X position',
range=(0, 2000),
increment=1,
tab='GCS'),
MPSetting('cropW',
int,
0,
'crop width',
range=(0, 2000),
increment=1,
tab='GCS'),
MPSetting('cropH',
int,
0,
'crop height',
range=(0, 2000),
increment=1,
tab='GCS'),
MPSetting('clock_sync', bool, False, 'GPS Clock Sync'),
MPSetting('flipV', bool, False, 'flip vertically'),
MPSetting('flipH', bool, False, 'flip horizontally'),
MPSetting('save_images', bool, False, 'save images'),
MPSetting('min_width', int, 32, 'min delta width'),
MPSetting('m_bandwidth',
int,
2000,
'max bandwidth on mavlink',
increment=1,
tab='GCS'),
MPSetting('m_maxqueue',
int,
20,
'Maximum images queue for mavlink',
tab='GCS'),
MPSetting('minspeed', int, 20,
'For airstart, minimum speed for capture to start'),
MPSetting('minalt',
int,
30,
'MinAltitude of images',
range=(0, 10000),
increment=1),
],
title='Camera Settings')
self.msocket = None
self.msend = None
self.last_heartbeat = time.time()
self.add_command('camera', self.cmd_camera, 'camera control',
['<start|stop|status>', 'set (CAMERASETTING)'])
self.add_completion_function('(CAMERASETTING)',
self.camera_settings.completion)
# prevent loopback of messages
for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
self.module('link').no_fwd_types.add(mtype)
print("camera initialised")
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <start|airstart|stop|status|queue|set>"
if len(args) == 0:
print(usage)
return
if args[0] == "start":
if not self.running:
self.encoder.set_crop(
(self.camera_settings.cropX, self.camera_settings.cropY,
self.camera_settings.cropW, self.camera_settings.cropH))
self.capture_thread = self.start_thread(
self.capture_threadfunc)
self.transmit_thread = self.start_thread(
self.transmit_threadfunc)
time.sleep(0.1)
self.running = True
self.send_message("Started cuav running")
print("Started cuav running")
else:
self.send_message("cuav already running")
print("cuav already running")
elif args[0] == "stop":
self.running = False
self.start_time = None
print("Stopped cuav")
self.send_message("Stopped cuav")
elif args[0] == "status":
print("status....")
elif args[0] == "set":
self.camera_settings.command(args[1:])
else:
print(usage)
def cap_image_CV(self):
'''capture one image'''
s = io.BytesIO()
self.camera.capture(s, "jpeg")
s.seek(0)
data = numpy.fromstring(s.getvalue(), dtype=numpy.uint8)
img = cv2.imdecode(data, 1)
if self.camera_settings.flipV:
img = cv2.flip(img, 0)[:, :]
if self.camera_settings.flipH:
img = cv2.flip(img, 1)[:, :]
if self.camera_settings.save_images:
cv2.imwrite("img%u.jpg" % self.capture_count, img)
return img
def capture_threadfunc(self):
'''image capture thread'''
last_t = time.time()
while True:
if not self.running:
self.encoder.reset()
time.sleep(0.1)
continue
if self.is_armed:
self.encoder.reset()
self.capture_count = 0
time.sleep(1)
continue
target_t = last_t + 0.95
now = time.time()
if now < target_t:
time.sleep(target_t - now)
img = self.cap_image_CV()
now = time.time()
if self.start_time is None:
self.start_time = now
tstamp_ms = int((now - self.start_time) * 1000)
self.encoder.min_width = self.camera_settings.min_width
self.encoder.quality = self.camera_settings.quality
enc = self.encoder.add_image(img, tstamp_ms)
self.encoder.report()
if len(enc) == 0:
continue
if self.capture_count == 0:
priority = 10000
else:
priority = 9000
pkt = cuav_command.ImageDelta(tstamp_ms, enc, priority)
if self.msend:
self.transmit_queue.put((pkt, priority, self.msend))
self.capture_count += 1
def transmit_threadfunc(self):
'''thread for image and message transmit to camera_ground
in addition to reading commands from the camera_ground'''
self.start_aircraft_bsend()
self.spacewarning = False
while (not self.unload_event.wait(0.05)) or self.airstart_triggered:
if self.msend is not None:
self.msend.tick(packet_count=1000,
max_queue=self.camera_settings.m_maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
while not self.transmit_queue.empty():
(pkt, priority, linktosend) = self.transmit_queue.get()
if self.msend:
self.send_object(pkt, priority, self.msend)
#update the stats
self.xmit_queue = []
self.efficiency = []
self.bandwidth_used = []
self.rtt_estimate = []
if self.msend is not None:
self.xmit_queue.append(self.msend.sendq_size())
self.efficiency.append(self.msend.get_efficiency())
self.bandwidth_used.append(self.msend.get_bandwidth_used())
self.rtt_estimate.append(self.msend.get_rtt_estimate())
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def start_aircraft_bsend(self):
'''start bsend for aircraft side'''
if self.msend is None:
self.msocket = cuav_command.MavSocket(self.mpstate.mav_master[0])
self.msend = block_xmit.BlockSender(mss=96,
sock=self.msocket,
dest_ip='mavlink',
dest_port=0,
backlog=5,
debug=False)
self.msend.set_bandwidth(self.camera_settings.m_bandwidth)
def start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.running = False
self.unload_event.set()
if self.capture_thread is not None:
self.capture_thread.join(1.0)
self.scan_thread.join(1.0)
self.transmit_thread.join(1.0)
print('camera unload OK')
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = pickle.loads(buf)
if obj == None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.ImageRequest):
self.handle_image_request(obj, bsend)
if isinstance(obj, cuav_command.ChangeCameraSetting):
self.camera_settings.set(obj.name, obj.value)
self.camera_settings_callback(obj)
if isinstance(obj, cuav_command.ChangeImageSetting):
self.image_settings.set(obj.name, obj.value)
self.image_settings_callback(obj)
if isinstance(obj, cuav_command.CommandPacket):
self.handle_command_packet(obj, bsend)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if m.get_type() in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
if self.msocket is not None:
self.msocket.incoming.append(m)
if self.mpstate.status.watch in [
"camera", "queue"
] and time.time() > self.last_watch + 1:
self.last_watch = time.time()
self.cmd_camera([
"status" if self.mpstate.status.watch == "camera" else "queue"
])
# update position interpolator
if m.get_type(
) == 'SYSTEM_TIME' and self.camera_settings.clock_sync and self.capture_thread is not None:
# optionally sync system clock on the capture side
self.sync_gps_clock(m.time_unix_usec)
if m.get_type(
) == 'VFR_HUD' and self.airstart_triggered and not self.running:
#if the airstart is triggered and we're flying, then start capture
if m.airspeed > self.camera_settings.minspeed or m.groundspeed > self.camera_settings.minspeed:
self.running = True
self.capture_thread = self.start_thread(
self.capture_threadfunc)
self.transmit_thread = self.start_thread(
self.transmit_threadfunc)
self.send_message("Started cuav running")
print("Started cuav running")
if m.get_type(
) == "HEARTBEAT" and m.type != mavutil.mavlink.MAV_TYPE_GCS:
was_armed = self.is_armed
self.is_armed = (m.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
def sync_gps_clock(self, time_usec):
'''sync system clock with GPS time'''
if time_usec == 0:
# no GPS lock
return
if os.geteuid() != 0:
# can only do this as root
return
time_seconds = time_usec * 1.0e-6
if self.have_set_gps_time and abs(time_seconds - time.time()) < 10:
# only change a 2nd time if time is off by 10 seconds
return
t1 = time.time()
cuav_util.set_system_clock(time_seconds)
t2 = time.time()
print("Changed system time by %.2f seconds" % (t2 - t1))
self.have_set_gps_time = True
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat(self.capture_count)
if self.msend is not None:
self.transmit_queue.put((pkt, None, 'msend'))
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
if self.msend is not None:
self.transmit_queue.put((pkt, 100, 'msend'))
def send_object_complete(self, obj, bsend):
'''called on complete of an send_object, cancelling send on other links'''
if obj.blockid is not None:
if self.msend is not None:
self.msend.cancel(obj.blockid)
def send_object(self, obj, priority=None, linktosend=None):
'''send an object to all links if linktosend is none
otherwise just send to the specified link'''
try:
buf = pickle.dumps(obj, pickle.HIGHEST_PROTOCOL)
except Exception as ex:
print("dump failed: ", ex)
return
if priority is None:
priority = 10000
#only send if the queue is not clogged
if not self.msend:
return
obj.blockid = self.msend.send(buf,
priority=priority,
callback=functools.partial(
self.send_object_complete, obj,
self.msend))
def handle_command_packet(self, obj, bsend):
'''handle CommandPacket from other end'''
stdout_saved = sys.stdout
buf = StringIO()
sys.stdout = buf
self.mpstate.functions.process_stdin(obj.command, immediate=True)
sys.stdout = stdout_saved
if str(buf.getvalue().strip()):
pkt = cuav_command.CommandResponse(str(buf.getvalue()).strip())
self.transmit_queue.put((pkt, None, self.msend))
class CUAVCompanionModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CUAVCompanionModule, self).__init__(mpstate, "CUAV", "CUAV companion")
self.led_state = LED_OFF
self.led_force = None
self.led_send_time = 0
self.button_change_time = 0
self.last_attitude_ms = 0
self.last_mission_check_ms = 0
self.last_wp_move_ms = 0
self.add_command('cuavled', self.cmd_cuavled, "cuav led command", ['<red|green|flash|off|refresh>'])
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cuav_settings = MPSettings(
[ MPSetting('wp_center', int, 2, 'center search USER number'),
MPSetting('wp_start', int, 1, 'start search USER number'),
MPSetting('wp_end', int, 3, 'end search USER number'),
MPSetting('wp_land',int, 4, 'landing start USER number'),
MPSetting('lookahead_default',int, 500, 'avoidance lookahead main'),
MPSetting('lookahead_search',int, 300, 'avoidance lookahead search'),
MPSetting('margin_exc_default',int, 100, 'avoidance exc main'),
MPSetting('margin_exc_search',int, 35, 'avoidance exc margin'),
MPSetting('auto_mission',bool, True, 'enable auto mission code') ])
self.add_command('cuav', self.cmd_cuav,
'cuav companion control',
['set (CUAVSETTING)'])
self.add_completion_function('(CUAVSETTING)', self.cuav_settings.completion)
self.wp_move_count = 0
self.last_lz_latlon = None
self.last_wp_list_ms = 0
self.started_landing = False
self.updated_waypoints = False
self.last_buzzer = time.time()
def send_message(self, msg):
'''send a msg to GCS for display'''
msg = "cuav: " + msg
print(msg)
try:
cam = self.module('camera_air')
cam.send_message(msg)
except Exception as ex:
print("err: ", ex)
def cmd_cuav(self, args):
'''handle cuav commands'''
if len(args) < 1:
print("usage: cuav set ...")
return
elif args[0] == "set":
self.cuav_settings.command(args[1:])
def cmd_cuavled(self, args):
'''handle cuavled commands'''
usage = "usage: cuavled red|green|flash|off|refresh"
if len(args) == 0:
print(usage)
return
if args[0] == 'red':
self.force_leds(LED_RED)
elif args[0] == 'green':
self.force_leds(LED_GREEN)
elif args[0] == 'flash':
self.force_leds(LED_FLASH)
elif args[0] == 'off':
self.force_leds(LED_OFF)
elif args[0] == 'refresh':
self.led_force = None
self.led_state = None
self.update_led_state()
def force_leds(self, state):
self.led_force = state
self.set_leds(state)
def set_relay(self, relaynum, value):
self.master.mav.command_long_send(self.target_system,
self.target_component,
mavutil.mavlink.MAV_CMD_DO_SET_RELAY, 0,
relaynum, value,
0, 0, 0, 0, 0)
def set_leds(self, state):
'''set two LEDs via relays'''
self.ack_wait = 2
self.led_state = state
self.led_send_time = time.time()
#self.set_relay(0, state[0])
#self.set_relay(1, state[1])
pattern = [0] * 24
plen = 3
if state[2] == 'RED':
pattern[0] = 255
elif state[2] == 'GREEN':
pattern[1] = 255
elif state[2] == 'FLASH':
pattern[0] = 255
pattern[1] = 255
pattern[3] = 2 # 2Hz flash
plen = 4
self.master.mav.led_control_send(self.settings.target_system,
self.settings.target_component,
0, 0, plen, pattern)
if state == LED_FLASH:
# also play warning tune
self.master.mav.play_tune_send(self.settings.target_system,
self.settings.target_component,
'AAAAAA', '')
self.last_buzzer = time.time()
def idle_task(self):
'''run periodic tasks'''
pass
def update_led_state(self):
'''update LED state'''
if self.led_force is not None:
led_state = self.led_force
elif self.master.motors_armed():
led_state = LED_RED
elif time.time() - self.button_change_time < 60:
led_state = LED_FLASH
else:
led_state = LED_GREEN
try:
wpmod = self.module('wp')
wploader = wpmod.wploader
wpcur = self.master.messages['MISSION_CURRENT'].seq
wp = wploader.wp(wpcur)
if wp.command == mavutil.mavlink.MAV_CMD_NAV_DELAY_AIRSPACE_CLEAR:
led_state = LED_FLASH
except Exception as ex:
pass
if led_state != self.led_state:
self.set_leds(led_state)
try:
self.send_message("Changing LEDs to: %s" % led_state[2])
except Exception as ex:
pass
now = time.time()
if led_state[2] == 'FLASH' and now - self.last_buzzer > 5:
self.last_buzzer = now
self.master.mav.play_tune_send(self.settings.target_system,
self.settings.target_component,
'AAAAAA','')
def find_user_wp(self, wploader, n):
'''find a USER_ waypoint number'''
for i in range(1, wploader.count()):
wp = wploader.wp(i)
if wp.command == mavutil.mavlink.MAV_CMD_USER_1 and wp.param1 == n:
# the USER_1 waypoint is just before the waypoint to use
return i+1
return None
def update_mission(self, m):
'''update mission status'''
if not self.cuav_settings.auto_mission:
return
wpmod = self.module('wp')
wploader = wpmod.wploader
if wploader.count() < 2 and self.last_attitude_ms - self.last_wp_list_ms > 5000:
self.last_wp_list_ms = self.last_attitude_ms
wpmod.cmd_wp(["list"])
wp_start = self.find_user_wp(wploader, self.cuav_settings.wp_start)
wp_center = self.find_user_wp(wploader, self.cuav_settings.wp_center)
wp_end = self.find_user_wp(wploader, self.cuav_settings.wp_end)
if (wp_center is None or
wp_start is None or
wp_end is None):
# not configured
return
if m.seq >= wp_start and m.seq <= wp_end:
lookahead = self.cuav_settings.lookahead_search
margin_exc = self.cuav_settings.margin_exc_search
else:
lookahead = self.cuav_settings.lookahead_default
margin_exc = self.cuav_settings.margin_exc_default
v = self.mav_param.get('AVD_LOOKAHEAD', None)
if v is not None and abs(v - lookahead) > 1:
self.send_message("Set AVD_LOOKAHEAD %u" % lookahead)
self.master.param_set_send('AVD_LOOKAHEAD', lookahead)
v = self.mav_param.get('AVD_MARGIN_EXCL', None)
if v is not None and abs(v - margin_exc) > 1:
self.send_message("Set AVD_MARGIN_EXCL %u" % margin_exc)
self.master.param_set_send('AVD_MARGIN_EXCL', margin_exc)
# run every 5 seconds
if self.last_attitude_ms - self.last_mission_check_ms < 5000:
return
self.last_mission_check_ms = self.last_attitude_ms
if self.updated_waypoints:
cam = self.module('camera_air')
if wpmod.loading_waypoints:
self.send_message("listing waypoints")
wpmod.cmd_wp(["list"])
else:
self.send_message("sending waypoints")
self.updated_waypoints = False
wploader.save("newwp.txt")
cam.send_file("newwp.txt")
if self.started_landing:
# no more to do
return
if self.last_attitude_ms - self.last_wp_move_ms < 2*60*1000:
# only move waypoints every 2 minutes
return
try:
cam = self.module('camera_air')
lz = cam.lz
target_latitude = cam.camera_settings.target_latitude
target_longitude = cam.camera_settings.target_longitude
target_radius = cam.camera_settings.target_radius
except Exception:
self.send_message("target not set")
return
lzresult = lz.calclandingzone()
if lzresult is None:
return
self.send_message("lzresult nr:%u avgscore:%u" % (lzresult.numregions, lzresult.avgscore))
if lzresult.numregions < 5 and lzresult.avgscore < 20000:
# only accept short lists if they have high scores
return
(lat, lon) = lzresult.latlon
# check it is within the target radius
if target_radius > 0:
dist = cuav_util.gps_distance(lat, lon, target_latitude, target_longitude)
self.send_message("dist %u" % dist)
if dist > target_radius:
return
# don't move more than 70m from the center of the search, this keeps us
# over more of the search area, and further from the fence
max_move = target_radius
if self.wp_move_count == 0:
# don't move more than 50m from center on first move
max_move = 35
if self.wp_move_count == 1:
# don't move more than 80m from center on 2nd move
max_move = 80
if dist > max_move:
bearing = cuav_util.gps_bearing(target_latitude, target_longitude, lat, lon)
(lat, lon) = cuav_util.gps_newpos(target_latitude, target_longitude, bearing, max_move)
# we may need to fetch the wp list
if self.last_attitude_ms - self.last_wp_list_ms > 120000 or wpmod.loading_waypoints:
self.last_wp_list_ms = self.last_attitude_ms
self.send_message("fetching waypoints")
wpmod.cmd_wp(["list"])
return
self.last_wp_move_ms = self.last_attitude_ms
self.wp_move_count += 1
self.send_message("Moving search to: (%f,%f) %u" % (lat, lon, self.wp_move_count))
wpmod.cmd_wp_movemulti([wp_center, wp_start, wp_end], (lat,lon))
wp_land = self.find_user_wp(wploader, self.cuav_settings.wp_land)
if (wp_land is not None and
self.wp_move_count >= 3 and
lzresult.numregions > 10 and
self.master.flightmode == "AUTO"):
self.send_message("Starting landing")
self.master.waypoint_set_current_send(wp_land)
self.started_landing = True
self.updated_waypoints = True
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
now = time.time()
if m.get_type() == "BUTTON_CHANGE":
if m.state & 1:
time_since = max(m.time_boot_ms - m.last_change_ms,0) * 1.0e-3
self.button_change_time = time.time() - time_since
self.update_led_state()
if m.get_type() == 'HEARTBEAT':
self.update_led_state()
if m.get_type() == 'COMMAND_ACK' and m.command == mavutil.mavlink.MAV_CMD_DO_SET_RELAY and self.ack_wait > 0:
self.ack_wait -= 1
if self.ack_wait == 0:
self.send_message("LEDs updated: %s" % self.led_state[2])
if m.get_type() == 'ATTITUDE':
if m.time_boot_ms < self.last_attitude_ms:
self.send_message("time wrapped")
self.led_state = None
self.last_mission_check_ms = 0
self.last_wp_move_ms = 0
self.last_wp_list_ms = 0
self.button_change_time = 0
self.last_attitude_ms = m.time_boot_ms
if m.get_type() == 'MISSION_CURRENT':
self.update_mission(m)
class CMACModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CMACModule, self).__init__(mpstate,
"CMAC",
"CMAC Checks",
public=True)
self.rate_period = mavutil.periodic_event(1.0 / 15)
self.is_armed = False
self.done_map_menu = False
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cmac_settings = MPSettings([
MPSetting('fence_maxdist', float, 1000,
'Max FencePoint Distance from south-CMAC'),
MPSetting('wp_maxdist', float, 500,
'Max WayPoint Distance from south-CMAC'),
MPSetting('rally_maxdist', float, 200,
'Max Rally Distance from south-CMAC'),
])
self.add_completion_function('(CMACCHECKSETTING)',
self.cmac_settings.completion)
self.add_command('cmaccheck', self.cmd_cmaccheck, 'cmac check control',
['check', 'set (CMACCHECKSETTING)'])
self.last_fence_fetch = 0
self.last_mission_fetch = 0
self.last_rally_fetch = 0
self.done_heartbeat_check = 0
self.check()
# swiped from ArduPilot's common.py:
def get_distance(self, loc1, loc2):
"""Get ground distance between two locations."""
dlat = loc2.lat - loc1.lat
try:
dlong = loc2.lng - loc1.lng
except AttributeError:
dlong = loc2.lon - loc1.lon
return math.sqrt((dlat * dlat) + (dlong * dlong)) * 1.113195e5
def check_map_menu(self):
# make the initial map menu
if not mp_util.has_wxpython:
return
if self.done_map_menu:
if not self.module('map'):
self.done_map_menu = False
return
if self.module('map'):
self.menu = MPMenuSubMenu(
'CMAC',
items=[
MPMenuItem('Load foamy mission CW',
'Load foamy mission CW',
'# cmaccheck loadFoamyMissionCW'),
MPMenuItem('Load foamy mission CCW',
'Load foamy mission CCW',
'# cmaccheck loadFoamyMissionCCW'),
MPMenuItem('Load rally points', 'Load rally points',
'# cmaccheck loadRally'),
MPMenuItem('Load foamy fence', 'Load foamy fence',
'# cmaccheck loadFoamyFence'),
])
self.module('map').add_menu(self.menu)
self.done_map_menu = True
def loadRally(self):
filename = "cmac-foamy-rally.txt"
filepath = pkg_resources.resource_filename(__name__, filename)
if os.path.exists(filepath):
rallymod = self.module('rally')
rallymod.cmd_rally(["load", filepath])
def loadFoamyFence(self):
filename = "cmac-foamy-fence.txt"
filepath = pkg_resources.resource_filename(__name__, filename)
if os.path.exists(filepath):
fencemod = self.module('fence')
fencemod.cmd_fence(["load", filepath])
def loadFoamyMission(self, filename):
filepath = pkg_resources.resource_filename(__name__, filename)
wpmod = self.module('wp')
wpmod.cmd_wp(["load", filepath])
def loadFoamyMissionCW(self):
self.loadFoamyMission("cmac-foamy-mission-cw.txt")
def loadFoamyMissionCCW(self):
self.loadFoamyMission("cmac-foamy-mission-ccw.txt")
def cmd_cmaccheck(self, args):
'''handle cmaccheck commands'''
usage = 'Usage: cmaccheck <set>'
if len(args) == 0:
print(usage)
return
if args[0] == "set":
self.cmac_settings.command(args[1:])
elif args[0] == "loadFoamyMissionCW":
self.loadFoamyMissionCW()
elif args[0] == "loadFoamyMissionCCW":
self.loadFoamyMissionCCW()
elif args[0] == "loadFoamyFence":
self.loadFoamyFence()
elif args[0] == "loadRally":
self.loadRally()
elif args[0] == "check":
self.check()
else:
print(usage)
return
def whinge(self, message):
self.console.writeln("CMAC: %s" % (message, ))
def check_parameters(self):
'''check key parameters'''
want_values = {
"FENCE_ACTION": 4,
"FENCE_MAXALT": 80,
"THR_FAILSAFE": 1,
"FS_SHORT_ACTN": 0,
"FS_LONG_ACTN": 1,
}
for key in want_values.keys():
want = want_values[key]
got = self.mav_param.get(key, None)
if got is None:
self.whinge("No param %s" % key)
return False
if got != want:
self.whinge('%s should be %f (not %s)' % (key, want, got))
return False
return True
def idle_task(self):
'''run periodic tasks'''
self.check_map_menu()
def check_fence_location(self):
fencemod = self.module('fence')
if fencemod is None:
self.whinge("Fence module not loaded")
return False
if not fencemod.have_list:
self.whinge("No fence list")
if self.is_armed:
return False
now = time.time()
if now - self.last_fence_fetch > 10:
self.last_fence_fetch = now
self.whinge("Running 'fence list'")
fencemod.list_fence(None)
return False
count = fencemod.fenceloader.count()
if count < 6:
self.whinge("Too few fence points")
return False
ret = True
for i in range(fencemod.fenceloader.count()):
p = fencemod.fenceloader.point(i)
loc = mavutil.location(p.lat, p.lng)
dist = self.get_distance(CMAC_LOCATION, loc)
if dist > self.cmac_settings.fence_maxdist:
self.whinge("Fencepoint %i too far away (%fm)" % (i, dist))
ret = False
return ret
def check_rally(self):
rallymod = self.module('rally')
if rallymod is None:
self.whinge("No rally module")
return False
if not rallymod.have_list:
self.whinge("No rally list")
if self.is_armed:
return False
now = time.time()
if now - self.last_rally_fetch > 10:
self.last_rally_fetch = now
self.whinge("Running 'rally list'")
rallymod.cmd_rally(["list"])
return False
count = rallymod.rallyloader.rally_count()
if count < 1:
self.whinge("Too few rally points")
return False
ret = True
for i in range(count):
r = rallymod.rallyloader.rally_point(i)
loc = mavutil.location(r.lat / 10000000.0, r.lng / 10000000.0)
dist = self.get_distance(CMAC_LOCATION, loc)
if dist > self.cmac_settings.rally_maxdist:
self.whinge("Rally Point %i too far away (%fm)" % (i, dist))
ret = False
# ensure we won't loiter over the runway when doing
# rally loitering:
v = self.mav_param.get("RTL_RADIUS", None)
if v is None or v == 0:
v = self.mav_param.get("WP_LOITER_RAD")
if v is None:
self.whinge("No RTL loiter radius available")
ret = False
# print("dist=%f v=%f", dist, v)
if dist < v + 30: # add a few metres of slop
self.whinge("Rally Point %i too close (%fm)" % (i, dist))
ret = False
return ret
def check_fence_health(self):
try:
sys_status = self.master.messages['SYS_STATUS']
except Exception:
return False
bits = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
present = ((sys_status.onboard_control_sensors_present & bits) == bits)
enabled = ((sys_status.onboard_control_sensors_enabled & bits) == bits)
healthy = ((sys_status.onboard_control_sensors_health & bits) == bits)
if not present or not enabled:
self.console.writeln('Fence should be enabled', fg='blue')
return False
if not healthy:
self.console.writeln('Fence unhealthy', fg='blue')
return False
return True
def check_fence(self):
ret = True
if not self.check_fence_health():
ret = False
if not self.check_fence_location():
ret = False
return ret
def check_mission(self):
wpmod = self.module('wp')
if wpmod is None:
self.whinge("No waypoint module")
return False
count = wpmod.wploader.count()
if count == 0:
self.whinge("No waypoints")
if self.is_armed:
return False
now = time.time()
if now - self.last_mission_fetch > 10:
self.whinge("Requesting waypoints")
self.last_mission_fetch = now
wpmod.wp_op = "list"
wpmod.master.waypoint_request_list_send()
return False
if count < 2:
self.whinge("Too few waypoints")
return False
ret = True
for i in range(count):
if i == 0:
# skip home
continue
w = wpmod.wploader.wp(i)
if w.command not in [
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
mavutil.mavlink.MAV_CMD_NAV_LAND
]:
continue
loc = mavutil.location(w.x, w.y)
dist = self.get_distance(CMAC_LOCATION, loc)
if dist > self.cmac_settings.wp_maxdist:
self.whinge("Waypoint %i too far away (%fm)" % (i, dist))
ret = False
return ret
def check_status(self):
try:
hb = self.master.messages['HEARTBEAT']
mc = self.master.messages['MISSION_CURRENT']
except Exception:
return False
self.is_armed = (hb.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
if not self.is_armed and hb.custom_mode == 0:
# disarmed in MANUAL we should be at WP 0
if mc.seq > 1:
self.whinge('Incorrect WP %u' % mc.seq)
return False
return True
def check(self):
success = True
self.check_map_menu()
if self.master.messages.get('HEARTBEAT') is None:
self.whinge("Waiting for heartbeat")
success = False
return
if not self.check_status():
success = False
if not self.check_parameters():
success = False
if not self.check_fence():
success = False
if not self.check_mission():
success = False
if not self.check_rally():
success = False
if not success:
self.whinge("CHECKS BAD")
return
if not self.is_armed:
self.whinge("CHECKS GOOD")
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if not self.done_heartbeat_check:
if self.master.messages.get('HEARTBEAT') is not None:
self.check()
self.done_heartbeat_check = True
if self.rate_period.trigger():
self.check()
class CUAVCompanionModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CUAVCompanionModule, self).__init__(mpstate, "CUAV",
"CUAV companion")
self.led_state = LED_OFF
self.led_force = None
self.led_send_time = 0
self.button_change_time = 0
self.last_attitude_ms = 0
self.last_mission_check_ms = 0
self.last_wp_move_ms = 0
self.add_command('cuavled', self.cmd_cuavled, "cuav led command",
['<red|green|flash|off|refresh>'])
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cuav_settings = MPSettings([
MPSetting('wp_center', int, 2, 'center search USER number'),
MPSetting('wp_start', int, 1, 'start search USER number'),
MPSetting('wp_end', int, 3, 'end search USER number'),
MPSetting('wp_land', int, 4, 'landing start USER number'),
MPSetting('auto_mission', bool, True, 'enable auto mission code')
])
self.add_command('cuav', self.cmd_cuav, 'cuav companion control',
['set (CUAVSETTING)'])
self.add_completion_function('(CUAVSETTING)',
self.cuav_settings.completion)
self.wp_move_count = 0
self.last_lz_latlon = None
self.last_wp_list_ms = 0
self.started_landing = False
self.updated_waypoints = False
self.last_buzzer = time.time()
def send_message(self, msg):
'''send a msg to GCS for display'''
msg = "cuav: " + msg
print(msg)
try:
cam = self.module('camera_air')
cam.send_message(msg)
except Exception as ex:
print("err: ", ex)
def cmd_cuav(self, args):
'''handle cuav commands'''
if len(args) < 1:
print("usage: cuav set ...")
return
elif args[0] == "set":
self.cuav_settings.command(args[1:])
def cmd_cuavled(self, args):
'''handle cuavled commands'''
usage = "usage: cuavled red|green|flash|off|refresh"
if len(args) == 0:
print(usage)
return
if args[0] == 'red':
self.force_leds(LED_RED)
elif args[0] == 'green':
self.force_leds(LED_GREEN)
elif args[0] == 'flash':
self.force_leds(LED_FLASH)
elif args[0] == 'off':
self.force_leds(LED_OFF)
elif args[0] == 'refresh':
self.led_force = None
self.led_state = None
self.update_led_state()
def force_leds(self, state):
self.led_force = state
self.set_leds(state)
def set_relay(self, relaynum, value):
self.master.mav.command_long_send(self.target_system,
self.target_component,
mavutil.mavlink.MAV_CMD_DO_SET_RELAY,
0, relaynum, value, 0, 0, 0, 0, 0)
def set_leds(self, state):
'''set two LEDs via relays'''
self.ack_wait = 2
self.led_state = state
self.led_send_time = time.time()
#self.set_relay(0, state[0])
#self.set_relay(1, state[1])
pattern = [0] * 24
plen = 3
if state[2] == 'RED':
pattern[0] = 255
elif state[2] == 'GREEN':
pattern[1] = 255
elif state[2] == 'FLASH':
pattern[0] = 255
pattern[1] = 255
pattern[3] = 2 # 2Hz flash
plen = 4
self.master.mav.led_control_send(self.settings.target_system,
self.settings.target_component, 0, 0,
plen, pattern)
if state == LED_FLASH:
# also play warning tune
self.master.mav.play_tune_send(self.settings.target_system,
self.settings.target_component,
'AAAAAA', '')
self.last_buzzer = time.time()
def idle_task(self):
'''run periodic tasks'''
pass
def update_led_state(self):
'''update LED state'''
if self.led_force is not None:
led_state = self.led_force
elif self.master.motors_armed():
led_state = LED_RED
elif time.time() - self.button_change_time < 60:
led_state = LED_FLASH
else:
led_state = LED_GREEN
try:
wpmod = self.module('wp')
wploader = wpmod.wploader
wpcur = self.master.messages['MISSION_CURRENT'].seq
wp = wploader.wp(wpcur)
if wp.command == mavutil.mavlink.MAV_CMD_NAV_DELAY_AIRSPACE_CLEAR:
led_state = LED_FLASH
except Exception as ex:
pass
if led_state != self.led_state:
self.set_leds(led_state)
try:
self.send_message("Changing LEDs to: %s" % led_state[2])
except Exception as ex:
pass
now = time.time()
if led_state[2] == 'FLASH' and now - self.last_buzzer > 5:
self.last_buzzer = now
self.master.mav.play_tune_send(self.settings.target_system,
self.settings.target_component,
'AAAAAA', '')
def find_user_wp(self, wploader, n):
'''find a USER_ waypoint number'''
for i in range(1, wploader.count()):
wp = wploader.wp(i)
if wp.command == mavutil.mavlink.MAV_CMD_USER_1 and wp.param1 == n:
# the USER_1 waypoint is just before the waypoint to use
return i + 1
return None
def update_mission(self):
'''update mission status'''
if not self.cuav_settings.auto_mission:
return
wpmod = self.module('wp')
wploader = wpmod.wploader
if wploader.count(
) < 2 and self.last_attitude_ms - self.last_wp_list_ms > 5000:
self.last_wp_list_ms = self.last_attitude_ms
wpmod.cmd_wp(["list"])
wp_start = self.find_user_wp(wploader, self.cuav_settings.wp_start)
wp_center = self.find_user_wp(wploader, self.cuav_settings.wp_center)
wp_end = self.find_user_wp(wploader, self.cuav_settings.wp_end)
if (wp_center is None or wp_start is None or wp_end is None):
# not configured
return
# run every 5 seconds
if self.last_attitude_ms - self.last_mission_check_ms < 5000:
return
self.last_mission_check_ms = self.last_attitude_ms
if self.updated_waypoints:
cam = self.module('camera_air')
if wpmod.loading_waypoints:
self.send_message("listing waypoints")
wpmod.cmd_wp(["list"])
else:
self.send_message("sending waypoints")
self.updated_waypoints = False
wploader.save("newwp.txt")
cam.send_file("newwp.txt")
if self.started_landing:
# no more to do
return
if self.last_attitude_ms - self.last_wp_move_ms < 2 * 60 * 1000:
# only move waypoints every 2 minutes
return
try:
cam = self.module('camera_air')
lz = cam.lz
target_latitude = cam.camera_settings.target_latitude
target_longitude = cam.camera_settings.target_longitude
target_radius = cam.camera_settings.target_radius
except Exception:
self.send_message("target not set")
return
lzresult = lz.calclandingzone()
if lzresult is None:
return
self.send_message("lzresult nr:%u avgscore:%u" %
(lzresult.numregions, lzresult.avgscore))
if lzresult.numregions < 5 and lzresult.avgscore < 20000:
# only accept short lists if they have high scores
return
(lat, lon) = lzresult.latlon
# check it is within the target radius
if target_radius > 0:
dist = cuav_util.gps_distance(lat, lon, target_latitude,
target_longitude)
self.send_message("dist %u" % dist)
if dist > target_radius:
return
# don't move more than 70m from the center of the search, this keeps us
# over more of the search area, and further from the fence
max_move = target_radius
if self.wp_move_count == 0:
# don't move more than 50m from center on first move
max_move = 35
if self.wp_move_count == 1:
# don't move more than 80m from center on 2nd move
max_move = 80
if dist > max_move:
bearing = cuav_util.gps_bearing(target_latitude,
target_longitude, lat, lon)
(lat, lon) = cuav_util.gps_newpos(target_latitude,
target_longitude, bearing,
max_move)
# we may need to fetch the wp list
if self.last_attitude_ms - self.last_wp_list_ms > 120000 or wpmod.loading_waypoints:
self.last_wp_list_ms = self.last_attitude_ms
self.send_message("fetching waypoints")
wpmod.cmd_wp(["list"])
return
self.last_wp_move_ms = self.last_attitude_ms
self.wp_move_count += 1
self.send_message("Moving search to: (%f,%f) %u" %
(lat, lon, self.wp_move_count))
wpmod.cmd_wp_movemulti([wp_center, wp_start, wp_end], (lat, lon))
wp_land = self.find_user_wp(wploader, self.cuav_settings.wp_land)
if (wp_land is not None and self.wp_move_count >= 3
and lzresult.numregions > 10
and self.master.flightmode == "AUTO"):
self.send_message("Starting landing")
self.master.waypoint_set_current_send(wp_land)
self.started_landing = True
self.updated_waypoints = True
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
now = time.time()
if m.get_type() == "BUTTON_CHANGE":
time_since = max(m.time_boot_ms - m.last_change_ms, 0) * 1.0e-3
self.button_change_time = time.time() - time_since
self.update_led_state()
if m.get_type() == 'HEARTBEAT':
self.update_led_state()
if m.get_type(
) == 'COMMAND_ACK' and m.command == mavutil.mavlink.MAV_CMD_DO_SET_RELAY and self.ack_wait > 0:
self.ack_wait -= 1
if self.ack_wait == 0:
self.send_message("LEDs updated: %s" % self.led_state[2])
if m.get_type() == 'ATTITUDE':
if m.time_boot_ms < self.last_attitude_ms:
self.send_message("time wrapped")
self.led_state = None
self.last_mission_check_ms = 0
self.last_wp_move_ms = 0
self.last_wp_list_ms = 0
self.button_change_time = 0
self.last_attitude_ms = m.time_boot_ms
if m.get_type() == 'MISSION_CURRENT':
self.update_mission()
class CUAVModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CUAVModule, self).__init__(mpstate, "CUAV", "CUAV checks")
self.console.set_status('RPM', 'RPM: --', row=8, fg='black')
self.console.set_status('RFind', 'RFind: --', row=8, fg='black')
self.console.set_status('Button', 'Button: --', row=8, fg='black')
self.console.set_status('ICE', 'ICE: --', row=8, fg='black')
self.rate_period = mavutil.periodic_event(1.0/15)
self.button_remaining = None
self.button_change = None
self.last_button_update = time.time()
self.last_target_update = time.time()
self.button_change_recv_time = 0
self.button_announce_time = 0
self.last_rpm_update = 0
self.last_rpm_value = None
self.last_rpm_announce = 0
self.showLandingZone = 0
self.showJoeZone = True
self.target = None
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cuav_settings = MPSettings(
[ MPSetting('rpm_threshold', int, 6000, 'RPM Threshold'),
MPSetting('wind_speed', float, 0, 'wind speed (m/s)'),
MPSetting('wind_direction', float, 0, 'wind direction (degrees)') ])
self.add_completion_function('(CUAVCHECKSETTING)', self.cuav_settings.completion)
self.add_command('cuavcheck', self.cmd_cuavcheck,
'cuav check control',
['set (CUAVCHECKSETTING)'])
#make the initial map menu
if mp_util.has_wxpython:
self.menu = MPMenuSubMenu('UAV Challenge', items=[MPMenuCheckbox('Show Landing Zone', 'Show Landing Zone', '# cuavcheck toggleLandingZone'), MPMenuCheckbox('Show Joe Zone', 'Show Joe Zone', '# cuavcheck toggleJoeZone')])
self.module('map').add_menu(self.menu)
def toggle_LandingZone(self):
'''show/hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
pos = self.module('map').click_position
'Create a new layer of two circles - at 30m and 80m radius around the above point'
if(self.showLandingZone):
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('LandingZone'))
self.mpstate.map.add_object(mp_slipmap.SlipCircle('LandingZoneInner', layer='LandingZone', latlon=pos, radius=30, linewidth=2, color=(0,0,255)))
self.mpstate.map.add_object(mp_slipmap.SlipCircle('LandingZoneOuter', layer='LandingZone', latlon=pos, radius=80, linewidth=2, color=(0,0,255)))
else:
self.mpstate.map.remove_object('LandingZoneInner')
self.mpstate.map.remove_object('LandingZoneOuter')
self.mpstate.map.remove_object('LandingZone')
def toggle_JoeZone(self):
'''show/hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
camera = self.module('camera')
if camera is None:
print("camera module is not loaded")
return
if camera.camera_settings.target_radius <= 0:
print("camera module target_radius is not set")
return
target = (camera.camera_settings.target_lattitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
self.target = target
'Create a new layer with given radius around the above point'
if self.showJoeZone:
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('JoeZone'))
self.mpstate.map.add_object(mp_slipmap.SlipCircle('JoeZoneCircle', layer='JoeZone',
latlon=(target[0],target[1]), radius=target[2], linewidth=2, color=(0,0,128)))
else:
self.mpstate.map.remove_object('JoeZoneCircle')
self.mpstate.map.remove_object('JoeZone')
def cmd_cuavcheck(self, args):
'''handle cuavcheck commands'''
usage = 'Usage: cuavcheck <set>'
if len(args) == 0:
print(usage)
return
if args[0] == "set":
self.cuav_settings.command(args[1:])
elif args[0] == "toggleLandingZone":
self.showLandingZone = not self.showLandingZone
self.toggle_LandingZone()
elif args[0] == "toggleJoeZone":
self.showJoeZone = not self.showJoeZone
self.toggle_JoeZone()
else:
print(usage)
return
def check_parms(self, parms, set=False):
'''check parameter settings'''
for p in parms.keys():
v = self.mav_param.get(p, None)
if v is None:
continue
if abs(v - parms[p]) > 0.0001:
if set:
self.console.writeln('Setting %s to %.1f (currently %.1f)' % (p, parms[p], v), fg='blue')
self.master.param_set_send(p, parms[p])
else:
self.console.writeln('%s should be %.1f (currently %.1f)' % (p, parms[p], v), fg='blue')
def check_rates(self):
'''check stream rates'''
parms = {
"SR0_EXTRA1" : 1.0,
"SR0_EXTRA2" : 2.0,
"SR0_EXTRA3" : 1.0,
"SR0_EXT_STAT" : 2.0,
"SR0_PARAMS" : 10.0,
"SR0_POSITION" : 2.0,
"SR0_RAW_CTRL" : 1.0,
"SR0_RAW_SENS" : 1.0,
"SR0_RC_CHAN" : 1.0,
"SR1_EXTRA1" : 1.0,
"SR1_EXTRA2" : 2.0,
"SR1_EXTRA3" : 1.0,
"SR1_EXT_STAT" : 2.0,
"SR1_PARAMS" : 10.0,
"SR1_POSITION" : 2.0,
"SR1_RAW_CTRL" : 1.0,
"SR1_RAW_SENS" : 1.0,
"SR1_RC_CHAN" : 1.0,
"SR2_EXTRA1" : 1.0,
"SR2_EXTRA2" : 2.0,
"SR2_EXTRA3" : 1.0,
"SR2_EXT_STAT" : 2.0,
"SR2_PARAMS" : 10.0,
"SR2_POSITION" : 2.0,
"SR2_RAW_CTRL" : 1.0,
"SR2_RAW_SENS" : 1.0,
"SR2_RC_CHAN" : 1.0,
"SR3_EXTRA1" : 1.0,
"SR3_EXTRA2" : 2.0,
"SR3_EXTRA3" : 1.0,
"SR3_EXT_STAT" : 2.0,
"SR3_PARAMS" : 10.0,
"SR3_POSITION" : 2.0,
"SR3_RAW_CTRL" : 1.0,
"SR3_RAW_SENS" : 1.0,
"SR3_RC_CHAN" : 1.0,
"FS_GCS_ENABLE" : 0,
"FS_GCS_ENABL" : 0,
}
self.check_parms(parms, True)
def idle_task(self):
'''run periodic tasks'''
now = time.time()
if now - self.last_button_update > 0.5:
self.last_button_update = now
self.update_button_display()
if self.last_rpm_update != 0 and now - self.last_rpm_update > 4:
self.console.set_status('RPM', 'RPM: --', row=8, fg='red')
self.say("Engine stopped")
self.last_rpm_update = 0
if now - self.last_target_update > 1 and self.showJoeZone:
self.last_target_update = now
camera = self.module('camera')
if camera is not None and camera.camera_settings.target_radius > 0:
target = (camera.camera_settings.target_lattitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
if target != self.target:
self.showJoeZone = False
self.toggle_JoeZone()
def update_button_display(self):
'''update the Button display on console'''
if self.button_change is None:
return
now = time.time()
time_since_change = (self.button_change.time_boot_ms - self.button_change.last_change_ms) * 0.001
time_since_change += now - self.button_change_recv_time
if time_since_change > 60:
color = 'black'
self.button_remaining = 0
else:
color = 'red'
self.button_remaining = 60 - time_since_change
remaining = int(self.button_remaining)
self.console.set_status('Button', 'Button: %u' % remaining, row=8, fg=color)
if remaining > 0 and now - self.button_announce_time > 60:
self.say("Button pressed")
self.button_announce_time = now
return
if now - self.button_announce_time >= 10 and remaining % 10 == 0 and time_since_change < 65:
self.say("%u seconds" % remaining)
self.button_announce_time = now
def rpm_check(self, m):
'''check for correct RPM range'''
thr = self.master.field('VFR_HUD', 'throttle', 0)
if thr >= 100 and m.rpm1 < self.cuav_settings.rpm_threshold:
self.console.set_status('RPM', 'RPM: %u' % m.rpm1, row=8, fg='red')
now = time.time()
if now - self.last_rpm_announce > 20:
self.say("RPM warning")
self.last_rpm_announce = now
def update_airspeed_estimate(self, m):
'''update airspeed estimate for helicopters'''
if self.cuav_settings.wind_speed <= 0:
return
from pymavlink.rotmat import Vector3
wind = Vector3(self.cuav_settings.wind_speed*math.cos(math.radians(self.cuav_settings.wind_direction)),
self.cuav_settings.wind_speed*math.sin(math.radians(self.cuav_settings.wind_direction)), 0)
ground = Vector3(m.vx*0.01, m.vy*0.01, 0)
airspeed = ground + wind
self.console.set_status('AirspeedEstimate', 'AirspeedEstimate: %u m/s' % airspeed.length(), row=8)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
now = time.time()
if m.get_type() == "BUTTON_CHANGE":
if self.button_change is not None:
if (m.time_boot_ms < self.button_change.time_boot_ms and
self.button_change.time_boot_ms - m.time_boot_ms < 30000):
# discard repeated packet from another link if older by less than 30s
return
self.button_change = m
self.button_change_recv_time = now
self.update_button_display()
if m.get_type() == "RPM":
self.console.set_status('RPM', 'RPM: %u' % m.rpm1, row=8)
self.last_rpm_update = now
if m.rpm1 > 50:
if self.last_rpm_value is None:
self.say("Engine started")
self.last_rpm_value = m.rpm1
self.rpm_check(m)
if m.get_type() == "RC_CHANNELS":
v = self.mav_param.get('ICE_START_CHAN', None)
if v is None:
return
v = getattr(m, 'chan%u_raw' % v)
if v <= 1300:
self.console.set_status('ICE', 'ICE: OFF', row=8, fg='red')
elif v >= 1700:
self.console.set_status('ICE', 'ICE: ON', row=8, fg='blue')
else:
self.console.set_status('ICE', 'ICE: AUTO', row=8, fg='green')
if m.get_type() == "RANGEFINDER" and 'ATTITUDE' in self.master.messages:
a = self.master.messages['ATTITUDE']
dist = m.distance * math.cos(a.roll) * math.cos(a.pitch)
self.console.set_status('RFind', 'RFind: %.1fm %uft' % (dist, dist*3.28084), row=8)
if m.get_type() == "VFR_HUD":
flying = False
if self.status.flightmode == "AUTO" or m.airspeed > 20 or m.groundspeed > 10:
flying = True
#if flying and self.settings.mavfwd != 0:
# print("Disabling mavfwd for flight")
# self.settings.mavfwd = 0
if m.get_type() == "GLOBAL_POSITION_INT":
self.update_airspeed_estimate(m)
if m.get_type() == 'NAMED_VALUE_FLOAT' and m.name == 'BAT3VOLT':
self.console.set_status('BAT3', 'Bat3: %.2f' % m.value, row=8)
if self.rate_period.trigger():
self.check_rates()
class CUAVModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CUAVModule, self).__init__(mpstate, "CUAV", "CUAV checks", public=True)
self.console.set_status('RPM', 'RPM: --', row=8, fg='black')
self.console.set_status('RFind', 'RFind: --', row=8, fg='black')
self.console.set_status('Button', 'Button: --', row=8, fg='black')
self.console.set_status('ICE', 'ICE: --', row=8, fg='black')
self.console.set_status('FuelPump', 'FuelPump: --', row=8, fg='black')
self.console.set_status('DNFZ', 'DNFZ -- --', row=6, fg='black')
self.rate_period = mavutil.periodic_event(1.0/15)
self.button_remaining = None
self.button_change = None
self.last_button_update = time.time()
self.last_target_update = time.time()
self.button_change_recv_time = 0
self.button_announce_time = 0
self.fuel_change = None
self.last_fuel_update = time.time()
self.fuel_change_recv_time = 0
self.last_rpm_update = 0
self.last_rpm_value = None
self.last_rpm_announce = 0
self.showLandingZone = 0
self.showJoeZone = True
self.target = None
self.last_recall_check = 0
self.is_armed = False
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cuav_settings = MPSettings(
[ MPSetting('rpm_threshold', int, 6000, 'RPM Threshold'),
MPSetting('wind_speed', float, 0, 'wind speed (m/s)'),
MPSetting('wind_direction', float, 0, 'wind direction (degrees)'),
MPSetting('button_pin', float, 0, 'button pin'),
MPSetting('fuel_pin', float, 1, 'fuel pin'),
MPSetting('wp_center', int, 2, 'center search USER number'),
MPSetting('wp_start', int, 1, 'start search USER number'),
MPSetting('wp_end', int, 3, 'end search USER number'),
MPSetting('wp_land',int, 4, 'landing start USER number'),
MPSetting('wp_recall', int, 5, 'recall Kraken USER number'),
MPSetting('wp_release', int, 6, 'release Kraken USER number'),
MPSetting('qnh_max_err', int, 50, 'maximum QNH error') ])
self.add_completion_function('(CUAVCHECKSETTING)', self.cuav_settings.completion)
self.add_command('cuavcheck', self.cmd_cuavcheck,
'cuav check control',
['checkparams',
'movetarget',
'resettarget',
'showJoeZone',
'set (CUAVCHECKSETTING)'])
#make the initial map menu
if mp_util.has_wxpython and self.module('map'):
self.menu = MPMenuSubMenu('UAV Challenge', items=[MPMenuCheckbox('Show Landing Zone', 'Show Landing Zone', '# cuavcheck toggleLandingZone'), MPMenuCheckbox('Show Joe Zone', 'Show Joe Zone', '# cuavcheck toggleJoeZone')])
self.module('map').add_menu(self.menu)
def find_user_wp(self, wploader, n):
'''find a USER_ waypoint number'''
for i in range(1, wploader.count()):
wp = wploader.wp(i)
if wp.command == mavutil.mavlink.MAV_CMD_USER_1 and wp.param1 == n:
# the USER_1 waypoint is just before the waypoint to use
return i+1
return None
def toggle_LandingZone(self):
'''show/hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
pos = self.module('map').click_position
'Create a new layer of two circles - at 30m and 80m radius around the above point'
if(self.showLandingZone):
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('LandingZone'))
self.mpstate.map.add_object(mp_slipmap.SlipCircle('LandingZoneInner', layer='LandingZone', latlon=pos, radius=30, linewidth=2, color=(0,0,255)))
self.mpstate.map.add_object(mp_slipmap.SlipCircle('LandingZoneOuter', layer='LandingZone', latlon=pos, radius=80, linewidth=2, color=(0,0,255)))
else:
self.mpstate.map.remove_object('LandingZoneInner')
self.mpstate.map.remove_object('LandingZoneOuter')
self.mpstate.map.remove_object('LandingZone')
def show_JoeZone(self):
'''show the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
camera = self.module('camera_ground')
if camera is None:
print("camera_ground module is not loaded")
return
target = (camera.camera_settings.target_latitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
self.target = target
for m in self.module_matching('map*'):
m.map.add_object(mp_slipmap.SlipClearLayer('JoeZone'))
m.map.add_object(mp_slipmap.SlipCircle('JoeZoneCircle', layer='JoeZone',
latlon=(target[0],target[1]), radius=target[2], linewidth=2, color=(0,0,128)))
def hide_JoeZone(self):
'''hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
for m in self.module_matching('map*'):
m.map.remove_object('JoeZoneCircle')
m.map.remove_object('JoeZone')
def toggle_JoeZone(self):
'''show/hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
camera = self.module('camera_ground')
if self.mpstate.map is None:
print("Map module not loaded")
return
if camera is None:
print("camera_ground module is not loaded")
return
if camera.camera_settings.target_radius <= 0:
print("camera_ground module target_radius is not set")
return
target = (camera.camera_settings.target_latitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
self.target = target
if self.showJoeZone:
self.show_JoeZone()
else:
self.hide_JoeZone()
def cmd_cuavcheck(self, args):
'''handle cuavcheck commands'''
usage = 'Usage: cuavcheck <set>'
if len(args) == 0:
print(usage)
return
if args[0] == "set":
self.cuav_settings.command(args[1:])
elif args[0] == "toggleLandingZone":
self.showLandingZone = not self.showLandingZone
self.toggle_LandingZone()
elif args[0] == "toggleJoeZone":
self.showJoeZone = not self.showJoeZone
self.toggle_JoeZone()
elif args[0] == "showJoeZone":
self.showJoeZone = True
self.toggle_JoeZone()
elif args[0] == "checkparams":
if self.check_parameters():
print("Parameters OK")
else:
print("Parameters bad")
if not self.check_fence():
print("Fence bad")
else:
print("Fence OK")
if not self.check_status():
print("Status bad")
else:
print("Status OK")
if not self.check_QNH():
print("QNH bad")
else:
print("QNH OK")
elif args[0] == "movetarget":
self.move_target()
elif args[0] == "resettarget":
self.reset_target()
else:
print(usage)
return
def move_target(self):
'''move target waypoints'''
wpmod = self.module('wp')
wploader = wpmod.wploader
wp_start = self.find_user_wp(wploader, self.cuav_settings.wp_start)
wp_center = self.find_user_wp(wploader, self.cuav_settings.wp_center)
wp_end = self.find_user_wp(wploader, self.cuav_settings.wp_end)
if (wp_center is None or
wp_start is None or
wp_end is None):
print("Target WPs not in mission")
return
latlon = self.module('map').click_position
if latlon is None:
print("No click position")
return
print("Moving %u waypoints" % (wp_end + 1 - wp_start), wp_center, wp_start, wp_end)
wpmod.cmd_wp_movemulti([wp_center, wp_start, wp_end], latlon)
def reset_target(self):
'''reset target waypoints'''
wpmod = self.module('wp')
wploader = wpmod.wploader
wp_start = self.find_user_wp(wploader, self.cuav_settings.wp_start)
wp_center = self.find_user_wp(wploader, self.cuav_settings.wp_center)
wp_end = self.find_user_wp(wploader, self.cuav_settings.wp_end)
if (wp_center is None or
wp_start is None or
wp_end is None):
print("Target WPs not in mission")
return
camera = self.module('camera_ground')
if camera is None:
print("camera_ground module is not loaded")
return
lat = camera.camera_settings.target_latitude
lon = camera.camera_settings.target_longitude
if lat == 0 or lon == 0:
print("target not set")
return
print("Resetting %u waypoints" % (wp_end + 1 - wp_start), wp_center, wp_start, wp_end)
wpmod.cmd_wp_movemulti([wp_center, wp_start, wp_end], (lat,lon))
def check_parms(self, parms, set=False):
'''check parameter settings'''
ret = True
for p in parms.keys():
v = self.mav_param.get(p, None)
if v is None:
self.console.writeln("Parameter %s unavailable" % p)
continue
if abs(v - parms[p]) > 0.0001:
if set:
self.console.writeln('Setting %s to %.1f (currently %.1f)' % (p, parms[p], v), fg='blue')
self.master.param_set_send(p, parms[p])
else:
self.console.writeln('%s should be %.1f (currently %.1f)' % (p, parms[p], v), fg='blue')
ret = False
return ret
def check_parameters(self):
'''check key parameters'''
# first see if this is a quadplane
v = self.mav_param.get('Q_ENABLE',None)
if v is None:
self.console.writeln('Q_ENABLE not available')
return False
if int(v) == 0:
# this is the relay aircraft
return self.check_parameters_relay()
else:
return self.check_parameters_retrieval()
def check_parameters_relay(self):
# relay aircraft should have low rates on SR1
rates = {
"SR1_EXTRA1" : 1.0,
"SR1_EXTRA2" : 1.0,
"SR1_EXTRA3" : 1.0,
"SR1_EXT_STAT" : 2.0,
"SR1_POSITION" : 2.0,
"SR1_RAW_CTRL" : 1.0,
"SR1_RAW_SENS" : 1.0,
"SR1_RC_CHAN" : 1.0
}
ret = self.check_parms(rates, True)
# some other key parameters, not auto-set
keyparams = {
"ARMING_CHECK" : 1,
"FS_GCS_ENABL" : 0,
"AVD_W_ACTION" : 2,
"FENCE_AUTOENABLE" : 1,
"RC_OPTIONS" : 4,
"SERIAL1_PROTOCOL" : 2,
"SYSID_ENFORCE" : 1,
"SYSID_MYGCS" : 253,
"AFS_ENABLE" : 1,
"AFS_TERM_ACTION" : 42,
"AFS_WP_COMMS" : 6,
"AFS_WP_GPS_LOSS" : 8,
"TERRAIN_FOLLOW" : 1,
"THR_FAILSAFE" : 0,
"GCS_PID_MASK" : 0,
"RTL_AUTOLAND" : 2,
}
if not self.check_parms(keyparams, False):
ret = False
return ret
def check_parameters_retrieval(self):
# retrieval aircraft should have low rates on SR1, higher rates on SR2
rates = {
"SR1_EXTRA1" : 1.0,
"SR1_EXTRA2" : 1.0,
"SR1_EXTRA3" : 1.0,
"SR1_EXT_STAT" : 2.0,
"SR1_POSITION" : 2.0,
"SR1_RAW_CTRL" : 1.0,
"SR1_RAW_SENS" : 1.0,
"SR1_RC_CHAN" : 1.0,
"SR2_EXTRA1" : 6.0,
"SR2_EXTRA2" : 4.0,
"SR2_EXTRA3" : 4.0,
"SR2_EXT_STAT" : 4.0,
"SR2_POSITION" : 6.0,
"SR2_RAW_CTRL" : 4.0,
"SR2_RAW_SENS" : 4.0,
"SR2_RC_CHAN" : 1.0,
}
ret = self.check_parms(rates, True)
# some other key parameters, not auto-set
keyparams = {
"ARMING_CHECK" : 1,
"Q_OPTIONS" : 8,
"AVD_ENABLE" : 1,
"ADSB_ENABLE" : 1,
"FS_GCS_ENABL" : 0,
"AVD_W_ACTION" : 2,
"FENCE_AUTOENABLE" : 1,
"RC_OPTIONS" : 4,
"SERIAL1_PROTOCOL" : 2,
"SERIAL2_PROTOCOL" : 2,
"SYSID_ENFORCE" : 1,
"SYSID_MYGCS" : 254,
"AFS_ENABLE" : 1,
"AFS_TERM_ACTION" : 42,
"AFS_WP_COMMS" : 6,
"AFS_WP_GPS_LOSS" : 8,
"Q_WVANE_GAIN" : 0.25,
"TERRAIN_FOLLOW" : 1,
"THR_FAILSAFE" : 0,
"GCS_PID_MASK" : 0,
"RTL_AUTOLAND" : 2,
}
if not self.check_parms(keyparams, False):
ret = False
return ret
def check_recall(self):
'''check for recalling Kraken'''
v = self.mav_param.get('Q_ENABLE',None)
if v is None or int(v) == 0:
return
wpmod = self.module('wp')
wploader = wpmod.wploader
wp_recall = self.find_user_wp(wploader, self.cuav_settings.wp_recall)
if wp_recall is None:
self.console.writeln('No recall WP', fg='blue')
return
try:
mc = self.master.messages['MISSION_CURRENT']
except Exception:
return
if mc.seq == wp_recall:
self.console.writeln('Recalling Kraken', fg='blue')
# use all links
for i in range(len(self.mpstate.mav_master)):
m = self.mpstate.mav_master[i]
src_saved = m.mav.srcSystem
try:
m.mav.srcSystem = 253
m.mav.command_long_send(
0, # target_system
0, # target_component
mavutil.mavlink.MAV_CMD_USER_2, # command
0, # confirmation
42, # param1
0, # param2
0, # param3
0, # param4
0, # param5
0, # param6
0) # param7
except Exception as ex:
print(ex)
m.mav.srcSystem = src_saved
map2 = self.module("map2")
if map2:
map2.map.set_follow(1)
map2.map.set_zoom(2000)
def check_release(self):
'''check for releasing Kraken'''
v = self.mav_param.get('Q_ENABLE',None)
if v is None or int(v) == 0:
return
wpmod = self.module('wp')
wploader = wpmod.wploader
wp_release = self.find_user_wp(wploader, self.cuav_settings.wp_release)
if wp_release is None:
self.console.writeln('No release WP', fg='blue')
return
try:
mc = self.master.messages['MISSION_CURRENT']
except Exception:
return
if mc.seq == wp_release:
self.console.writeln('Releasing Kraken', fg='blue')
# use all links
for i in range(len(self.mpstate.mav_master)):
m = self.mpstate.mav_master[i]
src_saved = m.mav.srcSystem
try:
# use 1st link
m.mav.srcSystem = 253
m.mav.command_long_send(
0, # target_system
0, # target_component
mavutil.mavlink.MAV_CMD_USER_2, # command
0, # confirmation
24, # param1
0, # param2
0, # param3
0, # param4
0, # param5
0, # param6
0) # param7
except Exception as ex:
print(ex)
m.mav.srcSystem = src_saved
def check_QNH(self):
'''check QNH settings'''
if self.is_armed:
return True
v = self.mav_param.get('AFS_QNH_PRESSURE', None)
if v is None:
self.console.writeln('AFS_QNH_PRESSURE not available', fg='blue')
return False
if int(v) == 0:
self.console.writeln('AFS_QNH_PRESSURE not set', fg='blue')
return False
misc = self.module('misc')
qest = misc.qnh_estimate()
pressure = self.master.field('SCALED_PRESSURE', 'press_abs', 0)
ground_temp = self.get_mav_param('GND_TEMP', 21)
qnh_alt = misc.altitude_difference(v, pressure, ground_temp)
amsl_alt = self.master.field('GLOBAL_POSITION_INT', 'alt', 0) * 0.001
err = qnh_alt - amsl_alt
if abs(err) > self.cuav_settings.qnh_max_err:
self.console.writeln('QNH Alt error %dm' % int(err), fg='blue')
self.console.writeln('AFS_QNH_PRESSURE should be %.1f' % qest, fg='blue')
return False
return True
def idle_task(self):
'''run periodic tasks'''
now = time.time()
if now - self.last_button_update > 0.5:
self.last_button_update = now
self.update_button_display()
if now - self.last_fuel_update > 1.0:
self.last_fuel_update = now
self.update_fuel_display()
if self.last_rpm_update != 0 and now - self.last_rpm_update > 4:
self.console.set_status('RPM', 'RPM: --', row=8, fg='red')
self.say("Engine stopped")
self.last_rpm_update = 0
if now - self.last_target_update > 1 and self.showJoeZone:
self.last_target_update = now
camera = self.module('camera_ground')
if camera is not None and camera.camera_settings.target_radius > 0:
target = (camera.camera_settings.target_latitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
if target != self.target:
self.showJoeZone = False
self.toggle_JoeZone()
if now - self.last_recall_check > 10:
self.last_recall_check = now
self.check_recall()
self.check_release()
self.network_status()
def network_status(self):
'''update display of network'''
v = self.mav_param.get('Q_ENABLE',None)
if v is None:
# only on porter GCS
return
a=[]
fname="/tmp/gcs_net.txt"
now = time.time()
mtime = 0
try:
f = open(fname, "r")
mtime = os.stat(fname).st_mtime
a = f.read().split()
except Exception as ex:
pass
if len(a) != 4 or now - mtime > 30:
self.console.set_status('Telstra', 'Telstra: --', row=6, fg='red')
self.console.set_status('Optus', 'Optus: --', row=6, fg='red')
return
if a[0:2] != ['1','1']:
color = 'red'
else:
color = 'green'
self.console.set_status('Telstra', 'Telstra: %s/%s' % (a[0],a[1]), row=7, fg=color)
if a[2:] != ['1','1']:
color = 'red'
else:
color = 'green'
self.console.set_status('Optus', 'Optus: %s/%s' % (a[2],a[3]), row=7, fg=color)
def update_button_display(self):
'''update the Button display on console'''
if self.button_change is None:
return
now = time.time()
time_since_change = (self.button_change.time_boot_ms - self.button_change.last_change_ms) * 0.001
time_since_change += now - self.button_change_recv_time
if time_since_change > 60:
color = 'black'
self.button_remaining = 0
else:
color = 'red'
self.button_remaining = 60 - time_since_change
remaining = int(self.button_remaining)
self.console.set_status('Button', 'Button: %u' % remaining, row=8, fg=color)
if remaining > 0 and now - self.button_announce_time > 60:
self.say("Button pressed")
self.button_announce_time = now
return
if now - self.button_announce_time >= 10 and remaining % 10 == 0 and time_since_change < 65:
self.say("%u seconds" % remaining)
self.button_announce_time = now
def update_fuel_display(self):
'''update the fuel display on console'''
if self.fuel_change is None:
return
now = time.time()
time_since_change = (self.fuel_change.time_boot_ms - self.fuel_change.last_change_ms) * 0.001
time_since_change += now - self.fuel_change_recv_time
self.console.set_status('FuelPump', 'FuelPump: %u' % int(time_since_change), row=8, fg='black')
def rpm_check(self, m):
'''check for correct RPM range'''
thr = self.master.field('VFR_HUD', 'throttle', 0)
if thr >= 100 and m.rpm1 < self.cuav_settings.rpm_threshold:
self.console.set_status('RPM', 'RPM: %u' % m.rpm1, row=8, fg='red')
now = time.time()
if now - self.last_rpm_announce > 20:
self.say("RPM warning")
self.last_rpm_announce = now
def update_airspeed_estimate(self, m):
'''update airspeed estimate from wind triangle'''
if not 'WIND' in self.master.messages or not 'GLOBAL_POSITION_INT' in self.master.messages:
return
wind = self.master.messages['WIND']
gpi = self.master.messages['GLOBAL_POSITION_INT']
from pymavlink.rotmat import Vector3
wind3d = Vector3(wind.speed*math.cos(math.radians(wind.direction)),
wind.speed*math.sin(math.radians(wind.direction)), 0)
ground = Vector3(gpi.vx*0.01, gpi.vy*0.01, 0)
airspeed = (ground + wind3d).length()
err = abs(airspeed - m.airspeed)
if err > 5:
color = 'red'
elif err > 3:
color = 'orange'
else:
color = 'green'
astr = self.speed_string(airspeed)
self.console.set_status('ASEst', 'ASEst: %s' % astr, row=8, fg=color)
def check_fence(self):
try:
sys_status = self.master.messages['SYS_STATUS']
except Exception:
return False
bits = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
present = ((sys_status.onboard_control_sensors_present & bits) == bits)
enabled = ((sys_status.onboard_control_sensors_enabled & bits) == bits)
healthy = ((sys_status.onboard_control_sensors_health & bits) == bits)
if not present or not enabled:
self.console.writeln('Fence should be enabled', fg='blue')
return False
if not healthy:
self.console.writeln('Fence unhealthy', fg='blue')
return False
return True
def check_status(self):
try:
hb = self.master.messages['HEARTBEAT']
mc = self.master.messages['MISSION_CURRENT']
except Exception:
return False
self.is_armed = (hb.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
wpmod = self.module('wp')
wploader = wpmod.wploader
seq = mc.seq
wp = wploader.wp(seq)
v = self.mav_param.get('Q_ENABLE', 0)
if self.is_armed and self.master.flightmode == "AUTO" and wp is not None and v == 0:
if wp.command == mavutil.mavlink.MAV_CMD_NAV_DELAY:
press1 = self.mav_param.get('GND_ABS_PRESS', None)
press2 = self.mav_param.get('GND_ABS_PRESS2', None)
if press1 is not None and press1 > 0:
scp = self.master.messages['SCALED_PRESSURE']
gnd_press1 = scp.press_abs * 100
self.master.param_set_send('GND_ABS_PRESS', gnd_press1)
if press2 is not None and press2 > 0:
scp = self.master.messages['SCALED_PRESSURE2']
gnd_press2 = scp.press_abs * 100
self.master.param_set_send('GND_ABS_PRESS2', gnd_press2)
if not self.is_armed and hb.custom_mode == 0:
# disarmed in MANUAL we should be at WP 0
if mc.seq > 1:
self.console.writeln('Incorrect WP %u' % mc.seq, fg='blue')
return False
return True
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
now = time.time()
if m.get_type() == "BUTTON_CHANGE":
if m.state & (1 << self.cuav_settings.fuel_pin):
self.fuel_change = m
self.fuel_change_recv_time = now
self.update_fuel_display()
if m.state & (1 << self.cuav_settings.button_pin):
if self.button_change is None or m.last_change_ms != self.button_change.last_change_ms:
print("button change", m.state)
if self.button_change is not None:
if (m.time_boot_ms < self.button_change.time_boot_ms and
self.button_change.time_boot_ms - m.time_boot_ms < 30000):
# discard repeated packet from another link if older by less than 30s
return
self.button_change = m
self.button_change_recv_time = now
self.update_button_display()
if m.get_type() == "RPM":
self.console.set_status('RPM', 'RPM: %u' % m.rpm1, row=8)
self.last_rpm_update = now
if m.rpm1 > 50:
if self.last_rpm_value is None:
self.say("Engine started")
self.last_rpm_value = m.rpm1
self.rpm_check(m)
if m.get_type() == "RC_CHANNELS":
v = self.mav_param.get('ICE_START_CHAN', None)
if v is None:
return
v = getattr(m, 'chan%u_raw' % v)
if v <= 1300:
self.console.set_status('ICE', 'ICE: OFF', row=8, fg='red')
elif v >= 1700:
self.console.set_status('ICE', 'ICE: ON', row=8, fg='blue')
else:
self.console.set_status('ICE', 'ICE: AUTO', row=8, fg='green')
if m.get_type() == "RANGEFINDER" and 'ATTITUDE' in self.master.messages:
a = self.master.messages['ATTITUDE']
dist = m.distance * math.cos(a.roll) * math.cos(a.pitch)
self.console.set_status('RFind', 'RFind: %.1fm %uft' % (dist, dist*3.28084), row=8)
if m.get_type() == "VFR_HUD":
self.update_airspeed_estimate(m)
if m.get_type() == 'NAMED_VALUE_FLOAT' and m.name == 'BAT3VOLT':
self.console.set_status('BAT3', 'Bat3: %.2f' % m.value, row=8)
if m.get_type() == 'COLLISION':
if m.action == 0:
color = 'green'
elif m.action == 1:
color = 'blue'
elif m.action == 2:
color = 'orange'
elif m.action == 3:
color = 'darkorange'
elif m.action == 4:
color = 'darkred'
elif m.action == 6:
color = 'yellow'
else:
color = 'red'
self.console.set_status('DNFZ', 'DNFZ %d %.0fm %.0fm %u' % (
m.id, m.horizontal_minimum_delta, m.altitude_minimum_delta, m.src), row=6, fg=color)
if self.rate_period.trigger():
self.check_status()
self.check_parameters()
self.check_fence()
self.check_QNH()
class CUAVModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CUAVModule, self).__init__(mpstate,
"CUAV",
"CUAV checks",
public=True)
self.console.set_status('RPM', 'RPM: --', row=8, fg='black')
self.console.set_status('RFind', 'RFind: --', row=8, fg='black')
self.console.set_status('Button', 'Button: --', row=8, fg='black')
self.console.set_status('ICE', 'ICE: --', row=8, fg='black')
self.console.set_status('FuelPump', 'FuelPump: --', row=8, fg='black')
self.console.set_status('DNFZ', 'DNFZ -- --', row=6, fg='black')
self.rate_period = mavutil.periodic_event(1.0 / 15)
self.button_remaining = None
self.button_change = None
self.last_button_update = time.time()
self.last_target_update = time.time()
self.button_change_recv_time = 0
self.button_announce_time = 0
self.fuel_change = None
self.last_fuel_update = time.time()
self.fuel_change_recv_time = 0
self.last_rpm_update = 0
self.last_rpm_value = None
self.last_rpm_announce = 0
self.showLandingZone = 0
self.showJoeZone = True
self.target = None
self.last_recall_check = 0
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cuav_settings = MPSettings([
MPSetting('rpm_threshold', int, 6000, 'RPM Threshold'),
MPSetting('wind_speed', float, 0, 'wind speed (m/s)'),
MPSetting('wind_direction', float, 0, 'wind direction (degrees)'),
MPSetting('button_pin', float, 0, 'button pin'),
MPSetting('fuel_pin', float, 1, 'fuel pin'),
MPSetting('wp_center', int, 2, 'center search USER number'),
MPSetting('wp_start', int, 1, 'start search USER number'),
MPSetting('wp_end', int, 3, 'end search USER number'),
MPSetting('wp_land', int, 4, 'landing start USER number'),
MPSetting('wp_recall', int, 5, 'recall Kraken USER number'),
MPSetting('wp_release', int, 6, 'release Kraken USER number')
])
self.add_completion_function('(CUAVCHECKSETTING)',
self.cuav_settings.completion)
self.add_command('cuavcheck', self.cmd_cuavcheck, 'cuav check control',
[
'checkparams', 'movetarget', 'resettarget',
'showJoeZone', 'set (CUAVCHECKSETTING)'
])
#make the initial map menu
if mp_util.has_wxpython and self.module('map'):
self.menu = MPMenuSubMenu(
'UAV Challenge',
items=[
MPMenuCheckbox('Show Landing Zone', 'Show Landing Zone',
'# cuavcheck toggleLandingZone'),
MPMenuCheckbox('Show Joe Zone', 'Show Joe Zone',
'# cuavcheck toggleJoeZone')
])
self.module('map').add_menu(self.menu)
def find_user_wp(self, wploader, n):
'''find a USER_ waypoint number'''
for i in range(1, wploader.count()):
wp = wploader.wp(i)
if wp.command == mavutil.mavlink.MAV_CMD_USER_1 and wp.param1 == n:
# the USER_1 waypoint is just before the waypoint to use
return i + 1
return None
def toggle_LandingZone(self):
'''show/hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
pos = self.module('map').click_position
'Create a new layer of two circles - at 30m and 80m radius around the above point'
if (self.showLandingZone):
self.mpstate.map.add_object(
mp_slipmap.SlipClearLayer('LandingZone'))
self.mpstate.map.add_object(
mp_slipmap.SlipCircle('LandingZoneInner',
layer='LandingZone',
latlon=pos,
radius=30,
linewidth=2,
color=(0, 0, 255)))
self.mpstate.map.add_object(
mp_slipmap.SlipCircle('LandingZoneOuter',
layer='LandingZone',
latlon=pos,
radius=80,
linewidth=2,
color=(0, 0, 255)))
else:
self.mpstate.map.remove_object('LandingZoneInner')
self.mpstate.map.remove_object('LandingZoneOuter')
self.mpstate.map.remove_object('LandingZone')
def show_JoeZone(self):
'''show the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
camera = self.module('camera_ground')
if camera is None:
print("camera_ground module is not loaded")
return
target = (camera.camera_settings.target_latitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
self.target = target
for m in self.module_matching('map*'):
m.map.add_object(mp_slipmap.SlipClearLayer('JoeZone'))
m.map.add_object(
mp_slipmap.SlipCircle('JoeZoneCircle',
layer='JoeZone',
latlon=(target[0], target[1]),
radius=target[2],
linewidth=2,
color=(0, 0, 128)))
def hide_JoeZone(self):
'''hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
for m in self.module_matching('map*'):
m.map.remove_object('JoeZoneCircle')
m.map.remove_object('JoeZone')
def toggle_JoeZone(self):
'''show/hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
camera = self.module('camera_ground')
if self.mpstate.map is None:
print("Map module not loaded")
return
if camera is None:
print("camera_ground module is not loaded")
return
if camera.camera_settings.target_radius <= 0:
print("camera_ground module target_radius is not set")
return
target = (camera.camera_settings.target_latitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
self.target = target
if self.showJoeZone:
self.show_JoeZone()
else:
self.hide_JoeZone()
def cmd_cuavcheck(self, args):
'''handle cuavcheck commands'''
usage = 'Usage: cuavcheck <set>'
if len(args) == 0:
print(usage)
return
if args[0] == "set":
self.cuav_settings.command(args[1:])
elif args[0] == "toggleLandingZone":
self.showLandingZone = not self.showLandingZone
self.toggle_LandingZone()
elif args[0] == "toggleJoeZone":
self.showJoeZone = not self.showJoeZone
self.toggle_JoeZone()
elif args[0] == "showJoeZone":
self.showJoeZone = True
self.toggle_JoeZone()
elif args[0] == "checkparams":
if self.check_parameters():
print("Parameters OK")
else:
print("Parameters bad")
if not self.check_fence():
print("Fence bad")
else:
print("Fence OK")
if not self.check_status():
print("Status bad")
else:
print("Status OK")
if not self.check_QNH():
print("QNH bad")
else:
print("QNH OK")
elif args[0] == "movetarget":
self.move_target()
elif args[0] == "resettarget":
self.reset_target()
else:
print(usage)
return
def move_target(self):
'''move target waypoints'''
wpmod = self.module('wp')
wploader = wpmod.wploader
wp_start = self.find_user_wp(wploader, self.cuav_settings.wp_start)
wp_center = self.find_user_wp(wploader, self.cuav_settings.wp_center)
wp_end = self.find_user_wp(wploader, self.cuav_settings.wp_end)
if (wp_center is None or wp_start is None or wp_end is None):
print("Target WPs not in mission")
return
latlon = self.module('map').click_position
if latlon is None:
print("No click position")
return
print("Moving %u waypoints" % (wp_end + 1 - wp_start), wp_center,
wp_start, wp_end)
wpmod.cmd_wp_movemulti([wp_center, wp_start, wp_end], latlon)
def reset_target(self):
'''reset target waypoints'''
wpmod = self.module('wp')
wploader = wpmod.wploader
wp_start = self.find_user_wp(wploader, self.cuav_settings.wp_start)
wp_center = self.find_user_wp(wploader, self.cuav_settings.wp_center)
wp_end = self.find_user_wp(wploader, self.cuav_settings.wp_end)
if (wp_center is None or wp_start is None or wp_end is None):
print("Target WPs not in mission")
return
camera = self.module('camera_ground')
if camera is None:
print("camera_ground module is not loaded")
return
lat = camera.camera_settings.target_latitude
lon = camera.camera_settings.target_longitude
if lat == 0 or lon == 0:
print("target not set")
return
print("Resetting %u waypoints" % (wp_end + 1 - wp_start), wp_center,
wp_start, wp_end)
wpmod.cmd_wp_movemulti([wp_center, wp_start, wp_end], (lat, lon))
def check_parms(self, parms, set=False):
'''check parameter settings'''
ret = True
for p in parms.keys():
v = self.mav_param.get(p, None)
if v is None:
self.console.writeln("Parameter %s unavailable" % p)
continue
if abs(v - parms[p]) > 0.0001:
if set:
self.console.writeln(
'Setting %s to %.1f (currently %.1f)' %
(p, parms[p], v),
fg='blue')
self.master.param_set_send(p, parms[p])
else:
self.console.writeln('%s should be %.1f (currently %.1f)' %
(p, parms[p], v),
fg='blue')
ret = False
return ret
def check_parameters(self):
'''check key parameters'''
# first see if this is a quadplane
v = self.mav_param.get('Q_ENABLE', None)
if v is None:
self.console.writeln('Q_ENABLE not available')
return False
if int(v) == 0:
# this is the relay aircraft
return self.check_parameters_relay()
else:
return self.check_parameters_retrieval()
def check_parameters_relay(self):
# relay aircraft should have low rates on SR1
rates = {
"SR1_EXTRA1": 1.0,
"SR1_EXTRA2": 1.0,
"SR1_EXTRA3": 1.0,
"SR1_EXT_STAT": 2.0,
"SR1_POSITION": 2.0,
"SR1_RAW_CTRL": 1.0,
"SR1_RAW_SENS": 1.0,
"SR1_RC_CHAN": 1.0
}
ret = self.check_parms(rates, True)
# some other key parameters, not auto-set
keyparams = {
"ARMING_CHECK": 1,
"FS_GCS_ENABL": 0,
"AVD_W_ACTION": 2,
"FENCE_AUTOENABLE": 1,
"RC_OPTIONS": 4,
"SERIAL1_PROTOCOL": 2,
"SYSID_ENFORCE": 1,
"SYSID_MYGCS": 253,
"AFS_ENABLE": 1,
"AFS_TERM_ACTION": 42,
"AFS_WP_COMMS": 6,
"AFS_WP_GPS_LOSS": 8,
"TERRAIN_FOLLOW": 1,
"THR_FAILSAFE": 0,
}
if not self.check_parms(keyparams, False):
ret = False
return ret
def check_parameters_retrieval(self):
# retrieval aircraft should have low rates on SR1, higher rates on SR2
rates = {
"SR1_EXTRA1": 1.0,
"SR1_EXTRA2": 1.0,
"SR1_EXTRA3": 1.0,
"SR1_EXT_STAT": 2.0,
"SR1_POSITION": 2.0,
"SR1_RAW_CTRL": 1.0,
"SR1_RAW_SENS": 1.0,
"SR1_RC_CHAN": 1.0,
"SR2_EXTRA1": 6.0,
"SR2_EXTRA2": 4.0,
"SR2_EXTRA3": 4.0,
"SR2_EXT_STAT": 4.0,
"SR2_POSITION": 6.0,
"SR2_RAW_CTRL": 4.0,
"SR2_RAW_SENS": 4.0,
"SR2_RC_CHAN": 1.0,
}
ret = self.check_parms(rates, True)
# some other key parameters, not auto-set
keyparams = {
"ARMING_CHECK": 1,
"Q_OPTIONS": 8,
"AVD_ENABLE": 1,
"ADSB_ENABLE": 1,
"FS_GCS_ENABL": 0,
"AVD_W_ACTION": 2,
"FENCE_AUTOENABLE": 1,
"RC_OPTIONS": 4,
"SERIAL1_PROTOCOL": 2,
"SERIAL2_PROTOCOL": 2,
"SYSID_ENFORCE": 1,
"SYSID_MYGCS": 254,
"AFS_ENABLE": 1,
"AFS_TERM_ACTION": 42,
"AFS_WP_COMMS": 6,
"AFS_WP_GPS_LOSS": 8,
"Q_WVANE_GAIN": 0.25,
"TERRAIN_FOLLOW": 1,
"THR_FAILSAFE": 0,
}
if not self.check_parms(keyparams, False):
ret = False
return ret
def check_recall(self):
'''check for recalling Kraken'''
v = self.mav_param.get('Q_ENABLE', None)
if v is None or int(v) == 0:
return
wpmod = self.module('wp')
wploader = wpmod.wploader
wp_recall = self.find_user_wp(wploader, self.cuav_settings.wp_recall)
if wp_recall is None:
self.console.writeln('No recall WP', fg='blue')
return
try:
mc = self.master.messages['MISSION_CURRENT']
except Exception:
return
if mc.seq == wp_recall:
self.console.writeln('Recalling Kraken', fg='blue')
src_saved = self.master.mav.srcSystem
self.master.mav.srcSystem = 253
self.master.mav.command_long_send(
0, # target_system
0, # target_component
mavutil.mavlink.MAV_CMD_USER_2, # command
0, # confirmation
42, # param1
0, # param2
0, # param3
0, # param4
0, # param5
0, # param6
0) # param7
self.master.mav.srcSystem = src_saved
def check_release(self):
'''check for releasing Kraken'''
v = self.mav_param.get('Q_ENABLE', None)
if v is None or int(v) == 0:
return
wpmod = self.module('wp')
wploader = wpmod.wploader
wp_release = self.find_user_wp(wploader, self.cuav_settings.wp_release)
if wp_release is None:
self.console.writeln('No release WP', fg='blue')
return
try:
mc = self.master.messages['MISSION_CURRENT']
except Exception:
return
if mc.seq == wp_release:
self.console.writeln('Releasing Kraken', fg='blue')
src_saved = self.master.mav.srcSystem
self.master.mav.srcSystem = 253
self.master.mav.command_long_send(
0, # target_system
0, # target_component
mavutil.mavlink.MAV_CMD_USER_2, # command
0, # confirmation
24, # param1
0, # param2
0, # param3
0, # param4
0, # param5
0, # param6
0) # param7
self.master.mav.srcSystem = src_saved
def idle_task(self):
'''run periodic tasks'''
now = self.get_time()
if now - self.last_button_update > 0.5:
self.last_button_update = now
self.update_button_display()
if now - self.last_fuel_update > 1.0:
self.last_fuel_update = now
self.update_fuel_display()
if self.last_rpm_update != 0 and now - self.last_rpm_update > 4:
self.console.set_status('RPM', 'RPM: --', row=8, fg='red')
self.say("Engine stopped")
self.last_rpm_update = 0
if now - self.last_target_update > 1 and self.showJoeZone:
self.last_target_update = now
camera = self.module('camera_ground')
if camera is not None and camera.camera_settings.target_radius > 0:
target = (camera.camera_settings.target_latitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
if target != self.target:
self.showJoeZone = False
self.toggle_JoeZone()
if now - self.last_recall_check > 10:
self.last_recall_check = now
self.check_recall()
self.check_release()
def update_button_display(self):
'''update the Button display on console'''
if self.button_change is None:
return
now = time.time()
time_since_change = (self.button_change.time_boot_ms -
self.button_change.last_change_ms) * 0.001
time_since_change += now - self.button_change_recv_time
if time_since_change > 60:
color = 'black'
self.button_remaining = 0
else:
color = 'red'
self.button_remaining = 60 - time_since_change
remaining = int(self.button_remaining)
self.console.set_status('Button',
'Button: %u' % remaining,
row=8,
fg=color)
if remaining > 0 and now - self.button_announce_time > 60:
self.say("Button pressed")
self.button_announce_time = now
return
if now - self.button_announce_time >= 10 and remaining % 10 == 0 and time_since_change < 65:
self.say("%u seconds" % remaining)
self.button_announce_time = now
def update_fuel_display(self):
'''update the fuel display on console'''
if self.fuel_change is None:
return
now = time.time()
time_since_change = (self.fuel_change.time_boot_ms -
self.fuel_change.last_change_ms) * 0.001
time_since_change += now - self.fuel_change_recv_time
self.console.set_status('FuelPump',
'FuelPump: %u' % int(time_since_change),
row=8,
fg='black')
def rpm_check(self, m):
'''check for correct RPM range'''
thr = self.master.field('VFR_HUD', 'throttle', 0)
if thr >= 100 and m.rpm1 < self.cuav_settings.rpm_threshold:
self.console.set_status('RPM', 'RPM: %u' % m.rpm1, row=8, fg='red')
now = time.time()
if now - self.last_rpm_announce > 20:
self.say("RPM warning")
self.last_rpm_announce = now
def update_airspeed_estimate(self, m):
'''update airspeed estimate for helicopters'''
if self.cuav_settings.wind_speed <= 0:
return
from pymavlink.rotmat import Vector3
wind = Vector3(
self.cuav_settings.wind_speed *
math.cos(math.radians(self.cuav_settings.wind_direction)),
self.cuav_settings.wind_speed *
math.sin(math.radians(self.cuav_settings.wind_direction)), 0)
ground = Vector3(m.vx * 0.01, m.vy * 0.01, 0)
airspeed = ground + wind
self.console.set_status('AirspeedEstimate',
'AirspeedEstimate: %u m/s' % airspeed.length(),
row=8)
def check_fence(self):
try:
sys_status = self.master.messages['SYS_STATUS']
except Exception:
return False
bits = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
present = ((sys_status.onboard_control_sensors_present & bits) == bits)
enabled = ((sys_status.onboard_control_sensors_enabled & bits) == bits)
healthy = ((sys_status.onboard_control_sensors_health & bits) == bits)
if not present or not enabled:
self.console.writeln('Fence should be enabled', fg='blue')
return False
if not healthy:
self.console.writeln('Fence unhealthy', fg='blue')
return False
return True
def check_status(self):
try:
hb = self.master.messages['HEARTBEAT']
mc = self.master.messages['MISSION_CURRENT']
except Exception:
return False
is_armed = (hb.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
if not is_armed and hb.custom_mode == 0:
# disarmed in MANUAL we should be at WP 0
if mc.seq > 1:
self.console.writeln('Incorrect WP %u' % mc.seq, fg='blue')
return False
return True
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
now = time.time()
if m.get_type() == "BUTTON_CHANGE":
if m.state & (1 << self.cuav_settings.fuel_pin):
self.fuel_change = m
self.fuel_change_recv_time = now
self.update_fuel_display()
if m.state & (1 << self.cuav_settings.button_pin):
if self.button_change is None or m.last_change_ms != self.button_change.last_change_ms:
print("button change", m.state)
if self.button_change is not None:
if (m.time_boot_ms < self.button_change.time_boot_ms
and self.button_change.time_boot_ms -
m.time_boot_ms < 30000):
# discard repeated packet from another link if older by less than 30s
return
self.button_change = m
self.button_change_recv_time = now
self.update_button_display()
if m.get_type() == "RPM":
self.console.set_status('RPM', 'RPM: %u' % m.rpm1, row=8)
self.last_rpm_update = now
if m.rpm1 > 50:
if self.last_rpm_value is None:
self.say("Engine started")
self.last_rpm_value = m.rpm1
self.rpm_check(m)
if m.get_type() == "RC_CHANNELS":
v = self.mav_param.get('ICE_START_CHAN', None)
if v is None:
return
v = getattr(m, 'chan%u_raw' % v)
if v <= 1300:
self.console.set_status('ICE', 'ICE: OFF', row=8, fg='red')
elif v >= 1700:
self.console.set_status('ICE', 'ICE: ON', row=8, fg='blue')
else:
self.console.set_status('ICE', 'ICE: AUTO', row=8, fg='green')
if m.get_type(
) == "RANGEFINDER" and 'ATTITUDE' in self.master.messages:
a = self.master.messages['ATTITUDE']
dist = m.distance * math.cos(a.roll) * math.cos(a.pitch)
self.console.set_status('RFind',
'RFind: %.1fm %uft' %
(dist, dist * 3.28084),
row=8)
if m.get_type() == "GLOBAL_POSITION_INT":
self.update_airspeed_estimate(m)
if m.get_type() == 'NAMED_VALUE_FLOAT' and m.name == 'BAT3VOLT':
self.console.set_status('BAT3', 'Bat3: %.2f' % m.value, row=8)
if m.get_type() == 'COLLISION':
if m.action == 0:
color = 'green'
elif m.action == 1:
color = 'blue'
elif m.action == 2:
color = 'orange'
elif m.action == 3:
color = 'darkorange'
elif m.action == 4:
color = 'darkred'
elif m.action == 6:
color = 'yellow'
else:
color = 'red'
self.console.set_status('DNFZ',
'DNFZ %d %.0fm %.0fm %u' %
(m.id, m.horizontal_minimum_delta,
m.altitude_minimum_delta, m.src),
row=6,
fg=color)
if self.rate_period.trigger():
self.check_parameters()
self.check_fence()
self.check_status()
class CameraGroundModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CameraGroundModule,
self).__init__(mpstate,
"camera_ground",
"cuav camera control (ground)",
public=True,
multi_vehicle=True)
self.unload_event = threading.Event()
self.unload_event.clear()
self.transmit_queue = Queue.Queue()
self.decoder = video_play.VideoReader()
self.is_armed = False
self.capture_count = 0
self.image = None
self.last_capture_count = None
self.handled_timestamps = {}
self.viewer = mp_image.MPImage(title='Image',
width=200,
height=200,
auto_size=True)
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.camera_settings = MPSettings([
MPSetting('m_bandwidth',
int,
500,
'max bandwidth on mavlink',
increment=1,
tab='GCS'),
MPSetting('m_maxqueue',
int,
5,
'Maximum images queue for mavlink',
tab='GCS'),
],
title='Camera Settings')
self.msocket = None
self.msend = None
self.last_heartbeat = time.time()
self.transmit_thread = self.start_thread(self.transmit_threadfunc)
self.add_command('camera', self.cmd_camera, 'camera control',
['<status>', 'set (CAMERASETTING)'])
self.add_completion_function('(CAMERASETTING)',
self.camera_settings.completion)
# prevent loopback of messages
for mtype in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
self.module('link').no_fwd_types.add(mtype)
print("camera ground initialised")
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <status|queue|set>"
if len(args) == 0:
print(usage)
return
if args[0] == "status":
print("status....")
elif args[0] == "set":
self.camera_settings.command(args[1:])
else:
print(usage)
def transmit_threadfunc(self):
'''thread for image and message transmit to camera_ground
in addition to reading commands from the camera_ground'''
self.start_bsend()
self.spacewarning = False
while not self.unload_event.wait(0.05):
if self.msend is not None:
self.msend.tick(packet_count=1000,
max_queue=self.camera_settings.m_maxqueue)
self.check_commands(self.msend)
self.send_heartbeats()
while not self.transmit_queue.empty():
(pkt, priority, linktosend) = self.transmit_queue.get()
if self.msend:
self.send_object(pkt, priority, self.msend)
#update the stats
self.xmit_queue = []
self.efficiency = []
self.bandwidth_used = []
self.rtt_estimate = []
if self.msend is not None:
self.xmit_queue.append(self.msend.sendq_size())
self.efficiency.append(self.msend.get_efficiency())
self.bandwidth_used.append(self.msend.get_bandwidth_used())
self.rtt_estimate.append(self.msend.get_rtt_estimate())
def send_heartbeats(self):
'''possibly send heartbeat msgs'''
now = time.time()
if now - self.last_heartbeat > 5:
self.last_heartbeat = now
self.send_heartbeat()
def start_bsend(self):
'''start bsend'''
if self.msend is None:
self.msocket = cuav_command.MavSocket(self.mpstate.mav_master[0])
self.msend = block_xmit.BlockSender(mss=96,
sock=self.msocket,
dest_ip='mavlink',
dest_port=0,
backlog=5,
debug=False)
self.msend.set_bandwidth(self.camera_settings.m_bandwidth)
def start_thread(self, fn):
'''start a thread running'''
t = threading.Thread(target=fn)
t.daemon = True
t.start()
return t
def unload(self):
'''unload module'''
self.running = False
self.unload_event.set()
if self.transmit_thread is not None:
self.transmit_thread.join(1.0)
print('camera unload OK')
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = pickle.loads(buf)
if obj == None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.ImageDelta):
self.handle_image_delta(obj, bsend)
if isinstance(obj, cuav_command.CommandPacket):
self.handle_command_packet(obj, bsend)
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if self.mpstate.status.watch in [
"camera", "queue"
] and time.time() > self.last_watch + 1:
self.last_watch = time.time()
self.cmd_camera([
"status" if self.mpstate.status.watch == "camera" else "queue"
])
if m.get_type(
) == "HEARTBEAT" and m.type != mavutil.mavlink.MAV_TYPE_GCS:
self.is_armed = (m.base_mode
& mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
if self.is_armed:
self.capture_count = 0
if m.get_type() in ['DATA16', 'DATA32', 'DATA64', 'DATA96']:
if self.msocket is not None:
self.msocket.incoming.append(m)
def send_heartbeat(self):
'''send a heartbeat'''
pkt = cuav_command.HeartBeat(self.capture_count)
if self.msend is not None:
self.transmit_queue.put((pkt, None, 'msend'))
def send_message(self, msg):
'''send a message'''
pkt = cuav_command.CameraMessage(msg)
if self.msend is not None:
self.transmit_queue.put((pkt, 100, 'msend'))
def send_object_complete(self, obj, bsend):
'''called on complete of an send_object, cancelling send on other links'''
if obj.blockid is not None:
if self.msend is not None:
self.msend.cancel(obj.blockid)
def send_object(self, obj, priority=None, linktosend=None):
'''send an object to all links if linktosend is none
otherwise just send to the specified link'''
try:
buf = pickle.dumps(obj, pickle.HIGHEST_PROTOCOL)
except Exception as ex:
print("dump failed: ", ex)
return
if priority is None:
priority = 10000
#only send if the queue is not clogged
if not self.msend:
return
obj.blockid = self.msend.send(buf,
priority=priority,
callback=functools.partial(
self.send_object_complete, obj,
self.msend))
def handle_command_packet(self, obj, bsend):
'''handle CommandPacket from other end'''
stdout_saved = sys.stdout
buf = StringIO()
sys.stdout = buf
self.mpstate.functions.process_stdin(obj.command, immediate=True)
sys.stdout = stdout_saved
if str(buf.getvalue().strip()):
pkt = cuav_command.CommandResponse(str(buf.getvalue()).strip())
self.transmit_queue.put((pkt, None, self.msend))
def handle_image_delta(self, obj, bsend):
'''handle a ImageDelta packet'''
if self.capture_count == 0:
if obj.priority < 10000:
print("Skipping early image")
return
s = io.BytesIO()
s.write(obj.delta)
s.seek(0)
(img, dt) = self.decoder.get_image(s)
if img is None:
return
if self.capture_count == 0:
print("Got first image: ", img.shape)
self.capture_count += 1
self.viewer.set_image(img, bgr=True)
def idle_task(self):
'''idle time handler'''
pass
