print "Last Heartbeat: %s" % vehicle.last_heartbeat print "Rangefinder: %s" % vehicle.rangefinder print "Rangefinder distance: %s" % vehicle.rangefinder.distance print "Rangefinder voltage: %s" % vehicle.rangefinder.voltage print "Heading: %s" % vehicle.heading print "Is Armable?: %s" % vehicle.is_armable print "System status: %s" % vehicle.system_status.state print "Mode: %s" % vehicle.mode.name # settable print "Armed: %s" % vehicle.armed # settable print("") # Change the mode to STABILIZE if vehicle.mode.name != 'STABILIZE': print("Changing mode to STABILIZE") vehicle.mode = VehicleMode("STABILIZE") print("") # Arming the vehicle print("Arming the vehicle") vehicle.armed = True time.sleep(1) print "Armed: %s" % vehicle.armed time.sleep(5) # Disarming the vehicle print("Disarming the vehicle") vehicle.armed = False time.sleep(1) print "Armed: %s" % vehicle.armed # settable
#Upload clear message and command messages to vehicle. print("Uploading %d waypoints to vehicle..." % len(messages)) cmds.upload() print "Arm and Takeoff" arm_and_takeoff(30) print "Starting mission" # Reset mission set to first (0) waypoint vehicle.commands.next = 0 # Set mode to AUTO to start mission: while (vehicle.mode.name != "AUTO"): vehicle.mode = VehicleMode("AUTO") time.sleep(0.1) # Monitor mission for 60 seconds then RTL and quit: time_start = time.time() while time.time() - time_start < 60: nextwaypoint = vehicle.commands.next print 'Distance to waypoint (%s): %s' % (nextwaypoint, distance_to_current_waypoint()) if nextwaypoint == len(messages): print "Exit 'standard' mission when start heading to final waypoint" break time.sleep(1) print 'Return to launch'
print("Baton found on port " + port) cmds = baton.commands cmds.download() #sync existing commands cmds.wait_ready() #wait til baton ready cmds.clear() #clear existing commands baton.home_location = baton.location.global_frame #set home location to current location print("Home location set!") for i in range(num): #add commands to the list defined earlier cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, lat[i], lon[i], 30)) try: cmds.upload() #upload commands print("Mission uploaded successfully!") uploaded = True except: print("Mission failed to upload") if uploaded == True: baton.mode = VehicleMode('THROW') baton.armed = True print("Baton is ready to deploy") else: print("Baton not found!")
def start(self): # Set mode to AUTO to start mission self.vehicle.mode = VehicleMode("AUTO")
def ChangeMode(vehicle, mode): while vehicle.mode != VehicleMode(mode): vehicle.mode = VehicleMode(mode) time.sleep(0.5) return True
def run(self,sim_runner): if self.first_enter: self.first_enter = False sim_runner.vehicle.mode = VehicleMode('GUIDED') sim_runner.vehicle.simple_goto(sim_runner.profile.target1)
#On upload la mission upload_mission(import_mission_filename) #On sauvegarde la mission save_mission(export_mission_filename) #On lance les moteurs et on décolle à une altitude donnée arm_and_takeoff(10) print("Starting mission") # On initialise l'indice du waypoint à atteindre vehicle.commands.next = 0 # On passe le drone en mode de vol AUTO : le drone suit automatiquement les waypoints uploadés vehicle.mode = VehicleMode("AUTO") # On fait tourner cette boucle While en permanence pour afficher la distance jusqu'au prochain waypoint # On impose à cette boucle de s'arrêter lorsque le dernier waypoint est atteint #c'est dans cette foucle qu'il faudrait intégrer l'évitement d'obstacle. #Lorsque l'on détecte un obstacle, on peut passer en mode LOITER (vol stationnaire), puis en mode GUIDED jusqu'à trouver un waypoint qui permette d'éviter l'obstacle #On repasse en mode AUTO F = fakePosition() while True: nextwaypoint = vehicle.commands.next print('Distance to waypoint (%s): %s' % (nextwaypoint, distance_to_current_waypoint())) if nextwaypoint == 6: #Dummy waypoint - as soon as we reach waypoint 4 this is true and we exit. print(
from dronekit import connect from dronekit import VehicleMode import time # Connect to UDP endpoint. vehicle = connect("tcp:127.0.0.1:5762", wait_ready=True) # Use returned Vehicle object to query device state - e.g. to get the mode: print("Mode: %s" % vehicle.mode.name) vehicle.mode = VehicleMode( 'GUIDED' ) time.sleep(2) print("Mode: %s" % vehicle.mode.name)
def return_to_base(): print('Returning to base') drone.mode = VehicleMode('RTL') sys.stdout.flush()
def clean_exit(self): self.vehicle.mode = VehicleMode("LAND") time.sleep(5) print "Program interrupted, switching to LAND mode." self.vehicle.close() os._exit(0)
from dronekit import connect, VehicleMode, LocationGlobalRelative print("stage 1 done") connection_port="/dev/ttyTHS1" baud=57600 vehicle = connect(connection_port, wait_ready=None, baud=baud) vehicle.mode = VehicleMode("MANUAL") print("stage 2 done") print('Autopilot Firmware version: %s' % vehicle.version) #print('Autopilot capabilities (supports ftp): %s' % vehicle.capabilities.ftp) print('Global Location: %s' % vehicle.location.global_frame) print('Global Location (relative altitude): %s' % vehicle.location.global_relative_frame) print('Local Location: %s' % vehicle.location.local_frame) print('Attitude: %s' % vehicle.attitude) print('Velocity: %s' % vehicle.velocity) print('GPS: %s' % vehicle.gps_0) print('Groundspeed: %s' % vehicle.groundspeed) print('Airspeed: %s' % vehicle.airspeed) print('Gimbal status: %s' % vehicle.gimbal) print('Battery: %s' % vehicle.battery) print('EKF OK?: %s' % vehicle.ekf_ok) print('Last Heartbeat: %s' % vehicle.last_heartbeat) print('Rangefinder: %s' % vehicle.rangefinder) print('Rangefinder distance: %s' % vehicle.rangefinder.distance) print('Rangefinder voltage: %s' % vehicle.rangefinder.voltage) print('Heading: %s' % vehicle.heading) print('Is Armable?: %s' % vehicle.is_armable) print('System status: %s' % vehicle.system_status.state) print('Mode: %s' % vehicle.mode.name)
def land(self): """ Switches the vehicle to LAND mode.""" self.vehicle.mode = VehicleMode("LAND") while True: print "Waiting for program to end" time.sleep(20)
# Get some vehicle attributes (state) print "Get some vehicle attribute values:" print " GPS: %s" % vehicle.gps_0 #print " Battery: %s" % vehicle.battery #print " Last Heartbeat: %s" % vehicle.last_heartbeat print " Is Armable?: %s" % vehicle.is_armable #print " System status: %s" % vehicle.system_status.state print " Mode: %s" % vehicle.mode.name # settable #print " Attitude: %s" % vehicle.attitude print " Altitude: %s" % vehicle.location.global_relative_frame.alt print " Home: %s" % vehicle.home_location # Initial Vehicle state. Using this to decide if vehicle is healthy for flight vehicle.armed = True vehicle.mode = VehicleMode("MANUAL") while not vehicle.armed and vehicle.mode.name == "MANUAL": print("Waiting until Vehicle is armed and in MANUAL mode") time.sleep(1) print("ARM STATUS: ", vehicle.armed, vehicle.is_armable) print " ### BEGINNING BURN IN 5 SECONDS ###" time.sleep(1) print " ### 5 ###" time.sleep(1) print " ### 4 ###"
def semantic76(head, poppedList): identifier = poppedList[3].value vehicle = symbolsTable[identifier] vehicle.mode = VehicleMode("AUTO") vehicle.airspeed = poppedList[1].value return InputToken(head, vehicle)
def run(self,sim_runner): if self.first_enter: self.first_enter = False sim_runner.vehicle.mode = VehicleMode('RTL')
face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_alt.xml') if face_cascade.empty(): raise IOError('Unable to load the face cascade classifier xml file') cap = cv2.VideoCapture(0) scaling_factor = 0.5 #vehicle = connect('127.0.0.1:14550', wait_ready=True) vehicle = connect('/dev/ttyUSB0', baud=57600) #vehicle=connect('/dev/ttyAMA0', baud=57600) vehicle.channels.overrides = {} desired_mode = "LOITER" while vehicle.mode != desired_mode: vehicle.mode = VehicleMode(desired_mode) print(" Waiting for mode change ...") time.sleep(0.5) print(" Mode: %s" % vehicle.mode.name) def drone_cntrl(cpx): if ((cpx > 80) & (cpx < 240)): vehicle.channels.overrides[4] = 1500 print("center") elif (cpx < 90): print("right move") vehicle.channels.overrides[4] = 1400 elif (cpx > 230): print("left move")
def run(self,sim_runner): if self.first_enter: sim_runner.vehicle.mode = VehicleMode('GUIDED') sim_runner.vehicle.simple_takeoff(20) self.first_enter = False
elif key=='a': # Alt Hold mode = 'ALT_HOLD' elif key=='p': # PosHold mode = 'POSHOLD' elif key=='l': # loiter mode = 'LOITER' elif key=='g': # guided mode = 'GUIDED' elif key=='t': # auto mode = 'AUTO' elif key=='r': # RTL mode = 'RTL' elif key=='d': # land mode = 'LAND' vehicle.mode = VehicleMode( mode ) # フライトモードの変更を指示 # 現在のフライトモードを表示 print("--------------------------" ) print(" Mode: %s" % vehicle.mode.name ) time.sleep(1) except( KeyboardInterrupt, SystemExit): # Ctrl+cが押されたら離脱 print( "SIGINTを検知" ) # フライトコントローラとの接続を閉じる vehicle.close() print("終了.") # 終了メッセージ
import math import time from dronekit import connect, VehicleMode from mission import read_mission, mission_to_locations from position_vector import PositionVector #connect to sitl vehicle = connect("127.0.0.1:14551", wait_ready=True, rate=50) #change to manual mode print "\nSet Vehicle.mode = MANUAL (currently: %s)" % vehicle.mode.name vehicle.mode = VehicleMode("MANUAL") while not vehicle.mode.name == 'MANUAL': #Wait until mode has changed print " Waiting for mode change ..." time.sleep(1) #check that vehicle is armable while not vehicle.is_armable: print " Waiting for vehicle to initialise..." time.sleep(1) #arm vehicle print "\nSet Vehicle.armed=True (currently: %s)" % vehicle.armed vehicle.armed = True while not vehicle.armed: print " Waiting for arming..." time.sleep(1) print " Vehicle is armed: %s" % vehicle.armed #get Vehicle Home location while not vehicle.home_location:
def detailed_search_autonomy(configs, autonomyToCV, gcs_timestamp, connection_timestamp, vehicle=None): print("\n######################## STARTING DETAILED SEARCH AUTONOMY ########################") autonomy.configs = configs # If comms is simulated, start comm simulation thread if configs["detailed_search_specific"]["comms_simulated"]["toggled_on"]: comm_sim = Thread(target=comm_simulation, args=(configs["detailed_search_specific"]["comms_simulated"]["comm_sim_file"], xbee_callback,)) comm_sim.start() # Otherwise, set up XBee device and add callback else: comm_sim = None autonomy.xbee = setup_xbee() autonomy.gcs_timestamp = gcs_timestamp autonomy.connection_timestamp = connection_timestamp autonomy.xbee.add_data_received_callback(xbee_callback) # If detailed search was not role-switched from quick scan, connect to new vehicle and takeoff if not vehicle: # Start SITL if vehicle is being simulated if (configs["vehicle_simulated"]): import dronekit_sitl sitl = dronekit_sitl.start_default(lat=35.328423, lon=-120.752505) connection_string = sitl.connection_string() else: if (configs["3dr_solo"]): connection_string = "udpin:0.0.0.0:14550" else: connection_string = "/dev/serial0" # Connect to vehicle vehicle = connect(connection_string, wait_ready=True) # Starts the update thread update = Thread(target=update_thread, args=(vehicle, configs["mission_control_MAC"])) update.start() takeoff(vehicle, configs["altitude"]) else: # Starts the update thread update = Thread(target=update_thread, args=(vehicle, configs["mission_control_MAC"])) update.start() # Change vehicle status to running change_status("running") # Continuously fly to POIs while not autonomy.stop_mission: if not POI_queue.empty() and not autonomy.pause_mission: poi = POI_queue.get() poi.alt = configs["altitude"] vehicle.simple_goto(poi) print("At POI, now orbiting") # TODO start CV scanning orbit_poi(vehicle, poi, configs) # Change flight mode to AUTO to start auto mission vehicle.mode = VehicleMode("AUTO") # print location while orbiting while vehicle.commands.next != vehicle.commands.count: print(vehicle.location.global_frame) print(vehicle.commands.next) print(vehicle.commands.count) time.sleep(1) # TODO stop CV scanning # Change flight mode back vehicle.mode = VehicleMode("GUIDED") else: change_status("waiting") # Holds the copter in place if receives pause if autonomy.pause_mission: vehicle.mode = VehicleMode("ALT_HOLD") change_status("paused") # Otherwise continue else: vehicle.mode = VehicleMode("GUIDED") land(vehicle) # Sets vehicle status to "ready" change_status("ready") autonomy.mission_completed = True update.join() # Wait for comm simulation thread to end if comm_sim: comm_sim.join() else: autonomy.xbee.close()
vehicle.simple_goto(wp2) #--- Here is where I can add more action later time.sleep(30) #-- Go to wp3 print("go to wp3") wp3 = LocationGlobalRelative(35.9872609, -95.8753037, 10) vehicle.simple_goto(wp3) #--- Here is where I can add more action later time.sleep(30) #-- Go to wp4 print("go to wp4") wp4 = LocationGlobalRelative(35.9872609, -95.8753037, 10) vehicle.simple_goto(wp4) #--- Here is where I can add more action later time.sleep(30) #--- Coming back print("Coming back") vehicle.mode = VehicleMode("RTL") time.sleep(20) #-- Close connection vehicle.close()
def startLandingSequence(): # should probably check the parameters for this.. vehicle.mode = VehicleMode("RTL") print 'Return to Land executed' print 'DONE'
def get_distance_metres(aLocation1, aLocation2): """ Returns the ground distance in metres between two `LocationGlobal` or `LocationGlobalRelative` objects. This method is an approximation, and will not be accurate over large distances and close to the earth's poles. It comes from the ArduPilot test code: https://github.com/diydrones/ardupilot/blob/master/Tools/autotest/common.py """ dlat = aLocation2.lat - aLocation1.lat dlong = aLocation2.lon - aLocation1.lon return math.sqrt((dlat * dlat) + (dlong * dlong)) * 1.113195e5 vehicle = connect('udp:127.0.0.1:14551', wait_ready=True) vehicle.mode = VehicleMode('GUIDED') vehicle.armed = True time.sleep(2) if vehicle.armed: print("Taking off") vehicle.simple_takeoff(10) time.sleep(15) goto(10, 0, vehicle.simple_goto) goto(0, -10, vehicle.simple_goto) goto(-20, 0, vehicle.simple_goto) goto(0, 20, vehicle.simple_goto) goto(20, 0, vehicle.simple_goto) goto(0, -10, vehicle.simple_goto) else: print("Vehicle not armed") print("Landing")
curr_location = LocationGlobalRelative(27.707568, 85.325080,20) a_location = LocationGlobalRelative(27.7083097,85.3246923, 20) distance=get_distance_metres(curr_location, a_location) print ("The mission distance is"+ str(distance)) while distance >= 5: vehicle.simple_goto(LocationGlobalRelative(27.7083097,85.3246923,20)) curr_location = vehicle.location.global_frame #print ("Current location is" + str(curr_location)) distance=get_distance_metres(curr_location, a_location) time.sleep(4) print( ) print("--------The remaining distance is "+ str(distance)) vehicle.mode = VehicleMode("LOITER") print ("Loitering for 10 seconds...") time.sleep(10) vehicle.mode = VehicleMode("LAND") vehicle.armed = False print("------------------Mission Complete ------------------") """ print(">>>>>>>>>>>>>>>>>Going towards target") vehicle.simple_goto(LocationGlobalRelative(27.608321,85.3322697,30))
print("Hold position for 3 seconds") set_attitude(duration=3) # Uncomment the lines below for testing roll angle and yaw rate. # Make sure that there is enough space for testing this. # set_attitude(roll_angle = 1, thrust = 0.5, duration = 3) # set_attitude(yaw_rate = 30, thrust = 0.5, duration = 3) # Move the drone forward and backward. # Note that it will be in front of original position due to inertia. print("Move forward") set_attitude(pitch_angle=-5, thrust=0.5, duration=3.21) print("Move backward") set_attitude(pitch_angle=5, thrust=0.5, duration=3) print("Setting LAND mode...") vehicle.mode = VehicleMode("LAND") time.sleep(1) # 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")
num = num + 1 point = str(num) # convert num to string type send 1 to server client_socket.send(point.encode('utf-8')) time.sleep(3) #1 while num < 5: drone_fly(latitude[num], longtitude[num]) num = num + 1 point = str(num) # convert num to string type send 2 to server client_socket.send(point.encode('utf-8')) time.sleep(3) #5(Call back) vehicle.mode = VehicleMode("RTL") print("Completed to Base") print("Finish") point = 'arrive' client_socket.send(point.encode('utf-8')) # Socket conection finish client_socket.close() # close socket connection except socket.error: # when socket connection failed print("EMERGENCY LAND!!") vehicle.mode = VehicleMode("LAND") time.sleep(1) print("Close vehicle object") vehicle.close() finally: client_socket.close()
num_sat = int(str(vehicle.gps_0).split('=')[2]) firebase.put('/', "sat1", num_sat) time.sleep(1) #opening a file for writing fff = open("gps_left.txt", "w") # 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(final_height) 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") gps_print_timer = time.time() start_time = time.time() first_line = "start_time: " + str(datetime.now()) + "\n" fff.write(first_line) print_timer = time.time() # 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:
def GNC_Node_Action(): print("Intitialisation process has begun") global processOrderFlag #Initialise instance of UAV class and the associated Autopilot` uav = UAV(idAgent=1) enemy = ENEMY(idAgent=2) print("Begin Subsribers and Publishers") # Initialise node, anonymous necessary for centralised system rospy.init_node('GNC_Node_Action', anonymous=True) rospy.loginfo("Initialised Node") ''' # Initialise publishing to TaskActions topic pubGlobal = rospy.Publisher("FriendInfo", AgentInfo, queue_size=10) pubLocal = rospy.Publisher('LocalInfo', AgentInfo, queue_size=10) rospy.loginfo("Initialised Publishing") # Initialise subscribing to TaskActions topic rospy.Subscriber("TaskActions", String, getOrders) #Every time a message is received, callback() is called rospy.loginfo("Initialised Subscribing TaskActions") ''' # Initialise subscribing to Orders topic #rospy.Subscriber("Orders", OrderList, uav.getOrders) #Every time a message is received, callback() is called #rospy.loginfo("Initialised Subscribing Orders") # Initialise subscribing to GPS location topic rospy.Subscriber("/mavros/global_position/local", Odometry, uav.getPosition) rospy.loginfo("Initialised Subscribing MAVROS/global_position/local" ) #local instead of global #rospy.Subscriber("/iris/mavros/global_position/global", NavSatFix, uav.getPosition) #rospy.loginfo("Initialised Subscribing MAVROS/global_position/local") #local instead of global # Initialise subscribing to Heading topic rospy.Subscriber("/mavros/global_position/compass_hdg", Float64, uav.getHeading) rospy.loginfo("Initialised Subscribing MAVROS/global_position/compass_hdg") # Initialise subscribing to IMU topic rospy.Subscriber("/mavros/imu/data", Imu, uav.getIMU) rospy.loginfo("Initialised Subscribing MAVROS/imu/data") #Initialise subscribing to BatteryState topic rospy.Subscriber("/mavros/battery", BatteryState, uav.getBatteryStatus) rospy.loginfo("Initialised Subscribing MAVROS/battery") #Initialise subscribing to State topic rospy.Subscriber("/mavros/state", State, uav.getState) rospy.loginfo("Initialised Subscribing MAVROS/state") #Initialise subscribing to WaypointReached topic rospy.Subscriber("/mavros/mission/reached", WaypointReached, waypointReached) rospy.loginfo("Initialised Subscribing MAVROS/mission/reached") #Initialise subscribing to WaypointReached topic rospy.Subscriber("/cam_p", Odometry, enemy.getPosition) rospy.loginfo("Initialised Subscribing /cam_p") import dronekit_sitl vehicle = connect( '127.0.0.1:14560', wait_ready=True ) #'udp:localhost:14551', wait_ready=True) #127.0.0.1:14560 #rospy.spin() #Keep the nodes active print("Begin Orders") vehicle.mode = VehicleMode("GUIDED") #uav.setFlightMode("GUIDED") rate = rospy.Rate(0.2) #Hz #uav.abortMission() #added #wait4connect() #added rate.sleep() #NECESSARY TO GIVE TIME TO GET THE ATTRIBUTES #uav.setHomeLocation(False, [0, 0, 0]) uav.arm() #uav.setFlightMode(mode='GUIDED') #Changed from STABILIZE to GUIDED and uncommented uav.takeOff() rate.sleep() rate.sleep() #uncommented #waypoints = [[5, 5, 3], [5, -5, 3], [-5, -5, 5], [-5, 5, 5]] waypoints = [[enemy.position[1], enemy.position[0], enemy.position[2] + 1]] print(waypoints) #t = np.arange(20) #for k in t: #waypoints.append([3*np.sin(k),k,5]) uav.go_to_defending(waypoints, vehicle) #uav.setFlightMode("AUTO") #uav.land() rate.sleep() #uav.disarm() rate.sleep() print('END') '''
import time from dronekit import connect, VehicleMode, LocationGlobalRelative, Command, LocationGlobal from pymavlink import mavutil import Tkinter as tk import os from time import sleep import serial ser = serial.Serial('/dev/ttyACM1', 57600) vehicle = connect("/dev/ttyUSB0", baud=57600) time.sleep(6) print("CONNECTED") vehicle.mode = VehicleMode("GUIDED") print("MODE SET TO GUIDED") reached = False time.sleep(2) def arm(): print("Arming motors") while vehicle.armed != True: vehicle.armed = True time.sleep(2) def arm_and_takeoff(altitude): arm() print("Taking Off") vehicle.simple_takeoff(altitude) time.sleep(2) while True: v_alt = vehicle.location.global_relative_frame.alt
def land(do): if do: # land if do is true vehicle.mode = VehicleMode('Land') time.sleep(10)