From 3d562046da68ed786751471797d94abb4f1d2cfd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 14 May 2021 11:58:49 +1000 Subject: [PATCH] autotest: add accuracy option to guided_achieve_heading --- Tools/autotest/common.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index ccb167bc0d..02ebb7c73e 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -4631,7 +4631,7 @@ class AutoTest(ABC): self.wait_distance(distance, accuracy=2) self.set_rc(3, 1500) - def guided_achieve_heading(self, heading): + def guided_achieve_heading(self, heading, accuracy=None): tstart = self.get_sim_time() while True: if self.get_sim_time_cached() - tstart > 200: @@ -4648,6 +4648,10 @@ class AutoTest(ABC): ) m = self.mav.recv_match(type='VFR_HUD', blocking=True) self.progress("heading=%d want=%d" % (m.heading, int(heading))) + if accuracy is not None: + delta = abs(m.heading - int(heading)) + if delta <= accuracy: + return if m.heading == int(heading): return