def set_up(): parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.') parser.add_argument('--connect', help="Vehicle connection target string. If not specified, SITL automatically started and used.") args = parser.parse_args() connection_string = args.connect sitl = None ################################################################################################ #Start SITL if no connection string specified ################################################################################################ if not connection_string: import dronekit_sitl #sitl = dronekit_sitl.start_default() #connection_string = sitl.connection_string() ardupath ="/home/uav/git/ardupilot" home = "41.714801,-86.241871,221,0" sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') sitl_args = ['-I{}'.format(0), '--home', home, '--model', '+', '--defaults', sitl_defaults] sitl = dronekit_sitl.SITL(path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') port = str(int(port) + 0 * 10) connection_string = ':'.join([tcp, ip, port])
def initializeVehicle(self, ardupath, connection_string, home, name): self.logActive = False home = home + ",221,0" if not connection_string: sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') sitl_args = [ '-I{}'.format(len(Vehicle.vehicles)), '--home', home, '--model', '+', '--defaults', sitl_defaults ] sitl = dronekit_sitl.SITL(path=os.path.join( ardupath, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') port = str(int(port) + len(Vehicle.vehicles) * 10) connection_string = ':'.join([tcp, ip, port]) print('Connecting to vehicle on: %s' % connection_string) self.vehicle = connect(connection_string, wait_ready=True) Vehicle.vehicles[name] = self.vehicle self.vehicle.wait_ready(timeout=500) return self.vehicle
def bringup_drone(self, connection_string=None): """Connect to a dronekit vehicle or instantiate an sitl simulator. Call this once everything is set up and you're ready to fly. This is some deep magic related to running multiple drones at once. We don't technically need it, since the network architecture I've set up has the drones all be independant (if you want to simulate multiple drones with the current setup you run multiple instances of the SITL simulator). Chris got this all working by talking with the dronekit devs, so he knows a lot more about it than I do. connection_string -- Connect to an existing mavlink (SITL or the actual ArduPilot). Provide None and it'll start its own simulator. """ if not connection_string: # Start SITL if no connection string specified print "Starting SITL" self.sitl = dronekit_sitl.SITL() self.sitl.download('copter', '3.3', verbose=True) sitl_args = ['--model', 'quad', '--home=32.990756,-117.128362,243,0', '--speedup', str(Pilot.sim_speedup), '--instance', str(self.instance)] working_dir = tempfile.mkdtemp() self.sitl.launch(sitl_args, verbose=True, await_ready=True, restart=True, wd=working_dir) time.sleep(6) # Allow time for the parameter to go back to EEPROM connection_string = "tcp:127.0.0.1:{0}".format(5760 + 10 * self.instance) #connection_string = "tcp:127.0.0.1:14550") new_sysid = self.instance + 1 vehicle = dronekit.connect(connection_string, wait_ready=True) while vehicle.parameters["SYSID_THISMAV"] != new_sysid: vehicle.parameters["SYSID_THISMAV"] = new_sysid time.sleep(0.1) time.sleep(5) # allow eeprom write vehicle.close() self.sitl.stop() # Do it again, and this time SYSID_THISMAV will have changed self.sitl.launch(sitl_args, verbose=True, await_ready=True, restart=True, use_saved_data=True, wd=working_dir) self.vehicle = dronekit.connect(connection_string, wait_ready=True) print vehicle else: # Connect to existing vehicle print 'Connecting to vehicle on: %s' % connection_string print "Connect to {0}, instance {1}".format(connection_string, self.instance) self.vehicle = dronekit.connect(connection_string, wait_ready=True) print "Success {0}".format(connection_string)
def bringup_drone(self, connection_string=None): """ Call this once everything is set up and you're ready to fly :param connection_string: Connect to an existing mavlink (SITL or the actual ArduPilot) Provide None and it'll start its own simulator :return: """ if not connection_string: # Start SITL if no connection string specified print "Starting SITL" self.sitl = dronekit_sitl.SITL() self.sitl.download('copter', '3.3', verbose=True) sitl_args = [ '--model', 'quad', '--home=32.990756,-117.128362,243,0', '--speedup', str(Pilot.sim_speedup), '--instance', str(self.instance) ] working_dir = tempfile.mkdtemp() self.sitl.launch(sitl_args, verbose=True, await_ready=True, restart=True, wd=working_dir) time.sleep(6) # Allow time for the parameter to go back to EEPROM connection_string = "tcp:127.0.0.1:{0}".format(5760 + 10 * self.instance) #connection_string = "tcp:127.0.0.1:14550") new_sysid = self.instance + 1 vehicle = dronekit.connect(connection_string, wait_ready=True) while vehicle.parameters["SYSID_THISMAV"] != new_sysid: vehicle.parameters["SYSID_THISMAV"] = new_sysid time.sleep(0.1) time.sleep(5) # allow eeprom write vehicle.close() self.sitl.stop() # Do it again, and this time SYSID_THISMAV will have changed self.sitl.launch(sitl_args, verbose=True, await_ready=True, restart=True, use_saved_data=True, wd=working_dir) self.vehicle = dronekit.connect(connection_string, wait_ready=True) print vehicle else: # Connect to existing vehicle print 'Connecting to vehicle on: %s' % connection_string print "Connect to {0}, instance {1}".format( connection_string, self.instance) self.vehicle = dronekit.connect(connection_string, wait_ready=True) print "Success {0}".format(connection_string)
def initialize_drone( self, vehicle_num, home="41.7144367,-86.2417136,221,0", connection_string="", ): self.vehicle_id = "UAV-" + str(vehicle_num) print("\nInitializing drone: " + self.vehicle_id) parser = argparse.ArgumentParser( description='Print out vehicle state information') parser.add_argument( '--connect', help= "vehicle connection target string. If not specified, SITL automatically started and used" ) args = parser.parse_args() connection_string = args.connect sitl = None #Start SITL if no connection string specified if not connection_string: import dronekit_sitl # connection_string = sitl.connection_string() ardupath = "/home/uav/git/ardupilot" self.home = home # In this example, all UAVs start on top of each other! sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') sitl_args = [ '-I{}'.format(vehicle_num), '--home', home, '--model', '+', '--defaults', sitl_defaults ] sitl = dronekit_sitl.SITL(path=os.path.join( ardupath, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') print(port + " " + str(vehicle_num)) port = str(int(port) + vehicle_num * 10) connection_string = ':'.join([tcp, ip, port]) ################################################################################################ # Connect to the Vehicle ################################################################################################ print 'Connecting to vehicle on: %s' % connection_string self.vehicle = connect(connection_string, wait_ready=True) self.vehicle.wait_ready(timeout=120) return self.vehicle, sitl time.sleep(10)
def connect_vehicle(instance, home): home_ = tuple(home) + (0,) home_ = ','.join(map(str, home_)) sitl_defaults = os.path.join(ARDUPATH, 'Tools', 'autotest', 'default_params', 'copter.parm') sitl_args = ['-I{}'.format(instance), '--home', home_, '--model', '+', '--defaults', sitl_defaults] sitl = dronekit_sitl.SITL(path=os.path.join(ARDUPATH, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') port = str(int(port) + instance * 10) conn_string = ':'.join([tcp, ip, port]) vehicle = dronekit.connect(conn_string) vehicle.wait_ready(timeout=120) return vehicle, sitl
def start_sitl(sitl_instance, ardupath, home=(41.71544, -86.24284, 0)): home_str = ','.join(map(str, tuple(home) + (0, ))) sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') sitl_args = [ '-I{}'.format(sitl_instance), '--home', home_str, '--model', '+', '--defaults', sitl_defaults ] sitl = dronekit_sitl.SITL( path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') port = str(int(port) + sitl_instance * 10) conn_string = ':'.join([tcp, ip, port]) return sitl, conn_string
def connect_vehicle(self, vehicle_type, vehicle_id, home, ardupath): wait_till_armable = True defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') if len(home) == 2: home = tuple(home) + (0,0) else: home = tuple(home) if vehicle_type == 'VRTL': sitl_args = ['--home', ','.join(map(str, home))] print "Trying to launch SIT instance" sitl = dronekit_sitl.SITL(path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') conn_string = ':'.join([tcp, ip, port]) vehicle = dronekit.connect(conn_string) vehicle.wait_ready(timeout=120) print "Vehicle connected" if wait_till_armable: while not vehicle.is_armable: time.sleep(3) print "Vehicle armable" time.sleep(3) self.vehicle = vehicle self.vid = vehicle_id handshake_message = DroneHandshakeMessage(self.vid, self.vehicle) self.handshake_to_dronology_msgs.put_message(handshake_message.from_vehicle(self.vehicle, self.vid)) print "sent handshake" self.state_msg_timer = Timer(self.state_interval, self.send_state_message)
################################################################################################ #Start SITL if no connection string specified ################################################################################################ if not connection_string: import dronekit_sitl #sitl = dronekit_sitl.start_default() #connection_string = sitl.connection_string() ardupath = "/home/uav/git/ardupilot" home = "41.7144367,-86.2417136,221,0" sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') sitl_args = [ '-I{}'.format(0), '--home', home, '--model', '+', '--defaults', sitl_defaults ] sitl = dronekit_sitl.SITL( path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') port = str(int(port) + 0 * 10) connection_string = ':'.join([tcp, ip, port]) #vehicle = dronekit.connect(conn_string) #vehicle.wait_ready(timeout=120) ################################################################################################ # Connect to the Vehicle ################################################################################################ print 'Connecting to vehicle on: %s' % connection_string vehicle = connect(connection_string, wait_ready=True) print vehicle.location.global_relative_frame.lon
'Print out vehicle state information. Connects to SITL on local PC by default.' ) parser.add_argument( '--connect', help= "vehicle connection target string. If not specified, SITL automatically started and used." ) args = parser.parse_args() connection_string = args.connect sitl = None if not connection_string: import dronekit_sitl sitl = dronekit_sitl.SITL() sitl.download("copter", "3.3") sitl.launch(["--home=51.449,5.493,1,0 --rate 30"], await_ready=True) connection_string = sitl.connection_string() drone = Cyclone(connection_string, configs) # drone.awake_script() drone.arm_and_takeoff(5) drone.set_airspeed(5) for i in range(len(listWPs)): print('Start Mission %d' % (i + 1)) drone.goto_wp_global(LocationGlobalRelative(*listWPs[i])) # drone.mode_rtl() del drone if sitl:
def _connect_vehicle(self, vehicle_type, vehicle_id, ip, instance, ardupath, rate, home, baud, speedup, defaults=None, wait_till_armable=True): """ Connect to a vehicle :param vehicle_type: :param vehicle_id: :param ip: :param instance: :param ardupath: :param rate: :param home: :param baud: :param speedup: :param wait_till_armable: :return: """ self._connection_initiated = True status = 0 vehicle = None if defaults is None: defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') if home is not None: if len(home) == 2: home = tuple(home) + (0, 0) else: home = tuple(home) try: if vehicle_type == DRONE_TYPE_PHYS: vehicle = dronekit.connect(ip, wait_ready=True, baud=baud) self._v_type = DRONE_TYPE_PHYS if vehicle_id is None: vehicle_id = ip elif vehicle_type == DRONE_TYPE_SITL_VRTL: if vehicle_id is None: vehicle_id = vehicle_type + str(instance) sitl_args = [ '-I{}'.format(str(instance)), '--model', '+', '--home', ','.join(map(str, home)), '--rate', str(rate), '--speedup', str(speedup), '--defaults', defaults ] _LOG.debug( 'Trying to launch SITL instance {} on port {}...'.format( instance, 5760 + instance * 10)) sitl = dronekit_sitl.SITL(path=os.path.join( ardupath, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') port = str(int(port) + instance * 10) conn_string = ':'.join([tcp, ip, port]) _LOG.debug('SITL instance {} launched on: {}'.format( instance, conn_string)) vehicle = dronekit.connect(conn_string, baud=baud) vehicle.wait_ready(timeout=120) _LOG.info('Vehicle {} connected on {}'.format( vehicle_id, conn_string)) self._v_type = DRONE_TYPE_SITL_VRTL self._sitl = sitl else: _LOG.warn( 'vehicle type {} not supported!'.format(vehicle_type)) status = -1 if wait_till_armable: # Force wait until vehicle is armable. (ensure all pre-flight checks are completed) while not vehicle.is_armable: time.sleep(3.0) # Allow some extra time for things to initialize time.sleep(3.0) with self._drone_lock: self._vehicle = vehicle self._vid = vehicle_id self._register_message_handlers(vehicle) except dronekit.APIException: status = -1 self._connection_complete = True if status >= 0: _LOG.info('Vehicle {} successfully initialized.'.format(self._vid)) self._handshake_out_msgs.put_message( message.DroneHandshakeMessage.from_vehicle( self._vehicle, self._vid)) self._state_msg_timer = util.etc.RepeatedTimer( self._state_t, self.send_state_message) else: _LOG.error('Vehicle {} failed to initialize.'.format(self._vid))
def connect_vehicle(self): #threading.Thread(target=self.connect_vehicle_thread).start() #def connect_vehicle_thread(self): print("Entering connect thread =======================") drone = self.drone ip = self.drone['ip'] vehicle = None baud = 57600 status = 1 ardupath = drone["ardupath"] defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') home = drone['home'] if len(home) == 2: home = tuple(home) + (0, 0) else: home = tuple(home) try: if self.drone['vehicle_type'] == 'PHYS': vehicle = dronekit.connect(ip, wait_ready=True, baud=baud) elif self.drone['vehicle_type'] == 'VRTL': sitl_args = [ '--model', '+', '--home', ','.join(map(str, home)), '--rate', str(10), '--speedup', str(1.0), '--defaults', defaults ] sitl = dronekit_sitl.SITL(path=os.path.join( ardupath, 'build', 'sitl', 'bin', 'arducopter')) sitl.launch(sitl_args, await_ready=True) tcp, ip, port = sitl.connection_string().split(':') conn_string = ':'.join([tcp, ip, port]) vehicle = dronekit.connect(conn_string, baud=baud) vehicle.wait_ready(timeout=120) self.sitl = sitl else: print("Error: connect_vehicle") while not vehicle.is_armable: time.sleep(3.0) time.sleep(3.0) self.vehicle = vehicle except dronekit.APIException: status = -1 if status >= 0: message = {} message['type'] = "handshake" message['uavid'] = drone['vehicle_id'] message['sendtimestamp'] = long(round(time.time() * 1000)) message['data'] = {} message['data']['home'] = {} message['data']['home']['x'] = home[0] message['data']['home']['y'] = home[1] message['data']['home']['z'] = home[2] self.to_dronology.put_message(json.dumps(message)) else: print("Error: connect_vehicle1") print("exiting connect thread +++++++++++++++++++++++++++++")