From 688e40a5ae3985d16ecd30befbfc863596877668 Mon Sep 17 00:00:00 2001
From: Mathias Gyldenberg <76220576+MathiasGyldenberg@users.noreply.github.com>
Date: Wed, 10 Sep 2025 19:28:36 +0200
Subject: [PATCH] Refactor Z axis full-step movement to G88 command

Moved Z axis full-step positioning logic from M88 to G88 command. Extracted movement code into a new move_z_to_next_fullstep() function, updated references in power_panic and LCD menu, and removed the old M88 implementation.
---
 Firmware/Marlin_main.cpp | 26 +++++---------------------
 Firmware/power_panic.cpp | 12 +-----------
 Firmware/stepper.cpp     | 16 ++++++++++++++++
 Firmware/stepper.h       |  2 ++
 Firmware/ultralcd.cpp    |  2 +-
 5 files changed, 25 insertions(+), 33 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 4342dd6bc..226efd84f 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -4606,15 +4606,15 @@ void process_commands()
             calibration_status_set(CALIBRATION_STATUS_LIVE_ADJUST);
             break;
 
-        /*!
-        ### G88 - Reserved <a href="https://reprap.org/wiki/G-code#G88:_Reserved">G88: Reserved</a>
 
-        Currently has no effect.
+        /*!
+        ### G88 - Move Z axis to next full-step position
         */
 
-        // Prusa3D specific: Don't know what it is for, it is in V2Calibration.gcode
-
 		    case 88:
+            if (axis_known_position[Z_AXIS]) {
+                move_z_to_next_fullstep();
+            }
 			      break;
 
 
@@ -5707,22 +5707,6 @@ void process_commands()
 	  break;
 #endif
 
-/*!
-    ### M88 - Move Z axis to a full-step position
-    */
-#ifdef TMC2130
-    case 88:
-      if (axis_known_position[Z_AXIS]) {
-        float target_z = current_position[Z_AXIS] + float(1024 - tmc2130_rd_MSCNT(Z_AXIS)) / (tmc2130_get_res(Z_AXIS) * cs.axis_steps_per_mm[Z_AXIS]) + 0.16f;
-        if (target_z <= max_pos[Z_AXIS]) {
-          current_position[Z_AXIS] = target_z;
-          plan_buffer_line_curposXYZE(homing_feedrate[Z_AXIS]/60);
-          st_synchronize();
-        }
-      }
-    break;
-#endif
-
     /*!
     ### M92 - Set Axis steps-per-unit <a href="https://reprap.org/wiki/G-code#M92:_Set_axis_steps_per_unit">M92: Set axis_steps_per_unit</a>
     Allows programming of steps per unit (usually mm) for motor drives. These values are reset to firmware defaults on power on, unless saved to EEPROM if available (M500 in Marlin)
diff --git a/Firmware/power_panic.cpp b/Firmware/power_panic.cpp
index 55c790cf4..fb9743bf7 100644
--- a/Firmware/power_panic.cpp
+++ b/Firmware/power_panic.cpp
@@ -135,17 +135,7 @@ void uvlo_() {
     st_synchronize();
     disable_e0();
 
-    // Read out the current Z motor microstep counter to move the axis up towards
-    // a full step before powering off. NOTE: we need to ensure to schedule more
-    // than "dropsegments" steps in order to move (this is always the case here
-    // due to UVLO_Z_AXIS_SHIFT being used)
-    uint16_t z_res = tmc2130_get_res(Z_AXIS);
-    uint16_t z_microsteps = tmc2130_rd_MSCNT(Z_AXIS);
-    current_position[Z_AXIS] += float(1024 - z_microsteps)
-                                / (z_res * cs.axis_steps_per_mm[Z_AXIS])
-                                + UVLO_Z_AXIS_SHIFT;
-    plan_buffer_line_curposXYZE(homing_feedrate[Z_AXIS]/60);
-    st_synchronize();
+    move_z_to_next_fullstep();
     poweroff_z();
 
     // Write the file position.
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 16fad21fc..65ad958e9 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -250,6 +250,22 @@ void invert_z_endstop(bool endstop_invert)
   z_endstop_invert = endstop_invert;
 }
 
+void move_z_to_next_fullstep() {
+  #ifdef TMC2130
+	  // Read out the current Z motor microstep counter to move the axis up towards
+    // a full step before powering off. NOTE: we need to ensure to schedule more
+    // than "dropsegments" steps in order to move (this is always the case here
+    // due to UVLO_Z_AXIS_SHIFT being used)
+    uint16_t z_res = tmc2130_get_res(Z_AXIS);
+    uint16_t z_microsteps = tmc2130_rd_MSCNT(Z_AXIS);
+    current_position[Z_AXIS] += float(1024 - z_microsteps)
+                                / (z_res * cs.axis_steps_per_mm[Z_AXIS])
+                                + UVLO_Z_AXIS_SHIFT;
+    plan_buffer_line_curposXYZE(homing_feedrate[Z_AXIS]/60);
+    st_synchronize();
+  #endif
+}
+
 //         __________________________
 //        /|                        |\     _________________         ^
 //       / |                        | \   /|               |\        |
diff --git a/Firmware/stepper.h b/Firmware/stepper.h
index c5c7cb6d3..dfc8bcd4c 100644
--- a/Firmware/stepper.h
+++ b/Firmware/stepper.h
@@ -67,6 +67,8 @@ bool enable_endstops(bool check); // Enable/disable endstop checking. Return the
 bool enable_z_endstop(bool check);
 void invert_z_endstop(bool endstop_invert);
 
+void move_z_to_next_fullstep(); //Move Z axis up to the next fullstep
+
 void checkStepperErrors(); //Print errors detected by the stepper
 
 extern block_t *current_block;  // A pointer to the block currently being traced
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 0f801cc60..7dde78b1d 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -4526,7 +4526,7 @@ static void lcd_calibration_menu()
     }
     MENU_ITEM_GCODE_P(_T(MSG_AUTO_HOME), G28W);
     if (axis_known_position[Z_AXIS]) {
-        MENU_ITEM_GCODE_P(_T(MSG_FULLSTEP_Z), PSTR("M88"));
+        MENU_ITEM_GCODE_P(_T(MSG_FULLSTEP_Z), PSTR("G88"));
     }
 #ifdef TMC2130
     MENU_ITEM_FUNCTION_P(_T(MSG_BELTTEST), lcd_belttest_v);