Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Plane: Stage servo wiggles one after another #27275

Draft
wants to merge 2 commits into
base: master
Choose a base branch
from
+42 −20
Draft

Plane: Stage servo wiggles one after another #27275

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e699cac
Plane: Stage control surface wiggles one after another
ohitstarik Oct 1, 2024
be16750
Autotest: Modify look_for_wiggle to test for individual servo movements
ohitstarik Oct 1, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 27 additions & 14 deletions ArduPlane/servos.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

}

Expand Down
21 changes: 15 additions & 6 deletions Tools/autotest/arduplane.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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')
Expand All @@ -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):
Expand Down
Loading