Altitude monitoring and wait for home lock

Tools/CI/MissionCheck.py

@@ -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)