From 17c42faad4102489a752d2fce2b701795e05c678 Mon Sep 17 00:00:00 2001 From: Andrii Fil Date: Tue, 3 Sep 2024 13:38:57 +0300 Subject: [PATCH] Tools: support MAV_CMD_EXTERNAL_WIND_ESTIMATE (move if) support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest (clean) support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest (remove lambda) support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest (timeout set wind) support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest (fix line too long) support MAV_CMD_EXTERNAL_WIND_ESTIMATE with autotest (clean) support MAV_CMD_EXTERNAL_WIND_ESTIMATE support MAV_CMD_EXTERNAL_WIND_ESTIMATE --- Tools/autotest/arduplane.py | 38 +++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 3ca7780e5ff453..098aa2bdc78b27 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -6052,6 +6052,43 @@ 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, timeout=1): + command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=speed, p3=direction) + + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException( + f"Failed to set wind speed or/and direction: speed != {speed} or direction != {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): + return True + + check_eq(1, 45, 45) + check_eq(2, 90, 90) + check_eq(3, 120, 120) + check_eq(4, 180, -180) + check_eq(5, 240, -120) + check_eq(6, 320, -40) + check_eq(7, 360, 0) + + command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=-2, p3=90, want_result=mavutil.mavlink.MAV_RESULT_DENIED) + command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=2, p3=-90, want_result=mavutil.mavlink.MAV_RESULT_DENIED) + command(mavutil.mavlink.MAV_CMD_EXTERNAL_WIND_ESTIMATE, p1=-2, p3=-90, want_result=mavutil.mavlink.MAV_RESULT_DENIED) + 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''' + self._MAV_CMD_EXTERNAL_WIND_ESTIMATE(self.run_cmd) + self._MAV_CMD_EXTERNAL_WIND_ESTIMATE(self.run_cmd_int) + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -6183,6 +6220,7 @@ def tests(self): self.GPSPreArms, self.SetHomeAltChange, self.ForceArm, + self.MAV_CMD_EXTERNAL_WIND_ESTIMATE, ]) return ret