def generate_search_coordinates(search_tasks): points = search_tasks[0] ''' Indexes of "points": 0 = North West, 1 = North East, 2 = South East, 3 = South West 0---------------1 | | | | | | 3---------------2 ''' # This is the North West corner of the search area starting_point = points[0] # How far south the drone shall search lowest_lat = points[3].lat # How far west the drone shall go lowest_lon = points[0].lon # How far east the drone shall go highest_lon = points[1].lon paths = [] current_location = starting_point next_location = None moveLatDirection = False while current_location.lat > lowest_lat: if next_location == None: # Start in northwest corner next_location = starting_point else: if moveLatDirection: next_location = current_location next_location = dronekit.LocationGlobal( next_location.lat - 0.0001, next_location.lon, 10) moveLatDirection = False else: # If we are max east, move left if current_location.lon >= highest_lon: next_location = dronekit.LocationGlobal( current_location.lat, lowest_lon, 10) # Move right else: next_location = dronekit.LocationGlobal( current_location.lat, highest_lon, 10) moveLatDirection = True paths.append(next_location) current_location = next_location return paths
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`. """ 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 dronekit.LocationGlobal: targetlocation = dronekit.LocationGlobal(newlat, newlon, original_location.alt) elif type(original_location) is dronekit.LocationGlobalRelative: targetlocation = dronekit.LocationGlobalRelative( newlat, newlon, original_location.alt) else: raise Exception("Invalid Location object passed") return targetlocation
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. For more information see: http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters """ 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 new_lat = original_location.lat + (dLat * 180 / math.pi) new_lon = original_location.lon + (dLon * 180 / math.pi) if type(original_location) is dronekit.LocationGlobal: target_location = dronekit.LocationGlobal(new_lat, new_lon, original_location.alt) elif type(original_location) is dronekit.LocationGlobalRelative: target_location = dronekit.LocationGlobalRelative( new_lat, new_lon, original_location.alt) else: raise Exception("Invalid Location object passed") return target_location
def notify_mission_end(self) -> None: # TODO for the demo, just check that end location is home location? assert self._connection lat_expected, lon_expected, alt_expected = \ self._mission.home_location[0:3] loc_expected: dronekit.LocationGlobal = \ dronekit.LocationGlobal(lat_expected, lon_expected, alt_expected) loc_actual: dronekit.LocationGlobal = \ self._connection.location.global_frame # have we visited all waypoints? visited_wps = self._visited_wps expected_wps = self._mission.waypoints if visited_wps == expected_wps: self._status._visited_all_wps = True else: logger.debug("failed to visit all waypoints\n" f"* visited waypoints: {visited_wps}\n" f"* expected waypoints: {expected_wps}") # check if the expected and actual location are roughly the same dist_to_home = distance_metres(loc_expected, loc_actual) logger.debug(f"distance to home: {dist_to_home:.3f} metres") if dist_to_home < DIST_APPROX_SAME: self._status._reached_home = True
def main(): target = "udpin:0.0.0.0:14550" vehicle = dronekit.connect(target, wait_ready=True) vehicle.mode = dronekit.VehicleMode("GUIDED") a_location = dronekit.LocationGlobal(35.80004274469795, -78.77133535151027, 154.3443094818955) vehicle.simple_goto(a_location, groundspeed=2.0) vehicle.close()
def calculate_marker_matrix(marker_list): matrix = [[0.0 for x in range(len(marker_list))] for y in range(len(marker_list))] for i in range(len(marker_list)): point1 = json.loads(marker_list[i]) for j in range(i, len(marker_list)): if i == j: matrix[i][j] = -1 continue point2 = json.loads(marker_list[j]) matrix[i][j] = get_distance_meters(dronekit.LocationGlobal(float(point1['lat']), float(point1['lon']), 0), dronekit.LocationGlobal(float(point2['lat']), float(point2['lon']), 0))
def destinationPoint(lat_org, lon_org, d=2, teta=180): "Methode permettant de calculer une position gps a partir d'une position GPS d'une distance et d'un cap" R = 6371009 teta *= pi / 180 lat_org *= pi / 180 lon_org *= pi / 180 lat = asin( sin(lat_org) * cos(d / R) + cos(lat_org) * sin(d / R) * cos(teta)) lon = lon_org + atan2( sin(teta) * sin(d / R) * cos(lat_org), cos(d / R) - sin(lat_org) * sin(lat)) lat *= 180 / pi lon = ((lon + 540) % 360 - 180) * 180 / pi print("targeted: " + str(lat) + " " + str(lon)) return dronekit.LocationGlobal(lat, lon, 0) #return a Location Object
def get_location_metres(original_location, pos): earth_radius = 6378137.0 #Radius of "spherical" earth #Coordinate offsets in radians dLat = pos[0]*100/earth_radius dLon = pos[1]*100/(earth_radius*math.cos(math.pi*original_location[0]/180)) #New position in decimal degrees newlat = original_location[0] + (dLat * 180/math.pi) newlon = original_location[1] + (dLon * 180/math.pi) targetlocation=dronekit.LocationGlobal(newlat, newlon,original_location[2]+pos[2]) #targetlocation=dronekit.LocationGlobalRelative(newlat, newlon,original_location[2]+pos[2]) return targetlocation;
def is_inside(check_point, obstacles): """ Checks if a point is inside any obstacle """ north_pole = dronekit.LocationGlobal(90.0000, 0.0000, altitude) for obstacle in obstacles: i = 0 n = len(obstacle) for k, point in enumerate(obstacle): f = doIntersect(obstacle[k], obstacle[(k + 1) % n], check_point, north_pole) if f: i += 1 # If the line segment from the point to the north pole intersects with an uneven number of sides of an obstacle, # the point is inside that obstacle if not i % 2 == 0: return True, obstacle return False, None
def __init__(self, name, instance_no=0, debug=True, speedup=5.0, binary=binary_path, params=params_path, home=home_default): """Creates a drone with given name and default state. """ self.name = name self.ino = instance_no self.debug = debug self.logging = False self.logfile = None self.speedup = int(speedup) if speedup is not None else None self.binary = binary self.params = params self.delta_x = 0 self.delta_y = 0 self.origin = dronekit.LocationGlobal(hd_latitude, hd_longitude, hd_altitude) self.home = home self.pipeincr = 10 * self.ino if self.debug: sys.stderr.write( init_squark.format(self.name, self.ino, self.binary, self.params, self.debug, self.speedup)) #<iam thinks these are obsolete; or should be> self.v = 0 self.e = 10.0 #</iam thinks these are obsolete; or should be> self.vehicle = None self.mavproxy = None self.sitl = None
def makeOffset(prev_point, point, next_point, r): """ Adds an offset to a point There are still mistakes, it needs to be corrected """ x1, y1 = prev_point.lon, prev_point.lat x2, y2 = point.lon, point.lat x3, y3 = next_point.lon, next_point.lat # Add a little offset so that there are no problems with the calculations if x2 - x1 == 0: x1 += 0.000001 if y2 - y1 == 0: y1 += 0.000001 if x3 - x2 == 0: x3 += 0.000001 if y3 - y2 == 0: y3 += 0.000001 # The sides of the triangle created by the points a = math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)) b = math.sqrt((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2)) c = math.sqrt((x1 - x3) * (x1 - x3) + (y1 - y3) * (y1 - y3)) theta = math.acos( (a * a + b * b - c * c) / (2 * a * b)) # The angle at the point theta_half = theta / 2 # So that the offset is positioned centrally # Angle between the line through the previous point and the point, and the x axis new_x_angle = math.atan2(y2 - y1, x2 - x1) # Add the offset on the x axis and rotate it first to align it with the line through the previous point # and the x axis, and then for theta half x_final, y_final = rotate_around_point(x2 + r, y2, x2, y2, new_x_angle + theta_half) return dronekit.LocationGlobal(y_final, x_final, altitude)
def message_received(client, server, message): # First position indicates message type message_type = ast.literal_eval(message)[0] # Adding new tasks that user draw on map if (message_type == 0): # Line task tasks = ast.literal_eval(message)[1] lines = tasks['line'] lines = [[ dronekit.LocationGlobal(point["lat"], point["lng"], altitude) for point in line ] for line in lines] global u_tasks u_tasks.extend(lines) # Pickup task pickup_tasks_temp = [] for pickup_task in tasks['pickup']: line = pickup_task["line"] weight = pickup_task["weight"] line = [ dronekit.LocationGlobal(line[0]["lat"], line[0]["lng"], altitude), dronekit.LocationGlobal(line[1]["lat"], line[1]["lng"], altitude), weight, "weight" ] pickup_tasks_temp.append(line) u_tasks.extend(pickup_tasks_temp) # Search task search_tasks = tasks['search'] search_tasks_temp = [] # Convert to DroneKit's Global Position Object for task in search_tasks: task_temp = [] for point in task: point = dronekit.LocationGlobal(point[0], point[1], altitude) task_temp.append(point) search_tasks_temp.append(task_temp) search_tasks = search_tasks_temp if len(search_tasks) > 0: search_tasks = Drone_Services.generate_search_coordinates( search_tasks) u_tasks.append(search_tasks) # Obstacles obstacles_from_client = tasks['obstacles'] obstacles_temp = [] for obstacle in obstacles_from_client: point_temp = [] for point in obstacle: point = dronekit.LocationGlobal(point[0], point[1], altitude) point_temp.append(point) obstacles_temp.append(point_temp) global obstacles obstacles.extend(obstacles_temp) # New Parameter recieved if (message_type == 1): """ Change Parameters """ vehicle_id = ast.literal_eval(message)[1] vehicle = Drone_Services.find_vehicle_by_id(vehicles, vehicle_id) new_max_speed = ast.literal_eval(message)[2] vehicle.max_speed = float(new_max_speed) vehicle.groundspeed = float(new_max_speed) new_max_battery_time = ast.literal_eval(message)[3] vehicle.max_battery_time = float(new_max_battery_time) vehicle.current_battery = float(new_max_battery_time) vehicle.start_up_time = time.time() new_max_carry_weight = ast.literal_eval(message)[4] vehicle.max_carry_weight = float(new_max_carry_weight) # Reacharge battery if (message_type == 2): vehicle_id = ast.literal_eval(message)[1] vehicle = Drone_Services.find_vehicle_by_id(vehicles, vehicle_id) vehicle.current_battery = vehicle.max_battery_time vehicle.start_up_time = time.time() # Open new cygwin terminal if (message_type == 3): subprocess.call('start cd C:\cygwin ^& cygwin', shell=True) # Start new simulated drone if (message_type == 4): Drone_Services.start_new_simulated_drone(vehicles, UAV_BASE_PORT, server) # Delete mission and stop vehicles if (message_type == 5): for vehicle in vehicles: vehicle.nextlocations = [] free_vehicles = [] obstacles = [] u_tasks = [] vehicle.simple_goto( dronekit.LocationGlobal(vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, altitude))
def get_bullshit_location(self): return dronekit.LocationGlobal(random.gauss(0, 10), random.gauss(0, 10), random.gauss(20, 5))
import dronekit as dk import dronekit_sitl as dks from mismeth import * import pymavlink as mav # WP 0 52.815022, -4.127964 sitl = dks.start_default(52.815022, -4.127964) home = dk.LocationGlobal(52.815022, -4.127964, 590) connection_string = sitl.connection_string() vehicle = dk.connect(connection_string, wait_ready=True) cmds = vehicle.commands cmds.download() cmds.wait_ready() ############## Send confirmation to air pi ##################################### ################################################################################ vehicle.mode = dk.VehicleMode("GUIDED") alt = 7 #vehicle.airspeed = 10 vehicle.groundspeed = 15 #52.814406-4.129175 #loc1 = get_location_metres(vehicle.location.global_frame,50,50) loc1 = dk.LocationGlobalRelative(52.814406, -4.129175, alt) loc2 = dk.LocationGlobalRelative(52.811242, -4.128031, alt) loc3 = dk.LocationGlobalRelative(52.811078, -4.125119, alt)
def start_fallback(data_list): fb = FallbackClass() low_latency_counter = 0 is_fallback_active = 1 # while True: while True: if (data_list[5] == 1): is_fallback_activated = 1 if (is_fallback_active == 1): # print("fallback_counter = " + str(fb.fallback_counter)) # print("low_latency_counter = " + str(low_latency_counter)) #print("Operation Time: " + str(i) + " sec") #cur_gps_lat_str = str(testing_gps[i][0]) # probably how we get gps by command like vehicle.getgps() or something like this #cur_gps_lon_str = str(testing_gps[i][1]) cur_gps_lat = data_list[6].location.global_frame.lat cur_gps_lon = data_list[6].location.global_frame.lon #delay_cur_gps = float(testing_latency[i]) # the socket delay in seconds for the last message. need to change to real parameter delay_cur_gps = float(data_list[2]) # reak delay if int(delay_cur_gps) != -1: print("Current latency: " + str(delay_cur_gps) + " ms.") print("Current GPS: " + cur_gps_lat_str + ", " + cur_gps_lon_str) #print(data_list) # delay_cur_gps = int(delay_cur_gps_str) if delay_cur_gps < HOT_SPOT_CALC_DELAY: low_latency_counter = low_latency_counter + 1 # print("Low latency (" + str(delay_cur_gps) + ")") hot_spot_calc(fb, fb.hot_spot_list, fb.distance_list, cur_gps_lat, cur_gps_lon) if low_latency_counter > 3: fb.fallback_counter = 0 low_latency_counter = 0 elif delay_cur_gps >= HOT_SPOT_CALC_DELAY: # print("High latency (" + str(delay_cur_gps) + ")") fb.fallback_counter = fb.fallback_counter + 1 low_latency_counter = 0 if (fb.fallback_counter >= FALLBACK_DELAY) | ( data_list[1] == 0): # number of seconds to activate fallback print("Fallback Activated.") print("Hot spot GPS destination: " + str(fb.hot_spot_to_return)) if fb.hot_spot_to_return[0] == "": fb.hot_spot_to_return[0] = data_list[6].home_location.lat fb.hot_spot_to_return[1] = data_list[6].home_location.lon fallback_lat = fb.hot_spot_to_return[0] fallback_lon = fb.hot_spot_to_return[1] fallback_location = dronekit.LocationGlobal( fallback_lat, fallback_lon, alt=40) # set location object for dronekit fb.fallback_counter = 0 data_list[1] = 0 data_list[3] = fallback_lat data_list[4] = fallback_lon data_list[5] = 0 is_fallback_active = 0 #data_list[6].mode = dronekit.VehicleMode("STABILIZE") #data_list[6].armed = True #while data_list[6].armed != True: #data_list[6].armed = True #data_list[6].flush() #print ("mode?", str(data_list[6].mode)) #print ("is armable?", data_list[6].is_armable) #print("is armed?", data_list[6].armed) #print ("not armed yet") #sleep(1) #print("is armed?", data_list[6].armed) data_list[6].mode = dronekit.VehicleMode("GUIDED") #print("is guided?", str(data_list[6].mode)) #data_list[6].flush() # REMEMBER TO SWITCH TO GUIDED MODE # IF SIGNAL IS RETRIEVED THEN SWITCH TO STABLE #data_list[6].airspeed=300 try: data_list[6].simple_goto( fallback_location, airspeed=12 ) # simple_goto(location, airspeed=None, groundspeed=None) | speed = 8 m/s except: data_list[6].mode = dronekit.VehicleMode("RTL") print("Unexpected Error: Please try again.") data_list[7] = 1 break #point1 = dronekit.LocationGlobal(-35.361354, 149165218, 20) #data_list[6].simple_goto(point1, 300) # simple_goto(location, airspeed=None, groundspeed=None) #sleep(10) #data_list[6].mode = dronekit.VehicleMode("RTL") # print("\n") sleep(1) #################################
def processJSON(self, json): if (not self.validated): if (json["type"] == "validate"): # If a salted hash is included at all if "token" in json: for entry in self.socket.validation_db: print("---") print("SALTED HASH FROM CLIENT: " + json["token"]) print("SALTED HASH FROM LOCAL DB: " + entry["saltedHash"]) # If the salted hash from the client matches one we have on record if entry["saltedHash"] == json["token"]: self.validated = True self.socket.write_message( '{"type":"validate", "status":true}') # Add them to the list of validated clients validated_clients.append(self.socket) print("Client #" + str(self.socket.id) + " validated!") break else: self.socket.write_message( '{"type":"validate", "status":false}') print( "Client #" + str(self.socket.id) + " attempted validation, but failed due to invalid token!" ) else: self.validated = False self.socket.write_message( '{"type":"validate", "status":false}') print( "Client #" + str(self.socket.id) + " attempted validation, but failed due to invalid token!" ) else: # Inform the client they are not validated self.socket.write_message( '{"type":"validate", "status":false}') else: # The actual API begins here # First we make sure we're actually connected to our vehicle by checking the vehicleWrapper class if (self.vehicleWrapper.connected): if (json["type"] == "get"): # Eventually this will be an IF chain for returning individual things if "listener" in json: print("Listener key present!") if (json["listener"] == None): print("Canceling listener...") try: self.listener.stop() self.listener = None except AttributeError: print("No listener to cancel! Ignoring...") # As long as it's a number as expected, and we don't already have a listener running elif (type(json["listener"]) is float or type(json["listener"]) is int): self.listener = tornado.ioloop.PeriodicCallback( self.returnAttributes, float(json["listener"])) self.listener.start() print("Starting listener...") else: reply_dict = self.fetchAttributes() reply_json = json_lib.dumps(reply_dict) self.socket.write_message(reply_json) # If the client just sends a get request with a blank list of attributes, assume they want ALL attributes # Setting attributes (Sanity checking is CRITICAL here) elif (json["type"] == "set"): requestedAttributes = json["attributes"] # Iterate over each item we want to set, one at a time for rootKey, value in requestedAttributes.items(): # for safety's sake we don't make any attempts to streamline, we have an IF statement for each root key in the attributes object if (rootKey == "armed"): # Now we check all possible values if (type(value) is bool): self.vehicleWrapper.vehicle.armed = value else: self.sendError( "set", "Type error while setting 'armed'.") elif (rootKey == "mode"): # Capitalize it just for good measure value = value.upper() if (value == "LOITER"): self.vehicleWrapper.vehicle.mode = dronekit.VehicleMode( "LOITER") elif (value == "STABILIZE"): self.vehicleWrapper.vehicle.mode = dronekit.VehicleMode( "STABILIZE") elif (value == "ALT_HOLD"): self.vehicleWrapper.vehicle.mode = dronekit.VehicleMode( "ALT_HOLD") elif (value == "GUIDED"): self.vehicleWrapper.vehicle.mode = dronekit.VehicleMode( "GUIDED") elif (value == "AUTO"): self.vehicleWrapper.vehicle.mode = dronekit.VehicleMode( "AUTO") elif (value == "RTL"): self.vehicleWrapper.vehicle.mode = dronekit.VehicleMode( "RTL") elif (value == "BRAKE"): self.vehicleWrapper.vehicle.mode = dronekit.VehicleMode( "BRAKE") else: self.sendError( "set", "Client attempted to set unsupported or invalid flight mode! Valid modes are LOITER, STABILIZE, ALT_HOLD, GUIDED, AUTO, RTL, and BRAKE." ) elif (rootKey == "location"): # Only home is settable, but we use this For loop to catch any doofus trying to set a global location this way for location_type, location_data in value.items(): if location_type == "home": if "lat" in location_data and "lon" in location_data and "alt" in location_data: self.vehicleWrapper.vehicle.home_location = dronekit.LocationGlobal( location_data["lat"], location_data["lon"], location_data["alt"]) else: self.sendError( "KeyError", "Location object is missing one or more mandatory components!" ) # Send error about location formatting else: self.sendError( "TypeError", "Cannot set such location objects! Only home is settable." ) #Send error about not being able to set other location types elif (rootKey == "groundspeed"): if type(value) is float or type( value) is int or type(value) is long: # We're good self.vehicleWrapper.vehicle.groundspeed = float( value) else: # We're not good, the client sent something that's not a number self.sendError( "TypeError", "Requested groundspeed value is not a valid float/int/long!" ) elif (rootKey == "airspeed"): if type(value) is float or type( value) is int or type(value) is long: # We're good self.vehicleWrapper.vehicle.airspeed = float( value) else: # We're not good, the client sent something that's not a number self.sendError( "TypeError", "Requested airspeed value is not a valid float/int/long!" ) elif (rootKey == "parameters"): for param, setting in value.items(): if type(setting) is float or type( setting) is int or type( setting) is long: if str( param ) in self.vehicleWrapper.vehicle.parameters.keys( ): # Make sure we're dealing with a number print("Attempting to set " + param + " to " + str(setting)) self.vehicleWrapper.vehicle.parameters[ str(param)] = float(setting) else: self.sendError( "KeyError", "You tried to set a parameter (" + str(param) + ") that is not available on this vehicle!" ) else: self.sendError( "TypeError", "The requested value for " + param + " was of the wrong type!") elif (rootKey == "channels"): for key, val in value.items(): if key == "overrides": overridesToSet = value["overrides"] invalid = False for ch, override in overridesToSet.items(): # Make sure this is a channel the vehicle actually has if (ch not in self.vehicleWrapper. vehicle.channels.keys()): invalid = True if (invalid == False): try: self.vehicleWrapper.vehicle.channels.overrides = overridesToSet except TypeError: self.sendError( "TypeError", "You tried to set a channel override with the wrong type of variable!" ) else: self.sendError( "KeyError", "You sent an invalid channel override!" ) else: self.sendError( "KeyError", "You are using improper syntax for setting channel overrides. You should only attempt to manipulate the overrides object." ) elif (json["type"] == "close"): self.socket.close() else: # Only certain API functions work here, like setting listeners if "listener" in json: print( "Listener key present! Starting listener despite no connection to drone..." ) if (json["listener"] == None): print("Canceling listener...") try: self.listener.stop() self.listener = None except AttributeError: print("No listener to cancel! Ignoring...") # As long as it's a number as expected, and we don't already have a listener running elif (type(json["listener"]) is float or type(json["listener"]) is int): self.listener = tornado.ioloop.PeriodicCallback( self.returnAttributes, float(json["listener"])) self.listener.start() print( "Starting listener (WARNING: No connection, so listener will do nothing until drone reconnects)..." ) drone.updateConnectionStatus()
def message_received(client, server, message): # First position indicates message type message_type = ast.literal_eval(message)[0] if (message_type == 0): """ adding new lines that user draw on map """ tasks = ast.literal_eval(message)[1] lines = tasks['line'] polygons = [] lines = [[ dronekit.LocationGlobal(point["lat"], point["lng"], 20) for point in line ] for line in lines] line_task.extend(lines) undistributed_tasks[0] = line_task #Polytask poly_tasks = [] for polygon in polygons: polygon = [[ dronekit.LocationGlobal(point["lat"], point["lng"], 20) for point in line ] for line in polygon] closing_line = [polygon[len(polygon) - 1][1], polygon[0][0]] polygon.append(closing_line) poly_tasks.append(polygon) polygon_task.extend(poly_tasks) undistributed_tasks[1] = poly_tasks #Search task search_tasks = tasks['search'] search_tasks_temp = [] #Convert to DroneKit's Global Position Object for task in search_tasks: task_temp = [] for point in task: point = dronekit.LocationGlobal(point[0], point[1], 20) task_temp.append(point) search_tasks_temp.append(task_temp) search_tasks = search_tasks_temp if len(search_tasks) > 0: undistributed_tasks[2] = generate_search_coordinates(search_tasks) # New Parameter recieved if (message_type == 1): """ Change Parameters """ vehicle_id = ast.literal_eval(message)[1] vehicle = Drone_Services.find_vehicle_by_id(vehicles, vehicle_id) new_max_speed = ast.literal_eval(message)[2] vehicle.max_speed = float(new_max_speed) vehicle.groundspeed = float(new_max_speed) new_max_battery_time = ast.literal_eval(message)[3] vehicle.max_battery_time = float(new_max_battery_time) vehicle.current_battery = float(new_max_battery_time) vehicle.start_up_time = time.time() new_max_carry_weight = ast.literal_eval(message)[4] vehicle.max_carry_weight = float(new_max_carry_weight) # Reacharge battery if (message_type == 2): vehicle_id = ast.literal_eval(message)[1] vehicle = Drone_Services.find_vehicle_by_id(vehicles, vehicle_id) vehicle.current_battery = vehicle.max_battery_time vehicle.start_up_time = time.time()
time.sleep(1) print "Taking off!" markut.simple_takeoff(args.altitude) while True: markut_current_altitude = markut.location.global_relative_frame.alt print " Altitude: ", markut_current_altitude if markut_current_altitude >= args.altitude * 0.95: print "Reached target altitude." break time.sleep(1) markut.airspeed = args.speed target_location = dronekit.LocationGlobal(TARGET_LATITUDE, TARGET_LONGITUDE, args.altitude) target_distance = get_distance_metres(markut.location.global_relative_frame, target_location) markut.simple_goto(target_location) while markut.mode.name == "GUIDED": remaining_distance = get_distance_metres(markut.location.global_relative_frame, target_location) print "Distance to target: ", remaining_distance if remaining_distance <= target_distance * 0.01: print "Reached target" break; time.sleep(2) markut.mode = dronekit.VehicleMode("LOITER") while not markut.mode.name == "LOITER": print "GUIDED -> LOITER" time.sleep(1)
def change_yaw(vehicle, yaw): """Sends RC override for yaw. Assumes yaw uses RC channel 4.""" print("RC override yaw:", yaw) vehicle.channels.overrides['4'] = yaw def change_thrust(vehicle, thrust): """Sends RC override for thrust. Assumes thrust uses RC channel 3.""" print("RC override thrust: ", thrust) vehicle.channels.overrides['3'] = thrust # Quadrant 1 TARGET_WAYPOINT = dronekit.LocationGlobal(-35.356833, 149.162703, None) # Quadrant 2 #TARGET_WAYPOINT = dronekit.LocationGlobal(-35.359124, 149.168475, None) # Quadrant 3 #TARGET_WAYPOINT = dronekit.LocationGlobal(-35.371669, 149.169827, None) # Quadrant 4 #TARGET_WAYPOINT = dronekit.LocationGlobal(-35.371415, 149.162192, None) def main(): vehicle = dronekit.connect('tcp:127.0.0.1:5763', wait_ready=True) arm(vehicle) move_to_target(vehicle, TARGET_WAYPOINT)
#vehicle = dronekit.connect('/dev/ttyAMA0', wait_ready=True, baud=57600) # vehicle is an instance of the Vehicle class # while not vehicle.is_armable: # time.sleep(0.01) # vehicle.arm() vehicle.disarm() time.sleep(2) vehicle.arm() time.sleep(2) bat = vehicle.battery bat.current bat.voltage bat.level gim = vehicle.gimbal loc = dronekit.LocationGlobal(0, 0) #while not KeyboardInterrupt: #gim.target_location(loc) #gim.pitch(3) #i=1 #loc = dronekit.LocationGlobalRelative(90,90,alt=3) vehicle.simple_takeoff(3) crtime = time.time() while time.time() - crtime < 5: print(vehicle.location.global_frame) print("Armed: " + str(vehicle.armed)) time.sleep(0.2) print("Setting AUTO mode") #vehicle.mode = dronekit.VehicleMode('AUTO') #vehicle.mode = VehicleMode("AUTO")
def main(path_to_config, ardupath=None): if ardupath is not None: global ARDUPATH ARDUPATH = ardupath global DO_CONT DO_CONT = True config = load_json(path_to_config) dronology = util.Connection() dronology.start() # A list of sitl instances. sitls = [] # A list of drones. (dronekit.Vehicle) vehicles = [] # A list of lists of lists (i.e., [ [ [lat0, lon0, alt0], ...] ...] # These are the waypoints each drone must go to! routes = [] # Example: # vehicle0 = vehicles[0] # waypoints_for_vehicle0 = routes[0] # for waypoint in waypoints_for_vehicle0: # lat, lon, alt = waypoint # vehicle0.simple_goto(lat, lon, alt) # The above example obviously won't work... you'll need to write some code to figure out when the current waypoint # has been reached and it's time to go to the next waypoint. # Define the shutdown behavior def stop(*args): global DO_CONT DO_CONT = False w0.join() for v, sitl in zip(vehicles, sitls): v.close() sitl.stop() dronology.stop() signal.signal(signal.SIGINT, stop) signal.signal(signal.SIGTERM, stop) # Start up all the drones specified in the json configuration file for i, v_config in enumerate(config): home = v_config['start'] vehicle, sitl = connect_vehicle(i, home) handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, get_vehicle_id(i)) dronology.send(str(handshake)) sitls.append(sitl) vehicles.append(vehicle) routes.append(v_config['waypoints']) # Create a thread for sending the state of drones back to Dronology w0 = threading.Thread(target=state_out_work, args=(dronology, vehicles)) # Start the thread. w0.start() server_addr = '127.0.0.1' port = '5000' # handle lead drone print("Starting Lead Drone") print("Pre-arm checks") while not vehicles[0].is_armable: print("Waiting for drone to initialize...") time.sleep(1) ''' print("Arming motors") vehicles[0].mode = dronekit.VehicleMode("GUIDED") vehicles[0].armed = True while not vehicle.armed: print("Waiting to arm...") time.sleep(1) ''' # Connect lead drone to server print("Connecting to server as Lead") wypts = routes[0] wypts = {'waypoints': wypts} r = requests.post('http://' + server_addr + ':' + port + '/createLeadDrone', data=json.dumps(wypts)) if r.status_code == 200: resp = json.loads(r.content) else: print('Error: {} Response'.format(r.status_code)) # handle second+ drones # random starting point from an example file home = [ 41.714867454724, -86.242300802635, 0] vehicle, sitl = connect_vehicle(1, home) handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, get_vehicle_id(1)) dronology.send(str(handshake)) sitls.append(sitl) vehicles.append(vehicle) print("Starting Auxillary Drone 1") print("Pre-arm checks") while not vehicles[1].is_armable: print("Waiting for drone to initialize...") time.sleep(1) print("Arming motors 0") vehicles[0].mode = dronekit.VehicleMode("GUIDED") vehicles[0].armed = True while not vehicles[0].armed: print("Waiting to arm...") time.sleep(1) print("Arming motors 1") print("Inside a loop") vehicles[1].mode = dronekit.VehicleMode("GUIDED") vehicles[1].armed = True while not vehicles[1].armed: print("Waiting to arm...") time.sleep(1) print("Connecting as aux drone 1") r = requests.get('http://' + server_addr + ':' + port + '/computeStraightLinePath') if r.status_code == 200: resp = json.loads(r.content) wypts1 = resp['waypoints']#dict(resp['waypoints']) routes.append(wypts1) else: print('Error: {} Response'.format(r.status_code)) # setup should be complete... # takeoff counter = 0 for vehicle in vehicles: starting_altitude = 10 vehicle.simple_takeoff(starting_altitude) # check to make sure we're at a safe height before raising the other ones while True: print(" Altitude: ", vehicle.location.global_relative_frame.alt) # Break and return from function just below target altitude. if vehicle.location.global_relative_frame.alt >= starting_altitude * 0.95: print("Reached target altitude for vehicle {}".format(counter)) break time.sleep(1) counter += 1 # lots of hardcoded stuff, need to change # base it off lead drone waypoints for i, wypts0 in enumerate(routes[0]): print routes[1] loc_0 = dronekit.LocationGlobal(wypts0[0], wypts0[1], 10) if i > 4: break loc_1 = dronekit.LocationGlobal(routes[1][i*6][0], routes[1][i*6][1], 10) vehicles[0].simple_goto(loc_0, groundspeed=10) vehicles[1].simple_goto(loc_1, groundspeed=10) for j in range(1,5): location = dronekit.LocationGlobal(routes[1][j+i*6][0], routes[1][j+i*6][1], 10) vehicles[1].simple_goto(location, groundspeed=10) # wait until ctrl c to exit while DO_CONT: time.sleep(5.0)
@vehicle.on_attribute('location.global_relative_frame') def global_relative_frame_listener(self, name, val): print ('%s attribute is: %s' % (name, val)) @vehicle.on_attribute('battery') def battery_listener(self, name, val): print ('%s attribute is: %s' % (name, val)) offsetLat = 0.5 offsetLon = 0.3 offsetAlt = 100 altitude = 50.0; homeLocation = dronekit.LocationGlobal(-34.364114, 149.166022, 30) gotoLocation = dronekit.LocationGlobal(-34.364114 + offsetLat, 149.166022 + offsetLon, 30 + offsetAlt); gotoLocation.lat += 0.5; vehicle.home_location = homeLocation # Etat du vehicule print "Get all vehicle attribute values:" print (" DRONE VERSION: %s" % vehicle.version) # Version de l'autopilot print (" LOCATION CAPABILITIES: %s" % vehicle.capabilities) print (" LOCATION GLOBAL FRAME: %s" % vehicle.location.global_frame ) # position gps global. Altitude par rapport au niveau de la mer (MSL) print (" LOCATION GLOBAL RELATIVE FRAME: %s" % vehicle.location.global_relative_frame ) # position par rapport e la base (e configurer comme on veut) print (" ATTITUDE: %s" % vehicle.attitude ) # Pitch, Roll, Yaw print (" VELOCITY: %s" % vehicle.velocity) # Vitesse vx, vy, vz print (" GIMBAL: %s" % vehicle.gimbal) print (" BATTERY: %s" % vehicle.battery ) # tension de la batterie en millivolt / niveau de batterie restantes / alimentation en ampere si l'autopilot supporte
def main(): #this is called if button 1 is pressed def interrupt_button_1(channel): time.sleep(KILL_TIME) if GPIO.input(BUTTON_RIGHT_PIN) == 0 and GPIO.input( BUTTON_LEFT_PIN) == 0: #killswitch vehicle.mode = dronekit.VehicleMode("STABILIZE") vehicle.armed = False logging.warning("KILL Button detected, disarming") time.sleep(STOP_TIME - KILL_TIME) if GPIO.input(BUTTON_LEFT_PIN) == 0: #land mode logging.warning("Land Button detected, Setting LAND mode") vehicle.mode = dronekit.VehicleMode("LAND") def signal_term_handler(signal, frame): logging.error('Caught SIGTERM (kill signal)') cleanup() end_gps_poller(gpsp) sys.exit(1) def cleanup(): # close vehicle object before exiting script logging.info("Closing connection to vehicle object & safety landing") try: vehicle.mode = dronekit.VehicleMode("LAND") vehicle.close() except: logging.warning( "Cleanup done, but there was no vehicle connection") else: logging.info("Done. Bye! :-)") writePidFile() gpsp = 'None' signal.signal(signal.SIGTERM, signal_term_handler) GPIO.setwarnings(False) setup_LED() setup_buttons() try: # Interrupt-Event hinzufuegen GPIO.add_event_detect(BUTTON_LEFT_PIN, GPIO.FALLING, callback=interrupt_button_1, bouncetime=300) parser = argparse.ArgumentParser( description='Tracks GPS position of your computer (Linux only).') parser.add_argument('--connect', help="vehicle connection target string.") parser.add_argument('--log', help="logging level") args = parser.parse_args() if args.connect: connection_string = args.connect else: print("no connection specified via --connect, exiting") sys.exit() if args.log: logging.basicConfig(filename='log-follow.log', level=args.log.upper()) else: print('No loging level specified, using WARNING') logging.basicConfig(filename='log-follow.log', level='WARNING') logging.warning( '################## Starting script log ##################') logging.info("System Time:" + time.strftime("%c")) logging.info("Flying altitude: %s" % FLY_ALTITUDE) logging.info("Time between GPS points: %s" % GPS_REFRESH) gpsp = GpsPoller() gpsp.start() make_LED('ORANGE') vehicle = 'None' while vehicle == 'None': vehicle = connect(connection_string) #arm_and_takeoff(START_ALTITUDE) last_alt = 0 last_dest = 'None' while not set_throw_wait(vehicle): time.sleep(1) # main loop while True: if vehicle.mode.name != "GUIDED": logging.warning("Flight mode changed - aborting follow-me") break if (gpsd.valid) != 0: dest = dronekit.LocationGlobal(gpsd.fix.latitude, gpsd.fix.longitude, gpsd.fix.altitude) alt_gpsbox = dest.alt delta_z = 0 if math.isnan(alt_gpsbox): # NO new Z info from box (altitude) if last_alt == 0: dest = dronekit.LocationGlobalRelative( dest.lat, dest.lon, FLY_ALTITUDE) else: dest.alt = last_alt else: # new Z info from box exists # before it's accepted, check altitude error alt_gpsbox_dilution = gpsd.fix.epv # check altitude and dilution reported from drone alt_drone = vehicle.location.global_frame.alt # the below error calculation is not very accurate # epv is the Vertical Dilution * 100, which is a unitless number # 4.1 is a value taken from cgps output, by comparing VDop and Verr alt_drone_dilution = vehicle.gps_0.epv / 4.1 logging.info( 'GPS box alt: %s +- %s | Drone alt: %s +- %s' % (alt_gpsbox, alt_gpsbox_dilution, alt_drone, alt_drone_dilution)) # check if the altitude "uncertainty bubbles" touch if alt_gpsbox + alt_gpsbox_dilution < alt_drone - alt_drone_dilution: delta_z = last_alt - (dest.alt + FLY_ALTITUDE) last_alt = alt_gpsbox + FLY_ALTITUDE dest = dronekit.LocationGlobal(dest.lat, dest.lon, last_alt) else: if last_alt == 0: dest = dronekit.LocationGlobalRelative( dest.lat, dest.lon, FLY_ALTITUDE) else: dest.alt = last_alt # check the distance between current and last position if last_dest == 'None': last_dest = dest distance = vincenty((last_dest.lat, last_dest.lon), (dest.lat, dest.lon)).meters distance = math.sqrt(distance * distance + delta_z * delta_z) logging.debug('Distance between points[m]: %s' % distance) # only send new point if distance is large enough if distance > MIN_DISTANCE: logging.info('Going to: %s' % dest) vehicle.simple_goto(dest, None, 30) last_dest = dest time.sleep( GPS_REFRESH) #this needs to be smaller than 1s (see above) else: logging.warn("Currently no good GPS Signal") time.sleep(GPS_REFRESH) # broke away from main loop make_LED('ORANGE') cleanup() end_gps_poller(gpsp) sys.exit(0) except socket.error: make_LED("RED") logging.error("Error: gpsd service does not seem to be running, " "plug in USB GPS or run run-fake-gps.sh") cleanup() end_gps_poller(gpsp) sys.exit(1) except (KeyboardInterrupt, SystemExit): make_LED("RED") logging.info("Caught keyboard interrupt") cleanup() end_gps_poller(gpsp) sys.exit(0) except Exception as e: make_LED("RED") logging.error("Caught unknown exception, trace follows:") logging.error(traceback.format_exc()) cleanup() end_gps_poller(gpsp) sys.exit(1)