auto define NUM_SERVO_PLUGS based on SERVO PINS

Marlin/src/HAL/AVR/HAL.cpp

@@ -81,6 +81,12 @@ void MarlinHAL::init() {
   #if HAS_SERVO_3
     OUT_WRITE(SERVO3_PIN, LOW);
   #endif
+  #if HAS_SERVO_4
+    OUT_WRITE(SERVO4_PIN, LOW);
+  #endif
+  #if HAS_SERVO_5
+    OUT_WRITE(SERVO5_PIN, LOW);
+  #endif
 
   init_pwm_timers();   // Init user timers to default frequency - 1000HZ
 

Marlin/src/HAL/LPC1768/HAL.cpp

@@ -111,6 +111,12 @@ void MarlinHAL::init() {
   #if HAS_SERVO_3
     INIT_SERVO(3);
   #endif
+  #if HAS_SERVO_4
+    INIT_SERVO(4);
+  #endif
+  #if HAS_SERVO_5
+    INIT_SERVO(5);
+  #endif
 
   //debug_frmwrk_init();
   //_DBG("\n\nDebug running\n");

Marlin/src/gcode/config/M43.cpp

@@ -118,7 +118,7 @@ inline void toggle_pins() {
 
 inline void servo_probe_test() {
 
-  #if !(NUM_SERVOS > 0 && HAS_SERVO_0)
+  #if !HAS_SERVO_0
 
     SERIAL_ERROR_MSG("SERVO not set up.");
 

Marlin/src/inc/Conditionals_post.h

@@ -2762,20 +2762,26 @@
 #endif
 
 // Servos
-#if PIN_EXISTS(SERVO0) && NUM_SERVOS > 0
-  #define HAS_SERVO_0 1
-#endif
-#if PIN_EXISTS(SERVO1) && NUM_SERVOS > 1
-  #define HAS_SERVO_1 1
-#endif
-#if PIN_EXISTS(SERVO2) && NUM_SERVOS > 2
-  #define HAS_SERVO_2 1
-#endif
-#if PIN_EXISTS(SERVO3) && NUM_SERVOS > 3
-  #define HAS_SERVO_3 1
-#endif
 #if NUM_SERVOS > 0
   #define HAS_SERVOS 1
+  #if PIN_EXISTS(SERVO0)
+    #define HAS_SERVO_0 1
+  #endif
+  #if PIN_EXISTS(SERVO1) && NUM_SERVOS > 1
+    #define HAS_SERVO_1 1
+  #endif
+  #if PIN_EXISTS(SERVO2) && NUM_SERVOS > 2
+    #define HAS_SERVO_2 1
+  #endif
+  #if PIN_EXISTS(SERVO3) && NUM_SERVOS > 3
+    #define HAS_SERVO_3 1
+  #endif
+  #if PIN_EXISTS(SERVO4) && NUM_SERVOS > 4
+    #define HAS_SERVO_4 1
+  #endif
+  #if PIN_EXISTS(SERVO5) && NUM_SERVOS > 5
+    #define HAS_SERVO_5 1
+  #endif
   #if defined(PAUSE_SERVO_OUTPUT) && defined(RESUME_SERVO_OUTPUT)
     #define HAS_PAUSE_SERVO_OUTPUT 1
   #endif

Marlin/src/inc/SanityCheck.h

@@ -961,9 +961,7 @@ static_assert(COUNT(arm) == LOGICAL_AXES, "AXIS_RELATIVE_MODES must contain " _L
 /**
  * Limited number of servos
  */
-#if NUM_SERVOS > NUM_SERVO_PLUGS
-  #error "The selected board doesn't support enough servos for your configuration. Reduce NUM_SERVOS."
-#endif
+static_assert(NUM_SERVOS <= NUM_SERVO_PLUGS, "NUM_SERVOS (or some servo index) is too large. The selected board only has " STRINGIFY(NUM_SERVO_PLUGS) " servos.");
 
 /**
  * Servo deactivation depends on servo endstops, switching nozzle, or switching extruder

Marlin/src/module/servo.cpp

@@ -37,22 +37,30 @@ hal_servo_t servo[NUM_SERVOS];
 #endif
 
 void servo_init() {
-  #if NUM_SERVOS >= 1 && HAS_SERVO_0
+  #if HAS_SERVO_0
     servo[0].attach(SERVO0_PIN);
     servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
   #endif
-  #if NUM_SERVOS >= 2 && HAS_SERVO_1
+  #if HAS_SERVO_1
     servo[1].attach(SERVO1_PIN);
     servo[1].detach();
   #endif
-  #if NUM_SERVOS >= 3 && HAS_SERVO_2
+  #if HAS_SERVO_2
     servo[2].attach(SERVO2_PIN);
     servo[2].detach();
   #endif
-  #if NUM_SERVOS >= 4 && HAS_SERVO_3
+  #if HAS_SERVO_3
     servo[3].attach(SERVO3_PIN);
     servo[3].detach();
   #endif
+  #if HAS_SERVO_4
+    servo[4].attach(SERVO4_PIN);
+    servo[4].detach();
+  #endif
+  #if HAS_SERVO_5
+    servo[5].attach(SERVO5_PIN);
+    servo[5].detach();
+  #endif
 }
 
 #endif // HAS_SERVOS

Marlin/src/module/servo.h

@@ -107,6 +107,9 @@
           , { ASRC(3,0), ASRC(3,1) }
           #if NUM_SERVOS > 4
             , { ASRC(4,0), ASRC(4,1) }
+            #if NUM_SERVOS > 5
+              , { ASRC(5,0), ASRC(5,1) }
+            #endif
           #endif
         #endif
       #endif

Marlin/src/module/settings.cpp

@@ -92,12 +92,6 @@
   #include "servo.h"
 #endif
 
-#if HAS_SERVOS && HAS_SERVO_ANGLES
-  #define EEPROM_NUM_SERVOS NUM_SERVOS
-#else
-  #define EEPROM_NUM_SERVOS NUM_SERVO_PLUGS
-#endif
-
 #include "../feature/fwretract.h"
 
 #if ENABLED(POWER_LOSS_RECOVERY)
@@ -318,7 +312,9 @@ typedef struct SettingsDataStruct {
   //
   // SERVO_ANGLES
   //
-  uint16_t servo_angles[EEPROM_NUM_SERVOS][2];          // M281 P L U
+  #if HAS_SERVO_ANGLES
+    uint16_t servo_angles[NUM_SERVOS][2];               // M281 P L U
+  #endif
 
   //
   // Temperature first layer compensation values
@@ -1051,13 +1047,12 @@ void MarlinSettings::postprocess() {
     //
     // Servo Angles
     //
+    #if HAS_SERVO_ANGLES
     {
       _FIELD_TEST(servo_angles);
-      #if !HAS_SERVO_ANGLES
-        uint16_t servo_angles[EEPROM_NUM_SERVOS][2] = { { 0, 0 } };
-      #endif
       EEPROM_WRITE(servo_angles);
     }
+    #endif
 
     //
     // Thermal first layer compensation values
@@ -2082,15 +2077,17 @@ void MarlinSettings::postprocess() {
       //
       // SERVO_ANGLES
       //
+      #if HAS_SERVO_ANGLES
       {
         _FIELD_TEST(servo_angles);
         #if ENABLED(EDITABLE_SERVO_ANGLES)
-          uint16_t (&servo_angles_arr)[EEPROM_NUM_SERVOS][2] = servo_angles;
+          uint16_t (&servo_angles_arr)[NUM_SERVOS][2] = servo_angles;
         #else
-          uint16_t servo_angles_arr[EEPROM_NUM_SERVOS][2];
+          uint16_t servo_angles_arr[NUM_SERVOS][2];
         #endif
         EEPROM_READ(servo_angles_arr);
       }
+      #endif
 
       //
       // Thermal first layer compensation values

Marlin/src/pins/pins_postprocess.h

@@ -487,8 +487,20 @@
   #define SUICIDE_PIN_STATE LOW
 #endif
 
-#ifndef NUM_SERVO_PLUGS
+#if PIN_EXISTS(SERVO5)
+  #define NUM_SERVO_PLUGS 6
+#elif PIN_EXISTS(SERVO4)
+  #define NUM_SERVO_PLUGS 5
+#elif PIN_EXISTS(SERVO3)
   #define NUM_SERVO_PLUGS 4
+#elif PIN_EXISTS(SERVO2)
+  #define NUM_SERVO_PLUGS 3
+#elif PIN_EXISTS(SERVO1)
+  #define NUM_SERVO_PLUGS 2
+#elif PIN_EXISTS(SERVO0)
+  #define NUM_SERVO_PLUGS 1
+#else
+  #define NUM_SERVO_PLUGS 0
 #endif
 
 // Only used within pins files

buildroot/tests/BIGTREE_GTR_V1_0

@@ -33,6 +33,7 @@ exec_test $1 $2 "BigTreeTech GTR | 6 Extruders | Quad Z + Endstops" "$3"
 restore_configs
 opt_set MOTHERBOARD BOARD_BTT_GTR_V1_0 SERIAL_PORT -1 \
         EXTRUDERS 3 TEMP_SENSOR_1 1 TEMP_SENSOR_2 1 \
+        SERVO1_PIN PE9 SERVO2_PIN PE11 \
         SERVO_DELAY '{ 300, 300, 300 }' \
         SWITCHING_TOOLHEAD_X_POS '{ 215, 0 ,0 }' \
         MPC_HEATER_POWER '{ 40.0f, 40.0f, 40.0f }' \