def __init__(self, mpstate): super(GasHeliModule, self).__init__(mpstate, "gas_heli", "Gas Heli", public=False) self.console.set_status('IGN', 'IGN', row=4) self.console.set_status('THR', 'THR', row=4) self.console.set_status('RPM', 'RPM: 0', row=4) self.add_command('gasheli', self.cmd_gasheli, 'gas helicopter control', ['<start|stop>', 'set (GASHELISETTINGS)']) self.gasheli_settings = mp_settings.MPSettings([ ('ignition_chan', int, 0), ('ignition_disable_time', float, 0.5), ('ignition_stop_time', float, 3), ('starter_chan', int, 0), ('starter_time', float, 3.0), ('starter_pwm_on', int, 2000), ('starter_pwm_off', int, 1000), ]) self.add_completion_function('(GASHELISETTINGS)', self.gasheli_settings.completion) self.starting_motor = False self.stopping_motor = False self.motor_t1 = None self.old_override = 0
def __init__(self, mpstate): """Initialise module""" super(FalconHILModule, self).__init__(mpstate, "falconhil", "Falcon 8+ HIL") self.status_callcount = 0 self.boredom_interval = 10 # seconds self.last_bored = time.time() self.mpstate.sim_start_time_sec = time.time( ) # Using this for the SYSTEM_TIME packet for results computation self.packets_mytarget = 0 self.packets_othertarget = 0 self.verbose = False self._sdk_connected = False logpath = os.path.join(self.mpstate.status.logdir, "falconlog.tlog") self.mpstate.falconlog = FalconLogWriter(logpath) # Open log mode = 4 self.mpstate.falconlog.hil_log(self.heartbeat_packet_for_mode(mode)) self._falcon_connection_manager = FalconConnectionManager(self.mpstate) self.connect_falcon( "169.254.149.19", 65101) # 169.254.149.19 / 169.254.248.207 / 169.254.128.221 # add command self.FalconHILModule_settings = mp_settings.MPSettings([ ('verbose', bool, False), ]) self.add_command('falcon', self.cmd_falcon, "falcon commands", ['status', 'readsystem', 'wp'])
def __init__(self, mpstate): super(ADSBModule, self).__init__(mpstate, "adsb", "ADS-B data support", public=True) self.threat_vehicles = {} self.active_threat_ids = [ ] # holds all threat ids the vehicle is evading self.add_command('adsb', self.cmd_ADSB, "adsb control", ["<status>", "set (ADSBSETTING)"]) self.ADSB_settings = mp_settings.MPSettings([ ("timeout", int, 5), # seconds ("threat_radius", int, 200), # meters ("show_threat_radius", bool, False), # threat_radius_clear = threat_radius*threat_radius_clear_multiplier ("threat_radius_clear_multiplier", int, 2), ("show_threat_radius_clear", bool, False) ]) self.add_completion_function('(ADSBSETTING)', self.ADSB_settings.completion) self.threat_detection_timer = mavutil.periodic_event(2) self.threat_timeout_timer = mavutil.periodic_event(2) self.tnow = self.get_time()
def __init__(self, mpstate): super(FakeGPSModule, self).__init__(mpstate, "fakegps") self.last_send = time.time() self.FakeGPS_settings = mp_settings.MPSettings([ ("nsats", int, 16), ("lat", float, -35.363261), ("lon", float, 149.165230), ("alt", float, 584.0), ("yaw", float, 0.0), ("rate", float, 5) ]) self.add_command('fakegps', self.cmd_FakeGPS, "fakegps control", ["<status>", "set (FAKEGPSSETTING)"]) self.add_completion_function('(FAKEGPSSETTING)', self.FakeGPS_settings.completion) if mp_util.has_wxpython: map = self.module('map') if map is not None: menu = MPMenuSubMenu( 'FakeGPS', items=[ MPMenuItem('SetPos', 'SetPos', '# fakegps setpos'), MPMenuItem( 'SetPos (with alt)', 'SetPosAlt', '# fakegps setpos ', handler=MPMenuCallTextDialog( title='Altitude (m)', default=self.mpstate.settings.guidedalt)) ]) map.add_menu(menu)
def __init__(self, mpstate): super(ViconModule, self).__init__(mpstate, "vicon", "vicon", public=False) self.console.set_status('VPos', 'VPos -- -- --', row=5) self.console.set_status('VAtt', 'VAtt -- -- --', row=5) self.vicon_settings = mp_settings.MPSettings([ ('host', str, "vicon"), ('origin_lat', float, -35.363261), ('origin_lon', float, 149.165230), ('origin_alt', float, 584.0), ('vision_rate', int, 14), ('vel_filter_hz', float, 30.0), ('gps_rate', int, 5), ('gps_nsats', float, 16), ('yaw_offset', float, 0.0) ]) self.add_command('vicon', self.cmd_vicon, 'VICON control', ["<start>", "<stop>", "set (VICONSETTING)"]) self.add_completion_function('(VICONSETTING)', self.vicon_settings.completion) self.vicon = None self.thread = threading.Thread(target=self.thread_loop) self.thread.start() self.pos = None self.att = None self.frame_count = 0 self.gps_count = 0 self.vision_count = 0 self.last_frame_count = 0 self.vel_filter = LowPassFilter2p.LowPassFilter2p(200.0, 30.0) self.actual_frame_rate = 0.0
def __init__(self, mpstate): """Initialise OSD module""" super(osd, self).__init__(mpstate, "osd", "") self.request_id = 1 self.osd_settings = mp_settings.MPSettings([ ('verbose', bool, False), ]) self.add_command('osd', self.cmd_osd, "OSD module", [ 'param-set <5|6> <1|2|3|4|5|6|7|8|9> (PARAMETER) (TYPES)', 'param-show <5|6> <1|2|3|4|5|6|7|8|9>', 'set (OSDSETTING)' ]) self.add_completion_function('(TYPES)', self.param_type_completion) self.type_map = { mavutil.mavlink.OSD_PARAM_NONE: "NONE", mavutil.mavlink.OSD_PARAM_SERIAL_PROTOCOL: "SERIAL_PROTOCOL", mavutil.mavlink.OSD_PARAM_SERVO_FUNCTION: "SERVO_FUNCTION", mavutil.mavlink.OSD_PARAM_AUX_FUNCTION: "AUX_FUNCTION", mavutil.mavlink.OSD_PARAM_FLIGHT_MODE: "FLIGHT_MODE", mavutil.mavlink.OSD_PARAM_FAILSAFE_ACTION: "FAILSAFE_ACTION", mavutil.mavlink.OSD_PARAM_FAILSAFE_ACTION_1: "FAILSAFE_ACTION_1", mavutil.mavlink.OSD_PARAM_FAILSAFE_ACTION_2: "FAILSAFE_ACTION_2" } self.invtype_map = {v: k for k, v in self.type_map.items()} self.param_list = {}
def __init__(self, mpstate): super(DGPSModule, self).__init__(mpstate, "DGPS", "DGPS injection support for SBP/RTCP/UBC") self.dgps_settings = mp_settings.MPSettings([ MPSetting('type', str, 'udp_host', 'DGPS connection type', choice=['udp_host', 'udp_client', 'tcp_client']), MPSetting('port', int, 13320, 'DGPS connection port', range=(0, 65535), increment=1), MPSetting('host', str, '127.0.0.1', 'DGPS connection host'), MPSetting('sendalllinks', bool, False, 'Send RTCM messages on all links'), ]) self.add_command( 'DGPS', self.cmd_dgps, 'DGPS control', ["<status>", "<start>", "<stop>", "set (DGPSSETTING)"]) self.add_completion_function('(DGPSSETTING)', self.dgps_settings.completion) self.dgps_settings.set_callback(self.settings_changed) self.port = None self.reset_stats() self.connect_dgps()
def __init__(self, mpstate): super(FTPModule, self).__init__(mpstate, "ftp") self.add_command('ftp', self.cmd_ftp, "file transfer", [ "<list|get|rm|rmdir|rename|mkdir|crc|cancel|status>", "set (FTPSETTING)", "put (FILENAME) (FILENAME)" ]) self.ftp_settings = mp_settings.MPSettings([('debug', int, 0), ('pkt_loss_tx', int, 0), ('pkt_loss_rx', int, 0)]) self.add_completion_function('(FTPSETTING)', self.ftp_settings.completion) self.seq = 0 self.session = 0 self.network = 0 self.last_op = None self.fh = None self.filename = None self.total_size = 0 self.read_gaps = set() self.read_retries = 0 self.last_read = None self.last_burst_read = None self.op_start = None self.dir_offset = 0 self.write_wait = False self.last_op_time = time.time() self.rtt = 0.5
def __init__(self, mpstate): """Initialise module""" super(ublox, self).__init__(mpstate, "ublox", "") self.auto = True self.verbose = False self.api_token = self.read_api_token() self.auto_failed = None self.last_auto = 0 self.fix_type = 0 self.time_boot_ms = 0 self.mga_offline_last_check = 0 self.mga_offline_data_uploaded = None self.state_dir = mp_util.dot_mavproxy('ublox') self.mga_cachedir = os.path.join(self.state_dir, "mga") try: os.makedirs(self.mga_cachedir) except OSError as e: if e.errno != 17: raise e self.mga_dbd_cachefile = os.path.join(self.mga_cachedir, "dbd.ubx") self.mga_offline = ub.mga.MGAOfflineCache(cachefile=os.path.join( self.mga_cachedir, "offline.ubx"), token=self.api_token) if self.mga_offline.should_request_fresh_data(): print("Apparently should request fresh data") # self.mga_offline.start_update_thread() self.ublox_settings = mp_settings.MPSettings([ ('verbose', bool, False), ('auto', bool, False), ]) self.add_command('ublox', self.cmd_ublox, "ublox module", ['status', 'set (AUTO)', 'reset', 'mga'])
def __init__(self, mpstate): super(FTPModule, self).__init__(mpstate, "ftp", public=True) self.add_command('ftp', self.cmd_ftp, "file transfer", [ "<list|get|rm|rmdir|rename|mkdir|crc|cancel|status>", "set (FTPSETTING)", "put (FILENAME) (FILENAME)" ]) self.ftp_settings = mp_settings.MPSettings([ ('debug', int, 0), ('pkt_loss_tx', int, 0), ('pkt_loss_rx', int, 0), ('max_backlog', int, 5), ('burst_read_size', int, 110), ('retry_time', float, 0.5) ]) self.add_completion_function('(FTPSETTING)', self.ftp_settings.completion) self.seq = 0 self.session = 0 self.network = 0 self.last_op = None self.fh = None self.filename = None self.callback = None self.total_size = 0 self.read_gaps = [] self.read_gap_times = {} self.read_retries = 0 self.duplicates = 0 self.last_read = None self.last_burst_read = None self.op_start = None self.dir_offset = 0 self.write_wait = False self.last_op_time = time.time() self.rtt = 0.5 self.reached_eof = False self.backlog = 0 self.burst_size = self.ftp_settings.burst_read_size
def __init__(self, mpstate): """Initialise module. We start poking the UAV for messages after this is called""" super(dataflash_logger, self).__init__( mpstate, "dataflash_logger", "logging of mavlink dataflash messages" ) self.sender = None self.stopped = False self.time_last_start_packet_sent = 0 self.time_last_stop_packet_sent = 0 self.dataflash_dir = self._dataflash_dir(mpstate) self.download = 0 self.prev_download = 0 self.last_status_time = time.time() self.last_seqno = 0 self.missing_blocks = {} self.acking_blocks = {} self.blocks_to_ack_and_nack = [] self.missing_found = 0 self.abandoned = 0 self.dropped = 0 self.log_settings = mp_settings.MPSettings( [('verbose', bool, False), ('df_target_system', int, 0), ('df_target_component', int, mavutil.mavlink.MAV_COMP_ID_LOG)] ) self.add_command('dataflash_logger', self.cmd_dataflash_logger, "dataflash logging control", ['status', 'start', 'stop', 'rotate', 'set (LOGSETTING)']) self.add_completion_function('(LOGSETTING)', self.log_settings.completion)
def __init__(self, mpstate): """Initialise module""" super(PacketMonitorModule, self).__init__(mpstate, "monitor", "monitoring MAVLink packets") self.status_callcount = 0 self.boredom_interval = 10 # seconds self.last_bored = time.time() self.jamming_on = False self.cur_seqnum = None self.cur_radio_seqnum = None self.cur_radio_status_seqnum = None #self.logfile = open('packet_log.txt', mode='w') # possible extension: receive filename as an argument pathname = os.path.abspath('log/') filename = datetime.now().strftime('%m%d_%H%M') + ".txt" full_filename = os.path.join(pathname, filename) self.logger = MAVPacketLogger(full_filename) self.packets_mytarget = 0 self.packets_othertarget = 0 self.verbose = False self.example_settings = mp_settings.MPSettings([ ('verbose', bool, False), ]) self.add_command('jamming', self.cmd_jamming, "jamming")
def __init__(self, mpstate): super(NtripModule, self).__init__(mpstate, "ntrip", "ntrip", public=False) self.ntrip_settings = mp_settings.MPSettings( [('caster', str, None), ('port', int, 2101), ('username', str, 'IBS'), ('password', str, 'IBS'), ('mountpoint', str, None), ('logfile', str, None)]) self.add_command('ntrip', self.cmd_ntrip, 'NTRIP control', ["<status>", "<start>", "<stop>", "set (NTRIPSETTING)"]) self.add_completion_function('(NTRIPSETTING)', self.ntrip_settings.completion) self.pos = None self.pkt_count = 0 self.last_pkt = None self.last_rate = None self.rate_total = 0 self.ntrip = None self.start_pending = False self.rate = 0 self.logfile = None self.id_counts = {} self.last_by_id = {}
def __init__(self, mpstate): super(NMEAModule, self).__init__(mpstate, "NMEA", "NMEA output", public=True, multi_instance=True) cmdname = "nmea" if self.instance > 1: cmdname += "%u" % self.instance self.add_command(cmdname, self.cmd_nmea, "nmea control", ['<status|set>']) self.serial_line = None self.serial = None self.socat = None self.log_output = None self.log_output_filepath = None self.udp_output_port = None self.udp_output_address = None self.output_time = 0.0 self.num_sat = 21 self.hdop = 1.21 self.fix_quality = 1 self.last_time_boot_ms = 0 self.nmea_settings = mp_settings.MPSettings([ ('target_system', int, 1), ('wgs84_to_amsl', float, -41.2), ]) self.add_completion_function('(NMEASETTING)', self.nmea_settings.completion) self.sent_count = 0
def __init__(self, mpstate): super(MapModule, self).__init__(mpstate, "map", "map display", public=True) self.lat = None self.lon = None self.heading = 0 self.wp_change_time = 0 self.fence_change_time = 0 self.rally_change_time = 0 self.have_simstate = False self.have_vehicle = {} self.move_wp = -1 self.moving_wp = None self.moving_fencepoint = None self.moving_rally = None self.mission_list = None self.icon_counter = 0 self.click_position = None self.click_time = 0 self.draw_line = None self.draw_callback = None self.have_global_position = False self.vehicle_type_name = 'plane' self.ElevationMap = mp_elevation.ElevationModel() self.map_settings = mp_settings.MPSettings([ ('showgpspos', int, 0), ('showgps2pos', int, 1), ('showsimpos', int, 0), ('showahrs2pos', int, 0), ('brightness', float, 1), ('rallycircle', bool, False), ('loitercircle', bool, False) ]) service = 'OviHybrid' if 'MAP_SERVICE' in os.environ: service = os.environ['MAP_SERVICE'] import platform mpstate.map = mp_slipmap.MPSlipMap(service=service, elevation=True, title='Map') mpstate.map_functions = {'draw_lines': self.draw_lines} mpstate.map.add_callback(functools.partial(self.map_callback)) self.add_command('map', self.cmd_map, "map control", ['icon', 'set (MAPSETTING)']) self.add_completion_function('(MAPSETTING)', self.map_settings.completion) self.default_popup = MPMenuSubMenu('Popup', items=[]) self.add_menu( MPMenuItem('Fly To', 'Fly To', '# guided ', handler=MPMenuCallTextDialog(title='Altitude (m)', default=100))) self.add_menu(MPMenuItem('Set Home', 'Set Home', '# map sethome ')) self.add_menu( MPMenuItem('Terrain Check', 'Terrain Check', '# terrain check')) self.add_menu( MPMenuItem('Show Position', 'Show Position', 'showPosition'))
def __init__(self, mpstate): super(EMUECUModule, self).__init__(mpstate, "emuecu", "emuecu", public=False) self.emuecu_settings = mp_settings.MPSettings([('port', int, 102)]) self.add_command('emu', self.cmd_emu, 'EMUECU control', ["<send>", "set (EMUECUSETTING)"]) self.add_completion_function('(EMUECUSETTING)', self.emuecu_settings.completion)
def __init__(self, mpstate): '''Initialise module''' super(swarm, self).__init__(mpstate, "swarm", "swarm module", multi_vehicle=True) # array of tuples for (SYSID, COMPID, FOLL_SYSID, veh_type) of all detected vehicles self.vehicleListing = [] self.add_command('swarm', self.cmd_swarm, "swarm control", ["<status>", "set (SWARMSETTING)"]) self.swarm_settings = mp_settings.MPSettings( [("takeoffalt", int, 10)]) # meters self.add_completion_function('(SWARMSETTING)', self.swarm_settings.completion) # Which params to show on the GUI per vehicle self.parmsToShow = ["FOLL_OFS_X", "FOLL_OFS_Y", "FOLL_OFS_Z"] # time of last HB for each vehicle. Key is tuple of sysid,compid. Value is time of last HB self.vehicleLastHB = {} self.needHBupdate_timer = mavutil.periodic_event(1) # Periodic event to update the GUI self.needGUIupdate = False self.needGUIupdate_timer = mavutil.periodic_event(1) # Periodic event to send param (offset) requests (5 Hz) self.requestParams_timer = mavutil.periodic_event(5) # Periodic event re-get params (0.1 Hz) self.RerequestParams_timer = mavutil.periodic_event(0.1) # List of any vehicles we still need to get params for self.vehParamsToGet = [] # All vehicle positions. Dict. Key is sysid, value is tuple of (lat,lon,alt) self.allVehPos = {} self.validVehicles = frozenset([mavutil.mavlink.MAV_TYPE_FIXED_WING, mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT, mavutil.mavlink.MAV_TYPE_SUBMARINE, mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER, mavutil.mavlink.MAV_TYPE_DODECAROTOR, mavutil.mavlink.MAV_TYPE_AIRSHIP]) # The GUI self.gui = SwarmUI( self.parmsToShow, takeoffalt=self.swarm_settings.get('takeoffalt'))
def __init__(self, mpstate): super(FakeGPSModule, self).__init__(mpstate, "fakegps") self.last_send = time.time() self.FakeGPS_settings = mp_settings.MPSettings([("nsats", int, 16), ("lat", float, -35.363261), ("lon", float, 149.165230), ("alt", float, 584.0), ("rate", float, 5)]) self.add_command('fakegps', self.cmd_FakeGPS, "fakegps control", ["<status>", "set (FAKEGPSSETTING)"]) self.add_completion_function('(FAKEGPSSETTING)', self.FakeGPS_settings.completion)
def __init__(self, mpstate): super(SerialModule, self).__init__(mpstate, "serial", "serial control handling") self.add_command('serial', self.cmd_serial, 'remote serial control', ['<lock|unlock|send>', 'set (SERIALSETTING)']) self.serial_settings = mp_settings.MPSettings([('port', int, 0), ('baudrate', int, 57600), ('timeout', int, 500)]) self.add_completion_function('(SERIALSETTING)', self.serial_settings.completion) self.locked = False
def __init__(self, mpstate): super(NSHModule, self).__init__(mpstate, "nsh", "remote nsh shell") self.add_command('nsh', self.cmd_nsh, 'nsh shell control', ['<start|stop>', 'set (SERIALSETTING)']) self.serial_settings = mp_settings.MPSettings([ ('port', int, mavutil.mavlink.SERIAL_CONTROL_DEV_SHELL), ('baudrate', int, 57600) ]) self.add_completion_function('(SERIALSETTING)', self.serial_settings.completion) self.last_packet = time.time() self.last_check = time.time() self.started = False
def __init__(self, mpstate): """Initialise module""" super(generator, self).__init__(mpstate, "generator", "") self.generator_settings = mp_settings.MPSettings([ ('verbose', bool, False), ]) self.add_command('generator', self.cmd_generator, "generator module", ['status', 'set (LOGSETTING)']) self.console_row = 6 self.console.set_status('Generator', 'No generator messages', row=self.console_row)
def __init__(self, mpstate): """Initialise module""" super(CropqModule, self).__init__(mpstate, "cropq", "") self.mpstate.console = RemoteConsole(self) # configure local webserver self.config = None self.config_file = None self.local_server = LocalServer(self) self.local_server.start() # configure API communication self.comm_interval = 10 # seconds self.comm_start = False self.api_url = None self.username = None self.password = None self.vehicle_server_id = None # configure data collection self.allow_data_collection_threads = True self.data_col_profiles = {} self.datapoints = [] self.last_comm = time.time() # self.status_callcount = 0 self.packets_mytarget = 0 self.packets_othertarget = 0 self.status_text_sent = [] self.io_text_sent = [] self.params_last_sent = {} self.mpstats_last_sent = {} self.heartbeats_latest_20 = [] self.comm_log_latest_100 = [] self.direct_command_queue = [] self.direct_commands = {} self.mav_ack_alerts = MavAckAlerts(self) self.remote_settings = mp_settings.MPSettings([ ('verbose', bool, False), ]) self.add_command( 'cropq', self.cmd_cropq, "cropq module", ['set (LOGSETTING)', 'i (seconds)', 'interval (seconds)'])
def __init__(self, mpstate): """Initialise module""" super(message, self).__init__(mpstate, "message", "") self.status_callcount = 0 self.boredom_interval = 10 # seconds self.last_bored = time.time() self.packets_mytarget = 0 self.packets_othertarget = 0 self.verbose = False self.message_settings = mp_settings.MPSettings([('verbose', bool, False)]) self.add_command('message', self.cmd_message, "message module", [])
def __init__(self, mpstate): """Initialise module""" super(system_time, self).__init__(mpstate, "system_time", "") self.last_sent = 0 self.last_sent_ts1 = 0 self.last_sent_timesync = 0 self.module_load_time = time.time() self.system_time_settings = mp_settings.MPSettings([ ('verbose', bool, False), ('interval_timesync', int, 10), ('interval', int, 10) ]) self.add_command('system_time', self.cmd_system_time, "system_time module", ['status', 'set (LOGSETTING)'])
def __init__(self, mpstate): """Initialise module. We start poking the UAV for messages after this is called""" super(dataflash_logger, self).__init__(mpstate, "dataflash_logger", "logging of mavlink dataflash messages") self.new_log_started = False self.stopped = False self.time_last_start_packet_sent = 0 self.time_last_stop_packet_sent = 0 self.dataflash_dir = self._dataflash_dir(mpstate) self.log_settings = mp_settings.MPSettings( [ ('verbose', bool, False), ]) self.add_command('dataflash_logger', self.cmd_dataflash_logger, "dataflash logging control", ['status','start','stop','set (LOGSETTING)']) self.add_completion_function('(LOGSETTING)', self.log_settings.completion)
def __init__(self, mpstate): """Initialise module""" super(optitrack, self).__init__(mpstate, "optitrack", "optitrack") self.optitrack_settings = mp_settings.MPSettings([ ('server', str, '127.0.0.1'), ('client', str, '127.0.0.1'), ('msg_intvl_ms', int, 75), ('obj_id', int, 1) ]) self.add_command('optitrack', self.cmd_optitrack, "optitrack control", ['<start>', 'set (OPTITRACKSETTING)']) self.streaming_client = NatNetClient.NatNetClient() # Configure the streaming client to call our rigid body handler on the emulator to send data out. self.streaming_client.rigid_body_listener = self.receive_rigid_body_frame self.last_msg_time = 0 self.started = False
def __init__(self, mpstate): super(FollowTestModule, self).__init__(mpstate, "followtest", "followtest module") self.add_command('followtest', self.cmd_followtest, "followtest control", ['set (FOLLOWSETTING)']) self.follow_settings = mp_settings.MPSettings([("radius", float, 100.0), ("altitude", float, 50.0), ("speed", float, 10.0), ("type", str, 'guided'), ("vehicle_throttle", float, 0.5), ("disable_msg", bool, False)]) self.add_completion_function('(FOLLOWSETTING)', self.follow_settings.completion) self.target_pos = None self.last_update = 0 self.circle_dist = 0
def __init__(self): self.console = textconsole.SimpleConsole() self.map = None self.map_functions = {} self.vehicle_type = None self.settings = mp_settings.MPSettings( [ ('link', int, 1), ('altreadout', int, 10), ('distreadout', int, 200), ('heartbeat', int, 1), ('numcells', int, 1), ('mavfwd', int, 1), ('mavfwd_rate', int, 0), ('streamrate', int, 4), ('streamrate2', int, 4), ('heartbeatreport', int, 1), ('moddebug', int, 0), ('rc1mul', int, 1), ('rc2mul', int, 1), ('rc4mul', int, 1), ('shownoise', int, 1), ('basealt', int, 0), ('wpalt', int, 100), ('parambatch', int, 10), ('requireexit', int, 0)] ) 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.functions = MAVFunctions() self.select_extra = {} self.continue_mode = False self.aliases = {}
def __init__(self, mpstate): """Initialise module""" super(example, self).__init__(mpstate, "example", "") self.status_callcount = 0 self.boredom_interval = 10 # seconds self.last_bored = time.time() self.packets_mytarget = 0 self.packets_othertarget = 0 self.example_settings = mp_settings.MPSettings([ ('verbose', bool, False), ]) self.add_command('example', self.cmd_example, "example module", ['status', 'set (LOGSETTING)'])
def __init__(self, mpstate): """Initialise module""" super(llcamera, self).__init__(mpstate, "example", "") self.status_callcount = 0 self.boredom_interval = 10 # seconds self.last_bored = time.time() self.packets_mytarget = 0 self.packets_othertarget = 0 self.verbose = False self.example_settings = mp_settings.MPSettings( [ ('verbose', bool, False), ]) self.add_command('llcamera', self.cmd_example, "llcamera module", ['status','set (LOGSETTING)']) self.context = gp.gp_context_new() self.camera = gp.check_result(gp.gp_camera_new()) self.path = datetime.datetime.now().strftime("%Y_%m_%d_%H_%M") self.targetPath = os.path.join('/home/ubuntu/Pictures', self.path) self.vehicleLat = 0.0 # Current Vehicle Latitude self.vehicleLon = 0.0 # Current Vehicle Longitude self.vehicleHdg = 0.0 # Current Vehicle Heading self.vehicleAMSL = 0.0 # Current Vehicle Altitude above mean sea level self.vehicleRoll = 0.0 # Current Vehicle Roll self.vehiclePitch = 0.0 # Current Vehicle Pitch self.vehicleYaw = 0.0 self.imageLogFilePath = os.path.join(self.targetPath, 'log_' + self.path + '.txt') print(self.imageLogFilePath) if not os.path.exists(self.targetPath): os.makedirs(self.targetPath) file = open(self.imageLogFilePath, 'w') file.write('Filename\tLatitude\tLongitude\tAlt (AMSL)\tRoll\tPitch\tYaw\n') file.close() while True: try: self.camera.init(self.context) except gp.GPhoto2Error as ex: if ex.code == gp.GP_ERROR_MODEL_NOT_FOUND: # no camera, try again in 2 seconds time.sleep(2) continue # some other error we can't handle here raise # operation completed successfully so exit loop break # continue with rest of program print('Camera connected.')