Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Stage control surface wiggles one after another

This commit is contained in:
Tarik 2024-10-01 01:16:00 -04:00 committed by Andrew Tridgell
parent b73bef5b13
commit 9d58bfb91e
1 changed files with 27 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduPlane/servos.cpp
View File

@ -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);
}