Merge pull request #3812 from thinkyhead/rc_mbl_index_finders

Two index finding functions for MBL

Marlin/Marlin_main.cpp

@@ -3008,7 +3008,7 @@ inline void gcode_G28() {
       return;
     }
 
-    int8_t ix, iy;
+    int8_t px, py;
     float z;
 
     switch (state) {
@@ -3023,10 +3023,10 @@ inline void gcode_G28() {
           SERIAL_PROTOCOLPGM("\nZ offset: ");
           SERIAL_PROTOCOL_F(mbl.z_offset, 5);
           SERIAL_PROTOCOLLNPGM("\nMeasured points:");
-          for (int y = 0; y < MESH_NUM_Y_POINTS; y++) {
-            for (int x = 0; x < MESH_NUM_X_POINTS; x++) {
+          for (py = 0; py < MESH_NUM_Y_POINTS; py++) {
+            for (px = 0; px < MESH_NUM_X_POINTS; px++) {
               SERIAL_PROTOCOLPGM("  ");
-              SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
+              SERIAL_PROTOCOL_F(mbl.z_values[py][px], 5);
             }
             SERIAL_EOL;
           }
@@ -3058,8 +3058,8 @@ inline void gcode_G28() {
         }
         // If there's another point to sample, move there with optional lift.
         if (probe_point < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
-          mbl.zigzag(probe_point, ix, iy);
-          _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy));
+          mbl.zigzag(probe_point, px, py);
+          _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py));
           probe_point++;
         }
         else {
@@ -3082,8 +3082,8 @@ inline void gcode_G28() {
 
       case MeshSet:
         if (code_seen('X')) {
-          ix = code_value_long() - 1;
-          if (ix < 0 || ix >= MESH_NUM_X_POINTS) {
+          px = code_value_long() - 1;
+          if (px < 0 || px >= MESH_NUM_X_POINTS) {
             SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n");
             return;
           }
@@ -3093,8 +3093,8 @@ inline void gcode_G28() {
           return;
         }
         if (code_seen('Y')) {
-          iy = code_value_long() - 1;
-          if (iy < 0 || iy >= MESH_NUM_Y_POINTS) {
+          py = code_value_long() - 1;
+          if (py < 0 || py >= MESH_NUM_Y_POINTS) {
             SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n");
             return;
           }
@@ -3110,7 +3110,7 @@ inline void gcode_G28() {
           SERIAL_PROTOCOLPGM("Z not entered.\n");
           return;
         }
-        mbl.z_values[iy][ix] = z;
+        mbl.z_values[py][px] = z;
         break;
 
       case MeshSetZOffset:
@@ -5904,39 +5904,35 @@ inline void gcode_M410() { stepper.quick_stop(); }
    * Use either 'M421 X<mm> Y<mm> Z<mm>' or 'M421 I<xindex> J<yindex> Z<mm>'
    */
   inline void gcode_M421() {
-    float x = 0, y = 0, z = 0;
-    int8_t i = 0, j = 0;
-    bool err = false, hasX, hasY, hasZ, hasI, hasJ;
-    if ((hasX = code_seen('X'))) x = code_value();
-    if ((hasY = code_seen('Y'))) y = code_value();
-    if ((hasI = code_seen('I'))) i = code_value();
-    if ((hasJ = code_seen('J'))) j = code_value();
+    int8_t px, py;
+    float z = 0;
+    bool hasX, hasY, hasZ, hasI, hasJ;
+    if ((hasX = code_seen('X'))) px = mbl.probe_index_x(code_value());
+    if ((hasY = code_seen('Y'))) py = mbl.probe_index_y(code_value());
+    if ((hasI = code_seen('I'))) px = code_value();
+    if ((hasJ = code_seen('J'))) py = code_value();
     if ((hasZ = code_seen('Z'))) z = code_value();
 
     if (hasX && hasY && hasZ) {
 
-      int8_t ix = mbl.select_x_index(x),
-             iy = mbl.select_y_index(y);
-
-      if (ix >= 0 && iy >= 0)
-        mbl.set_z(ix, iy, z);
+      if (px >= 0 && py >= 0)
+        mbl.set_z(px, py, z);
       else {
         SERIAL_ERROR_START;
         SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
       }
     }
     else if (hasI && hasJ && hasZ) {
-      if (i >= 0 && i < MESH_NUM_X_POINTS && j >= 0 && j < MESH_NUM_Y_POINTS)
-        mbl.set_z(i, j, z);
+      if (px >= 0 && px < MESH_NUM_X_POINTS && py >= 0 && py < MESH_NUM_Y_POINTS)
+        mbl.set_z(px, py, z);
       else {
         SERIAL_ERROR_START;
         SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
       }
     }
-    else 
-    {
+    else {
       SERIAL_ERROR_START;
-      SERIAL_ERRORLNPGM(MSG_ERR_M421_REQUIRES_XYZ);
+      SERIAL_ERRORLNPGM(MSG_ERR_M421_PARAMETERS);
     }
   }
 
@@ -7303,52 +7299,52 @@ void mesh_buffer_line(float x, float y, float z, const float e, float feed_rate,
     set_current_to_destination();
     return;
   }
-  int pix = mbl.select_x_index(current_position[X_AXIS] - home_offset[X_AXIS]);
-  int piy = mbl.select_y_index(current_position[Y_AXIS] - home_offset[Y_AXIS]);
-  int ix = mbl.select_x_index(x - home_offset[X_AXIS]);
-  int iy = mbl.select_y_index(y - home_offset[Y_AXIS]);
-  pix = min(pix, MESH_NUM_X_POINTS - 2);
-  piy = min(piy, MESH_NUM_Y_POINTS - 2);
-  ix = min(ix, MESH_NUM_X_POINTS - 2);
-  iy = min(iy, MESH_NUM_Y_POINTS - 2);
-  if (pix == ix && piy == iy) {
+  int pcx = mbl.cel_index_x(current_position[X_AXIS] - home_offset[X_AXIS]);
+  int pcy = mbl.cel_index_y(current_position[Y_AXIS] - home_offset[Y_AXIS]);
+  int cx = mbl.cel_index_x(x - home_offset[X_AXIS]);
+  int cy = mbl.cel_index_y(y - home_offset[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) {
     // Start and end on same mesh square
     planner.buffer_line(x, y, z, e, feed_rate, extruder);
     set_current_to_destination();
     return;
   }
   float nx, ny, nz, ne, normalized_dist;
-  if (ix > pix && TEST(x_splits, ix)) {
-    nx = mbl.get_x(ix) + home_offset[X_AXIS];
+  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, ix);
+    CBI(x_splits, cx);
   }
-  else if (ix < pix && TEST(x_splits, pix)) {
-    nx = mbl.get_x(pix) + home_offset[X_AXIS];
+  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, pix);
+    CBI(x_splits, pcx);
   }
-  else if (iy > piy && TEST(y_splits, iy)) {
-    ny = mbl.get_y(iy) + home_offset[Y_AXIS];
+  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, iy);
+    CBI(y_splits, cy);
   }
-  else if (iy < piy && TEST(y_splits, piy)) {
-    ny = mbl.get_y(piy) + home_offset[Y_AXIS];
+  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, piy);
+    CBI(y_splits, pcy);
   }
   else {
     // Already split on a border

Marlin/configuration_store.cpp

@@ -62,7 +62,7 @@
  *  201            z_offset (float) (added in V23)
  *  205            mesh_num_x (uint8 as set in firmware)
  *  206            mesh_num_y (uint8 as set in firmware)
- *  207  M421 XYZ  z_values[][] (float x9, by default)
+ *  207 G29 S3 XYZ z_values[][] (float x9, by default)
  *
  * AUTO BED LEVELING
  *  243  M851      zprobe_zoffset (float)
@@ -733,13 +733,13 @@ void Config_PrintSettings(bool forReplay) {
     SERIAL_ECHOPAIR(" X", MESH_NUM_X_POINTS);
     SERIAL_ECHOPAIR(" Y", MESH_NUM_Y_POINTS);
     SERIAL_EOL;
-    for (uint8_t y = 0; y < MESH_NUM_Y_POINTS; y++) {
-      for (uint8_t x = 0; x < MESH_NUM_X_POINTS; x++) {
+    for (uint8_t py = 1; py <= MESH_NUM_Y_POINTS; py++) {
+      for (uint8_t px = 1; px <= MESH_NUM_X_POINTS; px++) {
         CONFIG_ECHO_START;
-        SERIAL_ECHOPAIR("  M421 X", mbl.get_x(x));
-        SERIAL_ECHOPAIR(" Y", mbl.get_y(y));
+        SERIAL_ECHOPAIR("  G29 S3 X", px);
+        SERIAL_ECHOPAIR(" Y", py);
         SERIAL_ECHOPGM(" Z");
-        SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
+        SERIAL_PROTOCOL_F(mbl.z_values[py-1][px-1], 5);
         SERIAL_EOL;
       }
     }

Marlin/language.h

@@ -147,7 +147,7 @@
 #define MSG_Z2_MAX                          "z2_max: "
 #define MSG_Z_PROBE                         "z_probe: "
 #define MSG_ERR_MATERIAL_INDEX              "M145 S<index> out of range (0-1)"
-#define MSG_ERR_M421_REQUIRES_XYZ           "M421 requires XYZ or IJZ parameters"
+#define MSG_ERR_M421_PARAMETERS             "M421 requires XYZ or IJZ parameters"
 #define MSG_ERR_MESH_XY                     "Mesh XY or IJ cannot be resolved"
 #define MSG_ERR_M428_TOO_FAR                "Too far from reference point"
 #define MSG_ERR_M303_DISABLED               "PIDTEMP disabled"

Marlin/mesh_bed_leveling.h

@@ -37,34 +37,40 @@
 
     void reset();
 
-    static FORCE_INLINE float get_x(int8_t i) { return MESH_MIN_X + (MESH_X_DIST) * i; }
-    static FORCE_INLINE float get_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
-    void set_z(int8_t ix, int8_t iy, float z) { z_values[iy][ix] = z; }
-
-    inline void zigzag(int8_t index, int8_t &ix, int8_t &iy) {
-      ix = index % (MESH_NUM_X_POINTS);
-      iy = index / (MESH_NUM_X_POINTS);
-      if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
+    static FORCE_INLINE float get_probe_x(int8_t i) { return MESH_MIN_X + (MESH_X_DIST) * i; }
+    static FORCE_INLINE float get_probe_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
+    void set_z(const int8_t px, const int8_t py, const float z) { z_values[py][px] = z; }
+
+    inline void zigzag(int8_t index, int8_t &px, int8_t &py) {
+      px = index % (MESH_NUM_X_POINTS);
+      py = index / (MESH_NUM_X_POINTS);
+      if (py & 1) px = (MESH_NUM_X_POINTS - 1) - px; // Zig zag
     }
 
     void set_zigzag_z(int8_t index, float z) {
-      int8_t ix, iy;
-      zigzag(index, ix, iy);
-      set_z(ix, iy, z);
+      int8_t px, py;
+      zigzag(index, px, py);
+      set_z(px, py, z);
+    }
+
+    int8_t cel_index_x(float x) {
+      int8_t cx = int(x - (MESH_MIN_X)) / (MESH_X_DIST);
+      return constrain(cx, 0, (MESH_NUM_X_POINTS) - 2);
+    }
+
+    int8_t cel_index_y(float y) {
+      int8_t cy = int(y - (MESH_MIN_Y)) / (MESH_Y_DIST);
+      return constrain(cy, 0, (MESH_NUM_Y_POINTS) - 2);
     }
 
-    int8_t select_x_index(float x) {
-      for (uint8_t i = MESH_NUM_X_POINTS; i--;)
-        if (fabs(x - get_x(i)) <= (MESH_X_DIST) / 2)
-          return i;
-      return -1;
+    int8_t probe_index_x(float x) {
+      int8_t px = int(x - (MESH_MIN_X) + (MESH_X_DIST) / 2) / (MESH_X_DIST);
+      return (px >= 0 && px < (MESH_NUM_X_POINTS)) ? px : -1;
     }
 
-    int8_t select_y_index(float y) {
-      for (uint8_t i = MESH_NUM_Y_POINTS; i--;)
-        if (fabs(y - get_y(i)) <= (MESH_Y_DIST) / 2)
-          return i;
-      return -1;
+    int8_t probe_index_y(float y) {
+      int8_t py = int(y - (MESH_MIN_Y) + (MESH_Y_DIST) / 2) / (MESH_Y_DIST);
+      return (py >= 0 && py < (MESH_NUM_Y_POINTS)) ? py : -1;
     }
 
     float calc_z0(float a0, float a1, float z1, float a2, float z2) {
@@ -74,18 +80,18 @@
     }
 
     float get_z(float x0, float y0) {
-      int8_t x_index = select_x_index(x0);
-      int8_t y_index = select_y_index(y0);
-      if (x_index < 0 || y_index < 0) return z_offset;
+      int8_t cx = cel_index_x(x0),
+             cy = cel_index_y(y0);
+      if (cx < 0 || cy < 0) return z_offset;
       float z1 = calc_z0(x0,
-                         get_x(x_index), z_values[y_index][x_index],
-                         get_x(x_index + 1), z_values[y_index][x_index + 1]);
+                         get_probe_x(cx), z_values[cy][cx],
+                         get_probe_x(cx + 1), z_values[cy][cx + 1]);
       float z2 = calc_z0(x0,
-                         get_x(x_index), z_values[y_index + 1][x_index],
-                         get_x(x_index + 1), z_values[y_index + 1][x_index + 1]);
+                         get_probe_x(cx), z_values[cy + 1][cx],
+                         get_probe_x(cx + 1), z_values[cy + 1][cx + 1]);
       float z0 = calc_z0(y0,
-                         get_y(y_index), z1,
-                         get_y(y_index + 1), z2);
+                         get_probe_y(cy), z1,
+                         get_probe_y(cy + 1), z2);
       return z0 + z_offset;
     }
   };

Marlin/ultralcd.cpp

@@ -1013,9 +1013,9 @@ void lcd_cooldown() {
     lcd_goto_menu(_lcd_level_bed_moving);
 
     // _mbl_goto_xy runs the menu loop until the move is done
-    int8_t ix, iy;
-    mbl.zigzag(_lcd_level_bed_position, ix, iy);
-    _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy));
+    int8_t px, py;
+    mbl.zigzag(_lcd_level_bed_position, px, py);
+    _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py));
 
     // After the blocking function returns, change menus
     lcd_goto_menu(_lcd_level_bed_get_z);