Tools: autotest.py: look at EKF flags to determine armability

Tools/autotest/apmrover2.py

@@ -19,12 +19,8 @@ HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 24
 homeloc = None
 
 
-def wait_ready_to_arm(mavproxy):
-    # wait for EKF and GPS checks to pass
-    mavproxy.expect('IMU0 is using GPS')
-
 def arm_rover(mavproxy, mav):
-    wait_ready_to_arm(mavproxy);
+    wait_ready_to_arm(mav);
 
     mavproxy.send('arm throttle\n')
     mavproxy.expect('ARMED')

Tools/autotest/arducopter.py

@@ -31,10 +31,6 @@ homeloc = None
 num_wp = 0
 
 
-def wait_ready_to_arm(mavproxy):
-    # wait for EKF and GPS checks to pass
-    mavproxy.expect('IMU0 is using GPS')
-
 def hover(mavproxy, mav, hover_throttle=1500):
     mavproxy.send('rc 3 %u\n' % hover_throttle)
     return True
@@ -1051,7 +1047,7 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
         setup_rc(mavproxy)
         homeloc = mav.location()
 
-        wait_ready_to_arm(mavproxy)
+        wait_ready_to_arm(mav)
 
         # Arm
         print("# Arm motors")
@@ -1409,7 +1405,7 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
         print("Lowering rotor speed")
         mavproxy.send('rc 8 1000\n')
 
-        wait_ready_to_arm(mavproxy)
+        wait_ready_to_arm(mav)
 
         # Arm
         print("# Arm motors")

Tools/autotest/arduplane.py

@@ -19,14 +19,10 @@ WIND = "0,180,0.2"  # speed,direction,variance
 
 homeloc = None
 
-def wait_ready_to_arm(mavproxy):
-    # wait for EKF and GPS checks to pass
-    mavproxy.expect('IMU0 is using GPS')
-
 def takeoff(mavproxy, mav):
     """Takeoff get to 30m altitude."""
 
-    wait_ready_to_arm(mavproxy)
+    wait_ready_to_arm(mav)
 
     mavproxy.send('arm throttle\n')
     mavproxy.expect('ARMED')

Tools/autotest/common.py

@@ -10,6 +10,29 @@ from pysim import util
 # messages. This keeps the output to stdout flowing
 expect_list = []
 
+class AutoTestTimeoutException(Exception):
+    pass
+
+def wait_ready_to_arm(mav, timeout=None):
+    # wait for EKF checks to pass
+    return wait_ekf_happy(mav, timeout=timeout)
+
+def wait_ekf_happy(mav, timeout=30):
+    """Wait for EKF to be happy"""
+
+    tstart = get_sim_time(mav)
+    required_value = 831
+    print("Waiting for EKF value %u" % (required_value))
+    while timeout is None or get_sim_time(mav) < tstart + timeout:
+        m = mav.recv_match(type='EKF_STATUS_REPORT', blocking=True)
+        current = m.flags
+        if (tstart - get_sim_time(mav)) % 5 == 0:
+            print("Wait EKF.flags: required:%u current:%u" % (required_value, current))
+        if current == required_value:
+            print("EKF Flags OK")
+            return
+    print("Failed to get EKF.flags=%u" % required_value)
+    raise AutoTestTimeoutException()
 
 def expect_list_clear():
     """clear the expect list."""