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