autotest: reimplement wait_location with new object-based approach

the existing wait_and_maintain method is problematic when it comes to waiting on different value types.

Tools/autotest/arducopter.py

@@ -7090,22 +7090,23 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
 
     def check_avoidance_corners(self):
         self.takeoff(10, mode="LOITER")
+        here = self.mav.location()
         self.set_rc(2, 1400)
         west_loc = mavutil.location(-35.363007,
                                     149.164911,
-                                    0,
+                                    here.alt,
                                     0)
         self.wait_location(west_loc, accuracy=6)
         north_loc = mavutil.location(-35.362908,
                                      149.165051,
-                                     0,
+                                     here.alt,
                                      0)
         self.reach_heading_manual(0)
         self.wait_location(north_loc, accuracy=6, timeout=200)
         self.reach_heading_manual(90)
         east_loc = mavutil.location(-35.363013,
                                     149.165194,
-                                    0,
+                                    here.alt,
                                     0)
         self.wait_location(east_loc, accuracy=6)
         self.reach_heading_manual(225)
@@ -7556,18 +7557,19 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
 
             self.takeoff(10, mode="LOITER")
             self.set_rc(2, 1400)
-            west_loc = mavutil.location(-35.362919, 149.165055, 0, 0)
+            here = self.mav.location()
+            west_loc = mavutil.location(-35.362919, 149.165055, here.alt, 0)
             self.wait_location(west_loc, accuracy=1)
             self.reach_heading_manual(0)
-            north_loc = mavutil.location(-35.362881, 149.165103, 0, 0)
+            north_loc = mavutil.location(-35.362881, 149.165103, here.alt, 0)
             self.wait_location(north_loc, accuracy=1)
             self.set_rc(2, 1500)
             self.set_rc(1, 1600)
-            east_loc = mavutil.location(-35.362986, 149.165227, 0, 0)
+            east_loc = mavutil.location(-35.362986, 149.165227, here.alt, 0)
             self.wait_location(east_loc, accuracy=1)
             self.set_rc(1, 1500)
             self.set_rc(2, 1600)
-            south_loc = mavutil.location(-35.363025, 149.165182, 0, 0)
+            south_loc = mavutil.location(-35.363025, 149.165182, here.alt, 0)
             self.wait_location(south_loc, accuracy=1)
             self.set_rc(2, 1500)
             self.do_RTL()

Tools/autotest/arduplane.py

@@ -175,10 +175,11 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
     def fly_RTL(self):
         """Fly to home."""
         self.progress("Flying home in RTL")
+        target_loc = self.homeloc
+        target_loc.alt += 100
         self.change_mode('RTL')
-        self.wait_location(self.homeloc,
+        self.wait_location(target_loc,
                            accuracy=120,
-                           target_altitude=self.homeloc.alt+100,
                            height_accuracy=20,
                            timeout=180)
         self.progress("RTL Complete")
@@ -1877,8 +1878,8 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
         self.wait_ready_to_arm()
         self.homeloc = self.mav.location()
 
-        guided_loc = mavutil.location(-35.39723762, 149.07284612, 99.0, 0)
+        guided_loc = mavutil.location(-35.39723762, 149.07284612, self.homeloc.alt+99.0, 0)
-        rally_loc = mavutil.location(-35.3654952000, 149.1558698000, 100, 0)
+        rally_loc = mavutil.location(-35.3654952000, 149.1558698000, self.homeloc.alt+100, 0)
 
         terrain_wait_path(self.homeloc, rally_loc, 10)
 
@@ -1898,19 +1899,23 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
         self.change_mode("GUIDED")
         self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT)
         self.progress("Flying to guided location")
-        self.wait_location(guided_loc,
+        self.wait_location(
-                           accuracy=200,
+            guided_loc,
-                           target_altitude=None,
+            accuracy=200,
-                           timeout=600)
+            timeout=600,
+            height_accuracy=10,
+        )
 
         self.progress("Reached guided location")
         self.set_parameter("RALLY_LIMIT_KM", 50)
         self.change_mode("RTL")
         self.progress("Flying to rally point")
-        self.wait_location(rally_loc,
+        self.wait_location(
-                           accuracy=200,
+            rally_loc,
-                           target_altitude=None,
+            accuracy=200,
-                           timeout=600)
+            timeout=600,
+            height_accuracy=10,
+        )
         self.progress("Reached rally point with TERRAIN_FOLLOW")
 
         # Fly back to guided location
@@ -1933,17 +1938,21 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
         self.check_rally_upload_download(rally_item)
 
         # Once back at guided location re-trigger RTL
-        self.wait_location(guided_loc,
+        self.wait_location(
-                           accuracy=200,
+            guided_loc,
-                           target_altitude=None,
+            accuracy=200,
-                           timeout=600)
+            timeout=600,
+            height_accuracy=10,
+        )
 
         self.change_mode("RTL")
         self.progress("Flying to rally point")
-        self.wait_location(rally_loc,
+        self.wait_location(
-                           accuracy=200,
+            rally_loc,
-                           target_altitude=None,
+            accuracy=200,
-                           timeout=600)
+            timeout=600,
+            height_accuracy=10,
+        )
         self.progress("Reached rally point with terrain alt frame")
 
         self.context_pop()

Tools/autotest/rover.py

@@ -1369,7 +1369,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         # location copied in from rover-test-rally.txt:
         loc = mavutil.location(40.071553,
                                -105.229401,
-                               0,
+                               1583,
                                0)
 
         self.wait_location(loc, accuracy=accuracy, minimum_duration=10, timeout=45)

Tools/autotest/vehicle_test_suite.py

@@ -342,6 +342,117 @@ class Telem(object):
         self.update_read()
 
 
+class WaitAndMaintain(object):
+    def __init__(self,
+                 test_suite,
+                 minimum_duration=None,
+                 progress_print_interval=1,
+                 timeout=30,
+                 ):
+        self.test_suite = test_suite
+        self.minimum_duration = minimum_duration
+        self.achieving_duration_start = None
+        self.timeout = timeout
+        self.last_progress_print = 0
+        self.progress_print_interval = progress_print_interval
+
+    def run(self):
+        self.announce_test_start()
+
+        tstart = self.test_suite.get_sim_time_cached()
+        while True:
+            now = self.test_suite.get_sim_time_cached()
+            current_value = self.get_current_value()
+            if now - self.last_progress_print > self.progress_print_interval:
+                self.print_progress(now, current_value)
+                self.last_progress_print = now
+
+            # check for timeout
+            if now - tstart > self.timeout:
+                raise self.timeoutexception()
+
+            # handle the case where we are are achieving our value:
+            if self.validate_value(current_value):
+                if self.achieving_duration_start is None:
+                    self.achieving_duration_start = now
+                if (self.minimum_duration is None or
+                        now - self.achieving_duration_start > self.minimum_duration):
+                    self.announce_success()
+                    return True
+                continue
+
+            # handle the case where we are not achieving our value:
+            self.achieving_duration_start = None
+
+    def progress(self, text):
+        self.test_suite.progress(text)
+
+    def announce_test_start(self):
+        self.progress(self.announce_start_text())
+
+    def announce_success(self):
+        self.progress(self.success_text())
+
+    def print_progress(self, now, value):
+        text = self.progress_text(value)
+        if self.achieving_duration_start is not None:
+            delta = now - self.achieving_duration_start
+            text += f" (maintain={delta:.1f}/{self.minimum_duration})"
+        self.progress(text)
+
+
+class WaitAndMaintainLocation(WaitAndMaintain):
+    def __init__(self, test_suite, target, accuracy=5, height_accuracy=1, **kwargs):
+        super(WaitAndMaintainLocation, self).__init__(test_suite, **kwargs)
+        self.target = target
+        self.height_accuracy = height_accuracy
+        self.accuracy = accuracy
+
+    def announce_start_text(self):
+        t = self.target
+        if self.height_accuracy is not None:
+            return ("Waiting for distance to Location (%.4f, %.4f, %.2f) (h_err<%f, v_err<%.2f " %
+                    (t.lat, t.lng, t.alt*0.01, self.accuracy, self.height_accuracy))
+        return ("Waiting for distance to Location (%.4f, %.4f) (h_err<%f" %
+                (t.lat, t.lng, self.accuracy))
+
+    def get_target_value(self):
+        return self.loc
+
+    def get_current_value(self):
+        return self.test_suite.mav.location()
+
+    def horizontal_error(self, value):
+        return self.test_suite.get_distance(value, self.target)
+
+    def vertical_error(self, value):
+        return math.fabs(value.alt*0.01 - self.target.alt*0.01)
+
+    def validate_value(self, value):
+        if self.horizontal_error(value) > self.accuracy:
+            return False
+
+        if self.height_accuracy is None:
+            return True
+
+        if self.vertical_error(value) > self.height_accuracy:
+            return False
+
+        return True
+
+    def success_text(self):
+        return "Reached location"
+
+    def timeoutexception(self):
+        return AutoTestTimeoutException("Failed to attain location")
+
+    def progress_text(self, current_value):
+        if self.height_accuracy is not None:
+            return (f"Want=({self.target.lat:.7f},{self.target.lng:.7f},{self.target.alt:.2f}) Got=({current_value.lat:.7f},{current_value.lng:.7f},{current_value.alt:.2f}) dist={self.horizontal_error(current_value):.2f} vdist={self.vertical_error(current_value):.2f}")  # noqa
+
+        return (f"Want=({self.target.lat},{self.target.lng}) distance={self.horizontal_error(current_value)}")
+
+
 class MSP_Generic(Telem):
     def __init__(self, destination_address):
         super(MSP_Generic, self).__init__(destination_address)
@@ -7490,38 +7601,9 @@ class TestSuite(ABC):
                                       0, # "land dir"
                                       0) # flags
 
-    def wait_location(self,
+    def wait_location(self, loc, **kwargs):
-                      loc,
+        waiter = WaitAndMaintainLocation(self, loc, **kwargs)
-                      accuracy=5.0,
+        waiter.run()
-                      timeout=30,
-                      target_altitude=None,
-                      height_accuracy=-1,
-                      **kwargs):
-        """Wait for arrival at a location."""
-        def get_distance_to_loc():
-            return self.get_distance(self.mav.location(), loc)
-
-        def validator(value2, empty=None):
-            if value2 <= accuracy:
-                if target_altitude is not None:
-                    height_delta = math.fabs(self.mav.location().alt - target_altitude)
-                    if height_accuracy != -1 and height_delta > height_accuracy:
-                        return False
-                return True
-            else:
-                return False
-        debug_text = "Distance to Location (%.4f, %.4f) " % (loc.lat, loc.lng)
-        if target_altitude is not None:
-            debug_text += ",at altitude %.1f height_accuracy=%.1f, d" % (target_altitude, height_accuracy)
-        self.wait_and_maintain(
-            value_name=debug_text,
-            target=0,
-            current_value_getter=lambda: get_distance_to_loc(),
-            accuracy=accuracy,
-            validator=lambda value2, target2: validator(value2, None),
-            timeout=timeout,
-            **kwargs
-        )
 
     def assert_current_waypoint(self, wpnum):
         seq = self.mav.waypoint_current()
@@ -10774,7 +10856,7 @@ Also, ignores heartbeats not from our target system'''
         # Disable heading and yaw test on rover type
 
         if self.is_rover():
-            test_alt = False
+            test_alt = True
             test_heading = False
             test_yaw_rate = False
         else:
@@ -10835,6 +10917,16 @@ Also, ignores heartbeats not from our target system'''
                 0,  # yawrate
             )
 
+        def testpos(self, targetpos : mavutil.location, test_alt : bool, frame_name : str, frame):
+            send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame)
+            self.wait_location(
+                targetpos,
+                accuracy=wp_accuracy,
+                timeout=timeout,
+                height_accuracy=(2 if test_alt else None),
+                minimum_duration=2,
+            )
+
         for frame in MAV_FRAMES_TO_TEST:
             frame_name = mavutil.mavlink.enums["MAV_FRAME"][frame].name
             self.start_subtest("Testing Set Position in %s" % frame_name)
@@ -10842,37 +10934,25 @@ Also, ignores heartbeats not from our target system'''
             targetpos.lat += 0.0001
             if test_alt:
                 targetpos.alt += 5
-            send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame)
+            testpos(self, targetpos, test_alt, frame_name, frame)
-            self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout,
-                               target_altitude=(targetpos.alt if test_alt else None),
-                               height_accuracy=2, minimum_duration=2)
 
             self.start_subtest("Changing Longitude")
             targetpos.lng += 0.0001
             if test_alt:
                 targetpos.alt -= 5
-            send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame)
+            testpos(self, targetpos, test_alt, frame_name, frame)
-            self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout,
-                               target_altitude=(targetpos.alt if test_alt else None),
-                               height_accuracy=2, minimum_duration=2)
 
             self.start_subtest("Revert Latitude")
             targetpos.lat -= 0.0001
             if test_alt:
                 targetpos.alt += 5
-            send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame)
+            testpos(self, targetpos, test_alt, frame_name, frame)
-            self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout,
-                               target_altitude=(targetpos.alt if test_alt else None),
-                               height_accuracy=2, minimum_duration=2)
 
             self.start_subtest("Revert Longitude")
             targetpos.lng -= 0.0001
             if test_alt:
                 targetpos.alt -= 5
-            send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame)
+            testpos(self, targetpos, test_alt, frame_name, frame)
-            self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout,
-                               target_altitude=(targetpos.alt if test_alt else None),
-                               height_accuracy=2, minimum_duration=2)
 
             if test_heading:
                 self.start_subtest("Testing Yaw targetting in %s" % frame_name)
@@ -10900,9 +10980,13 @@ Also, ignores heartbeats not from our target system'''
                     math.radians(42),  # yaw
                     0,  # yawrate
                 )
-                self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout,
+                self.wait_location(
-                                   target_altitude=(targetpos.alt if test_alt else None),
+                    targetpos,
-                                   height_accuracy=2, minimum_duration=2)
+                    accuracy=wp_accuracy,
+                    timeout=timeout,
+                    height_accuracy=(2 if test_alt else None),
+                    minimum_duration=2,
+                )
                 self.wait_heading(42, minimum_duration=5, timeout=timeout)
 
                 self.start_subtest("Revert Latitude and Heading")
@@ -10929,9 +11013,13 @@ Also, ignores heartbeats not from our target system'''
                     math.radians(0),  # yaw
                     0,  # yawrate
                 )
-                self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout,
+                self.wait_location(
-                                   target_altitude=(targetpos.alt if test_alt else None),
+                    targetpos,
-                                   height_accuracy=2, minimum_duration=2)
+                    accuracy=wp_accuracy,
+                    timeout=timeout,
+                    height_accuracy=(2 if test_alt else None),
+                    minimum_duration=2,
+                )
                 self.wait_heading(0, minimum_duration=5, timeout=timeout)
 
             if test_yaw_rate:
@@ -10967,9 +11055,12 @@ Also, ignores heartbeats not from our target system'''
                 self.wait_yaw_speed(target_rate, timeout=timeout,
                                     called_function=lambda plop, empty: send_yaw_rate(
                                         target_rate, None), minimum_duration=5)
-                self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout,
+                self.wait_location(
-                                   target_altitude=(targetpos.alt if test_alt else None),
+                    targetpos,
-                                   height_accuracy=2)
+                    accuracy=wp_accuracy,
+                    timeout=timeout,
+                    height_accuracy=(2 if test_alt else None),
+                )
 
                 self.start_subtest("Revert Latitude and invert Yaw rate")
                 target_rate = -1.0
@@ -10979,9 +11070,12 @@ Also, ignores heartbeats not from our target system'''
                 self.wait_yaw_speed(target_rate, timeout=timeout,
                                     called_function=lambda plop, empty: send_yaw_rate(
                                         target_rate, None), minimum_duration=5)
-                self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout,
+                self.wait_location(
-                                   target_altitude=(targetpos.alt if test_alt else None),
+                    targetpos,
-                                   height_accuracy=2)
+                    accuracy=wp_accuracy,
+                    timeout=timeout,
+                    height_accuracy=(2 if test_alt else None),
+                )
                 self.start_subtest("Changing Yaw rate to zero")
                 target_rate = 0.0
                 self.wait_yaw_speed(target_rate, timeout=timeout,