Esempio n. 1
0
 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()
Esempio n. 2
0
 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)'])
Esempio n. 3
0
 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)
Esempio n. 4
0
    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()
Esempio n. 5
0
    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")
Esempio n. 6
0
 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()
Esempio n. 7
0
    def __init__(self):
        self.input_queue = Queue.Queue()
        self.rl = None
        self.console = wxconsole.MessageConsole(title='MAVExplorer')
        self.exit = False
        self.status = MEStatus()
        self.settings = MPSettings(
            [ MPSetting('marker', str, '+', 'data marker', tab='Graph'),
              MPSetting('condition', str, None, 'condition'),
              MPSetting('xaxis', str, None, 'xaxis'),
              MPSetting('linestyle', str, None, 'linestyle'),
              MPSetting('show_flightmode', bool, True, 'show flightmode'),
              MPSetting('legend', str, 'upper left', 'legend position'),
              MPSetting('legend2', str, 'upper right', 'legend2 position')
              ]
            )

        self.mlog = None
        self.command_map = command_map
        self.completions = {
            "set"       : ["(SETTING)"],
            "condition" : ["(VARIABLE)"],
            "graph"     : ['(VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE)'],
            "map"       : ['(VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE)']
            }
        self.aliases = {}
        self.graphs = []
        self.flightmode_selections = []
        self.last_graph = GraphDefinition('Untitled', '', '', [], None)
Esempio n. 8
0
    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)
Esempio n. 9
0
    def __init__(self):
        self.input_queue = multiproc.Queue()
        self.rl = None
        self.console = wxconsole.MessageConsole(title='MAVExplorer')
        self.exit = False
        self.status = MEStatus()
        self.settings = MPSettings([
            MPSetting('marker', str, '+', 'data marker', tab='Graph'),
            MPSetting('condition', str, None, 'condition'),
            MPSetting('xaxis', str, None, 'xaxis'),
            MPSetting('linestyle', str, None, 'linestyle'),
            MPSetting('show_flightmode', bool, True, 'show flightmode'),
            MPSetting('sync_xzoom', bool, True, 'sync X-axis zoom'),
            MPSetting('sync_xmap', bool, True, 'sync X-axis zoom for map'),
            MPSetting('legend', str, 'upper left', 'legend position'),
            MPSetting('legend2', str, 'upper right', 'legend2 position'),
            MPSetting('title', str, None, 'Graph title'),
            MPSetting('debug', int, 0, 'debug level'),
            MPSetting('paramdocs', bool, True, 'show param docs'),
        ])

        self.mlog = None
        self.mav_param = None
        self.filename = None
        self.command_map = command_map
        self.completions = {
            "set": ["(SETTING)"],
            "condition": ["(VARIABLE)"],
            "graph": [
                '(VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE)'
            ],
            "map": ['(VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE)'],
            "param": ['download', 'check', 'help (PARAMETER)'],
        }
        self.aliases = {}
        self.graphs = []
        self.flightmode_selections = []
        self.last_graph = GraphDefinition(self.settings.title, '', '', [],
                                          None)

        #pipe to the wxconsole for any child threads (such as the save dialog box)
        self.parent_pipe_recv_console, self.child_pipe_send_console = multiproc.Pipe(
            duplex=False)
        #pipe for creating graphs (such as from the save dialog box)
        self.parent_pipe_recv_graph, self.child_pipe_send_graph = multiproc.Pipe(
            duplex=False)
        self.param_help = param_help.ParamHelp()

        tConsoleWrite = threading.Thread(target=self.pipeRecvConsole)
        tConsoleWrite.daemon = True
        tConsoleWrite.start()
        tGraphWrite = threading.Thread(target=self.pipeRecvGraph)
        tGraphWrite.daemon = True
        tGraphWrite.start()
Esempio n. 10
0
    def __init__(self):
        self.input_queue = queue.Queue()
        self.rl = None
        self.console = wxconsole.MessageConsole(title='MAVExplorer')
        self.exit = False
        self.status = MEStatus()
        self.settings = MPSettings([
            MPSetting('marker', str, '+', 'data marker', tab='Graph'),
            MPSetting('condition', str, None, 'condition'),
            MPSetting('xaxis', str, None, 'xaxis'),
            MPSetting('linestyle', str, None, 'linestyle'),
            MPSetting('show_flightmode', bool, True, 'show flightmode'),
            MPSetting('legend', str, 'upper left', 'legend position'),
            MPSetting('legend2', str, 'upper right', 'legend2 position')
        ])

        self.mlog = None
        self.command_map = command_map
        self.completions = {
            "set": ["(SETTING)"],
            "condition": ["(VARIABLE)"],
            "graph": [
                '(VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE)'
            ],
            "map": ['(VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE)']
        }
        self.aliases = {}
        self.graphs = []
        self.flightmode_selections = []
        self.last_graph = GraphDefinition('Untitled', '', '', [], None)

        #pipe to the wxconsole for any child threads (such as the save dialog box)
        self.parent_pipe_recv_console, self.child_pipe_send_console = Pipe(
            duplex=False)
        #pipe for creating graphs (such as from the save dialog box)
        self.parent_pipe_recv_graph, self.child_pipe_send_graph = Pipe(
            duplex=False)

        tConsoleWrite = threading.Thread(target=self.pipeRecvConsole)
        tConsoleWrite.daemon = True
        tConsoleWrite.start()
        tGraphWrite = threading.Thread(target=self.pipeRecvGraph)
        tGraphWrite.daemon = True
        tGraphWrite.start()
Esempio n. 11
0
    def __init__(self):
        self.message_count = {}
        self.message_field_count = {}
        self.arrays = dict()
        self.plot_processes = []
        self.send_queues = []
        self.recv_queues = []
        self.master_rect = None
        self.flightmode_list = []
        self.input_queue = Queue.Queue()
        self.rl = None
        #self.console = wxconsole.MessageConsole(title='MAVHawkview')
        self.exit = False
        self.websocket_enabled = None
        
        self.log_max_timestamp = None
        self.log_min_timestamp = None

        
            
        self.status = MEStatus()
        self.settings = MPSettings(
            [ MPSetting('marker', str, '+', 'data marker', tab='Graph'),
              MPSetting('condition', str, None, 'condition'),
              MPSetting('xaxis', str, None, 'xaxis'),
              MPSetting('linestyle', str, None, 'linestyle'),
              MPSetting('flightmode', str, None, 'flightmode', choice=['apm','px4']),
              MPSetting('legend', str, 'upper left', 'legend position'),
              MPSetting('legend2', str, 'upper right', 'legend2 position'),
              MPSetting('grid', str, 'off', 'grid', choice=['on','off'])
              ]
            )

        self.mlog = None
#         self.command_map = command_map
        self.completions = {
            "set"       : ["(SETTING)"],
            "condition" : ["(VARIABLE)"],
            "graph"     : ['(VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE)'],
            "map"       : ['(VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE) (VARIABLE)']
            }
        self.aliases = {}
        self.graphs = []
        self.flightmode_selections = []
Esempio n. 12
0
    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")
Esempio n. 13
0
def process(args):
    '''process a file'''
    file_index = 0

    if os.path.isdir(args[0]):
        files = file_list(args[0], ['jpg', 'pgm', 'png'])
    else:
        files = args

    fname = files[file_index]

    settings = MPSettings([
        MPSetting('MetersPerPixel',
                  float,
                  0.2,
                  range=(0, 10),
                  increment=0.01,
                  digits=2,
                  tab='Image Processing'),
        MPSetting('MinRegionArea',
                  float,
                  0.15,
                  range=(0, 100),
                  increment=0.05,
                  digits=2),
        MPSetting('MaxRegionArea',
                  float,
                  2.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('MinRegionSize',
                  float,
                  0.1,
                  range=(0, 100),
                  increment=0.05,
                  digits=2),
        MPSetting(
            'MaxRegionSize', float, 2, range=(0, 100), increment=0.1,
            digits=1),
        MPSetting('MaxRarityPct',
                  float,
                  0.02,
                  range=(0, 100),
                  increment=0.01,
                  digits=2),
        MPSetting('RegionMergeSize',
                  float,
                  3.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('minscore',
                  int,
                  0,
                  'Min Score',
                  range=(0, 1000),
                  increment=1,
                  tab='Scoring'),
        MPSetting('filter_type',
                  str,
                  'simple',
                  'Filter Type',
                  choice=['simple', 'compactness']),
        MPSetting('brightness',
                  float,
                  1.0,
                  'Display Brightness',
                  range=(0.1, 10),
                  increment=0.1,
                  digits=2,
                  tab='Display')
    ],
                          title='Settings')

    menu = MPMenuSubMenu(
        'View',
        items=[
            MPMenuItem('Original Image', 'Original Image', 'originalImage'),
            MPMenuItem('Unquantised Image', 'Unquantised Image',
                       'unquantisedImage'),
            MPMenuItem('Thresholded Image', 'Thresholded Image',
                       'thresholdedImage'),
            MPMenuItem('Neighbours Image', 'Neighbours Image',
                       'neighboursImage'),
            MPMenuItem('Regions Image', 'Regions Image', 'regionsImage'),
            MPMenuItem('Prune Large Image', 'Prune Large Image',
                       'prunelargeImage'),
            MPMenuItem('Merged Image', 'Merged Large', 'mergedImage'),
            MPMenuItem('Pruned Image', 'Pruned Image', 'prunedImage'),
            MPMenuItem('Fit Window', 'Fit Window', 'fitWindow'),
            MPMenuItem('Full Zoom', 'Full Zoom', 'fullSize'),
            MPMenuItem('Next Image', 'Next Image', 'nextImage'),
            MPMenuItem('Previous Image', 'Previous Image', 'previousImage')
        ])

    im_orig = cuav_util.LoadImage(fname)
    im_numpy = numpy.ascontiguousarray(cv.GetMat(im_orig))
    im_rgb = cv.fromarray(im_numpy)
    cv.CvtColor(im_rgb, im_rgb, cv.CV_BGR2RGB)

    # create the various views
    view = mp_image.MPImage(title='FullImage', can_zoom=True, can_drag=True)
    view.set_popup_menu(menu)

    dlg = wxsettings.WXSettings(settings)

    selected_image = "original"

    while dlg.is_alive() and view.is_alive():
        last_change = settings.last_change()
        scan_parms = {
            'MinRegionArea': settings.MinRegionArea,
            'MaxRegionArea': settings.MaxRegionArea,
            'MinRegionSize': settings.MinRegionSize,
            'MaxRegionSize': settings.MaxRegionSize,
            'MaxRarityPct': settings.MaxRarityPct,
            'RegionMergeSize': settings.RegionMergeSize,
            'SaveIntermediate': 1.0,
            'MetersPerPixel': settings.MetersPerPixel
        }

        t0 = time.time()
        regions = scanner.scan(im_numpy, scan_parms)
        regions = cuav_region.RegionsConvert(regions,
                                             cuav_util.image_shape(im_orig),
                                             cuav_util.image_shape(im_orig),
                                             False)
        t1 = time.time()
        print("Processing %s took %.2f seconds" % (fname, t1 - t0))
        show_image(view, selected_image, im_rgb, fname)

        while last_change == settings.last_change() and dlg.is_alive():
            new_index = file_index
            for event in view.events():
                if isinstance(event, MPMenuItem):
                    if event.returnkey == 'nextImage':
                        new_index = (file_index + 1) % len(files)
                    elif event.returnkey == 'previousImage':
                        new_index = (file_index - 1) % len(files)
                    elif event.returnkey.endswith("Image"):
                        selected_image = event.returnkey[:-5]
                        show_image(view, selected_image, im_rgb, fname)
            if new_index != file_index:
                file_index = new_index
                fname = files[file_index]
                im_orig = cuav_util.LoadImage(fname)
                im_numpy = numpy.ascontiguousarray(cv.GetMat(im_orig))
                im_rgb = cv.fromarray(im_numpy)
                cv.CvtColor(im_rgb, im_rgb, cv.CV_BGR2RGB)
                break
            time.sleep(0.1)
Esempio n. 14
0
def process(folderfile):
    '''process a file'''
    file_index = 0

    files = []
    if os.path.isdir(folderfile):
        files = file_list(folderfile, ['jpg', 'jpeg', 'png'])
    else:
        files.append(folderfile)

    fname = files[file_index]

    settings = MPSettings([
        MPSetting('MetersPerPixel',
                  float,
                  0.2,
                  range=(0, 10),
                  increment=0.01,
                  digits=2,
                  tab='Image Processing'),
        MPSetting('MinRegionArea',
                  float,
                  0.15,
                  range=(0, 100),
                  increment=0.05,
                  digits=2),
        MPSetting('MaxRegionArea',
                  float,
                  2.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('MinRegionSize',
                  float,
                  0.1,
                  range=(0, 100),
                  increment=0.05,
                  digits=2),
        MPSetting(
            'MaxRegionSize', float, 2, range=(0, 100), increment=0.1,
            digits=1),
        MPSetting('MaxRarityPct',
                  float,
                  0.02,
                  range=(0, 100),
                  increment=0.01,
                  digits=2),
        MPSetting('RegionMergeSize',
                  float,
                  3.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('minscore',
                  int,
                  0,
                  'Min Score',
                  range=(0, 1000),
                  increment=1,
                  tab='Scoring'),
        MPSetting(
            'filter_type', str, 'simple', 'Filter Type', choice=['simple']),
        MPSetting('brightness',
                  float,
                  1.0,
                  'Display Brightness',
                  range=(0.1, 10),
                  increment=0.1,
                  digits=2,
                  tab='Display')
    ],
                          title='Settings')

    menu = MPMenuSubMenu(
        'View',
        items=[
            MPMenuItem('Original Image', 'Original Image', '_1original.pnm'),
            MPMenuItem('Unquantised Image', 'Unquantised Image',
                       '_1unquantised.pnm'),
            MPMenuItem('Thresholded Image', 'Thresholded Image',
                       '_2thresholded.pnm'),
            MPMenuItem('Neighbours Image', 'Neighbours Image',
                       '_3neighbours.pnm'),
            MPMenuItem('Regions Image', 'Regions Image', '_4regions.pnm'),
            MPMenuItem('Prune Large Image', 'Prune Large Image',
                       '_5prunelarge.pnm'),
            MPMenuItem('Merged Image', 'Merged Large', '_6merged.pnm'),
            MPMenuItem('Pruned Image', 'Pruned Image', '_7pruned.pnm'),
            MPMenuItem('Fit Window', 'Fit Window', 'fitWindow'),
            MPMenuItem('Full Zoom', 'Full Zoom', 'fullSize'),
            MPMenuItem('Next Image', 'Next Image', 'nextImage'),
            MPMenuItem('Previous Image', 'Previous Image', 'previousImage')
        ])

    im_orig = cv2.imread(fname, -1)
    im_numpy = numpy.ascontiguousarray(im_orig)

    # create the various views
    view = mp_image.MPImage(title='FullImage', can_zoom=True, can_drag=True)
    view.set_popup_menu(menu)

    dlg = wxsettings.WXSettings(settings)

    selected_image = "_1original.pnm"

    while dlg.is_alive() and view.is_alive():
        last_change = settings.last_change()
        scan_parms = {
            'MinRegionArea': settings.MinRegionArea,
            'MaxRegionArea': settings.MaxRegionArea,
            'MinRegionSize': settings.MinRegionSize,
            'MaxRegionSize': settings.MaxRegionSize,
            'MaxRarityPct': settings.MaxRarityPct,
            'RegionMergeSize': settings.RegionMergeSize,
            'SaveIntermediate': 1.0,
            'MetersPerPixel': settings.MetersPerPixel
        }

        t0 = time.time()
        regions = scanner.scan(im_numpy, scan_parms)
        regions = cuav_region.RegionsConvert(regions,
                                             cuav_util.image_shape(im_orig),
                                             cuav_util.image_shape(im_orig),
                                             False)
        t1 = time.time()
        print("Processing %s took %.2f seconds" % (fname, t1 - t0))
        show_image(view, str(file_index + 1) + selected_image, im_orig, fname)

        while last_change == settings.last_change() and dlg.is_alive():
            new_index = file_index
            for event in view.events():
                if isinstance(event, MPMenuItem):
                    if event.returnkey == 'nextImage':
                        new_index = (file_index + 1) % len(files)
                    elif event.returnkey == 'previousImage':
                        new_index = (file_index - 1) % len(files)
                    elif event.returnkey.endswith("pnm"):
                        selected_image = event.returnkey
                        show_image(view,
                                   str(file_index + 1) + selected_image,
                                   im_orig, fname)
                if new_index != file_index:
                    file_index = new_index
                    fname = files[file_index]
                    im_orig = cv2.imread(fname, -1)
                    im_numpy = numpy.ascontiguousarray(im_orig)
                    break
                time.sleep(0.1)
        #remove all the pnm's
        directory = os.getcwd()
        lst = os.listdir(directory)

        for item in lst:
            if item.endswith(".pnm"):
                os.remove(os.path.join(directory, item))
Esempio n. 15
0
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()
Esempio n. 16
0
def process(args):
  '''process a set of files'''

  global slipmap, mosaic
  scan_count = 0
  files = []
  for a in args:
    if os.path.isdir(a):
      files.extend(file_list(a, ['jpg', 'pgm', 'png']))
    else:
      if a.find('*') != -1:
        files.extend(glob.glob(a))
      else:
        files.append(a)
  files.sort()
  num_files = len(files)
  print("num_files=%u" % num_files)
  region_count = 0

  slipmap = mp_slipmap.MPSlipMap(service=opts.service, elevation=True, title='Map')
  icon = slipmap.icon('redplane.png')
  slipmap.add_object(mp_slipmap.SlipIcon('plane', (0,0), icon, layer=3, rotation=0,
                                         follow=True,
                                         trail=mp_slipmap.SlipTrail()))

  for flag in opts.flag:
    a = flag.split(',')
    lat = a[0]
    lon = a[1]
    icon = 'flag.png'
    if len(a) > 2:
      icon = a[2] + '.png'
      icon = slipmap.icon(icon)
      slipmap.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))

  if opts.mission:
    from pymavlink import mavwp
    wp = mavwp.MAVWPLoader()
    wp.load(opts.mission)
    boundary = wp.polygon()
    slipmap.add_object(mp_slipmap.SlipPolygon('mission', boundary, layer=1,
                                              linewidth=1, colour=(255,255,255)))


  if opts.mavlog:
    mpos = mav_position.MavInterpolator()
    mpos.set_logfile(opts.mavlog)
  else:
    mpos = None

  if opts.gammalog is not None:
    gamma = parse_gamma_log(opts.gammalog)
  else:
    gamma = None

  if opts.kmzlog:
    kmzpos = mav_position.KmlPosition(opts.kmzlog)
  else:
    kmzpos = None

  if opts.triggerlog:
    triggerpos = mav_position.TriggerPosition(opts.triggerlog)
  else:
    triggerpos = None

  # create a simple lens model using the focal length
  C_params = cam_params.CameraParams(lens=opts.lens, sensorwidth=opts.sensorwidth)

  if opts.camera_params:
    C_params.load(opts.camera_params)

  camera_settings = MPSettings(
    [ MPSetting('roll_stabilised', bool, opts.roll_stabilised, 'Roll Stabilised'),
      MPSetting('altitude', int, opts.altitude, 'Altitude', range=(0,10000), increment=1),
      MPSetting('minalt', int, 30, 'MinAltitude', range=(0,10000), increment=1),
      MPSetting('mpp100', float, 0.0977, 'MPPat100m', range=(0,10000), increment=0.001),
      MPSetting('filter_type', str, 'simple', 'Filter Type',
                choice=['simple', 'compactness']),
      MPSetting('quality', int, 75, 'Compression Quality', range=(1,100), increment=1),
      MPSetting('thumbsize', int, opts.thumbsize, 'Thumbnail Size', range=(10, 200), increment=1),
      MPSetting('minscore', int, opts.minscore, 'Min Score', range=(0,1000), increment=1, tab='Scoring'),
      MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1,
                digits=2, tab='Display')
      ],
    title='Camera Settings'
    )

  image_settings = MPSettings(
    [ MPSetting('MinRegionArea', float, 0.15, range=(0,100), increment=0.05, digits=2, tab='Image Processing'),
      MPSetting('MaxRegionArea', float, 2.0, range=(0,100), increment=0.1, digits=1),
      MPSetting('MinRegionSize', float, 0.1, range=(0,100), increment=0.05, digits=2),
      MPSetting('MaxRegionSize', float, 2, range=(0,100), increment=0.1, digits=1),
      MPSetting('MaxRarityPct',  float, 0.02, range=(0,100), increment=0.01, digits=2),
      MPSetting('RegionMergeSize', float, 3.0, range=(0,100), increment=0.1, digits=1),
      MPSetting('SaveIntermediate', bool, opts.debug)
      ],
    title='Image Settings')
  
  mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params,
                              camera_settings=camera_settings,
                              image_settings=image_settings,
                              start_menu=True,
                              classify=opts.categories,
                              thumb_size=opts.mosaic_thumbsize)

  joelog = cuav_joe.JoeLog(None)

  if opts.view:
    viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)

  if camera_settings.filter_type == 'compactness':
    calculate_compactness = True
    print("Using compactness filter")
  else:
    calculate_compactness = False

  for f in files:
      if not mosaic.started():
        print("Waiting for startup")
        while not mosaic.started():
          mosaic.check_events()
          time.sleep(0.01)

      if mpos:
        # get the position by interpolating telemetry data from the MAVLink log file
        # this assumes that the filename contains the timestamp 
        if gamma is not None:
          frame_time = parse_gamma_time(f, gamma)
        else:
          frame_time = cuav_util.parse_frame_time(f)
        frame_time += opts.time_offset
        if camera_settings.roll_stabilised:
          roll = 0
        else:
          roll = None
        try:
          pos = mpos.position(frame_time, roll=roll)
        except Exception:
          print("No position available for %s" % frame_time)
          # skip this frame
          continue
      elif kmzpos is not None:
        pos = kmzpos.position(f)
      elif triggerpos is not None:
        pos = triggerpos.position(f)
      else:
        # get the position using EXIF data
        pos = mav_position.exif_position(f)
        pos.time += opts.time_offset

      # update the plane icon on the map
      if pos is not None:
        slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
        if camera_settings.altitude > 0:
          pos.altitude = camera_settings.altitude

      # check for any events from the map
      slipmap.check_events()
      mosaic.check_events()

      im_orig = cuav_util.LoadImage(f)
      (w,h) = cuav_util.image_shape(im_orig)

      if not opts.camera_params:
        C_params.set_resolution(w, h)
      
      im_full = im_orig
        
      im_640 = cv.CreateImage((640, 480), 8, 3)
      cv.Resize(im_full, im_640, cv.CV_INTER_NN)
      im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
      im_full = numpy.ascontiguousarray(cv.GetMat(im_full))

      count = 0
      total_time = 0

      t0=time.time()
      img_scan = im_full

      scan_parms = {}
      for name in image_settings.list():
        scan_parms[name] = image_settings.get(name)
      scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])

      if pos is not None:
        (sw,sh) = cuav_util.image_shape(img_scan)
        altitude = pos.altitude
        if altitude < camera_settings.minalt:
          altitude = camera_settings.minalt
        scan_parms['MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0

        regions = scanner.scan(img_scan, scan_parms)
      else:
        regions = scanner.scan(img_scan)
      regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full), calculate_compactness)
      count += 1
      t1=time.time()

      frame_time = pos.time

      regions = cuav_region.filter_regions(im_full, regions, frame_time=frame_time,
                                           min_score=camera_settings.minscore,
                                           filter_type=camera_settings.filter_type)

      scan_count += 1

      if pos and len(regions) > 0:
        altitude = camera_settings.altitude
        if altitude <= 0:
          altitude = None
        joelog.add_regions(frame_time, regions, pos, f, width=w, height=h,
                           altitude=altitude)

      mosaic.add_image(pos.time, f, pos)

      region_count += len(regions)

      if len(regions) > 0:
          composite = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)), regions)
          thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
          mosaic.add_regions(regions, thumbs, f, pos)

      if opts.view:
        img_view = img_scan
        (wview,hview) = cuav_util.image_shape(img_view)
        mat = cv.fromarray(img_view)
        for r in regions:
          r.draw_rectangle(mat, (255,0,0))
        cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
        viewer.set_image(mat)
        viewer.set_title('Image: ' + os.path.basename(f))

      total_time += (t1-t0)
      if t1 != t0:
          print('%s scan %.1f fps  %u regions [%u/%u]' % (
              os.path.basename(f), count/total_time, region_count, scan_count, num_files))
Esempio n. 17
0
    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")
Esempio n. 18
0
    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")
Esempio n. 19
0
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)
Esempio n. 20
0
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()
Esempio n. 21
0
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()
Esempio n. 22
0
def process(args):
    '''process a set of files'''

    global slipmap, mosaic
    scan_count = 0
    files = []
    for a in args:
        if os.path.isdir(a):
            files.extend(file_list(a, ['jpg', 'pgm', 'png']))
        else:
            if a.find('*') != -1:
                files.extend(glob.glob(a))
            else:
                files.append(a)
    files.sort()
    num_files = len(files)
    print("num_files=%u" % num_files)
    region_count = 0

    slipmap = mp_slipmap.MPSlipMap(service=opts.service,
                                   elevation=True,
                                   title='Map')
    icon = slipmap.icon('redplane.png')
    slipmap.add_object(
        mp_slipmap.SlipIcon('plane', (0, 0),
                            icon,
                            layer=3,
                            rotation=0,
                            follow=True,
                            trail=mp_slipmap.SlipTrail()))

    if opts.mission:
        from pymavlink import mavwp
        wp = mavwp.MAVWPLoader()
        wp.load(opts.mission)
        boundary = wp.polygon()
        slipmap.add_object(
            mp_slipmap.SlipPolygon('mission',
                                   boundary,
                                   layer=1,
                                   linewidth=1,
                                   colour=(255, 255, 255)))

    if opts.mavlog:
        mpos = mav_position.MavInterpolator()
        mpos.set_logfile(opts.mavlog)
    else:
        mpos = None

    if opts.kmzlog:
        kmzpos = mav_position.KmlPosition(opts.kmzlog)
    else:
        kmzpos = None

    if opts.triggerlog:
        triggerpos = mav_position.TriggerPosition(opts.triggerlog)
    else:
        triggerpos = None

    # create a simple lens model using the focal length
    C_params = cam_params.CameraParams(lens=opts.lens,
                                       sensorwidth=opts.sensorwidth)

    if opts.camera_params:
        C_params.load(opts.camera_params)

    camera_settings = MPSettings([
        MPSetting('roll_stabilised', bool, True, 'Roll Stabilised'),
        MPSetting(
            'altitude', int, 0, 'Altitude', range=(0, 10000), increment=1),
        MPSetting('filter_type',
                  str,
                  'simple',
                  'Filter Type',
                  choice=['simple', 'compactness']),
        MPSetting('fullres', bool, False, 'Full Resolution'),
        MPSetting('quality',
                  int,
                  75,
                  'Compression Quality',
                  range=(1, 100),
                  increment=1),
        MPSetting('thumbsize',
                  int,
                  60,
                  'Thumbnail Size',
                  range=(10, 200),
                  increment=1),
        MPSetting('minscore',
                  int,
                  75,
                  'Min Score',
                  range=(0, 1000),
                  increment=1,
                  tab='Scoring'),
        MPSetting('brightness',
                  float,
                  1.0,
                  'Display Brightness',
                  range=(0.1, 10),
                  increment=0.1,
                  digits=2,
                  tab='Display')
    ],
                                 title='Camera Settings')

    image_settings = MPSettings([
        MPSetting('MinRegionArea',
                  float,
                  0.15,
                  range=(0, 100),
                  increment=0.05,
                  digits=2,
                  tab='Image Processing'),
        MPSetting('MaxRegionArea',
                  float,
                  2.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('MinRegionSize',
                  float,
                  0.1,
                  range=(0, 100),
                  increment=0.05,
                  digits=2),
        MPSetting(
            'MaxRegionSize', float, 2, range=(0, 100), increment=0.1,
            digits=1),
        MPSetting('MaxRarityPct',
                  float,
                  0.02,
                  range=(0, 100),
                  increment=0.01,
                  digits=2),
        MPSetting('RegionMergeSize',
                  float,
                  3.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('SaveIntermediate', bool, False)
    ],
                                title='Image Settings')

    mosaic = cuav_mosaic.Mosaic(slipmap,
                                C=C_params,
                                camera_settings=camera_settings,
                                image_settings=image_settings,
                                start_menu=True)

    joelog = cuav_joe.JoeLog(None)

    if opts.view:
        viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)

    if camera_settings.filter_type == 'compactness':
        calculate_compactness = True
        print("Using compactness filter")
    else:
        calculate_compactness = False

    for f in files:
        if not mosaic.started():
            print("Waiting for startup")
            while not mosaic.started():
                mosaic.check_events()
                time.sleep(0.01)

        if mpos:
            # get the position by interpolating telemetry data from the MAVLink log file
            # this assumes that the filename contains the timestamp
            frame_time = cuav_util.parse_frame_time(f) + opts.time_offset
            if camera_settings.roll_stabilised:
                roll = 0
            else:
                roll = None
            try:
                pos = mpos.position(frame_time, roll=roll)
            except Exception:
                print("No position available for %s" % frame_time)
                # skip this frame
                continue
        elif kmzpos is not None:
            pos = kmzpos.position(f)
        elif triggerpos is not None:
            pos = triggerpos.position(f)
        else:
            # get the position using EXIF data
            pos = mav_position.exif_position(f)
            pos.time += opts.time_offset

        # update the plane icon on the map
        if pos is not None:
            slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
            if camera_settings.altitude > 0:
                pos.altitude = camera_settings.altitude

        # check for any events from the map
        slipmap.check_events()
        mosaic.check_events()

        im_orig = cuav_util.LoadImage(f)
        (w, h) = cuav_util.image_shape(im_orig)

        if not opts.camera_params:
            C_params.set_resolution(w, h)

        im_full = im_orig

        im_640 = cv.CreateImage((640, 480), 8, 3)
        cv.Resize(im_full, im_640, cv.CV_INTER_NN)
        im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
        im_full = numpy.ascontiguousarray(cv.GetMat(im_full))

        count = 0
        total_time = 0

        t0 = time.time()
        if camera_settings.fullres:
            img_scan = im_full
        else:
            img_scan = im_640

        scan_parms = {}
        for name in image_settings.list():
            scan_parms[name] = image_settings.get(name)
        scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])

        if pos is not None:
            (sw, sh) = cuav_util.image_shape(img_scan)
            mpp = cuav_util.meters_per_pixel(pos, C=C_params)
            if mpp is not None:
                scan_parms['MetersPerPixel'] = mpp * (w / float(sw))
            regions = scanner.scan(img_scan, scan_parms)
        else:
            regions = scanner.scan(img_scan)
        regions = cuav_region.RegionsConvert(regions,
                                             cuav_util.image_shape(img_scan),
                                             cuav_util.image_shape(im_full),
                                             calculate_compactness)
        count += 1
        t1 = time.time()

        frame_time = pos.time

        regions = cuav_region.filter_regions(
            im_full,
            regions,
            frame_time=frame_time,
            min_score=camera_settings.minscore,
            filter_type=camera_settings.filter_type)

        scan_count += 1

        mosaic.add_image(pos.time, f, pos)

        if pos and len(regions) > 0:
            altitude = camera_settings.altitude
            if altitude <= 0:
                altitude = None
            joelog.add_regions(frame_time,
                               regions,
                               pos,
                               f,
                               width=w,
                               height=h,
                               altitude=altitude)

        region_count += len(regions)

        if len(regions) > 0:
            composite = cuav_mosaic.CompositeThumbnail(
                cv.GetImage(cv.fromarray(im_full)), regions)
            thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
            mosaic.add_regions(regions, thumbs, f, pos)

        if opts.view:
            img_view = img_scan
            (wview, hview) = cuav_util.image_shape(img_view)
            mat = cv.fromarray(img_view)
            for r in regions:
                r.draw_rectangle(mat, (255, 0, 0))
            cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
            viewer.set_image(mat)
            viewer.set_title('Image: ' + os.path.basename(f))

        total_time += (t1 - t0)
        if t1 != t0:
            print('%s scan %.1f fps  %u regions [%u/%u]' %
                  (os.path.basename(f), count / total_time, region_count,
                   scan_count, num_files))
Esempio n. 23
0
                              mouse_events=False,
                              key_events=True,
                              can_zoom=True,
                              can_drag=True)

menu = MPMenuTop([])
view_menu = MPMenuSubMenu('View',
                          [MPMenuItem('Next Image\tCtrl+N', 'Next Image', 'nextImage'),
                           MPMenuItem('Previous Image\tCtrl+P', 'Previous Image', 'previousImage')
                          ])
menu.add(view_menu)
view_image.set_menu(menu)


settings = MPSettings(
    [ MPSetting('threshold', int, 6100, 'High Threshold', tab='Settings', range=(0,65535)),
      MPSetting('blue_threshold', float, 0.75, 'Blue Threshold', range=(0,1)),
      MPSetting('green_threshold', float, 0.4, 'Green Threshold', range=(0,1))])

changed = True

def settings_callback(setting):
    '''called on a changed setting'''
    global changed
    changed = True

settings.set_callback(settings_callback)

WXSettings(settings)

image_idx = 0
Esempio n. 24
0
    def __init__(self):
        self.console = textconsole.SimpleConsole()
        self.map = None
        self.map_functions = {}
        self.vehicle_type = None
        self.vehicle_name = None
        from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
        self.settings = MPSettings([
            MPSetting('link',
                      int,
                      1,
                      'Primary Link',
                      tab='Link',
                      range=(0, 4),
                      increment=1),
            MPSetting('streamrate',
                      int,
                      4,
                      'Stream rate link1',
                      range=(-1, 20),
                      increment=1),
            MPSetting('streamrate2',
                      int,
                      4,
                      'Stream rate link2',
                      range=(-1, 20),
                      increment=1),
            MPSetting('heartbeat',
                      int,
                      1,
                      'Heartbeat rate',
                      range=(0, 5),
                      increment=1),
            MPSetting('mavfwd', bool, True, 'Allow forwarded control'),
            MPSetting('mavfwd_rate', bool, False,
                      'Allow forwarded rate control'),
            MPSetting('shownoise', bool, True, 'Show non-MAVLink data'),
            MPSetting('baudrate',
                      int,
                      opts.baudrate,
                      'baudrate for new links',
                      range=(0, 10000000),
                      increment=1),
            MPSetting('rtscts', bool, opts.rtscts, 'enable flow control'),
            MPSetting('select_timeout', float, 0.5, 'select timeout'),
            MPSetting('altreadout',
                      int,
                      10,
                      'Altitude Readout',
                      range=(0, 100),
                      increment=1,
                      tab='Announcements'),
            MPSetting('distreadout',
                      int,
                      200,
                      'Distance Readout',
                      range=(0, 10000),
                      increment=1),
            MPSetting('moddebug',
                      int,
                      0,
                      'Module Debug Level',
                      range=(0, 3),
                      increment=1,
                      tab='Debug'),
            MPSetting('compdebug',
                      int,
                      0,
                      'Computation Debug Mask',
                      range=(0, 3),
                      tab='Debug'),
            MPSetting('flushlogs', bool, False, 'Flush logs on every packet'),
            MPSetting('requireexit', bool, False, 'Require exit command'),
            MPSetting('wpupdates', bool, True, 'Announce waypoint updates'),
            MPSetting('basealt',
                      int,
                      0,
                      'Base Altitude',
                      range=(0, 30000),
                      increment=1,
                      tab='Altitude'),
            MPSetting('wpalt',
                      int,
                      100,
                      'Default WP Altitude',
                      range=(0, 10000),
                      increment=1),
            MPSetting('rallyalt',
                      int,
                      90,
                      'Default Rally Altitude',
                      range=(0, 10000),
                      increment=1),
            MPSetting('terrainalt',
                      str,
                      'Auto',
                      'Use terrain altitudes',
                      choice=['Auto', 'True', 'False']),
            MPSetting('rally_breakalt',
                      int,
                      40,
                      'Default Rally Break Altitude',
                      range=(0, 10000),
                      increment=1),
            MPSetting('rally_flags',
                      int,
                      0,
                      'Default Rally Flags',
                      range=(0, 10000),
                      increment=1),
            MPSetting('source_system',
                      int,
                      255,
                      'MAVLink Source system',
                      range=(0, 255),
                      increment=1,
                      tab='MAVLink'),
            MPSetting('source_component',
                      int,
                      0,
                      'MAVLink Source component',
                      range=(0, 255),
                      increment=1),
            MPSetting('target_system',
                      int,
                      0,
                      'MAVLink target system',
                      range=(0, 255),
                      increment=1),
            MPSetting('target_component',
                      int,
                      0,
                      'MAVLink target component',
                      range=(0, 255),
                      increment=1)
        ])

        self.completions = {
            "script": ["(FILENAME)"],
            "set": ["(SETTING)"],
            "module":
            ["list", "load (AVAILMODULES)", "<unload|reload> (LOADEDMODULES)"]
        }

        self.status = MPStatus()

        # master mavlink device
        self.mav_master = None

        # mavlink outputs
        self.mav_outputs = []

        # SITL output
        self.sitl_output = None

        self.mav_param = mavparm.MAVParmDict()
        self.modules = []
        self.modules_idle = []  # modules with idle callbacks
        self.modules_packet = []  # modules that want to read packets
        self.public_modules = {}
        self.functions = MAVFunctions()
        self.select_extra = {}
        self.continue_mode = False
        self.aliases = {}
        self.max_rx_packets = None
        self.logqueue = None
        self.logqueue_raw = None
        self.rx_blacklist = set(
        )  # A set of message types which could never be delegated to packet handlers (for use by DroneAPI high speed processing)
Esempio n. 25
0
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))
Esempio n. 26
0
    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")
Esempio n. 27
0
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))
Esempio n. 28
0
File: algtest.py Progetto: 1ee7/cuav
def process(args):
	'''process a file'''
        file_index = 0

        if os.path.isdir(args[0]):
          files = file_list(args[0], ['jpg', 'pgm', 'png'])
        else:
          files = args

        fname = files[file_index]

        settings = MPSettings(
          [
          MPSetting('MetersPerPixel', float, 0.2, range=(0,10), increment=0.01, digits=2, tab='Image Processing'),
          MPSetting('MinRegionArea', float, 0.15, range=(0,100), increment=0.05, digits=2),
          MPSetting('MaxRegionArea', float, 2.0, range=(0,100), increment=0.1, digits=1),
          MPSetting('MinRegionSize', float, 0.1, range=(0,100), increment=0.05, digits=2),
          MPSetting('MaxRegionSize', float, 2, range=(0,100), increment=0.1, digits=1),
          MPSetting('MaxRarityPct',  float, 0.02, range=(0,100), increment=0.01, digits=2),
          MPSetting('RegionMergeSize', float, 3.0, range=(0,100), increment=0.1, digits=1),
          MPSetting('minscore', int, 0, 'Min Score', range=(0,1000), increment=1, tab='Scoring'),
          MPSetting('filter_type', str, 'simple', 'Filter Type', choice=['simple', 'compactness']),
          MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1,
                    digits=2, tab='Display')
          ],
          title='Settings'
          )

        menu = MPMenuSubMenu('View',
                             items=[
          MPMenuItem('Original Image', 'Original Image', 'originalImage'),
          MPMenuItem('Unquantised Image', 'Unquantised Image', 'unquantisedImage'),
          MPMenuItem('Thresholded Image', 'Thresholded Image', 'thresholdedImage'),
          MPMenuItem('Neighbours Image', 'Neighbours Image', 'neighboursImage'),
          MPMenuItem('Regions Image', 'Regions Image', 'regionsImage'),
          MPMenuItem('Prune Large Image', 'Prune Large Image', 'prunelargeImage'),
          MPMenuItem('Merged Image', 'Merged Large', 'mergedImage'),
          MPMenuItem('Pruned Image', 'Pruned Image', 'prunedImage'),
          MPMenuItem('Fit Window', 'Fit Window', 'fitWindow'),
          MPMenuItem('Full Zoom',  'Full Zoom', 'fullSize'),
          MPMenuItem('Next Image', 'Next Image', 'nextImage'),
          MPMenuItem('Previous Image',  'Previous Image', 'previousImage')])
        
        im_orig = cuav_util.LoadImage(fname)
        im_numpy = numpy.ascontiguousarray(cv.GetMat(im_orig))
        im_rgb = cv.fromarray(im_numpy)
        cv.CvtColor(im_rgb, im_rgb, cv.CV_BGR2RGB)

        # create the various views
        view = mp_image.MPImage(title='FullImage', can_zoom=True, can_drag=True)
        view.set_popup_menu(menu)

        dlg = wxsettings.WXSettings(settings)

        selected_image = "original"
        
        while dlg.is_alive() and view.is_alive():
        	last_change = settings.last_change()
                scan_parms = {
                  'MinRegionArea' : settings.MinRegionArea,
                  'MaxRegionArea' : settings.MaxRegionArea,
                  'MinRegionSize' : settings.MinRegionSize,
                  'MaxRegionSize' : settings.MaxRegionSize,
                  'MaxRarityPct'  : settings.MaxRarityPct,
                  'RegionMergeSize' : settings.RegionMergeSize,
                  'SaveIntermediate' : 1.0,
                  'MetersPerPixel' : settings.MetersPerPixel
                  }

                t0 = time.time()
                regions = scanner.scan(im_numpy, scan_parms)
                regions = cuav_region.RegionsConvert(regions,
                                                     cuav_util.image_shape(im_orig),
                                                     cuav_util.image_shape(im_orig), False)    
                t1=time.time()
                print("Processing %s took %.2f seconds" % (fname, t1-t0))
                show_image(view, selected_image, im_rgb, fname)

                while last_change == settings.last_change() and dlg.is_alive():
                	new_index = file_index
                	for event in view.events():
                        	if isinstance(event, MPMenuItem):
                                        if event.returnkey == 'nextImage':
                                        	new_index = (file_index + 1) % len(files)
                                        elif event.returnkey == 'previousImage':
                                        	new_index = (file_index - 1) % len(files)
                                	elif event.returnkey.endswith("Image"):
                                        	selected_image = event.returnkey[:-5]
                                                show_image(view, selected_image, im_rgb, fname)
                        if new_index != file_index:
                        	file_index = new_index
                                fname = files[file_index]
                                im_orig = cuav_util.LoadImage(fname)
                                im_numpy = numpy.ascontiguousarray(cv.GetMat(im_orig))
                                im_rgb = cv.fromarray(im_numpy)
                                cv.CvtColor(im_rgb, im_rgb, cv.CV_BGR2RGB)
                                break
                        time.sleep(0.1)
Esempio n. 29
0
def process(args):
    """process a set of files"""

    global slipmap, mosaic
    scan_count = 0
    files = []
    for a in args:
        if os.path.isdir(a):
            files.extend(file_list(a, ["jpg", "pgm", "png"]))
        else:
            if a.find("*") != -1:
                files.extend(glob.glob(a))
            else:
                files.append(a)
    files.sort()
    num_files = len(files)
    print("num_files=%u" % num_files)
    region_count = 0

    slipmap = mp_slipmap.MPSlipMap(service=opts.service, elevation=True, title="Map")
    icon = slipmap.icon("redplane.png")
    slipmap.add_object(
        mp_slipmap.SlipIcon("plane", (0, 0), icon, layer=3, rotation=0, follow=True, trail=mp_slipmap.SlipTrail())
    )

    for flag in opts.flag:
        a = flag.split(",")
        lat = a[0]
        lon = a[1]
        icon = "flag.png"
        if len(a) > 2:
            icon = a[2] + ".png"
            icon = slipmap.icon(icon)
            slipmap.add_object(
                mp_slipmap.SlipIcon(
                    "icon - %s" % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False
                )
            )

    if opts.mission:
        from pymavlink import mavwp

        wp = mavwp.MAVWPLoader()
        wp.load(opts.mission)
        plist = wp.polygon_list()
        if len(plist) > 0:
            for i in range(len(plist)):
                slipmap.add_object(
                    mp_slipmap.SlipPolygon(
                        "Mission-%s-%u" % (opts.mission, i),
                        plist[i],
                        layer="Mission",
                        linewidth=2,
                        colour=(255, 255, 255),
                    )
                )

    if opts.mavlog:
        mpos = mav_position.MavInterpolator()
        mpos.set_logfile(opts.mavlog)
    else:
        mpos = None

    if opts.gammalog is not None:
        gamma = parse_gamma_log(opts.gammalog)
    else:
        gamma = None

    if opts.kmzlog:
        kmzpos = mav_position.KmlPosition(opts.kmzlog)
    else:
        kmzpos = None

    if opts.triggerlog:
        triggerpos = mav_position.TriggerPosition(opts.triggerlog)
    else:
        triggerpos = None

    # create a simple lens model using the focal length
    C_params = cam_params.CameraParams(lens=opts.lens, sensorwidth=opts.sensorwidth)

    if opts.camera_params:
        C_params.load(opts.camera_params)

    if opts.target:
        target = opts.target.split(",")
    else:
        target = [0, 0, 0]

    camera_settings = MPSettings(
        [
            MPSetting("roll_stabilised", bool, opts.roll_stabilised, "Roll Stabilised"),
            MPSetting("altitude", int, opts.altitude, "Altitude", range=(0, 10000), increment=1),
            MPSetting("minalt", int, 30, "MinAltitude", range=(0, 10000), increment=1),
            MPSetting("mpp100", float, 0.0977, "MPPat100m", range=(0, 10000), increment=0.001),
            MPSetting("rotate180", bool, opts.rotate_180, "rotate180"),
            MPSetting("filter_type", str, "compactness", "Filter Type", choice=["simple", "compactness"]),
            MPSetting("target_lattitude", float, float(target[0]), "target latitude", increment=1.0e-7),
            MPSetting("target_longitude", float, float(target[1]), "target longitude", increment=1.0e-7),
            MPSetting("target_radius", float, float(target[2]), "target radius", increment=1),
            MPSetting("quality", int, 75, "Compression Quality", range=(1, 100), increment=1),
            MPSetting("thumbsize", int, opts.thumbsize, "Thumbnail Size", range=(10, 200), increment=1),
            MPSetting("minscore", int, opts.minscore, "Min Score", range=(0, 1000), increment=1, tab="Scoring"),
            MPSetting(
                "brightness", float, 1.0, "Display Brightness", range=(0.1, 10), increment=0.1, digits=2, tab="Display"
            ),
        ],
        title="Camera Settings",
    )

    image_settings = MPSettings(
        [
            MPSetting("MinRegionArea", float, 0.05, range=(0, 100), increment=0.05, digits=2, tab="Image Processing"),
            MPSetting("MaxRegionArea", float, 4.0, range=(0, 100), increment=0.1, digits=1),
            MPSetting("MinRegionSize", float, 0.02, range=(0, 100), increment=0.05, digits=2),
            MPSetting("MaxRegionSize", float, 3.0, range=(0, 100), increment=0.1, digits=1),
            MPSetting("MaxRarityPct", float, 0.02, range=(0, 100), increment=0.01, digits=2),
            MPSetting("RegionMergeSize", float, 1.0, range=(0, 100), increment=0.1, digits=1),
            MPSetting("BlueEmphasis", bool, opts.blue_emphasis),
            MPSetting("SaveIntermediate", bool, opts.debug),
        ],
        title="Image Settings",
    )

    mosaic = cuav_mosaic.Mosaic(
        slipmap,
        C=C_params,
        camera_settings=camera_settings,
        image_settings=image_settings,
        start_menu=True,
        classify=opts.categories,
        thumb_size=opts.mosaic_thumbsize,
    )

    joelog = cuav_joe.JoeLog(None)

    if opts.view:
        viewer = mp_image.MPImage(title="Image", can_zoom=True, can_drag=True)

    for f in files:
        if not mosaic.started():
            print("Waiting for startup")
            while not mosaic.started():
                mosaic.check_events()
                time.sleep(0.01)

        if mpos:
            # get the position by interpolating telemetry data from the MAVLink log file
            # this assumes that the filename contains the timestamp
            if gamma is not None:
                frame_time = parse_gamma_time(f, gamma)
            else:
                frame_time = cuav_util.parse_frame_time(f)
            frame_time += opts.time_offset
            if camera_settings.roll_stabilised:
                roll = 0
            else:
                roll = None
            try:
                pos = mpos.position(frame_time, roll=roll)
            except Exception:
                print("No position available for %s" % frame_time)
                # skip this frame
                continue
        elif kmzpos is not None:
            pos = kmzpos.position(f)
        elif triggerpos is not None:
            pos = triggerpos.position(f)
        else:
            # get the position using EXIF data
            pos = mav_position.exif_position(f)
            pos.time += opts.time_offset

        # update the plane icon on the map
        if pos is not None:
            slipmap.set_position("plane", (pos.lat, pos.lon), rotation=pos.yaw)
            if camera_settings.altitude > 0:
                pos.altitude = camera_settings.altitude

        # check for any events from the map
        slipmap.check_events()
        mosaic.check_events()

        im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180)
        if im_orig is None:
            continue
        (w, h) = cuav_util.image_shape(im_orig)

        if not opts.camera_params:
            C_params.set_resolution(w, h)

        im_full = im_orig

        im_640 = cv.CreateImage((640, 480), 8, 3)
        cv.Resize(im_full, im_640, cv.CV_INTER_NN)
        im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
        im_full = numpy.ascontiguousarray(cv.GetMat(im_full))

        count = 0
        total_time = 0

        t0 = time.time()
        img_scan = im_full

        scan_parms = {}
        for name in image_settings.list():
            scan_parms[name] = image_settings.get(name)
        scan_parms["SaveIntermediate"] = float(scan_parms["SaveIntermediate"])
        scan_parms["BlueEmphasis"] = float(scan_parms["BlueEmphasis"])

        if pos is not None:
            (sw, sh) = cuav_util.image_shape(img_scan)
            altitude = pos.altitude
            if altitude < camera_settings.minalt:
                altitude = camera_settings.minalt
            scan_parms["MetersPerPixel"] = camera_settings.mpp100 * altitude / 100.0

            regions = scanner.scan(img_scan, scan_parms)
        else:
            regions = scanner.scan(img_scan)
        regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full))
        count += 1
        t1 = time.time()

        frame_time = pos.time

        if pos:
            for r in regions:
                r.latlon = cuav_util.gps_position_from_image_region(r, pos, w, h, altitude=altitude)

            if camera_settings.target_radius > 0 and pos is not None:
                regions = cuav_region.filter_radius(
                    regions,
                    (camera_settings.target_lattitude, camera_settings.target_longitude),
                    camera_settings.target_radius,
                )

        regions = cuav_region.filter_regions(
            im_full,
            regions,
            frame_time=frame_time,
            min_score=camera_settings.minscore,
            filter_type=camera_settings.filter_type,
        )

        scan_count += 1

        if pos and len(regions) > 0:
            altitude = camera_settings.altitude
            if altitude <= 0:
                altitude = None
            joelog.add_regions(frame_time, regions, pos, f, width=w, height=h, altitude=altitude)

        mosaic.add_image(pos.time, f, pos)

        region_count += len(regions)

        if len(regions) > 0:
            composite = cuav_mosaic.CompositeThumbnail(cv.GetImage(cv.fromarray(im_full)), regions)
            thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
            mosaic.add_regions(regions, thumbs, f, pos)

        if opts.view:
            img_view = img_scan
            (wview, hview) = cuav_util.image_shape(img_view)
            mat = cv.fromarray(img_view)
            for r in regions:
                r.draw_rectangle(mat, (255, 0, 0))
            cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
            viewer.set_image(mat)
            viewer.set_title("Image: " + os.path.basename(f))
            if opts.saveview:
                cv.CvtColor(mat, mat, cv.CV_RGB2BGR)
                cv.SaveImage("view-" + os.path.basename(f), mat)

        total_time += t1 - t0
        if t1 != t0:
            print(
                "%s scan %.1f fps  %u regions [%u/%u]"
                % (os.path.basename(f), count / total_time, region_count, scan_count, num_files)
            )
        # raw_input("hit ENTER when ready")

    print("All images processed")
    while True:
        # check for any events from the map
        slipmap.check_events()
        mosaic.check_events()
        time.sleep(0.2)
Esempio n. 30
0
    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)
Esempio n. 31
0
def process(args):
    '''process a set of files'''

    global slipmap, mosaic
    scan_count = 0
    files = []
    if os.path.isdir(args.directory):
        files.extend(file_list(args.directory, ['jpg', 'pgm', 'png']))
    else:
        if args.directory.find('*') != -1:
            files.extend(glob.glob(args.directory))
        else:
            files.append(args.directory)
    files.sort()
    num_files = len(files)
    print("num_files=%u" % num_files)
    region_count = 0

    slipmap = mp_slipmap.MPSlipMap(service=args.service,
                                   elevation=True,
                                   title='Map')
    icon = slipmap.icon('redplane.png')
    slipmap.add_object(
        mp_slipmap.SlipIcon('plane', (0, 0),
                            icon,
                            layer=3,
                            rotation=0,
                            follow=True,
                            trail=mp_slipmap.SlipTrail()))

    for flag in args.flag:
        a = flag.split(',')
        lat = a[0]
        lon = a[1]
        icon = 'flag.png'
        if len(a) > 2:
            icon = a[2] + '.png'
            icon = slipmap.icon(icon)
            slipmap.add_object(
                mp_slipmap.SlipIcon('icon - %s' % str(flag),
                                    (float(lat), float(lon)),
                                    icon,
                                    layer=3,
                                    rotation=0,
                                    follow=False))

    if args.mission:
        from pymavlink import mavwp
        wp = mavwp.MAVWPLoader()
        wp.load(args.mission.name)
        plist = wp.polygon_list()
        if len(plist) > 0:
            for i in range(len(plist)):
                slipmap.add_object(
                    mp_slipmap.SlipPolygon('Mission-%s-%u' %
                                           (args.mission.name, i),
                                           plist[i],
                                           layer='Mission',
                                           linewidth=2,
                                           colour=(255, 255, 255)))

    if args.mavlog:
        mpos = mav_position.MavInterpolator()
        mpos.set_logfile(args.mavlog.name)
    else:
        mpos = None

    if args.gammalog is not None:
        gamma = parse_gamma_log(args.gammalog)
    else:
        gamma = None

    if args.kmzlog:
        kmzpos = mav_position.KmlPosition(args.kmzlog.name)
    else:
        kmzpos = None

    if args.triggerlog:
        triggerpos = mav_position.TriggerPosition(args.triggerlog.name)
    else:
        triggerpos = None

    # create a simple lens model using the focal length

    if args.camera_params:
        C_params = cam_params.CameraParams.fromfile(args.camera_params.name)
    else:
        C_params = cam_params.CameraParams(lens=args.lens,
                                           sensorwidth=args.sensorwidth,
                                           xresolution=args.xresolution,
                                           yresolution=args.yresolution)

    if args.target:
        target = args.target.split(',')
    else:
        target = [0, 0, 0]

    camera_settings = MPSettings([
        MPSetting('roll_stabilised', bool, args.roll_stabilised,
                  'Roll Stabilised'),
        MPSetting('altitude',
                  int,
                  args.altitude,
                  'Altitude',
                  range=(0, 10000),
                  increment=1),
        MPSetting(
            'minalt', int, 30, 'MinAltitude', range=(0, 10000), increment=1),
        MPSetting('mpp100',
                  float,
                  0.0977,
                  'MPPat100m',
                  range=(0, 10000),
                  increment=0.001),
        MPSetting('rotate180', bool, args.rotate_180, 'rotate180'),
        MPSetting('filter_type',
                  str,
                  'compactness',
                  'Filter Type',
                  choice=['simple', 'compactness']),
        MPSetting('target_lattitude',
                  float,
                  float(target[0]),
                  'target latitude',
                  increment=1.0e-7),
        MPSetting('target_longitude',
                  float,
                  float(target[1]),
                  'target longitude',
                  increment=1.0e-7),
        MPSetting('target_radius',
                  float,
                  float(target[2]),
                  'target radius',
                  increment=1),
        MPSetting('quality',
                  int,
                  75,
                  'Compression Quality',
                  range=(1, 100),
                  increment=1),
        MPSetting('thumbsize',
                  int,
                  args.thumbsize,
                  'Thumbnail Size',
                  range=(10, 200),
                  increment=1),
        MPSetting('minscore',
                  int,
                  args.minscore,
                  'Min Score',
                  range=(0, 1000),
                  increment=1,
                  tab='Scoring'),
        MPSetting('brightness',
                  float,
                  1.0,
                  'Display Brightness',
                  range=(0.1, 10),
                  increment=0.1,
                  digits=2,
                  tab='Display'),
    ],
                                 title='Camera Settings')

    image_settings = MPSettings([
        MPSetting('MinRegionArea',
                  float,
                  0.05,
                  range=(0, 100),
                  increment=0.05,
                  digits=2,
                  tab='Image Processing'),
        MPSetting('MaxRegionArea',
                  float,
                  4.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('MinRegionSize',
                  float,
                  0.02,
                  range=(0, 100),
                  increment=0.05,
                  digits=2),
        MPSetting('MaxRegionSize',
                  float,
                  3.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('MaxRarityPct',
                  float,
                  0.02,
                  range=(0, 100),
                  increment=0.01,
                  digits=2),
        MPSetting('RegionMergeSize',
                  float,
                  1.0,
                  range=(0, 100),
                  increment=0.1,
                  digits=1),
        MPSetting('BlueEmphasis', bool, args.blue_emphasis),
        MPSetting('SaveIntermediate', bool, args.debug)
    ],
                                title='Image Settings')

    mosaic = cuav_mosaic.Mosaic(slipmap,
                                C=C_params,
                                camera_settings=camera_settings,
                                image_settings=image_settings,
                                start_menu=True,
                                classify=args.categories,
                                thumb_size=args.mosaic_thumbsize)

    joelog = cuav_joe.JoeLog(None)

    if args.view:
        viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)

    start_time = time.time()
    for f in files:
        if not mosaic.started():
            print("Waiting for startup")
            if args.start:
                mosaic.has_started = True
            while not mosaic.started():
                mosaic.check_events()
                time.sleep(0.01)

        if mpos:
            # get the position by interpolating telemetry data from the MAVLink log file
            # this assumes that the filename contains the timestamp
            if gamma is not None:
                frame_time = parse_gamma_time(f, gamma)
            else:
                frame_time = cuav_util.parse_frame_time(f)
            frame_time += args.time_offset
            if camera_settings.roll_stabilised:
                roll = 0
            else:
                roll = None
            try:
                pos = mpos.position(frame_time, roll=roll)
            except Exception:
                print("No position available for %s" % frame_time)
                # skip this frame
                continue
        elif kmzpos is not None:
            pos = kmzpos.position(f)
        elif triggerpos is not None:
            pos = triggerpos.position(f)
        else:
            # get the position using EXIF data
            pos = mav_position.exif_position(f)
            pos.time += args.time_offset

        # update the plane icon on the map
        if pos is not None:
            slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
            if camera_settings.altitude > 0:
                pos.altitude = camera_settings.altitude

        # check for any events from the map
        slipmap.check_events()
        mosaic.check_events()

        im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180)
        if im_orig is None:
            continue
        (w, h) = cuav_util.image_shape(im_orig)

        if False:
            im_640 = cv.CreateImage((640, 480), 8, 3)
            cv.Resize(im_full, im_640, cv.CV_INTER_NN)
            im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))

        im_full = im_orig
        im_full = numpy.ascontiguousarray(cv.GetMat(im_full))

        count = 0
        total_time = 0

        t0 = time.time()
        img_scan = im_full

        scan_parms = {}
        for name in image_settings.list():
            scan_parms[name] = image_settings.get(name)
        scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])
        scan_parms['BlueEmphasis'] = float(scan_parms['BlueEmphasis'])

        if pos is not None:
            (sw, sh) = cuav_util.image_shape(img_scan)
            altitude = pos.altitude
            if altitude < camera_settings.minalt:
                altitude = camera_settings.minalt
            scan_parms[
                'MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0

            regions = scanner.scan(img_scan, scan_parms)
        else:
            regions = scanner.scan(img_scan)
        regions = cuav_region.RegionsConvert(regions,
                                             cuav_util.image_shape(img_scan),
                                             cuav_util.image_shape(im_full))
        count += 1
        t1 = time.time()

        frame_time = pos.time

        if pos:
            for r in regions:
                r.latlon = cuav_util.gps_position_from_image_region(
                    r, pos, w, h, altitude=altitude, C=C_params)

            if camera_settings.target_radius > 0 and pos is not None:
                regions = cuav_region.filter_radius(
                    regions, (camera_settings.target_lattitude,
                              camera_settings.target_longitude),
                    camera_settings.target_radius)

        regions = cuav_region.filter_regions(
            im_full,
            regions,
            frame_time=frame_time,
            min_score=camera_settings.minscore,
            filter_type=camera_settings.filter_type)

        scan_count += 1

        if pos and len(regions) > 0:
            altitude = camera_settings.altitude
            if altitude <= 0:
                altitude = None
            joelog.add_regions(frame_time,
                               regions,
                               pos,
                               f,
                               width=w,
                               height=h,
                               altitude=altitude)

        mosaic.add_image(pos.time, f, pos)

        region_count += len(regions)

        if len(regions) > 0:
            composite = cuav_mosaic.CompositeThumbnail(
                cv.GetImage(cv.fromarray(im_full)), regions)
            thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
            mosaic.add_regions(regions, thumbs, f, pos)

        if args.view:
            img_view = img_scan
            (wview, hview) = cuav_util.image_shape(img_view)
            mat = cv.fromarray(img_view)
            for r in regions:
                r.draw_rectangle(mat, (255, 0, 0))
            cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
            viewer.set_image(mat)
            viewer.set_title('Image: ' + os.path.basename(f))
            if args.saveview:
                cv.CvtColor(mat, mat, cv.CV_RGB2BGR)
                cv.SaveImage('view-' + os.path.basename(f), mat)

        total_time += (t1 - t0)
        if t1 != t0:
            print('%s scan %.1f fps  %u regions [%u/%u]' %
                  (os.path.basename(f), count / total_time, region_count,
                   scan_count, num_files))
        #raw_input("hit ENTER when ready")

    print("All images processed (%u seconds)" % (time.time() - start_time))
    while True:
        # check for any events from the map
        slipmap.check_events()
        mosaic.check_events()
        time.sleep(0.2)
Esempio n. 32
0
    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")
Esempio n. 33
0
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)))
Esempio n. 34
0
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()
Esempio n. 35
0
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))
Esempio n. 36
0
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()
Esempio n. 37
0
                              can_zoom=True,
                              can_drag=True)

menu = MPMenuTop([])
view_menu = MPMenuSubMenu('View', [
    MPMenuItem('Next Image\tCtrl+N', 'Next Image', 'nextImage'),
    MPMenuItem('Previous Image\tCtrl+P', 'Previous Image', 'previousImage')
])
menu.add(view_menu)
view_image.set_menu(menu)

settings = MPSettings([
    MPSetting('threshold',
              int,
              6100,
              'High Threshold',
              tab='Settings',
              range=(0, 65535)),
    MPSetting('blue_threshold', float, 0.75, 'Blue Threshold', range=(0, 1)),
    MPSetting('green_threshold', float, 0.4, 'Green Threshold', range=(0, 1))
])

changed = True


def settings_callback(setting):
    '''called on a changed setting'''
    global changed
    changed = True


def show_value(x, y):
Esempio n. 38
0
    def __init__(self):
        self.console = textconsole.SimpleConsole()
        self.map = None
        self.map_functions = {}
        self.vehicle_type = None
        self.vehicle_name = None
        from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
        self.settings = MPSettings([
            MPSetting('link',
                      int,
                      1,
                      'Primary Link',
                      tab='Link',
                      range=(0, 4),
                      increment=1),
            MPSetting('streamrate',
                      int,
                      4,
                      'Stream rate link1',
                      range=(-1, 20),
                      increment=1),
            MPSetting('streamrate2',
                      int,
                      4,
                      'Stream rate link2',
                      range=(-1, 20),
                      increment=1),
            MPSetting('heartbeat',
                      int,
                      1,
                      'Heartbeat rate',
                      range=(0, 5),
                      increment=1),
            MPSetting('mavfwd', bool, True, 'Allow forwarded control'),
            MPSetting('mavfwd_rate', bool, False,
                      'Allow forwarded rate control'),
            MPSetting('shownoise', bool, True, 'Show non-MAVLink data'),
            MPSetting('altreadout',
                      int,
                      10,
                      'Altitude Readout',
                      range=(0, 100),
                      increment=1,
                      tab='Announcements'),
            MPSetting('distreadout',
                      int,
                      200,
                      'Distance Readout',
                      range=(0, 10000),
                      increment=1),
            MPSetting('moddebug',
                      int,
                      0,
                      'Module Debug Level',
                      range=(0, 3),
                      increment=1,
                      tab='Debug'),
            MPSetting('flushlogs', bool, False, 'Flush logs on every packet'),
            MPSetting('requireexit', bool, False, 'Require exit command'),
            MPSetting('basealt',
                      int,
                      0,
                      'Base Altitude',
                      range=(0, 30000),
                      increment=1,
                      tab='Altitude'),
            MPSetting('wpalt',
                      int,
                      100,
                      'Default WP Altitude',
                      range=(0, 10000),
                      increment=1),
            MPSetting('rallyalt',
                      int,
                      90,
                      'Default Rally Altitude',
                      range=(0, 10000),
                      increment=1),
            MPSetting('terrainalt',
                      str,
                      'Auto',
                      'Use terrain altitudes',
                      choice=['Auto', 'True', 'False']),
            MPSetting('wpupdates', bool, True, 'Show waypoint updates'),
            MPSetting('rally_breakalt',
                      int,
                      40,
                      'Default Rally Break Altitude',
                      range=(0, 10000),
                      increment=1),
            MPSetting('rally_flags',
                      int,
                      0,
                      'Default Rally Flags`',
                      range=(0, 10000),
                      increment=1),
            MPSetting('baudrate',
                      int,
                      opts.baudrate,
                      'baudrate for new links',
                      range=(0, 10000000),
                      increment=1),
            MPSetting('rtscts', bool, opts.rtscts, 'enable flow control'),
            MPSetting('select_timeout', float, 0.01, 'select timeout')
        ])

        self.completions = {"script": ["(FILENAME)"], "set": ["(SETTING)"]}

        self.status = MPStatus()

        # master mavlink device
        self.mav_master = None

        # mavlink outputs
        self.mav_outputs = []

        # SITL output
        self.sitl_output = None

        self.mav_param = mavparm.MAVParmDict()
        self.modules = []
        self.public_modules = {}
        self.functions = MAVFunctions()
        self.select_extra = {}
        self.continue_mode = False
        self.aliases = {}
Esempio n. 39
0
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))
Esempio n. 40
0
def process(args):
  '''process a set of files'''

  global slipmap, mosaic
  scan_count = 0
  files = []
  if os.path.isdir(args.directory):
    files.extend(file_list(args.directory, ['jpg', 'jpeg', 'png']))
  else:
    if args.directory.find('*') != -1:
      files.extend(glob.glob(args.directory))
    else:
      files.append(args.directory)
  files.sort()
  num_files = len(files)
  print("num_files=%u" % num_files)
  region_count = 0

  slipmap = mp_slipmap.MPSlipMap(service=args.service, elevation=True, title='Map')
  if args.vehicle_type == "Copter":
      icon = slipmap.icon('redcopter.png')
  else:
      icon = slipmap.icon('redplane.png')
  slipmap.add_object(mp_slipmap.SlipIcon('plane', (0,0), icon, layer=3, rotation=0,
                                         follow=True,
                                         trail=mp_slipmap.SlipTrail()))

  for flag in args.flag:
    a = flag.split(',')
    lat = a[0]
    lon = a[1]
    icon = 'flag.png'
    if len(a) > 2:
      icon = a[2] + '.png'
    icon = slipmap.icon(icon)
    slipmap.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))

  if args.mission:
    from pymavlink import mavwp
    wp = mavwp.MAVWPLoader()
    wp.load(args.mission.name)
    plist = wp.polygon_list()
    if len(plist) > 0:
        for i in range(len(plist)):
          slipmap.add_object(mp_slipmap.SlipPolygon('Mission-%s-%u' % (args.mission.name,i), plist[i], layer='Mission',
                                     linewidth=2, colour=(255,255,255)))

  if args.mavlog:
    mpos = mav_position.MavInterpolator()
    mpos.set_logfile(args.mavlog.name)
  else:
    mpos = None

  if args.gammalog is not None:
    gamma = parse_gamma_log(args.gammalog)
  else:
    gamma = None

  if args.kmzlog:
    kmzpos = mav_position.KmlPosition(args.kmzlog.name)
  else:
    kmzpos = None

  if args.triggerlog:
    triggerpos = mav_position.TriggerPosition(args.triggerlog.name)
  else:
    triggerpos = None

  # create a simple lens model using the focal length

  if args.camera_params:
    C_params = cam_params.CameraParams.fromfile(args.camera_params.name)
  else:
    im_orig = cv2.imread(files[0])
    (w,h) = cuav_util.image_shape(im_orig)
    C_params = cam_params.CameraParams(lens=args.lens, sensorwidth=args.sensorwidth, xresolution=w, yresolution=h)

  if args.target:
    target = args.target.split(',')
  else:
    target = [0,0,0]
    
  camera_settings = MPSettings(
    [ MPSetting('roll_stabilised', bool, args.roll_stabilised, 'Roll Stabilised'),
      MPSetting('pitch_stabilised', bool, args.pitch_stabilised, 'Pitch Stabilised'),
      MPSetting('roll_offset', float, args.roll_offset, 'Roll Offset'),
      MPSetting('pitch_offset', float, args.pitch_offset, 'Pitch Offset'),
      MPSetting('altitude', int, args.altitude, 'Altitude', range=(0,10000), increment=1),
      MPSetting('minalt', int, 30, 'MinAltitude', range=(0,10000), increment=1),
      MPSetting('mpp100', float, 0.0977, 'MPPat100m', range=(0,10000), increment=0.001),
      MPSetting('rotate180', bool, False, 'rotate180'),
      MPSetting('filter_type', str, 'simple', 'Filter Type',
                choice=['simple']),
      MPSetting('target_lattitude', float, float(target[0]), 'target latitude', increment=1.0e-7),
      MPSetting('target_longitude', float, float(target[1]), 'target longitude', increment=1.0e-7),
      MPSetting('target_radius', float, float(target[2]), 'target radius', increment=1),
      MPSetting('quality', int, 75, 'Compression Quality', range=(1,100), increment=1),
      MPSetting('thumbsize', int, args.thumbsize, 'Thumbnail Size', range=(10, 200), increment=1),
      MPSetting('RegionHue', int, ags.targethue, 'Target Hue (0 to disable)', range=(0,180), increment=1, tab='Imaging'),
      MPSetting('map_thumbsize', int, args.map_thumbsize, 'Map Thumbnail Size', range=(10, 200), increment=1),
      MPSetting('minscore', int, args.minscore, 'Min Score', range=(0,1000), increment=1, tab='Scoring'),
      MPSetting('brightness', float, 1.0, 'Display Brightness', range=(0.1, 10), increment=0.1,
                digits=2, tab='Display'),      
      ],
    title='Camera Settings'
    )

  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),
      MPSetting('MinRegionSize', float, 0.2, range=(0,100), increment=0.05, digits=2),
      MPSetting('MaxRegionSize', float, 1.0, range=(0,100), increment=0.1, digits=1),
      MPSetting('MaxRarityPct',  float, 0.02, range=(0,100), increment=0.01, digits=2),
      MPSetting('RegionMergeSize', float, 1.0, range=(0,100), increment=0.1, digits=1),
      MPSetting('SaveIntermediate', bool, args.debug)
      ],
    title='Image Settings')
  
  mosaic = cuav_mosaic.Mosaic(slipmap, C=C_params,
                              camera_settings=camera_settings,
                              image_settings=image_settings,
                              start_menu=True,
                              classify=args.categories,
                              thumb_size=args.mosaic_thumbsize,
                              map_thumb_size=args.map_thumbsize)

  joelog = cuav_joe.JoeLog(None)

  if args.view:
    viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)

  start_time = time.time()
  for f in files:
      if not mosaic.started():
        print("Waiting for startup")
        if args.start:
          mosaic.has_started = True
        while not mosaic.started():
          mosaic.check_events()
          time.sleep(0.01)

      if mpos:
        # get the position by interpolating telemetry data from the MAVLink log file
        # this assumes that the filename contains the timestamp 
        if gamma is not None:
          frame_time = parse_gamma_time(f, gamma)
        else:
          frame_time = cuav_util.parse_frame_time(f)
        frame_time += args.time_offset
        if camera_settings.roll_stabilised:
          roll = 0
        else:
          roll = None
        if camera_settings.pitch_stabilised:
          pitch = 0
        else:
          pitch = None
        try:
          pos = mpos.position(frame_time, roll=roll, pitch=pitch,
                              roll_offset=camera_settings.roll_offset,
                              pitch_offset=camera_settings.pitch_offset)
        except Exception:
          print("No position available for %s" % frame_time)
          # skip this frame
          continue
      elif kmzpos is not None:
        pos = kmzpos.position(f)
      elif triggerpos is not None:
        pos = triggerpos.position(f)
      else:
        # get the position using EXIF data
        pos = mav_position.exif_position(f)
        pos.time += args.time_offset

      # update the vehicle icon on the map
      if pos is not None:
        slipmap.set_position("plane", (pos.lat, pos.lon), rotation=pos.yaw)
        if camera_settings.altitude > 0:
          pos.altitude = camera_settings.altitude

      # check for any events from the map
      slipmap.check_events()
      mosaic.check_events()

      #im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180)
      im_orig = cv2.imread(f)

      if im_orig is None:
        continue
      (w,h) = cuav_util.image_shape(im_orig)

      if args.downsample:
        im_full = cv2.resize(im_orig, (0,0), fx=0.5, fy=0.5)
      else:
        im_full = im_orig

      count = 0
      total_time = 0

      t0=time.time()
      img_scan = im_full

      scan_parms = {}
      for name in image_settings.list():
        scan_parms[name] = image_settings.get(name)
      scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])

      if pos is not None:
        (sw,sh) = cuav_util.image_shape(img_scan)
        altitude = pos.altitude
        if altitude < camera_settings.minalt:
          altitude = camera_settings.minalt
        scan_parms['MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0

        regions = scanner.scan(img_scan, scan_parms)
      else:
        regions = scanner.scan(img_scan)
      regions = cuav_region.RegionsConvert(regions, cuav_util.image_shape(img_scan), cuav_util.image_shape(im_full))
      count += 1
      t1=time.time()

      frame_time = pos.time

      regions = cuav_region.filter_regions(im_full, regions, min_score=camera_settings.minscore,
                                           filter_type=camera_settings.filter_type,
                                           target_hue=camera_settings.RegionHue)

      if pos:
        for r in regions:
          r.latlon = cuav_util.gps_position_from_image_region(r, pos, w, h, altitude=altitude, C=C_params)

        if camera_settings.target_radius > 0 and pos is not None:
          regions = cuav_region.filter_radius(regions, (camera_settings.target_lattitude,
                                                        camera_settings.target_longitude),
                                              camera_settings.target_radius)

      scan_count += 1

      if pos and len(regions) > 0:
        joelog.add_regions(frame_time, regions, pos, f)

      mosaic.add_image(pos.time, f, pos)

      region_count += len(regions)

      if len(regions) > 0:
          composite = cuav_region.CompositeThumbnail(im_full, regions)
          thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
          thumbsRGB = []

          #colour space conversion
          for thumb in thumbs:
              thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
          mosaic.add_regions(regions, thumbsRGB, f, pos)

      if args.view:
        img_view = img_scan
        (wview,hview) = cuav_util.image_shape(img_view)
        for r in regions:
          r.draw_rectangle(img_view, (255,0,0))
        #cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
        img_view = cv2.cvtColor(img_view, cv2.COLOR_BGR2RGB)
        viewer.set_image(img_view)
        viewer.set_title('Image: ' + os.path.basename(f))

      total_time += (t1-t0)
      if t1 != t0:
          print('%s scan %.1f fps  %u regions [%u/%u]' % (
              os.path.basename(f), count/total_time, region_count, scan_count, num_files))
      #raw_input("hit ENTER when ready")

  print("All images processed (%u seconds)" % (time.time() - start_time))
  while True:
      # check for any events from the map
      slipmap.check_events()
      mosaic.check_events()
      time.sleep(0.2)
Esempio n. 41
0
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
Esempio n. 42
0
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)))
Esempio n. 43
0
    def __init__(self):
        self.console = textconsole.SimpleConsole()
        self.map = None
        self.map_functions = {}
        self.vehicle_type = None
        self.vehicle_name = None
        from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
        self.settings = MPSettings(
            [ MPSetting('link', int, 1, 'Primary Link', tab='Link', range=(0,4), increment=1),
              MPSetting('streamrate', int, 4, 'Stream rate link1', range=(-1,100), increment=1),
              MPSetting('streamrate2', int, 4, 'Stream rate link2', range=(-1,100), increment=1),
              MPSetting('heartbeat', int, 1, 'Heartbeat rate', range=(0,5), increment=1),
              MPSetting('mavfwd', bool, True, 'Allow forwarded control'),
              MPSetting('mavfwd_rate', bool, False, 'Allow forwarded rate control'),
              MPSetting('shownoise', bool, True, 'Show non-MAVLink data'),
              MPSetting('baudrate', int, opts.baudrate, 'baudrate for new links', range=(0,10000000), increment=1),
              MPSetting('rtscts', bool, opts.rtscts, 'enable flow control'),
              MPSetting('select_timeout', float, 0.01, 'select timeout'),

              MPSetting('altreadout', int, 10, 'Altitude Readout',
                        range=(0,100), increment=1, tab='Announcements'),
              MPSetting('distreadout', int, 200, 'Distance Readout', range=(0,10000), increment=1),

              MPSetting('moddebug', int, opts.moddebug, 'Module Debug Level', range=(0,3), increment=1, tab='Debug'),
              MPSetting('compdebug', int, 0, 'Computation Debug Mask', range=(0,3), tab='Debug'),
              MPSetting('flushlogs', bool, False, 'Flush logs on every packet'),
              MPSetting('requireexit', bool, False, 'Require exit command'),
              MPSetting('wpupdates', bool, True, 'Announce waypoint updates'),

              MPSetting('basealt', int, 0, 'Base Altitude', range=(0,30000), increment=1, tab='Altitude'),
              MPSetting('wpalt', int, 100, 'Default WP Altitude', range=(0,10000), increment=1),
              MPSetting('rallyalt', int, 90, 'Default Rally Altitude', range=(0,10000), increment=1),
              MPSetting('terrainalt', str, 'Auto', 'Use terrain altitudes', choice=['Auto','True','False']),
              MPSetting('rally_breakalt', int, 40, 'Default Rally Break Altitude', range=(0,10000), increment=1),
              MPSetting('rally_flags', int, 0, 'Default Rally Flags', range=(0,10000), increment=1),

              MPSetting('source_system', int, 255, 'MAVLink Source system', range=(0,255), increment=1, tab='MAVLink'),
              MPSetting('source_component', int, 0, 'MAVLink Source component', range=(0,255), increment=1),
              MPSetting('target_system', int, 0, 'MAVLink target system', range=(0,255), increment=1),
              MPSetting('target_component', int, 0, 'MAVLink target component', range=(0,255), increment=1),
              MPSetting('state_basedir', str, None, 'base directory for logs and aircraft directories'),
              MPSetting('allow_unsigned', bool, True, 'whether unsigned packets will be accepted'),

              MPSetting('dist_unit', str, 'm', 'distance unit', choice=['m', 'nm', 'miles'], tab='Units'),
              MPSetting('height_unit', str, 'm', 'height unit', choice=['m', 'feet']),
              MPSetting('speed_unit', str, 'm/s', 'height unit', choice=['m/s', 'knots', 'mph']),

              MPSetting('vehicle_name', str, '', 'Vehicle Name', tab='Vehicle'),
            ])

        self.completions = {
            "script"         : ["(FILENAME)"],
            "set"            : ["(SETTING)"],
            "status"         : ["(VARIABLE)"],
            "module"    : ["list",
                           "load (AVAILMODULES)",
                           "<unload|reload> (LOADEDMODULES)"]
            }

        self.status = MPStatus()

        # master mavlink device
        self.mav_master = None

        # mavlink outputs
        self.mav_outputs = []
        self.sysid_outputs = {}

        # SITL output
        self.sitl_output = None

        self.mav_param = mavparm.MAVParmDict()
        self.modules = []
        self.public_modules = {}
        self.functions = MAVFunctions()
        self.select_extra = {}
        self.continue_mode = False
        self.aliases = {}
        import platform
        self.system = platform.system()
Esempio n. 44
0
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