Tools: Common : create generic wait_and_maintain function, upgrade all wait functions to use it

Tools/autotest/common.py

@@ -2736,137 +2736,149 @@ class AutoTest(ABC):
         while tstart + seconds_to_wait > tnow:
             tnow = self.get_sim_time()
 
-    def wait_altitude(self, alt_min, alt_max, timeout=30, relative=False):
+    def wait_altitude(self, altitude_min, altitude_max, relative=False, timeout=30, **kwargs):
         """Wait for a given altitude range."""
-        previous_alt = None
+        assert altitude_min <= altitude_max, "Minimum altitude should be less than maximum altitude."
 
-        tstart = self.get_sim_time()
+        def get_altitude(alt_relative=False, timeout2=30):
-        self.progress("Waiting for altitude between %.02f and %.02f" %
+            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=timeout2)
-                      (alt_min, alt_max))
+            if msg:
-        last_wait_alt_msg = 0
+                if alt_relative:
-        while self.get_sim_time_cached() < tstart + timeout:
+                    return msg.relative_alt / 1000.0  # mm -> m
-            m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
+                else:
-            if m is None:
+                    return msg.alt / 1000.0  # mm -> m
-                continue
+            raise MsgRcvTimeoutException("Failed to get Global Position")
-            if relative:
-                alt = m.relative_alt/1000.0 # mm -> m
-            else:
-                alt = m.alt/1000.0 # mm -> m
 
-            if previous_alt is None:
+        def validator(value2, target2=None):
-                climb_rate = "-"
+            if altitude_min <= value2 <= altitude_max:
-            else:
-                climb_rate = "%.02f" % (alt - previous_alt)
-            previous_alt = alt
-            ok = alt >= alt_min and alt <= alt_max
-            if ok or self.get_sim_time_cached() - last_wait_alt_msg > 1:
-                self.progress("Wait Altitude: Cur:%.02f min:%.02f max:%.02f climb_rate: %s"
-                              % (alt, alt_min, alt_max, climb_rate))
-                last_wait_alt_msg = self.get_sim_time_cached()
-            if ok:
-                self.progress("Altitude OK")
                 return True
-        raise WaitAltitudeTimout("Failed to attain altitude range")
+            else:
+                return False
 
-    def wait_groundspeed(self, gs_min, gs_max, timeout=30):
+        self.wait_and_maintain(value_name="Altitude", target=altitude_min, current_value_getter=lambda: get_altitude(relative, timeout), accuracy=(altitude_max - altitude_min), validator=lambda value2, target2: validator(value2, target2), timeout=timeout, **kwargs)
+
+    def wait_groundspeed(self, speed_min, speed_max, timeout=30, **kwargs):
         """Wait for a given ground speed range."""
-        self.progress("Waiting for groundspeed between %.1f and %.1f" %
+        assert speed_min <= speed_max, "Minimum speed should be less than maximum speed."
-                      (gs_min, gs_max))
-        last_print = 0
-        tstart = self.get_sim_time()
-        while self.get_sim_time_cached() < tstart + timeout:
-            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
-            if self.get_sim_time_cached() - last_print > 1:
-                self.progress("Wait groundspeed %.3f, target:%.3f" %
-                              (m.groundspeed, gs_min))
-                last_print = self.get_sim_time_cached()
-            if m.groundspeed >= gs_min and m.groundspeed <= gs_max:
-                return True
-        raise WaitGroundSpeedTimeout("Failed to attain groundspeed range")
 
-    def wait_roll(self, roll, accuracy, timeout=30):
+        def get_groundspeed(timeout2):
+            msg = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=timeout2)
+            if msg:
+                return msg.groundspeed
+            raise MsgRcvTimeoutException("Failed to get Groundspeed")
+
+        def validator(value2, target2=None):
+            if speed_min <= value2 <= speed_max:
+                return True
+            else:
+                return False
+
+        self.wait_and_maintain(value_name="Groundspeed", target=speed_min, current_value_getter=lambda: get_groundspeed(timeout), accuracy=(speed_max - speed_min), validator=lambda value2, target2: validator(value2, target2), timeout=timeout, **kwargs)
+
+    def wait_roll(self, roll, accuracy, timeout=30, **kwargs):
         """Wait for a given roll in degrees."""
-        tstart = self.get_sim_time()
+        def get_roll(timeout2):
-        self.progress("Waiting for roll of %d at %s" % (roll, time.ctime()))
+            msg = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=timeout2)
-        while self.get_sim_time_cached() < tstart + timeout:
+            if msg:
-            m = self.mav.recv_match(type='ATTITUDE', blocking=True)
+                p = math.degrees(msg.pitch)
-            p = math.degrees(m.pitch)
+                r = math.degrees(msg.roll)
-            r = math.degrees(m.roll)
+                self.progress("Roll %d Pitch %d" % (r, p))
-            self.progress("Roll %d Pitch %d" % (r, p))
+                return r
-            if math.fabs(r - roll) <= accuracy:
+            raise MsgRcvTimeoutException("Failed to get Roll")
-                self.progress("Attained roll %d" % roll)
-                return True
-        raise WaitRollTimeout("Failed to attain roll %d" % roll)
 
-    def wait_pitch(self, pitch, accuracy, timeout=30):
+        def validator(value2, target2):
+            return math.fabs((value2 - target2 + 180) % 360 - 180) <= accuracy
+
+        self.wait_and_maintain(value_name="Roll", target=roll, current_value_getter=lambda: get_roll(timeout), validator=lambda value2, target2: validator(value2, target2), accuracy=accuracy, timeout=timeout, **kwargs)
+
+    def wait_pitch(self, pitch, accuracy, timeout=30, **kwargs):
         """Wait for a given pitch in degrees."""
-        tstart = self.get_sim_time()
+        def get_pitch(timeout2):
-        self.progress("Waiting for pitch of %.02f at %s" % (pitch, time.ctime()))
+            msg = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=timeout2)
-        while self.get_sim_time_cached() < tstart + timeout:
+            if msg:
-            m = self.mav.recv_match(type='ATTITUDE', blocking=True)
+                p = math.degrees(msg.pitch)
-            p = math.degrees(m.pitch)
+                r = math.degrees(msg.roll)
-            r = math.degrees(m.roll)
+                self.progress("Pitch %d Roll %d" % (p, r))
-            self.progress("Pitch %d Roll %d" % (p, r))
+                return p
-            if math.fabs(p - pitch) <= accuracy:
+            raise MsgRcvTimeoutException("Failed to get Pitch")
-                self.progress("Attained pitch %d" % pitch)
-                return True
-        raise WaitPitchTimeout("Failed to attain pitch %d" % pitch)
 
-    def wait_heading(self, heading, accuracy=5, timeout=30):
+        def validator(value2, target2):
-        """Wait for a given heading."""
+            return math.fabs((value2 - target2 + 180) % 360 - 180) <= accuracy
+
+        self.wait_and_maintain(value_name="Pitch", target=pitch, current_value_getter=lambda: get_pitch(timeout), validator=lambda value2, target2: validator(value2, target2), accuracy=accuracy, timeout=timeout, **kwargs)
+
+    def wait_and_maintain(self, value_name, target, current_value_getter, validator=None, accuracy=2.0, timeout=30, **kwargs):
         tstart = self.get_sim_time()
-        self.progress("Waiting for heading %.02f with accuracy %.02f" %
+        achieving_duration_start = None
-                      (heading, accuracy))
+        sum_of_achieved_values = 0.0
+        last_value = 0.0
+        count_of_achieved_values = 0
+        called_function = kwargs.get("called_function", None)
+        minimum_duration = kwargs.get("minimum_duration", 0)
+        self.progress("Waiting for %s %.02f with accuracy %.02f" % (value_name, target, accuracy))
         last_print_time = 0
-        while True:
+        while self.get_sim_time_cached() < tstart + timeout:  # if we failed to received message with the getter the sim time isn't updated
-            now = self.get_sim_time_cached()
+            last_value = current_value_getter()
-            if now - tstart >= timeout:
+            if called_function is not None:
-                break
+                called_function(last_value, target)
-            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
+            if self.get_sim_time_cached() - last_print_time > 1:
-            if now - last_print_time > 1:
+                self.progress("%s=%0.2f (want %f +- %f)" %
-                self.progress("Heading %d (want %f +- %f)" %
+                              (value_name, last_value, target, accuracy))
-                              (m.heading, heading, accuracy))
+                last_print_time = self.get_sim_time_cached()
-                last_print_time = now
+            if validator is not None:
-            if math.fabs(m.heading - heading) <= accuracy:
+                is_value_valid = validator(last_value, target)
-                self.progress("Attained heading %d" % heading)
+            else:
-                return True
+                is_value_valid = math.fabs(last_value - target) <= accuracy
-        raise WaitHeadingTimeout("Failed to attain heading %d" % heading)
+            if is_value_valid:
+                sum_of_achieved_values += last_value
+                count_of_achieved_values += 1.0
+                if achieving_duration_start is None:
+                    achieving_duration_start = self.get_sim_time_cached()
+                if self.get_sim_time_cached() - achieving_duration_start >= minimum_duration:
+                    self.progress("Attained %s=%f" % (value_name, sum_of_achieved_values / count_of_achieved_values))
+                    return True
+            else:
+                achieving_duration_start = None
+                sum_of_achieved_values = 0.0
+                count_of_achieved_values = 0
+        raise AutoTestTimeoutException("Failed to attain %s %f, reach %f" % (value_name, target, (sum_of_achieved_values / count_of_achieved_values) if count_of_achieved_values != 0 else last_value))
 
-    def wait_distance(self, distance, accuracy=5, timeout=30):
+    def wait_heading(self, heading, accuracy=5, timeout=30, **kwargs):
+        """Wait for a given heading."""
+        def get_heading_wrapped(timeout2):
+            msg = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=timeout2)
+            if msg:
+                return msg.heading
+            raise MsgRcvTimeoutException("Failed to get heading")
+
+        def validator(value2, target2):
+            return math.fabs((value2 - target2 + 180) % 360 - 180) <= accuracy
+
+        self.wait_and_maintain(value_name="Heading", target=heading, current_value_getter=lambda: get_heading_wrapped(timeout), validator=lambda value2, target2: validator(value2, target2), accuracy=accuracy, timeout=timeout, **kwargs)
+
+    def wait_distance(self, distance, accuracy=2, timeout=30, **kwargs):
         """Wait for flight of a given distance."""
-        tstart = self.get_sim_time()
         start = self.mav.location()
-        last_distance_message = 0
-        while self.get_sim_time_cached() < tstart + timeout:
-            pos = self.mav.location()
-            delta = self.get_distance(start, pos)
-            if self.get_sim_time_cached() - last_distance_message >= 1:
-                self.progress("Distance=%.2f meters want=%.2f" %
-                              (delta, distance))
-                last_distance_message = self.get_sim_time_cached()
-            if math.fabs(delta - distance) <= accuracy:
-                self.progress("Attained distance %.02f meters OK" % delta)
-                return True
-            if delta > (distance + accuracy):
-                raise WaitDistanceTimeout("Failed distance - overshoot delta=%f dist=%f"
-                                          % (delta, distance))
-        raise WaitDistanceTimeout("Failed to attain distance %u" % distance)
 
-    def wait_distance_to_location(self, location, distance_min, distance_max, timeout=30):
+        def get_distance():
-        last_distance_message = 0
+            return self.get_distance(start, self.mav.location())
-        tstart = self.get_sim_time()
+
-        while self.get_sim_time_cached() < tstart + timeout:
+        def validator(value2, target2):
-            pos = self.mav.location()
+            return (value2 - target2) >= accuracy
-            delta = self.get_distance(location, pos)
+
-            if self.get_sim_time_cached() - last_distance_message >= 1:
+        self.wait_and_maintain(value_name="Distance", target=distance, current_value_getter=lambda: get_distance(), validator=lambda value2, target2: validator(value2, target2), accuracy=accuracy, timeout=timeout, **kwargs)
-                self.progress("Distance=%.2f meters want <%.2f and >%.2f" %
+
-                              (delta, distance_min, distance_max))
+    def wait_distance_to_location(self, location, distance_min, distance_max, timeout=30, **kwargs):
-                last_distance_message = self.get_sim_time_cached()
+        """Wait for flight of a given distance."""
-            if (delta >= distance_min and delta <= distance_max):
+        assert distance_min <= distance_max, "Distance min should be less than distance max."
-                self.progress("Attained distance %.02f meters OK" % delta)
+
-                return True
+        def get_distance():
-        raise WaitDistanceTimeout("Failed to attain distance <%.2f and >%.2f" %
+            return self.get_distance(location, self.mav.location())
-                                  (distance_min, distance_max))
+
+        def validator(value2, target2=None):
+            return distance_min <= value2 <= distance_max
+
+        self.wait_and_maintain(value_name="Distance", target=distance_min, current_value_getter=lambda: get_distance(), validator=lambda value2, target2: validator(value2, target2), accuracy=(distance_max - distance_min), timeout=timeout, **kwargs)
 
     def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operator.eq):
         """wait for channel value comparison (default condition is equality)"""
@@ -2915,36 +2927,28 @@ class AutoTest(ABC):
 
     def wait_location(self,
                       loc,
-                      accuracy=5,
+                      accuracy=5.0,
                       timeout=30,
                       target_altitude=None,
-                      height_accuracy=-1):
+                      height_accuracy=-1,
+                      **kwargs):
         """Wait for arrival at a location."""
-        tstart = self.get_sim_time()
+        def get_distance_to_loc():
-        if target_altitude is None:
+            return self.get_distance(self.mav.location(), loc)
-            target_altitude = loc.alt
+
-        self.progress("Waiting for location"
+        def validator(value2, empty=None):
-                      "%.4f,%.4f at altitude %.1f height_accuracy=%.1f" %
+            if value2 <= accuracy:
-                      (loc.lat, loc.lng, target_altitude, height_accuracy))
+                if target_altitude is not None:
-        last_distance_message = 0
+                    height_delta = math.fabs(self.mav.location().alt - target_altitude)
-        closest = 10000000
+                    if height_accuracy != -1 and height_delta > height_accuracy:
-        last = 0
+                        return False
-        while self.get_sim_time() < tstart + timeout:
-            pos = self.mav.location()
-            delta = self.get_distance(loc, pos)
-            if self.get_sim_time_cached() - last_distance_message >= 1:
-                self.progress("Distance %.2f meters alt %.1f" % (delta, pos.alt))
-                last_distance_message = self.get_sim_time_cached()
-            if closest > delta:
-                closest = delta
-            last = delta
-            if delta <= accuracy:
-                height_delta = math.fabs(pos.alt - target_altitude)
-                if height_accuracy != -1 and height_delta > height_accuracy:
-                    continue
-                self.progress("Reached location (%.2f meters)" % delta)
                 return True
-        raise WaitLocationTimeout("Failed to attain location (want=<%f) (closest=%f) (last=%f)" % (accuracy, closest, last))
+            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" % (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 wait_current_waypoint(self, wpnum, timeout=60):
         tstart = self.get_sim_time()