autotest: added wait_mode() helper

Tools/autotest/arducopter.py

@@ -18,7 +18,7 @@ def arm_motors(mavproxy):
     '''arm motors'''
     print("Arming motors")
     mavproxy.send('switch 6\n') # stabilize mode
-    mavproxy.expect('STABILIZE>')
+    mav.wait_mode('STABILIZE')
     mavproxy.send('rc 3 1000\n')
     mavproxy.send('rc 4 2000\n')
     mavproxy.expect('APM: ARMING MOTORS')
@@ -41,7 +41,7 @@ def disarm_motors(mavproxy):
 def takeoff(mavproxy, mav):
     '''takeoff get to 30m altitude'''
     mavproxy.send('switch 6\n') # stabilize mode
-    mavproxy.expect('STABILIZE>')
+    mav.wait_mode('STABILIZE')
     mavproxy.send('rc 3 1500\n')
     wait_altitude(mav, 30, 40)
     print("TAKEOFF COMPLETE")
@@ -50,10 +50,8 @@ def takeoff(mavproxy, mav):
 
 def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60):
     '''hold loiter position'''
-    mavproxy.send('switch 2\n') # loiter mode
-    mavproxy.expect('LOITER>')
-    mavproxy.send('status\n')
-    mavproxy.expect('>')
+    mavproxy.send('switch 5\n') # loiter mode
+    mav.wait_mode('LOITER')
     m = mav.recv_match(type='VFR_HUD', blocking=True)
     start_altitude = m.alt
     tstart = time.time()
@@ -74,7 +72,7 @@ def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60):
 def fly_square(mavproxy, mav, side=50, timeout=120):
     '''fly a square, flying N then E'''
     mavproxy.send('switch 6\n')
-    mavproxy.expect('STABILIZE>')
+    mav.wait_mode('STABILIZE')
     tstart = time.time()
     failed = False
 
@@ -134,9 +132,7 @@ def land(mavproxy, mav, timeout=60):
     '''land the quad'''
     print("STARTING LANDING")
     mavproxy.send('switch 6\n')
-    mavproxy.expect('STABILIZE>')
-    mavproxy.send('status\n')
-    mavproxy.expect('>')
+    mav.wait_mode('STABILIZE')
 
     # start by dropping throttle till we have lost 5m
     mavproxy.send('rc 3 1380\n')

Tools/autotest/common.py

@@ -35,6 +35,9 @@ class location(object):
         self.lng = lng
         self.alt = alt
 
+    def __str__(self):
+        return "lat=%.6f,lon=%.6f,alt=%.1f" % (self.lat, self.lng, self.alt)
+
 def get_distance(loc1, loc2):
     '''get ground distance between two locations'''
     dlat 		= loc2.lat - loc1.lat
@@ -72,6 +75,20 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
     return False
 
 
+def wait_roll(mav, roll, accuracy, timeout=30):
+    '''wait for a given roll in degrees'''
+    tstart = time.time()
+    print("Waiting for roll of %u" % roll)
+    while time.time() < tstart + timeout:
+        m = mav.recv_match(type='ATTITUDE', blocking=True)
+        r = math.degrees(m.roll)
+        print("Roll %u" % r)
+        if math.fabs(r - roll) <= accuracy:
+            return True
+    print("Failed to attain roll %u" % roll)
+    return False
+
+
 
 def wait_heading(mav, heading, accuracy=5, timeout=30):
     '''wait for a given heading'''
@@ -155,3 +172,7 @@ def save_wp(mavproxy, mav):
     mavproxy.send('rc 7 1000\n')
     mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
     mavproxy.send('wp list\n')
+
+def wait_mode(mav, mode):
+    '''wait for a flight mode to be engaged'''
+    mav.recv_match(condition='MAV.flightmode=="%s"' % mode, blocking=True)