def test_get_coordinates_other(self):
relative_location = LocationGlobalRelative(3.0, 2.0, 1.0)
self.relative_mock.configure_mock(return_value=relative_location)
self.assertEqual(self.geometry.get_coordinates(self.locations_mock),
(3.0, 2.0, 1.0))
self.relative_mock.assert_called_once_with()
while not drone.armed:
print("waiting for arming...")
time.sleep(1)
print("Ready for takeoff, taking off...")
drone.simple_takeoff(TargetAltitude)
while True:
Altitude = drone.location.global_relative_frame.alt
print("Altitude", Altitude)
time.sleep(1)
if Altitude >= TargetAltitude * 0.95:
print("Altitude reached")
break
#Vehicle Connection
drone = connect("127.0.0.1:14551",wait_ready=True)
arm_and_takeoff(20)
drone.airspeed = 10
a_location = LocationGlobalRelative(-35.362144, 149.164409, 20)
drone.simple_goto(a_location)
time.sleep(50)
print (" System status: %s" % vehicle.system_status.state)
print (" GPS: %s" % vehicle.gps_0)
print (" Alt: %s" % vehicle.location.global_relative_frame.alt)
# Continuous loop reading messages and handling commands
while True:
received_data = ser.read()
time.sleep(0.03)
data_left = ser.inWaiting()
received_data += ser.read(data_left)
received_str = received_data.decode()
if received_str.startswith("DRONE_CMD_TAKEOFF"):
arm_and_takeoff(10)
elif received_str.startswith("DRONE_CMD_GOTO"):
lat = float(re.search("(?<=;).*(?=:)", received_str).group())
lon = float(re.search("(?<=:)(.*)(?=DRONE_END)", received_str).group())
if not vehicle.armed:
arm_and_takeoff(10)
print("Set default/target airspeed to 3")
vehicle.airspeed = 3
print("Going towards GPS point " + str(lat) + ", " + str(lon))
point1 = LocationGlobalRelative(lat, lon, 20)
vehicle.simple_goto(point1)
elif received_str.startswith( "DRONE_CMD_RTH"):
vehicle.mode = VehicleMode("RTL")
print("returning home")
elif received_str.startswith("DRONE_CMD_LAND"):
vehicle.mode = VehicleMode("LAND")
else :
ser.write(received_data)
#another drone is broadcasting its position, check if escape is needed
e = escape((lat, lon, alt), (d[1], d[3], d[5]))
if e is not None:
#If escape is needed, return the escape point
return LocationGlobalRelative(e[0], e[1], 5)
#else: nothing to return
return None
#Hardcoded for testing purposes
mission = [
#LocationGlobalRelative(50.862077, 4.684963, 5),#In front of the castle
#LocationGlobalRelative(50.862003, 4.684558, 5),#In front of the castle
#LocationGlobalRelative(50.862428, 4.684319, 5),#In front of the castle
LocationGlobalRelative(50.862262, 4.684424, 5) #,
#LocationGlobalRelative(50.861970, 4.682687, 5) #birds place
]
esc_point = None
arm_and_takeoff(vehicle, 4)
vehicle.airspeed = 1.5
time.sleep(10)
lat, lon, alt = getGPS(vehicle)
ind = 0
eind = []
while running:
if ind == len(mission):
arm_and_takeoff(vehicles[1], 10)
print("Set default/target airspeed to 3")
vehicles[0].airspeed = 10
vehicles[1].airspeed = 10
time.sleep(2)
print("v#1 Going towards v#2 point for 30 seconds ...")
target_act=[ vehicles[1]._location._lat, vehicles[1]._location._lon]
rssi_obs=[]
while True:
v2_lat= vehicles[1]._location._lat
v2_lon= vehicles[1]._location._lon
v1_lat= vehicles[0]._location._lat
v1_lon= vehicles[0]._location._lon
point1 = LocationGlobalRelative(v2_lat, v2_lon, 20)
#vehicles.simple_goto(point1)
rssival, v1_xy = rssi([v1_lat,v1_lon],[v2_lat, v2_lon])
rssi_obs.append([v1_xy[0], v1_xy[1], rssival])
next_target_list=generate_next4(v1_xy[0], v1_xy[1])
print (next_target_list)
#generate four moving candidate, return a list
rssi_targetlist = predict_rssi (next_target_list)
#return the list with corresponding rssi value
maxrssi=max(rssi_targetlist)
maxrssiind = rssi_targetlist.index(maxrssi)
target=next_target_list[maxrssiind]
def go_to_point(lat, long, alt):
point = LocationGlobalRelative(lat, long, alt)
vehicle.simple_goto(point)
def handleRCs(self, channels):
# check socket for a new ROI from the app
self.checkSocket()
# if we have never received an ROI
if not self.rawROI:
return
# smooth ROI and calculate translateVel for follow
self.filterROI()
# if we are not warmed up, dont do anything
if self.followState == FOLLOW_WAIT:
return
'''
Position Control
Note: All follow controllers return position and velocity of the
drone in "absolute" space (as opposed to relative to the ROI)
Pos: Lat, lon, alt. Alt is relative to home location. NEU frame..
Vel: Speed in the x,y, and z directions. NEU frame. vel.z needs
to be inverted before passing to the autopilot.
'''
# Look At Me Mode (Vehicle stays put)
if self.followState == FOLLOW_LOOKAT:
pos, vel = self.pathController.move(channels)
# Free Look Follow Mode (Vehicle controls are similar to FLY)
elif self.followState == FOLLOW_FREELOOK:
pos, vel = self.pathController.move(channels,
newHeading=self.camYaw,
newOrigin=self.filteredROI,
roiVel=self.translateVel)
# Follow Me Mode (Vehicle controls are similar to ORBIT)
elif self.followState == FOLLOW_ORBIT:
pos, vel = self.pathController.move(
channels,
cruiseSpeed=self.pathHandler.cruiseSpeed,
newroi=self.filteredROI,
roiVel=self.translateVel)
elif self.followState == FOLLOW_LEASH:
pos, vel = self.pathController.move(channels,
newroi=self.filteredROI,
roiVel=self.translateVel)
# learn any changes to controller alt offset to apply it to other controllers when instantiated (to avoid jerks)
self.followControllerAltOffset = pos.alt - self.filteredROI.alt
# mavlink expects 10^7 integer for accuracy
latInt = int(pos.lat * 10000000)
lonInt = int(pos.lon * 10000000)
# Convert from NEU to NED to send to autopilot
vel.z *= -1
# create the SET_POSITION_TARGET_GLOBAL_INT command
msg = self.vehicle.message_factory.set_position_target_global_int_encode(
0, # time_boot_ms (not used)
0,
1, # target system, target component
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
0b0000110111000000, # Pos Vel type_mask
latInt,
lonInt,
pos.alt, # x, y, z positions
vel.x,
vel.y,
vel.z, # x, y, z velocity in m/s
0,
0,
0, # x, y, z acceleration (not used)
0,
0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
''' Pointing Control'''
# If followState mandates that the user controls the pointing
if self.followState == FOLLOW_FREELOOK:
self.manualPitch(channels[THROTTLE])
self.manualYaw(channels)
self.handleFreeLookPointing()
# If followState mandates that the copter should point at the ROI
else:
# Adjust the height of the ROI using the paddle
# we pass in both the filtered gimbal paddle value as well as the raw one
self.updateROIAltOffset(channels[RAW_PADDLE])
# we make a copy of the ROI to allow us to add in an altitude offset
# this means the ROI doesn't get polluted with the alt nudging
tempROI = LocationGlobalRelative(self.filteredROI.lat,
self.filteredROI.lon,
self.filteredROI.alt)
self.handleLookAtPointing(tempROI)
#Arm and take off to altitude of 5 meters
arm_and_takeoff(5)
while True:
if vehicle.mode.name != "GUIDED":
print("User has changed flight modes - aborting follow-me")
break
# Read the GPS state from the laptop
next(gpsd)
# Once we have a valid location (see gpsd documentation) we can start moving our vehicle around
if (gpsd.valid & gps.LATLON_SET) != 0:
altitude = 30 # in meters
dest = LocationGlobalRelative(gpsd.fix.latitude,
gpsd.fix.longitude, altitude)
print("Going to: %s" % dest)
# A better implementation would only send new waypoints if the position had changed significantly
vehicle.simple_goto(dest)
# Send a new target every two seconds
# For a complete implementation of follow me you'd want adjust this delay
time.sleep(2)
except socket.error:
print(
"Error: gpsd service does not seem to be running, plug in USB GPS or run run-fake-gps.sh"
)
sys.exit(1)
time.sleep(1)
print("Target altitude reached.")
return None
##### MAIN EXECUTABLE #####
vehicle = connectMyCopter()
arm_and_takeoff(10)
# set the default speed
vehicle.airspeed = 7
# go to wp1
print("go to wp1")
wp1 = LocationGlobalRelative(47.193077,-1.465763,10)
vehicle.simple_goto(wp1)
# Wait 30sec
time.sleep(30)
# Coming back
print("Coming Back")
vehicle.mode = VehicleMode("RTL")
time.sleep(20)
#Close connection
vehicle.close()
vehicle = connect(connection_string, wait_ready=True, baud=57600)
if simulation:
vehicle.mode = VehicleMode("GUIDED")
while True:
if (vehicle.mode == "GUIDED"):
break
time.sleep(0.1)
if (simulation):
print("Running simulation...")
sim.load_target((os.path.dirname(os.path.realpath(__file__))) +
'/Resources/target.PNG')
print("Target loaded.")
target = LocationGlobalRelative(
vehicle.location.global_relative_frame.lat + 0.00002,
vehicle.location.global_relative_frame.lon - 0.00002,
vehicle.location.global_relative_frame.alt)
sim.set_target_location(target)
print("Target set.")
arm_and_takeoff(vehicle, 10)
else:
video.startCamera()
if ((re.compile("3*")).match(cv2.__version__)):
fourcc = cv2.VideoWriter_fourcc(*'XVID')
else:
fourcc = cv2.cv.CV_FOURCC(*'XVID')
i = len([
name
for name in os.listdir((os.path.dirname(os.path.realpath(__file__))) +
break
time.sleep(1)
def my_goto_function(target):
print('Target position is: %s' % target)
print('Current position of vehicle is: %s' % vehicle.location.global_frame)
print('Traveling')
vehicle.simple_goto(target, groundspeed=10)
time.sleep(30)
print('Current position of vehicle is: %s' % vehicle.location.global_frame)
arm_and_takeoff(10)
initialLoc = LocationGlobalRelative(vehicle.location.global_frame.lat,
vehicle.location.global_frame.lon,
vehicle.location.global_frame.alt)
point1 = LocationGlobalRelative(vehicle.location.global_frame.lat - 0.00008,
vehicle.location.global_frame.lon + 0.00008,
vehicle.location.global_frame.alt)
my_goto_function(point1)
point2 = LocationGlobalRelative(vehicle.location.global_frame.lat + 0.0001,
vehicle.location.global_frame.lon,
vehicle.location.global_frame.alt)
my_goto_function(point2)
my_goto_function(initialLoc)
####################################
# determine angle to target relative to drone and world
phi1 = math.atan2(vcoord, hcoord)
phi2 = yaw - phi1
# determine distance to target
hyp = math.hypot(hcoord, vcoord)
# determine location relative to drone, with global orientation
y = hyp * math.sin(phi2)
x = hyp * math.cos(phi2)
print "Coordinates acquired: ", x, ", ", y, ", ", z
# convert coordinates from feet relative to drone to global latitude and longitude
xfactor = 1 / 298171.5253016029
yfactor = 1 / 363999.33433628065
x2 = (x * xfactor) + currentlocation.lon
y2 = (y * yfactor) + currentlocation.lat
print "Coordinates refactored: ", x2, ", ", y2, ", ", z, "; deploying"
coords = LocationGlobalRelative(y2, x2, z)
print "Going to: ", (coords.lon - launchloc.lon) / xfactor, ", ", (
coords.lat - launchloc.lat) / yfactor, ", ", coords.alt - launchloc.alt
currentlocation = vehicle.location.global_relative_frame
# print location, first in global lat and lon, then in feet from drone
print "Location: ", currentlocation.lon, " ", currentlocation.lat, " ", currentlocation.alt, " Rloc: ", (
currentlocation.lon - launchloc.lon) / xfactor, ", ", (
currentlocation.lat -
launchloc.lat) / yfactor, ", ", currentlocation.alt - launchloc.alt
# begin travelling to target
vehicle.simple_goto(coords)
# loop to output location to track progress
i = 0
while i < 18:
time.sleep(0.5)
currentlocation = vehicle.location.global_relative_frame
# Wait until the vehicle reaches a safe height before processing the goto
# (otherwise the command after Vehicle.simple_takeoff will execute
# immediately).
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 >= aTargetAltitude * 0.95:
print("Reached target altitude")
break
time.sleep(1)
lat = vehicle.location.global_relative_frame.lat
lon = vehicle.location.global_relative_frame.lon
alt = 5
start_point = LocationGlobalRelative(lat, lon, alt)
arm_and_takeoff(5)
T1 = time.time()
def fly_to_GPS(aLocation, groundspeed):
"""
Fly to a specific GPS point
"""
currentLocation = vehicle.location.global_relative_frame
#targetLocation = LocationGlobalRelative(34.25654,146.549946,3)
targetDistance = get_distance_metres(currentLocation, aLocation)
vehicle.simple_goto(aLocation, groundspeed)
def orbit_poi(vehicle, poi, configs):
waypoints = [] # waypoints in LLA
altitude = configs["altitude"]
poi_scan_altitude = configs["detailed_search_specific"][
"poi_scan_altitude"]
waypoint_tolerance = configs["waypoint_tolerance"]
radius = configs["radius"] # radius of circle travelled
orbit_number = configs["orbit_number"] # how many times we repeat orbit
x, y, z = geodetic2ecef(poi.lat, poi.lon, poi_scan_altitude) # LLA -> ECEF
n, e, d = ecef2ned(x, y, z, poi.lat, poi.lon,
poi_scan_altitude) # ECEF -> NED
cmds = vehicle.commands
cmds.clear()
# add circle of waypoints around the poi
c = math.sqrt(2) / 2
point_list = [[1, 0], [c, c], [0, 1], [-c, c], [-1, 0], [-c, -c], [0, -1],
[c, -c], [1, 0]]
for orbit in range(orbit_number):
for point in point_list:
a = (radius * point[0]) + n
b = (radius * point[1]) + e
lat, lon, alt = ned2geodetic(a, b, d, poi.lat, poi.lon,
poi_scan_altitude) # NED -> LLA
waypoints.append(LocationGlobalRelative(lat, lon, alt))
# Go to center of POI
if (configs["vehicle_type"] == "VTOL"):
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0,
waypoint_tolerance, 0, 0, poi.lat, poi.lon,
poi_scan_altitude))
elif (configs["vehicle_type"] == "Quadcopter"):
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
poi.lat, poi.lon, poi_scan_altitude))
# Transition to quadcopter if applicable
if (configs["vehicle_type"] == "VTOL"):
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, 0, 0,
mavutil.mavlink.MAV_VTOL_STATE_MC, 0, 0, 0, 0, 0, 0))
# Circular waypoints
if (configs["vehicle_type"] == "VTOL"):
for point in waypoints:
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0,
waypoint_tolerance, 0, 0, 0, point.lat, point.lon,
point.alt))
elif (configs["vehicle_type"] == "Quadcopter"):
for point in waypoints:
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
point.lat, point.lon, point.alt))
# Transition to forward flight if applicable
if (configs["vehicle_type"] == "VTOL"):
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, 0, 0,
mavutil.mavlink.MAV_VTOL_STATE_MC, 0, 0, 0, 0, 0, 0))
# Add dummy endpoint
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
poi.lat, poi.lon, poi_scan_altitude))
print("Upload new commands to vehicle")
cmds.upload()
def search(coordinates):
global batteryPercent, personFound, personLocation, numOfRescued, emergencyLand, velocity,\
testCoordinates, stopDrone, Retreat, multi_rescue
altitude = 5
# Get mission coordinates.
print("BEFORE - coordinates: {}" .format(coordinates))
coordinates = search_algorithm(coordinates, altitude)
print("AFTER - coordinates: {}" .format(coordinates))
global vehicle, DroneAction, bodyLocation
arm_n_takeoff(altitude, vehicle) # 3.05m == 10ft
vehicle.airspeed = 20 # Set drone speed in m/s.
index = 1 # Used for printing current waypoint #
source = 0 # 0 for first available webcam
cap = cv2.VideoCapture(source) # Loading image from the camera
if cap is None or not cap.isOpened(): # If unable to open the camera
print('Warning: unable to open video source:', source)
else:
print('Opening video source:', source)
# Execute until no more coordinates. This loop controls what the drone does if special case arises.
for wp in coordinates:
# Create LocationGlobalRelative variable of current waypoint to then pass to simple_goto()
destination = LocationGlobalRelative(wp[0], wp[1], altitude)
# Get distance between current location and destination. Used to detect arrival to waypoint.
distance = get_distance_meters(vehicle.location.global_relative_frame, destination)
# These 2 variables are used for comparison to detect if drone is stuck at waypoint.
init_distance = distance
count = 1
# Update telemtry
telemetry()
print("\nHeading to waypoint {}: {}\n" .format(index, wp))
DroneAction = "\nHeading to waypoint {}: {}\n" .format(index, wp)
# Tells drone to move to destination.
vehicle.simple_goto(destination)
# Keep moving to destination as long as battery ok and no UI interaction occurs.
while distance >= 1.5 and not Retreat and not emergencyLand and not stopDrone and batteryPercent > 20:
# Detects if drone gets stuck while heading to waypoint by monitoring change in distance. If 10 iterations have passed
# and the current distance is 95% or greater of the initial distance; then drone is stuck.
# simple_goto cmd might have gotten lost.
if count == 15 and distance/init_distance >= .95:
print("Getting un-stuck.")
vehicle.simple_goto(destination) # Resend goto cmd to finish heading to wp.
count = 1 # Reset count variable to detect if stuck again.
init_distance = distance # Update the ini_distance var so that it does not contain starting distance
else:
init_distance = distance # Update the ini_distance var so that it does not contain starting distance
# If drone randomly changes to RTL go back to GUIDED.
if vehicle.mode.name == 'RTL':
vehicle.mode = VehicleMode("GUIDED")
while not vehicle.mode.name == "GUIDED":
print("FIXING RANDOM RTL...")
print("FIXED: IN GUIDED AGAIN...")
vehicle.simple_goto(destination)
# Set the 'personFound' variable to be equal to the value outputted from the CV model
personFound = CV_Model(cap, multi_rescue)
if personFound:
if multi_rescue: # If rescueing multiple do not RTL at first target find.
# Store person location
personLocation.append( (vehicle.location.global_frame.lat, vehicle.location.global_frame.lon) )
print("\nFOUND PERSON AT: ({0:.4f}, {1:.4f})" .format(personLocation[-1][-2], \
personLocation[-1][-1])) # Last appended lat & lon
bodyLocation = "\nFound person at: ({0:.4f}, {1:.4f})" .format(personLocation[-1][-2], personLocation[-1][-1])
# Lower flag to not trigger again, count person, and sleep for 2 seconds to avoid detecting same person.
personFound = False
numOfRescued += 1
sleep(2)
else: # Finds 1 target and RTL.
print("\nFOUND PERSON AT: ({0:.4f}, {1:.4f})" .format(vehicle.location.global_frame.lat, \
vehicle.location.global_frame.lon)) # Last appended lat & lon
print("Mission Complete!")
bodyLocation = "\nFOUND PERSON AT: ({0:.4f}, {1:.4f})" .format(vehicle.location.global_frame.lat, vehicle.location.global_frame.lon)
land()
# Stop the camera
cap.release()
cv2.destroyAllWindows()
print("Remaining distance: {0:.2f}m | Speed: {1:.2f}mph" .format(distance, velocity))
#DroneAction = "\nHeading to waypoint {}: {}\n" .format(index, wp)
DroneAction = "Waypoint:" + str(index) + " | Remaining distance: {0:.2f}m".format(distance)
telemetry()
distance = get_distance_meters(vehicle.location.global_frame, destination)
# Increment count. At count = 10 the code will detect if the distance has changed or not.
count += 1
# Used to slow down the amount output printed.
sleep(.5)
#vehicle.simple_goto(destination)
'''NOTE: NOT WORKING - Uncomment next two lines to test Retreat.'''
#sleep(4)
#stopDrone = True
'''Uncomment next two lines to test Retreat.'''
#sleep(4)
#Retreat = True
'''Uncomment next two lines to test emergencyLand.'''
#sleep(4)
#emergencyLand = True
'''Uncomment next two lines to test finding people.'''
#if index == 4:
#sleep(3)
#personFound = True
# If user presses Retreat button exit and RTL
if Retreat:
print("\n-------------------------------------"
"\nRETREAAAAT!"
"\n-------------------------------------")
DroneAction = "Retreating..."
land()
Retreat = False
# If user presses emergency land button, drone lands.
# Vehicle object closed and script exits.
if emergencyLand:
print("\n-------------------------------------"
"\nEMERGENCY LAND!"
"\n-------------------------------------")
DroneAction = "Emergency Landing..."
land()
emergencyLand = False
# If battery low, RTL.
if batteryPercent <= 20:
print("\n-------------------------------------"
"\nBATTERY LOW! END OF MISSION."
"\n-------------------------------------")
DroneAction = "Battery Low... Ending Mission"
sleep(.5)
#emergencyLand = True
land()
index += 1
land()
from math import sin, cos, sqrt, atan2, radians
import RPi.GPIO as GPIO
import time
import subprocess
GPIO.setwarnings(False)
#---------USER INPUTS-------------
target_lat = 52.2296756 # Where the dispenser lands
target_lng = 21.0122287 # Where the dispenser lands
impact_lat = 41.390997 # Where the payload lands
impact_lng = -73.953330 # Where the payload lands
impact_alt = 5 # The altitude where the payload lands
impact_point = LocationGlobalRelative(impact_lat, impact_lng, impact_alt) # Sets location for impact point
dist_thresh = 9000000 # Distance Threshold
alt_thresh = 5000 # Altitude Threshold
#-----------SETUP-----------------
loop_flag = 1
alt_flag = 0
low_alt_flag = 0
dist_flag = 0
connection_string = ('/dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00') # Path that RPI looks to connect to Pixhawk
vehicle = connect(connection_string, wait_ready = True) # Connects to vehicle
print("Vehicle Connected, waiting 20 seconds")
time.sleep(1)
while True:
print "Altitude: ", vehicle.location.global_relative_frame.alt
if vehicle.location.global_relative_frame.alt >= targetAltitude * 0.95:
print "Reached target altitude"
break
time.sleep(1)
# 座標指定で移動1 北
# 目標ロケーションの設定
target1_location_lat = vehicle.location.global_relative_frame.lat + 0.002
target1_location_lon = vehicle.location.global_relative_frame.lon
target1_location_alt = 30
aLocation = LocationGlobalRelative(target1_location_lat, target1_location_lon,
target1_location_alt)
vehicle.simple_goto(aLocation)
# 目標のロケーションに達するまで待つ
# 場所検知がうまくいかないため、時間指定(60秒)待つ
#while True:
for i in range(60):
# ロケーション情報の表示
### このように常にモニタリングしておきたい属性がある場合はリスナー関数を作って登録しておくとすっきりします。
### https://dronekit-python.readthedocs.io/en/latest/automodule.html#dronekit.Vehicle.add_attribute_listener
print "\n Current location: %s " % vehicle.location.global_relative_frame
print "\n Traget location: %s " % aLocation
# if vehicle.location.global_relative_frame.lat >= target1_location_lat * 0.9:
# print "Reached target lat"
def move_to_position(self, lat, lon, alt):
location = LocationGlobalRelative(lat, lon, alt)
self.vehicle.simple_goto(location)
print " "
print "Altitude = %.0fcm" % z_cm
print "Marker found x = %5.0f cm y = %5.0f cm -> angle_x = %5f angle_y = %5f" % (
x_cm, y_cm, angle_x * rad_2_deg, angle_y * rad_2_deg)
north, east = uav_to_ne(x_cm, y_cm, vehicle.attitude.yaw)
print "Marker N = %5.0f cm E = %5.0f cm Yaw = %.0f deg" % (
north, east, vehicle.attitude.yaw * rad_2_deg)
marker_lat, marker_lon = get_location_metres(
uav_location, north * 0.01, east * 0.01)
#-- If angle is good, descend
if check_angle_descend(angle_x, angle_y, angle_descend):
print "Low error: descending"
location_marker = LocationGlobalRelative(
marker_lat, marker_lon,
uav_location.alt - (land_speed_cms * 0.01 / freq_send))
else:
location_marker = LocationGlobalRelative(
marker_lat, marker_lon, uav_location.alt)
vehicle.simple_goto(location_marker)
print "UAV Location Lat = %.7f Lon = %.7f" % (uav_location.lat,
uav_location.lon)
print "Commanding to Lat = %.7f Lon = %.7f" % (
location_marker.lat, location_marker.lon)
#--- COmmand to land
if z_cm <= land_alt_cm:
if vehicle.mode == "GUIDED":
print(" -->>COMMANDING TO LAND<<")
def carrymission(target):
actualdistance = calculatedistance(target,
vehicle.location.global_relative_frame)
vehicle.simple_goto(target)
while vehicle.mode == "GUIDED":
currentdist = calculatedistance(target,
vehicle.location.global_relative_frame)
if currentdist < 0.01 * actualdistance:
time.sleep(2)
break
time.sleep(2)
time.sleep(1)
print("target reached")
return None
wp1 = LocationGlobalRelative(10.12057624, 76.35245919, 10)
vehicle = connectmycopter()
armandtakeoff(10)
carrymission(wp1)
vehicle.mode = VehicleMode("LAND")
while vehicle.mode != "LAND":
print("waiting to change to land")
print("changedtoland")
while true:
sleep(1)
vehicle.close()
def filterROI(self):
'''Filters app ROI using a 5th order linear filter and calculates an associated roi velocity'''
# store last 5 raw roi values
self.rawROIQueue.append(self.rawROI)
# only run filter if we have enough data in the queues.
if len(self.rawROIQueue) == MIN_RAW_ROI_QUEUE_LENGTH and len(
self.filteredROIQueue) == MIN_FILT_ROI_QUEUE_LENGTH:
num = [
0, 0.000334672774973874, 0.00111965413719632,
-0.000469533537393159, -0.000199779184127412
]
den = [
1, -3.48113699710809, 4.56705782792063, -2.67504447769757,
0.589908661075676
]
filteredLat = (
(num[4] * self.rawROIQueue[0].lat + num[3] *
self.rawROIQueue[1].lat + num[2] * self.rawROIQueue[2].lat +
num[1] * self.rawROIQueue[3].lat +
num[0] * self.rawROIQueue[4].lat) -
(den[4] * self.filteredROIQueue[0].lat +
den[3] * self.filteredROIQueue[1].lat +
den[2] * self.filteredROIQueue[2].lat +
den[1] * self.filteredROIQueue[3].lat)) / den[0]
filteredLon = (
(num[4] * self.rawROIQueue[0].lon + num[3] *
self.rawROIQueue[1].lon + num[2] * self.rawROIQueue[2].lon +
num[1] * self.rawROIQueue[3].lon +
num[0] * self.rawROIQueue[4].lon) -
(den[4] * self.filteredROIQueue[0].lon +
den[3] * self.filteredROIQueue[1].lon +
den[2] * self.filteredROIQueue[2].lon +
den[1] * self.filteredROIQueue[3].lon)) / den[0]
filteredAlt = ROI_ALT_FILTER_GAIN * self.filteredROIQueue[-1].alt + (
1 - ROI_ALT_FILTER_GAIN
) * self.rawROI.alt # index -1 should refer to most recent value in the queue.
self.filteredROI = LocationGlobalRelative(filteredLat, filteredLon,
filteredAlt)
else:
self.filteredROI = self.rawROI
# store last 4 filtered roi values
self.filteredROIQueue.append(self.filteredROI)
# only called once - when we have an ROI from phone
if len(self.filteredROIQueue) == 1:
# initialize ROI velocity for Guided controller
self.roiVelocity = Vector3(0, 0, 0)
# initialize the altitude maintained for each controller
# the first ROI sent from the app is supposed to have 0 altitude, but in case it doesn't subtract it out.
self.followControllerAltOffset = self.vehicle.location.global_relative_frame.alt - self.rawROI.alt
logger.log(
'[Follow Me]: First ROI received from app has altitude: %f m' %
self.rawROI.alt)
# go into Look At Me as default state. iOS app changes state to Orbit after 3 seconds.
self.initState(FOLLOW_LOOKAT)
elif len(self.filteredROIQueue) > 1 and self.roiDeltaTime:
#calculate vector from previous to new roi in North,East,Up
vec = location_helpers.getVectorFromPoints(
self.filteredROIQueue[-2], self.filteredROIQueue[-1])
# calculate velocity based on time difference
# roiVelocity in NEU frame
self.roiVelocity = vec / self.roiDeltaTime
# calculate desiredYaw and desiredPitch from new ROI
self.desiredYaw, self.desiredPitch = location_helpers.calcYawPitchFromLocations(
self.vehicle.location.global_relative_frame, self.filteredROI)
#x component accel limit
if self.roiVelocity.x > self.translateVel.x:
self.translateVel.x += ACCEL_PER_TICK
self.translateVel.x = min(self.translateVel.x, self.roiVelocity.x)
elif self.roiVelocity.x < self.translateVel.x:
self.translateVel.x -= ACCEL_PER_TICK
self.translateVel.x = max(self.translateVel.x, self.roiVelocity.x)
#y component accel limit
if self.roiVelocity.y > self.translateVel.y:
self.translateVel.y += ACCEL_PER_TICK
self.translateVel.y = min(self.translateVel.y, self.roiVelocity.y)
elif self.roiVelocity.y < self.translateVel.y:
self.translateVel.y -= ACCEL_PER_TICK
self.translateVel.y = max(self.translateVel.y, self.roiVelocity.y)
#z component accel limit
if self.roiVelocity.z > self.translateVel.z:
self.translateVel.z += ACCEL_PER_TICK
self.translateVel.z = min(self.translateVel.z, self.roiVelocity.z)
elif self.roiVelocity.z < self.translateVel.z:
self.translateVel.z -= ACCEL_PER_TICK
self.translateVel.z = max(self.translateVel.z, self.roiVelocity.z)
def test_goto(connpath):
vehicle = connect(connpath, wait_ready=True)
# NOTE these are *very inappropriate settings*
# to make on a real vehicle. They are leveraged
# exclusively for simulation. Take heed!!!
vehicle.parameters['FS_GCS_ENABLE'] = 0
vehicle.parameters['FS_EKF_THRESH'] = 100
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
# Don't let the user try to fly autopilot is booting
i = 60
while not vehicle.is_armable and i > 0:
time.sleep(1)
i = i - 1
assert_equals(vehicle.is_armable, True)
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
i = 60
while vehicle.mode.name != 'GUIDED' and i > 0:
# print " Waiting for guided %s seconds..." % (i,)
time.sleep(1)
i = i - 1
assert_equals(vehicle.mode.name, 'GUIDED')
# Arm copter.
vehicle.armed = True
i = 60
while not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0:
# print " Waiting for arming %s seconds..." % (i,)
time.sleep(1)
i = i - 1
assert_equals(vehicle.armed, True)
# Take off to target altitude
vehicle.simple_takeoff(aTargetAltitude)
# Wait until the vehicle reaches a safe height before
# processing the goto (otherwise the command after
# Vehicle.simple_takeoff will execute immediately).
while True:
# print " Altitude: ", vehicle.location.global_relative_frame.alt
# Test for altitude just below target, in case of undershoot.
if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
# print "Reached target altitude"
break
assert_equals(vehicle.mode.name, 'GUIDED')
time.sleep(1)
arm_and_takeoff(10)
# print "Going to first point..."
point1 = LocationGlobalRelative(-35.361354, 149.165218, 20)
vehicle.simple_goto(point1)
# sleep so we can see the change in map
time.sleep(3)
# print "Going to second point..."
point2 = LocationGlobalRelative(-35.363244, 149.168801, 20)
vehicle.simple_goto(point2)
# sleep so we can see the change in map
time.sleep(3)
# print "Returning to Launch"
vehicle.mode = VehicleMode("RTL")
vehicle.close()
break
time.sleep(1)
arm_and_takeoff(ALT)
dist = 0
while 1:
a = vehicle.location.global_frame.lon #get Longitude and Latitude
b = vehicle.location.global_frame.lat
print "longitude=",a
print "latitude=",b
lat = 0
lon = 0.000011503*STEP_SIZE # move in this direction with the distance of STEP_SIZE. (in degrees)
new_lat = b + lat
new_lon = a + lon
print "Going towards east point"
point1 = LocationGlobalRelative(new_lat, new_lon, ALT) #set destination accordinag to the step size of the drone.
vehicle.simple_goto(point1) #give command to the drone so it will go in that direction
time.sleep(5) #give it 5 seconds to reach there
yaw = vehicle.attitude.yaw #get yaw
yaw = math.degrees(yaw) #convert it in degrees
alpha = math.degrees(np.arctan2(lat,lon)) # It is the direction of the drone in Polar Coordinate System
if alpha < 0:
alpha = alpha + 360
alpha = alpha + 90 # The camera attached with the drone is 90 degrees rotated respect to the drone
direction = yaw + alpha # actual direction of the picture captured by the raspberry pi
dist = dist + STEP_SIZE
data = struct.pack('ff',direction,STEP_SIZE) #pack direction and step size in one packet
s.send(data) #send that packet to the raspberry pi
data = s.recv(1024) # wait for the acknowledgement from the raspberry pi
if data == 'pic_taken':
print "confirmation : pic has taken"
if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
print("Reached target altitude")
break
time.sleep(1)
#get home/landing point GPS
start_point=vehicle.location.global_relative_frame
arm_and_takeoff(3)
T1=time.time()
print("Set default/target airspeed to 3")
vehicle.airspeed = 3
point1 = LocationGlobalRelative(-35.361354, 149.165218,3)
point2 = LocationGlobalRelative(-35.363244, 149.168801,3)
point3 = LocationGlobalRelative(-35.365489, 149.164801,3)
point4 = LocationGlobalRelative(-35.361452, 149.161801,3)
i=0
N=8
while i<N
print("Going towards P1")
fly_to_GPS(point1)
print("Going towards P2")
fly_to_GPS(point2)
print("Going towards P3")
fly_to_GPS(point3)
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 >= aTargetAltitude * 0.95:
print("Reached target altitude")
break
time.sleep(1)
arm_and_takeoff(3)
print("Set default/target airspeed to 3")
vehicle.airspeed = 3
print("Going towards first point for 30 seconds ...")
point1 = LocationGlobalRelative(-35.361354, 149.165218,2)
vehicle.simple_goto(point1)
# sleep so we can see the change in map
time.sleep(10)
print("Returning to Launch")
vehicle.mode = VehicleMode("RTL")
# Close vehicle object before exiting script
print("Close vehicle object")
vehicle.close()
if Altitude >= TargetAltitude * 0.95:
print("Altitude reached")
break
#Vehicle Connection
#Esto es basicamente la direccion que tiene el dron para que lo podamos conectar en las terminales de Ubuntu.
#Y la velocidad a la que esta yendo al igual que la altura que queremos.
drone = connect('127.0.0.1:14551', wait_ready=True)
arm_and_takeoff(20)
drone.airspeed = 10
#Estas cuatro lineas de codigo son para darles las cordenadas a nuestro dron de a donde tiene que ir en la mision.
a_location = LocationGlobalRelative(20.7377260, -103.4570155, 20)
b_location = LocationGlobalRelative(20.7376890, -103.4565404, 20)
c_location = LocationGlobalRelative(20.7371951, -103.4565840, 20)
d_location = LocationGlobalRelative(20.7372334, -103.4570624, 20)
print("Despegue Exitoso")
print("Going to point a")
#Esto que queda sirve para que el dron vaya a las cordenadas y regrese a casa.
drone.simple_goto(a_location)
time.sleep(40)
print("a location reached going to point b")
drone.simple_goto(b_location)
time.sleep(40)
print("b location reached going to point c")
drone.simple_goto(c_location)
'd', 0
) # initializing direction value 0-> no val 1->right 2->centre 3->left
p1 = multiprocessing.Process(
target=cv
) # assigning parallel processes to detect faces and collision simultaneously
p2 = multiprocessing.Process(target=collision)
p1.start()
p2.start()
loc = [28.46951466793185, 77.53698735091803] # vehicle home location
vehicle = connectMyCopter()
arm_and_takeoff(5)
pt1 = LocationGlobalRelative(loc[0], loc[1], 10)
vehicle.simple_goto(pt1)
while True:
print("the direction i'm receiving is ", direc.value)
d = collision()
if d < 20:
print('collsion')
set_velocity_body(vehicle, 0, gnd_speed, 0)
vehicle.simple_goto(pt1)
print vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon
def excecuteMission(vehicle):
print("Excecuting Mission")
waypoint = LocationGlobalRelative(-35.362270,149.165091,20)
vehicle.simple_goto(waypoint)
time.sleep(30)
#vehicle.mode = VehicleMode("LAND")
vehicle.armed = False
time.sleep(1)
currstate = 'idle'
#currstate='land'
elif currstate == 'arm':
print("main thread enter arm state")
arm_and_takeoff(2)
print("Set default/target airspeed to 3")
vehicle.airspeed = 3
currstate = 'armed'
elif currstate == 'wp':
print("main thread enter wp state")
print("Going towards first point for 30 seconds ...")
point1 = LocationGlobalRelative(-35.361354, 149.165218, 20)
vehicle.simple_goto(point1)
# sleep so we can see the change in map
time.sleep(30)
print(
"Going towards second point for 30 seconds (groundspeed set to 10 m/s) ..."
)
point2 = LocationGlobalRelative(-35.363244, 149.168801, 20)
vehicle.simple_goto(point2, groundspeed=10)
# sleep so we can see the change in map
time.sleep(30)
currstate = 'rtl'
elif currstate == 'localwp':
print(
def _make_relative_location(self, x, y, z=0.0):
return LocationGlobalRelative(x, y, z)
