From e699cacd6f97fb9f1bb9469c07457ee8df1decca Mon Sep 17 00:00:00 2001 From: Tarik Date: Tue, 1 Oct 2024 01:16:00 -0400 Subject: [PATCH 1/2] Plane: Stage control surface wiggles one after another --- ArduPlane/servos.cpp | 41 +++++++++++++++++++++++++++-------------- 1 file changed, 27 insertions(+), 14 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 496255a75a54c..9ef0353f29fd8 100644 --- 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); } From be16750656e51e0d0fefdc0fc52227f3cf11f84a Mon Sep 17 00:00:00 2001 From: Tarik Date: Tue, 1 Oct 2024 02:25:35 -0400 Subject: [PATCH 2/2] 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 179cf99cb0f0f..c8bcccb24735a 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -5845,18 +5845,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') @@ -5876,6 +5881,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):