Plane: NAV_ALTITUDE_WAIT to set servos of all control surfaces

magicrub · Apr 23, 2024 · 6f7c442 · 6f7c442
1 parent ef78da2
commit 6f7c442
Showing 1 changed file with 10 additions and 3 deletions.
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
@@ -378,10 +378,17 @@ void ModeAuto::wiggle_servos()
         wiggle.stage = 0;
         servo_value = 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);
 
+    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS ; i++) {
+        const SRV_Channel *chan = SRV_Channels::srv_channel(i);
+        if (chan == nullptr) {
+            continue;
+        }
+        const SRV_Channel::Aux_servo_function_t func = chan->get_function();
+        if (SRV_Channel::is_control_surface(func)) {
+            SRV_Channels::set_output_scaled(func, servo_value);
+        }
+    }
 }