From 4c20bc2d9d8f369055325baefd0e55bf08d1ac88 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 6 May 2021 11:11:31 +1000 Subject: [PATCH] autotest: ensure reasonable yawrate on takeoff in FlyEachFrame --- Tools/autotest/arducopter.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index adc8f651bb..7d3ab10495 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -6640,7 +6640,18 @@ class AutoTestCopter(AutoTest): model=model, wipe=True, ) + + # add a listener that verifies yaw looks good: + def verify_yaw(mav, m): + if m.get_type() != 'ATTITUDE': + return + yawspeed_thresh_rads = math.radians(10) + if m.yawspeed > yawspeed_thresh_rads: + raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" % + (math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame)) + self.install_message_hook(verify_yaw) self.takeoff(10) + self.remove_message_hook(verify_yaw) self.do_RTL() def create_simple_relhome_mission(self, items_in, target_system=1, target_component=1):