fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 18:08:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Altitude monitoring and wait for home lock

Browse Source
This commit is contained in:
sander
2016-02-27 11:24:05 +01:00
committed by Lorenz Meier
parent 14f358a975
commit b9ae592e2b
1 changed files with 67 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/CI/MissionCheck.py
+67 -21
View File
@@ -17,7 +17,7 @@ connection_string = '127.0.0.1:14540'
import_mission_filename = 'VTOLmission.txt'
max_execution_time = 250
alt_acceptance_radius = 10
################################################################################################
# Init
@@ -32,6 +32,7 @@ parser = argparse.ArgumentParser()
parser.add_argument("-c", "--connect", help="connection string")
parser.add_argument("-f", "--filename", help="mission filename")
parser.add_argument("-t", "--timeout", help="execution timeout")
parser.add_argument("-a", "--altrad", help="altitude acceptance radius")
args = parser.parse_args()
if args.connect:
@@ -40,10 +41,12 @@ if args.filename:
import_mission_filename = args.filename
if args.timeout:
max_execution_time = args.timeout
if args.altrad:
alt_acceptance_radius = args.altrad
mission_failed = False
MAV_MODE_AUTO = 4
# start time counter
@@ -54,8 +57,14 @@ elapsed_time = time.time() - start_time
# Connect to the Vehicle
print "Connecting"
vehicle = connect(connection_string, wait_ready=False)
vehicle.armed = False
vehicle = connect(connection_string, wait_ready=True)
while not vehicle.system_status.state == "STANDBY" or vehicle.gps_0.fix_type < 3:
if time.time() - start_time > 20:
print "FAILED: SITL did not reach standby with GPS fix within 20 seconds"
sys.exit(98)
print "Waiting for vehicle to initialise... %s " % vehicle.system_status.state
time.sleep(1)
# Display basic vehicle state
print " Type: %s" % vehicle._vehicle_type
@@ -65,12 +74,6 @@ print " GPS: %s" % vehicle.gps_0
print " Alt: %s" % vehicle.location.global_relative_frame.alt
while not vehicle.system_status.state == "STANDBY":
print " Waiting for vehicle to initialise... %s " % vehicle.system_status.state
time.sleep(1)
################################################################################################
# Functions
################################################################################################
@@ -82,7 +85,6 @@ def readmission(aFileName):
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:
@@ -116,15 +118,13 @@ def upload_mission(aFileName):
#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)
print ' Uploaded mission with %s items' % len(missionlist)
vehicle.commands.upload()
return missionlist
@@ -174,19 +174,23 @@ def printfile(aFileName):
for line in f:
print ' %s' % line.strip()
################################################################################################
# Listeners
################################################################################################
current_sequence = -1
current_sequence_changed = False
current_landed_state = -1
home_position_set = False
#Create a message listener for mission sequence number
@vehicle.on_message('MISSION_CURRENT')
def listener(self, name, mission_current):
global current_sequence
global current_sequence, current_sequence_changed
if (current_sequence <> mission_current.seq):
current_sequence = mission_current.seq;
current_sequence_changed = True
print 'current mission sequence: %s' % mission_current.seq
#Create a message listener for mission sequence number
@@ -195,13 +199,28 @@ 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
#Create a message listener for home position fix
@vehicle.on_message('HOME_POSITION')
def listener(self, name, home_position):
global home_position_set
home_position_set = True
################################################################################################
# Start mission test
################################################################################################
while not home_position_set:
if time.time() - start_time > 30:
print "FAILED: getting home position 30 seconds"
sys.exit(98)
print "Waiting for home position..."
time.sleep(1)
#Upload mission from file
missionlist = upload_mission(import_mission_filename)
time.sleep(2)
@@ -213,22 +232,40 @@ vehicle._master.mav.command_long_send(vehicle._master.target_system, vehicle._ma
0, 0, 0, 0, 0, 0)
time.sleep(1)
# Arm vehicle
vehicle.armed = True
while not vehicle.system_status.state == "ACTIVE":
if time.time() - start_time > 30:
print "FAILED: vehicle did not arm within 30 seconds"
sys.exit(98)
print "Waiting for vehicle to arm..."
time.sleep(1)
# Wait for completion of mission items
while (current_sequence < len(missionlist)-1 and elapsed_time < max_execution_time):
time.sleep(1)
if current_sequence > 0 and current_sequence_changed:
if missionlist[current_sequence-1].z - alt_acceptance_radius > vehicle.location.global_relative_frame.alt or missionlist[current_sequence-1].z + alt_acceptance_radius < vehicle.location.global_relative_frame.alt:
print "waypoint %s out of bounds altitude %s gps altitude: %s" % (current_sequence, missionlist[current_sequence-1].z, vehicle.location.global_relative_frame.alt)
mission_failed = True
current_sequence_changed = False
elapsed_time = time.time() - start_time
print "Mission items have been executed"
if elapsed_time < max_execution_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"
if elapsed_time < max_execution_time:
print "Vehicle has landed"
# Disarm vehicle
vehicle.armed = False
@@ -238,11 +275,20 @@ elapsed_time = time.time() - start_time
# Close vehicle object before exiting script
vehicle.close()
time.sleep(2)
# Validate time constraint
if elapsed_time < max_execution_time:
if elapsed_time <= max_execution_time and not mission_failed:
print "Mission succesful time elapsed %s" % elapsed_time
sys.exit(0)
print "Mission FAILED to execute within %s seconds" % max_execution_time
sys.exit(99)
if elapsed_time > max_execution_time:
print "Mission FAILED to execute within %s seconds" % max_execution_time
sys.exit(99)
if mission_failed:
print "Mission FAILED out of bounds"
sys.exit(100)
print "Mission FAILED something strange happened"
sys.exit(101)