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
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")
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
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)
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)
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 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()
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()
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
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
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,
)
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)
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
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
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")
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)
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()
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
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)
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)
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)
'''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)
def setup_solo_sitl():
global sitl
sitl = SITL()
sitl.download('solo', '1.2.0')
sitl.launch(sitl_args, await_ready=True, restart=True)
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
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()
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
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)
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
"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)
},
]
# 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):
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)
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, ))
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)
""""
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)
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'
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
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')
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)
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")
#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.
"""
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)
def setup_sitl():
global sitl
sitl = SITL()
sitl.download('copter', '3.3')
sitl.launch(sitl_args, await_ready=True, restart=True)
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')
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):
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()