Add gcode.cpp, motion.*, queue.* - Apply to some G-codes.

Marlin/src/feature/mbl/mesh_bed_leveling.cpp

@@ -26,6 +26,8 @@
 
   #include "mesh_bed_leveling.h"
 
+  #include "../../module/motion.h"
+
   mesh_bed_leveling mbl;
 
   uint8_t mesh_bed_leveling::status;
@@ -49,4 +51,63 @@
     ZERO(z_values);
   }
 
+  /**
+   * Prepare a mesh-leveled linear move in a Cartesian setup,
+   * splitting the move where it crosses mesh borders.
+   */
+  void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits, uint8_t y_splits) {
+    int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X)),
+        cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y)),
+        cx2 = mbl.cell_index_x(RAW_X_POSITION(destination[X_AXIS])),
+        cy2 = mbl.cell_index_y(RAW_Y_POSITION(destination[Y_AXIS]));
+    NOMORE(cx1, GRID_MAX_POINTS_X - 2);
+    NOMORE(cy1, GRID_MAX_POINTS_Y - 2);
+    NOMORE(cx2, GRID_MAX_POINTS_X - 2);
+    NOMORE(cy2, GRID_MAX_POINTS_Y - 2);
+
+    if (cx1 == cx2 && cy1 == cy2) {
+      // Start and end on same mesh square
+      line_to_destination(fr_mm_s);
+      set_current_to_destination();
+      return;
+    }
+
+    #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
+
+    float normalized_dist, end[XYZE];
+
+    // Split at the left/front border of the right/top square
+    const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+    if (cx2 != cx1 && TEST(x_splits, gcx)) {
+      COPY(end, destination);
+      destination[X_AXIS] = LOGICAL_X_POSITION(mbl.index_to_xpos[gcx]);
+      normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
+      destination[Y_AXIS] = MBL_SEGMENT_END(Y);
+      CBI(x_splits, gcx);
+    }
+    else if (cy2 != cy1 && TEST(y_splits, gcy)) {
+      COPY(end, destination);
+      destination[Y_AXIS] = LOGICAL_Y_POSITION(mbl.index_to_ypos[gcy]);
+      normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
+      destination[X_AXIS] = MBL_SEGMENT_END(X);
+      CBI(y_splits, gcy);
+    }
+    else {
+      // Already split on a border
+      line_to_destination(fr_mm_s);
+      set_current_to_destination();
+      return;
+    }
+
+    destination[Z_AXIS] = MBL_SEGMENT_END(Z);
+    destination[E_AXIS] = MBL_SEGMENT_END(E);
+
+    // Do the split and look for more borders
+    mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
+
+    // Restore destination from stack
+    COPY(destination, end);
+    mesh_line_to_destination(fr_mm_s, x_splits, y_splits);
+  }
+
 #endif // MESH_BED_LEVELING

Marlin/src/feature/mbl/mesh_bed_leveling.h

@@ -120,4 +120,6 @@ public:
 
 extern mesh_bed_leveling mbl;
 
+void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF);
+
 #endif // _MESH_BED_LEVELING_H_

Marlin/src/feature/ubl/G26_Mesh_Validation_Tool.cpp

@@ -33,6 +33,7 @@
   #include "../../Marlin.h"
   #include "../../module/planner.h"
   #include "../../module/stepper.h"
+  #include "../../module/motion.h"
   #include "../../module/temperature.h"
   #include "../../lcd/ultralcd.h"
   #include "../../gcode/parser.h"
@@ -129,7 +130,6 @@
 
   // External references
 
-  extern float feedrate_mm_s; // must set before calling prepare_move_to_destination
   extern Planner planner;
   #if ENABLED(ULTRA_LCD)
     extern char lcd_status_message[];

Marlin/src/feature/ubl/ubl.cpp

@@ -30,6 +30,7 @@
   #include "../../module/configuration_store.h"
   #include "../../core/serial.h"
   #include "../../module/planner.h"
+  #include "../../module/motion.h"
 
   #include "math.h"
 

Marlin/src/feature/ubl/ubl_motion.cpp

@@ -28,6 +28,7 @@
   #include "../../Marlin.h"
   #include "../../module/planner.h"
   #include "../../module/stepper.h"
+  #include "../../module/motion.h"
 
   #include <math.h>