Autotest: Modify look_for_wiggle to check for independent servo movement

ArduPilot · Oct 1, 2024 · 847c909 · 847c909
1 parent 0ca3738
commit 847c909
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 20 deletions.
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
@@ -359,28 +359,41 @@ void ModeAuto::wiggle_servos()
         return;
     }
 
-    int16_t servo_value;
-    // move over full range for 2 seconds
+    int16_t servo_valueElevator;
+    int16_t servo_valueAileronRudder;
+    // Wiggle the control surfaces in stages: elevators first, then rudders + ailerons, through the full range over 4 seconds
     if (wiggle.stage != 0) {
-        wiggle.stage += 2;
+        wiggle.stage += 1;
     }
     if (wiggle.stage == 0) {
-        servo_value = 0;
-    } else if (wiggle.stage < 50) {
-        servo_value = wiggle.stage * (4500 / 50);
+        servo_valueElevator = 0;
+        servo_valueAileronRudder = 0;
+    } else if (wiggle.stage < 25) { 
+        servo_valueElevator = wiggle.stage * (4500 / 25);      
+        servo_valueAileronRudder = 0;
+    } else if (wiggle.stage < 75) {
+        servo_valueElevator = (50 - wiggle.stage) * (4500 / 25);        
+        servo_valueAileronRudder = 0;
     } else if (wiggle.stage < 100) {
-        servo_value = (100 - wiggle.stage) * (4500 / 50);        
-    } else if (wiggle.stage < 150) {
-        servo_value = (100 - wiggle.stage) * (4500 / 50);        
+        servo_valueElevator = (wiggle.stage - 100) * (4500 / 25);        
+        servo_valueAileronRudder = 0;
+    } else if (wiggle.stage < 125) {
+        servo_valueElevator = 0;
+        servo_valueAileronRudder = (wiggle.stage - 100) * (4500 / 25);
+    } else if (wiggle.stage < 175) {
+        servo_valueElevator = 0;
+        servo_valueAileronRudder = (150 - wiggle.stage) * (4500 / 25);  
     } else if (wiggle.stage < 200) {
-        servo_value = (wiggle.stage-200) * (4500 / 50);        
+        servo_valueElevator = 0;
+        servo_valueAileronRudder = (wiggle.stage - 200) * (4500 / 25); 
     } else {
         wiggle.stage = 0;
-        servo_value = 0;
+        servo_valueElevator = 0;
+        servo_valueAileronRudder = 0;
     }
-    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_value);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_value);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_value);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, servo_valueAileronRudder);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, servo_valueElevator);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, servo_valueAileronRudder);
 
 }
 

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
@@ -5175,18 +5175,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')
@@ -5206,6 +5211,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):