@ -15,6 +15,7 @@
# Settings
# Settings
################################################################################################
################################################################################################
from __future__ import print_function
connection_string = ' 127.0.0.1:14540 '
connection_string = ' 127.0.0.1:14540 '
import_mission_filename = ' VTOL_TAKEOFF.mission '
import_mission_filename = ' VTOL_TAKEOFF.mission '
@ -58,22 +59,22 @@ elapsed_time = time.time() - start_time
# Connect to the Vehicle
# Connect to the Vehicle
print " Connecting "
print ( " Connecting " )
vehicle = connect ( connection_string , wait_ready = True )
vehicle = connect ( connection_string , wait_ready = True )
while not vehicle . system_status . state == " STANDBY " or vehicle . gps_0 . fix_type < 3 :
while not vehicle . system_status . state == " STANDBY " or vehicle . gps_0 . fix_type < 3 :
if time . time ( ) - start_time > 20 :
if time . time ( ) - start_time > 20 :
print " FAILED: SITL did not reach standby with GPS fix within 20 seconds "
print ( " FAILED: SITL did not reach standby with GPS fix within 20 seconds " )
sys . exit ( 98 )
sys . exit ( 98 )
print " Waiting for vehicle to initialise... %s " % vehicle . system_status . state
print ( " Waiting for vehicle to initialise... %s " % vehicle . system_status . state )
time . sleep ( 1 )
time . sleep ( 1 )
# Display basic vehicle state
# Display basic vehicle state
print " Type: %s " % vehicle . _vehicle_type
print ( " Type: %s " % vehicle . _vehicle_type )
print " Armed?: %s " % vehicle . armed
print ( " Armed?: %s " % vehicle . armed )
print " System status: %s " % vehicle . system_status . state
print ( " System status: %s " % vehicle . system_status . state )
print " GPS: %s " % vehicle . gps_0
print ( " GPS: %s " % vehicle . gps_0 )
print " Alt: %s " % vehicle . location . global_relative_frame . alt
print ( " Alt: %s " % vehicle . location . global_relative_frame . alt )
################################################################################################
################################################################################################
@ -106,7 +107,7 @@ def upload_mission(aFileName):
#Add new mission to vehicle
#Add new mission to vehicle
for command in missionlist :
for command in missionlist :
cmds . add ( command )
cmds . add ( command )
print ' Uploaded mission with %s items ' % len ( missionlist )
print ( ' Uploaded mission with %s items ' % len ( missionlist ) )
vehicle . commands . upload ( )
vehicle . commands . upload ( )
return missionlist
return missionlist
@ -128,7 +129,7 @@ def listener(self, name, mission_current):
if ( current_sequence < > mission_current . seq ) :
if ( current_sequence < > mission_current . seq ) :
current_sequence = mission_current . seq ;
current_sequence = mission_current . seq ;
current_sequence_changed = True
current_sequence_changed = True
print ' current mission sequence: %s ' % mission_current . seq
print ( ' current mission sequence: %s ' % mission_current . seq )
#Create a message listener for mission sequence number
#Create a message listener for mission sequence number
@vehicle . on_message ( ' EXTENDED_SYS_STATE ' )
@vehicle . on_message ( ' EXTENDED_SYS_STATE ' )
@ -152,9 +153,9 @@ def listener(self, name, home_position):
while not home_position_set :
while not home_position_set :
if time . time ( ) - start_time > 30 :
if time . time ( ) - start_time > 30 :
print " FAILED: getting home position 30 seconds "
print ( " FAILED: getting home position 30 seconds " )
sys . exit ( 98 )
sys . exit ( 98 )
print " Waiting for home position... "
print ( " Waiting for home position... " )
time . sleep ( 1 )
time . sleep ( 1 )
@ -172,9 +173,9 @@ vehicle.armed = True
while not vehicle . system_status . state == " ACTIVE " :
while not vehicle . system_status . state == " ACTIVE " :
if time . time ( ) - start_time > 30 :
if time . time ( ) - start_time > 30 :
print " FAILED: vehicle did not arm within 30 seconds "
print ( " FAILED: vehicle did not arm within 30 seconds " )
sys . exit ( 98 )
sys . exit ( 98 )
print " Waiting for vehicle to arm... "
print ( " Waiting for vehicle to arm... " )
time . sleep ( 1 )
time . sleep ( 1 )
@ -185,13 +186,13 @@ while (current_sequence < len(missionlist)-1 and elapsed_time < max_execution_ti
if current_sequence > 0 and current_sequence_changed :
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 :
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 )
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
mission_failed = True
current_sequence_changed = False
current_sequence_changed = False
elapsed_time = time . time ( ) - start_time
elapsed_time = time . time ( ) - start_time
if elapsed_time < max_execution_time :
if elapsed_time < max_execution_time :
print " Mission items have been executed "
print ( " Mission items have been executed " )
# wait for the vehicle to have landed
# wait for the vehicle to have landed
while ( current_landed_state != 1 and elapsed_time < max_execution_time ) :
while ( current_landed_state != 1 and elapsed_time < max_execution_time ) :
@ -199,7 +200,7 @@ while (current_landed_state != 1 and elapsed_time < max_execution_time):
elapsed_time = time . time ( ) - start_time
elapsed_time = time . time ( ) - start_time
if elapsed_time < max_execution_time :
if elapsed_time < max_execution_time :
print " Vehicle has landed "
print ( " Vehicle has landed " )
# Disarm vehicle
# Disarm vehicle
vehicle . armed = False
vehicle . armed = False
@ -213,16 +214,16 @@ time.sleep(2)
# Validate time constraint
# Validate time constraint
if elapsed_time < = max_execution_time and not mission_failed :
if elapsed_time < = max_execution_time and not mission_failed :
print " Mission succesful time elapsed %s " % elapsed_time
print ( " Mission succesful time elapsed %s " % elapsed_time )
sys . exit ( 0 )
sys . exit ( 0 )
if elapsed_time > max_execution_time :
if elapsed_time > max_execution_time :
print " Mission FAILED to execute within %s seconds " % max_execution_time
print ( " Mission FAILED to execute within %s seconds " % max_execution_time )
sys . exit ( 99 )
sys . exit ( 99 )
if mission_failed :
if mission_failed :
print " Mission FAILED out of bounds "
print ( " Mission FAILED out of bounds " )
sys . exit ( 100 )
sys . exit ( 100 )
print " Mission FAILED something strange happened "
print ( " Mission FAILED something strange happened " )
sys . exit ( 101 )
sys . exit ( 101 )