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 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 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 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 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 copter_sitl_guided_to():
"""
Returns a copter SITL instance for use in test cases
Status: hovering at 10m AGL
Mode: Guided
"""
# Launch a SITL using local copy of Copter 4
sitl = SITL(sitl_filename, instance=0)
sitl.launch(['--home=51.454531,-2.629158,589,353'])
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
# Connect to UAV to setup base parameters
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['SIM_SPEEDUP'] = 10 # speed up SITL for rapid startup
veh.parameters['FRAME_CLASS'] = 2 # I'm a hex
veh.close() # close vehicle instance after setting base parameters
# Stop and relaunch SITL to enable distance sensor and battery monitor
sitl.stop()
sitl.launch(['--home=51.454531,-2.629158,589,353'],
use_saved_data=True,
verbose=True)
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
# Connect again
veh = connect(sitl.connection_string(), vehicle_class=DroneTreeVehicle)
# wait until armable
while not veh.is_armable:
sleep(0.5)
veh.parameters['SIM_SPEEDUP'] = 5 # set sim speed for rapid take-off
veh.arm() # arm veh
veh.mode = VehicleMode('GUIDED') # set veh mode to GUIDED
veh.wait_simple_takeoff(10) # take-off to 10m AGL
veh.close() # close vehicle after guided takeoff
yield sitl
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
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 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 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()
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 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 copter_sitl_ground():
"""
Returns a copter SITL instance for use in test cases
Status: on ground
Mode: Stabilize
"""
# Launch a SITL using local copy of Copter 4
sitl = SITL(sitl_filename, instance=0)
sitl.launch(['--home=51.454531,-2.629158,589,353'])
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
print(sitl.connection_string())
# Connect to UAV to setup base parameters
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['SIM_SPEEDUP'] = 10 # speed up SITL for rapid startup
veh.parameters['FRAME_CLASS'] = 2 # I'm a hex
veh.close() # close veh instance after setting base parameters
# Stop and relaunch SITL to enable distance sensor and battery monitor
sitl.stop()
sitl.launch(['--home=51.454531,-2.629158,589,353'],
use_saved_data=True,
verbose=True)
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
yield sitl
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
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)
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 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 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 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_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()
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
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
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)
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)
class APMSim(object):
existing = []
usedINum = mav.manager.list()
@staticmethod
def nextINum():
port = mav.nextUnused(APMSim.usedINum, range(0, 254))
return port
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)
@staticmethod
def create():
index = APMSim.nextINum()
try:
result = APMSim(index)
return result
except Exception as ee:
APMSim.usedINum.remove(index)
raise ee
def setParamAndRelaunch(self, key, value):
wd = self.sitl.wd
v = connect(self.connStr, wait_ready=True)
v.parameters.set(key, value, wait_ready=True)
v.close()
self.sitl.stop()
self.sitl.launch(self.args,
await_ready=True,
restart=True,
wd=wd,
use_saved_data=True)
v = connect(self.connStr, wait_ready=True)
# This fn actually rate limits itself to every 2s.
# Just retry with persistence to get our first param stream.
v._master.param_fetch_all()
v.wait_ready()
actualValue = v._params_map[key]
assert actualValue == value
v.close()
def close(self):
self.sitl.stop()
APMSim.usedINum.remove(self.iNum)
APMSim.existing.remove(self)
@staticmethod
def clean():
for sitl in APMSim.existing:
sitl.stop()
APMSim.existing = []
'''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 execute_mission(mission, time_limit):
# TODO: allow 'binary' and 'speedup' to be passed as arguments
home = [40.071374969556928, -105.22978898137808, 1583.702759, 246]
speedup = 1
binary = '/experiment/source/build/sitl/bin/ardurover'
vehicle = sitl = None
try:
model_arg = '--model=rover'
home_arg = '--home={}'.format(','.join(map(str, home)))
defaults_arg = "--defaults=/experiment/source/Tools/autotest/default_params/rover.parm"
sitl = SITL(binary)
sitl.launch([home_arg, model_arg, defaults_arg],
verbose=True,
await_ready=True,
restart=False,
speedup=speedup)
# launch mavproxy -- ports aren't being forwarded!
# sim_uri = sitl.connection_string()
# mavproxy_cmd = \
# "mavproxy.py --console --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out localhost:14550 & /bin/bash"
# subprocess.call(mavproxy_cmd, shell=True)
# time.sleep(1)
# dronekit is broken! it always tries to connect to 127.0.0.1:5760
print("try to connect to vehicle...")
dronekit_connects_to = 'tcp:127.0.0.1:5760'
vehicle = dronekit.connect(dronekit_connects_to, wait_ready=True)
# TODO: add time limit
while not vehicle.is_armable:
time.sleep(0.2)
# TODO: add time limit
# arm the rover
vehicle.armed = True
while not vehicle.armed:
print("waiting for the rover to be armed...")
time.sleep(0.1)
vehicle.armed = True
# TODO: add time limit
issue_mission(vehicle, mission)
# trigger the mission by switching the vehicle's mode to "AUTO"
vehicle.mode = VehicleMode("AUTO")
# monitor the mission
last_wp = vehicle.commands.count
mission_complete = [False]
waypoints_visited = []
try:
def on_waypoint(self, name, message):
wp = int(message.seq)
# print("Reached WP #{}".format(wp))
# record the (relevant) state of the vehicle
waypoints_visited.append((wp, snapshot(vehicle)))
if wp == last_wp:
mission_complete[0] = True
vehicle.add_message_listener('MISSION_ITEM_REACHED', on_waypoint)
# wait until the last waypoint is reached or a time limit has expired
time_started = timer()
while not mission_complete[0]:
time_elapsed = timer() - time_started
if time_limit and time_elapsed > time_limit:
return waypoints_visited[:]
time.sleep(0.2)
return waypoints_visited[:]
# remove the listener
finally:
vehicle.remove_message_listener('MISSION_ITEM_REACHED',
on_waypoint)
# close the connection and shutdown the simulator
finally:
if vehicle:
vehicle.close()
if sitl:
sitl.stop()
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)
)
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.
"""
print "Basic pre-arm checks"
# Don't let the user try to arm until autopilot is ready
while not vehicle.is_armable:
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)
print('Connecting to vehicle2 on: %s' % connection_string2)
vehicle2 = connect(connection_string2, wait_ready=True)
def arm_and_takeoff(vehicle, aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
def main():
parser = argparse.ArgumentParser(description='Gps navigation of a single UAV.')
parser.add_argument("x",help ="+x -> EAST and -x -> WEST",type=float,metavar="X")
parser.add_argument("y",help="+y -> NORTH and -y -> SOUTH",type=float,metavar="Y")
parser.add_argument("d",help="total distance by single uav",type=int,metavar="distance")
parser.add_argument('--connect',
help="Vehicle connection target string. If not specified, SITL automatically started and used.")
parser.add_argument('--baudrate',type=int,
help="Vehicle baudrate settings specify 57600 when connecting with radio. If not specified, SITL automatically started and used.",default=115200)
args = parser.parse_args()
connection_string = args.connect
sitl = None
#Start SITL if no connection string specified
if not connection_string or not args.baudrate:
from dronekit_sitl import SITL
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=53.367063,-3.109593,0,0']
sitl.launch(sitl_args, await_ready=True, restart=True)
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,baud=args.baudrate)
quad = QuadController(vehicle)
home_location = quad.home_position()
init_pos = altered_position(home_location,args.x,args.y)
start = init_pos
end = altered_position(start,0,args.d)
print "Basic pre-arm checks"
# Don't try to arm until autopilot is ready
while not quad.is_armable:
print " Waiting for vehicle to initialise..."
time.sleep(1)
print "Arming motors"
# Copter should arm in GUIDED mode
quad.mode = "GUIDED"
quad.arm = True
# Confirm vehicle armed before attempting to take off
while not quad.arm:
print " Waiting for arming..."
time.sleep(1)
print "Vehicl armed status: " + str(quad.arm)
print "taking off .."
print quad.mode
quad.takeoff()
print "going to first way point..."
quad.goto(init_pos)
if not quad.goto(init_pos,groundspeed=1):
print "some event has gone horribly wrong"
return
while not QuadController.get_distance((quad.current_location.lat,quad.current_location.lon),init_pos):
time.sleep(0.001)#to avoid consuming much CPU
print "arrived at first way point"
#print "settting yaw"
#quad.set_yaw(90)
#time.sleep(5)
#print quad.heading
print "following path..."
path = QuadController.path(start,end)
for p in path[1:]:
print "going to Lat:%.6f, Lon:%.6f" %(p[0],p[1])
if not quad.goto((p[0],p[1])):
print "some event has gone horribly wrong"
return
while not QuadController.get_distance((quad.current_location.lat,quad.current_location.lon),(p[0],p[1])):
time.sleep(0.01)
print quad.current_location
print "vehicle returning to launch..."
quad.mode = "RTL"
def setup_sitl():
global sitl
sitl = SITL('copter', '3.3-rc5')
sitl.launch(sitl_args, await_ready=True, restart=True)
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)
""""
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 setup_solo_sitl():
global sitl
sitl = SITL()
sitl.download('solo', '1.2.0')
sitl.launch(sitl_args, await_ready=True, restart=True)
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.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)
# 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 = self.webserver + route
try:
r = requests.post(url, self.get_drone_data())
self.logger.info("\nServer Responded With: " + str(r.status_code) +
" " + str(r.text) + "\n")
except requests.HTTPError:
self.logger.info(str(requests.HTTPError))
def get_data_from_server(self, route):
url = self.webserver + route + "/" + str(self.id)
try:
r = requests.get(url)
self.logger.info("\nServer Responded With: " + str(r.status_code) +
" " + str(r.text) + "\n")
return Config(json.loads(r.text))
except requests.HTTPError:
self.logger.info("HTTP " + str(requests.HTTPError))
return "NO_DATA"
# =============================VEHICLE INFORMATION FUNCTIONS=================================================
# =======================================================================================================
def get_drone_data(self):
# creates a dictionary object out of the drone data
return type(
'obj', (object, ), {
"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()
string_to_print = "Drone ID: " + str(
drone_params.id
) + "\nDrone IP: " + str(drone_params.ip) + "\nDrone A/S: " + str(
drone_params.airspeed
) + "\nDrone Location: (" + str(drone_params.latitude) + ", " + str(
drone_params.longitude) + ", " + str(
drone_params.altitude) + ")" + "\nDrone Armed: " + str(
drone_params.armed) + "\nDrone Mode: " + drone_params.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):
# print("ENTERING FORMATION:", formationName)
self.move_to_formation(10)
# print("About to Enter Loop:", self.vehicle.mode.name)
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):
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 in enumerate(swarm_params.Drones):
if swarm_params.Drones[idx].id == droneID:
while (swarm_params.Drones[idx].altitude <= altitude * 0.95):
swarm_params = self.get_data_from_server("/Swarm")
print("Waiting for Drone: " +
str(swarm_params.Drones[idx].id) + " to reach " +
str(altitude))
time.sleep(1)
self.logger.info("Drone: " + swarm_params.Drones[idx].id +
" reached " + str(altitude) + "...")
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 goto(self):
pass
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 to_string(self):
return ("id: " + self.id + ", " + "ip" + self.ip + ", " +
"self.webserver" + self.webserver)
from dronekit_sitl import SITL
import mission
args = ['--model', 'plane',]
sitl = SITL() # load a binary path (optional)
sitl.download("plane", "3.8.0", verbose=False)
sitl.launch(args, verbose=False, await_ready=True, restart=False)
sitl.block_until_ready(verbose=False)
mission.start_flight(sitl.connection_string())
print(sitl.position)
code = sitl.complete(verbose=False)
sitl.poll()
sitl.stop()
def copter_sitl_auto_to():
"""
Returns a copter SITL instance for use in test cases
Status: starting auto takeoff(to 20m AGL), 13 waypoints
Mode: Auto
"""
# Launch a SITL using local copy of Copter 4
sitl = SITL(sitl_filename, instance=0)
sitl.launch(['--home=51.454531,-2.629158,589,353'])
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
# Connect to UAV to setup base parameters
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['SIM_SPEEDUP'] = 10 # speed up SITL for rapid startup
veh.parameters['FRAME_CLASS'] = 2 # I'm a hex
veh.close() # close vehicle instance after setting base parameters
# Stop and relaunch SITL to enable distance sensor and battery monitor
sitl.stop()
sitl.launch(['--home=51.454531,-2.629158,589,353'],
use_saved_data=True,
verbose=True)
# Wait for sitl to be ready before passing to test cases
sitl.block_until_ready()
# Connect again
veh = connect(sitl.connection_string(),
wait_ready=True,
vehicle_class=DroneTreeVehicle)
# upload an arbitrary mission
mission = MAVWPLoader()
filename = join(abspath(sys.path[0]), 'executable_mission.txt')
mission.load(filename)
# bit of a hack: fix the sequence number
for w in mission.wpoints:
w.seq = w.seq + 1
cmds = veh.commands
cmds.clear()
wplist = mission.wpoints[:]
# add a dummy command on the front
# just the first WP used as a placeholder
# upload seems to skip the first WP
cmds.add(wplist[0])
for wp in wplist:
cmds.add(wp)
cmds.upload()
# wait until armable
while not veh.is_armable:
sleep(0.5)
veh.parameters['SIM_SPEEDUP'] = 5 # set sim speed for rapid take-off
sleep(0.5) # wait for parameter change to take place
veh.arm() # arm vehicle
# Perform an auto take-off
veh.mode = VehicleMode('AUTO') # change mode to auto
veh.channels.overrides['3'] = 1700 # raise throttle to confirm T/O
# wait for take-off
while veh.location.global_relative_frame.alt < 0.5:
sleep(0.5)
veh.close() # close vehicle instance after auto takeoff
yield sitl
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
def setup_solo_sitl():
global sitl
sitl = SITL()
sitl.download('solo', '1.2.0')
sitl.launch(sitl_args, await_ready=True, restart=True)
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.
"""
print "Basic pre-arm checks"
# Don't try to arm until autopilot is ready
class Simulator:
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)
def signal_handler(self, signal, frame):
self.logger.debug("Exiting simulator")
self.stop()
sys.exit(0)
def stop(self):
self.stream_simulator.stop_thread()
self.logger.debug("Stopping server and control module")
self.server_thread.stop_thread()
self.control_thread.stop_thread()
self.logger.debug("Sleeping")
time.sleep(5.0)
self.logger.debug("Stopping SITL")
self.sitl.stop()
def setup_sitl():
global sitl
sitl = SITL()
sitl.download('copter', '3.3')
sitl.launch(sitl_args, await_ready=True, restart=True)
# mav.remove_message_listener(vehicle)
time.sleep(1) # mavlink not thread safe...
for thread in change_sysid_threads:
print("Waiting for thread...")
thread.join()
print("Sleeping a little to let SITLs go away...")
time.sleep(2)
print("Launching SITLs")
for i in range(0, len(sitls)):
sitl = sitls[i][0]
sitl_args = sitls[i][1]
sitl.launch(sitl_args,
await_ready=True,
restart=True,
use_saved_data=True,
wd=sitl.wd,
verbose=True)
# create another connection so a GCS can be connected for visualisation
if args.extra_connection:
hub_connection_strings.append(args.extra_connection)
# dronekit-python (us!) port:
connection_string = "udpout:localhost:2345"
hub_connection_strings.append("udpin:localhost:2345")
hub_should_quit = False
def mavlink_hub_target():
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 main():
"""
The Main function of this script.
"""
args = _parse_arguments()
util.log_init(
"sitl_A%s_%s.txt" % (args.id, util.get_latest_log("latest_sitl.txt")),
util.log_level[args.level])
shared.AGENT_ID = 'A%s' % args.id
shared.AGENT_COUNT = args.n
shared.CURRENT_ALGORITHM = args.algorithm
shared.AGENT_CHARACTER = args.character
shared.des_alt = args.alt
util.log_info("AGENT_ID = %s" % shared.AGENT_ID)
util.log_info("Algorithm: %s" % shared.CURRENT_ALGORITHM)
util.log_info("Agent type: %s" % shared.AGENT_CHARACTER)
print "Start simulator (SITL)"
sitl = SITL(args.pix) # initialize SITL with firmware path
if shared.AGENT_ID in start_loc:
sitl_args = ['--home=%s' % start_loc[shared.AGENT_ID]]
else:
sitl_args = ['--home=%s' % start_loc['FFF']]
# Pre-recorded coordinates.
#sitl_args = ['-I0', '--model', 'quad', '--home=31.301201,121.498192,9,353']
sitl.launch(sitl_args, await_ready=True, restart=True)
# Connect to the vehicle. (Spawn an instance of Vehicle named "vehicle")
# connection port is coded in the file name of the firmware like "ac3.4.5_port5760"
# use regular expression to search the string and extract port number
port = re.search(r'port\d{4}', args.pix)
port = re.search(r'\d{4}', port.group()).group()
print "Connecting to copter on: TCP: 127.0.0.1:%s" % port
copter = nav.connect('tcp:127.0.0.1:%s' % port, wait_ready=True, rate=20)
util.log_info("Copter connected. Firmware: %s" % copter.version)
if not args.xbee: # simulate XBee using ZeroMQ
[pub, sub] = comm.zmq_init(comm_port_list[shared.AGENT_ID],
comm_port_list)
subscriber_thread = comm.Subscriber(shared.AGENT_ID, sub)
subscriber_thread.start()
xbee = pub # make xbee the publisher
util.log_info("ZeroMQ initialzied.")
else: # use actual xbee ports
ser = serial.Serial(args.xbee, 57600)
xbee = comm.xbee_init(ser)
util.log_info("Xbee initialzed.")
info = "IFO,%s connected with firmware %s" % (shared.AGENT_ID,
copter.version)
comm.xbee_broadcast(xbee, info)
_add_listeners(copter)
takeoff_thread = nav.Takeoff(copter, xbee, shared.des_alt, 3)
purge_thread = comm.Purge(shared.neighbors)
broadcast_thread = comm.Broadcast(shared.AGENT_ID, copter, xbee)
flocking_thread = _choose_algorithm(copter, xbee, shared.neighbors)
takeoff_thread.start()
takeoff_thread.join() # wait until takeoff procedure completed
if shared.status['airborne']: # only execute the threads when airborne
util.log_info("Copter is airborne, starting threads.")
broadcast_thread.start()
purge_thread.start()
flocking_thread.start()
# main loop
while True:
try:
time.sleep(.2)
except KeyboardInterrupt:
break
if shared.status['airborne']:
# echo exiting status
if shared.status['exiting']:
info = "IFO,%s %s-ing." % (shared.AGENT_ID,
shared.status['command'])
comm.xbee_broadcast(xbee, info)
util.log_info(info)
# if an rtl or land command is received, kill flocking and set the `exiting` flag
elif shared.status['command'] == 'RTL' or shared.status[
'command'] == 'LAND':
shared.status['thread_flag'] |= shared.FLOCKING_FLAG
nav.set_mode(copter, shared.status['command'])
shared.status['exiting'] = True
if not flocking_thread.is_alive(): # break the loop if finished
break
nav.wait_for_disarm(copter) # wait for disarm
comm.xbee_broadcast(xbee, 'IFO,%s terminated.' % shared.AGENT_ID)
# clean up
purge_thread.stop()
while purge_thread.is_alive():
util.log_info('Waiting for purge to shutdown')
purge_thread.join(3)
util.log_info('Purge killed.')
broadcast_thread.stop()
while broadcast_thread.is_alive():
util.log_info('Waiting for broadcast to shutdown')
broadcast_thread.join(3)
util.log_info('Broadcast killed.')
copter.close()
util.log_info("Copter shutdown.")
if args.xbee:
xbee.halt()
ser.close()
util.log_info("Xbee and serial closed.")
else:
subscriber_thread.stop()
while subscriber_thread.is_alive():
util.log_info('Waiting for Subscriber to shutdown')
subscriber_thread.join(3)
util.log_info('Subscriber killed.')
sitl.stop()
util.log_info("SITL shutdown.")