From 0c0250750ba268a3b0ac1cf0f463089f19a8b65c Mon Sep 17 00:00:00 2001
From: enricoturri1966 <enricoturri@seznam.cz>
Date: Tue, 1 Feb 2022 08:07:53 +0100
Subject: [PATCH] #7843 - Added processing of gcode lines G60 and G61 for
 Marlin firmwares

---
 src/libslic3r/GCode/GCodeProcessor.cpp | 45 ++++++++++++++++++++++++++
 src/libslic3r/GCode/GCodeProcessor.hpp |  7 ++++
 2 files changed, 52 insertions(+)

diff --git a/src/libslic3r/GCode/GCodeProcessor.cpp b/src/libslic3r/GCode/GCodeProcessor.cpp
index f60ad896b3..995964eb5f 100644
--- a/src/libslic3r/GCode/GCodeProcessor.cpp
+++ b/src/libslic3r/GCode/GCodeProcessor.cpp
@@ -1188,6 +1188,7 @@ void GCodeProcessor::reset()
 
     m_start_position = { 0.0f, 0.0f, 0.0f, 0.0f };
     m_end_position = { 0.0f, 0.0f, 0.0f, 0.0f };
+    m_saved_position = { 0.0f, 0.0f, 0.0f, 0.0f };
     m_origin = { 0.0f, 0.0f, 0.0f, 0.0f };
     m_cached_position.reset();
     m_wiping = false;
@@ -1616,6 +1617,13 @@ void GCodeProcessor::process_gcode_line(const GCodeReader::GCodeLine& line, bool
                     default: break;
                     }
                     break;
+                case '6':
+                    switch (cmd[2]) {
+                    case '0': { process_G60(line); break; } // Save Current Position
+                    case '1': { process_G61(line); break; } // Return to Saved Position
+                    default: break;
+                    }
+                    break;
                 case '9':
                     switch (cmd[2]) {
                     case '0': { process_G90(line); break; } // Set to Absolute Positioning
@@ -2828,6 +2836,43 @@ void GCodeProcessor::process_G28(const GCodeReader::GCodeLine& line)
     process_G1(new_gline);
 }
 
+void GCodeProcessor::process_G60(const GCodeReader::GCodeLine& line)
+{
+    if (m_flavor == gcfMarlinLegacy || m_flavor == gcfMarlinFirmware)
+        m_saved_position = m_end_position;
+}
+
+void GCodeProcessor::process_G61(const GCodeReader::GCodeLine& line)
+{
+    if (m_flavor == gcfMarlinLegacy || m_flavor == gcfMarlinFirmware) {
+        bool modified = false;
+        if (line.has_x()) {
+            m_end_position[X] = m_saved_position[X];
+            modified = true;
+        }
+        if (line.has_y()) {
+            m_end_position[Y] = m_saved_position[Y];
+            modified = true;
+        }
+        if (line.has_z()) {
+            m_end_position[Z] = m_saved_position[Z];
+            modified = true;
+        }
+        if (line.has_e()) {
+            m_end_position[E] = m_saved_position[E];
+            modified = true;
+        }
+        if (line.has_f())
+            m_feedrate = line.f();
+
+        if (!modified)
+            m_end_position = m_saved_position;
+
+
+        store_move_vertex(EMoveType::Travel);
+    }
+}
+
 void GCodeProcessor::process_G90(const GCodeReader::GCodeLine& line)
 {
     m_global_positioning_type = EPositioningType::Absolute;
diff --git a/src/libslic3r/GCode/GCodeProcessor.hpp b/src/libslic3r/GCode/GCodeProcessor.hpp
index 2f3ee4e5d6..153f4a9c5c 100644
--- a/src/libslic3r/GCode/GCodeProcessor.hpp
+++ b/src/libslic3r/GCode/GCodeProcessor.hpp
@@ -517,6 +517,7 @@ namespace Slic3r {
 
         AxisCoords m_start_position; // mm
         AxisCoords m_end_position; // mm
+        AxisCoords m_saved_position; // mm
         AxisCoords m_origin; // mm
         CachedPosition m_cached_position;
         bool m_wiping;
@@ -661,6 +662,12 @@ namespace Slic3r {
         // Move to origin
         void process_G28(const GCodeReader::GCodeLine& line);
 
+        // Save Current Position
+        void process_G60(const GCodeReader::GCodeLine& line);
+
+        // Return to Saved Position
+        void process_G61(const GCodeReader::GCodeLine& line);
+ 
         // Set to Absolute Positioning
         void process_G90(const GCodeReader::GCodeLine& line);