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)
