def emergency_stop(self):
self.vehicle.mode = VehicleMode("STABILIZED")
t = time.time()
while time.time() < (t + 30):
self.__override_controller(0, 0, 0, 0)
try:
VideoStream = cv2.VideoCapture(0) # index of your camera
VideoStream.set(3, 1920)
VideoStream.set(4, 1080)
except:
print "problem opening input stream"
f.write("\n problem opening input stream")
FailTime = 0
while (VideoStream.isOpened()):
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# Still can not find the goal
if (correct == -10):
vehicle.mode = VehicleMode("RTL")
print("Returning to Launch")
f.write("\n\n Returning to Launch")
print("\n Failed")
f.write("\n\n Failed")
break
# check until plane land
if (loc.alt == 0):
vehicle.mode = VehicleMode("LAND")
print("LAND")
f.write("\n LAND")
print("\n Completed")
f.write("\n\n Completed")
break
def is_guided(vehicle):
return vehicle.mode == VehicleMode("GUIDED")
def startMission(self, vehicle):
print "Mission started!"
#print vehicle.location.global_frame
vehicle.commands.next=0
vehicle.mode = VehicleMode("AUTO")
def land_drone(self):
'''Land the drone at its current location.'''
print 'Vehicle {0} landing'.format(self.instance)
self.vehicle.mode = VehicleMode('LAND')
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(10)
print("Set default/target airspeed to 3")
vehicle.airspeed = 2
#print "Going towards first point for 30 seconds ..."
#point1 = LocationGlobalRelative(48.462101, -123.312699, 10)
#vehicle.simple_goto(point1, groundspeed=2)
# sleep so we can see the change in map
time.sleep(5)
print("Returning to Launch")
vehicle.mode = VehicleMode("RTL")
#Close vehicle object before exiting script
print("Close vehicle object")
vehicle.close()
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
0,
0, # x, y, z positions (not used)
velocity_x,
velocity_y,
velocity_z, # x, y, z velocity in m/s
0,
0,
0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)
0,
0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)
# send command to vehicle on 1 Hz cycle
for x in range(0, duration):
print("###### 2 ######")
vehicle.send_mavlink(msg)
time.sleep(1)
# Main
arm_and_takeoff(10)
#send_ned_velocity(3,0,0,5)
#time.sleep(1)
send_ned_velocity(30, 30, 30, 15)
time.sleep(1)
print("###### 3 ######")
vehicle.mode = VehicleMode('LAND')
# Close Connection
vehicle.close()
logging.info("Starting collision avoidance scheme")
t_collision.start()
logging.info('Create a new mission (for current location)')
adds_square_mission(vehicle.location.global_frame, 50)
# From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently).
arm_and_takeoff(10)
logging.info("Starting mission")
# Reset mission set to first (0) waypoint
vehicle.commands.next = 0
# Set mode to AUTO to start mission
vehicle.mode = VehicleMode("AUTO")
# Monitor mission.
# Demonstrates getting and setting the command number
# Uses distance_to_current_waypoint(), a convenience function for finding the
# distance to the next waypoint.
while True:
try:
nextwaypoint = vehicle.commands.next
if vehicle.commands.count != 0:
logging.info('Distance to waypoint (%s): %s', nextwaypoint,
geo.distance_to_current_waypoint(vehicle))
if nextwaypoint == 3: # Skip to next waypoint
logging.info('Skipping to Waypoint 5 when reach waypoint 3')
Returns the distance in meters between two points (lat1,lon1) and (lat2,lon2)
"""
return Geodesic.WGS84.Inverse(lat1, lon1, lat2, lon2)['s12']
TARGET_ALTITUDE = 40
print('conectando al vehiculo')
vehicle = connect('127.0.0.1:14550', wait_ready=True)
print('conectado')
print('chequeando si el vehiculo es armable')
while True:
armable = vehicle.is_armable
if armable: break
print('listo para armarse')
print('cambiando a modo Guiado')
vehicle.mode = VehicleMode('GUIDED')
print('Despegando')
vehicle.armed = True
vehicle.simple_takeoff(TARGET_ALTITUDE)
while True:
if abs(TARGET_ALTITUDE - vehicle.location.global_relative_frame.alt) <= 1:
print('selected altitude was reached')
break
else:
print('Current altitude: ' +
str(vehicle.location.global_relative_frame.alt))
lat = 2.148971
lon = -73.944397
alt = TARGET_ALTITUDE
box_size = 100
paket = sari_paket
elif sira == 'm':
paket = mavi_paket
print("Bırakılan paket: " + sira)
servo_komut(drone, paket, t.servo_acik) # Servo değerini belirle!!
sleep(7)
servo_komut(drone, paket, t.servo_kapali)
sleep(1)
print('Paketler bırakıldı')
### Görev tamam
print('Görev tamam')
## Kalkış konumuna git
drone.mode = VehicleMode("RTL")
while t.hedef_varis(drone, ev_konum):
sleep(1)
print("Kalkış noktasına ulaşıldı")
## İniş
drone.mode = VehicleMode("LAND")
while drone.location.global_relative_frame.alt > 0.25:
print(drone.location.global_relative_frame.alt)
sleep(0.5)
print("İniş gerçekleşti")
## Cihaz kapatma komutları
print("Kapatılıyor")
if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.90:
print("Reached target altitude")
break
time.sleep(1)
def auto_altitude():
cmds = vehicle.commands
cmds.clear()
vehicle.flush()
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 2))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6939372, 35.4610946 , 2))# 2 m
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6939372, 35.4610946 , 1))# 2 m
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6939372, 35.4610946 , 0.75))# 2 m
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6939372, 35.4610946 , 0.5))# 2 m
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6939372, 35.4610946 , 0.4))# 2 m
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6939372, 35.4610946 , 0.3))# 2 m
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6939372, 35.4610946 , 0.25))# 2 m
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6939372, 35.4610946 , 0.15))# 2 m
cmds.upload()
arm_and_takeoff(2)
auto_altitude()
vehicle.commands.next = 0
vehicle.mode = VehicleMode("AUTO")
time.sleep(1)
while vehicle.rangefinder.distance > 0.20:
nextwaypoints = vehicle.commands.next
print("landing")
vehicle.mode = VehicleMode("LOITER")
import time
from dronekit import connect, VehicleMode, LocationGlobal, LocationGlobalRelative
connection_string = "udp:127.0.0.1:14551"
print(f"Connecting to vehicle on: {connection_string}")
vehicle = connect(connection_string, wait_ready=True)
while not vehicle.is_armable:
print(" Waiting for vehicle to initialise...")
time.sleep(1)
print("Arming motors")
vehicle.mode = VehicleMode("MANUAL")
vehicle.armed = True
while not vehicle.armed:
print (" Waiting for arming...")
time.sleep(1)
def setMode(self, mode):
self.vehicle.mode = VehicleMode(mode)
def land_vehicle(self):
self.vehicle.mode = VehicleMode("LAND")
print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90, relative=True)
print("Velocity North and East")
send_global_velocity(NORTH, EAST, 0, DURATION)
send_global_velocity(0, 0, 0, 1)
print("Yaw 90 relative (to previous yaw heading)")
condition_yaw(90, relative=True)
print("Velocity South and East")
send_global_velocity(SOUTH, EAST, 0, DURATION)
send_global_velocity(0, 0, 0, 1)
"""
The example is completing. LAND at current location.
"""
print("Setting LAND mode...")
vehicle.mode = VehicleMode("LAND")
#Close vehicle object before exiting script
print "Close vehicle object"
vehicle.close()
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
print("Completed")
#Original starting location
home = vehicle.location.global_relative_frame
#FLY NORTHWEST < 20 METERS
target1 = get_location(vehicle.location.global_relative_frame, 14.14, -14.14)
goto(target1)
#FLY EAST 20 METERS
target2 = get_location(vehicle.location.global_relative_frame, 0, 20)
goto(target2)
#RETURN TO START
goto(home)
####################################
# Add this at the end
####################################
print("Returning to Launch")
vehicle.mode = VehicleMode("LAND") # Note we replace RTL with LAND
# Close vehicle object before exiting script
print("Close vehicle object")
vehicle.close()
# Close vehicle object before exiting script
vehicle.close()
time.sleep(5)
print("Completed")
def rtl():
print("Vehicle Returning to LAND mode")
vehicle.mode = VehicleMode("RTL")
vehicle.simple_takeoff(tgt_altitude)
#--wait
while True :
altitude = vehicle.location.global_relative_frame.alt
if altitude >= tgt_altitude -1:
print("Altitude reached")
break
time.sleep(1)
#----Main
arm_and_takeoff(10)
vehicle.airspeed = 7
print("go to wp1")
wp1 = LocationGlobalRelative(35.9872609, -958753037, 10)
vehicle.simple_goto(wp1)
time.sleep(30)
print("Coming Back")
vehicle.mode = VehicleMode("Rtl")
time.sleep(20)
vehicle.close
def ChangeMode(vehicle, mode):
while vehicle.mode != VehicleMode(mode):
vehicle.mode = VehicleMode(mode)
time.sleep(0.5)
return True
else:
while not connection: # Use try and except, because the name of the USB connection file may change.
try:
vehicle = connect(connection_string + index, wait_ready=True)
print("Connect to " + connection_string + index)
connection = True
except:
print("Cannot connect to " + connection_string + index)
index = str(int(index) + 1)
# If you still cannot find the file after 10 trials, there's something wrong with the pixhawk connection.
if index.__len__() >= 2:
exit()
# vehicle = connect('192.168.2.2:14555', wait_ready = True) # This is for UDP connection.
mode_string = "ALT_HOLD" # There are "MANUAL", "STABILIZE" and "ALT_HOLD" there modes available.
vehicle.mode = VehicleMode(mode_string)
#Create a message listener for all messages.
@vehicle.on_message('*')
def listener(self, name, message):
print 'message: %s' % message
# You can use this for listening to the channel values.
@vehicle.on_message('ATTITUDE')
def attitude_listener(self, name, msg):
print '---%s' % (msg)
time.sleep(3)
time.sleep(1)
print 'Create a new mission (for current location)'
adds_square_mission(vehicle.location.global_frame,50)
# From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently).
arm_and_takeoff(10)
print "Starting mission"
# Reset mission set to first (0) waypoint
vehicle.commands.next=0
# Set mode to AUTO to start mission
vehicle.mode = VehicleMode("AUTO")
# Monitor mission.
# Demonstrates getting and setting the command number
# Uses distance_to_current_waypoint(), a convenience function for finding the
# distance to the next waypoint.
while True:
nextwaypoint=vehicle.commands.next
print 'Distance to waypoint (%s): %s' % (nextwaypoint, distance_to_current_waypoint())
#if nextwaypoint==3: #Skip to next waypoint
# print 'Skipping to Waypoint 5 when reach waypoint 3'
# vehicle.commands.next = 5
if nextwaypoint==5: #Dummy waypoint - as soon as we reach waypoint 4 this is true and we exit.
print("Current Altitude: %d" %
vehicle.location.global_relative_frame.alt)
if vehicle.location.global_relative_frame.alt >= aTargetAltitude * .95:
break
time.sleep(1)
print("Target altitude reached")
return None
#### Mission################################
vehicle = connectMyCopter()
print("About to takeoff..")
vehicle.mode = VehicleMode("GUIDED")
arm_and_takeoff(2) ##Tell drone to fly 2 meters in the sky
vehicle.mode = VehicleMode(
"LAND") ##Once drone reaches altitude, tell it to land
time.sleep(2)
print("End of function")
print("Arducopter version: %s" % vehicle.version)
while True:
time.sleep(2)
vehicle.close()
### End of script
def find_target_and_land_drone(self, event):
''' Searches for a target and then attempts to land on the target. '''
if ('target' in event):
target = event['target']
else:
target = None
self.target_found = False
self.landing_state = 0 # TODO Change to enumerated value
# 0: Take off and head to target
# 1: At target GPS location, no sighting
# 2: Landing, high altitude
# 3: Landing, medium altitude
# 4: Landing, low altitude
# 5: Landing, on ground
# 9: Abort
#10: Manual control
# Search until either target is found or a timeout is reached
self.landing_state = 1 # 1: At target GPS location, no sighting
# Initialize landing camera hardware and subscription
self.hw_landing_cam = hardware.LandingCamera(
target=target, simulated=self.simulated_landing_camera)
pub.subscribe(self.landing_adjustment_cb,
'sensor-messages.landingcam-data')
print 'Subscribed'
print 'Start search for target'
# TODO Add movement during initial search
# Michael: I think this will work. We can test when we need.
# time_start = time.time()
# timeout = 2 # 1 second to switch modes
# while not(self.pilot.vehicle.mode == VehicleMode('CIRCLE')):
# time_elapsed = time.time() - time_start
# if (time_elapsed >= timeout):
# break
# time.sleep(0.1)
# If switch to CIRCLE was unsuccessful, revert back to stationary hold in GUIDED mode
# if not (self.pilot.vehicle.mode == VehicleMode('CIRCLE')):
# print("Unsuccessful switch to CIRCLE mode")
# self.pilot.vehicle.mode = VehicleMode('GUIDED')
# self.pilot.vehicle.parameters['PLND_ENABLED'] = 1
# self.pilot.vehicle.parameters['PLND_TYPE'] = 1
# self.pilot.vehicle.flush()
logging.info('\'find_target_and_land_drone\', PLND_ENABLED is ' +
str(self.pilot.vehicle.parameters['PLND_ENABLED']))
time_start = time.time()
timeout = 15
# time_switch = 5
while (1):
if (self.target_found == True):
self.pilot.vehicle.mode = VehicleMode('LAND')
self.pilot.vehicle.flush()
print 'Found target, start landing'
logging.info(
'\'find_target_and_land_drone\', Found target and start landing'
)
break
time_elapsed = time.time() - time_start
if (time_elapsed >= timeout
and self.pilot.vehicle.mode == VehicleMode('GUIDED')):
# default 3 turns with 1.5 m radius
self.pilot.send_circle_command()
print 'Target not found, begin circling'
logging.info(
'\'find_target_and_land_drone\', Target not found in %d seconds, begin circle'
% time_elapsed)
time_start = time.time()
if (time_elapsed >= timeout
and self.pilot.vehicle.mode == VehicleMode('CIRCLE')):
self.landing_state = 9 # 9: Abort
print('Target not found in %0.3f seconds' % timeout)
logging.info(
'\'find_target_and_land_drone\', Target not found in %d seconds in circle mode'
% time_elapsed)
break
if not (self.pilot.vehicle.mode == VehicleMode('GUIDED')
or self.pilot.vehicle.mode == VehicleMode('CIRCLE')):
self.landing_state = 10
break
time.sleep(0.1) # TODO Can adjust if different responsiveness is
# required
# changed 0.5 -> 0.1
time_start = time.time()
timeout = 10 # timeout of 5 seconds
'Start landing'
while (self.landing_state in [2, 3, 4]):
if (self.target_found == False):
# TODO Circle around? Adjust height? Loiter?
time_elapsed = time.time() - time_start
if (time_elapsed >= timeout):
self.landing_state = 9 # 9: Abort
print('Target lost for %0.3f seconds during landing' %
time_elapsed)
logging.info(
'\'find_target_and_land_drone\', Target lost for %d seconds during landing. Mission aborted.'
% timeout)
break
else:
time_start = time.time()
if not (self.pilot.vehicle.mode == VehicleMode('GUIDED')
or self.pilot.vehicle.mode == VehicleMode('LAND')):
self.landing_state = 10
break
time.sleep(0.1)
# Either landed or aborted, but stop landing camera
self.hw_landing_cam.stop()
# self.pilot.vehicle.parameters['PLND_ENABLED'] = 0
pub.unsubscribe(self.landing_adjustment_cb,
'sensor-messages.landingcam-data')
if (self.landing_state == 9):
print('Landing aborted. Continuing mission.')
logging.info(
'\'find_target_and_land_drone\', Landing aborted. Continuing mission.'
)
#self.pilot.vehicle.mode = VehicleMode('RTL') # TODO Change to a more
# controlled fly to waypoint
# and land
elif (self.landing_state == 5):
print('Landed successfully')
logging.info('\'find_target_and_land_drone\', Landed successfully')
# TODO Transfer info, add signpost garble here
else:
print('ERROR, landing_state is: ' + str(self.landing_state))
def land():
print "Landing"
vehicle.mode = VehicleMode("LAND")
time.sleep(10)
vehicle.armed = False
print "Ya se apago"
print("targetgps %f %f.." % (targetgps[0], targetgps[1]))
# LocationGlobalRelative(-35.361354, 149.167218, 20)
#vehicles[0].simple_goto(LocationGlobalRelative(v2_lat,v2_lon,20))
vehicles[0].simple_goto(
LocationGlobalRelative(targetgps[0], targetgps[1], 20))
print("v#1 curr loc ...")
print(v1_xy)
print((vehicles[0]._location._lat, vehicles[0]._location._lon))
print("rssi value %f\n\n" % rssival)
time.sleep(5)
# 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) ...")
print("Returning to Launch")
vehicles[0].mode = VehicleMode("RTL")
vehicles[1].mode = VehicleMode("RTL")
# Close vehicles object before exiting script
print("Close vehicles object")
vehicles[0].close()
vehicles[1].close()
# Shut down simulator if it was started.
if sitl:
sitl.stop()
sitl2.stop()
parser.add_argument('--connect')
args = parser.parse_args()
connection_string = args.connect
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
vehicle = connect(connection_string, wait_ready=True)
return vehicle
vehicle = connectMycopter()
while vehicle.is_armable != True:
print("waiting to get armed")
time.sleep(1)
print("vehicle is now armable")
vehicle.mode = VehicleMode("GUIDED")
while vehicle.mode != 'GUIDED':
print("waiting to change to guide")
time.sleep(1)
print("have fun")
vehicle.armed = True
while vehicle.armed == False:
print("waiting")
time.sleep(1)
print("drone armed")
return [w, x, y, z]
print('Create a new mission (for current location)')
adds_square_mission(vehicle.location.global_frame, 2.5)
# From Copter 3.3 you will be able to take off using a mission item. Plane must take off using a mission item (currently).
arm_and_takeoff_nogps(2.5)
print("Starting mission")
# Reset mission set to first (0) waypoint
vehicle.commands.next = 0
# Set mode to AUTO to start mission
vehicle.mode = VehicleMode("AUTO")
# Monitor mission.
# Demonstrates getting and setting the command number
# Uses distance_to_current_waypoint(), a convenience function for finding the
# distance to the next waypoint.
initial_time = time.time()
while True and (time.time() - initial_time <
60) and (vehicle.location.global_relative_frame.alt > 1.5):
nextwaypoint = vehicle.commands.next
print('Distance to waypoint (%s): %s' %
(nextwaypoint, distance_to_current_waypoint()))
if nextwaypoint == 3: #Skip to next waypoint
def RTL_Mode():
print("PreRTL......")
vehicle.mode = VehicleMode("RTL")
if key == ord('w'):
flightAssist.send_ned_velocity(veh_control, 2, 0, 0, 1) #forward
elif key == ord('s'):
flightAssist.send_ned_velocity(veh_control, -2, 0, 0, 1) #backward
elif key == ord('a'):
flightAssist.send_ned_velocity(veh_control, 0, -2, 0, 1) #left
elif key == ord('d'):
flightAssist.send_ned_velocity(veh_control, 0, 2, 0, 1) #right
elif(key == ord('q')):
yaw = math.degrees(attitude.yaw) #yaw left
flightAssist.condition_yaw(veh_control, yaw-5)
elif(key == ord('e')):
yaw = math.degrees(attitude.yaw) #yaw right
flightAssist.condition_yaw(veh_control,yaw + 5)
elif(key == ord('8')):
flightAssist.send_ned_velocity(veh_control, 0, 0, -2, 1) #down
elif(key == ord('2')):
flightAssist.send_ned_velocity(veh_control, 0,0,2, 1) #up
elif (key == ord('1')):
break
else:
flightAssist.send_ned_velocity(veh_control,0,0,0,1) #still
print("Returning to Launch")
veh_control.mode = VehicleMode("RTL")
print("Close vehicle object")
veh_control.close()
if sitl is not None:
sitl.stop()
def takeoff(t):
vehicle.armed = True
vehicle.mode = VehicleMode("GUIDED")
vehicle.simple_takeoff(t)
print "takeoff at " + str(t)
