From c3bdd7244860b62919c3cd691dab19f90abbb7e2 Mon Sep 17 00:00:00 2001 From: Andrii Fil Date: Fri, 30 Aug 2024 17:21:09 +0300 Subject: [PATCH] support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest (timeout set wind) --- Tools/autotest/arduplane.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 5ab3b4be16fc56..4c64eb42faecea 100644 --- 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)