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