def visit_waypoint(self, waypoint):
"""
Fly to the coordinates of the waypoint
This function only returns when the solo has visited the waypoint
Args:
waypoint: a WayPoint
"""
# Here we don't need to take the lock, since we want to be able to send heartbeats while we visit waypoints
location = LocationGlobalRelative(lat=waypoint.location.latitude, lon=waypoint.location.longitude, alt=self.height)
self.vehicle.simple_goto(location=location, airspeed=self.speed)
latlon_to_m = 1.113195e5 # converts lat/lon to meters
while self.vehicle.mode == "GUIDED":
veh_loc = self.vehicle.location.global_relative_frame
diff_lat_m = (location.lat - veh_loc.lat) * latlon_to_m
diff_lon_m = (location.lon - veh_loc.lon) * latlon_to_m
diff_alt_m = location.alt - veh_loc.alt
dist_xyz = math.sqrt(diff_lat_m**2 + diff_lon_m**2 + diff_alt_m**2)
if dist_xyz > self.distance_threshold and not self.is_halted:
time.sleep(0.5)
else:
if not self.is_halted:
self.logger.info("Solo arrived at waypoint")
else:
self.logger.info("Solo was halted")
self.is_halted = False # reset the self.is_halted attribute
return
def north_west_20():
############################################### Target Location 20 meters North West ####################################
print('Move 20 meters North West')
startingLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon)
target_lat = vehicle.location.global_relative_frame.lat + (0.00015)
target_lon = vehicle.location.global_relative_frame.lon - (0.00015)
target_alt = vehicle.location.global_relative_frame.alt
targetLocation = LocationGlobalRelative(target_lat,target_lon, target_alt)
# Go to destination
vehicle.simple_goto(targetLocation)
# Just wait for it to get there!
print("ARE WE THERE YET?")
print('')
currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon)
while vehicle.mode == 'GUIDED':
stdout.write("\033[F") #back to previous line
stdout.write("\033[K") #clear line
currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon)
distance_to_target = get_distance_meters(currentLocation, targetLocation)
print ('Distance: {0} Ground speed: {1} '.format(distance_to_target,vehicle.groundspeed))
time.sleep(1)
if distance_to_target <= .5:
print('Destination Reached')
break
# How close did it get?
endLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon)
print("Starting position: " + str(startingLocation.lat) + ", " + str(startingLocation.lon))
print("Target position: " + str(targetLocation.lat) + ", " + str(targetLocation.lon))
print("End position: " + str(endLocation.lat) + ", " + str(endLocation.lon))
print('')
def rtl_10():
################################# Return to launch with altitude of 10 meters ########################################
print('Return to launch with altitude of 10 meters')
startingLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon)
target_lat = launchLocation.lat
target_lon = launchLocation.lon
target_alt = 10
targetLocation = LocationGlobalRelative(target_lat,target_lon,target_alt)
# Go to destination
vehicle.simple_goto(targetLocation)
# Just wait for it to get there!
print("ARE WE THERE YET?")
print()
currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon)
while vehicle.mode == 'GUIDED':
stdout.write("\033[F") #back to previous line
stdout.write("\033[K") #clear line
currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon)
distance_to_target = get_distance_meters(currentLocation, targetLocation)
print ('Distance: {0} Ground speed: {1} '.format(distance_to_target,vehicle.groundspeed))
time.sleep(1)
if distance_to_target <= .5:
print('Destination Reached')
break
# How close did it get?
endLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon)
print("Starting position: " + str(startingLocation.lat) + ", " + str(startingLocation.lon))
print("Target position: " + str(targetLocation.lat) + ", " + str(targetLocation.lon))
print("End position: " + str(endLocation.lat) + ", " + str(endLocation.lon))
print('')
def goInDirection(vehicle, direction, d, loc):
pos = vehicle.location.global_relative_frame
if direction == "NW":
x = pos.lat + .01
z = pos.lon - .01
target_point = LocationGlobalRelative(x, z, 10)
vehicle.simple_goto(target_point)
cur_pos = vehicle.location.global_relative_frame
dist = distance.distance((pos.lat, pos.lon), (cur_pos.lat, cur_pos.lon)).meters
while dist < d and vehicle.mode.name == "GUIDED":
print(cur_pos)
time.sleep(.3)
cur_pos = vehicle.location.global_relative_frame
dist = distance.distance((pos.lat, pos.lon), (cur_pos.lat, cur_pos.lon)).meters
loc.add_xcor(cur_pos.lon)
loc.add_ycor(cur_pos.lat)
if direction == "E":
x = pos.lon + .01
target_point = LocationGlobalRelative(pos.lat, x, 15)
vehicle.simple_goto(target_point)
cur_pos = vehicle.location.global_relative_frame
dist = distance.distance((pos.lat, pos.lon), (cur_pos.lat, cur_pos.lon)).meters
while dist < d and vehicle.mode.name == "GUIDED":
time.sleep(.3)
print(cur_pos)
cur_pos = vehicle.location.global_relative_frame
dist = distance.distance((pos.lat, pos.lon), (cur_pos.lat, cur_pos.lon)).meters
loc.add_xcor(cur_pos.lon)
loc.add_ycor(cur_pos.lat)
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
print("Basic pre-arm checks")
print("Arming motors")
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
print("Vehicle armed!")
#Takeoff
print("Taking off!")
vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude
lat = vehicle.location.global_relative_frame.lat
lon = vehicle.location.global_relative_frame.lon
alt = vehicle.location.global_relative_frame.alt
print('Current location after takeoff is: {0},{1},{2}'.format(lat,lon,alt))
# 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)
def create_vehicle():
# Connect to the Vehicle.
# Set `wait_ready=True` to ensure default attributes are populated before `connect()` returns.
print "\nConnecting to vehicle on: %s" % connection_string
vehicle = connect(connection_string, wait_ready=True)
print "Arming motors"
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
# Confirm vehicle armed before attempting to take off
print "Taking off!"
vehicle.simple_takeoff(target_altitude) # Take off to target altitude
# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
# after Vehicle.simple_takeoff will execute immediately).
print "Altitude: %s" % vehicle.location.global_relative_frame.alt
aTargetAltitude = 30
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)
return vehicle
def get_camera_resolution(self):
self.logger.debug("sending gopro mavlink message")
command = mavlink.GOPRO_COMMAND_VIDEO_SETTINGS
msg = self.vehicle.message_factory.gopro_get_request_encode(0,
mavlink.MAV_COMP_ID_GIMBAL, # target system, target component
command)
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
# wait some time for the video settings to be updated
time.sleep(1)
num_resolution = self.goproManager.videoResolution
# we will only handle a subpart of all available resolutions
# the others will not be used
if num_resolution == GOPRO_RESOLUTION.GOPRO_RESOLUTION_480p:
return 480
elif num_resolution == GOPRO_RESOLUTION.GOPRO_RESOLUTION_720p:
return 720
elif num_resolution == GOPRO_RESOLUTION.GOPRO_RESOLUTION_960p:
return 960
elif num_resolution == GOPRO_RESOLUTION.GOPRO_RESOLUTION_1080p:
return 1080
elif num_resolution == GOPRO_RESOLUTION.GOPRO_RESOLUTION_1440p:
return 1440
else:
return 0 # something went wrong
def land(self):
self.solo_lock.acquire()
self.vehicle.mode = VehicleMode("LAND")
while self.vehicle.mode != "LAND":
self.vehicle.mode = VehicleMode("LAND")
time.sleep(0.1)
self.logger.info("Landing Solo...")
self.solo_lock.release()
def travel(targetLoc, currLoc):
while vehicle.mode.name == 'GUIDED':
if get_distance_meters(targetLoc, currLoc) < 1:
break
time.sleep(1)
currLat = currLoc.lat
currLon = currLoc.lon
print(" Current Lat: " + str(currLat) + ", Lon: " + str(currLon))
def changeAlt(vehicle, altitude_target):
cur_location = vehicle.location.global_relative_frame
target_point = LocationGlobalRelative(cur_location.lat, cur_location.lon, altitude_target)
vehicle.simple_goto(target_point)
while vehicle.mode.name == "GUIDED":
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
if vehicle.location.global_relative_frame.alt >= altitude_target * 0.95:
print("Reached target altitude")
break
time.sleep(.5)
def custom_sleep(drone_model, sleep_time):
current_time = 0
while (current_time < sleep_time):
lat = vehicle.location.global_relative_frame.lat
lon = vehicle.location.global_relative_frame.lon
#drone_model.update_status(lat,lon)
#ws.send(drone_model.toJSON())
print('Current location is: {0},{1}'.format(lat, lon))
time.sleep(1)
current_time += 1
def goto(destination, vehicle):
print("Target Coords: " + str(destination))
vehicle.simple_goto(destination)
totalDistance = getDistance(vehicle.location.global_relative_frame,
destination)
while vehicle.mode.name == "GUIDED":
distanceFromTarget = getDistance(
vehicle.location.global_relative_frame, destination)
if distanceFromTarget < (totalDistance * .03):
print("Reached destination!")
break
time.sleep(5)
def my_return_function(vehicle, home_lat, home_lon, home_alt):
home = (home_lat, home_lon)
home_point = LocationGlobalRelative(home_lat, home_lon, home_alt)
vehicle.simple_goto(home_point)
while vehicle.mode == "GUIDED":
lat, lon, alt = print_location(vehicle, mute=True)
curr = (lat, lon)
dist = distance.distance(home, curr)
if dist < .5 or dist > 25:
break
time.sleep(.5)
def initialize_drone(
self,
vehicle_num,
home="41.7144367,-86.2417136,221,0",
connection_string="",
):
self.vehicle_id = "UAV-" + str(vehicle_num)
print("\nInitializing drone: " + self.vehicle_id)
parser = argparse.ArgumentParser(
description='Print out vehicle state information')
parser.add_argument(
'--connect',
help=
"vehicle connection target string. If not specified, SITL automatically started and used"
)
args = parser.parse_args()
connection_string = args.connect
sitl = None
#Start SITL if no connection string specified
if not connection_string:
import dronekit_sitl
# connection_string = sitl.connection_string()
ardupath = "/home/uav/git/ardupilot"
self.home = home # In this example, all UAVs start on top of each other!
sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest',
'default_params', 'copter.parm')
sitl_args = [
'-I{}'.format(vehicle_num), '--home', home, '--model', '+',
'--defaults', sitl_defaults
]
sitl = dronekit_sitl.SITL(path=os.path.join(
ardupath, 'build', 'sitl', 'bin', 'arducopter'))
sitl.launch(sitl_args, await_ready=True)
tcp, ip, port = sitl.connection_string().split(':')
print(port + " " + str(vehicle_num))
port = str(int(port) + vehicle_num * 10)
connection_string = ':'.join([tcp, ip, port])
################################################################################################
# Connect to the Vehicle
################################################################################################
print 'Connecting to vehicle on: %s' % connection_string
self.vehicle = connect(connection_string, wait_ready=True)
self.vehicle.wait_ready(timeout=120)
return self.vehicle, sitl
time.sleep(10)
def flyHome(vehicle, point):
pos = vehicle.location.global_relative_frame
vehicle.simple_goto(point)
dist = distance.distance((pos.lat, pos.lon), (point.lat, point.lon)).meters
alt_dist = abs(point.alt - pos.alt)
while dist > 1 and vehicle.mode.name == "GUIDED" and alt_dist > .5:
print("Distance %f" % dist)
print(pos)
time.sleep(.3)
pos = vehicle.location.global_relative_frame
alt_dist = abs(point.alt - pos.alt)
dist = distance.distance((pos.lat, pos.lon),
(point.lat, point.lon)).meters
def adjust_altitude(height):
while vehicle.mode.name == "GUIDED":
altitudeIncrease = LocationGlobalRelative(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, height)
vehicle.simple_goto(altitudeIncrease)
remaining_distance = abs(height - vehicle.location.global_frame.alt)
print(remaining_distance)
if remaining_distance < 0.10:
location = vehicle.location.global_frame
print("Reached target altitude {}. Lat: {}, Lon: {}".format(location.alt, location.lat, location.lon))
break;
time.sleep(0.5)
def arm(self):
self.solo_lock.acquire()
self.vehicle.mode = VehicleMode("GUIDED")
while self.vehicle.mode != "GUIDED":
time.sleep(0.1)
self.logger.debug("control granted")
if self.vehicle.armed is False:
# Don't let the user try to arm until autopilot is ready
self.logger.debug("waiting for vehicle to initialise...")
while not self.vehicle.is_armable:
time.sleep(1)
self.vehicle.armed = True
self.logger.info("the solo is now armed")
self.solo_lock.release()
def goto_target(target_location):
vehicle.simple_goto(target_location)
while vehicle.mode.name == "GUIDED":
remaining_distance = get_distance_meters(vehicle.location.global_frame, target_location)
#x = vehicle.location.global_frame.lat
#y = vehicle.location.global_frame.lon
#x_pos.append(x)
#y_pos.append(y)
if remaining_distance < 1:
print("Reached target-> lat: {} lon: {} alt: {}".format(vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, vehicle.location.global_frame.alt))
break;
time.sleep(0.5)
def run(self):
while not self.quit:
if self.waypoint_queue.is_empty():
time.sleep(1)
else:
self.logger.debug("getting waypoint")
waypoint = self.waypoint_queue.remove_waypoint()
self.logger.info("the solo is flying to a new waypoint")
self.solo.visit_waypoint(waypoint)
self.logger.info("the solo arrived at the waypoint")
time.sleep(0.1)
if self.rth and not self.waypoint_queue.is_empty():
home = self.waypoint_queue.remove_waypoint()
self.logger.info("the solo is returning to his launch location")
self.solo.visit_waypoint(home)
self.solo.land()
def control_gimbal(self, pitch=None, roll=None, yaw=None):
self.solo_lock.acquire()
self.logger.info("Operating Gimbal...")
gmbl = self.vehicle.gimbal
if pitch is None:
pitch = gmbl.pitch()
if roll is None:
roll = gmbl.roll()
if yaw is None:
yaw = gmbl.yaw()
while gmbl.pitch != pitch:
gmbl.rotate(pitch, roll, yaw)
print gmbl.pitch
time.sleep(0.1)
gmbl.release
self.logger.info("Gimbal Operation Complete")
self.solo_lock.release()
def get_camera_fps(self):
self.logger.debug("sending gopro mavlink message")
command = mavlink.GOPRO_COMMAND_VIDEO_SETTINGS
msg = self.vehicle.message_factory.gopro_get_request_encode(0,
mavlink.MAV_COMP_ID_GIMBAL, # target system, target component
command)
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
# wait some time for the video settings to be updated
time.sleep(1)
num_frame_rate = self.goproManager.videoFrameRate
if num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_12:
return 12
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_15:
return 15
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_24:
return 24
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_25:
return 25
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_30:
return 30
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_48:
return 48
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_50:
return 50
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_60:
return 60
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_80:
return 80
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_90:
return 90
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_100:
return 100
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_120:
return 120
elif num_frame_rate == GOPRO_FRAME_RATE.GOPRO_FRAME_RATE_240:
return 250
else:
return 0 # something went wrong
def goto(targetLocation, gotoFunction=vehicle.simple_goto):
#Determine where to fly the drone
current = vehicle.location.global_relative_frame
targetDistance = get_distance(current, targetLocation)
#Fly the drone
gotoFunction(targetLocation)
#Fly until reached tolerance of destination
while vehicle.mode.name == "GUIDED":
#Update server
nowLocation = vehicle.location.global_relative_frame
#Print information
print("Location: %s, %s, - Alt: %s" %
(nowLocation.lat, nowLocation.lon, nowLocation.alt))
#Check tolerance
remaining = get_distance(nowLocation, targetLocation)
if remaining <= targetDistance * 0.05:
print("Reached target")
break
#Sleep
time.sleep(2)
def takeoff(self):
self.solo_lock.acquire()
if self.vehicle.mode != 'GUIDED':
self.logger.error("DroneDirectError: 'takeoff({0})' was not executed. \
Vehicle was not in GUIDED mode".format(self.height))
self.solo_lock.release()
return -1
while not self.vehicle.armed:
self.logger.debug("waiting for arming...")
time.sleep(1)
if self.vehicle.system_status != SystemStatus('STANDBY'):
self.logger.debug("solo was already airborne")
loc = self.vehicle.location.global_frame
loc.alt = loc.alt + self.height
self.vehicle.commands.goto(loc)
self.vehicle.flush()
self.logger.debug("command flushed")
self.solo_lock.release()
return
self.logger.info("the solo is now taking off")
self.vehicle.simple_takeoff(self.height)
# Wait until the vehicle reaches a safe height
# while self.vehicle.mode == 'GUIDED':
while True:
self.logger.debug("solo is in {0} mode".format(self.vehicle.mode))
if self.vehicle.location.global_relative_frame.alt >= self.height * 0.95: # Trigger just below target alt.
self.logger.info("the solo is now ready to fly")
self.solo_lock.release()
return 0
time.sleep(1)
# Sometimes the Solo will switch out of GUIDED mode during takeoff
# If this happens, we will return -1 so we can try again
self.logger.error("DroneDirectError: 'takeoff({0})' was interrupted. \
Vehicle was swicthed out of GUIDED mode".format(self.height))
self.solo_lock.release()
return -1
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
print("Basic pre-arm checks")
# Don't try to arm until autopilot is ready
print("home: " + str(vehicle.location.global_relative_frame.lat))
print("Arming motors")
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
print("Taking off!")
vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude
while True:
# Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt >= aTargetAltitude * .95:
print("Reached target altitude")
break
time.sleep(1)
def my_goto_function(vehicle, offset_lat, offset_lon, offset_alt):
lat, lon, alt = print_location(vehicle, mute=True)
start = (lat, lon)
goal_lat, goal_lon, goal_alt = lat + offset_lat, lon + offset_lon, alt + offset_alt
goal_point = LocationGlobalRelative(goal_lat, goal_lon, goal_alt)
goal = (goal_lat, goal_lon)
vehicle.simple_goto(goal_point)
while vehicle.mode == "GUIDED":
lat_temp, lon_temp, alt_temp = print_location(vehicle, mute=True)
curr = (lat_temp, lon_temp)
dist = distance.distance(goal, curr)
if dist < .5 or dist > 25:
break
time.sleep(.5)
time.sleep(10)
lat, lon, alt = print_location(vehicle)
print("TRAVEL COMPLETED: Horizontal Distanced traveled {:.2f}m".format(
distance.distance(start, (lat, lon)).m))
def set_attitude(roll_angle=0.0,
pitch_angle=0.0,
yaw_angle=None,
yaw_rate=0.0,
use_yaw_rate=False,
thrust=0.5,
duration=0):
"""
Note that from AC3.3 the message should be re-sent more often than every
second, as an ATTITUDE_TARGET order has a timeout of 1s.
In AC3.2.1 and earlier the specified attitude persists until it is canceled.
The code below should work on either version.
Sending the message multiple times is the recommended way.
"""
send_attitude_target(roll_angle, pitch_angle, yaw_angle, yaw_rate, False,
thrust)
start = time.time()
while time.time() - start < duration:
send_attitude_target(roll_angle, pitch_angle, yaw_angle, yaw_rate,
False, thrust)
time.sleep(0.1)
# Reset attitude, or it will persist for 1s more due to the timeout
send_attitude_target(0, 0, 0, 0, True, thrust)
def goto(drone, target):
# Get drone location and display
drone_location = drone.location.global_relative_frame
print("\nDrone location: {}, {}".format(drone_location.lat,
drone_location.lon))
print("Flying to target: {}, {}".format(target.lat, target.lon))
# Go to the target location
drone.simple_goto(target)
# Drone location progress loop
while drone.mode == "GUIDED":
# Print distance to target
distance = get_distance_meters(drone.location.global_relative_frame,
target)
print("Distance to target: {} m".format(distance))
# Stop if the drone is within 5 meters of the target
if distance < 5:
break
time.sleep(0.5)
def hover_5():
#################################################### Hover for 5 seconds ################################################
print('Hovering for 5 seconds:')
print('5')
time.sleep(1)
stdout.write("\033[F") #back to previous line
stdout.write("\033[K") #clear line
print('4')
time.sleep(1)
stdout.write("\033[F") #back to previous line
stdout.write("\033[K") #clear line
print('3')
time.sleep(1)
stdout.write("\033[F") #back to previous line
stdout.write("\033[K") #clear line
print('2')
time.sleep(1)
stdout.write("\033[F") #back to previous line
stdout.write("\033[K") #clear line
print('1')
time.sleep(1)
stdout.write("\033[F") #back to previous line
stdout.write("\033[K") #clear line
print('')
def goto_target_compass(current_location, north, south, east, west, final_distance):
lat_change = 0
lon_change = 0
if north:
south = False
lat_change = 10
if south:
north = False
lat_change = -10
if east:
west = False
lon_change = 10
if west:
east = False
lon_change = -10
target_location = LocationGlobalRelative(current_location.lat + lat_change, current_location.lon + lon_change, current_location.alt)
while vehicle.mode.name == "GUIDED":
vehicle.simple_goto(target_location)
remaining_distance = get_distance_meters(current_location, vehicle.location.global_frame)
#x = vehicle.location.global_frame.lat
#y = vehicle.location.global_frame.lon
#x_pos.append(x)
#y_pos.append(y)
if remaining_distance >= final_distance:
print("Reached target-> lat: {} lon: {} alt: {}".format(current_location.lat, current_location.lon, current_location.alt))
break;
time.sleep(0.5)
def arm_and_takeoff(target_altitude):
while not vehicle.is_armable:
print "Waiting for the vehicle to initialize...."
timed.sleep(1)
print "Arming motors"
# Copter should be in GUIDED mode for the
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
while not vehicle.armed:
print "Waiting for the vehicle to arm...."
timed.sleep(1)
print "Taking off!"
vehicle.simple_takeoff(target_altitude)
while True:
print "Altitude: ", vehicle.location.global_relative_frame.alt
if vehicle.location.global_relative_frame.alt >= target_altitude*0.95:
print "Reached target location"
break
timed.sleep(1)
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
print "Basic pre-arm checks"
# Don't try to arm until autopilot is ready
while not vehicle.is_armable:
print " Waiting for vehicle to initialise..."
time.sleep(1)
print "Arming motors"
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
while not vehicle.armed:
print " Waiting for arming..."
time.sleep(1)
print "Taking off!"
vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude
while True:
print " Altitude: ", vehicle.location.global_relative_frame.alt
if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95:
print "Reached target altitude"
break
time.sleep(1)