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 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_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 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()
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.
"""
print("Basic pre-arm checks")
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)
connection_string = sitl.connection_string()
vehicle = dronekit.connect(connection_string, wait_ready=True)
new_compass_use = 10
sitl.block_until_ready()
sitl.poll()
sitl.using_sim
sitl.stop()
sitl.poll()
sitl.launch(copter_args)
sitl.stop()
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)
connection_string = sitl.connection_string()
vehicle = dronekit.connect(connection_string, wait_ready=True)
new_compass_use = 10
sitl.block_until_ready()
sitl.poll()
sitl.using_sim
sitl.stop()
sitl.poll()
sitl.launch(copter_args)
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
}
],
},
]
# ready data for many dronekit-sitl processes
simulation_count = 2
for i in range(0,simulation_count):
print("Creating simulator (SITL) %d" % (i,))
from dronekit_sitl import SITL
sitl = SITL(instance=i, path=args.binary, defaults_filepath=args.defaults, gdb=True)
# sitl.download('copter', '3.3', verbose=True)
sitls.append(sitl)
hub_connection_strings.append(sitl.connection_string())
# start the SITLs one at a time, giving each a unique SYSID_THISMAV
def set_params_target(i, new_sysid, connection_string):
lat = -35.363261
lon = 149.165230
sitl_args = ['--model', 'quad', '--home=%s,%s,584,353' % (lat,lon,) ]
print("%d: Launching SITL (%s)" % (i,str(sitl_args)))
sitls[i].launch(sitl_args, await_ready=True, verbose=True, speedup=50,)
print("Sleeping a little here")
time.sleep(5)
print("%d: Connecting to its vehicle 1" % (i,))
vehicle = dronekit.connect(connection_string, wait_ready=True, target_system=1, heartbeat_timeout=60)
while vehicle.parameters["SYSID_THISMAV"] != new_sysid:
print("%d: Resetting its SYID_THISMAV to %d" % (i, new_sysid,))
vehicle.parameters["SYSID_THISMAV"] = new_sysid
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()
