mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
autotest: ensure reasonable yawrate on takeoff in FlyEachFrame
This commit is contained in:
parent
ba4634a775
commit
0564ba1870
@ -6640,7 +6640,18 @@ class AutoTestCopter(AutoTest):
|
|||||||
model=model,
|
model=model,
|
||||||
wipe=True,
|
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.takeoff(10)
|
||||||
|
self.remove_message_hook(verify_yaw)
|
||||||
self.do_RTL()
|
self.do_RTL()
|
||||||
|
|
||||||
def create_simple_relhome_mission(self, items_in, target_system=1, target_component=1):
|
def create_simple_relhome_mission(self, items_in, target_system=1, target_component=1):
|
||||||
|
Loading…
Reference in New Issue
Block a user