mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-12 02:17:36 +08:00
Use print() function in both Python 2 and Python 3
Legacy __print__ statements are syntax errors in Python 3 but __print()__ function works as expected in both Python 2 and Python 3.
This commit is contained in:
@@ -15,6 +15,7 @@
|
||||
# Settings
|
||||
################################################################################################
|
||||
|
||||
from __future__ import print_function
|
||||
connection_string = '127.0.0.1:14540'
|
||||
|
||||
import_mission_filename = 'VTOL_TAKEOFF.mission'
|
||||
@@ -58,22 +59,22 @@ elapsed_time = time.time() - start_time
|
||||
|
||||
|
||||
# Connect to the Vehicle
|
||||
print "Connecting"
|
||||
print("Connecting")
|
||||
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"
|
||||
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
|
||||
print("Waiting for vehicle to initialise... %s " % vehicle.system_status.state)
|
||||
time.sleep(1)
|
||||
|
||||
# Display basic vehicle state
|
||||
print " Type: %s" % vehicle._vehicle_type
|
||||
print " Armed?: %s" % vehicle.armed
|
||||
print " System status: %s" % vehicle.system_status.state
|
||||
print " GPS: %s" % vehicle.gps_0
|
||||
print " Alt: %s" % vehicle.location.global_relative_frame.alt
|
||||
print(" Type: %s" % vehicle._vehicle_type)
|
||||
print(" Armed?: %s" % vehicle.armed)
|
||||
print(" System status: %s" % vehicle.system_status.state)
|
||||
print(" GPS: %s" % vehicle.gps_0)
|
||||
print(" Alt: %s" % vehicle.location.global_relative_frame.alt)
|
||||
|
||||
|
||||
################################################################################################
|
||||
@@ -106,7 +107,7 @@ def upload_mission(aFileName):
|
||||
#Add new mission to vehicle
|
||||
for command in missionlist:
|
||||
cmds.add(command)
|
||||
print ' Uploaded mission with %s items' % len(missionlist)
|
||||
print(' Uploaded mission with %s items' % len(missionlist))
|
||||
vehicle.commands.upload()
|
||||
return missionlist
|
||||
|
||||
@@ -128,7 +129,7 @@ def listener(self, name, mission_current):
|
||||
if (current_sequence <> mission_current.seq):
|
||||
current_sequence = mission_current.seq;
|
||||
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
|
||||
@vehicle.on_message('EXTENDED_SYS_STATE')
|
||||
@@ -152,9 +153,9 @@ def listener(self, name, home_position):
|
||||
|
||||
while not home_position_set:
|
||||
if time.time() - start_time > 30:
|
||||
print "FAILED: getting home position 30 seconds"
|
||||
print("FAILED: getting home position 30 seconds")
|
||||
sys.exit(98)
|
||||
print "Waiting for home position..."
|
||||
print("Waiting for home position...")
|
||||
time.sleep(1)
|
||||
|
||||
|
||||
@@ -172,9 +173,9 @@ 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"
|
||||
print("FAILED: vehicle did not arm within 30 seconds")
|
||||
sys.exit(98)
|
||||
print "Waiting for vehicle to arm..."
|
||||
print("Waiting for vehicle to arm...")
|
||||
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 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
|
||||
current_sequence_changed = False
|
||||
elapsed_time = time.time() - start_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
|
||||
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
|
||||
|
||||
if elapsed_time < max_execution_time:
|
||||
print "Vehicle has landed"
|
||||
print("Vehicle has landed")
|
||||
|
||||
# Disarm vehicle
|
||||
vehicle.armed = False
|
||||
@@ -213,16 +214,16 @@ time.sleep(2)
|
||||
|
||||
# Validate time constraint
|
||||
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)
|
||||
|
||||
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)
|
||||
|
||||
if mission_failed:
|
||||
print "Mission FAILED out of bounds"
|
||||
print("Mission FAILED out of bounds")
|
||||
sys.exit(100)
|
||||
|
||||
print "Mission FAILED something strange happened"
|
||||
print("Mission FAILED something strange happened")
|
||||
sys.exit(101)
|
||||
|
||||
Reference in New Issue
Block a user