def __init__(self, mpstate): super(RCModule, self).__init__(mpstate, "rc", "rc command handling", public = True) self.override = [ 0 ] * 8 self.last_override = [ 0 ] * 8 self.override_counter = 0 self.add_command('rc', self.cmd_rc, "RC input control", ['<1|2|3|4|5|6|7|8|all>']) self.add_command('switch', self.cmd_switch, "flight mode switch control", ['<0|1|2|3|4|5|6>']) if self.sitl_output: self.override_period = mavutil.periodic_event(20) else: self.override_period = mavutil.periodic_event(1)
def __init__(self, mpstate): super(SetRefModule, self).__init__(mpstate, "setref", "Sets IMU 0 reference point",public=True) self.override = [ 0 ] * 16 self.last_override = [ 0 ] * 16 self.override_counter = 0 self.add_command('setref', self.cmd_ref, "Set IMU reference point", ['']) self.add_command('print_ref', self.print_ref, "print IMU reference point", ['']) self.waiting_for_command = True if self.sitl_output: self.override_period = mavutil.periodic_event(20) else: self.override_period = mavutil.periodic_event(1)
def init(_mpstate): '''initialise module''' global mpstate mpstate = _mpstate mpstate.rc_state = rc_state() state = mpstate.rc_state mpstate.command_map['rc'] = (cmd_rc, "RC input control") mpstate.command_map['switch'] = (cmd_switch, "flight mode switch control") mpstate.completions['rc'] = ['<1|2|3|4|5|6|7|8|all>'] mpstate.completions['switch'] = ['<0|1|2|3|4|5|6>'] if mpstate.sitl_output: state.override_period = mavutil.periodic_event(20) else: state.override_period = mavutil.periodic_event(1)
def __init__(self): self.gps = None self.msgs = {} self.msg_count = {} self.sock = 0 self.counters = {'MasterIn' : [], 'MasterOut' : 0, 'FGearIn' : 0, 'FGearOut' : 0, 'Slave' : 0} self.setup_mode = opts.setup self.mav_error = 0 self.altitude = 0 self.last_altitude_announce = 0.0 self.last_distance_announce = 0.0 self.exit = False self.flightmode = 'MAV' self.last_mode_announce = 0 self.logdir = None self.last_heartbeat = 0 self.last_message = 0 self.heartbeat_error = False self.last_apm_msg = None self.last_apm_msg_time = 0 self.highest_msec = 0 self.have_gps_lock = False self.lost_gps_lock = False self.last_gps_lock = 0 self.watch = None self.last_streamrate1 = -1 self.last_streamrate2 = -1 self.last_seq = 0 self.armed = False self.current_depth = 0 self.xcenter = 0 self.ycenter = 0 self.circle_depth = 0 self.isCircle = 0 self.depth_stream = collections.deque([0]*5, 5) self.sock_failure_data = False self.socket_open = False self.pwm_val = 0 self.auto = False # integrate rc module self.override = [ 0 ] * 16 self.last_override = [ 0 ] * 16 self.override_counter = 0 self.sitl_output = False self.disarm_flag = False self.auto_t_started = False if self.sitl_output: self.override_period = mavutil.periodic_event(20) else: self.override_period = mavutil.periodic_event(1)
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, master, sysID=1, compoID=250): if master == "/dev/ttyACM0": master = detect_pixhawk() print "Pixhawk Master is", master baud = 115200 reboot = False else: baud = 57600 reboot = True self.conn = mavutil.mavlink_connection(device=master, baud=baud, autoreconnect=True) if reboot: logging.info("Reboot pixhawk !") self.conn.reboot_autopilot() time.sleep(0.5) self.sysID = sysID self.compoID = compoID self.heartbeat_period = mavutil.periodic_event(1) self.heartbeat_period.frequency = 1.0 self.recv_msg_count = 0 self.bad_data_count = 0 self.param_names = {} for i in range(1, 8): self.param_names["RC%d_MIN" % i] = None self.param_names["RC%d_MAX" % i] = None self.initialize()
def __init__(self, mav_param, observer): self.mav_param_set = set() self.mav_param_count = 0 self.param_period = mavutil.periodic_event(1) self.fetch_one = 0 self.mav_param = mav_param self.observer = observer
def __init__(self, mpstate): super(BatteryModule, self).__init__(mpstate, "battery", "battery commands") self.add_command('bat', self.cmd_bat, "show battery information") self.last_battery_announce = 0 self.last_battery_announce_time = 0 self.last_battery_cell_announce_time = 0 self.battery_level = -1 self.voltage_level = -1 self.current_battery = -1 self.battery2_voltage = -1 self.per_cell = 0 self.servo_voltage = -1 self.high_servo_voltage = -1 self.last_servo_warn_time = 0 self.last_vcc_warn_time = 0 self.settings.append( MPSetting('battwarn', int, 1, 'Battery Warning Time', tab='Battery')) self.settings.append( MPSetting('batwarncell', float, 3.7, 'Battery cell Warning level')) self.settings.append( MPSetting('servowarn', float, 4.3, 'Servo voltage warning level')) self.settings.append( MPSetting('vccwarn', float, 4.3, 'Vcc voltage warning level')) self.settings.append(MPSetting('numcells', int, 0, range=(0,10), increment=1)) self.battery_period = mavutil.periodic_event(5)
def __init__(self): self.wp_op = None self.wp_save_filename = None self.wploader = mavwp.MAVWPLoader() self.loading_waypoints = False self.loading_waypoint_lasttime = time.time() self.last_waypoint = 0 self.wp_period = mavutil.periodic_event(0.5)
def __init__(self, mpstate): super(ADSBModule, self).__init__(mpstate, "adsb", "ADS-B data support") 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, 10), # 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.threat_detection_timer = mavutil.periodic_event(2) self.threat_timeout_timer = mavutil.periodic_event(2)
def __init__(self, mav_param, logdir, vehicle_name, parm_file): self.mav_param_set = set() self.mav_param_count = 0 self.param_period = mavutil.periodic_event(1) self.fetch_one = 0 self.mav_param = mav_param self.logdir = logdir self.vehicle_name = vehicle_name self.parm_file = parm_file
def __init__(self, mpstate): super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public=True) self.wp_op = None self.wp_save_filename = None self.wploader = mavwp.MAVWPLoader() self.loading_waypoints = False self.loading_waypoint_lasttime = time.time() self.last_waypoint = 0 self.wp_period = mavutil.periodic_event(0.5) self.undo_wp = None self.undo_type = None self.undo_wp_idx = -1 self.add_command( "wp", self.cmd_wp, "waypoint management", ["<list|clear|move|remove|loop|set|undo|movemulti|param>", "<load|update|save|show> (FILENAME)"], ) if self.continue_mode and self.logdir != None: waytxt = os.path.join(mpstate.status.logdir, "way.txt") if os.path.exists(waytxt): self.wploader.load(waytxt) print("Loaded waypoints from %s" % waytxt) if mp_util.has_wxpython: self.menu_added_console = False self.menu_added_map = False self.menu = MPMenuSubMenu( "Mission", items=[ MPMenuItem("Clear", "Clear", "# wp clear"), MPMenuItem("List", "List", "# wp list"), MPMenuItem( "Load", "Load", "# wp load ", handler=MPMenuCallFileDialog(flags=wx.FD_OPEN, title="Mission Load", wildcard="*.txt"), ), MPMenuItem( "Save", "Save", "# wp save ", handler=MPMenuCallFileDialog( flags=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT, title="Mission Save", wildcard="*.txt" ), ), MPMenuItem( "Draw", "Draw", "# wp draw ", handler=MPMenuCallTextDialog(title="Mission Altitude (m)", default=100), ), MPMenuItem("Undo", "Undo", "# wp undo"), MPMenuItem("Loop", "Loop", "# wp loop"), ], )
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, mpstate): super(ParamModule, self).__init__(mpstate, "param", "parameter handling", public = True) self.mav_param_set = set() self.mav_param_count = 0 self.param_period = mavutil.periodic_event(1) self.fetch_one = 0 self.add_command('param', self.cmd_param, "parameter handling", ["<fetch|download>", "<set|show|fetch|help> (PARAMETER)", "<load|save> (FILENAME)"]) if self.continue_mode and self.logdir != None: parmfile = os.path.join(self.logdir, 'mav.parm') if os.path.exists(parmfile): mpstate.mav_param.load(parmfile)
def __init__(self, mav_param, logdir, vehicle_name, parm_file): self.mav_param_set = set() self.mav_param_count = 0 self.param_period = mavutil.periodic_event(1) self.fetch_one = dict() self.mav_param = mav_param self.logdir = logdir self.vehicle_name = vehicle_name self.parm_file = parm_file self.fetch_set = None self.xml_filepath = None self.new_sysid_timestamp = time.time() self.autopilot_type_by_sysid = {} self.param_types = {}
def __init__(self, mpstate): super(BatteryModule, self).__init__(mpstate, "battery", "battery commands") self.add_command('bat', self.cmd_bat, "show battery information") self.last_battery_announce = 0 self.last_battery_announce_time = 0 self.last_battery_cell_announce_time = 0 self.battery_level = -1 self.voltage_level = -1 self.per_cell = 0 self.settings.append( MPSetting('battwarn', int, 1, 'Battery Warning Time')) self.settings.append( MPSetting('batwarncell', float, 3.7, 'Battery cell Warning level')) self.settings.append(MPSetting('numcells', int, 0, range=(0,10), increment=1)) self.battery_period = mavutil.periodic_event(0.3)
def __init__(self, mpstate): super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public = True) self.wp_op = None self.wp_save_filename = None self.wploader = mavwp.MAVWPLoader() self.loading_waypoints = False self.loading_waypoint_lasttime = time.time() self.last_waypoint = 0 self.wp_period = mavutil.periodic_event(0.5) self.add_command('wp', self.cmd_wp, 'waypoint management', ["<list|clear>", "<load|update|save> (FILENAME)"]) if self.continue_mode and self.logdir != None: waytxt = os.path.join(mpstate.status.logdir, 'way.txt') if os.path.exists(waytxt): self.wploader.load(waytxt) print("Loaded waypoints from %s" % waytxt)
def __init__(self, mpstate): super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public = True) self.wp_op = None self.wp_requested = {} self.wp_received = {} self.wp_save_filename = None self.wploader_by_sysid = {} self.loading_waypoints = False self.loading_waypoint_lasttime = time.time() self.last_waypoint = 0 self.wp_period = mavutil.periodic_event(0.5) self.undo_wp = None self.undo_type = None self.undo_wp_idx = -1 self.wploader.expected_count = 0 self.add_command('wp', self.cmd_wp, 'waypoint management', ["<list|clear|move|remove|loop|set|undo|movemulti|changealt|param|status>", "<load|update|save|savecsv|show> (FILENAME)"]) if self.continue_mode and self.logdir is not None: waytxt = os.path.join(mpstate.status.logdir, 'way.txt') if os.path.exists(waytxt): self.wploader.load(waytxt) print("Loaded waypoints from %s" % waytxt) self.menu_added_console = False self.menu_added_map = False if mp_util.has_wxpython: self.menu = MPMenuSubMenu('Mission', items=[MPMenuItem('Editor', 'Editor', '# wp editor'), MPMenuItem('Clear', 'Clear', '# wp clear'), MPMenuItem('List', 'List', '# wp list'), MPMenuItem('Load', 'Load', '# wp load ', handler=MPMenuCallFileDialog(flags=('open',), title='Mission Load', wildcard='*.txt')), MPMenuItem('Save', 'Save', '# wp save ', handler=MPMenuCallFileDialog(flags=('save', 'overwrite_prompt'), title='Mission Save', wildcard='*.txt')), MPMenuItem('Draw', 'Draw', '# wp draw ', handler=MPMenuCallTextDialog(title='Mission Altitude (m)', default=100)), MPMenuItem('Undo', 'Undo', '# wp undo'), MPMenuItem('Loop', 'Loop', '# wp loop'), MPMenuItem('Add NoFly', 'Loop', '# wp noflyadd')])
def connectionMade(self): self.host = self.transport.getHost() print "Starting MAVLink via {0}".format(self.host) self.mav.set_callback(self.master_callback,self) if hasattr(self.mav, 'set_send_callback'): self.mav.set_send_callback(self.master_send_callback, self) self.linkerror = False self.link_delayed = False self.last_heartbeat = 0 self.last_message = 0 self.highest_msec = 0 #0.1 s send heartbeat self.wait_heartbeat() self.msg_period = mavutil.periodic_event(1.0/15) self.connect_set_up() #self.WIRE_PROTOCOL_VERSION = "1.0" self.fd = self.transport.fileno() self.l.start(0.1)
def main(): # Create a Joystick object js = JoyStick() # GCS Handling global in_emergcs; # create a mavlink serial instance master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate, source_system=opts.SOURCE_SYSTEM) #master.mav.command_long_send(master.target_system, master.target_component, mavutil.mavlink.MAV_CMD_DO_SWITCH_GCS, 0, 0, 255, 0, 0, 0, 0, 0,) # wait for the heartbeat msg to find the system ID wait_heartbeat(master) print("Got HeartBeat") # Set the GCS_SYSID so that we handle lower packet com #mavparam = mavparm.MAVParmDict() #mavparam.mavset(master, SYSID_MYGCS, 255, retries=3) # Create loop timing js_period = mavutil.periodic_event(opts.rate) while True: # Sanity check to make sure that the traffic we want to be flowing is... # See note at bottom for messages read_messages(master) # This should trigger at 60Hz and send rc override js_input = js.get_rc_override() handle_GCS_change(master, js_input[8:]) if in_emergcs and js_period.trigger(): # Send RC commands over mavlink master.mav.rc_channels_override_send(master.target_system, master.target_component, *js_input[:8])
def __init__(self, mpstate): super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public = True) self.wp_op = None self.wp_save_filename = None self.wploader = mavwp.MAVWPLoader() self.loading_waypoints = False self.loading_waypoint_lasttime = time.time() self.last_waypoint = 0 self.wp_period = mavutil.periodic_event(0.5) self.add_command('wp', self.cmd_wp, 'waypoint management', ["<list|clear|move|remove|loop|set>", "<load|update|save> (FILENAME)"]) if self.continue_mode and self.logdir != None: waytxt = os.path.join(mpstate.status.logdir, 'way.txt') if os.path.exists(waytxt): self.wploader.load(waytxt) print("Loaded waypoints from %s" % waytxt) self.menu_added_console = False self.menu_added_map = False from MAVProxy.modules.lib.mp_menu import * self.menu = MPMenuSubMenu('Mission', items=[MPMenuItem('Clear', 'Clear', '# wp clear'), MPMenuItem('List', 'List', '# wp list'), MPMenuItem('Load', 'Load', '# wp load ', handler=MPMenuCallFileDialog(flags=wx.FD_OPEN, title='Mission Load', wildcard='*.txt')), MPMenuItem('Save', 'Save', '# wp save ', handler=MPMenuCallFileDialog(flags=wx.FD_SAVE|wx.FD_OVERWRITE_PROMPT, title='Mission Save', wildcard='*.txt')), MPMenuItem('Draw', 'Draw', '# wp draw ', handler=MPMenuCallTextDialog(title='Mission Altitude (m)', default=100)), MPMenuItem('Loop', 'Loop', '# wp loop')])
context = zmq.Context() socket_sub = context.socket(zmq.SUB) socket_sub.connect('tcp://%s:%d' % config.zmq_pub_unreal_proxy) socket_sub.setsockopt(zmq.SUBSCRIBE, config.topic_unreal_state) socket_sub.setsockopt(zmq.SUBSCRIBE, config.topic_unreal_drone_rgb_camera % drone_num) mav1 = mavutil.mavlink_connection('udp:127.0.0.1:%d' % (14551 + 0 * 10)) print("Waiting for HEARTBEAT") mav1.wait_heartbeat() print("Heartbeat from APM (system %u component %u)" % (mav1.target_system, mav1.target_component)) event = mavutil.periodic_event(0.3) event1hz = mavutil.periodic_event(1) freq = 30 pub_position_event = mavutil.periodic_event(freq) def set_rcs(rc1, rc2, rc3, rc4): global mav1 values = [1500] * 8 values[0] = rc1 values[1] = rc2 values[2] = rc3 values[3] = rc4 mav1.mav.rc_channels_override_send(mav1.target_system, mav1.target_component, *values)
mav1.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) mav2.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) print("Setting declination") mav1.mav.param_set_send(mav1.target_system, mav1.target_component, 'COMPASS_DEC', radians(12.33)) mav2.mav.param_set_send(mav2.target_system, mav2.target_component, 'COMPASS_DEC', radians(12.33)) set_attitude(1060, 1160) event = mavutil.periodic_event(30) pevent = mavutil.periodic_event(0.3) rc3_min = 1060 rc3_max = 1850 rc4_min = 1080 rc4_max = 1500 rc3 = rc3_min rc4 = 1160 delta3 = 2 delta4 = 1 use_pitch = 1 MAV_ACTION_CALIBRATE_GYRO = 17 mav1.mav.action_send(mav1.target_system, mav1.target_component, MAV_ACTION_CALIBRATE_GYRO) mav2.mav.action_send(mav2.target_system, mav2.target_component,
def __init__(self): self.mav_param_set = set() self.mav_param_count = 0 self.param_period = mavutil.periodic_event(1) self.fetch_one = 0
def __init__(self, mpstate): super(CUAVModule, self).__init__(mpstate, "CUAV", "CUAV checks") self.console.set_status('Bottle', 'Bottle: --', row=8, fg='green') self.rate_period = mavutil.periodic_event(1.0/15)
def __init__(self, mpstate): super(MyPiModule, self).__init__(mpstate, "MyPiModule", "my commands") # my cmd self.add_command('mybat', self.cmd_mybat, "my battery information") self.add_command('myshut', self.cmd_myshutdown, "to shutdown") self.add_command('myreboot', self.cmd_myreboot, "to reboot") self.add_command('myrtl', self.cmd_myrtl, "change flight mode to RTL") # my settings self.settings.append(MPSetting('mytimebat', int, 5, 'Battery Interval Time sec', tab='my')) self.settings.append(MPSetting('mytimerc', int, 5, 'RC Interval Time sec')) self.settings.append(MPSetting('mydebug', bool, False, 'Debug')) self.settings.append(MPSetting('myminvolt', int, 10000, 'Minimum battery voltage before shutdown')) self.settings.append(MPSetting('myminremain', int, 10, 'Minimum battery remaining before shutdown')) self.settings.append(MPSetting('mydelayinit', int, 30, 'Delay before shutdown or reboot')) self.settings.append(MPSetting('myrcvideo', int, 6, 'Radio channel to change video on/off')) self.settings.append(MPSetting('myrcwlan0', int, 8, 'Radio channel to change wlan0 on/off')) self.settings.append(MPSetting('myrcyaw', int, 4, 'Radio channel to reboot/shutdown')) self.settings.append(MPSetting('myrcroll', int, 1, 'Radio channel to reboot/shutdown')) self.myversion = "1.4" # stats self.VFR_HUD = 0 self.SYS_STATUS = 0 self.HEARTBEAT = 0 self.RC_CHANNELS_RAW = 0 self.battery_period_trigger = 0 self.start_time = time.time() ### battery low : self.shutdown_by_lowbat = False self.shutdown_by_lowbat_time = 0 ### ByRadio resquested self.shutdown_by_radio = False self.shutdown_by_radio_time = 0 self.reboot_by_radio = False self.reboot_by_radio_time = 0 ### ByCmd resquested self.shutdown_by_cmd = False self.shutdown_by_cmd_time = 0 self.reboot_by_cmd = False self.reboot_by_cmd_time = 0 # default values self.armed = False self.mystate = 0 self.myvolt = 0 self.mythrottle = 0 self.mycurrent = 0 self.myremaining = 0 self.myrcraw = [0,0,0,0,0,0,0,0,0] self.wlan0_up = False self.video_on = True self.rtl_on = False self.last_battery_check_time = time.time() self.last_rc_check_time = time.time() self.battery_period = mavutil.periodic_event(5) self.FORMAT = '%Y-%m-%d %H:%M:%S' #self.FORMAT2 = '%Hh%Mm%Ss' self.mycountermessage = 0 # default to servo range of 1000 to 1700 #self.RC1_MIN = self.get_mav_param('RC1_MIN', 0) #self.RC1_MAX = self.get_mav_param('RC1_MAX', 0) self.RC_low_mark = [1200,1200,1200,1200,1200,1200,1200,1200,1200] self.RC_high_mark = [1700,1700,1700,1700,1700,1700,1700,1700,1700] self.myseverity = 0 self.mytext = "nulltext" self.myip = "0.0.0.0" # to send statustext self.master2 = mavutil.mavlink_connection("udp:127.0.0.1:14550", input=False, dialect="common")
def __init__(self, mpstate): super(Autopilotmodule, self).__init__(mpstate, "autopilot", "autopilot command ", public=True) self.add_command('autopilot', self.cmd_ap, "Autopilot input control") self.add_command('current_imu', self.print_imu, "prints out current IMU data", [ '' ]) self.add_command('open_socket', self.cmd_sock, "connect to socket", ['sockno']) self.add_command('close_socket', self.close_sock, "close socket", ['sockno']) self.add_command('current_depth', self.cmd_depth, "get current object depth") self.add_command('kill', self.cmd_kill, "sets rc values to 0") # camera parameters self.cam = None self.most_recent = None self.CAPTURE_WIDTH = 640.0 self.CAPTURE_HEIGHT = 480.0 self.HALF_CAPTURE_WIDTH = self.CAPTURE_WIDTH/2 self.HALF_CAPTURE_HEIGHT = self.CAPTURE_HEIGHT/2 #create cross-correlation templates self.col_temp = np.ones((self.CAPTURE_HEIGHT,1), np.uint8) #ROWS by 1 array self.row_temp = np.ones((1,self.CAPTURE_WIDTH), np.uint8) #1 by COLS array # for keeping track of vision loop speed self.t = 0 # classifier output self.target_in_frame = False self.last_frames = collections.deque([0]*10, 10) self.frame = 0 # RC radio data trims self.ch1_trim = 1500 self.ch2_trim = 1500 # Initialize PID coefficients self.x_Ap = 0.7 self.x_Ai = 0.05 self.x_Ad = 0.6 self.y_Ap = 0.8 self.y_Ai = 0.05 self.y_Ad = 0.6 # initialize errors self.x_error, self.y_error = 0, 0 self.old_x_error, self.old_y_error = 0, 0 self.xcenter = 0 self.ycenter = 0 # initialize error accumulators self.x_sigma = collections.deque([0,0,0,0], 4) self.y_sigma = collections.deque([0,0,0,0], 4) # initialize deltas self.x_delta = 0 self.y_delta = 0 # initialize timer self.t = time.time() self.dt = 0.5 # flag for pid first output behavior self.delta_flag = True self.sigma_flag = True mpstate = None # socket variables self.sock_option = False self.auto = False # set initial PWM values for movement states self.pwm_val = 1400 self.hover_pwm_val = 1530 # Depth values from realsense self.depth = 0 self.last_depth = collections.deque([0]*10, 10) # for summing last ten depth samples self.target_altitude, self.depth = 0, 0 self.waiting_for_command = True # override commands for motors self.override = [ 0 ] * 16 self.last_override = [ 0 ] * 16 self.override_counter = 0 # consistant IMU data update self.check_imu_counter = 0 #AHRS Filtering algorithm variables self.recipNorm = 0 self.beta = 0.1 self.sampleFreq = 512.0 # sample frequency in Hz self.q0 = 1.0 self.q1 = 0.0 self.q2 = 0.0 self.q3 = 0.0 # quaternion of sensor frame relative to auxiliary frame self.port = 0 self.s0, self.s1, self.s2, self.s3 = 0, 0, 0, 0 self.qDot1, self.qDot2, self.qDot3, self.qDot4 = 0, 0, 0, 0 self.hx, self.hy = 0, 0 self._2q0mx, self._2q0my, self._2q0mz, self._2q1mx, self._2bx, self._2bz, self._4bx, self._4bz, self._2q0, self._2q1, self._2q2, self._2q3, self._2q0q2, self._2q2q3, self.q0q0, self.q0q1, self.q0q2, self.q0q3, self.q1q1, self.q1q2, self.q1q3, self.q2q2, self.q2q3, self.q3q3 = 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 if self.sitl_output: self.override_period = mavutil.periodic_event(20) else: self.override_period = mavutil.periodic_event(1)
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 initMAVProxy(): from optparse import OptionParser parser = OptionParser("mavproxy.py [options]") parser.add_option("--master", dest="master", action='append', metavar="DEVICE[,BAUD]", help="MAVLink master port and optional baud rate", default=[]) parser.add_option("--out", dest="output", action='append', metavar="DEVICE[,BAUD]", help="MAVLink output port and optional baud rate", default=[]) parser.add_option("--baudrate", dest="baudrate", type='int', help="default serial baud rate", default=115200) parser.add_option("--sitl", dest="sitl", default=None, help="SITL output port") parser.add_option("--streamrate",dest="streamrate", default=4, type='int', help="MAVLink stream rate") parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', default=255, help='MAVLink source system for this GCS') parser.add_option("--target-system", dest='TARGET_SYSTEM', type='int', default=1, help='MAVLink target master system') parser.add_option("--target-component", dest='TARGET_COMPONENT', type='int', default=1, help='MAVLink target master component') parser.add_option("--logfile", dest="logfile", help="MAVLink master logfile", default='mav.tlog') parser.add_option("-a", "--append-log", dest="append_log", help="Append to log files", action='store_true', default=False) parser.add_option("--quadcopter", dest="quadcopter", help="use quadcopter controls", action='store_true', default=False) parser.add_option("--setup", dest="setup", help="start in setup mode", action='store_true', default=False) parser.add_option("--nodtr", dest="nodtr", help="disable DTR drop on close", action='store_true', default=False) parser.add_option("--show-errors", dest="show_errors", help="show MAVLink error packets", action='store_true', default=False) parser.add_option("--speech", dest="speech", help="use text to speach", action='store_true', default=False) parser.add_option("--num-cells", dest="num_cells", help="number of LiPo battery cells", type='int', default=0) parser.add_option("--aircraft", dest="aircraft", help="aircraft name", default=None) parser.add_option("--cmd", dest="cmd", help="initial commands", default=None) parser.add_option("--console", action='store_true', help="use GUI console") parser.add_option("--map", action='store_true', help="load map module") parser.add_option( '--load-module', action='append', default=[], help='Load the specified module. Can be used multiple times, or with a comma separated list') parser.add_option("--mav09", action='store_true', default=False, help="Use MAVLink protocol 0.9") parser.add_option("--auto-protocol", action='store_true', default=False, help="Auto detect MAVLink protocol version") parser.add_option("--nowait", action='store_true', default=False, help="don't wait for HEARTBEAT on startup") parser.add_option("--continue", dest='continue_mode', action='store_true', default=False, help="continue logs") parser.add_option("--dialect", default="ardupilotmega", help="MAVLink dialect") parser.add_option("--rtscts", action='store_true', help="enable hardware RTS/CTS flow control") (opts, args) = parser.parse_args() mavproxy.opts=opts if opts.mav09: os.environ['MAVLINK09'] = '1' from pymavlink import mavutil, mavparm mavutil.set_dialect(opts.dialect) # global mavproxy state mpstate = MPState() mavproxy.mpstate=mpstate mpstate.status.exit = False mpstate.command_map = command_map mpstate.continue_mode = opts.continue_mode if opts.speech: # start the speech-dispatcher early, so it doesn't inherit any ports from # modules/mavutil load_module('speech') if not opts.master: serial_list = mavutil.auto_detect_serial(preferred_list=['*FTDI*',"*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*"]) if len(serial_list) == 1: opts.master = [serial_list[0].device] else: print(''' Please choose a MAVLink master with --master For example: --master=com14 --master=/dev/ttyUSB0 --master=127.0.0.1:14550 Auto-detected serial ports are: ''') for port in serial_list: print("%s" % port) sys.exit(1) # container for status information mpstate.status.target_system = opts.TARGET_SYSTEM mpstate.status.target_component = opts.TARGET_COMPONENT mpstate.mav_master = [] # open master link for mdev in opts.master: if ',' in mdev and not os.path.exists(mdev): port, baud = mdev.split(',') else: port, baud = mdev, opts.baudrate m = mavutil.mavlink_connection(port, autoreconnect=True, baud=int(baud)) m.mav.set_callback(master_callback, m) if hasattr(m.mav, 'set_send_callback'): m.mav.set_send_callback(master_send_callback, m) if opts.rtscts: m.set_rtscts(True) m.linknum = len(mpstate.mav_master) m.linkerror = False m.link_delayed = False m.last_heartbeat = 0 m.last_message = 0 m.highest_msec = 0 mpstate.mav_master.append(m) mpstate.status.counters['MasterIn'].append(0) # log all packets from the master, for later replay open_logs() # open any mavlink UDP ports for p in opts.output: if ',' in p and not os.path.exists(p): port, baud = p.split(',') else: port, baud = p, opts.baudrate mpstate.mav_outputs.append(mavutil.mavlink_connection(port, baud=int(baud), input=False)) if opts.sitl: mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False) mpstate.settings.numcells = opts.num_cells mpstate.settings.streamrate = opts.streamrate mpstate.settings.streamrate2 = opts.streamrate mavproxy.msg_period = mavutil.periodic_event(1.0/15) mavproxy.heartbeat_period = mavutil.periodic_event(1) mavproxy.battery_period = mavutil.periodic_event(0.1) mavproxy.heartbeat_check_period = mavutil.periodic_event(0.33) mpstate.rl = rline.rline("MAV> ", mpstate) if opts.setup: mpstate.rl.set_prompt("") if 'HOME' in os.environ and not opts.setup: start_script = os.path.join(os.environ['HOME'], ".mavinit.scr") if os.path.exists(start_script): run_script(start_script) if opts.aircraft is not None: start_script = os.path.join(opts.aircraft, "mavinit.scr") if os.path.exists(start_script): run_script(start_script) else: print("no script %s" % start_script) if not opts.setup: # some core functionality is in modules standard_modules = ['log','rally','fence','param','relay', 'tuneopt','arm','mode','calibration','rc','wp','auxopt','quadcontrols','test'] for m in standard_modules: load_module(m, quiet=True) if opts.console: process_stdin('module load console') if opts.map: process_stdin('module load map') for module in opts.load_module: modlist = module.split(',') for mod in modlist: process_stdin('module load %s' % mod) if opts.cmd is not None: cmds = opts.cmd.split(';') for c in cmds: process_stdin(c) # run main loop as a thread mpstate.status.thread = threading.Thread(target=main_loop) mpstate.status.thread.daemon = True mpstate.status.thread.start()
print("Heartbeat from APM (system %u component %u)" % (mav2.target_system, mav2.target_system)) print("Waiting for MANUAL mode") mav1.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) mav2.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) print("Setting declination") mav1.mav.param_set_send(mav1.target_system, mav1.target_component, 'COMPASS_DEC', radians(12.33)) mav2.mav.param_set_send(mav2.target_system, mav2.target_component, 'COMPASS_DEC', radians(12.33)) set_attitude(1060, 1160) event = mavutil.periodic_event(30) pevent = mavutil.periodic_event(0.3) rc3_min = 1060 rc3_max = 1850 rc4_min = 1080 rc4_max = 1500 rc3 = rc3_min rc4 = 1160 delta3 = 2 delta4 = 1 use_pitch = 1 MAV_ACTION_CALIBRATE_GYRO = 17 mav1.mav.action_send(mav1.target_system, mav1.target_component, MAV_ACTION_CALIBRATE_GYRO) mav2.mav.action_send(mav2.target_system, mav2.target_component, MAV_ACTION_CALIBRATE_GYRO)
# log all packets from the master, for later replay open_logs() # open any mavlink UDP ports for p in opts.output: mpstate.mav_outputs.append(mavutil.mavlink_connection(p, baud=opts.baudrate, input=False)) if opts.sitl: mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False) mpstate.settings.numcells = opts.num_cells mpstate.settings.streamrate = opts.streamrate mpstate.settings.streamrate2 = opts.streamrate msg_period = mavutil.periodic_event(1.0/15) heartbeat_period = mavutil.periodic_event(1) battery_period = mavutil.periodic_event(0.1) heartbeat_check_period = mavutil.periodic_event(0.33) mpstate.rl = rline.rline("MAV> ", mpstate) if opts.setup: mpstate.rl.set_prompt("") if 'HOME' in os.environ and not opts.setup: start_script = os.path.join(os.environ['HOME'], ".mavinit.scr") if os.path.exists(start_script): run_script(start_script) if opts.aircraft is not None: start_script = os.path.join(opts.aircraft, "mavinit.scr")
def __init__(self, period, cmd): self.period = period self.cmd = cmd self.event = mavutil.periodic_event(1.0/period)