Rewrote the fitting routine of the induction sensor points to the measured points

from Linear Least Squares to Nonlinear Least Squares to maintain unity length of machine axes.
Improved the scanning of the 1st row of the induction points.

Firmware/Marlin_main.cpp

@@ -2844,6 +2844,42 @@ void process_commands()
         break;
     }
 
+#if 0
+    case 48: // M48: scan the bed induction sensor points, print the sensor trigger coordinates to the serial line for visualization on the PC.
+    {
+        // Disable the default update procedure of the display. We will do a modal dialog.
+        lcd_update_enable(false);
+        // Let the planner use the uncorrected coordinates.
+        mbl.reset();
+        world2machine_reset();
+        // Move the print head close to the bed.
+        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
+        st_synchronize();
+        // Home in the XY plane.
+        set_destination_to_current();
+        setup_for_endstop_move();
+        home_xy();
+        int8_t verbosity_level = 0;
+        if (code_seen('V')) {
+            // Just 'V' without a number counts as V1.
+            char c = strchr_pointer[1];
+            verbosity_level = (c == ' ' || c == '\t' || c == 0) ? 1 : code_value_short();
+        }
+        bool success = scan_bed_induction_points(verbosity_level);
+        clean_up_after_endstop_move();
+        // Print head up.
+        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
+        st_synchronize();
+        lcd_update_enable(true);
+        lcd_implementation_clear();
+        // lcd_return_to_status();
+        lcd_update();
+        break;
+    }
+#endif
+
     case 47:
         lcd_diag_show_end_stops();
         break;

Firmware/mesh_bed_calibration.cpp

@@ -12,9 +12,18 @@ extern float home_retract_mm_ext(int axis);
 float world2machine_rotation_and_skew[2][2];
 float world2machine_shift[2];
 
+// Weight of the Y coordinate for the least squares fitting of the bed induction sensor targets.
+// Only used for the first row of the points, which may not befully in reach of the sensor.
+#define WEIGHT_FIRST_ROW (0.2f)
+
 #define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER)
 #define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER)
 
+// Scaling of the real machine axes against the programmed dimensions in the firmware.
+// The correction is tiny, here around 0.5mm on 250mm length.
+#define MACHINE_AXIS_SCALE_X ((250.f + 0.5f) / 250.f)
+#define MACHINE_AXIS_SCALE_Y ((250.f + 0.5f) / 250.f)
+
 // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
 // The points are ordered in a zig-zag fashion to speed up the calibration.
 const float bed_ref_points[] PROGMEM = {
@@ -45,6 +54,12 @@ const float bed_ref_points_4[] PROGMEM = {
 
 static inline float sqr(float x) { return x * x; }
 
+
+#if 0
+// Linear Least Squares fitting of the bed to the measured induction points.
+// This method will not maintain a unity length of the machine axes.
+// This may be all right if the sensor points are measured precisely,
+// but it will stretch or shorten the machine axes if the measured data is not precise enough.
 bool calculate_machine_skew_and_offset_LS(
     // Matrix of maximum 9 2D points (18 floats)
     const float  *measured_pts,
@@ -136,13 +151,11 @@ bool calculate_machine_skew_and_offset_LS(
 
         // Recalculate A and b for the y values.
         // Note the weighting of the first row of values.
-//        const float weight_1st_row = 0.5f;
-        const float weight_1st_row = 0.2f;
         for (uint8_t r = 0; r < 3; ++ r) {
             for (uint8_t c = 0; c < 3; ++ c) {
                 acc = 0;
                 for (uint8_t i = 0; i < npts; ++ i) {
-                    float w = (i < 3) ? weight_1st_row : 1.f;
+                    float w = (i < 3) ? WEIGHT_FIRST_ROW : 1.f;
                     float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
                     float b = (c == 2) ? 1.f : measured_pts[2 * i + c];
                     acc += a * b * w;
@@ -151,7 +164,7 @@ bool calculate_machine_skew_and_offset_LS(
             }
             acc = 0.f;
             for (uint8_t i = 0; i < npts; ++ i) {
-                float w = (i < 3) ? weight_1st_row : 1.f;
+                float w = (i < 3) ? WEIGHT_FIRST_ROW : 1.f;
                 float a = (r == 2) ? 1.f : measured_pts[2 * i + r];
                 float b = pgm_read_float(true_pts+i*2+1);
                 acc += w * a * b;
@@ -333,6 +346,318 @@ bool calculate_machine_skew_and_offset_LS(
     return true;
 }
 
+#else
+
+// Non-Linear Least Squares fitting of the bed to the measured induction points
+// using the Gauss-Newton method.
+// This method will maintain a unity length of the machine axes,
+// which is the correct approach if the sensor points are not measured precisely.
+bool calculate_machine_skew_and_offset_LS(
+    // Matrix of maximum 9 2D points (18 floats)
+    const float  *measured_pts,
+    uint8_t       npts,
+    const float  *true_pts,
+    // Resulting correction matrix.
+    float        *vec_x,
+    float        *vec_y,
+    float        *cntr,
+    // Temporary values, 49-18-(2*3)=25 floats
+    //    , float *temp
+    int8_t        verbosity_level
+    )
+{
+    if (verbosity_level >= 10) {
+        // Show the initial state, before the fitting.
+        SERIAL_ECHOPGM("X vector, initial: ");
+        MYSERIAL.print(vec_x[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_x[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("Y vector, initial: ");
+        MYSERIAL.print(vec_y[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_y[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("center, initial: ");
+        MYSERIAL.print(cntr[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(cntr[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        for (uint8_t i = 0; i < npts; ++i) {
+            SERIAL_ECHOPGM("point #");
+            MYSERIAL.print(int(i));
+            SERIAL_ECHOPGM(" measured: (");
+            MYSERIAL.print(measured_pts[i * 2], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
+            SERIAL_ECHOPGM("); target: (");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
+            SERIAL_ECHOPGM("), error: ");
+            MYSERIAL.print(sqrt(
+                sqr(pgm_read_float(true_pts + i * 2) - measured_pts[i * 2]) +
+                sqr(pgm_read_float(true_pts + i * 2 + 1) - measured_pts[i * 2 + 1])), 5);
+            SERIAL_ECHOLNPGM("");
+        }
+        delay_keep_alive(100);
+    }
+
+    // Run some iterations of the Gauss-Newton method of non-linear least squares.
+    // Initial set of parameters:
+    // X,Y offset
+    cntr[0] = 0.f;
+    cntr[1] = 0.f;
+    // Rotation of the machine X axis from the bed X axis.
+    float a1 = 0;
+    // Rotation of the machine Y axis from the bed Y axis.
+    float a2 = 0;
+    for (int8_t iter = 0; iter < 100; ++iter) {
+        float c1 = cos(a1) * MACHINE_AXIS_SCALE_X;
+        float s1 = sin(a1) * MACHINE_AXIS_SCALE_X;
+        float c2 = cos(a2) * MACHINE_AXIS_SCALE_Y;
+        float s2 = sin(a2) * MACHINE_AXIS_SCALE_Y;
+        // Prepare the Normal equation for the Gauss-Newton method.
+        float A[4][4] = { 0.f };
+        float b[4] = { 0.f };
+        float acc;
+        for (uint8_t r = 0; r < 4; ++r) {
+            for (uint8_t c = 0; c < 4; ++c) {
+                acc = 0;
+                // J^T times J
+                for (uint8_t i = 0; i < npts; ++i) {
+                    // First for the residuum in the x axis:
+                    if (r != 1 && c != 1) {
+                        float a = 
+                             (r == 0) ? 1.f :
+                            ((r == 2) ? (-s1 * measured_pts[2 * i]) :
+                                        (-c2 * measured_pts[2 * i + 1]));
+                        float b = 
+                             (c == 0) ? 1.f :
+                            ((c == 2) ? (-s1 * measured_pts[2 * i]) :
+                                        (-c2 * measured_pts[2 * i + 1]));
+                        acc += a * b;
+                    }
+                    // Second for the residuum in the y axis. 
+                    // The first row of the points have a low weight, because their position may not be known
+                    // with a sufficient accuracy.
+                    if (r != 0 && c != 0) {
+                        float a = 
+                             (r == 1) ? 1.f :
+                            ((r == 2) ? ( c1 * measured_pts[2 * i]) :
+                                        (-s2 * measured_pts[2 * i + 1]));
+                        float b = 
+                             (c == 1) ? 1.f :
+                            ((c == 2) ? ( c1 * measured_pts[2 * i]) :
+                                        (-s2 * measured_pts[2 * i + 1]));
+                        float w = (i < 3) ? WEIGHT_FIRST_ROW : 1.f;
+                        acc += a * b * w;
+                    }
+                }
+                A[r][c] = acc;
+            }
+            // J^T times f(x)
+            acc = 0.f;
+            for (uint8_t i = 0; i < npts; ++i) {
+                {
+                    float j = 
+                         (r == 0) ? 1.f :
+                        ((r == 1) ? 0.f :
+                        ((r == 2) ? (-s1 * measured_pts[2 * i]) :
+                                    (-c2 * measured_pts[2 * i + 1])));
+                    float fx = c1 * measured_pts[2 * i] - s2 * measured_pts[2 * i + 1] + cntr[0] - pgm_read_float(true_pts + i * 2);
+                    acc += j * fx;
+                }
+                {
+                    float j = 
+                         (r == 0) ? 0.f :
+                        ((r == 1) ? 1.f :
+                        ((r == 2) ? ( c1 * measured_pts[2 * i]) :
+                                    (-s2 * measured_pts[2 * i + 1])));
+                    float fy = s1 * measured_pts[2 * i] + c2 * measured_pts[2 * i + 1] + cntr[1] - pgm_read_float(true_pts + i * 2 + 1);
+                    float w = (i < 3) ? WEIGHT_FIRST_ROW : 1.f;
+                    acc += j * fy * w;
+                }
+            }
+            b[r] = -acc;
+        }
+
+        // Solve for h by a Gauss iteration method.
+        float h[4] = { 0.f };
+        for (uint8_t gauss_iter = 0; gauss_iter < 100; ++gauss_iter) {
+            h[0] = (b[0] - A[0][1] * h[1] - A[0][2] * h[2] - A[0][3] * h[3]) / A[0][0];
+            h[1] = (b[1] - A[1][0] * h[0] - A[1][2] * h[2] - A[1][3] * h[3]) / A[1][1];
+            h[2] = (b[2] - A[2][0] * h[0] - A[2][1] * h[1] - A[2][3] * h[3]) / A[2][2];
+            h[3] = (b[3] - A[3][0] * h[0] - A[3][1] * h[1] - A[3][2] * h[2]) / A[3][3];
+        }
+
+        // and update the current position with h.
+        // It may be better to use the Levenberg-Marquart method here,
+        // but because we are very close to the solution alread,
+        // the simple Gauss-Newton non-linear Least Squares method works well enough.
+        cntr[0] += h[0];
+        cntr[1] += h[1];
+        a1 += h[2];
+        a2 += h[3];
+
+        if (verbosity_level >= 20) {
+            SERIAL_ECHOPGM("iteration: ");
+            MYSERIAL.print(iter, 0);
+            SERIAL_ECHOPGM("correction vector: ");
+            MYSERIAL.print(h[0], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(h[1], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(h[2], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(h[3], 5);
+            SERIAL_ECHOLNPGM("");
+            SERIAL_ECHOPGM("corrected x/y: ");
+            MYSERIAL.print(cntr[0], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(cntr[0], 5);
+            SERIAL_ECHOLNPGM("");
+            SERIAL_ECHOPGM("corrected angles: ");
+            MYSERIAL.print(180.f * a1 / M_PI, 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(180.f * a2 / M_PI, 5);
+            SERIAL_ECHOLNPGM("");
+        }
+    }
+
+    vec_x[0] =  cos(a1) * MACHINE_AXIS_SCALE_X;
+    vec_x[1] =  sin(a1) * MACHINE_AXIS_SCALE_X;
+    vec_y[0] = -sin(a2) * MACHINE_AXIS_SCALE_Y;
+    vec_y[1] =  cos(a2) * MACHINE_AXIS_SCALE_Y;
+
+    if (verbosity_level >= 1) {
+        SERIAL_ECHOPGM("correction angles: ");
+        MYSERIAL.print(180.f * a1 / M_PI, 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(180.f * a2 / M_PI, 5);
+        SERIAL_ECHOLNPGM("");
+    }
+
+    if (verbosity_level >= 10) {
+        // Show the adjusted state, before the fitting.
+        SERIAL_ECHOPGM("X vector new, inverted: ");
+        MYSERIAL.print(vec_x[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_x[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("Y vector new, inverted: ");
+        MYSERIAL.print(vec_y[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_y[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("center new, inverted: ");
+        MYSERIAL.print(cntr[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(cntr[1], 5);
+        SERIAL_ECHOLNPGM("");
+        delay_keep_alive(100);
+
+        SERIAL_ECHOLNPGM("Error after correction: ");
+        for (uint8_t i = 0; i < npts; ++i) {
+            float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1] + cntr[0];
+            float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1] + cntr[1];
+            SERIAL_ECHOPGM("point #");
+            MYSERIAL.print(int(i));
+            SERIAL_ECHOPGM(" measured: (");
+            MYSERIAL.print(measured_pts[i * 2], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
+            SERIAL_ECHOPGM("); corrected: (");
+            MYSERIAL.print(x, 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(y, 5);
+            SERIAL_ECHOPGM("); target: (");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
+            SERIAL_ECHOPGM("), error: ");
+            MYSERIAL.print(sqrt(sqr(pgm_read_float(true_pts + i * 2) - x) + sqr(pgm_read_float(true_pts + i * 2 + 1) - y)));
+            SERIAL_ECHOLNPGM("");
+        }
+    }
+
+    // Invert the transformation matrix made of vec_x, vec_y and cntr.
+    {
+        float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
+        float Ainv[2][2] = {
+            { vec_y[1] / d, -vec_y[0] / d },
+            { -vec_x[1] / d, vec_x[0] / d }
+        };
+        float cntrInv[2] = {
+            -Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
+            -Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
+        };
+        vec_x[0] = Ainv[0][0];
+        vec_x[1] = Ainv[1][0];
+        vec_y[0] = Ainv[0][1];
+        vec_y[1] = Ainv[1][1];
+        cntr[0] = cntrInv[0];
+        cntr[1] = cntrInv[1];
+    }
+
+    if (verbosity_level >= 1) {
+        // Show the adjusted state, before the fitting.
+        SERIAL_ECHOPGM("X vector, adjusted: ");
+        MYSERIAL.print(vec_x[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_x[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("Y vector, adjusted: ");
+        MYSERIAL.print(vec_y[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(vec_y[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("center, adjusted: ");
+        MYSERIAL.print(cntr[0], 5);
+        SERIAL_ECHOPGM(", ");
+        MYSERIAL.print(cntr[1], 5);
+        SERIAL_ECHOLNPGM("");
+        delay_keep_alive(100);
+    }
+
+    if (verbosity_level >= 2) {
+        SERIAL_ECHOLNPGM("Difference after correction: ");
+        for (uint8_t i = 0; i < npts; ++i) {
+            float x = vec_x[0] * pgm_read_float(true_pts + i * 2) + vec_y[0] * pgm_read_float(true_pts + i * 2 + 1) + cntr[0];
+            float y = vec_x[1] * pgm_read_float(true_pts + i * 2) + vec_y[1] * pgm_read_float(true_pts + i * 2 + 1) + cntr[1];
+            SERIAL_ECHOPGM("point #");
+            MYSERIAL.print(int(i));
+            SERIAL_ECHOPGM("measured: (");
+            MYSERIAL.print(measured_pts[i * 2], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(measured_pts[i * 2 + 1], 5);
+            SERIAL_ECHOPGM("); measured-corrected: (");
+            MYSERIAL.print(x, 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(y, 5);
+            SERIAL_ECHOPGM("); target: (");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
+            SERIAL_ECHOPGM("), error: ");
+            MYSERIAL.print(sqrt(sqr(measured_pts[i * 2] - x) + sqr(measured_pts[i * 2 + 1] - y)));
+            SERIAL_ECHOLNPGM("");
+        }
+        delay_keep_alive(100);
+    }
+
+    return true;
+}
+
+#endif
+
 void reset_bed_offset_and_skew()
 {
     eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
@@ -883,151 +1208,274 @@ canceled:
 // Searching in a zig-zag movement in a plane for the maximum width of the response.
 #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (4.f)
 #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
-inline bool improve_bed_induction_sensor_point3()
+inline bool improve_bed_induction_sensor_point3(int verbosity_level)
 {
     float center_old_x = current_position[X_AXIS];
     float center_old_y = current_position[Y_AXIS];
-    float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-    float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-    float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-    float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
-    float y = y0;
     float a, b;
+    {
+        float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+        float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+        float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+        float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+        float y = y0;
 
-    if (x0 < X_MIN_POS)
-        x0 = X_MIN_POS;
-    if (x1 > X_MAX_POS)
-        x1 = X_MAX_POS;
-    if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
-        y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
-    if (y1 > Y_MAX_POS)
-        y1 = Y_MAX_POS;
+        if (x0 < X_MIN_POS)
+            x0 = X_MIN_POS;
+        if (x1 > X_MAX_POS)
+            x1 = X_MAX_POS;
+        if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
+            y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
+        if (y1 > Y_MAX_POS)
+            y1 = Y_MAX_POS;
 
-#if 0
-    SERIAL_ECHOPGM("Initial position: ");
-    SERIAL_ECHO(center_old_x);
-    SERIAL_ECHOPGM(", ");
-    SERIAL_ECHO(center_old_y);
-    SERIAL_ECHOLNPGM("");
-#endif
+        if (verbosity_level >= 20) {
+            SERIAL_ECHOPGM("Initial position: ");
+            SERIAL_ECHO(center_old_x);
+            SERIAL_ECHOPGM(", ");
+            SERIAL_ECHO(center_old_y);
+            SERIAL_ECHOLNPGM("");
+        }
 
-    // Search in the X direction along a cross.
-    float dmax = 0.;
-    float xmax1 = 0.;
-    float xmax2 = 0.;
-    for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
+        // Search in the positive Y direction, until a maximum diameter is found.
+        float dmax = 0.f;
+        float xmax1 = 0.f;
+        float xmax2 = 0.f;
+        for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
+            enable_z_endstop(false);
+            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                continue;
+                // SERIAL_PROTOCOLPGM("Failed 1\n");
+                // current_position[X_AXIS] = center_old_x;
+                // goto canceled;
+            }
+            a = current_position[X_AXIS];
+            enable_z_endstop(false);
+            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                continue;
+                // SERIAL_PROTOCOLPGM("Failed 2\n");
+                // current_position[X_AXIS] = center_old_x;
+                // goto canceled;
+            }
+            b = current_position[X_AXIS];
+            if (verbosity_level > 20) {
+                SERIAL_ECHOPGM("Measured left ");
+                MYSERIAL.print(a, 5);
+                SERIAL_ECHOPGM("right ");
+                MYSERIAL.print(b, 5);
+                SERIAL_ECHOPGM(", ");
+                MYSERIAL.print(y, 5);
+                SERIAL_ECHOPGM(", ");
+                MYSERIAL.print(current_position[Z_AXIS], 5);
+                SERIAL_ECHOLNPGM("");
+            }
+            float d = b - a;
+            if (d > dmax) {
+                xmax1 = 0.5f * (a + b);
+                dmax = d;
+            } else if (dmax > 0.) {
+                y0 = y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
+                break;
+            }
+        }
+        if (dmax == 0.) {
+            SERIAL_PROTOCOLPGM("failed - not found\n");
+            goto canceled;
+        }
+
+        SERIAL_PROTOCOLPGM("ok 1\n");
+        // Search in the negative Y direction, until a maximum diameter is found.
+        dmax = 0.;
+        if (y0 + 1.f < y1)
+            y1 = y0 + 1.f;
+        for (float y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
+            enable_z_endstop(false);
+            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                continue;
+                /*
+                current_position[X_AXIS] = center_old_x;
+                SERIAL_PROTOCOLPGM("Failed 3\n");
+                goto canceled;
+                */
+            }
+            a = current_position[X_AXIS];
+            enable_z_endstop(false);
+            go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+            enable_z_endstop(true);
+            go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+            update_current_position_xyz();
+            if (! endstop_z_hit_on_purpose()) {
+                continue;
+                /*
+                current_position[X_AXIS] = center_old_x;
+                SERIAL_PROTOCOLPGM("Failed 4\n");
+                goto canceled;
+                */
+            }
+            b = current_position[X_AXIS];
+            if (verbosity_level > 20) {
+                SERIAL_ECHOPGM("Measured left ");
+                MYSERIAL.print(a, 5);
+                SERIAL_ECHOPGM("right ");
+                MYSERIAL.print(b, 5);
+                SERIAL_ECHOPGM(", ");
+                MYSERIAL.print(y, 5);
+                SERIAL_ECHOPGM(", ");
+                MYSERIAL.print(current_position[Z_AXIS], 5);
+                SERIAL_ECHOLNPGM("");
+            }
+            float d = b - a;
+            if (d > dmax) {
+                xmax2 = 0.5f * (a + b);
+                dmax = d;
+            } else if (dmax > 0.) {
+                y1 = y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
+                break;
+            }
+        }
+        // SERIAL_PROTOCOLPGM("ok 2\n");
+        // Go to the center.
         enable_z_endstop(false);
-        go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+        if (dmax == 0.f) {
+            // Found only the point going from ymin to ymax.
+            current_position[X_AXIS] = xmax1;
+            current_position[Y_AXIS] = y0;
+        } else {
+            // Both points found (from ymin to ymax and from ymax to ymin).
+            float p = 0.5f;
+            // If the first hit was on the machine boundary,
+            // give it a higher weight.
+            if (y0 == Y_MIN_POS_FOR_BED_CALIBRATION)
+                p = 0.75f;
+            current_position[X_AXIS] = p * xmax1 + (1.f - p) * xmax2;
+            current_position[Y_AXIS] = p * y0 + (1.f - p) * y1;
+        }
+        if (verbosity_level >= 20) {
+            SERIAL_ECHOPGM("Adjusted position: ");
+            SERIAL_ECHO(current_position[X_AXIS]);
+            SERIAL_ECHOPGM(", ");
+            SERIAL_ECHO(current_position[Y_AXIS]);
+            SERIAL_ECHOLNPGM("");
+        }
+        go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+        // delay_keep_alive(3000);
+    }
+
+    // Try yet to improve the X position.
+    {
+        float x0 = current_position[X_AXIS] - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
+        float x1 = current_position[X_AXIS] + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
+        if (x0 < X_MIN_POS)
+            x0 = X_MIN_POS;
+        if (x1 > X_MAX_POS)
+            x1 = X_MAX_POS;
+
+        // Search in the X direction along a cross.
+        enable_z_endstop(false);
+        go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
         enable_z_endstop(true);
-        go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+        go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
         update_current_position_xyz();
         if (! endstop_z_hit_on_purpose()) {
-            // SERIAL_PROTOCOLPGM("Failed 1\n");
             current_position[X_AXIS] = center_old_x;
             goto canceled;
         }
         a = current_position[X_AXIS];
         enable_z_endstop(false);
-        go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
+        go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
         enable_z_endstop(true);
-        go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
+        go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
         update_current_position_xyz();
         if (! endstop_z_hit_on_purpose()) {
-            // SERIAL_PROTOCOLPGM("Failed 2\n");
             current_position[X_AXIS] = center_old_x;
             goto canceled;
         }
         b = current_position[X_AXIS];
-        float d = b - a;
-        if (d > dmax) {
-            xmax1 = 0.5f * (a + b);
-            dmax = d;
-        } else {
-            y0 = y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
-            break;
-        }
-    }
-    if (dmax == 0.) {
-        // SERIAL_PROTOCOLPGM("failed - not found\n");
-        goto canceled;
+
+        // Go to the center.
+        enable_z_endstop(false);
+        current_position[X_AXIS] = 0.5f * (a + b);
+        go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
     }
 
-    // SERIAL_PROTOCOLPGM("ok 1\n");
-    // Search in the X direction along a cross.
-    dmax = 0.;
-    if (y0 + 1.f < y1)
-        y1 = y0 + 1.f;
-    for (float y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
+    return true;
+
+canceled:
+    // Go back to the center.
+    enable_z_endstop(false);
+    go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
+    return false;
+}
+
+// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
+// write the trigger coordinates to the serial line.
+// Useful for visualizing the behavior of the bed induction detector.
+inline void scan_bed_induction_sensor_point()
+{
+    float center_old_x = current_position[X_AXIS];
+    float center_old_y = current_position[Y_AXIS];
+    float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+    float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+    float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+    float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
+    float y = y0;
+
+    if (x0 < X_MIN_POS)
+        x0 = X_MIN_POS;
+    if (x1 > X_MAX_POS)
+        x1 = X_MAX_POS;
+    if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
+        y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
+    if (y1 > Y_MAX_POS)
+        y1 = Y_MAX_POS;
+
+    for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
         enable_z_endstop(false);
         go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
         enable_z_endstop(true);
         go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
         update_current_position_xyz();
-        if (! endstop_z_hit_on_purpose()) {
-            continue;
-            /*
-            current_position[X_AXIS] = center_old_x;
-            SERIAL_PROTOCOLPGM("Failed 3\n");
-            goto canceled;
-            */
+        if (endstop_z_hit_on_purpose()) {
+            SERIAL_ECHOPGM("Measured left: ");
+            MYSERIAL.print(current_position[X_AXIS], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(y, 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(current_position[Z_AXIS], 5);
+            SERIAL_ECHOLNPGM("");
         }
-        a = current_position[X_AXIS];
         enable_z_endstop(false);
         go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
         enable_z_endstop(true);
         go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
         update_current_position_xyz();
-        if (! endstop_z_hit_on_purpose()) {
-            continue;
-            /*
-            current_position[X_AXIS] = center_old_x;
-            SERIAL_PROTOCOLPGM("Failed 4\n");
-            goto canceled;
-            */
-        }
-        b = current_position[X_AXIS];
-        float d = b - a;
-        if (d > dmax) {
-            xmax2 = 0.5f * (a + b);
-            dmax = d;
-        } else {
-            y1 = y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
-            break;
+        if (endstop_z_hit_on_purpose()) {
+            SERIAL_ECHOPGM("Measured right: ");
+            MYSERIAL.print(current_position[X_AXIS], 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(y, 5);
+            SERIAL_ECHOPGM(", ");
+            MYSERIAL.print(current_position[Z_AXIS], 5);
+            SERIAL_ECHOLNPGM("");
         }
     }
-    // SERIAL_PROTOCOLPGM("ok 2\n");
-    // Go to the center.
-    enable_z_endstop(false);
-    if (dmax == 0.f) {
-        // Found only the point going from ymin to ymax.
-        current_position[X_AXIS] = xmax1;
-        current_position[Y_AXIS] = y0;
-    } else {
-        // Both points found (from ymin to ymax and from ymax to ymin).
-        float p = 0.5f;
-        // If the first hit was on the machine boundary,
-        // give it a higher weight.
-        if (y0 == Y_MIN_POS_FOR_BED_CALIBRATION)
-            p = 0.75f;
-        current_position[X_AXIS] = p * xmax1 + (1.f - p) * xmax2;
-        current_position[Y_AXIS] = p * y0 + (1.f - p) * y1;
-    }
-    /*
-    SERIAL_ECHOPGM("Adjusted position: ");
-    SERIAL_ECHO(current_position[X_AXIS]);
-    SERIAL_ECHOPGM(", ");
-    SERIAL_ECHO(current_position[Y_AXIS]);
-    SERIAL_ECHOLNPGM("");
-    */
-    go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-    // delay_keep_alive(3000);
-    return true;
 
-canceled:
-    // Go back to the center.
     enable_z_endstop(false);
+    current_position[X_AXIS] = center_old_x;
+    current_position[Y_AXIS] = center_old_y;
     go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
-    return false;
 }
 
 #define MESH_BED_CALIBRATION_SHOW_LCD
@@ -1089,9 +1537,10 @@ bool find_bed_offset_and_skew(int8_t verbosity_level)
         find_bed_induction_sensor_point_z();
 #if 1
         if (k == 0) {
+            // Improve the position of the 1st row sensor points by a zig-zag movement.
             int8_t i = 4;
             for (;;) {
-                if (improve_bed_induction_sensor_point3())
+                if (improve_bed_induction_sensor_point3(verbosity_level))
                     break;
                 if (-- i == 0)
                     return false;
@@ -1251,9 +1700,22 @@ bool improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level)
         // Improve the point position by searching its center in a current plane.
         int8_t n_errors = 3;
         for (int8_t iter = 0; iter < 8; ) {
+            if (verbosity_level > 20) {
+                SERIAL_ECHOPGM("Improving bed point ");
+                SERIAL_ECHO(mesh_point);
+                SERIAL_ECHOPGM(", iteration ");
+                SERIAL_ECHO(iter);
+                SERIAL_ECHOPGM(", z");
+                MYSERIAL.print(current_position[Z_AXIS], 5);
+                SERIAL_ECHOLNPGM("");
+            }
             bool found = false;
             if (mesh_point < 3) {
-                found = improve_bed_induction_sensor_point3();
+                // Because the sensor cannot move in front of the first row
+                // of the sensor points, the y position cannot be measured
+                // by a cross center method.
+                // Use a zig-zag search for the first row of the points.
+                found = improve_bed_induction_sensor_point3(verbosity_level);
             } else {
                 switch (method) {
                     case 0: found = improve_bed_induction_sensor_point(); break;
@@ -1277,6 +1739,15 @@ bool improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level)
                 enable_endstops(false);
                 enable_z_endstop(false);
                 go_to_current(homing_feedrate[Z_AXIS]);
+                if (verbosity_level > 20) {
+                    SERIAL_ECHOPGM("Improving bed point ");
+                    SERIAL_ECHO(mesh_point);
+                    SERIAL_ECHOPGM(", iteration ");
+                    SERIAL_ECHO(iter);
+                    SERIAL_ECHOPGM(" failed. Lowering z to ");
+                    MYSERIAL.print(current_position[Z_AXIS], 5);
+                    SERIAL_ECHOLNPGM("");
+                }
             }
         }
         if (verbosity_level >= 10)
@@ -1303,6 +1774,17 @@ bool improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level)
             current_position[Y_AXIS] = pts[mesh_point*2+1];
             go_to_current(homing_feedrate[X_AXIS]/60);
             delay_keep_alive(3000);
+            #if 0
+            if (verbosity_level > 20) {
+                SERIAL_ECHOPGM("Final measured bed point ");
+                SERIAL_ECHO(mesh_point);
+                SERIAL_ECHOPGM(": ");
+                MYSERIAL.print(current_position[X_AXIS], 5);
+                SERIAL_ECHOPGM(", ");
+                MYSERIAL.print(current_position[Y_AXIS], 5);
+                SERIAL_ECHOLNPGM("");
+            }
+            #endif
         }
     }
 
@@ -1341,6 +1823,17 @@ bool improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level)
             current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
             go_to_current(homing_feedrate[X_AXIS]/60);
             delay_keep_alive(3000);
+            #if 0
+            if (verbosity_level > 20) {
+                SERIAL_ECHOPGM("Final calculated bed point ");
+                SERIAL_ECHO(mesh_point);
+                SERIAL_ECHOPGM(": ");
+                MYSERIAL.print(st_get_position_mm(X_AXIS), 5);
+                SERIAL_ECHOPGM(", ");
+                MYSERIAL.print(st_get_position_mm(Y_AXIS), 5);
+                SERIAL_ECHOLNPGM("");
+            }
+            #endif
         }
     }
 
@@ -1360,3 +1853,65 @@ canceled:
     enable_z_endstop(endstop_z_enabled);
     return false;
 }
+
+bool scan_bed_induction_points(int8_t verbosity_level)
+{
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    // Reusing the z_values memory for the measurement cache.
+    // 7x7=49 floats, good for 16 (x,y,z) vectors.
+    float *pts = &mbl.z_values[0][0];
+    float *vec_x = pts + 2 * 9;
+    float *vec_y = vec_x + 2;
+    float *cntr  = vec_y + 2;
+    memset(pts, 0, sizeof(float) * 7 * 7);
+
+    // Cache the current correction matrix.
+    world2machine_initialize();
+    vec_x[0] = world2machine_rotation_and_skew[0][0];
+    vec_x[1] = world2machine_rotation_and_skew[1][0];
+    vec_y[0] = world2machine_rotation_and_skew[0][1];
+    vec_y[1] = world2machine_rotation_and_skew[1][1];
+    cntr[0] = world2machine_shift[0];
+    cntr[1] = world2machine_shift[1];
+    // and reset the correction matrix, so the planner will not do anything.
+    world2machine_reset();
+
+    bool endstops_enabled  = enable_endstops(false);
+    bool endstop_z_enabled = enable_z_endstop(false);
+
+    // Collect a matrix of 9x9 points.
+    for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+        // Don't let the manage_inactivity() function remove power from the motors.
+        refresh_cmd_timeout();
+
+        // Move up.
+        current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
+        enable_endstops(false);
+        enable_z_endstop(false);
+        go_to_current(homing_feedrate[Z_AXIS]/60);
+        // Go to the measurement point.
+        // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+        current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
+        current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
+        // The calibration points are very close to the min Y.
+        if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
+            current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
+        go_to_current(homing_feedrate[X_AXIS]/60);
+        find_bed_induction_sensor_point_z();
+        scan_bed_induction_sensor_point();
+    }
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    enable_endstops(false);
+    enable_z_endstop(false);
+
+    // Don't let the manage_inactivity() function remove power from the motors.
+    refresh_cmd_timeout();
+
+    enable_endstops(endstops_enabled);
+    enable_z_endstop(endstop_z_enabled);
+    return true;
+}

Firmware/mesh_bed_calibration.h

@@ -31,4 +31,9 @@ extern bool find_bed_offset_and_skew(int8_t verbosity_level);
 extern bool improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level);
 extern void reset_bed_offset_and_skew();
 
+// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
+// write the trigger coordinates to the serial line.
+// Useful for visualizing the behavior of the bed induction detector.
+extern bool scan_bed_induction_points(int8_t verbosity_level);
+
 #endif /* MESH_BED_CALIBRATION_H */