Merge pull request #4310 from thinkyhead/rc_mbl_position_shift

Fix position shift with MBL

Marlin/Marlin_main.cpp

 #if ENABLED(MESH_BED_LEVELING)
 
 // This function is used to split lines on mesh borders so each segment is only part of one mesh area
-void mesh_buffer_line(float x, float y, float z, const float e, float fr_mm_s, const uint8_t& extruder, uint8_t x_splits = 0xff, uint8_t y_splits = 0xff) {
-  if (!mbl.active()) {
-    planner.buffer_line(x, y, z, e, fr_mm_s, extruder);
-    set_current_to_destination();
-    return;
-  }
-  int pcx = mbl.cell_index_x(RAW_CURRENT_POSITION(X_AXIS)),
-      pcy = mbl.cell_index_y(RAW_CURRENT_POSITION(Y_AXIS)),
-      cx = mbl.cell_index_x(RAW_POSITION(x, X_AXIS)),
-      cy = mbl.cell_index_y(RAW_POSITION(y, Y_AXIS));
-  NOMORE(pcx, MESH_NUM_X_POINTS - 2);
-  NOMORE(pcy, MESH_NUM_Y_POINTS - 2);
-  NOMORE(cx,  MESH_NUM_X_POINTS - 2);
-  NOMORE(cy,  MESH_NUM_Y_POINTS - 2);
-  if (pcx == cx && pcy == cy) {
+void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_splits = 0xff) {
+  int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X_AXIS)),
+      cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y_AXIS)),
+      cx2 = mbl.cell_index_x(RAW_POSITION(destination[X_AXIS], X_AXIS)),
+      cy2 = mbl.cell_index_y(RAW_POSITION(destination[Y_AXIS], Y_AXIS));
+  NOMORE(cx1, MESH_NUM_X_POINTS - 2);
+  NOMORE(cy1, MESH_NUM_Y_POINTS - 2);
+  NOMORE(cx2, MESH_NUM_X_POINTS - 2);
+  NOMORE(cy2, MESH_NUM_Y_POINTS - 2);
+
+  if (cx1 == cx2 && cy1 == cy2) {
     // Start and end on same mesh square
-    planner.buffer_line(x, y, z, e, fr_mm_s, extruder);
+    line_to_destination(fr_mm_m);
     set_current_to_destination();
     return;
   }
-  float nx, ny, nz, ne, normalized_dist;
-  if (cx > pcx && TEST(x_splits, cx)) {
-    nx = mbl.get_probe_x(cx) + home_offset[X_AXIS];
-    normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
-    ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
-    nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
-    ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
-    CBI(x_splits, cx);
-  }
-  else if (cx < pcx && TEST(x_splits, pcx)) {
-    nx = mbl.get_probe_x(pcx) + home_offset[X_AXIS];
-    normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
-    ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
-    nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
-    ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
-    CBI(x_splits, pcx);
-  }
-  else if (cy > pcy && TEST(y_splits, cy)) {
-    ny = mbl.get_probe_y(cy) + home_offset[Y_AXIS];
-    normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
-    nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
-    nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
-    ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
-    CBI(y_splits, cy);
-  }
-  else if (cy < pcy && TEST(y_splits, pcy)) {
-    ny = mbl.get_probe_y(pcy) + home_offset[Y_AXIS];
-    normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
-    nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
-    nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
-    ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
-    CBI(y_splits, pcy);
+
+  #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist)
+
+  float normalized_dist, end[NUM_AXIS];
+
+  // Split at the left/front border of the right/top square
+  int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
+  if (cx2 != cx1 && TEST(x_splits, gcx)) {
+    memcpy(end, destination, sizeof(end));
+    destination[X_AXIS] = mbl.get_probe_x(gcx) + home_offset[X_AXIS] + position_shift[X_AXIS];
+    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)) {
+    memcpy(end, destination, sizeof(end));
+    destination[Y_AXIS] = mbl.get_probe_y(gcy) + home_offset[Y_AXIS] + position_shift[Y_AXIS];
+    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
-    planner.buffer_line(x, y, z, e, fr_mm_s, extruder);
+    line_to_destination(fr_mm_m);
     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
-  destination[X_AXIS] = nx;
-  destination[Y_AXIS] = ny;
-  destination[Z_AXIS] = nz;
-  destination[E_AXIS] = ne;
-  mesh_buffer_line(nx, ny, nz, ne, fr_mm_s, extruder, x_splits, y_splits);
-  destination[X_AXIS] = x;
-  destination[Y_AXIS] = y;
-  destination[Z_AXIS] = z;
-  destination[E_AXIS] = e;
-  mesh_buffer_line(x, y, z, e, fr_mm_s, extruder, x_splits, y_splits);
+  mesh_line_to_destination(fr_mm_m, x_splits, y_splits);
+
+  // Restore destination from stack
+  memcpy(destination, end, sizeof(end));
+  mesh_line_to_destination(fr_mm_m, x_splits, y_splits);
 }
 #endif  // MESH_BED_LEVELING
 
     }
     else {
       #if ENABLED(MESH_BED_LEVELING)
-        mesh_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], MMM_TO_MMS_SCALED(feedrate_mm_m), active_extruder);
-        return false;
-      #else
-        line_to_destination(MMM_SCALED(feedrate_mm_m));
+        if (mbl.active()) {
+          mesh_line_to_destination(MMM_SCALED(feedrate_mm_m));
+          return false;
+        }
+        else
       #endif
+          line_to_destination(MMM_SCALED(feedrate_mm_m));
     }
     return true;
   }

Marlin/SanityCheck.h

     #error "MESH_BED_LEVELING does not yet support DELTA printers."
   #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
     #error "Select AUTO_BED_LEVELING_FEATURE or MESH_BED_LEVELING, not both."
-  #elif MESH_NUM_X_POINTS > 7 || MESH_NUM_Y_POINTS > 7
-    #error "MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS need to be less than 8."
+  #elif MESH_NUM_X_POINTS > 9 || MESH_NUM_Y_POINTS > 9
+    #error "MESH_NUM_X_POINTS and MESH_NUM_Y_POINTS must be less than 10."
   #endif
 #elif ENABLED(MANUAL_BED_LEVELING)
   #error "MESH_BED_LEVELING is required for MANUAL_BED_LEVELING."

Marlin/configuration_store.cpp

  *  203            z_offset (float)
  *  207            mesh_num_x (uint8 as set in firmware)
  *  208            mesh_num_y (uint8 as set in firmware)
- *  209 G29 S3 XYZ z_values[][] (float x9, by default)
+ *  209 G29 S3 XYZ z_values[][] (float x9, by default, up to float x 81)
  *
  * AUTO BED LEVELING
  *  245  M851      zprobe_zoffset (float)