From d507805891a2ed8912c8243f2caf02cbba4283ee Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 31 Jul 2018 12:02:11 +0200 Subject: [PATCH] Tools: move guided_achieve_heading to common --- Tools/autotest/arducopter.py | 19 ------------------- Tools/autotest/common.py | 19 +++++++++++++++++++ 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index e21c083fbb..539895e485 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1341,25 +1341,6 @@ class AutoTestCopter(AutoTest): if ex is not None: raise ex - def guided_achieve_heading(self, heading): - tstart = self.get_sim_time() - self.run_cmd(mavutil.mavlink.MAV_CMD_CONDITION_YAW, - heading, # target angle - 10, # degrees/second - 1, # -1 is counter-clockwise, 1 clockwise - 0, # 1 for relative, 0 for absolute - 0, # p5 - 0, # p6 - 0, # p7 - ) - while True: - if self.get_sim_time() - tstart > 200: - raise NotAchievedException() - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - self.progress("heading=%f want=%f" % (m.heading, heading)) - if m.heading == heading: - return - def fly_guided_change_submode(self): '''Ensure we can move around in guided after a takeoff command''' diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 19628b1ece..84aac14e6d 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -567,6 +567,25 @@ class AutoTest(ABC): self.mav.recv_match(condition='RC_CHANNELS.chan3_raw==1500', blocking=True) + def guided_achieve_heading(self, heading): + tstart = self.get_sim_time() + self.run_cmd(mavutil.mavlink.MAV_CMD_CONDITION_YAW, + heading, # target angle + 10, # degrees/second + 1, # -1 is counter-clockwise, 1 clockwise + 0, # 1 for relative, 0 for absolute + 0, # p5 + 0, # p6 + 0, # p7 + ) + while True: + if self.get_sim_time() - tstart > 200: + raise NotAchievedException() + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + self.progress("heading=%f want=%f" % (m.heading, heading)) + if m.heading == heading: + return + ################################################# # WAIT UTILITIES #################################################