From 0ee02288833cdfe795a57c765f6d0f5f1d1b9714 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 20 Apr 2021 21:18:17 +1000 Subject: [PATCH] autotest: fix flapping fly-home-land-and-disarm One of the tests just happened to be within acceptance radius of waypoint 8 - so when we set that as a waypoint we never saw it actually be our waypoint so the set_waypoint failed --- Tools/autotest/arduplane.py | 4 ++++ Tools/autotest/common.py | 9 +++++++++ 2 files changed, 13 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 36dabd4c2b..9d96716de1 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -696,6 +696,10 @@ class AutoTestPlane(AutoTest): self.progress("Using %s to fly home" % filename) self.load_mission(filename) self.change_mode("AUTO") + # don't set current waypoint to 8 unless we're distant from it + # or we arrive instantly and never see it as our current + # waypoint: + self.wait_distance_to_waypoint(8, 100, 10000000) self.set_current_waypoint(8) self.drain_mav() # TODO: reflect on file to find this magic waypoint number? diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index f95e0bfbfc..a0fb198224 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -4984,6 +4984,15 @@ class AutoTest(ABC): **kwargs ) + def wait_distance_to_waypoint(self, wp_num, distance_min, distance_max, **kwargs): + # TODO: use mission_request_partial_list_send + wps = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) + m = wps[wp_num] + self.progress("m: %s" % str(m)) + loc = mavutil.location(m.x / 1.0e7, m.y / 1.0e7, 0, 0) + self.progress("loc: %s" % str(loc)) + self.wait_distance_to_location(loc, distance_min, distance_max, **kwargs) + def wait_distance_to_location(self, location, distance_min, distance_max, timeout=30, **kwargs): """Wait for flight of a given distance.""" assert distance_min <= distance_max, "Distance min should be less than distance max."