testing updates

Tools/autotest/common.py

@@ -80,7 +80,7 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30):
         m = mav.recv_match(type='VFR_HUD', blocking=True)
         climb_rate =  m.alt - previous_alt
         previous_alt = m.alt
-        print("Altitude %u, rate: %u" % (m.alt, climb_rate))
+        print("Wait Altitude %u, alt:%u, rate: %u" % (m.alt, alt_min , climb_rate))
         if abs(climb_rate) > 0:
             tstart = time.time();
         if m.alt >= alt_min and m.alt <= alt_max:
@@ -140,6 +140,8 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
         print("Distance %.2f meters" % delta)
         if math.fabs(delta - distance) <= accuracy:
             return True
+        if(delta > (distance + accuracy)):
+            return False
     print("Failed to attain distance %u" % distance)
     return False
 
@@ -181,14 +183,18 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, timeout=400):
         seq = m.seq
         m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
         wp_dist = m.wp_dist
-        print("test: WP %u (wp_dist=%u)" % (seq, wp_dist))
+        print("test: WP %u (wp_dist=%u), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, current_wp, wpnum_end))
         if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
             print("test: Starting new waypoint %u" % seq)
             tstart = time.time()
             current_wp = seq
             # the wp_dist check is a hack until we can sort out the right seqnum
             # for end of mission
-        if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2):
+        #if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2):
+        if (current_wp == wpnum_end and wp_dist < 2):
+            print("Reached final waypoint %u" % seq)
+            return True
+        if (current_wp == 255):
             print("Reached final waypoint %u" % seq)
             return True
         if seq > current_wp+1:
@@ -202,8 +208,6 @@ def save_wp(mavproxy, mav):
     mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==2000', blocking=True)
     mavproxy.send('rc 7 1000\n')
     mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
-    #mavproxy.send('wp list\n')
-    #mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
 
 def wait_mode(mav, mode):
     '''wait for a flight mode to be engaged'''