From 5ea787a46d53bf8a73fd36f4c79c91576bd8a303 Mon Sep 17 00:00:00 2001 From: Tarik Date: Tue, 1 Oct 2024 02:25:35 -0400 Subject: [PATCH] Autotest: Modify look_for_wiggle to test for individual servo movements --- Tools/autotest/arduplane.py | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 10324ad08b421..cf8ed466a395d 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -5847,18 +5847,23 @@ def MAV_CMD_NAV_ALTITUDE_WAIT(self): ) ]) + # Set initial conditions for servo wiggle testing + servo_wiggled = {1: False, 2: False, 4: False} + def look_for_wiggle(mav, m): if m.get_type() == 'SERVO_OUTPUT_RAW': # Throttle must be zero if m.servo3_raw != 1000: raise NotAchievedException( "Throttle must be 0 in altitude wait, got %f" % m.servo3_raw) - # Aileron, elevator and rudder must all be the same - # However, aileron is revered, so we must un-reverse it - value = 1500 - (m.servo1_raw - 1500) - if (m.servo2_raw != value) or (m.servo4_raw != value): - raise NotAchievedException( - "Aileron, elevator and rudder must be the same") + + # Check if all servos wiggle + if m.servo1_raw != 1500: + servo_wiggled[1] = True + if m.servo2_raw != 1500: + servo_wiggled[2] = True + if m.servo4_raw != 1500: + servo_wiggled[4] = True # Start mission self.change_mode('AUTO') @@ -5878,6 +5883,10 @@ def look_for_wiggle(mav, m): if not self.mode_is('AUTO'): raise NotAchievedException("Must still be in AUTO") + # Raise error if not all servos have wiggled + if not all(servo_wiggled.values()): + raise NotAchievedException("Not all servos have moved within the test frame") + self.disarm_vehicle() def start_flying_simple_rehome_mission(self, items):