From 214b6a92208710fe5ce367dcdb012fe6d96bf460 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Gu=C3=B0ni=20M=C3=A1r=20Gilbert?= <gudni.m.g@gmail.com>
Date: Sun, 28 Aug 2022 12:30:34 +0000
Subject: [PATCH] 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        |  1 +
 Firmware/Marlin_main.cpp | 76 +++++++++++++++++++++++-----------------
 2 files changed, 45 insertions(+), 32 deletions(-)

diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 4e8246c3e..de7d7a99a 100755
--- a/Firmware/Marlin.h
+++ b/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();
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index f1cca9457..6af4475c3 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -2088,16 +2088,14 @@ bool check_commands() {
 	
 }
 
-
-// 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)
+/// @brief Safely move Z-axis by distance delta (mm)
+/// @param delta travel distance in mm
+/// @param plan plan the move if the axis is homed (non-blocking)
+/// @returns The actual travel distance in mm. Endstop may limit the requested move. Note that
+/// when plan = true and the printer is homed, the function returns 0.
+float raise_z(float delta, bool plan)
 {
-    if (current_position[Z_AXIS] >= target)
-        return;
+    float travel_z = current_position[Z_AXIS];
 
     // 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'))
-        {
-            fastLoadLength = code_value();
-        }
+        if (code_seen('L')) fastLoadLength = code_value();
 
-        if (code_seen('Z'))
-        {
-            z_target = code_value();
-        }
+        // Z lift. For safety only allow positive values
+        if (code_seen('Z')) z_target = fabs(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')) {
-            unloadLength = code_value();
-        }
+        if (code_seen('U')) unloadLength = code_value();
 
-        if (code_seen('Z'))
-        {
-            z_target = code_value();
-        }
+        // For safety only allow positive values
+        if (code_seen('Z')) z_target = fabs(code_value());
 
         // Raise the Z axis
-        raise_z_above(z_target, false);
+        float delta = raise_z(z_target, false);
 
-        if (MMU2::mmu2.Enabled()) {
-            MMU2::mmu2.unload();
-        } else {
-            unload_filament(unloadLength, false);
-        }
+        // Unload filament
+        if (MMU2::mmu2.Enabled())  MMU2::mmu2.unload();
+        else unload_filament(unloadLength, false);
+
+        // Restore Z axis
+        raise_z(-delta);
     }
     break;