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