def main():
try:
logging.basicConfig(filename='test_fly.log', level='DEBUG')
sc = beacon_search.SearchController('/dev/ttyS0,921600',
lvs_detection=False)
#sc=beacon_search.SearchController('192.168.4.2:14551', lvs_detection = True)
sc.start_log
sc.arm_and_takeoff()
#STEP1 go to start
logging.info('Going to start')
start_location = dronekit.LocationGlobalRelative(
53.476033, 9.929890, 1)
sc.goto_position_above_terrain(start_location)
sc.wait_until_there()
#STEP2 go to finish
logging.info('Going to finish')
finish_location = dronekit.LocationGlobalRelative(
53.476038, 9.929576, 1)
sc.goto_position_above_terrain(finish_location)
sc.wait_until_there()
#STEP4 land
logging.info('landing')
sc.vehicle.mode = dronekit.VehicleMode("LAND")
except:
logging.error("Caught exeption")
logging.error(traceback.format_exc())
sys.exit(1)
def executeControl(self, vs, vehicle, out, log_name):
"""
Control loop. Searches for target. If it finds the target,
next_state is set to Initial_Descent_State. Otherwise,
the drone rises vertically to 10 meters.
"""
x_m, y_m, x_pix, y_pix, frame = find_target(vs, vehicle)
if x_m is not None: # if a target was found, x_m won't be None.
self.set_next_state('target_found')
# record this sighting
self.targ_sighting_loc = vehicle.location.global_relative_frame
else:
if self.targ_sighting_loc is None:
# find current location
loc_cur = vehicle.location.global_relative_frame
# set desired location to have same lat, long but be 10 meters high
loc_des = dronekit.LocationGlobalRelative(
loc_cur.lat, loc_cur.lon, 10)
else:
loc_des = self.targ_sighting_loc
vehicle.simple_goto(loc_des)
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 update_location(self, newlocation):
"""
changes the current location of the UAV
"""
points = dronekit.LocationGlobalRelative(new_location[0],
newlocation[1], self.altitude)
self.vehicle.simple_goto(points)
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 __goto_lla(self, lat, lon, alt):
"""
Go to the specified lat, lon, alt
:param lat:
:param lon:
:param alt:
:return:
"""
self._vehicle.simple_goto(
dronekit.LocationGlobalRelative(lat, lon, alt))
def goto(drone, lat, lon, alt, speed=1):
drone.simple_goto(dronekit.LocationGlobalRelative(lat, lon, alt))
DronekitHelpers.set_speed(drone, speed)
trg = Lla(lat, lon, alt)
dist = trg.distance(DronekitHelpers.get_lla(drone))
while dist > 2.0:
time.sleep(1.0)
dist = trg.distance(DronekitHelpers.get_lla(drone))
def goto(self, latitude, longitude, ground_speed=None):
destination = dronekit.LocationGlobalRelative(float(latitude),
float(longitude), 0)
self._vehicle.simple_goto(destination, groundspeed=ground_speed)
while self._vehicle.groundspeed < 3:
time.sleep(1)
logger.debug('Speed up groundspeed...%s',
self._vehicle.groundspeed)
return
def goto(head, gotoFunction=vehicle.simple_goto):
currentLocation = (vehicle.location.global_relative_frame.lat,
vehicle.location.global_relative_frame.lon)
move = distance.distance(0.0005)
new = move.destination(currentLocation, head)
targps = (new[0], new[1])
tar_loc = dronekit.LocationGlobalRelative(targps, 3.5)
#targetLocation=get_location_metres(currentLocation, dNorth, dEast)
# targetDistance=get_distance_metres(currentLocation, targetLocation)
gotoFunction(targetLocation)
def sendLookFromSpookyNED_simple(self, ned, vel=[0, 0, 0], useVel=False):
if not self.vehicle:
return False
drone_llh = self._spooky_to_vehicle_llh_relative(ned)
print "sendLookFromSpookyNED_simple ned=", ned, "drone_llh=", drone_llh, "useVel=", useVel, "vel=", vel
point1 = dronekit.LocationGlobalRelative(drone_llh[0], drone_llh[1],
drone_llh[2])
self.vehicle.simple_goto(point1)
def fly_to_destination(self):
"""
Start the traversing stage of flight
"""
# Using relative means the altitude is relative to the drone's home altitude rather than mean sea level
location = dronekit.LocationGlobalRelative(
self.mission_lat, # Mission latitude
self.mission_lon, # Mission longitude
float(self.mission_height) # Mission altitude
)
self.vehicle.simple_goto(location)
def go_forward(self, forward_distance, bearing):
curr = self.vehicle.location.global_frame
d = distance.distance(meters=forward_distance)
dest = d.destination(geopy.Point(curr.lat, curr.lon), bearing)
drone_dest = dronekit.LocationGlobalRelative(
dest.latitude, dest.longitude, self.params["FLY_ALTITUDE"])
logging.info('Going to: %s' % drone_dest)
#self.goto_position_above_terrain(drone_dest)
self.vehicle.simple_goto(drone_dest)
time.sleep(self.params["MEANDER_MIN_TIMEOUT"])
while self.vehicle.groundspeed > self.params["MEANDER_MIN_SPEED"]:
time.sleep(self.params["MEANDER_MIN_SPEED_TIMEOUT"])
def git(vehicle, sag, on, sleep):
current = vehicle.location.global_relative_frame
yaw = fixYaw(math.degrees(vehicle.attitude.yaw))
ddlat = MeterToDd(on)
ddlon = MeterToDd(sag)
lat = current.lat + ddlat
lon = current.lon + ddlon
goPoint = (lat, lon)
x, y = rotatePoint((current.lat, current.lon), goPoint,
-1 * math.radians(yaw))
go = dk.LocationGlobalRelative(x, y, current.alt)
vehicle.simple_goto(go)
time.sleep(sleep)
def get_distance_left(self):
"""
Return the horizontal distance to the next waypoint
"""
try:
lat = self.mission_lat
lon = self.mission_lon
alt = self.mission_height
mission_location = dronekit.LocationGlobalRelative(lat, lon, alt)
return get_distance_metres(self.vehicle.location.global_frame,
mission_location)
except:
return "No distance data"
def distance_to_current_waypoint(self):
"""
Gets distance in metres to the current waypoint.
"""
nextwaypoint = self.cmds.next
if nextwaypoint == 0:
return None
missionitem = self.cmds[nextwaypoint - 1] #commands are zero indexed
if missionitem.command == mavutil.mavlink.MAV_CMD_NAV_WAYPOINT:
lat = missionitem.x
lon = missionitem.y
alt = missionitem.z
targetWaypointLocation = dronekit.LocationGlobalRelative(
lat, lon, alt)
distancetopoint = frame_conversion.get_distance_metres(
self.vehicle.location.global_frame, targetWaypointLocation)
return distancetopoint
if missionitem.command == mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH:
targetWaypointLocation = self.vehicle.home_location
distancetopoint = frame_conversion.get_distance_metres(
self.vehicle.location.global_frame, targetWaypointLocation)
return distancetopoint
def go_to(vehicle, args):
"""Make an movement of drone according to the latitude/longitude inputted
We think that the drone has arrived the target position when the remaining distance decreases to a so small
value that we can ignore it. There isn't any callback function provided since the script runs synchronously,
but it can be added easily if needed one day.
Args:
vehicle: Object of drone.
args: Dictionary that contains action information
* Lat: Latitude of target position.
* Lon: Longitude of target position.
* Time: Expected time of the task (second).
"""
lat = args['Lat']
lon = args['Lon']
t = args['Time']
current_location = vehicle.location.global_relative_frame
target_location = dronekit.LocationGlobalRelative(lat, lon,
current_location.alt)
target_distance = _get_distance_metres(current_location, target_location)
# Set the airspeed
if not t == 0:
vehicle.airspeed = target_distance / (t * 1.0)
vehicle.simple_goto(target_location)
while vehicle.mode.name == "GUIDED": # Stop action if we are no longer in guided mode.
remaining_distance = _get_distance_metres(
vehicle.location.global_frame, target_location)
print "Distance to target: ", remaining_distance
if remaining_distance <= 0.5: # Just below target, in case of undershoot.
print "Reached target"
break
time.sleep(2)
def goto(self, lat, lon, alt):
self.vehicle.simple_goto(dronekit.LocationGlobalRelative(
lat, lon, alt))
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)
loc4 = dk.LocationGlobalRelative(52.81316, -4.121333, alt)
loc5 = dk.LocationGlobalRelative(52.814772, -4.120708, alt)
loc6 = dk.LocationGlobalRelative(52.815950, -4.121686, alt)
loc7 = dk.LocationGlobalRelative(52.815850, -4.125336, alt)
loc8 = dk.LocationGlobalRelative(52.815822, -4.126181, alt)
loc9 = dk.LocationGlobalRelative(52.814656, -4.126853, alt)
loc10 = dk.LocationGlobalRelative(52.813269, -4.125403, alt)
loc11 = dk.LocationGlobalRelative(52.813547, -4.128811, alt)
###### TASK 1 #######
arm_takeoff(vehicle, alt)
vehicle.simple_goto(loc1)
disparity = disparity/16 #Scale to (0~255) int16
#print("xxxxxxxxxxxxx",disparity.min(), disparity.max())
#disparity = disparity.astype(np.float16)/disparity.max().astype(np.float16)*255.0
disparity = disparity.astype(np.float16)/55.0*255.0
#print(disparity.min(), disparity.max())
disparity = disparity.astype(np.uint8)
#print(disparity.min(), disparity.max())
#disparity = cv2.medianBlur(disparity, 5)
return disparit
print("takeoff")
arm_and_takeoff(10)
print("takeoffcompleted")
if vehicle.mode == VehicleMode("GUIDED"):
lat,lon=waypoint_list[0]
point = dronekit.LocationGlobalRelative(lat,lon,4) #lat, long, alt
#vehicle.simple_goto(point)
#time.sleep(3)
i = 0
j = 0
rows, cols = (3, 3)
a = [[0 for i in range(cols)] for j in range(rows)]
midpt_of_roi_x = [[0 for i in range(cols)] for j in range(rows)]
midpt_of_roi_y = [[0 for i in range(cols)] for j in range(rows)]
vel_scaling_in_down = [[0 for i in range(cols)] for j in range(rows-1)]
vel_scaling_in_right = [[0 for i in range(cols-1)] for j in range(rows)]
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
video_names=os.listdir("./videos")
out = cv2.VideoWriter('./videos/video'+str(len(video_names))+'.mp4',fourcc, 10.0, (660, 205))
np.array([28.757301884300027, 77.1111430725439]),
np.array([28.755862966722887, 77.11914434911056]),
np.array([28.75316500148104, 77.12027272985]),
np.array([28.7539743939341, 77.11227145328346]),
np.array([28.754513985278184, 77.11657983297322]),
np.array([28.75118649741841, 77.11104048765779]),
np.array([28.757122017973817, 77.11647725692173])
])
p = rectangle_mid_point(1000, 1000, 28.753300, 77.116118, 0)
cell_list = coordinates(m, n, p[0][0], p[0][1], 10, 10, 0)
temp = initial_wps_func(N, 28.749498, 77.111593, 0, 900, 900)
points = []
for pts in temp:
pts = dronekit.LocationGlobalRelative(pts[0], pts[1], 35)
points.append(pts)
UAVlocdict = dict()
vehicle = list() # list of different thread UAVs
for i in range(N):
UAVthread = UAVSwarmBot("127.0.0.1:" + str(14550 + i * 10))
UAVthread.start()
vehicle.append(UAVthread)
UAVlocdict[i] = UAVthread
UAVthread.g_list = cell_list
UAVthread.UAVID += i
UAVthread.arm_and_takeoff(5)
UAVthread.vehicle.simple_goto(points[i])
time.sleep(1)
def ctrl_drone(): #main function
stage = 0 ######0
pausestage = 0
running = 1
custname = ""
custmob = ""
targps = (0, 0)
currgps = (0, 0)
print("\n[SYSTEM] System Ready... Run main script")
print("[SYSTEM] Wait for SMS")
while running == 1:
if vehicle.mode.name != "GUIDED":
#print("[SYSTEM] NOT IN GUIDED - PAUSE")
#print(vehicle.mode.name)
pausestage = stage
#stage = -1
if stage == -1 and vehicle.mode.name == "GUIDED":
print("[SYSTEM] Resume")
stage = oldstage
if stage == 0:
if smsrec == 1:
stage = 1
time.sleep(1.5)
else:
time.sleep(1)
if stage == 1:
print("[SYSTEM] SMS Received")
msg = list(read_sms())
msglen = len(msg)
#print(msg)
ncount = 0
ccount = 0
msgstart = 0
msgend = 0
nostart = 0
noend = 0
for i in range(msglen):
if msg[(msglen - 1) - i] == '\n':
ncount = ncount + 1
if ncount == 3:
msgend = (msglen - 1) - i - 1
#print(msgend)
if ncount == 4:
msgstart = (msglen - 1) - i + 1
#print(msgstart)
if msg[(msglen - 1) - i] == ',':
ccount = ccount + 1
if ccount == 3:
noend = (msglen - 1) - i - 1
#print(noend)
if ccount == 4:
nostart = (msglen - 1) - i + 2
#print(nostart)
custname = ''.join(str(e) for e in msg[msgstart:msgend])
#custmob = ''.join(str(e) for e in (["\""] + msg[nostart:noend] + ["\""]))
custmob = ''.join(str(e) for e in (msg[nostart:noend]))
#custname = str(msg[msgstart:msgend])
#custmob = str(msg[nostart:noend])
if len(custname) == 0 or len(custmob) == 0:
print("[SYSTEM] Failed to grab cust details")
stage = 0
time.sleep(1)
else:
stage = 2
print(custname)
print(custmob)
if stage == 2:
#msg = "Hi, " + custname ". We currently have: USB Cable. Would you like one? (Yes)"
msg = "".join(
str(e) for e in [
"Hi ", custname,
". We currently have: USB Cable. Would you like one? (Yes)"
])
send_sms(custmob, msg)
print(msg)
print("[SYSTEM] Wait reply...")
stage = 3
#running = 0
if stage == 3:
if smsrec == 1:
stage = 4
time.sleep(1.5)
else:
time.sleep(1)
if stage == 4:
print("[SYSTEM] SMS Received")
msg = list(read_sms())
msglen = len(msg)
#print(msg)
ncount = 0
ccount = 0
msgstart = 0
msgend = 0
nostart = 0
noend = 0
for i in range(msglen):
if msg[(msglen - 1) - i] == '\n':
ncount = ncount + 1
if ncount == 3:
msgend = (msglen - 1) - i - 1
#print(msgend)
if ncount == 4:
msgstart = (msglen - 1) - i + 1
#print(msgstart)
if msg[(msglen - 1) - i] == ',':
ccount = ccount + 1
if ccount == 3:
noend = (msglen - 1) - i - 1
#print(noend)
if ccount == 4:
nostart = (msglen - 1) - i + 2
#print(nostart)
custmsg = ''.join(str(e) for e in msg[msgstart:msgend])
reccustmob = ''.join(str(e) for e in (msg[nostart:noend]))
print(custmsg)
print(reccustmob)
if not reccustmob == custmob:
print("uh oh")
if len(custmsg) == 0 or len(reccustmob) == 0:
print("[SYSTEM] Failed to grab cust details")
stage = 2
time.sleep(1)
else:
if custmsg == "Yes" or custmsg == "yes":
stage = 5
time.sleep(1)
else:
stage = 3
if stage == 5:
msg = "Send gps lat&lon coords (with space between)" #lazy but im already using commas to separate message out
send_sms(custmob, msg)
print(msg)
print("[SYSTEM] Wait reply...")
stage = 6
if stage == 6:
if smsrec == 1:
stage = 7
time.sleep(1.5)
else:
time.sleep(1)
if stage == 7:
print("[SYSTEM] SMS Received")
msg = list(read_sms())
msglen = len(msg)
ncount = 0
ccount = 0
msgstart = 0
msgend = 0
nostart = 0
noend = 0
for i in range(msglen):
if msg[(msglen - 1) - i] == '\n':
ncount = ncount + 1
if ncount == 3:
msgend = (msglen - 1) - i - 1
#print(msgend)
if ncount == 4:
msgstart = (msglen - 1) - i + 1
#print(msgstart)
if msg[(msglen - 1) - i] == ',':
ccount = ccount + 1
if ccount == 3:
noend = (msglen - 1) - i - 1
#print(noend)
if ccount == 4:
nostart = (msglen - 1) - i + 2
#print(nostart)
gps = ''.join(str(e) for e in msg[msgstart:msgend])
reccustmob = ''.join(str(e) for e in (msg[nostart:noend]))
print(gps)
print(reccustmob)
if not reccustmob == custmob:
print("uh oh")
if len(gps) == 0 or len(reccustmob) == 0:
print("[SYSTEM] Failed to grab cust details")
stage = 2
time.sleep(1)
targps = (float(gps.split()[0]), float(gps.split()[1]))
currgps = (vehicle.location.global_relative_frame.lat,
vehicle.location.global_relative_frame.lon)
print(targps)
print(currgps)
dis = distance.distance(currgps, targps).km
print(dis)
if dis < 0.5:
print("[SYSTEM] Good target")
stage = 8
time.sleep(1)
else:
print("[SYSTEM] Big Distance")
running = 0
if stage == 8:
alt = 3.5
print("[DRONE] Arm and takeoff...")
vehicle.armed = True
while not vehicle.armed:
print("[DRONE] Arming...")
time.sleep(1)
print("[DRONE] Armed")
print("[DRONE] Takeoff to %fm" % alt)
vehicle.simple_takeoff(alt) #take off to 3.5m height
while True:
print("[DRONE] Altitude: ",
vehicle.location.global_relative_frame.alt)
#Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt >= alt * 0.95:
print("[DRONE] Reached target altitude")
break
time.sleep(1)
stage = 9
if stage == 9:
print("[DRONE] Go to customer GPS:")
print(targps)
tar_loc = dronekit.LocationGlobalRelative(targps, 3.5)
condition_yaw(0, False)
vehicle.simple_goto(tar_loc, groundspeed=3)
remainingDistance = distance.distance(
(vehicle.location.global_frame.lat,
vehicle.location.global_frame.lon),
(tar_loc.lat, tar_loc.lon)).m
while remainingDistance > 1:
remainingDistance = distance.distance(
(vehicle.location.global_frame.lat,
vehicle.location.global_frame.lon),
(tar_loc.lat, tar_loc.lon)).m
print("Distance to target: %f" % remainingDistance)
time.sleep(2)
stage = 10
if stage == 10:
custmob = "+447914157048"
print("[DRONE] At customer GPS")
print("[SYSTEM] Call Customer")
print("[GSM] Dial")
msg = "ATD+ " + custmob + ";\n"
msg = list(bytearray(
msg.encode())) #convert message into sendable data
print(msg)
buff_send(0x00, msg)
time1 = time.time() #record time AT sent for timeout
stage = 11 #move onto next stage
if stage == 11: #listen for response from gsm module (we expect 'OK')
bufflen = buff_check(
0x00) #check if data is received on uart0 buffer of spi2uart
if bufflen[0] > 0: #if buffer has bytes
print("[GSM] Response detected")
stage = 12 #move to read buffer
#time.sleep(1)
else:
time.sleep(1) #else wait for response
if time.time(
) - time1 > 10: #if no response in 10 seconds, return to stage 0 and resend AT
print("[GSM] Response timeout, retrying call...")
stage = 10
if stage == 12: #read response from module, ensure it is 'OK', otherwise retry
print("[GSM] Get Reponse...")
msg = buff_read(0x00, bufflen[0]) #read uart0 received bytes
msg2 = uart_decode(msg) #decode into text
if (msg2.strip("\n\r\0") == "OK"
): #if expected response from GSM module
print("[GSM] Response: OK\n[GSM] Call underway")
stage = 13
time.sleep(1)
else: #if response not as expected, then return to stage 0
print("[GSM] Respone: Call FAIL: %s" % msg2.strip("\n\r\0"))
print(msg)
running = 0
if stage == 13:
print("[GSM] Enable DTMF")
msg = "AT+DDET=1\n"
msg = list(bytearray(
msg.encode())) #convert message into sendable data
print(msg)
buff_send(0x00, msg)
time1 = time.time() #record time AT sent for timeout
stage = 14 #move onto next stage
if stage == 14: #listen for response from gsm module (we expect 'OK')
bufflen = buff_check(
0x00) #check if data is received on uart0 buffer of spi2uart
if bufflen[0] > 0: #if buffer has bytes
print("[GSM] Response detected")
stage = 15 #move to read buffer
#time.sleep(1)
else:
time.sleep(1) #else wait for response
if time.time(
) - time1 > 10: #if no response in 10 seconds, return to stage 0 and resend AT
print("[GSM] Response timeout, retrying DTMF enable...")
stage = 13
if stage == 15: #read response from module, ensure it is 'OK', otherwise retry
print("[GSM] Get Reponse...")
msg = buff_read(0x00, bufflen[0]) #read uart0 received bytes
msg2 = uart_decode(msg) #decode into text
if (msg2.strip("\n\r\0") == "OK"
): #if expected response from GSM module
print("[GSM] Response: OK\n[GSM] DTMF Listening...")
stage = 16
time.sleep(1)
else: #if response not as expected, then return to stage 0
print("[GSM] Respone: FAIL: %s" % msg2.strip("\n\r\0"))
print(msg)
running = 13
if stage == 16:
dtmf = "100"
newdtmf = 0
lastin = time.time()
while True:
bufflen = buff_check(
0x00
) #check if data is received on uart0 buffer of spi2uart
if bufflen[0] > 0: #if buffer has bytes
print("[GSM] Response detected")
msg = buff_read(0x00,
bufflen[0]) #read uart0 received bytes
msg2 = uart_decode(msg) #decode into text
print(msg2)
tar = list("+DTMF:")
if set(tar).issubset(set(list(msg2))):
print("[GSM] DTMF Input detected")
msg2 = msg2.split("+DTMF:")
dtmf = list(msg2[1])[1]
print("[GSM] PARSED: " + dtmf)
newdtmf = 1
lastin = time.time()
#time.sleep(1)
else:
time.sleep(1) #else wait for response
if newdtmf == 1:
newdtmf = 0
if dtmf == "2":
print("Forwards")
goto(0.5, 0)
time.sleep(0.5)
if dtmf == "4":
print("left")
goto(0, -0.5)
time.sleep(0.5)
if dtmf == "6":
print("right")
goto(0, 0.5)
time.sleep(0.5)
if dtmf == "8":
print("back")
goto(-0.5, 0)
time.sleep(0.5)
if dtmf == "#":
print("DROP TIME BABY")
stage = 17
break
if time.time() - lastin > 30:
print("input timeout")
stage = 17
break
if stage == 17:
print("[GSM] Hangup Call")
msg = "ATH\n"
msg = list(bytearray(
msg.encode())) #convert message into sendable data
print(msg)
buff_send(0x00, msg)
time1 = time.time() #record time AT sent for timeout
stage = 18 #move onto next stage
if stage == 18: #listen for response from gsm module (we expect 'OK')
bufflen = buff_check(
0x00) #check if data is received on uart0 buffer of spi2uart
if bufflen[0] > 0: #if buffer has bytes
print("[GSM] Response detected")
stage = 19 #move to read buffer
#time.sleep(1)
else:
time.sleep(1) #else wait for response
if time.time(
) - time1 > 10: #if no response in 10 seconds, return to stage 0 and resend AT
print("[GSM] Response timeout, retrying Hangup...")
stage = 17
if stage == 19: #read response from module, ensure it is 'OK', otherwise retry
print("[GSM] Get Reponse...")
msg = buff_read(0x00, bufflen[0]) #read uart0 received bytes
msg2 = uart_decode(msg) #decode into text
if (msg2.strip("\n\r\0") == "OK"
): #if expected response from GSM module
print("[GSM] Response: OK")
stage = 20
vehicle.mode = "LAND"
running = 0
#time.sleep(1)
else: #if response not as expected, then return to stage 0
print("[GSM] Respone: FAIL: %s" % msg2.strip("\n\r\0"))
print(msg)
stage = 17
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS,
timeout=1)
# INITIALIZING DRONE !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
connection_string = '/dev/ttyACM0' # Establishing Connection With PIXHAWK
vehicle = dk.connect(
connection_string, wait_ready=True,
baud=115200) # PIXHAWK is PLUGGED to NUC (RPi too?) VIA USB
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
altitude = 30
speed = 1.5
waypoint1 = dk.LocationGlobalRelative(cmds[0].x, cmds[0].y, altitude)
arm_and_takeoff(altitude)
vehicle.airspeed = speed # set drone speed to be used with simple_goto
vehicle.simple_goto(waypoint1) # trying to reach 1st waypoint
with open(time_stamp + '_logs.txt', 'a+') as f:
f.write('Altitude: ' + str(altitude))
f.write('\n')
f.write('Airspeed: ' + str(speed))
f.write('\n')
# time.sleep(30)
# ----------------------------------------------
detected = 0
counter = 10
while not detected:
# read next image
ret, frame = cap.read()
#RECORDING VIDEO SETUP
dir_original = 'ORIGINAL'
dir_opt_flow = 'OPT_FLOW'
time_stamp = strftime("%Y-%m-%d_%H:%M:%S", time.localtime(time.time()))
fourcc = cv.VideoWriter_fourcc(*'XVID')
#set file to write original camera input
out_original = cv.VideoWriter(os.path.join(dir_original,'original_'+time_stamp+'.avi'),fourcc, 8.0, (640,480))
#set file to write processed frames with optical flow
out_opt_flow = cv.VideoWriter(os.path.join(dir_opt_flow, 'opt_flow'+time_stamp+'.avi'),fourcc, 8.0, (640,480))
#Downloading Destination Coordinates from Flight Controller
cmds = vehicle.commands
cmds.download()
cmds.wait_ready()
waypoint2 = dk.LocationGlobalRelative(cmds[0].x, cmds[0].y, 3) # Destination point 1
#waypoint2 = dk.LocationGlobalRelative(cmds[1].x, cmds[1].y, 3) # Destination point 2
# START Flying
arm_and_takeoff(3)
vehicle.airspeed = 0.5 # set drone speed to be used with simple_goto
#vehicle.simple_goto(waypoint1)#trying to reach 1st waypoint
#time.sleep(20)
cam = cv.VideoCapture(0) # Get Initial Image from camera
ret, prev = cam.read()
h, w = prev.shape[:2]
prevgray = cv.cvtColor(prev, cv.COLOR_BGR2GRAY)
while True: # Image processing/avoidance loop
ret, img = cam.read()
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)
def konumcevirme(location, altitude):
a = dk.LocationGlobalRelative(location[0], location[1], altitude)
return a
def set_home(self,cmd): self.vehicle.simple_goto(dronekit.LocationGlobalRelative(cmd["data"]["x"], cmd["data"]["y"], cmd["data"]["z"]))
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!
# 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()
# At this point, all of the "behind the scenes stuff" has been set up.
# It's time to write some code that:
# 1. Starts up the drones (set the mode to guided, arm, takeoff)
# 2. Sends the drones to their waypoints
# 3. Hopefully avoids collisions!
# You're encouraged to restructure this code as necessary to fit your own design.
# Hopefully it's flexible enough to support whatever ideas you have in mind.
# Create an array called "done" that keeps track of whether the drone has reached its destination
done = []
# Create an array called "curr_dest" that keeps track of the number waypoint that the drone is currently going
# Start up the drones
for vehicle in vehicles:
# Set mode to guided
#set_mode(vehicle, "GUIDED")
# Arm vechicle
#if vehicle.armed != True:
# while not vehicle.is_armable:
# time.sleep(2)
# vehicle.armed = True
# while vehicle.armed != True:
# vehicle.armed = True
#Takeoff
arm_and_takeoff(vehicle, 20)
time.sleep(10)
print("Starting drone: ", vehicle)
done.append(False)
curr_dest.append(-1)
location = vehicle.location.global_relative_frame
LOCATIONS.append(location)
# Get all drones location at every second
location_timer = RepeatedTimer(1, get_vehicle_locations, vehicles)
# Send drones to their waypoints
while DO_CONT:
print("Sending drones to waypoints")
for i, vehicle in enumerate(vehicles):
if not done[i]:
way_number = curr_dest[i]
#if len(routes[i]) == way_number:
# done[i] = True
# set_mode(vehicle, "LAND")
#print ("Waynumber: ", way_number, "curr_dest", curr_dest[i])
if way_number == -1:
ready=True
else:
ready = check_distance(i, routes[i][way_number])
if ready:
print (way_number, " vs ", len(routes[i])-1)
if way_number == len(routes[i])-1:
print("Landing..", way_number, curr_dest[i])
set_mode(vehicle, "LAND")
time.sleep(10)
done[i]=True
break
curr_dest[i] += 1
way_number=curr_dest[i]
print ("Going to ", routes[i][way_number])
vehicle.simple_goto(dronekit.LocationGlobalRelative(routes[i][way_number][0], routes[i][way_number][1], routes[i][way_number][2]))
time.sleep(2.0)
# wait until ctrl c to exit
while DO_CONT:
time.sleep(5.0)
def dispatch(self, sandbox: 'Sandbox', state: State,
environment: Environment, config: Configuration) -> None:
loc = dronekit.LocationGlobalRelative(self.latitude, self.longitude,
self.altitude)
sandbox.connection.simple_goto(loc)
vis = cv.cvtColor(img, cv.COLOR_GRAY2BGR)
for (x1, y1), (x2, y2) in lines:
if ((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)) ** 0.5 > 7:
# if ((x2 - x1) * (x2 - x1) ** 0.5 <= 5) and ((x2 - x1) * (x2 - x1) ** 0.5 > 2) and (
# (y2 - y1) * (y2 - y1) ** 0.5 <= 5) and ((y2 - y1) * (y2 - y1) ** 0.5 > 2):
# cv.circle(vis, (x1, y1), 15, (0, 0, 255), -1)
cv.circle(vis, (x2, y2), 15, (0, 0, 255), -1)
return vis
# cmds = vehicle.commands # Download the waypoint from mission planner
# cmds.download()
# cmds.wait_ready()
# waypoint = dk.LocationGlobalRelative(cmds[0].x, cmds[0].y, 10) # Destination
waypoint = dk.LocationGlobalRelative(WPX,WPY, 10) # Destination
arm_and_takeoff(3)
vehicle.airspeed = 1
vehicle.simple_goto(waypoint)
while True:
print("continue to the waypoint")
vehicle.simple_goto(waypoint)
ret, frame = cap.read()
if frame is None:
break
frame = imutils.resize(frame, width=FRAME_SIZE)
frame_gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
flow = cv.calcOpticalFlowFarneback(
old_gray, frame_gray, None, 0.5, 5, 30, 3, 5, 1.2, 0)
if modoAvoid == "RTL":
mensj(
"Waypoint de evasion RTL alcanzado. Se actualiza siguiente waypoint",
0)
modo = "RTL"
changeMode(vehiculo, "RTL")
else:
modo = "MISION"
mensj(
"No quedan waypoints evasivos que recorrer. Se vuelve a modo MISSION",
0)
changeMode(vehiculo, "AUTO")
else:
actualAvoid = NewWaypointEv[0]
wpLocation = dronekit.LocationGlobalRelative(
NewWaypointEv[0][0], NewWaypointEv[0][1],
vehiculo.location.global_relative_frame.alt)
vehiculo.simple_goto(wpLocation)
print("estoy avoid y en mision es 1")
# corresponde a realizar evasion por sobre una evasion
enMision = 1
# si Auxvar es = 1, ya se esta en modo avoid, por lo que no se carga uno nuevamente
del NewWaypointEv[0]
sensors1.clean_data()
else:
print(
" estoy en el else de avoid de evasion enMision 1 y el valor de actual AVOID es %s"
% actualAvoid)
distanciaAnewWayp = evasion.distanciaEntre2Coor([
vehiculo.location.global_relative_frame.lat,
vehiculo.location.global_relative_frame.lon
