Ejemplo n.º 1
0
	def connection(self,args):
		args=str(args)
		if args is 'sitl' or args is None:
			self._log("Starting copter simulator (SITL)")
			from dronekit_sitl import SITL
			sitl = SITL()
			self.sitl=sitl
			sitl.download('copter', '3.3', verbose=True)
			sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,0']
			sitl.launch(sitl_args, await_ready=True, restart=True)
			connection_string='tcp:127.0.0.1:5760'
		elif args=='0':
			connection_string='/dev/ttyACM0'
		elif args=='1': 
			connection_string='/dev/ttyACM1'
		else:
			connection_string=str(args)
		self._log('Connecting to vehicle on: %s' % connection_string)
		vehicle = connect(connection_string, wait_ready=True)

		# Register observers
		vehicle.add_attribute_listener('location',self.location_callback)
		#vehicle.add_attribute_listener('battery',self.battery_callback)
		#vehicle.add_attribute_listener('heading',self.heading_callback)
		return vehicle
Ejemplo n.º 2
0
def test_can_arm():
    sitl = SITL()
    sitl.download('copter', '3.3')
    sitl.launch(copter_args, verbose=True, await_ready=True)
    vehicle = dronekit.connect("tcp:127.0.0.1:5760", wait_ready=True)
    # Wait for vehicle to be armable
    timeout = time.time() + 10
    while not vehicle.is_armable and time.time() < timeout:
        time.sleep(0.35)
    timed_out = time.time() >= timeout
    if timed_out:
        sitl.stop()
    assert_false(timed_out, "Vehicle init timed out")

    # Arm it
    vehicle.mode = dronekit.VehicleMode("GUIDED")
    vehicle.armed = True

    # Confirm that it armed
    timeout = time.time() + 10
    while not vehicle.armed and time.time() < timeout:
        time.sleep(0.35)
    timed_out = time.time() >= timeout
    vehicle.close()
    sitl.stop()
    assert_false(timed_out, "Vehicle arming timed out")
Ejemplo n.º 3
0
def initializeDrone(connectionString, isSimulator):
    if isSimulator:
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = [
            '-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353'
        ]
        sitl.launch(sitl_args, await_ready=True, restart=True)
        print "Connecting to vehicle on: 'tcp:127.0.0.1:5760'"
        vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
        # print information about vehicle
        print "Global Location: %s" % vehicle.location.global_frame
        print "Global Location (relative altitude): %s" % vehicle.location.global_relative_frame
        print "Local Location: %s" % vehicle.location.local_frame  #NED
        print "Attitude: %s" % vehicle.attitude
        print "Velocity: %s" % vehicle.velocity
        print "GPS: %s" % vehicle.gps_0
        print "Groundspeed: %s" % vehicle.groundspeed
        print "Airspeed: %s" % vehicle.airspeed
        print "Battery: %s" % vehicle.battery
        print "EKF OK?: %s" % vehicle.ekf_ok
        print "Last Heartbeat: %s" % vehicle.last_heartbeat
        print "Rangefinder: %s" % vehicle.rangefinder
        print "Rangefinder distance: %s" % vehicle.rangefinder.distance
        print "Rangefinder voltage: %s" % vehicle.rangefinder.voltage
        print "Heading: %s" % vehicle.heading
        print "Is Armable?: %s" % vehicle.is_armable
        print "System status: %s" % vehicle.system_status.state
        print "Mode: %s" % vehicle.mode.name  # settable
        print "Armed: %s" % vehicle.armed
    else:
        # Connect to the Vehicle
        print 'Connecting to vehicle;'
        vehicle = connect(connectionString, wait_ready=True)
        # print information about vehicle
        print "Global Location: %s" % vehicle.location.global_frame
        print "Global Location (relative altitude): %s" % vehicle.location.global_relative_frame
        print "Local Location: %s" % vehicle.location.local_frame  #NED
        print "Attitude: %s" % vehicle.attitude
        print "Velocity: %s" % vehicle.velocity
        print "GPS: %s" % vehicle.gps_0
        print "Groundspeed: %s" % vehicle.groundspeed
        print "Airspeed: %s" % vehicle.airspeed
        print "Battery: %s" % vehicle.battery
        print "EKF OK?: %s" % vehicle.ekf_ok
        print "Last Heartbeat: %s" % vehicle.last_heartbeat
        print "Rangefinder: %s" % vehicle.rangefinder
        print "Rangefinder distance: %s" % vehicle.rangefinder.distance
        print "Rangefinder voltage: %s" % vehicle.rangefinder.voltage
        print "Heading: %s" % vehicle.heading
        print "Is Armable?: %s" % vehicle.is_armable
        print "System status: %s" % vehicle.system_status.state
        print "Mode: %s" % vehicle.mode.name  # settable
        print "Armed: %s" % vehicle.armed
Ejemplo n.º 4
0
    def __init__(self, config):
        self.default_webserver_ip = config.ip
        self.default_port = config.port
        self.default_webserver_port = config.webport
        self.id = config.id
        self.ip = config.ip
        self.home_location = LocationGlobalRelative(config.lat, config.lon,
                                                    config.alt)
        self.sitl = config.sitl
        # Follow instructions @ https://github.com/abearman/sparrow-dev/wiki/How-to-use-the-DroneKit-SITL-simulator
        self.instance = self.id - 1
        self.sitlInstance = SITL(instance=self.instance)
        self.sitlInstance.download('copter', '3.3', verbose=True)
        # system (e.g. "copter", "solo"), version (e.g. "3.3"), verbose

        if self.sitl:
            sitl_args = [
                '--simin=127.0.0.1:4000', '--simout=127.0.0.1:4001', '--model',
                'quad', '--home=' + str(self.home_location.lat) + "," +
                str(self.home_location.lon) + "," +
                str(self.home_location.alt) + ",360"
            ]
            print(str(sitl_args))
            self.sitlInstance.launch(sitl_args,
                                     verbose=False,
                                     await_ready=False,
                                     restart=True)
            self.connection_string = self.sitlInstance.connection_string()
        else:
            self.connection_string = '/dev/tty/ACM0'

        self.formation = None
        self.webserver = str(self.ip) + ":" + str(self.default_webserver_port)
        print('\nConnecting to vehicle on: %s\n' % self.connection_string)
        #print(site.USER_SITE)
        self.vehicle = connect(self.connection_string)
        print("Drone: " + str(self.id) + " connected!")
        self.drone_data = self.get_drone_data()
        # print(self.drone_data.__dict__.get(str(self.id)))
        # self.print_drone_data()
        self.mission = Mission(self.vehicle)
        # logger config
        self.logger = logging.getLogger("/logs/drone" + str(self.id) + "_log")
        self.logger.setLevel(logging.DEBUG)
        fh = logging.FileHandler("drone" + str(self.id) + "_log")
        fh.setLevel(logging.DEBUG)
        ch = logging.StreamHandler()
        ch.setLevel(logging.DEBUG)
        formatter = logging.Formatter(
            '%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        fh.setFormatter(formatter)
        self.logger.addHandler(fh)
        self.logger.addHandler(ch)
Ejemplo n.º 5
0
    def __init__(self, index):

        self.iNum = index
        self.args = sitl_args + ['-I' + str(index)]
        self.sitl = SITL()
        self.sitl.download('copter', '3.3')
        self.sitl.launch(self.args, await_ready=True, restart=True)

        self.connStr = tcp_master(index)

        self.setParamAndRelaunch('SYSID_THISMAV', index + 1)

        APMSim.existing.append(self)
Ejemplo n.º 6
0
 def __init__(self, sample_id, core_id, initial_profile, config):
     ardupilot_dir = config['root_dir'] + 'experiment/elf/'
     out_dir = config['root_dir'] + 'experiment/output/'
     self.elf_dir = "%s%d/" % (ardupilot_dir, 0)
     self.exp_out_dir = "%s%s/%d/" % (out_dir, 'PA', 0)
     self.ready = True
     self.states = []
     self.profiles = []
     self.sim_id = "%d_%d" % (sample_id, core_id)
     copter_file = self.elf_dir + "ArduCopter.elf"
     self.delta = 0.1
     self.sitl = SITL(path=copter_file)
     home_str = "%.6f,%.6f,%.2f,%d" % (
         initial_profile.lat, initial_profile.lon, initial_profile.alt,
         initial_profile.yaw)
     sitl_args = [
         '-S',
         '-I%d' % bug_id, '--home=' + home_str, '--model', '+',
         '--speedup=1', '--defaults=' + self.elf_dir + 'copter.parm'
     ]
     self.sitl.launch(sitl_args,
                      await_ready=True,
                      restart=True,
                      wd=self.elf_dir)
     port_number = 5760 + bug_id * 10
     self.missions = [
         Mission1(),
         Mission2(),
         Mission3(),
         Mission4(),
         Mission5()
     ]
     try:
         self.vehicle = connect('tcp:127.0.0.1:%d' % port_number,
                                wait_ready=True,
                                rate=10)
     except APIException:
         print("Cannot connect")
         self.sitl.stop()
         self.sitl.p.stdout.close()
         self.sitl.p.stderr.close()
         self.ready = False
         return
     for k, v in initial_profile.params.items():
         self.vehicle.parameters[k] = v
     while not self.vehicle.is_armable:
         print('initializing...')
         time.sleep(1)
     self.arm_vehicle()
     time.sleep(2)
     print(self.vehicle.version)
Ejemplo n.º 7
0
 def test_set_current(self):
     sitl = SITL()
     sitl.download('copter', '3.3', verbose=True)
     sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
     sitl.launch(sitl_args, await_ready=True, restart=True)
     print "Connecting to vehicle on: 'tcp:127.0.0.1:5760'"
     vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
     print " Battery: %s" % vehicle.battery
     battery_test = Battery(vehicle.battery)
     # check number negative the limit
     battery_test.set_current(NEGATIVE_NUMBER)
     self.assertEqual(battery_test.current, 0)
     # check number positive the limit
     battery_test.set_current(POSITIVE_NUMBER)
     self.assertEqual(battery_test.get_current(), POSITIVE_NUMBER)
     # check ZERO
     battery_test.set_current(0)
     self.assertEqual(battery_test.get_current(), 0)
     #check char that non number
     battery_test.set_current(CHARACTER)
     self.assertEqual(battery_test.get_current(), 0)
     #check char that  number
     battery_test.set_current(CHAR_NUMBER)
     self.assertEqual(battery_test.get_current(), int(CHAR_NUMBER))
     sitl.stop()
Ejemplo n.º 8
0
 def test_land(self):
     sitl = SITL()
     sitl.download('copter', '3.3', verbose=True)
     sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
     sitl.launch(sitl_args, await_ready=True, restart=True)
     drone = Drone('tcp:127.0.0.1:5760')
     headBefor =drone.vehicle.heading
     locBefor = drone.vehicle.location.global_relative_frame
     drone.take_off(25)
     print drone.vehicle.location.global_relative_frame
     drone.land()
     print drone.vehicle.location.global_relative_frame
     drone.take_off(4)
     print drone.vehicle.location.global_relative_frame
     drone.land()
     print drone.vehicle.location.global_relative_frame
     drone.take_off(2)
     print drone.vehicle.location.global_relative_frame
     drone.land()
     print drone.vehicle.location.global_relative_frame
     drone.take_off(10)
     print drone.vehicle.location.global_relative_frame
     drone.land()
     print drone.vehicle.location.global_relative_frame
     drone.vehicle.close()
     sitl.stop()
Ejemplo n.º 9
0
    def _start_SITL(self):
        from dronekit_sitl import SITL
        sitl = SITL()
        sitl.download(
            system="copter", version="3.3", verbose=False
        )  # ...or download system (e.g. "copter") and version (e.g. "3.3")
        sitl.launch(["--home=52.52.512593, 13.321893,0,90"],
                    verbose=False,
                    await_ready=False,
                    restart=False)
        sitl.block_until_ready(
            verbose=False)  # explicitly wait until receiving commands

        connection_string = sitl.connection_string()
        return connection_string
Ejemplo n.º 10
0
def start_sitl_rover():
    '''start a SITL session using sensible defaults.
    This should be the simplest way to start a sitl session'''
    print("Starting copter simulator (SITL)")
    sitl = SITL()
    sitl.download('rover', '2.50', verbose=True)

    sitl_args = [
        '-I0', '--model', 'rover', '--home=42.2278287,-8.72184010,584,353'
    ]
    sitl.launch(sitl_args, await_ready=False, restart=True, verbose=True)
    sitl.block_until_ready(verbose=True)
    return sitl
Ejemplo n.º 11
0
 def from_config(cls, config_path: str, sitl_path: str):
     """
     Create new Drone from a configuration file. Ignores connection_string to use SITL.
     """
     # pylint: disable=arguments-differ
     with open(config_path, "r") as file:
         configuration = json.load(file)
         sitl = SITL(path=sitl_path)
         sitl.launch([])
         vehicle_mode = configuration["vehicle_mode"]
         broker_ip = configuration["broker"]["ip"]
         broker_port = configuration["broker"]["port"]
         node_descriptor = configuration["node"]
         return cls(
             broker_ip,
             broker_port,
             node_descriptor,
             sitl,
             vehicle_mode,
         )
Ejemplo n.º 12
0
    def __init__(self, config):
        self.default_webserver_ip = config.ip
        self.default_port = config.port
        self.default_webserver_port = config.webport
        self.id = config.id
        self.ip = config.ip
        self.sitl = config.sitl
        self.sitlInstance = SITL('/home/dino/.dronekit/sitl/copter-3.3/apm')
        if self.sitl:
            sitl_args = [
                '--instance ' + str(self.id - 1), '-I0', '--model', 'quad',
                '--home=-35.363261,149.165230,584,353'
            ]
            self.sitlInstance.launch(sitl_args,
                                     verbose=False,
                                     await_ready=False,
                                     restart=True)
            self.connection_string = self.sitlInstance.connection_string()
        else:
            self.connection_string = '/dev/tty/ACM0'

        self.formation = None
        self.webserver = str(self.ip) + ":" + str(self.default_webserver_port)
        print('\nConnecting to vehicle on: %s\n' % self.connection_string)
        self.vehicle = connect(self.connection_string)
        print("Drone: " + str(self.id) + " connected!")
        self.drone_data = self.get_drone_data()
        self.print_drone_data()
        self.mission = Mission(self.vehicle)
        # logger config
        self.logger = logging.getLogger("drone" + (str)(self.id) + "_log")
        self.logger.setLevel(logging.DEBUG)
        fh = logging.FileHandler("drone" + (str)(self.id) + "_log")
        fh.setLevel(logging.DEBUG)
        ch = logging.StreamHandler()
        ch.setLevel(logging.DEBUG)
        formatter = logging.Formatter(
            '%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        fh.setFormatter(formatter)
        self.logger.addHandler(fh)
        self.logger.addHandler(ch)
Ejemplo n.º 13
0
    def connection(self, args):
        args = str(args)
        if args is 'sitl' or args is None:
            self._log("Starting copter simulator (SITL)")
            from dronekit_sitl import SITL
            sitl = SITL()
            self.sitl = sitl
            sitl.download('copter', '3.3', verbose=True)
            sitl_args = [
                '-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,0'
            ]
            sitl.launch(sitl_args, await_ready=True, restart=True)
            connection_string = 'tcp:127.0.0.1:5760'
        elif args == '0':
            connection_string = '/dev/ttyACM0'
        elif args == '1':
            connection_string = '/dev/ttyACM1'
        else:
            connection_string = str(args)
        self._log('Connecting to vehicle on: %s' % connection_string)
        vehicle = connect(connection_string, wait_ready=True)

        # Register observers
        vehicle.add_attribute_listener('location', self.location_callback)
        #vehicle.add_attribute_listener('battery',self.battery_callback)
        #vehicle.add_attribute_listener('heading',self.heading_callback)
        return vehicle
Ejemplo n.º 14
0
def connectMyCopter():

    parser = argparse.ArgumentParser(description='commands')
    parser.add_argument('--connect')
    args = parser.parse_args()

    connection_string = args.connect
    '''
	if not connection_string:
		import dronekit_sitl
		sitl = dronekit_sitl.start_default()
		connection_string = sitl.connection_string()

	vehicle = connect(connection_string,wait_ready=True, baud=57600)
	'''

    if not connection_string:
        import dronekit_sitl
        from dronekit_sitl import SITL

        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = [
            '-I0', '--model', 'quad',
            '--home=28.468772344967554,77.53801532669809,200,353'
        ]
        sitl.launch(sitl_args, verbose=True, await_ready=False, restart=True)

        #vehicle = connect(connection_string,wait_ready=True, baud=57600)
    vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
    return vehicle
Ejemplo n.º 15
0
    def __init__(self,
                 iNum,
                 extraArgs,
                 rate,
                 speedup,
                 vType="copter",
                 version="3.3"):
        # type: (int, list[str], int, int, str, str) -> None
        """
        :param iNum: instance number, affect SITL system ID
        :param home: (lat,lng,alt,yaw)
        """
        self.iNum = iNum
        self.rate = rate
        self.args = extraArgs + ['-I' + str(iNum)] + [
            '--speedup', str(speedup)
        ] + ['-r', str(rate)]
        self.vType = vType
        self.version = version

        sitl = SITL()
        self.sitl = sitl

        @retry(5)
        def download():
            sitl.download(vType, version)

        download()

        @retry(2)
        def launch():
            try:
                sitl.launch(self.args, await_ready=True, restart=True)
                print(
                    "launching APM SITL:",
                    *(self.args + ["URI=" + self.connStr] +
                      ["PID=" + str(sitl.p.pid)]))
                self.setParamAndRelaunch('SYSID_THISMAV', self.iNum + 1)

                @self.withVehicle()
                def set(vehicle):
                    vehicle.parameters['FS_GCS_ENABLE'] = 0
                    vehicle.parameters['FS_EKF_THRESH'] = 100

                set()

            except:
                self.close()
                raise

        launch()
        print("APM SITL on", self.connStr, "is up and running")
Ejemplo n.º 16
0
	def __init__(self , connectionString,mode):

         
            if mode =='r': #run on vihcle
                print 'Connecting to vehicle on: %s' % connectionString
                self.vehicle = connect(connectionString, baud=57600)
        
            if mode =='s': #run on simultor
                self.sitl = SITL()
                self.sitl.download('copter', '3.3', verbose=True)
                self.sitl_args = ['-I0','--gimbal' ,'--model', 'quad', '--home=31.7396,35.1956,3,-1']
                self.sitl.launch(self.sitl_args, await_ready=True, restart=True)
                self.vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
Ejemplo n.º 17
0
 def test_take_off(self):
     sitl = SITL()
     sitl.download('copter', '3.3', verbose=True)
     sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
     sitl.launch(sitl_args, await_ready=True, restart=True)
     drone = Drone('tcp:127.0.0.1:5760')
     headBefor =drone.vehicle.heading
     locBefor = drone.vehicle.location.global_relative_frame
     drone.take_off(3)
     headAfter = drone.vehicle.heading
     locAfter = drone.vehicle.location.global_relative_frame
     self.assertTrue( round(locAfter.alt-locBefor.alt)==3 and ((headBefor+5)%360<headAfter or(headBefor-5)%360 >headAfter))
     drone.vehicle.close()
     sitl.stop()
Ejemplo n.º 18
0
def connect_virtual_vehicle(instance, home):
    sitlx = SITL()
    sitlx.download('copter', '3.3', verbose=True)
    instance_arg = '-I%s' %(str(instance))
    print("Drone instance is: %s" % instance_arg)
    home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2]))
    sitl_args = [instance_arg, '--model', 'quad', home_arg]
    sitlx.launch(sitl_args, await_ready=True)
    tcp, ip, port = sitlx.connection_string().split(':')
    port = str(int(port) + instance * 10)
    conn_string = ':'.join([tcp, ip, port])
    print('Connecting to vehicle on: %s' % conn_string)

    vehicle = connect(conn_string)
    vehicle.wait_ready(timeout=120)
    print("Reached here")
    return vehicle, sitlx
Ejemplo n.º 19
0
def sitlUp(instance=0):
    global sitls
    args = sitl_args + ['-I' + str(instance)]
    sitl = SITL()
    sitl.download('copter', '3.3')

    sitl.launch(args, await_ready=True, restart=True)

    key = 'SYSID_THISMAV'
    value = instance + 1

    setParamAndRelaunch(sitl, args, tcp_master(instance), key, value)

    sitls.append(sitl)
Ejemplo n.º 20
0
def start_default_sitl(lat=None, lon=None):
    print "Starting copter simulator (SITL)"
    from dronekit_sitl import SITL
    sitl = SITL()
    sitl.download('copter', '3.3', verbose=True)
    if ((lat is not None and lon is None) or
        (lat is None and lon is not None)):
        print("Supply both lat and lon, or neither")
        exit(1)
    sitl_args = ['-I0', '--model', 'quad', ]
    if lat is not None:
        sitl_args.append('--home=%f,%f,584,353' % (lat,lon,))
    sitl.launch(sitl_args, await_ready=True, restart=True)
    connection_string='tcp:127.0.0.1:5760'
    return (sitl, connection_string)
Ejemplo n.º 21
0
from dronekit_sitl import SITL
import dronekit
import time
from dronekit_sitl import version_list
from dronekit_sitl import download
import sys

copter_args = ['-S', '--model', 'quad', '--home=-35.363261,149.165230,584,353']

sitl = SITL()
sitl.launch(copter_args, await_ready=True, use_saved_data=True)
vehicle = dronekit.connect(connection_string, wait_ready=True)
print vehicle.parameters["COMPASS_USE"]
vehicle.close()
sitl.stop()

sys.exit(0)

sitl = SITL()
sitl.stop()
sitl.download('copter', '3.3')
sitl.launch(copter_args, verbose=True, await_ready=True, use_saved_data=False)
sitl.stop()

sys.exit(0)

versions = version_list()
models = list(versions.keys())
print models.sort()

download('copter', '3.3', None)
Ejemplo n.º 22
0
'''Runs simulator using python'''

import os
from time import sleep
from dronekit_sitl import SITL
SIM = {}
BIN = os.getenv("SITL_BIN")
DO_DOWNLOAD = True
if BIN is not None:
    DO_DOWNLOAD = False
    SIM["path"] = BIN
    DEFAULTS = os.getenv("SITL_DEFAULTS_FILEPATH")
    if DEFAULTS is not None:
        SIM["defaults_filepath"] = DEFAULTS
SIM = SITL(**SIM)
if DO_DOWNLOAD:
    SIM.download('copter', '3.3', verbose=False)

SIM.launch([
    '--model',
    'quad',
], await_ready=True, restart=False)
print("Simulator lanched")

while True:
    sleep(5)
Ejemplo n.º 23
0
def setup_solo_sitl():
    global sitl
    sitl = SITL()
    sitl.download('solo', '1.2.0')
    sitl.launch(sitl_args, await_ready=True, restart=True)
Ejemplo n.º 24
0
vehicle_setup[1] = {
    "lat": vehicle_setup[0]["lat"] + 0.00064,
    "lon": vehicle_setup[0]["lon"] + 0.00032,
    #    "target-lat": -35.363261 + 1.25*0.00120,
    "target-lat": -35.363261 + -1.25 * 0.00120,
    #    "target-lat": -35.363261 + 0.00032,
    "target-lon": 149.165230 - 0.5 * 0.00128,
}

# ready data for many dronekit-sitl processes
simulation_count = 2
for i in range(0, simulation_count):
    print("Creating simulator (SITL) %d" % (i, ))
    from dronekit_sitl import SITL
    sitl = SITL(instance=i,
                path=args.binary,
                defaults_filepath=args.defaults,
                gdb=True)
    #    sitl.download('copter', '3.3', verbose=True)
    lat = -35.363261 + i * 0.00128
    lat = vehicle_setup[i]["lat"]
    lon = vehicle_setup[i]["lon"]
    sitl_args = ['--model', 'quad',
                 '--home=%s,%s,584,353' % (
                     lat,
                     lon,
                 )]
    sitls.append([sitl, sitl_args])
    hub_connection_strings.append(sitl.connection_string())


# start the SITLs one at a time, giving each a unique SYSID_THISMAV
Ejemplo n.º 25
0
def test_sitl():
    sitl = SITL()
    sitl.download('copter', '3.3')
    sitl.launch(copter_args)
    sitl.block_until_ready()
    
    assert_equals(sitl.poll(), None, 'SITL should still be running.')
    assert_equals(sitl.using_sim, False, 'SITL for copter-3.3 should not be using pysim')

    sitl.stop()
    assert sitl.poll() != None, 'SITL should stop running after kill.'

    # Test "relaunch"
    sitl.launch(copter_args)
    try:
    	sitl.launch(copter_args)
    	assert False, 'SITL should fail to launch() again when running'
    except:
    	pass
    try:
    	sitl.launch(copter_args, restart=True)
    except:
    	assert False, 'SITL should succeed in launch() when restart=True'

    sitl.stop()
Ejemplo n.º 26
0
import os
from nose.tools import assert_equals, with_setup
from dronekit_sitl import SITL

sitl = SITL('copter', '3.3-rc5')
sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']

if 'SITL_SPEEDUP' in os.environ:
    sitl_args += ['--speedup', str(os.environ['SITL_SPEEDUP'])]
if 'SITL_RATE' in os.environ:
    sitl_args += ['-r', str(os.environ['SITL_RATE'])]


def setup_sitl():
    sitl.launch(sitl_args, await_ready=True, restart=True)


def teardown_sitl():
    sitl.stop()


def with_sitl(fn):
    @with_setup(setup_sitl, teardown_sitl)
    def test(*args, **kargs):
        return fn('tcp:127.0.0.1:5760', *args, **kargs)

    return test
Ejemplo n.º 27
0
class Drone:
    def __init__(self, config):
        self.default_webserver_ip = config.ip
        self.default_port = config.port
        self.default_webserver_port = config.webport
        self.id = config.id
        self.ip = config.ip
        self.home_location = LocationGlobalRelative(config.lat, config.lon,
                                                    config.alt)
        self.sitl = config.sitl
        # Follow instructions @ https://github.com/abearman/sparrow-dev/wiki/How-to-use-the-DroneKit-SITL-simulator
        self.instance = self.id - 1
        self.sitlInstance = SITL(instance=self.instance)
        self.sitlInstance.download('copter', '3.3', verbose=True)
        # system (e.g. "copter", "solo"), version (e.g. "3.3"), verbose

        if self.sitl:
            sitl_args = [
                '--simin=127.0.0.1:4000', '--simout=127.0.0.1:4001', '--model',
                'quad', '--home=' + str(self.home_location.lat) + "," +
                str(self.home_location.lon) + "," +
                str(self.home_location.alt) + ",360"
            ]
            print(str(sitl_args))
            self.sitlInstance.launch(sitl_args,
                                     verbose=False,
                                     await_ready=False,
                                     restart=True)
            self.connection_string = self.sitlInstance.connection_string()
        else:
            self.connection_string = '/dev/tty/ACM0'

        self.formation = None
        self.webserver = str(self.ip) + ":" + str(self.default_webserver_port)
        print('\nConnecting to vehicle on: %s\n' % self.connection_string)
        #print(site.USER_SITE)
        self.vehicle = connect(self.connection_string)
        print("Drone: " + str(self.id) + " connected!")
        self.drone_data = self.get_drone_data()
        # print(self.drone_data.__dict__.get(str(self.id)))
        # self.print_drone_data()
        self.mission = Mission(self.vehicle)
        # logger config
        self.logger = logging.getLogger("/logs/drone" + str(self.id) + "_log")
        self.logger.setLevel(logging.DEBUG)
        fh = logging.FileHandler("drone" + str(self.id) + "_log")
        fh.setLevel(logging.DEBUG)
        ch = logging.StreamHandler()
        ch.setLevel(logging.DEBUG)
        formatter = logging.Formatter(
            '%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        fh.setFormatter(formatter)
        self.logger.addHandler(fh)
        self.logger.addHandler(ch)

        # Always add the drone to swarm last.

    # =============================ATTRIBUTE LISTENER CALLBACKS==============================================
    # =======================================================================================================
    def location_callback(self):
        self.update_self_to_swarm("/Swarm")
        self.logger.info("Drone Location Changed: " +
                         str(self.vehicle.location.global_relative_frame))
        if self.vehicle.location.global_relative_frame.alt < 2 and self.vehicle.mode.name == "GUIDED":  # if the vehicle is in guided mode and alt is less than 2m slow it the f down
            self.vehicle.airspeed = .2

    def armed_callback(self):
        self.logger.info("Drone Armed Status Changed: " +
                         str(self.vehicle.armed))
        self.update_self_to_swarm("/Swarm")

    def mode_callback(self):
        self.logger.info("Mode Changed: " + str(self.vehicle.mode.name))
        self.update_self_to_swarm("/Swarm")

    # =============================COMMUNICATION TO SERVER===================================================
    # =======================================================================================================

    def update_self_to_swarm(self, route):
        url = 'http://' + self.webserver + route + "?id=" + str(
            self.id)  # + "/" + str(self.id) + "?id=" + str(self.id)
        data = self.get_drone_data().__dict__[(str(self.id))]
        try:
            # r = requests.post(url,data)
            r = requests.put(url, json=data)
            self.logger.info("\nServer Responded With: " + str(r.status_code) +
                             " " + str(r.text) + "\n")
            return r.status_code
        except requests.HTTPError:
            self.logger.info(str(requests.HTTPError))
            return r.status_code

    def get_data_from_server(self, route):
        url = 'http://' + self.webserver + route  # + "/" + str(self.id)  # + "/" + str(self.id)
        try:
            r = requests.get(url)
            json_data = json.loads(r.text)
            parsed_data = Config(json_data)
            self.logger.info("\nServer Responded With: " + str(r.status_code) +
                             " " + str(r.text) + "\n")
            return parsed_data
        except requests.HTTPError:
            self.logger.info("HTTP " + str(requests.HTTPError))

    # =============================VEHICLE INFORMATION FUNCTIONS=================================================
    # =======================================================================================================
    def get_drone_data(self):
        # creates a dictionary object out of the drone data
        return type(
            'obj', (object, ), {
                str(self.id): {
                    "id": self.id,
                    "ip": self.ip,
                    "airspeed": self.vehicle.airspeed,
                    "latitude": self.vehicle.location.global_frame.lat,
                    "longitude": self.vehicle.location.global_frame.lon,
                    "altitude":
                    self.vehicle.location.global_relative_frame.alt,
                    "armable": self.vehicle.is_armable,
                    "armed": self.vehicle.armed,
                    "mode": self.vehicle.mode.name
                }
            })

    def print_drone_data(self):
        drone_params = self.get_drone_data().__dict__.get(str(self.id))
        string_to_print = "Drone ID: " + str(
            drone_params.get("id")) + "\nDrone IP: " + str(
                drone_params.get("ip")) + "\nDrone A/S: " + str(
                    drone_params.get(
                        "airspeed")) + "\nDrone Location: (" + str(
                            drone_params.get("latitude")) + ", " + str(
                                drone_params.get("longitude")) + ", " + str(
                                    drone_params.get("altitude")
                                ) + ")" + "\nDrone Armed: " + str(
                                    drone_params.get("armed")
                                ) + "\nDrone Mode: " + drone_params.get("mode")
        print(string_to_print)

    # =============================VEHICLE CONTROL FUNCTIONS=================================================
    # =======================================================================================================
    def set_parameter(self, paramName,
                      value):  # Sets a specified param to specified value
        self.vehicle.parameters[paramName] = value

    def set_airspeed(self, value):
        self.vehicle.airspeed = value

    def set_mode(self, mode):
        self.vehicle.mode = VehicleMode(mode)

    def set_formation(self, formationName):
        self.formation = formationName

    def move_to_formation(self, aTargetAltitude):
        drone_params = self.get_drone_data()
        droneLat = float(
            drone_params)  # would be better to just get the location object...
        droneLon = float(drone_params['longitude'])
        droneAlt = float(drone_params['altitude'])

        # Check altitude is 10 metres so we can manuver around eachother

        if aTargetAltitude is 10:

            if (self.formation == "triangle"):

                if (self.id == "1"):
                    # Master, so take point
                    self.vehicle.simple_goto(
                        self.vehicle.location.global_frame.lat,
                        self.vehicle.location.global_frame.lon,
                        aTargetAltitude)

                elif (self.id == "2"):
                    # Slave 1, so take back-left
                    # print("Drone 2 Moving To Position")
                    self.vehicle.simple_goto(droneLat - .0000018,
                                             droneLon - .0000018,
                                             aTargetAltitude - 3)
                    # print("Master loc:",droneLat,",",droneLon,",",droneAlt)
                    self.logger.info(
                        "My Loc:" +
                        str(self.vehicle.location.global_relative_frame.lat) +
                        "," +
                        str(self.vehicle.location.global_relative_frame.lon) +
                        "," +
                        str(self.vehicle.location.global_relative_frame.alt))

                elif (self.id == "3"):
                    # Slave 2, so take back-right
                    # print("Drone 3 Moving To Position")
                    self.vehicle.simple_goto(droneLat - .0000018,
                                             droneLon + .0000018,
                                             aTargetAltitude + 3)

                    # print("Master loc:",droneLat,",",droneLon,",",droneAlt)
                    self.logger.info(
                        "My Loc:" +
                        str(self.vehicle.location.global_relative_frame.lat) +
                        "," +
                        str(self.vehicle.location.global_relative_frame.lon) +
                        "," +
                        str(self.vehicle.location.global_relative_frame.alt))

                else:
                    self.logger.info("Cannot Position This Drone In Formation")
            # Add more else if for more formation types
            else:
                self.logger.info("No such formation: " + self.formation)

        else:
            print("Invalid formation altitude!")
            print(
                "Please change formation altitude to 10 metres so the drones can manuver around eachother safetly!"
            )

    def follow_in_formation(self, droneID):
        self.move_to_formation(10)

    def arm(self):
        self.enable_gps()

        self.logger.info("Basic pre-arm checks")

        while not self.vehicle.is_armable:
            self.logger.info(" Waiting for vehicle to initialize...")
            time.sleep(1)
        self.vehicle.mode = VehicleMode("GUIDED")

        self.logger.info("Arming motors")
        # Copter should arm in GUIDED mode
        self.vehicle.armed = True

        # Confirm vehicle armed before attempting to take off
        while not self.vehicle.armed:
            self.logger.info("Waiting for arming...")
            self.vehicle.armed = True
            time.sleep(1)

        self.vehicle.add_attribute_listener('location.global_relative_frame',
                                            self.location_callback)
        self.vehicle.add_attribute_listener('armed', self.armed_callback)
        self.vehicle.add_attribute_listener('mode', self.mode_callback)

        self.logger.info("Vehicle Armed!")

    def disable_gps(self):  # http://ardupilot.org/copter/docs/parameters.html
        if not self.sitl:  # don't try updating params in sitl cuz it doesn't work. problem on their end
            self.vehicle.parameters["ARMING_CHECK"] = 0
            self.vehicle.parameters["GPS_TYPE"] = 3
            self.vehicle.parameters["GPS_AUTO_CONFIG"] = 0
            self.vehicle.parameters["GPS_AUTO_SWITCH"] = 0
            self.vehicle.parameters["FENCE_ENABLE"] = 0

    def enable_gps(self):  # http://ardupilot.org/copter/docs/parameters.html
        if not self.sitl:
            self.vehicle.parameters["ARMING_CHECK"] = 1
            self.vehicle.parameters["GPS_TYPE"] = 1
            self.vehicle.parameters["GPS_AUTO_CONFIG"] = 1
            self.vehicle.parameters["GPS_AUTO_SWITCH"] = 1
            self.vehicle.parameters["FENCE_ENABLE"] = 0

    def arm_no_GPS(self):

        self.logger.info("Arming motors NO GPS")
        self.vehicle.mode = VehicleMode("SPORT")

        while not self.vehicle.armed:
            self.logger.info(" Waiting for arming...")
            self.disable_gps()
            time.sleep(3)
            self.vehicle.armed = True

        self.update_self_to_swarm("/swarm")

    def shutdown(self):
        self.vehicle.remove_attribute_listener(
            'location.global_relative_frame', self.location_callback)
        self.vehicle.remove_attribute_listener('armed', self.armed_callback)
        self.vehicle.remove_attribute_listener('mode', self.mode_callback)
        self.vehicle.close()

    # =================================MISSION FUNCTIONS=====================================================
    # =======================================================================================================

    def wait_for_drone_match_altitude(self, droneID):
        self.wait_for_drone_reach_altitude(
            droneID, self.vehicle.location.global_relative_frame.alt)

    def wait_for_swarm_to_match_altitude(self):
        swarm_params = self.get_data_from_server("/Swarm")

        for idx in enumerate(swarm_params.Drones[0]):
            self.wait_for_drone_match_altitude(idx)

    def wait_for_drone_reach_altitude(self, droneID, altitude):
        swarm_params = self.get_data_from_server("/Swarm")

        for idx, drone in enumerate(swarm_params.Drones[0]):
            if swarm_params.Drones[idx][1].get("id") == droneID:
                while (swarm_params.Drones[idx][1].altitude <=
                       altitude * 0.95):
                    swarm_params = self.get_data_from_server("/Swarm")
                    print("Waiting for Drone: " +
                          str(swarm_params.Drones[idx][1].get("id")) +
                          " to reach " + str(altitude))
                    time.sleep(1)
                self.logger.info("Drone: " +
                                 swarm_params.Drones[idx][1].get("id") +
                                 " reached " + str(altitude) + "...")

    def wait_for_swarm_ready(self):
        # drone_data.Drones[drone_id][1 (indexing bug)].get("ip")
        swarm_ready_status = []

        while not assert_true(swarm_ready_status):
            swarm_params = self.get_swarm_data("/Swarm")
            swarm_size = swarm_params.Drones.__len__()
            print("Found " + (str)(swarm_size) + "Drones in the Swarm.")

            for idx, drone in enumerate(swarm_params.Drones):
                if not swarm_params:
                    print("No Drones Found in the Swarm.")
                else:
                    if not swarm_params.Drones[idx][1]:
                        print("No Drones Found in the Swarm.")
                    else:
                        print("Drone: " +
                              (str)(swarm_params.Drones[idx][1].get("id") +
                                    " found in swarm"))
                        swarm_ready_status.append(1)
                        time.sleep(1)
            assert_true(swarm_ready_status)
        # if swarm_is_not_ready and all drones have been checked, do loop again
        print("Swarm is ready!")

    def goto_formation(self, formation, formationAltitude, bool):
        # Formation on X,Y axes

        swarm_data = self.get_data_from_server("/Swarm")
        # Form up on first drone in swarm for now
        head_drone_data = swarm_data.Drones[0][1]
        head_drone_loc = LocationGlobalRelative(
            head_drone_data.get("latitude"), head_drone_data.get("longitude"),
            head_drone_data.get("altitude"))
        """
        Transition into formation so they don't crash
        Safe altitude is different for some drones so they don't maneuver into position at the same altitude

        3 Steps:
            1. Move to a safe altitude to manuver in
            2. Move to the position inside the formation it should be in
            3. Move to the correct altitude inside the formation

        Formation occurs on drone object (ID:1).
        ID:1 should only change its altitude.

        """

        if formation == "triangle":
            for idx, drone in enumerate(swarm_data.Drones):
                if self.id == 1:
                    waypoint = LocationGlobalRelative(head_drone_loc.lat,
                                                      head_drone_loc.lon,
                                                      formationAltitude)
                    self.vehicle.simple_goto(waypoint)

                elif self.id == 2:

                    # Maneuver 5 metres below formation altitude
                    if bool:
                        safeAltitude = formationAltitude
                    elif not bool:
                        safeAltitude = formationAltitude - 5
                    self.vehicle.simple_goto(drone.location.global_frame.lat,
                                             drone.location.global_frame.lon,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat - .0000027,
                                             head_drone_loc.lon - .0000027,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat - .0000027,
                                             head_drone_loc.lon - .0000027,
                                             formationAltitude)

                elif self.id == 3:

                    # Maneuver 5 metres above formation altitude
                    if bool:
                        safeAltitude = formationAltitude
                    elif not bool:
                        safeAltitude = formationAltitude + 5
                    self.vehicle.simple_goto(drone.location.global_frame.lat,
                                             drone.location.global_frame.lon,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat + .0000027,
                                             head_drone_loc.lon - .0000027,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat + .0000027,
                                             head_drone_loc.lon - .0000027,
                                             formationAltitude)

        elif formation == "stacked":
            # Special formation altitude represents the separation on the Z axis between the drones
            special_formation_altitude = 3

            for idx, drone in enumerate(swarm_data.Drones):
                if self.id == 1:

                    # Maneuver to formation altitude
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon,
                                             formationAltitude)

                elif self.id == 2:

                    # Maneuver 5 metres below formation altitude
                    if bool:
                        safeAltitude = formationAltitude
                    elif not bool:
                        safeAltitude = formationAltitude - 5
                    self.vehicle.simple_goto(drone.location.global_frame.lat,
                                             drone.location.global_frame.lon,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon, safeAltitude)
                    self.vehicle.simple_goto(
                        head_drone_loc.lat, head_drone_loc.lon,
                        formationAltitude - special_formation_altitude)

                elif self.id == 3:

                    # Maneuver 5 metres above formation altitude
                    if bool:
                        safeAltitude = formationAltitude
                    elif not bool:
                        safeAltitude = formationAltitude + 5
                    self.vehicle.simple_goto(drone.location.global_frame.lat,
                                             drone.location.global_frame.lon,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon, safeAltitude)
                    self.vehicle.simple_goto(
                        head_drone_loc.lat, head_drone_loc.lon,
                        formationAltitude + special_formation_altitude)

        elif formation == "xaxis":
            for idx, drone in enumerate(swarm_data.Drones):
                if self.id == 1:

                    # Maneuver to formation altitude
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon,
                                             formationAltitude)

                elif self.id == 2:

                    # Maneuver 5 metres below formation altitude
                    if bool:
                        safeAltitude = formationAltitude
                    elif not bool:
                        safeAltitude = formationAltitude - 5
                    self.vehicle.simple_goto(drone.location.global_frame.lat,
                                             drone.location.global_frame.lon,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat - .0000027,
                                             head_drone_loc.lon, safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat - .0000027,
                                             head_drone_loc.lon,
                                             formationAltitude)

                elif self.id == 3:

                    # Maneuver 5 metres above formation altitude
                    if bool:
                        safeAltitude = formationAltitude
                    elif not bool:
                        safeAltitude = formationAltitude + 5
                    self.vehicle.simple_goto(drone.location.global_frame.lat,
                                             drone.location.global_frame.lon,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat + .0000027,
                                             head_drone_loc.lon, safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat + .0000027,
                                             head_drone_loc.lon,
                                             formationAltitude)

        elif formation == "yaxis":
            for idx, drone in enumerate(swarm_data.Drones):
                if self.id == 1:

                    # Maneuver to formation altitude
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon,
                                             formationAltitude)

                elif self.id == 2:

                    # Maneuver 5 metres below formation altitude
                    if bool:
                        safeAltitude = formationAltitude
                    elif not bool:
                        safeAltitude = formationAltitude - 5
                    self.vehicle.simple_goto(drone.location.global_frame.lat,
                                             drone.location.global_frame.lon,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon - .0000027,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon - .0000027,
                                             formationAltitude)

                elif self.id == 3:

                    # Maneuver 5 metres above formation altitude
                    if bool:
                        safeAltitude = formationAltitude
                    elif not bool:
                        safeAltitude = formationAltitude + 5
                    self.vehicle.simple_goto(drone.location.global_frame.lat,
                                             drone.location.global_frame.lon,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon + .0000027,
                                             safeAltitude)
                    self.vehicle.simple_goto(head_drone_loc.lat,
                                             head_drone_loc.lon + .0000027,
                                             formationAltitude)

    def wait_for_next_formation(self, seconds):
        for idx in range(0, seconds):
            self.logger.info("Waiting " + str(seconds) +
                             " seconds before next flight formation... " +
                             str(idx + 1) + "/" + str(seconds))
            time.sleep(1)

    def wait_for_formation(self, seconds):
        for idx in range(0, seconds):
            self.logger.info("Waiting for drones to form up... " +
                             str(idx + 1) + "/" + str(seconds))
            time.sleep(1)

    """def arm_and_takeoff(self, aTargetAltitude):
        self.arm()

        self.logger.info("Taking off!")
        self.vehicle.simple_takeoff(aTargetAltitude)  # Take off to target altitude

        while True:
            self.logger.info("Vehicle Altitude: " + str(self.vehicle.location.global_relative_frame.alt))
            if self.vehicle.location.global_relative_frame.alt >= aTargetAltitude * .95:
                self.logger.info("Reached target altitude")
                self.update_self_to_swarm("/Swarm")
                break
            time.sleep(.75)
    """

    def arm_and_takeoff(self, aTargetAltitude):

        #Arms vehicle and fly to aTargetAltitude.

        self.logger.info("Basic pre-arm checks")
        # Don't try to arm until autopilot is ready
        while not self.vehicle.is_armable:
            self.logger.info(" Waiting for vehicle to initialize...")
            # self.vehicle.gps_0.fix_type.__add__(2)
            # self.vehicle.gps_0.__setattr__(self.vehicle.gps_0.fix_type, 3)
            self.logger.info(self.vehicle.gps_0.fix_type)
            self.logger.info(self.vehicle.gps_0.satellites_visible)
            time.sleep(2)

            self.logger.info("Arming motors")

        self.vehicle.add_attribute_listener('location.global_relative_frame',
                                            self.location_callback)
        self.vehicle.add_attribute_listener('armed', self.armed_callback)
        self.vehicle.add_attribute_listener('mode', self.mode_callback)

        # Copter should arm in GUIDED mode
        self.vehicle.mode = VehicleMode("GUIDED")
        self.vehicle.armed = True

        # Confirm vehicle armed before attempting to take off
        while not self.vehicle.armed:
            self.logger.info(" Waiting for arming...")
            time.sleep(1)

            self.logger.info("Taking off!")
        self.vehicle.simple_takeoff(
            aTargetAltitude)  # Take off to target altitude

        while self.vehicle.location.global_relative_frame.alt < aTargetAltitude * 0.95:
            self.logger.info(
                " Currently flying... Alt: " +
                str(self.vehicle.location.global_relative_frame.alt))
            time.sleep(1)

        if self.vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
            self.logger.info("Reached target altitude")

    def land_vehicle(self):
        self.logger.info("Returning to Launch!!!")

        if self.sitl:
            self.vehicle.airspeed = 3
        else:
            self.vehicle.parameters["RTL_ALT"] = 0.0
            self.vehicle.airspeed = 1

        self.set_mode("RTL")
        while self.vehicle.mode.name != "RTL":
            self.logger.info("Vehicle Mode Didn't Change")
            self.set_mode("RTL")
            time.sleep(1)
        # http://ardupilot.org/copter/docs/parameters.html#rtl-alt-rtl-altitude
        while self.vehicle.location.global_relative_frame.alt > 0.2:
            self.logger.info(
                "Altitude: " +
                str(self.vehicle.location.global_relative_frame.alt))
            time.sleep(1)
        self.logger.info("Landed!")

    def over_fix(self, lat, lon):
        #Negate to make sense in english
        #Should be True,False,False
        loc = LocationGlobal(lat, lon)
        if (self.vehicle.location.global_frame.lat - loc.lat) < 0.000005:
            if (self.vehicle.location.global_frame.lon - loc.lon) < 0.000005:
                return False
            return True
        return True
from dronekit_sitl import SITL
import dronekit
import time
from dronekit_sitl import version_list
from dronekit_sitl import download
import sys

copter_args = ['-S', '--model', 'quad', '--home=-35.363261,149.165230,584,353']

sitl = SITL()
sitl.launch(copter_args, await_ready=True, use_saved_data=True)
vehicle = dronekit.connect(connection_string, wait_ready=True)
print vehicle.parameters["COMPASS_USE"]
vehicle.close()
sitl.stop()

sys.exit(0)

sitl = SITL()
sitl.stop()
sitl.download('copter', '3.3')
sitl.launch(copter_args, verbose=True, await_ready=True, use_saved_data=False)
sitl.stop()

sys.exit(0)

versions = version_list()
models = list(versions.keys())
print models.sort()

download('copter','3.3', None)
Ejemplo n.º 29
0
	def connect2FCU(self):
		connection_string = self.args.connect
		sitl=None
		if not self.args.connect:
	    		print "The connect string was not specified. Running SITL!"
			from dronekit_sitl import SITL
			sitl = SITL()
			sitl.download('copter', '3.3', verbose=True)
			sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
			sitl.launch(sitl_args, await_ready=True, restart=True)
			connection_string = 'tcp:127.0.0.1:5760'

		# Connect to the Vehicle. 
		#   Set `wait_ready=True` to ensure default attributes are populated before `connect()` returns.
		print "\nConnecting to vehicle on: %s" % connection_string
		vehicle = connect(connection_string, wait_ready=True)
		vehicle.wait_ready('autopilot_version')
		# Get all vehicle attributes (state)

		print "======================================================="
		print " Autopilot Firmware version: %s" % vehicle.version
		print "   Major version number: %s" % vehicle.version.major
		print "   Minor version number: %s" % vehicle.version.minor
		print "   Patch version number: %s" % vehicle.version.patch
		print "   Release type: %s" % vehicle.version.release_type()
		print "   Release version: %s" % vehicle.version.release_version()
		print "   Stable release?: %s" % vehicle.version.is_stable()
		'''
		print " Autopilot capabilities"
		print "   Supports MISSION_FLOAT message type: %s" % vehicle.capabilities.mission_float
		print "   Supports PARAM_FLOAT message type: %s" % vehicle.capabilities.param_float
		print "   Supports MISSION_INT message type: %s" % vehicle.capabilities.mission_int
		print "   Supports COMMAND_INT message type: %s" % vehicle.capabilities.command_int
		print "   Supports PARAM_UNION message type: %s" % vehicle.capabilities.param_union
		print "   Supports ftp for file transfers: %s" % vehicle.capabilities.ftp
		print "   Supports commanding attitude offboard: %s" % vehicle.capabilities.set_attitude_target
		print "   Supports commanding position and velocity targets in local NED frame: %s" % vehicle.capabilities.set_attitude_target_local_ned
		print "   Supports set position + velocity targets in global scaled integers: %s" % vehicle.capabilities.set_altitude_target_global_int
		print "   Supports terrain protocol / data handling: %s" % vehicle.capabilities.terrain
		print "   Supports direct actuator control: %s" % vehicle.capabilities.set_actuator_target
		print "   Supports the flight termination command: %s" % vehicle.capabilities.flight_termination
		print "   Supports mission_float message type: %s" % vehicle.capabilities.mission_float
		print "   Supports onboard compass calibration: %s" % vehicle.capabilities.compass_calibration
		print " Global Location: %s" % vehicle.location.global_frame
		print " Global Location (relative altitude): %s" % vehicle.location.global_relative_frame
		print " Local Location: %s" % vehicle.location.local_frame
		print " Attitude: %s" % vehicle.attitude
		print " Velocity: %s" % vehicle.velocity
		print " GPS: %s" % vehicle.gps_0
		print " Gimbal status: %s" % vehicle.gimbal
		print " Battery: %s" % vehicle.battery
		print " EKF OK?: %s" % vehicle.ekf_ok
		print " Last Heartbeat: %s" % vehicle.last_heartbeat
		print " Rangefinder: %s" % vehicle.rangefinder
		print " Rangefinder distance: %s" % vehicle.rangefinder.distance
		print " Rangefinder voltage: %s" % vehicle.rangefinder.voltage
		print " Heading: %s" % vehicle.heading
		print " Is Armable?: %s" % vehicle.is_armable
		print " System status: %s" % vehicle.system_status.state
		print " Groundspeed: %s" % vehicle.groundspeed    # settable
		print " Airspeed: %s" % vehicle.airspeed    # settable
		print " Mode: %s" % vehicle.mode.name    # settable
		print " Armed: %s" % vehicle.armed    # settable
		'''
		print "======================================================="
		return vehicle, sitl
Ejemplo n.º 30
0
                "target-lon": 0,
                "target-alt": 50,
                "expected-heading": 90,
            }
        ],
    },
]



# ready data for many dronekit-sitl processes
simulation_count = 2
for i in range(0,simulation_count):
    print("Creating simulator (SITL) %d" % (i,))
    from dronekit_sitl import SITL
    sitl = SITL(instance=i, path=args.binary, defaults_filepath=args.defaults, gdb=True)
#    sitl.download('copter', '3.3', verbose=True)
    sitls.append(sitl)
    hub_connection_strings.append(sitl.connection_string())

# start the SITLs one at a time, giving each a unique SYSID_THISMAV
def set_params_target(i, new_sysid, connection_string):
    lat = -35.363261
    lon = 149.165230
    sitl_args = ['--model', 'quad', '--home=%s,%s,584,353' % (lat,lon,) ]
    print("%d: Launching SITL (%s)" % (i,str(sitl_args)))
    sitls[i].launch(sitl_args, await_ready=True, verbose=True, speedup=50,)
    print("Sleeping a little here")
    time.sleep(5)
    print("%d: Connecting to its vehicle 1" % (i,))
    vehicle = dronekit.connect(connection_string, wait_ready=True, target_system=1, heartbeat_timeout=60)
Ejemplo n.º 31
0
    },
]

# ready data for many dronekit-sitl processes
simulation_count = 2
for i in range(0, simulation_count):
    print("Creating simulator (SITL) %d" % (i, ))
    from dronekit_sitl import SITL
    breakpoints = []
    #    breakpoints = ["GCS_Common.cpp:1031"]
    #    breakpoints = ["GCS_MAVLINK::handle_mission_item"]
    #    breakpoints = ["GCS_MAVLINK::handle_mission_count"]
    #    breakpoints = ["Plane::auto_takeoff_check"]
    sitl = SITL(instance=i,
                path=args.binary,
                defaults_filepath=args.defaults,
                gdb=True,
                valgrind=False,
                gdb_breakpoints=breakpoints)
    #    sitl.download('copter', '3.3', verbose=True)
    sitls.append(sitl)
    hub_connection_strings.append(sitl.connection_string())


def load_mission(vehicle):
    # load a simple mission so we can take off
    cmds = vehicle.commands

    print(" Clear any existing commands")
    cmds.clear()

    def check_mission_commands(conn, name, m):
Ejemplo n.º 32
0
class Vehicle():

	#######################################
	# Constructor
	#######################################
	def __init__(self , connectionString,mode):

         
            if mode =='r': #run on vihcle
                print 'Connecting to vehicle on: %s' % connectionString
                self.vehicle = connect(connectionString, baud=57600)
        
            if mode =='s': #run on simultor
                self.sitl = SITL()
                self.sitl.download('copter', '3.3', verbose=True)
                self.sitl_args = ['-I0','--gimbal' ,'--model', 'quad', '--home=31.7396,35.1956,3,-1']
                self.sitl.launch(self.sitl_args, await_ready=True, restart=True)
                self.vehicle = connect('tcp:127.0.0.1:5760', wait_ready=True)
            
                
    #######################################
    # Arm And TakeOff
    #######################################
	def arm_and_takeoff(self,takeOffAltitude):

		self.vehicle_pre_arm_check()       # chack if the vehicle is armable
		print "Basic pre-arm checks"
		# Don't let the user try to arm until autopilot is ready
		while not self.vehicle.is_armable:
			print " Waiting for vehicle to initialise..."
		time.sleep(1)

		self.vehicle_arm()						# Arm vehicle
		
		self.vehicle_arming_check()				# Wait for the drone to arm
		
		time.sleep(1)							# Sleep for Sec
		
		self.vehicle_take_off(takeOffAltitude)	# TakeOff
		
		#self.print_vehicle_full_state_info()	# Print the vehicle status
		

		
#######################################################################################################################################################################
#######################################################################################################################################################################		
#######################################################################################################################################################################		
		
		
		
	##############################################
	#				SETTERS						 #
	##############################################
	
	def air_speed(self,speed):
		self.vehicle.airspeed = speed
		
	def rtl(self):
		self.vehicle.mode = VehicleMode("RTL")
	
	def closeVec(self):
		self.vehicle.close()

	def set_mode(self,mode):
		self.vehicle.mode=mode
	
	def set_armed(self,bool):
		self.vehicle.armed =bool    
	def gimbal_rotate(self,pitch=0,roll=0,yaw=0):
		self.vehicle.gimbal.rotate(pitch,roll,yaw)

	def set_groundspeed(self,speed):
		self.vehicle.groundspeed= speed
		return    
##############################################
	#				GETTERS						 #
	##############################################

	def get_location(self):                 return self.vehicle.location.global_frame

	def get_location_latitude(self):        return self.vehicle.location.lat

	def get_location_longitude(self):   	return self.vehicle.location.lon

	def get_location_altitude(self):    	return self.vehicle.location.alt
		
	def get_location_global_frame(self):	return self.vehicle.location.global_frame

	def get_location_global_relative(self):	return self.vehicle.location.global_relative_frame

	def get_location_local_frame(self): 	return self.vehicle.location.local_frame

	def get_attitude(self):                 return self.vehicle.attitude

	def get_gimbal(self):                   return self.vehicle.gimbal

	def get_velocity(self):                 return self.vehicle.velocity

	def get_gps(self):                      return self.vehicle.gps_0

	def get_heading(self):                  return self.vehicle.heading

	def is_armable(self):                   return self.vehicle.is_armable

	def get_system_status(self):    		return self.vehicle.system_status.state

	def get_groundspeed(self):      		return self.vehicle.groundspeed

	def get_airspeed(self):         		return self.vehicle.airspeed

	def get_mode(self):                 	return self.vehicle.mode.name

	def get_home_location(self):        	return self.vehicle.home_location

	def get_battery_voltage(self):          return self.vehicle.battery.voltage
	
	def get_battery_current (self):         return self.vehicle.battery.current
	
	def get_battery_level (self):           
		if self.vehicle.battery.level is None:
			return -1
		return self.vehicle.battery.level

	
	def get_distance_metres(self,aLocation1, aLocation2):
		dlat        = aLocation2.lat - aLocation1.lat
		dlong       = aLocation2.lon - aLocation1.lon
		return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5


	##############################################
	#				PRINTS						 #
	##############################################
	
	def print_vehicle_state(self):
		print '|  Pitch: $%.2f  |  Yaw: $%.2f  |  Roll: $%.2f  |  Heading: $%.2f  |' % (self.vehicle.attitude.pitch, self.vehicle.attitude.yaw, self.vehicle.attitude.roll, self.vehicle.heading)
		
		
	def print_vehicle_info(self):
		print "Attitude"
		print "========"
		print 'pitch: $%.2f' % (self.vehicle.attitude.pitch) 
		print 'yaw:   $%.2f' % (self.vehicle.attitude.yaw) 
		print 'roll:  $%.2f' % (self.vehicle.attitude.roll)
		print ' '
		#print 'Battery'
		#print '======='
		#print 'Voltage: $%.2f' % (self.vehicle.battery.voltage)
		#print 'Current: $%.2f' % (self.vehicle.battery.current)
		#print 'Level:   $%.2f' % (self.vehicle.battery.level)
		
		
	def print_vehicle_full_state_info(self):
		# Get all vehicle attributes (state)
		print "\nGet all vehicle attribute values:"
		print " Global Location: ...................... %s" % self.vehicle.location.global_frame
		print " Global Location (relative altitude): .. %s" % self.vehicle.location.global_relative_frame
		print " Local Location: ....................... %s" % self.vehicle.location.local_frame
		print " Attitude: ............................. %s" % self.vehicle.attitude
		print " Velocity: ............................. %s" % self.vehicle.velocity
		print " GPS: .................................. %s" % self.vehicle.gps_0
		print " Gimbal status: ........................ %s" % self.vehicle.gimbal
		print " Battery: .............................. %s" % self.vehicle.battery
		print " EKF OK?: .............................. %s" % self.vehicle.ekf_ok
		print " Last Heartbeat: ....................... %s" % self.vehicle.last_heartbeat
		print " Rangefinder: .......................... %s" % self.vehicle.rangefinder
		print " Rangefinder distance: ................. %s" % self.vehicle.rangefinder.distance
		print " Rangefinder voltage: .................. %s" % self.vehicle.rangefinder.voltage
		print " Heading: .............................. %s" % self.vehicle.heading
		print " Is Armable?: .......................... %s" % self.vehicle.is_armable
		print " System status: ........................ %s" % self.vehicle.system_status.state
		print " Groundspeed: .......................... %s" % self.vehicle.groundspeed	# settable
		print " Airspeed: ............................. %s" % self.vehicle.airspeed		# settable
		print " Mode: ................................. %s" % self.vehicle.mode.name    # settable
		print " Armed: ................................ %s" % self.vehicle.armed		# settable
		print "\n \n"
		
	##############################################
	#				VEHICLE						 #
	##############################################
	
	# Loof until the drone is initialise
	def vehicle_pre_arm_check(self):
		while not self.vehicle.is_armable:
			print " Waiting for vehicle to initialise..."
			time.sleep(1)
	
	# Start to arm the motors
	def vehicle_arm(self):
		print "==> Vehicle Start Arming"
		self.vehicle.mode = VehicleMode("GUIDED")	# Copter should arm in GUIDED mode
		self.vehicle.armed = True
		return 1

	# Loof until the the motors are armed
	def vehicle_arming_check(self):
		while not self.vehicle.armed:
			print "==> Waiting for vehicle to arm"
			time.sleep(1)
			#self.vehicle.armed = True
	
	# Taking off the drone to the given altitude - Loof until the the drone reaches the altitude
	def vehicle_take_off(self, takeOffAltitude):
		print "Vehicle Taking Off!"
		self.vehicle.simple_takeoff(takeOffAltitude) # Take off to target altitude

		# Wait until the vehicle reaches a safe height before processing the goto
		while True:
			print " Altitude: ", self.vehicle.location.global_relative_frame.alt	  
			if self.vehicle.location.global_relative_frame.alt >= takeOffAltitude*0.95: #Trigger just below target alt.
				print "Reached target altitude"
				break
			time.sleep(1)
			
	# Take the drone to the specifide location, 
	def simpleGoTo(self, lat, lng, alt, velocity = -1):
		#dest = LocationGlobalRelative(lat, lon, height)
		# set the default travel speed
		if velocity == -1:
			self.vehicle.airspeed = 1
		else:
			self.vehicle.airspeed = velocity
		dest = LocationGlobalRelative(lat, lng, alt)
		self.vehicle.simple_goto(dest)
		
	
	def vehicle_RTL(self):
		self.vehicle.mode = VehicleMode("RTL")
		self.vehicle.flush()
			
	# Take the drone to 
	def vehicle_goto_location(self, location):
		currentLocation = self.vehicle.location
		targetDistance = get_distance_metres(currentLocation, location)
		gotoFunction(location);
		self.vehicle.flush()
		while not api.exit and self.vehicle.mode.name=="GUIDED": #Stop action if we are no longer in guided mode.
			remainingDistance = get_distance_metres(self.vehicle.location, location)
			if remainingDistance <= targetDistance * 0.01: #Just below target, in case of undershoot.
				print "Reached target"
				break;
			time.sleep(2)

			
	def vehicle_condition_yaw(self, heading, relative=False):
		"""
			Send MAV_CMD_CONDITION_YAW message to point vehicle at a specified heading (in degrees).
			This method sets an absolute heading by default, but you can set the `relative` parameter
			to `True` to set yaw relative to the current yaw heading.
			By default the yaw of the vehicle will follow the direction of travel. After setting 
			the yaw using this function there is no way to return to the default yaw "follow direction 
			of travel" behaviour
		"""
		if relative:
			is_relative=1 #yaw relative to direction of travel
		else:
			is_relative=0 #yaw is an absolute angle
		# create the CONDITION_YAW command using command_long_encode()
		msg = self.vehicle.message_factory.command_long_encode(
			0, 0,        # target system, target component
			mavutil.mavlink.MAV_CMD_CONDITION_YAW, #command
			0, 			 #confirmation
			heading,     # param 1, yaw in degrees
			0,           # param 2, yaw speed deg/s
			1,           # param 3, direction -1 ccw, 1 cw
			is_relative, # param 4, relative offset 1, absolute angle 0
			0, 0, 0)     # param 5 ~ 7 not used
		
		# send command to vehicle
		self.vehicle.send_mavlink(msg)
	
	
	
	def vehicle_rotate_camera_gimbal(self, location):
		"""
			Send MAV_CMD_DO_SET_ROI message to point camera gimbal at a 
			specified region of interest (LocationGlobal).
			The vehicle may also turn to face the ROI.
		"""
		
		# create the MAV_CMD_DO_SET_ROI command
		msg = self.vehicle.message_factory.command_long_encode(
			0, 0,    # target system, target component
			mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command
			0, #confirmation
			0, 0, 0, 0, #params 1-4
			location.lat,
			location.lon,
			location.alt
			)
		# send command to vehicle
		self.vehicle.send_mavlink(msg)
		
		
		
		
		
		
	"""
		Functions to make it easy to convert between the different frames-of-reference. In particular these
		make it easy to navigate in terms of "metres from the current position" when using commands that take 
		absolute positions in decimal degrees.
		The methods are approximations only, and may be less accurate over longer distances, and when close 
		to the Earth's poles.
		Specifically, it provides:
		* get_location_metres - Get LocationGlobal (decimal degrees) at distance (m) North & East of a given LocationGlobal.
		* get_distance_metres - Get the distance between two LocationGlobal objects in metres
		* get_bearing - Get the bearing in degrees to a LocationGlobal
	"""

	def get_location_metres(original_location, dNorth, dEast):
		"""
		Returns a LocationGlobal object containing the latitude/longitude `dNorth` and `dEast` metres from the 
		specified `original_location`. The returned LocationGlobal has the same `alt` value
		as `original_location`.
		The function is useful when you want to move the vehicle around specifying locations relative to 
		the current vehicle position.
		The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles.
		"""
		earth_radius=6378137.0 #Radius of "spherical" earth
		#Coordinate offsets in radians
		dLat = dNorth/earth_radius
		dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180))

		#New position in decimal degrees
		newlat = original_location.lat + (dLat * 180/math.pi)
		newlon = original_location.lon + (dLon * 180/math.pi)
		if type(original_location) is LocationGlobal:
			targetlocation=LocationGlobal(newlat, newlon,original_location.alt)
		elif type(original_location) is LocationGlobalRelative:
			targetlocation=LocationGlobalRelative(newlat, newlon,original_location.alt)
		else:
			raise Exception("Invalid Location object passed")
			
		return targetlocation;


	def get_distance_metres(self,aLocation1, aLocation2):
		"""
		Returns the ground distance in metres between two LocationGlobal objects.
		This method is an approximation, and will not be accurate over large distances and close to the 
		"""
		dlat = aLocation2.lat - aLocation1.lat
		dlong = aLocation2.lon - aLocation1.lon
		return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5


	def get_bearing(self,aLocation1, aLocation2):
		"""
		Returns the bearing between the two LocationGlobal objects passed as parameters.
		This method is an approximation, and may not be accurate over large distances and close to the 
		earth's poles.
		"""	
		off_x = aLocation2.lon - aLocation1.lon
		off_y = aLocation2.lat - aLocation1.lat
		bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795
		if bearing < 0:
			bearing += 360.00
		return bearing;



	"""
	Functions to move the vehicle to a specified position (as opposed to controlling movement by setting velocity components).
	The methods include:
	* goto_position_target_global_int - Sets position using SET_POSITION_TARGET_GLOBAL_INT command in 
		MAV_FRAME_GLOBAL_RELATIVE_ALT_INT frame
	* goto_position_target_local_ned - Sets position using SET_POSITION_TARGET_LOCAL_NED command in 
		MAV_FRAME_BODY_NED frame
	* goto - A convenience function that can use Vehicle.simple_goto (default) or 
		goto_position_target_global_int to travel to a specific position in metres 
		North and East from the current location. 
		This method reports distance to the destination.
	"""

	def goto_position_target_global_int(aLocation):
		"""
		Send SET_POSITION_TARGET_GLOBAL_INT command to request the vehicle fly to a specified LocationGlobal.
		See the above link for information on the type_mask (0=enable, 1=ignore). 
		At time of writing, acceleration and yaw bits are ignored.
		"""
		msg = self.vehicle.message_factory.set_position_target_global_int_encode(
			0,       # time_boot_ms (not used)
			0, 0,    # target system, target component
			mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame
			0b0000111111111000, # type_mask (only speeds enabled)
			aLocation.lat*1e7, # lat_int - X Position in WGS84 frame in 1e7 * meters
			aLocation.lon*1e7, # lon_int - Y Position in WGS84 frame in 1e7 * meters
			aLocation.alt, # alt - Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT
			0, # X velocity in NED frame in m/s
			0, # Y velocity in NED frame in m/s
			0, # Z velocity in NED frame in m/s
			0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
			0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 
		# send command to vehicle
		self.vehicle.send_mavlink(msg)



	def goto_position_target_local_ned(north, east, down):
		"""	
		Send SET_POSITION_TARGET_LOCAL_NED command to request the vehicle fly to a specified 
		location in the North, East, Down frame.
		It is important to remember that in this frame, positive altitudes are entered as negative 
		"Down" values. So if down is "10", this will be 10 metres below the home altitude.
		Starting from AC3.3 the method respects the frame setting. Prior to that the frame was
		ignored. For more information see: 
		http://dev.ardupilot.com/wiki/copter-commands-in-guided-mode/#set_position_target_local_ned
		See the above link for information on the type_mask (0=enable, 1=ignore). 
		At time of writing, acceleration and yaw bits are ignored.
		"""
		msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
			0,       # time_boot_ms (not used)
			0, 0,    # target system, target component
			mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
			0b0000111111111000, # type_mask (only positions enabled)
			north, east, down, # x, y, z positions (or North, East, Down in the MAV_FRAME_BODY_NED frame
			0, 0, 0, # x, y, z velocity in m/s  (not used)
			0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
			0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 
		# send command to vehicle
		self.vehicle.send_mavlink(msg)
		
		
	def send_ned_velocity(velocity_x, velocity_y, velocity_z, duration):
		"""
		Move vehicle in direction based on specified velocity vectors and
		for the specified duration.
		This uses the SET_POSITION_TARGET_LOCAL_NED command with a type mask enabling only 
		velocity components 
		(http://dev.ardupilot.com/wiki/copter-commands-in-guided-mode/#set_position_target_local_ned).
		
		Note that from AC3.3 the message should be re-sent every second (after about 3 seconds
		with no message the velocity will drop back to zero). In AC3.2.1 and earlier the specified
		velocity persists until it is canceled. The code below should work on either version 
		(sending the message multiple times does not cause problems).
		
		See the above link for information on the type_mask (0=enable, 1=ignore). 
		At time of writing, acceleration and yaw bits are ignored.
		"""
		msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
			0,       # time_boot_ms (not used)
			0, 0,    # target system, target component
			mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
			0b0000111111000111, # type_mask (only speeds enabled)
			0, 0, 0, # x, y, z positions (not used)
			velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
			0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
			0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 

		# send command to vehicle on 1 Hz cycle
		for x in range(0,duration):
			self.vehicle.send_mavlink(msg)
			time.sleep(1)
		
    


	def send_global_velocity(velocity_x, velocity_y, velocity_z, duration):
		"""
		Move vehicle in direction based on specified velocity vectors.
		This uses the SET_POSITION_TARGET_GLOBAL_INT command with type mask enabling only 
		velocity components 
		(http://dev.ardupilot.com/wiki/copter-commands-in-guided-mode/#set_position_target_global_int).
		
		Note that from AC3.3 the message should be re-sent every second (after about 3 seconds
		with no message the velocity will drop back to zero). In AC3.2.1 and earlier the specified
		velocity persists until it is canceled. The code below should work on either version 
		(sending the message multiple times does not cause problems).
		
		See the above link for information on the type_mask (0=enable, 1=ignore). 
		At time of writing, acceleration and yaw bits are ignored.
		"""
		msg = self.vehicle.message_factory.set_position_target_global_int_encode(
			0,       # time_boot_ms (not used)
			0, 0,    # target system, target component
			mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame
			0b0000111111000111, # type_mask (only speeds enabled)
			0, # lat_int - X Position in WGS84 frame in 1e7 * meters
			0, # lon_int - Y Position in WGS84 frame in 1e7 * meters
			0, # alt - Altitude in meters in AMSL altitude(not WGS84 if absolute or relative)
			# altitude above terrain if GLOBAL_TERRAIN_ALT_INT
			velocity_x, # X velocity in NED frame in m/s
			velocity_y, # Y velocity in NED frame in m/s
			velocity_z, # Z velocity in NED frame in m/s
			0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)
			0, 0)    # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 

		# send command to vehicle on 1 Hz cycle
		for x in range(0,duration):
			self.vehicle.send_mavlink(msg)
			time.sleep(1)    
Ejemplo n.º 33
0
target_systems = []

if connection_string:
    # Create a MAVLink connection:
    mav = dronekit.mavlink.MAVConnection(connection_string)
    target_systems = [1]
else:
    simulation_count = int(args.simulation_count)
    hub_connection_strings = []

    # start many dronekit-sitl processes
    port = 5760  # massive assumption
    for i in range(0, simulation_count):
        print("Creating simulator (SITL) %d" % (i, ))
        from dronekit_sitl import SITL
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        lat = -35.363261 + i * 0.00008
        sitl_args = [
            '-I' + str(i), '--model', 'quad',
            '--home=' + str(lat) + ',149.165230,584,353'
        ]
        sitls.append([sitl, sitl_args])
        hub_connection_strings.append('tcp:127.0.0.1:' + str(port))
        port += 10  # *cough*

    # start the SITLs one at a time, giving each a unique SYSID_THISMAV
    def change_sysid_target(i, new_sysid, connection_string):
        print("%d: Launching SITL (%s)" % (i, str(sitls[i][1])))
        sitls[i][0].launch(sitls[i][1], await_ready=True)
        print("%d: Connecting to its vehicle 1" % (i, ))
Ejemplo n.º 34
0
    def test_move_left(self):
        """"
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,0']
        sitl.launch(sitl_args, await_ready=True, restart=True)
        drone = Drone('tcp:127.0.0.1:5760')
        drone.take_off(2)
        print drone.vehicle.location.global_relative_frame
        drone.move_left(2)
        print drone.vehicle.location.global_relative_frame
        drone.land()
        drone.vehicle.close()
        sitl.stop()
        """""
        """"
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,45']
        sitl.launch(sitl_args, await_ready=True, restart=True)
        drone = Drone('tcp:127.0.0.1:5760')
        drone.take_off(2)
        print drone.vehicle.location.global_relative_frame
        drone.move_left(2)
        print drone.vehicle.location.global_relative_frame
        drone.land()
        drone.vehicle.close()
        sitl.stop()
        #self.assertTrue(True)
        """
        """"
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,90']
        sitl.launch(sitl_args, await_ready=True, restart=True)
        drone = Drone('tcp:127.0.0.1:5760')
        drone.take_off(2)
        print drone.vehicle.location.global_relative_frame
        drone.move_left(2)
        print drone.vehicle.location.global_relative_frame
        drone.land()
        drone.vehicle.close()
        sitl.stop()
        #self.assertTrue(True)
        """
        """"
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,135']
        sitl.launch(sitl_args, await_ready=True, restart=True)
        drone = Drone('tcp:127.0.0.1:5760')
        drone.take_off(2)
        print drone.vehicle.location.global_relative_frame
        drone.move_left(2)
        print drone.vehicle.location.global_relative_frame
        drone.land()
        drone.vehicle.close()
        sitl.stop()
        #self.assertTrue(True)
        """
        """"
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,180']
        sitl.launch(sitl_args, await_ready=True, restart=True)
        drone = Drone('tcp:127.0.0.1:5760')
        drone.take_off(2)
        print drone.vehicle.location.global_relative_frame
        drone.move_left(2)
        print drone.vehicle.location.global_relative_frame
        drone.land()
        drone.vehicle.close()
        sitl.stop()
        #self.assertTrue(True)
        """
        """"
        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,225']
        sitl.launch(sitl_args, await_ready=True, restart=True)
        drone = Drone('tcp:127.0.0.1:5760')
        drone.take_off(2)
        print drone.vehicle.location.global_relative_frame
        drone.move_left(2)
        print drone.vehicle.location.global_relative_frame
        drone.land()
        drone.vehicle.close()
        sitl.stop()
        #self.assertTrue(True)
        """

        sitl = SITL()
        sitl.download('copter', '3.3', verbose=True)
        sitl_args = ['-I0', '--model', 'quad', '--home=31.768923,35.193595,0,270']
        sitl.launch(sitl_args, await_ready=True, restart=True)
        drone = Drone('tcp:127.0.0.1:5760')
        drone.take_off(2)
        print drone.vehicle.location.global_relative_frame
        drone.move_left(2)
        print drone.vehicle.location.global_relative_frame
        drone.land()
        drone.vehicle.close()
        sitl.stop()
        #self.assertTrue(True)

        """"
Ejemplo n.º 35
0
    def __init__(self):
        # SITL is the system in the loop simulator from dronekit
        self.sitl = SITL()
        self.sitl.download('solo', '1.2.0', verbose=True)
        sitl_args = ['solo', '--home=51.011447,3.711648,5,0']
        self.sitl.launch(sitl_args,
                         await_ready=True,
                         restart=True,
                         verbose=True)

        #############################################################################################################
        # This is old code from when the onboard and simulator code were no modules yet
        # It is used to find the correct directories and start the correct processes, and might prove useful in the future

        # # This simulator can be invoked from the /simulator directory and the /tests directory
        # # Hardcoding relative paths should be avoided
        # current_dir = os.path.abspath(os.curdir)
        # parent_dir = os.path.dirname(current_dir)
        # drone_dir = os.path.join(parent_dir, "drone")
        # simulator_dir = os.path.join(parent_dir, "simulator")
        # onboard_dir = os.path.join(parent_dir, "onboard")
        # # Search for the drone directory containing the /simulator and /onboard directories
        # # Or stop if we find the /simulator and /onboard directories
        # while not (os.path.exists(drone_dir) or (os.path.exists(simulator_dir) and os.path.exists(onboard_dir))):
        #     current_dir = parent_dir
        #     parent_dir = os.path.dirname(current_dir)
        #     if parent_dir == current_dir:
        #         print "Could not find files"
        #         sys.exit(1)  # we did not find one of the necessary files
        #     drone_dir = os.path.join(parent_dir, "drone")
        #     simulator_dir = os.path.join(parent_dir, "simulator")
        #     onboard_dir = os.path.join(parent_dir, "onboard")
        # if os.path.exists(drone_dir):
        #     # we found the drone directory
        #     simulator_dir = os.path.join(drone_dir, "simulator")
        #     onboard_dir = os.path.join(drone_dir, "onboard")
        # video_dir = os.path.join(simulator_dir, "videos")
        # video_footage = os.path.join(video_dir, "testfootage.h264")
        # server = os.path.join(onboard_dir, "server.py")
        # control_module = os.path.join(onboard_dir, "control_module.py")
        # if not (os.path.exists(simulator_dir) and
        #         os.path.exists(onboard_dir) and
        #         os.path.exists(video_footage) and
        #         os.path.exists(server) and
        #         os.path.exists(control_module)):
        #     print "Could not find files"
        #     sys.exit(1)  # we did not find one of the necessary files

        # The server and control modules were started as seperate processes
        # This gave issues when trying to measure the code coverage, hence we changed it to threads

        # env_vars = os.environ.copy()
        # env_vars["COVERAGE_PROCESS_START"] = ".coveragerc"
        # self.server_process = Popen(['python2', server, '--level', 'debug', '--simulate'], env=env_vars)
        # self.control_process = Popen(['python2', control_module, '--level', 'debug', '--simulate'], env=env_vars)
        #############################################################################################################

        # Now that we have modules, we can just do this
        current_dir = os.path.dirname(
            os.path.realpath(__file__))  # this is the directory of the file
        video_footage = os.path.join(current_dir, 'videos', 'testfootage.h264')
        self.stream_simulator = StreamSimulator(video_footage)
        self.stream_simulator.start()

        log_level = logging.DEBUG
        simulation_logger = logging.getLogger("Simulator")
        formatter = logging.Formatter(logformat, datefmt=dateformat)
        handler = logging.StreamHandler(stream=sys.stdout)
        # To log to a file, this could be used
        # handler = logging.FileHandler(filename=log_file)
        handler.setFormatter(formatter)
        handler.setLevel(log_level)
        simulation_logger.addHandler(handler)
        simulation_logger.setLevel(log_level)
        self.logger = simulation_logger

        self.server_thread = ServerSimulator(logger=simulation_logger)
        self.control_thread = ControlModuleSimulator(logger=simulation_logger,
                                                     log_lvl=log_level)
        self.server_thread.start()
        self.control_thread.start()

        # capture kill signals to send it to the subprocesses
        signal.signal(signal.SIGINT, self.signal_handler)
        signal.signal(signal.SIGTERM, self.signal_handler)
Ejemplo n.º 36
0
parser = argparse.ArgumentParser(description="Demonstrates how to create attributes from MAVLink messages. ")
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 args.connect:
    print "Starting copter simulator (SITL)"
    from dronekit_sitl import SITL

    sitl = SITL()
    sitl.download("copter", "3.3", verbose=True)
    sitl_args = ["-I0", "--model", "quad", "--home=-35.363261,149.165230,584,353"]
    sitl.launch(sitl_args, await_ready=True, restart=True)
    connection_string = "tcp:127.0.0.1:5760"


# Connect to the Vehicle
print "Connecting to vehicle on: %s" % connection_string
vehicle = connect(connection_string, wait_ready=True, vehicle_class=MyVehicle)

# Add observer for the custom attribute


def raw_imu_callback(self, attr_name, value):
    # attr_name == 'raw_imu'
Ejemplo n.º 37
0
def start_sitl():
    """Launch a SITL using local copy of Copter 4,
    then set up the simulator for a rangefinder.
    Only works for Windows or Linux.  Including
    binary in the project is ugly, but only used
    for testing."""
    sitl_path = sitl_file_path()
    sitl = SITL(sitl_path)
    sitl.launch(['--home=51.454531,-2.629158,589,353'])

    veh = connect(sitl.connection_string(), vehicle_class=DroneTreeVehicle)

    veh.parameters['SIM_SONAR_SCALE'] = 0.00001
    veh.parameters['RNGFND2_ORIENT'] = 0
    veh.parameters['RNGFND2_SCALING'] = 10
    veh.parameters['RNGFND2_PIN'] = 0
    veh.parameters['RNGFND2_TYPE'] = 1
    veh.parameters['RNGFND2_MAX_CM'] = 5000
    veh.parameters['RNGFND2_MIN_CM'] = 5000

    veh.parameters['SIM_BATT_VOLTAGE'] = 12.59
    veh.parameters['BATT_MONITOR'] = 4

    veh.parameters['TERRAIN_ENABLE'] = 0

    veh.parameters['ARMING_CHECK'] = 16384  # mission only
    veh.parameters['FRAME_CLASS'] = 2  # I'm a hex

    veh.close()

    sitl.stop()
    sitl.launch(['--home=51.454531,-2.629158,589,353'],
                use_saved_data=True,
                verbose=True)
    sitl.block_until_ready()

    return sitl
Ejemplo n.º 38
0
def test_preserve_eeprom():
    # Start an SITL instance and change COMPASS_USE
    sitl = SITL()
    sitl.download('copter', '3.3')
    sitl.launch(copter_args, verbose=True, await_ready=True, use_saved_data=False)
    connection_string = sitl.connection_string()
    vehicle = dronekit.connect(connection_string, wait_ready=True)
    new_compass_use = 10
    print("Changing COMPASS_USE to {0}".format(new_compass_use))
    while vehicle.parameters["COMPASS_USE"] != new_compass_use:
        vehicle.parameters["COMPASS_USE"] = new_compass_use
        time.sleep(0.1)
    print("Changed COMPASS_USE to {0}".format(new_compass_use))
    time.sleep(5) # give parameters time to write
    sitl.stop()
    vehicle.close()

    # Now see if it persisted
    sitl.launch(copter_args, await_ready=True, use_saved_data=True)
    vehicle = dronekit.connect(connection_string, wait_ready=True)
    assert_equals(new_compass_use, vehicle.parameters["COMPASS_USE"])

    vehicle.close()
    sitl.stop()
 def test_download_404(self):
     sitl = SITL()
     sitl.download('rocket', '1.123.098.123')
Ejemplo n.º 40
0
import argparse
parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.basic_takeoff.')
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
connection_string1 = None
connection_string2 = None

if not connection_string:
    from dronekit_sitl import SITL
    print(site.USER_SITE)
    sitl1 = SITL()
    sitl1.download('copter', '3.3', verbose=True)
    sitl2 = SITL()
    sitl2.download('copter', '3.3', verbose=True)
    sitl_args1 = ['-I0', '--simin=127.0.0.1:4000', '--simout=127.0.0.1:4001', '--model',
                         'quad']
    sitl_args2 = ['-I1', '--simin=127.0.0.1:4000', '--simout=127.0.0.1:4001', '--model',
                         'quad']
    sitlInstance1 = sitl1.launch(sitl_args1, verbose=False, await_ready=False, restart=True)
    sitlInstance2 = sitl2.launch(sitl_args2, verbose=False, await_ready=False, restart=True)
    #    sitl = dronekit_sitl.start_default()
    connection_string1 = sitl1.connection_string()
    connection_string2 = sitl2.connection_string()

print('Connecting to vehicle1 on: %s' % connection_string1)
vehicle1 = connect(connection_string1, wait_ready=True)
Ejemplo n.º 41
0
                    i = i - 1
                elif list[rand] == 'b':
                    j = j - 1


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 = SITL()

# Start SITL if no connection string specified
if not connection_string:
    import dronekit_sitl
    sitl = dronekit_sitl.start_default()
    connection_string = 'tcp:127.0.0.1:5762'

# Connect to the Vehicle
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True)


def arm_and_takeoff(aTargetAltitude):

    print("Basic pre-arm checks")
Ejemplo n.º 42
0
#Set up option parsing to get connection string
import argparse  
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 args.connect:
    print "Starting copter simulator (SITL)"
    from dronekit_sitl import SITL
    sitl = SITL()
    sitl.download('copter', '3.3', verbose=True)
    sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
    sitl.launch(sitl_args, await_ready=True, restart=True)
    connection_string = 'tcp:127.0.0.1:5760'


# Connect to the Vehicle
print 'Connecting to vehicle on: %s' % connection_string
vehicle = connect(connection_string, wait_ready=True)


def arm_and_takeoff(aTargetAltitude):
    """
    Arms vehicle and fly to aTargetAltitude.
    """
Ejemplo n.º 43
0
class SimRunner:
    def arm_vehicle(self):
        arm_time = 0
        self.vehicle.armed = True

        while not self.vehicle.armed:
            time.sleep(0.2)
            print('waiting for armed...')
            arm_time += 0.2
            if arm_time > 30:
                print("Arm fail")
                self.vehicle.close()
                self.sitl.stop()
                self.sitl.p.stdout.close()
                self.sitl.p.stderr.close()
                self.ready = False
                return

    def __init__(self, sample_id, core_id, initial_profile, config):
        ardupilot_dir = config['root_dir'] + 'experiment/elf/'
        out_dir = config['root_dir'] + 'experiment/output/'
        self.elf_dir = "%s%d/" % (ardupilot_dir, 0)
        self.exp_out_dir = "%s%s/%d/" % (out_dir, 'PA', 0)
        self.ready = True
        self.states = []
        self.profiles = []
        self.sim_id = "%d_%d" % (sample_id, core_id)
        copter_file = self.elf_dir + "ArduCopter.elf"
        self.delta = 0.1
        self.sitl = SITL(path=copter_file)
        home_str = "%.6f,%.6f,%.2f,%d" % (
            initial_profile.lat, initial_profile.lon, initial_profile.alt,
            initial_profile.yaw)
        sitl_args = [
            '-S',
            '-I%d' % bug_id, '--home=' + home_str, '--model', '+',
            '--speedup=1', '--defaults=' + self.elf_dir + 'copter.parm'
        ]
        self.sitl.launch(sitl_args,
                         await_ready=True,
                         restart=True,
                         wd=self.elf_dir)
        port_number = 5760 + bug_id * 10
        self.missions = [
            Mission1(),
            Mission2(),
            Mission3(),
            Mission4(),
            Mission5()
        ]
        try:
            self.vehicle = connect('tcp:127.0.0.1:%d' % port_number,
                                   wait_ready=True,
                                   rate=10)
        except APIException:
            print("Cannot connect")
            self.sitl.stop()
            self.sitl.p.stdout.close()
            self.sitl.p.stderr.close()
            self.ready = False
            return
        for k, v in initial_profile.params.items():
            self.vehicle.parameters[k] = v
        while not self.vehicle.is_armable:
            print('initializing...')
            time.sleep(1)
        self.arm_vehicle()
        time.sleep(2)
        print(self.vehicle.version)

    def run(self):
        takeoff_mission = self.missions[0]
        takeoff_mission.run(self)
        time.sleep(10)
        temp_state = []
        waypoint_num = 3
        T = 2

        ### 4 missions. Each mission has 3 waypoints. For each waypoint, the vehicle runs for 2 seconds
        for i in range(0, 4):
            current_location = self.vehicle.location.global_frame
            if i % 2 == 0:
                target_delta = [
                    random.uniform(0.0002, 0.0003) / waypoint_num,
                    random.uniform(0.0002, 0.0003) / waypoint_num,
                    random.uniform(20, 30) / waypoint_num
                ]
            else:
                target_delta = [
                    random.uniform(0.0002, 0.0003) / waypoint_num,
                    random.uniform(-0.0003, -0.0002) / waypoint_num,
                    random.uniform(-30, -20) / waypoint_num
                ]
            # count = 0
            for j in range(1, waypoint_num + 1):
                profile = LocationGlobal(
                    current_location.lat + target_delta[0] * j,
                    current_location.lon + target_delta[1] * j,
                    current_location.alt + target_delta[2] * j)
                self.profiles.append([profile.lat, profile.lon, profile.alt])
                self.vehicle.simple_goto(profile)
                current_t = 0
                temp_state = []

                while current_t < T:
                    ### record the [lat,lon,alt,pitch,yaw,roll,velocity] every 0.1 seconds
                    temp_state.append([
                        self.vehicle.location.global_frame.lat,
                        self.vehicle.location.global_frame.lon,
                        self.vehicle.location.global_frame.alt,
                        self.vehicle.attitude.pitch, self.vehicle.attitude.yaw,
                        self.vehicle.attitude.roll, self.vehicle.velocity[0],
                        self.vehicle.velocity[1], self.vehicle.velocity[2]
                    ])
                    time.sleep(0.1)
                    current_t += 0.1
                    # count += 1
                    # print(count)
                    # print_info(self.vehicle)
                self.states.append(temp_state)

        self.vehicle.close()
        self.sitl.stop()

        np.save(self.exp_out_dir + "states_np_%s" % self.sim_id,
                np.array(self.states))
        np.save(self.exp_out_dir + "profiles_np_%s" % self.sim_id,
                np.array(self.profiles))

        print("Output Execution Path...")
        with open(self.exp_out_dir + "raw_%s.txt" % self.sim_id,
                  "w") as execution_file:
            ep_line = self.sitl.stdout.readline(0.01)
            while ep_line is not None:
                execution_file.write(ep_line)
                ep_line = self.sitl.stdout.readline(0.01)

        self.sitl.p.stdout.close()
        self.sitl.p.stderr.close()
        print("Simulation %s completed." % self.sim_id)

    def run1(self):
        takeoff_mission = self.missions[0]
        takeoff_mission.run(self)
        time.sleep(10)
        home_location = self.vehicle.location.global_frame
        temp_state = []
        waypoint_num = 3
        T = 2
        ## first mission : guided mode
        for i in range(0, 5):
            current_location = self.vehicle.location.global_frame
            if i % 2 == 0:
                target_delta = [
                    random.uniform(0.0002, 0.0003) / waypoint_num,
                    random.uniform(0.0002, 0.0003) / waypoint_num,
                    random.uniform(20, 30) / waypoint_num
                ]
            else:
                target_delta = [
                    random.uniform(0.0002, 0.0003) / waypoint_num,
                    random.uniform(-0.0003, -0.0002) / waypoint_num,
                    random.uniform(-30, -20) / waypoint_num
                ]
            for j in range(1, waypoint_num + 1):
                profile = LocationGlobal(
                    current_location.lat + target_delta[0] * j,
                    current_location.lon + target_delta[1] * j,
                    current_location.alt + target_delta[2] * j)
                self.profiles.append([profile.lat, profile.lon, profile.alt])
                self.vehicle.simple_goto(profile)
                current_t = 0
                temp_state = []
                while current_t < T:
                    temp_state.append([
                        self.vehicle.location.global_frame.lat,
                        self.vehicle.location.global_frame.lon,
                        self.vehicle.location.global_frame.alt,
                        self.vehicle.attitude.pitch, self.vehicle.attitude.yaw,
                        self.vehicle.attitude.roll, self.vehicle.velocity[0],
                        self.vehicle.velocity[1], self.vehicle.velocity[2]
                    ])
                    time.sleep(0.1)
                    current_t += 0.1
                self.states.append(temp_state)

        ## second mission : acro mode
        self.vehicle.channels.overrides['1'] = 1400
        self.vehicle.channels.overrides['2'] = 1400
        self.vehicle.channels.overrides['3'] = 1500
        self.vehicle.channels.overrides['4'] = 1500
        self.vehicle.mode = VehicleMode('ACRO')
        for i in range(0, waypoint_num):
            current_location = self.vehicle.location.global_frame
            self.profiles.append([
                current_location.lat, current_location.lon,
                current_location.alt
            ])
            current_t = 0
            temp_state = []
            while current_t < T:
                self.vehicle.channels.overrides['3'] = 1500
                temp_state.append([
                    self.vehicle.location.global_frame.lat,
                    self.vehicle.location.global_frame.lon,
                    self.vehicle.location.global_frame.alt,
                    self.vehicle.attitude.pitch, self.vehicle.attitude.yaw,
                    self.vehicle.attitude.roll, self.vehicle.velocity[0],
                    self.vehicle.velocity[1], self.vehicle.velocity[2]
                ])
                time.sleep(0.1)
                current_t += 0.1
                # print_info(self.vehicle)
            self.states.append(temp_state)

        # ## third mission : auto mode
        # cmds = self.vehicle.commands
        # cmds.clear()
        # waypoint_in_auto = [random.uniform(0.0002,0.0003)/waypoint_num,random.uniform(0.0002,0.0003)/waypoint_num,random.uniform(20,30)/waypoint_num]
        # for j in range(1,waypoint_num+1):
        #     profile = LocationGlobal(current_location.lat+target_delta[0]*j,current_location.lon+target_delta[1]*j,current_location.alt+target_delta[2]*j)
        #     self.profiles.append([profile.lat,profile.lon,profile.alt])
        #     cmds.add(Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,0,0,0,0,0,0,profile.lat,profile.lon,profile.alt))
        # cmds.upload()
        # self.vehicle.commands.next = 0
        # self.vehicle.mode = VehicleMode('AUTO')
        # for j in range(0,waypoint_num):
        #     current_t = 0
        #     temp_state = []
        #     while current_t < T:
        #         temp_state.append([self.vehicle.location.global_frame.lat,self.vehicle.location.global_frame.lon,self.vehicle.location.global_frame.alt
        #         ,self.vehicle.attitude.pitch,self.vehicle.attitude.yaw,self.vehicle.attitude.roll,self.vehicle.velocity[0],self.vehicle.velocity[1],self.vehicle.velocity[2]])
        #         time.sleep(0.1)
        #         current_t += 0.1
        #     self.vehicle.commands.next += 1
        #     self.states.append(temp_state)

        #### fourth mission : rtl mode
        # self.vehicle.mode = VehicleMode('RTL')
        # for j in range(0,waypoint_num):
        #     self.profiles.append([home_location.lat,home_location.lon,home_location.alt])
        #     current_t = 0
        #     temp_state = []
        #     while current_t < T:
        #         temp_state.append([self.vehicle.location.global_frame.lat,self.vehicle.location.global_frame.lon,self.vehicle.location.global_frame.alt
        #         ,self.vehicle.attitude.pitch,self.vehicle.attitude.yaw,self.vehicle.attitude.roll,self.vehicle.velocity[0],self.vehicle.velocity[1],self.vehicle.velocity[2]])
        #         time.sleep(0.1)
        #         current_t += 0.1
        #     self.states.append(temp_state)
        self.vehicle.close()
        self.sitl.stop()

        np.save(self.exp_out_dir + "states_np_%s" % self.sim_id,
                np.array(self.states))
        np.save(self.exp_out_dir + "profiles_np_%s" % self.sim_id,
                np.array(self.profiles))

        print("Output Execution Path...")
        with open(self.exp_out_dir + "raw_%s.txt" % self.sim_id,
                  "w") as execution_file:
            ep_line = self.sitl.stdout.readline(0.01)
            while ep_line is not None:
                execution_file.write(ep_line)
                ep_line = self.sitl.stdout.readline(0.01)

        self.sitl.p.stdout.close()
        self.sitl.p.stderr.close()
        print("Simulation %s completed." % self.sim_id)
Ejemplo n.º 44
0
def setup_sitl():
    global sitl
    sitl = SITL()
    sitl.download('copter', '3.3')
    sitl.launch(sitl_args, await_ready=True, restart=True)
Ejemplo n.º 45
0
def setup_sitl():
    global sitl
    sitl = SITL('copter', '3.3-rc5')
    sitl.launch(sitl_args, await_ready=True, restart=True)
    description='Control Copter and send commands in GUIDED mode ')
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 args.connect:
    print "Starting copter simulator (SITL)"
    from dronekit_sitl import SITL
    sitl = SITL()
    sitl.download('copter', '3.3', verbose=True)
    sitl_args = [
        '-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353'
    ]
    sitl.launch(sitl_args, await_ready=True, restart=True)
    connection_string = 'tcp:127.0.0.1:5760'

# Connect to the Vehicle
print 'Connecting to vehicle on: %s' % connection_string
vehicle = connect(connection_string, wait_ready=True)


def arm_and_takeoff(aTargetAltitude):
    """
    Arms vehicle and fly to aTargetAltitude.
 def test_download_404(self):
     sitl = SITL()
     sitl.download('rocket', '1.123.098.123')
Ejemplo n.º 48
0
import sys
import os
import dronekit
import time
import re
from dronekit_sitl import SITL

print('launching SITL...')
s = SITL(path=os.path.join(os.path.dirname(__file__), 'build/out/apm'))
s.launch([], wd=os.path.join(os.path.dirname(__file__), 'build/test'), verbose=True)

print('connecting...')
time.sleep(1)
vehicle = dronekit.connect('tcp:127.0.0.1:5760')

print('waiting a fair amount of time...')
start = time.time()
while time.time() - start < 10:
    time.sleep(.1)

    line = s.stdout.readline(0.01)
    if line:
        sys.stdout.write(line)

    line = s.stderr.readline(0.01)
    if line:
        sys.stderr.write(line)

print('checking for messages...')
messages = False
def logme(*args):
Ejemplo n.º 49
0
def test_preserve_eeprom():
    # Start an SITL instance and change SYSID_THISMAV
    print "test"
    sitl = SITL()
    sitl.download('copter', '3.3')
    new_sysid = 10
    working_dir = tempfile.mkdtemp()
    sitl.launch(copter_args, verbose=True, await_ready=True, use_saved_data=True, wd=working_dir)
    vehicle = dronekit.connect("tcp:127.0.0.1:5760", wait_ready=True)
    print "Changing SYSID_THISMAV to {0}".format(new_sysid)
    while vehicle.parameters["SYSID_THISMAV"] != new_sysid:
        vehicle.parameters["SYSID_THISMAV"] = new_sysid
        time.sleep(0.1)
    time.sleep(6)   # Allow time for the parameter to go back to EEPROM
    print "Changed SYSID_THISMAV to {0}".format(new_sysid)
    vehicle.close()
    sitl.stop()

    # Now see if it persisted
    sitl.launch(copter_args, await_ready=True, use_saved_data=True, wd=working_dir)
    vehicle = dronekit.connect("tcp:127.0.0.1:5760", wait_ready=True)
    assert_equals(new_sysid, vehicle.parameters["SYSID_THISMAV"])
    vehicle.close()
    sitl.stop()