mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 21:55:34 +08:00
Initial mission check script using dronekit
This commit is contained in:
sander
Lorenz Meier
@@ -0,0 +1,205 @@
|
||||
################################################################################################
|
||||
# Settings
|
||||
################################################################################################
|
||||
|
||||
import_mission_filename = "VTOLmission.txt"
|
||||
max_execution_time = 250
|
||||
|
||||
|
||||
################################################################################################
|
||||
# Init
|
||||
################################################################################################
|
||||
|
||||
# Import DroneKit-Python
|
||||
from dronekit import connect, VehicleMode, Command
|
||||
import time, sys
|
||||
|
||||
# start time counter
|
||||
start_time = time.time()
|
||||
elapsed_time = time.time() - start_time
|
||||
|
||||
# Connect to the Vehicle, wait_ready will not work on PX4
|
||||
print "Connecting"
|
||||
vehicle = connect('127.0.0.1:14550', wait_ready=True)
|
||||
vehicle.armed = False
|
||||
|
||||
while not vehicle.system_status.state == "STANDBY":
|
||||
print " Waiting for vehicle to initialise... %s " % vehicle.system_status.state
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
################################################################################################
|
||||
# Functions
|
||||
################################################################################################
|
||||
|
||||
|
||||
def readmission(aFileName):
|
||||
"""
|
||||
Load a mission from a file into a list. The mission definition is in the Waypoint file
|
||||
format (http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
|
||||
This function is used by upload_mission().
|
||||
"""
|
||||
print "\nReading mission from file: %s" % aFileName
|
||||
cmds = vehicle.commands
|
||||
missionlist=[]
|
||||
with open(aFileName) as f:
|
||||
for i, line in enumerate(f):
|
||||
if i==0:
|
||||
if not line.startswith('QGC WPL 110'):
|
||||
raise Exception('File is not supported WP version')
|
||||
else:
|
||||
linearray=line.split('\t')
|
||||
ln_index=int(linearray[0])
|
||||
ln_currentwp=int(linearray[1])
|
||||
ln_frame=int(linearray[2])
|
||||
ln_command=int(linearray[3])
|
||||
ln_param1=float(linearray[4])
|
||||
ln_param2=float(linearray[5])
|
||||
ln_param3=float(linearray[6])
|
||||
ln_param4=float(linearray[7])
|
||||
ln_param5=float(linearray[8])
|
||||
ln_param6=float(linearray[9])
|
||||
ln_param7=float(linearray[10])
|
||||
ln_autocontinue=int(linearray[11].strip())
|
||||
cmd = Command( 0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, ln_param3, ln_param4, ln_param5, ln_param6, ln_param7)
|
||||
missionlist.append(cmd)
|
||||
return missionlist
|
||||
|
||||
|
||||
def upload_mission(aFileName):
|
||||
"""
|
||||
Upload a mission from a file.
|
||||
"""
|
||||
#Read mission from file
|
||||
missionlist = readmission(aFileName)
|
||||
|
||||
print "\nUpload mission from a file: %s" % import_mission_filename
|
||||
#Clear existing mission from vehicle
|
||||
print ' Clear mission'
|
||||
cmds = vehicle.commands
|
||||
cmds.clear()
|
||||
#Add new mission to vehicle
|
||||
for command in missionlist:
|
||||
cmds.add(command)
|
||||
print ' Upload mission with %s items' % len(missionlist)
|
||||
vehicle.commands.upload()
|
||||
return missionlist
|
||||
|
||||
|
||||
def download_mission():
|
||||
"""
|
||||
Downloads the current mission and returns it in a list.
|
||||
It is used in save_mission() to get the file information to save.
|
||||
"""
|
||||
print " Download mission from vehicle"
|
||||
missionlist=[]
|
||||
cmds = vehicle.commands
|
||||
cmds.download()
|
||||
cmds.wait_ready()
|
||||
for cmd in cmds:
|
||||
missionlist.append(cmd)
|
||||
return missionlist
|
||||
|
||||
def save_mission(aFileName):
|
||||
"""
|
||||
Save a mission in the Waypoint file format
|
||||
(http://qgroundcontrol.org/mavlink/waypoint_protocol#waypoint_file_format).
|
||||
"""
|
||||
print "\nSave mission from Vehicle to file: %s" % export_mission_filename
|
||||
#Download mission from vehicle
|
||||
missionlist = download_mission()
|
||||
#Add file-format information
|
||||
output='QGC WPL 110\n'
|
||||
#Add home location as 0th waypoint
|
||||
home = vehicle.home_location
|
||||
output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1)
|
||||
#Add commands
|
||||
for cmd in missionlist:
|
||||
commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (cmd.seq,cmd.current,cmd.frame,cmd.command,cmd.param1,cmd.param2,cmd.param3,cmd.param4,cmd.x,cmd.y,cmd.z,cmd.autocontinue)
|
||||
output+=commandline
|
||||
with open(aFileName, 'w') as file_:
|
||||
print " Write mission to file"
|
||||
file_.write(output)
|
||||
|
||||
|
||||
def printfile(aFileName):
|
||||
"""
|
||||
Print a mission file to demonstrate "round trip"
|
||||
"""
|
||||
print "\nMission file: %s" % aFileName
|
||||
with open(aFileName) as f:
|
||||
for line in f:
|
||||
print ' %s' % line.strip()
|
||||
|
||||
################################################################################################
|
||||
# Listeners
|
||||
################################################################################################
|
||||
|
||||
current_sequence = -1
|
||||
current_landed_state = -1
|
||||
|
||||
#Create a message listener for mission sequence number
|
||||
@vehicle.on_message('MISSION_CURRENT')
|
||||
def listener(self, name, mission_current):
|
||||
global current_sequence
|
||||
if (current_sequence <> mission_current.seq):
|
||||
current_sequence = mission_current.seq;
|
||||
print 'current mission sequence: %s' % mission_current.seq
|
||||
|
||||
#Create a message listener for mission sequence number
|
||||
@vehicle.on_message('EXTENDED_SYS_STATE')
|
||||
def listener(self, name, extended_sys_state):
|
||||
global current_landed_state
|
||||
if (current_landed_state <> extended_sys_state.landed_state):
|
||||
current_landed_state = extended_sys_state.landed_state;
|
||||
print 'Landed state: %s' % extended_sys_state.landed_state
|
||||
|
||||
|
||||
################################################################################################
|
||||
# Start mission test
|
||||
################################################################################################
|
||||
|
||||
#Upload mission from file
|
||||
missionlist = upload_mission(import_mission_filename)
|
||||
time.sleep(2)
|
||||
|
||||
# set mission mode the hard way
|
||||
vehicle._master.mav.command_long_send(vehicle._master.target_system, vehicle._master.target_component,
|
||||
176, 0,
|
||||
4,
|
||||
0, 0, 0, 0, 0, 0)
|
||||
time.sleep(1)
|
||||
|
||||
# Arm vehicle
|
||||
vehicle.armed = True
|
||||
|
||||
# Wait for completion of mission items
|
||||
while (current_sequence < len(missionlist)-1 and elapsed_time < max_execution_time):
|
||||
time.sleep(1)
|
||||
elapsed_time = time.time() - start_time
|
||||
|
||||
print "Mission items have been executed"
|
||||
|
||||
# wait for the vehicle to have landed
|
||||
while (current_landed_state != 1 and elapsed_time < max_execution_time):
|
||||
time.sleep(1)
|
||||
elapsed_time = time.time() - start_time
|
||||
|
||||
print "Vehicle has landed"
|
||||
|
||||
# Disarm vehicle
|
||||
vehicle.armed = False
|
||||
|
||||
# count elapsed time
|
||||
elapsed_time = time.time() - start_time
|
||||
|
||||
# Close vehicle object before exiting script
|
||||
vehicle.close()
|
||||
|
||||
# Validate time constraint
|
||||
if elapsed_time < max_execution_time:
|
||||
print "Mission succesful time elapsed %s" % elapsed_time
|
||||
sys.exit(0)
|
||||
|
||||
print "Mission FAILED to execute within %s seconds time elapsed %s" % max_execution_time % elapsed_time
|
||||
sys.exit(99)
|
||||
@@ -0,0 +1,9 @@
|
||||
QGC WPL 110
|
||||
0 1 0 16 0 0 0 0 47.3979949951 8.54559612274 25.0 1
|
||||
1 0 2 3000 4.0 0.0 0.0 0.0 47.3980331421 8.54578971863 25.0 1
|
||||
2 0 3 16 0.0 0.0 -0.0 0.0 47.399269104 8.54557228088 25.0 1
|
||||
3 0 3 16 0.0 0.0 -0.0 0.0 47.3992652893 8.54230213165 25.0 1
|
||||
4 0 3 16 0.0 0.0 -0.0 0.0 47.3974761963 8.54239082336 25.0 1
|
||||
5 0 3 16 0.0 0.0 -0.0 0.0 47.3976669312 8.54509449005 25.0 1
|
||||
6 0 2 3000 3.0 0.0 0.0 0.0 47.3977851868 8.54526233673 25.0 1
|
||||
7 0 3 21 25.0 0.0 -0.0 0.0 47.3979797363 8.54460906982 25.0 1
|
||||
Reference in New Issue
Block a user