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)
