PFW-1386 M701, M702: Move Z-axis using relative position

* Created a new function raise_z() which will move the Z axis by some
X millimeters. If plan = false, the function will return the
actual travel distance since the move is blocking.
* raise_z_above() is refactored to call raise_z()

Now the M701 and M702 will move the Z-axis relatively, and also
when done, revert the Z axis position when done.
This is a similar behavior as in Marlin 2.

Firmware/Marlin.h

@@ -475,6 +475,7 @@ void M600_wait_for_user(float HotendTempBckp);
 void M600_check_state(float nozzle_temp);
 void load_filament_final_feed();
 void marlin_wait_for_click();
+float raise_z(float delta, bool plan = true);
 void raise_z_above(float target, bool plan=true);
 
 extern "C" void softReset();

Firmware/Marlin_main.cpp

@@ -2088,16 +2088,14 @@ bool check_commands() {
 	
 }
 
-
+/// @brief Safely move Z-axis by distance delta (mm)
-// raise_z_above: slowly raise Z to the requested height
+/// @param delta travel distance in mm
-//
+/// @param plan plan the move if the axis is homed (non-blocking)
-// contrarily to a simple move, this function will carefully plan a move
+/// @returns The actual travel distance in mm. Endstop may limit the requested move. Note that
-// when the current Z position is unknown. In such cases, stallguard is
+/// when plan = true and the printer is homed, the function returns 0.
-// enabled and will prevent prolonged pushing against the Z tops
+float raise_z(float delta, bool plan)
-void raise_z_above(float target, bool plan)
 {
-    if (current_position[Z_AXIS] >= target)
+    float travel_z = current_position[Z_AXIS];
-        return;
 
     // Z needs raising
     current_position[Z_AXIS] = target;
@@ -2113,7 +2111,7 @@ void raise_z_above(float target, bool plan)
     {
         // current position is known or very low, it's safe to raise Z
         if(plan) plan_buffer_line_curposXYZE(max_feedrate[Z_AXIS]);
-        return;
+        return 0;
     }
 
     // ensure Z is powered in normal mode to overcome initial load
@@ -2127,6 +2125,9 @@ void raise_z_above(float target, bool plan)
 #endif //TMC2130
     plan_buffer_line_curposXYZE(homing_feedrate[Z_AXIS] / 60);
     st_synchronize();
+
+    // Get the final travel distance
+    travel_z = st_get_position_mm(Z_AXIS) - travel_z;
 #ifdef TMC2130
     if (endstop_z_hit_on_purpose())
     {
@@ -2137,6 +2138,21 @@ void raise_z_above(float target, bool plan)
     tmc2130_home_exit();
 #endif //TMC2130
     enable_z_endstop(z_endstop_enabled);
+    return travel_z;
+}
+
+// raise_z_above: slowly raise Z to the requested height
+//
+// contrarily to a simple move, this function will carefully plan a move
+// when the current Z position is unknown. In such cases, stallguard is
+// enabled and will prevent prolonged pushing against the Z tops
+void raise_z_above(float target, bool plan)
+{
+    if (current_position[Z_AXIS] >= target)
+        return;
+
+    // Use absolute value in case the current position is unknown
+    raise_z(fabs(current_position[Z_AXIS] - target), plan);
 }
 
 
@@ -8593,20 +8609,19 @@ Sigma_Exit:
             }
         }
 
-        if (code_seen('L'))
+        if (code_seen('L')) fastLoadLength = code_value();
-        {
-            fastLoadLength = code_value();
-        }
 
-        if (code_seen('Z'))
+        // Z lift. For safety only allow positive values
-        {
+        if (code_seen('Z')) z_target = fabs(code_value());
-            z_target = code_value();
-        }
 
         // Raise the Z axis
-        raise_z_above(z_target, false);
+        float delta = raise_z(z_target, false);
 
+        // Load filament
         gcode_M701(fastLoadLength, mmuSlotIndex);
+
+        // Restore Z axis
+        raise_z(-delta);
     }
     break;
 
@@ -8624,23 +8639,20 @@ Sigma_Exit:
     {
         float z_target = MIN_Z_FOR_UNLOAD;
         float unloadLength = FILAMENTCHANGE_FINALRETRACT;
-        if (code_seen('U')) {
+        if (code_seen('U')) unloadLength = code_value();
-            unloadLength = code_value();
-        }
 
-        if (code_seen('Z'))
+        // For safety only allow positive values
-        {
+        if (code_seen('Z')) z_target = fabs(code_value());
-            z_target = code_value();
-        }
 
         // Raise the Z axis
-        raise_z_above(z_target, false);
+        float delta = raise_z(z_target, false);
 
-        if (MMU2::mmu2.Enabled()) {
+        // Unload filament
-            MMU2::mmu2.unload();
+        if (MMU2::mmu2.Enabled())  MMU2::mmu2.unload();
-        } else {
+        else unload_filament(unloadLength, false);
-            unload_filament(unloadLength, false);
+
-        }
+        // Restore Z axis
+        raise_z(-delta);
     }
     break;