AutoTest: wait 1sec when saving wp

Tools/autotest/common.py

@@ -49,6 +49,12 @@ def get_bearing(loc1, loc2):
         bearing += 360.00
     return bearing;
 
+def wait_seconds(seconds_to_wait):
+    tstart = time.time();
+    tnow = tstart
+    while tstart + seconds_to_wait > tnow:
+        tnow = time.time()
+
 def wait_altitude(mav, alt_min, alt_max, timeout=30):
     climb_rate = 0
     previous_alt = 0
@@ -208,10 +214,15 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
     return False
 
 def save_wp(mavproxy, mav):
+    mavproxy.send('rc 7 1000\n')
+    mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
+    wait_seconds(1)
     mavproxy.send('rc 7 2000\n')
     mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True)
+    wait_seconds(1)
     mavproxy.send('rc 7 1000\n')
     mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
+    wait_seconds(1)
 
 def wait_mode(mav, mode, timeout=None):
     print("Waiting for mode %s" % mode)