Plane: Stage servo wiggles one after another

[ohitstarik]

This PR implements the fix proposed in issue #25851

The current code to wiggle the servos in mission waypoint option "Altitude_wait" currently does not wiggle left elevon, and none of the VTAIL Servos. This is due to the output mixers summing out to zero from the respective roll/pitch/yaw commands.

This fix stages the wiggles one after the other so nothing sums out to zero.

I waited to submit this PR until i got around to testing it at least with SITL.

Here's the test: https://www.youtube.com/watch?v=ryS6fTMAQS0

[IamPete1]

Looks like we need to update the auto test to check for the new behavior:

2024-06-08T16:43:29.2159751Z >>>> FAILED STEP: test.Plane at Sat Jun  8 16:43:29 2024
2024-06-08T16:43:29.2160143Z Failure Summary:
2024-06-08T16:43:29.2160401Z   test.Plane:
2024-06-08T16:43:29.2161560Z     MAV_CMD_NAV_ALTITUDE_WAIT (test MAV_CMD_NAV_ALTITUDE_WAIT mission item, wiggling only) (Aileron, elevator and rudder must be the same) (see /tmp/buildlogs/ArduPlane-MAV_CMD_NAV_ALTITUDE_WAIT.txt) (duration 0.2692389488220215 s)

[tridge]

good to merge once autotest fixed

[DjMixMasterDragon]

Just ran into this problem and stumbled upon issue. With having a faster loop rate you could scale this calculation dynamically such the movement over total time (4 seconds) integrity is maintained.

const int SCHED_LOOP_RATE = 200;  // Example: Set to desired loop rate like 50Hz, 100Hz, etc.
const int MIN_LOOP_RATE   = 50;   // Minimum allowable Loop Rate
// Calculate scaling factor based on the loop rate
const int SCALE_FACTOR = (SCHED_LOOP_RATE / MIN_LOOP_RATE);
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 += 1;
}

if (wiggle.stage == 0) {
    servo_valueElevator = 0;
    servo_valueAileronRudder = 0;
} else if (wiggle.stage < (25 * SCALE_FACTOR)) { 
    servo_valueElevator = wiggle.stage * (4500 / (25 * SCALE_FACTOR));
    servo_valueAileronRudder = 0;
} else if (wiggle.stage < (75 * SCALE_FACTOR)) {
    servo_valueElevator = ((50 * SCALE_FACTOR) - wiggle.stage) * (4500 / (25 * SCALE_FACTOR));
    servo_valueAileronRudder = 0;
} else if (wiggle.stage < (100 * SCALE_FACTOR)) {
    servo_valueElevator = (wiggle.stage - (100 * SCALE_FACTOR)) * (4500 / (25 * SCALE_FACTOR));
    servo_valueAileronRudder = 0;
} else if (wiggle.stage < (125 * SCALE_FACTOR)) {
    servo_valueElevator = 0;
    servo_valueAileronRudder = (wiggle.stage - (100 * SCALE_FACTOR)) * (4500 / (25 * SCALE_FACTOR));
} else if (wiggle.stage < (175 * SCALE_FACTOR)) {
    servo_valueElevator = 0;
    servo_valueAileronRudder = ((150 * SCALE_FACTOR) - wiggle.stage) * (4500 / (25 * SCALE_FACTOR));
} else if (wiggle.stage < (200 * SCALE_FACTOR)) {
    servo_valueElevator = 0;
    servo_valueAileronRudder = (wiggle.stage - (200 * SCALE_FACTOR)) * (4500 / (25 * SCALE_FACTOR));
} else {
    wiggle.stage = 0;
    servo_valueElevator = 0;
    servo_valueAileronRudder = 0;
}
// Set servo outputs
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);

}

SCHED_LOOP_RATE is hard-coded in above example but to further proof you could make the call to the scheduler like other parts of software already do: scheduler.get_loop_rate_hz() to retrieve...

[ohitstarik]

@IamPete1 id like to help with updating the autotest if possible - from what i recall it was testing to see if all control surfaces were moving at the same time (we're staging things one after the other intentionally now). Do you have any specific spots in the code i can look at off the top of your head to help me get started?

[IamPete1]

@IamPete1 id like to help with updating the autotest if possible - from what i recall it was testing to see if all control surfaces were moving at the same time (we're staging things one after the other intentionally now). Do you have any specific spots in the code i can look at off the top of your head to help me get started?

The check is here:

ardupilot/Tools/autotest/arduplane.py

Lines 5771 to 5782 in f2f1ac3

	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")

[peterbarker]

Ping @ohitstarik - still chasing this one?

Note that this code may have changed - we've merged https://github.com/ArduPilot/ardupilot/pull/27985/files which does servo-wiggling

[ohitstarik]

hey @peterbarker , yes i just let this slip by. that's a good feature added.

I'll look through the code - it doesnt seem immediately effecting of the changes here. Appreciate the heads up.

Just to make sure, are you saying that the problem was fixed in that PR or just that the wiggle mechanism may have changed + might effect this PR?

[IamPete1]

Just to make sure, are you saying that the problem was fixed in that PR or just that the wiggle mechanism may have changed + might effect this PR?

I don't think it has been fixed.

From my point of view the C++ here is good to merge, we just need to fix the auto test.

[ohitstarik]

Hey @IamPete1. Updated the Autotest. Let me know how this looks and if any modifications are needed.

Hold on let me get these to pass checks first