From e699cacd6f97fb9f1bb9469c07457ee8df1decca Mon Sep 17 00:00:00 2001 From: Tarik Date: Tue, 1 Oct 2024 01:16:00 -0400 Subject: [PATCH] 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); }