Raise when not retracting

Code to raise the probe before retraction is good, but the code should
always raise the probe, not just when retracting

Marlin/Marlin_main.cpp

@@ -1254,17 +1254,17 @@ inline void sync_plan_position() {
 
   }
 
-  static void retract_z_probe(const float z_after=Z_RAISE_AFTER_PROBING) {
+  static void retract_z_probe() {
 
     #ifdef SERVO_ENDSTOPS
 
       // Retract Z Servo endstop if enabled
       if (servo_endstops[Z_AXIS] >= 0) {
 
-        if (z_after > 0) {
+        #if Z_RAISE_AFTER_PROBING > 0
-          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_after);
+          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING);
           st_synchronize();
-        }
+        #endif
 
         #if SERVO_LEVELING
           servos[servo_endstops[Z_AXIS]].attach(0);
@@ -1343,8 +1343,13 @@ inline void sync_plan_position() {
     run_z_probe();
     float measured_z = current_position[Z_AXIS];
 
+    #if Z_RAISE_AFTER_PROBING > 0
+      do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_AFTER_PROBING);
+      st_synchronize();
+    #endif
+
     #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
-      if (retract_action & ProbeRetract) retract_z_probe(z_before);
+      if (retract_action & ProbeRetract) retract_z_probe();
     #endif
 
     if (verbose_level > 2) {