MarlinFirmware · thinkyhead · Feb 26, 2020 · Feb 25, 2020
diff --git a/Marlin/src/feature/bltouch.h b/Marlin/src/feature/bltouch.h
@@ -105,7 +105,4 @@ class BLTouch {
   static void mode_conv_proc(const bool M5V);
 };
 
-// Deploy/stow angles for use by servo.cpp / servo.h
-#define BLTOUCH_ANGLES { BLTOUCH_DEPLOY, BLTOUCH_STOW }
-
 extern BLTouch bltouch;
diff --git a/Marlin/src/gcode/config/M281.cpp b/Marlin/src/gcode/config/M281.cpp
@@ -30,6 +30,12 @@ void GcodeSuite::M281() {
   if (!parser.seenval('P')) return;
   const int servo_index = parser.value_int();
   if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
+    #if ENABLED(BLTOUCH)
+      if (servo_index == Z_PROBE_SERVO_NR) {
+        SERIAL_ERROR_MSG("BLTouch angles can't be changed.");
+        return;
+      }
+    #endif
     bool angle_change = false;
     if (parser.seen('L')) {
       servo_angles[servo_index][0] = parser.value_int();

diff --git a/Marlin/src/inc/Conditionals_LCD.h b/Marlin/src/inc/Conditionals_LCD.h
@@ -502,6 +502,10 @@
  * Set a flag for a servo probe (or BLTouch)
  */
 #define HAS_Z_SERVO_PROBE (defined(Z_PROBE_SERVO_NR) && Z_PROBE_SERVO_NR >= 0)
+#define HAS_SERVO_ANGLES (HAS_Z_SERVO_PROBE || EITHER(SWITCHING_EXTRUDER, SWITCHING_NOZZLE))
+#if !HAS_SERVO_ANGLES
+  #undef EDITABLE_SERVO_ANGLES
+#endif
 
 /**
  * Set flags for enabled probes

diff --git a/Marlin/src/inc/Conditionals_post.h b/Marlin/src/inc/Conditionals_post.h
@@ -1546,16 +1546,6 @@
 #define HAS_SERVO_3 (PIN_EXISTS(SERVO3) && NUM_SERVOS > 3)
 #define HAS_SERVOS  (NUM_SERVOS > 0)
 
-#if HAS_SERVOS && !defined(Z_PROBE_SERVO_NR)
-  #define Z_PROBE_SERVO_NR -1
-#endif
-
-#define HAS_SERVO_ANGLES (EITHER(SWITCHING_EXTRUDER, SWITCHING_NOZZLE) || (HAS_Z_SERVO_PROBE && defined(Z_PROBE_SERVO_NR)))
-
-#if !HAS_SERVO_ANGLES || ENABLED(BLTOUCH)
-  #undef EDITABLE_SERVO_ANGLES
-#endif
-
 // Sensors
 #define HAS_FILAMENT_WIDTH_SENSOR (PIN_EXISTS(FILWIDTH))
 

diff --git a/Marlin/src/module/configuration_store.cpp b/Marlin/src/module/configuration_store.cpp
@@ -2532,7 +2532,7 @@ void MarlinSettings::reset() {
   //
 
   #if ENABLED(EDITABLE_SERVO_ANGLES)
-    COPY(servo_angles, base_servo_angles);
+    COPY(servo_angles, base_servo_angles);  // When not editable only one copy of servo angles exists
   #endif
 
   //
@@ -3082,7 +3082,7 @@ void MarlinSettings::reset() {
             #endif
           #elif ENABLED(SWITCHING_NOZZLE)
             case SWITCHING_NOZZLE_SERVO_NR:
-          #elif (ENABLED(BLTOUCH) && defined(BLTOUCH_ANGLES)) || (defined(Z_SERVO_ANGLES) && defined(Z_PROBE_SERVO_NR))
+          #elif ENABLED(BLTOUCH) || (HAS_Z_SERVO_PROBE && defined(Z_SERVO_ANGLES))
             case Z_PROBE_SERVO_NR:
           #endif
             CONFIG_ECHO_START();

diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h
@@ -31,46 +31,65 @@
 #if HAS_SERVO_ANGLES
 
   #if ENABLED(SWITCHING_EXTRUDER)
-    #ifndef SWITCHING_EXTRUDER_E23_SERVO_NR
-      #define SWITCHING_EXTRUDER_E23_SERVO_NR -1
-    #endif
+    // Switching extruder can have 2 or 4 angles
     #if EXTRUDERS > 3
       #define REQ_ANGLES 4
     #else
       #define REQ_ANGLES 2
     #endif
-    #define SADATA    SWITCHING_EXTRUDER_SERVO_ANGLES
-    #define ASRC(N,E) (SWITCHING_EXTRUDER_SERVO_NR == N ? asrc[E] : SWITCHING_EXTRUDER_E23_SERVO_NR == N ? asrc[E+2] : 0)
-  #elif ENABLED(SWITCHING_NOZZLE)
-    #define SADATA    SWITCHING_NOZZLE_SERVO_ANGLES
-    #define ASRC(N,E) (SWITCHING_NOZZLE_SERVO_NR == N ? asrc[E] : 0)
-  #elif defined(Z_PROBE_SERVO_NR)
-    #define ASRC(N,E) (Z_PROBE_SERVO_NR == N ? asrc[E] : 0)
+    constexpr uint16_t sase[] = SWITCHING_EXTRUDER_SERVO_ANGLES;
+    static_assert(COUNT(sase) == REQ_ANGLES, "SWITCHING_EXTRUDER_SERVO_ANGLES needs " STRINGIFY(REQ_ANGLES) " angles.");
+  #else
+    constexpr uint16_t sase[4] = { 0 };
+  #endif
+
+  #if ENABLED(SWITCHING_NOZZLE)
+    constexpr uint16_t sasn[] = SWITCHING_NOZZLE_SERVO_ANGLES;
+    static_assert(COUNT(sasn) == 2, "SWITCHING_NOZZLE_SERVO_ANGLES needs 2 angles.");
+  #else
+    constexpr uint16_t sasn[2] = { 0 };
+  #endif
+
+  #ifdef Z_PROBE_SERVO_NR
     #if ENABLED(BLTOUCH)
       #include "../feature/bltouch.h"
+      #undef Z_SERVO_ANGLES
+      #define Z_SERVO_ANGLES { BLTOUCH_DEPLOY, BLTOUCH_STOW }
     #endif
-    #ifdef BLTOUCH_ANGLES
-      #define SADATA  BLTOUCH_ANGLES
-    #elif defined(Z_SERVO_ANGLES)
-      #define SADATA  Z_SERVO_ANGLES
-    #else
-      #error "Servo angles are needed!"
-    #endif
+    constexpr uint16_t sazp[] = Z_SERVO_ANGLES;
+    static_assert(COUNT(sazp) == 2, "Z_SERVO_ANGLES needs 2 angles.");
+  #else
+    constexpr uint16_t sazp[2] = { 0 };
   #endif
 
+  #ifndef SWITCHING_EXTRUDER_SERVO_NR
+    #define SWITCHING_EXTRUDER_SERVO_NR -1
+  #endif
+  #ifndef SWITCHING_EXTRUDER_E23_SERVO_NR
+    #define SWITCHING_EXTRUDER_E23_SERVO_NR -1
+  #endif
+  #ifndef SWITCHING_NOZZLE_SERVO_NR
+    #define SWITCHING_NOZZLE_SERVO_NR -1
+  #endif
+  #ifndef Z_PROBE_SERVO_NR
+    #define Z_PROBE_SERVO_NR -1
+  #endif
+
+  #define ASRC(N,I) (                                  \
+      N == SWITCHING_EXTRUDER_SERVO_NR     ? sase[I]   \
+    : N == SWITCHING_EXTRUDER_E23_SERVO_NR ? sase[I+2] \
+    : N == SWITCHING_NOZZLE_SERVO_NR       ? sasn[I]   \
+    : N == Z_PROBE_SERVO_NR                ? sazp[I]   \
+    : 0                                                )
+
   #if ENABLED(EDITABLE_SERVO_ANGLES)
     extern uint16_t servo_angles[NUM_SERVOS][2];
-    #define BASE_SERVO_ANGLES base_servo_angles
+    #define CONST_SERVO_ANGLES base_servo_angles
   #else
-    #define BASE_SERVO_ANGLES servo_angles
-  #endif
-
-  constexpr uint16_t asrc[] = SADATA;
-  #if REQ_ANGLES
-    static_assert(COUNT(asrc) == REQ_ANGLES, "SWITCHING_EXTRUDER_SERVO_ANGLES needs " STRINGIFY(REQ_ANGLES) " angles.");
+    #define CONST_SERVO_ANGLES servo_angles
   #endif
 
-  constexpr uint16_t BASE_SERVO_ANGLES [NUM_SERVOS][2] = {
+  constexpr uint16_t CONST_SERVO_ANGLES [NUM_SERVOS][2] = {
       { ASRC(0,0), ASRC(0,1) }
     #if NUM_SERVOS > 1
       , { ASRC(1,0), ASRC(1,1) }