support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest (timeout set wind)

ArduPilot · Aug 30, 2024 · c3bdd72 · c3bdd72
1 parent bebfbf5
commit c3bdd72
Showing 1 changed file with 12 additions and 8 deletions.
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
@@ -6053,19 +6053,23 @@ def ForceArm(self):
             self.disarm_vehicle()
 
     def _MAV_CMD_EXTERNAL_WIND_ESTIMATE(self, command):
+        self.reboot_sitl()
+
         def cmp_with_variance(a, b, p):
             return abs(a - b) < p
 
-        def check_eq(speed, direction, ret_dir):
+        def check_eq(speed, direction, ret_dir, timeout=1):
             command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=speed, p3=direction)
-            for i in range(10):
+
+            tstart = self.get_sim_time()
+            while True:
+                if self.get_sim_time_cached() - tstart > timeout:
+                    raise NotAchievedException("Failed to set wind speed or/and direction: speed != {} or direction != {}".format(
+                        speed, direction))
+
                 m = self.assert_receive_message("WIND")
-
                 if cmp_with_variance(m.speed, speed, 0.5) and cmp_with_variance(m.direction, ret_dir, 5):
-                    break
-            else:
-                raise NotAchievedException("Wind is not equal to given values {} != {} or {} != {}".format(
-                    m.speed, speed, m.direction, ret_dir))
+                    return True
 
         check_eq(1, 45, 45)
         check_eq(2, 90, 90)
@@ -6081,7 +6085,7 @@ def check_eq(speed, direction, ret_dir):
         command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=2, p3=370, want_result=mavutil.mavlink.MAV_RESULT_DENIED)
 
     def MAV_CMD_EXTERNAL_WIND_ESTIMATE(self):
-        '''test _MAV_CMD_EXTERNAL_WIND_ESTIMATE as a mavlink command'''
+        '''test MAV_CMD_EXTERNAL_WIND_ESTIMATE as a mavlink command'''
         self._MAV_CMD_EXTERNAL_WIND_ESTIMATE(self.run_cmd)
         self._MAV_CMD_EXTERNAL_WIND_ESTIMATE(self.run_cmd_int)