From 13599a73c7d2c356b20695e2e1556952b2f5d3cf Mon Sep 17 00:00:00 2001
From: Brian <bgort@users.noreply.github.com>
Date: Wed, 17 May 2017 19:03:00 -0400
Subject: [PATCH 1/2] Add `G7` gcode command to move between UBL mesh points -
 can be augmented in the future to enable for other leveling systems

Quite simple, but did not want to modify `G1` as the additional checking would slow it down.

Tested & working.
---
 Marlin/Marlin_main.cpp | 38 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 38 insertions(+)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 352c01eb61..1d29853667 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -3395,6 +3395,34 @@ inline void gcode_G4() {
 
 #endif // BEZIER_CURVE_SUPPORT
 
+#if ENABLED(AUTO_BED_LEVELING_UBL) //todo:  enable for other leveling systems?
+/**
+ * G7: Move X & Y axes to mesh coordinates
+ */
+inline void gcode_G7(
+  #if IS_SCARA
+    bool fast_move=false
+  #endif
+) {
+  if (IsRunning()) {
+    destination[X_AXIS] = code_seen('I') ? pgm_read_float(&ubl.mesh_index_to_xpos[code_has_value() ? code_value_int() : 0]) : current_position[X_AXIS];
+    destination[Y_AXIS] = code_seen('J') ? pgm_read_float(&ubl.mesh_index_to_ypos[code_has_value() ? code_value_int() : 0]) : current_position[Y_AXIS];
+    destination[Z_AXIS] = current_position[Z_AXIS]; //todo: perhaps add Z-move support?
+    destination[E_AXIS] = current_position[E_AXIS];
+
+    if (code_seen('F') && code_value_linear_units() > 0.0)
+      feedrate_mm_s = MMM_TO_MMS(code_value_linear_units());
+
+    #if IS_SCARA
+      fast_move ? prepare_uninterpolated_move_to_destination() : prepare_move_to_destination();
+    #else
+      prepare_move_to_destination();
+    #endif
+  }
+}
+#endif
+
+
 #if ENABLED(FWRETRACT)
 
   /**
@@ -9982,6 +10010,16 @@ void process_next_command() {
           break;
       #endif // BEZIER_CURVE_SUPPORT
 
+      #if ENABLED(AUTO_BED_LEVELING_UBL)
+        case 7:
+          #if IS_SCARA
+            gcode_G7(codenum == 0);
+          #else
+            gcode_G7();
+            #endif
+          break;
+      #endif
+
       #if ENABLED(FWRETRACT)
         case 10: // G10: retract
         case 11: // G11: retract_recover

From e09b4ce4a5a552eb93c509ff38d3e6d475296b34 Mon Sep 17 00:00:00 2001
From: Brian <bgort@users.noreply.github.com>
Date: Wed, 17 May 2017 19:46:16 -0400
Subject: [PATCH 2/2] Add checking

---
 Marlin/Marlin_main.cpp | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 1d29853667..34b1566160 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -51,6 +51,7 @@
  * G3  - CCW ARC
  * G4  - Dwell S<seconds> or P<milliseconds>
  * G5  - Cubic B-spline with XYZE destination and IJPQ offsets
+ * G7  - Coordinated move between UBL mesh points (I & J)
  * G10 - Retract filament according to settings of M207
  * G11 - Retract recover filament according to settings of M208
  * G12 - Clean tool
@@ -3405,8 +3406,18 @@ inline void gcode_G7(
   #endif
 ) {
   if (IsRunning()) {
-    destination[X_AXIS] = code_seen('I') ? pgm_read_float(&ubl.mesh_index_to_xpos[code_has_value() ? code_value_int() : 0]) : current_position[X_AXIS];
-    destination[Y_AXIS] = code_seen('J') ? pgm_read_float(&ubl.mesh_index_to_ypos[code_has_value() ? code_value_int() : 0]) : current_position[Y_AXIS];
+    const bool hasI = code_seen('I');
+    const int8_t ix = code_has_value() ? code_value_int() : 0;
+    const bool hasJ = code_seen('J');
+    const int8_t iy = code_has_value() ? code_value_int() : 0;
+
+    if ((hasI && !WITHIN(ix, 0, GRID_MAX_POINTS_X - 1)) || (hasJ && !WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1))) {
+      SERIAL_ECHOLNPGM(MSG_ERR_MESH_XY);
+      return;
+    }
+
+    destination[X_AXIS] = hasI ? pgm_read_float(&ubl.mesh_index_to_xpos[ix]) : current_position[X_AXIS];
+    destination[Y_AXIS] = hasJ ? pgm_read_float(&ubl.mesh_index_to_ypos[iy]) : current_position[Y_AXIS];
     destination[Z_AXIS] = current_position[Z_AXIS]; //todo: perhaps add Z-move support?
     destination[E_AXIS] = current_position[E_AXIS];