Polished the bed skew and shift calibration.

Firmware/Marlin.h

@@ -225,6 +225,7 @@ void enquecommand_front(const char *cmd, bool from_progmem = false);
 //put an ASCII command at the end of the current buffer, read from flash
 #define enquecommand_P(cmd) enquecommand(cmd, true)
 #define enquecommand_front_P(cmd) enquecommand_front(cmd, true)
+void repeatcommand_front();
 
 void prepare_arc_move(char isclockwise);
 void clamp_to_software_endstops(float target[3]);

Firmware/Marlin_main.cpp

@@ -418,6 +418,10 @@ static bool cmdbuffer_front_already_processed = false;
 // String of a command, which is to be executed right now.
 #define CMDBUFFER_CURRENT_STRING (cmdbuffer+bufindr+1)
 
+// Enable debugging of the command buffer.
+// Debugging information will be sent to serial line.
+// #define CMDBUFFER_DEBUG
+
 static int serial_count = 0;
 static boolean comment_mode = false;
 static char *strchr_pointer; // just a pointer to find chars in the command string like X, Y, Z, E, etc
@@ -495,6 +499,7 @@ void serial_echopair_P(const char *s_P, unsigned long v)
 void cmdqueue_pop_front()
 {
     if (buflen > 0) {
+#ifdef CMDBUFFER_DEBUG
         SERIAL_ECHOPGM("Dequeing ");
         SERIAL_ECHO(cmdbuffer+bufindr+1);
         SERIAL_ECHOLNPGM("");
@@ -509,6 +514,7 @@ void cmdqueue_pop_front()
         SERIAL_ECHOPGM(", bufsize ");
         SERIAL_ECHO(sizeof(cmdbuffer));
         SERIAL_ECHOLNPGM("");
+#endif /* CMDBUFFER_DEBUG */
         if (-- buflen == 0) {
             // Empty buffer.
             if (serial_count == 0)
@@ -526,6 +532,7 @@ void cmdqueue_pop_front()
                 // skip to the start and find the nonzero command.
                 for (bufindr = 0; cmdbuffer[bufindr] == 0; ++ bufindr) ;
             }
+#ifdef CMDBUFFER_DEBUG
             SERIAL_ECHOPGM("New indices: buflen ");
             SERIAL_ECHO(buflen);
             SERIAL_ECHOPGM(", bufindr ");
@@ -537,6 +544,7 @@ void cmdqueue_pop_front()
             SERIAL_ECHOPGM(" new command on the top: ");
             SERIAL_ECHO(cmdbuffer+bufindr+1);
             SERIAL_ECHOLNPGM("");
+#endif /* CMDBUFFER_DEBUG */
         }
     }
 }
@@ -559,20 +567,26 @@ bool cmdqueue_could_enqueue_front(int len_asked)
         return false;
     // Adjust the end of the write buffer based on whether a partial line is in the receive buffer.
     int endw = (serial_count > 0) ? (bufindw + MAX_CMD_SIZE + 1) : bufindw;
-    if (bufindw < bufindr)
+    if (bufindw < bufindr) {
+        int bufindr_new = bufindr - len_asked - 2;
         // Simple case. There is a contiguous space between the write buffer and the read buffer.
-        return endw + len_asked + 2 < bufindr;
-    // Otherwise the free space is split between the start and end.
-    if (len_asked + 2 <= bufindr) {
-        // Could fit at the start.
-        bufindr -= len_asked + 2;
-        return true;
-    }
-    int bufindr_new = sizeof(cmdbuffer) - len_asked - 2;
-    if (endw <= bufindr_new) {
-        memset(cmdbuffer, 0, bufindr);
-        bufindr = bufindr_new;
-        return true;
+        if (endw <= bufindr_new) {
+            bufindr = bufindr_new;
+            return true;
+        }
+    } else {
+        // Otherwise the free space is split between the start and end.
+        if (len_asked + 2 <= bufindr) {
+            // Could fit at the start.
+            bufindr -= len_asked + 2;
+            return true;
+        }
+        int bufindr_new = sizeof(cmdbuffer) - len_asked - 2;
+        if (endw <= bufindr_new) {
+            memset(cmdbuffer, 0, bufindr);
+            bufindr = bufindr_new;
+            return true;
+        }
     }
     return false;
 }
@@ -640,24 +654,59 @@ bool cmdqueue_could_enqueue_back(int len_asked)
     return false;
 }
 
-void cmdqueue_dump_to_serial()
+#ifdef CMDBUFFER_DEBUG
+static void cmdqueue_dump_to_serial_single_line(int nr, const char *p)
+{
+    SERIAL_ECHOPGM("Entry nr: ");
+    SERIAL_ECHO(nr);
+    SERIAL_ECHOPGM(", type: ");
+    SERIAL_ECHO(int(*p));
+    SERIAL_ECHOPGM(", cmd: ");
+    SERIAL_ECHO(p+1);  
+    SERIAL_ECHOLNPGM("");
+}
+
+static void cmdqueue_dump_to_serial()
 {
-    SERIAL_ECHOLNPGM("Content of the buffer: ");
     if (buflen == 0) {
         SERIAL_ECHOLNPGM("The command buffer is empty.");
     } else {
-        SERIAL_ECHOPGM("Number of entries: ");
+        SERIAL_ECHOPGM("Content of the buffer: entries ");
         SERIAL_ECHO(buflen);
+        SERIAL_ECHOPGM(", indr ");
+        SERIAL_ECHO(bufindr);
+        SERIAL_ECHOPGM(", indw ");
+        SERIAL_ECHO(bufindw);
         SERIAL_ECHOLNPGM("");
+        int nr = 0;
+        if (bufindr < bufindw) {
+            for (const char *p = cmdbuffer + bufindr; p < cmdbuffer + bufindw; ++ nr) {
+                cmdqueue_dump_to_serial_single_line(nr, p);
+                // Skip the command.
+                for (++p; *p != 0; ++ p);
+                // Skip the gaps.
+                for (++p; p < cmdbuffer + bufindw && *p == 0; ++ p);
+            }
+        } else {
+            for (const char *p = cmdbuffer + bufindr; p < cmdbuffer + sizeof(cmdbuffer); ++ nr) {
+                cmdqueue_dump_to_serial_single_line(nr, p);
+                // Skip the command.
+                for (++p; *p != 0; ++ p);
+                // Skip the gaps.
+                for (++p; p < cmdbuffer + sizeof(cmdbuffer) && *p == 0; ++ p);
+            }
+            for (const char *p = cmdbuffer; p < cmdbuffer + bufindw; ++ nr) {
+                cmdqueue_dump_to_serial_single_line(nr, p);
+                // Skip the command.
+                for (++p; *p != 0; ++ p);
+                // Skip the gaps.
+                for (++p; p < cmdbuffer + bufindw && *p == 0; ++ p);
+            }
+        }
+        SERIAL_ECHOLNPGM("End of the buffer.");
     }
-    if (bufindr < bufindw) {
-
-    } else {
-//        for (uint8_t i = 0; i < BUFSIZE; ++ i)
-//            SERIAL_ECHO(cmdbuffer[(i+bufindw)%BUFSIZE]);
-    }
-    SERIAL_ECHOLNPGM("End of the buffer.");
 }
+#endif /* CMDBUFFER_DEBUG */
 
 //adds an command to the main command buffer
 //thats really done in a non-safe way.
@@ -684,6 +733,9 @@ void enquecommand(const char *cmd, bool from_progmem)
         if (bufindw == sizeof(cmdbuffer))
             bufindw = 0;
         ++ buflen;
+#ifdef CMDBUFFER_DEBUG
+        cmdqueue_dump_to_serial();
+#endif /* CMDBUFFER_DEBUG */
     } else {
         SERIAL_ECHO_START;
         SERIAL_ECHORPGM(MSG_Enqueing);
@@ -692,7 +744,9 @@ void enquecommand(const char *cmd, bool from_progmem)
         else
             SERIAL_ECHO(cmd);
         SERIAL_ECHOLNPGM("\" failed: Buffer full!");
+#ifdef CMDBUFFER_DEBUG
         cmdqueue_dump_to_serial();
+#endif /* CMDBUFFER_DEBUG */
     }
 }
 
@@ -706,10 +760,14 @@ void enquecommand_front(const char *cmd, bool from_progmem)
             strcpy_P(cmdbuffer + bufindr + 1, cmd);
         else
             strcpy(cmdbuffer + bufindr + 1, cmd);
+        ++ buflen;
         SERIAL_ECHO_START;
         SERIAL_ECHOPGM("Enqueing to the front: \"");
         SERIAL_ECHO(cmdbuffer + bufindr + 1);
         SERIAL_ECHOLNPGM("\"");
+#ifdef CMDBUFFER_DEBUG
+        cmdqueue_dump_to_serial();
+#endif /* CMDBUFFER_DEBUG */
     } else {
         SERIAL_ECHO_START;
         SERIAL_ECHOPGM("Enqueing to the front: \"");
@@ -718,10 +776,20 @@ void enquecommand_front(const char *cmd, bool from_progmem)
         else
             SERIAL_ECHO(cmd);
         SERIAL_ECHOLNPGM("\" failed: Buffer full!");
+#ifdef CMDBUFFER_DEBUG
         cmdqueue_dump_to_serial();
+#endif /* CMDBUFFER_DEBUG */
     }
 }
 
+// Mark the command at the top of the command queue as new.
+// Therefore it will not be removed from the queue.
+void repeatcommand_front()
+{
+    cmdbuffer_front_already_processed = true;
+} 
+
+
 void setup_killpin()
 {
   #if defined(KILL_PIN) && KILL_PIN > -1
@@ -1051,14 +1119,21 @@ void get_command()
         
         // Store the current line into buffer, move to the next line.
         cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_USB;
+#ifdef CMDBUFFER_DEBUG
         SERIAL_ECHO_START;
         SERIAL_ECHOPGM("Storing a command line to buffer: ");
         SERIAL_ECHO(cmdbuffer+bufindw+1);
         SERIAL_ECHOLNPGM("");
+#endif /* CMDBUFFER_DEBUG */
         bufindw += strlen(cmdbuffer+bufindw+1) + 2;
         if (bufindw == sizeof(cmdbuffer))
             bufindw = 0;
         ++ buflen;
+#ifdef CMDBUFFER_DEBUG
+        SERIAL_ECHOPGM("Number of commands in the buffer: ");
+        SERIAL_ECHO(buflen);
+        SERIAL_ECHOLNPGM("");
+#endif /* CMDBUFFER_DEBUG */
       } // end of 'not comment mode'
       serial_count = 0; //clear buffer
       // Don't call cmdqueue_could_enqueue_back if there are no characters waiting
@@ -1434,6 +1509,15 @@ static void homeaxis(int axis) {
   }
 }
 
+void home_xy()
+{
+    set_destination_to_current();
+    homeaxis(X_AXIS);
+    homeaxis(Y_AXIS);
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+    endstops_hit_on_purpose();
+}
+
 void refresh_cmd_timeout(void)
 {
   previous_millis_cmd = millis();
@@ -1488,6 +1572,15 @@ void process_commands()
   #ifdef FILAMENT_RUNOUT_SUPPORT
     SET_INPUT(FR_SENS);
   #endif
+
+#ifdef CMDBUFFER_DEBUG
+  SERIAL_ECHOPGM("Processing a GCODE command: ");
+  SERIAL_ECHO(cmdbuffer+bufindr+1);
+  SERIAL_ECHOLNPGM("");
+  SERIAL_ECHOPGM("In cmdqueue: ");
+  SERIAL_ECHO(buflen);
+  SERIAL_ECHOLNPGM("");
+#endif /* CMDBUFFER_DEBUG */
   
   unsigned long codenum; //throw away variable
   char *starpos = NULL;
@@ -1903,12 +1996,9 @@ void process_commands()
       			  } 
               // 1st mesh bed leveling measurement point, corrected.
               world2machine_initialize();
-              current_position[X_AXIS] = world2machine_rotation_and_skew[0][0] * pgm_read_float(bed_ref_points) + world2machine_rotation_and_skew[0][1] * pgm_read_float(bed_ref_points+1) + world2machine_shift[0];
-              current_position[Y_AXIS] = world2machine_rotation_and_skew[1][0] * pgm_read_float(bed_ref_points) + world2machine_rotation_and_skew[1][1] * pgm_read_float(bed_ref_points+1) + world2machine_shift[1];
+              destination[X_AXIS] = world2machine_rotation_and_skew[0][0] * pgm_read_float(bed_ref_points) + world2machine_rotation_and_skew[0][1] * pgm_read_float(bed_ref_points+1) + world2machine_shift[0];
+              destination[Y_AXIS] = world2machine_rotation_and_skew[1][0] * pgm_read_float(bed_ref_points) + world2machine_rotation_and_skew[1][1] * pgm_read_float(bed_ref_points+1) + world2machine_shift[1];
               world2machine_reset();
-//              mbl.get_meas_xy(0, 0, destination[X_AXIS], destination[Y_AXIS], false);
-              // destination[X_AXIS] = MESH_MIN_X - X_PROBE_OFFSET_FROM_EXTRUDER;
-              // destination[Y_AXIS] = MESH_MIN_Y - Y_PROBE_OFFSET_FROM_EXTRUDER;
               destination[Z_AXIS] = MESH_HOME_Z_SEARCH;    // Set destination away from bed
               feedrate = homing_feedrate[Z_AXIS]/10;
               current_position[Z_AXIS] = 0;
@@ -2225,7 +2315,7 @@ void process_commands()
                 // We don't know where we are! HOME!
                 // Push the commands to the front of the message queue in the reverse order!
                 // There shall be always enough space reserved for these commands.
-                enquecommand_front_P((PSTR("G80")));
+                repeatcommand_front(); // repeat G80 with all its parameters
                 enquecommand_front_P((PSTR("G28 W0")));
                 break;
             }
@@ -2645,79 +2735,75 @@ void process_commands()
       }
      break;
 
-    case 45:
+    case 44: // M45: Reset the bed skew and offset calibration.
+        // Reset the skew and offset in both RAM and EEPROM.
         reset_bed_offset_and_skew();
         world2machine_reset();
+        // Wait for the motors to stop and update the current position with the absolute values.
+        st_synchronize();
+        current_position[X_AXIS] = st_get_position_mm(X_AXIS);
+        current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
         break;
 
-    case 46: // M46: mesh_bed_calibration with manual Z up
-        {
-            // Firstly check if we know where we are
-            if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ){
-                // We don't know where we are! HOME!
-                // Push the commands to the front of the message queue in the reverse order!
-                // There shall be always enough space reserved for these commands.
-                enquecommand_front_P((PSTR("M46")));
-                enquecommand_front_P((PSTR("G28 X Y")));
-                break;
+    case 45: // M46: bed skew and offset with manual Z up
+    {
+        // 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();
+        // Let the user move the Z axes up to the end stoppers.
+        if (lcd_calibrate_z_end_stop_manual()) {
+            // 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 = find_bed_offset_and_skew(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(false);
-            if (lcd_calibrate_z_end_stop_manual()) {
-                mbl.reset();
-                setup_for_endstop_move();
-                find_bed_offset_and_skew();
-                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();
+            // Second half: The fine adjustment.
+            // Let the planner use the uncorrected coordinates.
+            mbl.reset();
+            world2machine_reset();
+            // Home in the XY plane.
+            setup_for_endstop_move();
+            home_xy();
+            success = improve_bed_offset_and_skew(1, 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();
+            if (success) {
+                // Mesh bed leveling.
                 // Push the commands to the front of the message queue in the reverse order!
                 // There shall be always enough space reserved for these commands.
-                enquecommand_front_P((PSTR("M47")));
-                enquecommand_front_P((PSTR("G28 X Y")));
-              } else {
-                // User canceled the operation. Give up.
-                lcd_update_enable(true);
-                lcd_implementation_clear();
-                // lcd_return_to_status();
-                lcd_update();
+                enquecommand_front_P((PSTR("G80")));
             }
-        }
-    break;
 
-    case 47:
-    {
-          // Firstly check if we know where we are
-          if ( !( axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) ) {
-              // We don't know where we are! HOME!
-              // Push the commands to the front of the message queue in the reverse order!
-              // There shall be always enough space reserved for these commands.
-              enquecommand_front_P((PSTR("M47")));
-              enquecommand_front_P((PSTR("G28 X Y")));
-              break;
-          }
-          lcd_update_enable(false);
-          mbl.reset();
-          setup_for_endstop_move();
-          bool success = improve_bed_offset_and_skew(1);
-          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_update();
-          if (success) {
-              // Mesh bed leveling.
-              // Push the commands to the front of the message queue in the reverse order!
-              // There shall be always enough space reserved for these commands.
-              enquecommand_front_P((PSTR("G80")));
-          }
-          break;
+            lcd_update_enable(true);
+            lcd_implementation_clear();
+            // lcd_return_to_status();
+            lcd_update();
+        }
+        break;
     }
 
-    case 48:
+    case 47:
         lcd_diag_show_end_stops();
         break;
 

Firmware/mesh_bed_calibration.cpp

@@ -50,48 +50,51 @@ bool calculate_machine_skew_and_offset_LS(
     // Resulting correction matrix.
     float        *vec_x,
     float        *vec_y,
-    float        *cntr
+    float        *cntr,
     // Temporary values, 49-18-(2*3)=25 floats
 //    , float *temp
+    int8_t        verbosity_level
     )
 {
-    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("");
+    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("");
 
-    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("Y vector, initial: ");
+        MYSERIAL.print(vec_y[0], 5);
         SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(measured_pts[i*2+1], 5);
-        SERIAL_ECHOPGM("); target: (");
-        MYSERIAL.print(pgm_read_float(true_pts+i*2  ), 5);
+        MYSERIAL.print(vec_y[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("center, initial: ");
+        MYSERIAL.print(cntr[0], 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);
+        MYSERIAL.print(cntr[1], 5);
         SERIAL_ECHOLNPGM("");
-    }
-    delay_keep_alive(100);
 
+        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);
+    }
 
     {
         // Create covariance matrix for A, collect the right hand side b.
@@ -165,47 +168,50 @@ bool calculate_machine_skew_and_offset_LS(
         cntr[1] = x[2];
     }
 
-    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);
+    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(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)));
+        MYSERIAL.print(vec_x[1], 5);
         SERIAL_ECHOLNPGM("");
-    }
 
-    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("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_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("");
+        }
+    }
 
 #if 0
     // Normalize the vectors. We expect, that the machine axes may be skewed a bit, but the distances are correct.
@@ -273,47 +279,53 @@ bool calculate_machine_skew_and_offset_LS(
         cntr[1] = cntrInv[1];
     }
 
-    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("");
-
-    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);
+    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(measured_pts[i*2+1], 5);
-        SERIAL_ECHOPGM("); measured-corrected: (");
-        MYSERIAL.print(x, 5);
+        MYSERIAL.print(vec_x[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("Y vector, adjusted: ");
+        MYSERIAL.print(vec_y[0], 5);
         SERIAL_ECHOPGM(", ");
-        MYSERIAL.print(y, 5);
-        SERIAL_ECHOPGM("); target: (");
-        MYSERIAL.print(pgm_read_float(true_pts+i*2  ), 5);
+        MYSERIAL.print(vec_y[1], 5);
+        SERIAL_ECHOLNPGM("");
+
+        SERIAL_ECHOPGM("center, adjusted: ");
+        MYSERIAL.print(cntr[0], 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)));
+        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);
     }
-    delay_keep_alive(100);
 
     return true;
 }
@@ -362,8 +374,10 @@ void world2machine_initialize()
     };
 
     bool reset = false;
-    if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y))
+    if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y)) {
+        SERIAL_ECHOLNPGM("Undefined bed correction matrix.");
         reset = true;
+    }
     else {
         // Length of the vec_x shall be close to unity.
         float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
@@ -392,7 +406,7 @@ void world2machine_initialize()
     }
 
     if (reset) {
-        SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
+        // SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
         reset_bed_offset_and_skew();
         world2machine_reset();
     } else {
@@ -864,7 +878,7 @@ canceled:
 
 #define MESH_BED_CALIBRATION_SHOW_LCD
 
-bool find_bed_offset_and_skew()
+bool find_bed_offset_and_skew(int8_t verbosity_level)
 {
     // Reusing the z_values memory for the measurement cache.
     // 7x7=49 floats, good for 16 (x,y,z) vectors.
@@ -874,9 +888,6 @@ bool find_bed_offset_and_skew()
     float *cntr  = vec_y + 2;
     memset(pts, 0, sizeof(float) * 7 * 7);
 
-    // Let the planner use the uncorrected coordinates.
-    world2machine_reset();
-
 #ifdef MESH_BED_CALIBRATION_SHOW_LCD
     lcd_implementation_clear();
     lcd_print_at_PGM(0, 0, MSG_FIND_BED_OFFSET_AND_SKEW_LINE1);
@@ -908,71 +919,7 @@ bool find_bed_offset_and_skew()
         cntr[1] += pt[1];
     }
 
-#if 0
-    // Average the X and Y vectors. They may not be perpendicular, if the printer is built incorrectly.
-    {
-        float  len;
-        // Average the center point.
-        cntr[0] *= 1.f/4.f;
-        cntr[1] *= 1.f/4.f;
-        cntr[2] *= 1.f/4.f;
-        // Average the X vector.
-        vec_x[0] = (pts[2 * 1 + 0] - pts[2 * 3 + 0]) / 2.f;
-        vec_x[1] = (pts[2 * 1 + 1] - pts[2 * 3 + 1]) / 2.f;
-        len = sqrt(vec_x[0]*vec_x[0] + vec_x[1]*vec_x[1]);
-        if (0) {
-        // if (len < MEAS_NUM_X_DIST) {
-            // Scale the vector up to MEAS_NUM_X_DIST lenght.
-            float factor = MEAS_NUM_X_DIST / len;
-            vec_x[0] *= factor;
-            vec_x[0] *= factor;
-        } else {
-            // The vector is longer than MEAS_NUM_X_DIST. The X/Y axes are skewed.
-            // Verify the maximum skew?
-        }
-        // Average the Y vector.
-        vec_y[0] = (pts[2 * 2 + 0] - pts[2 * 0 + 0]) / 2.f;
-        vec_y[1] = (pts[2 * 2 + 1] - pts[2 * 0 + 1]) / 2.f;
-        len = sqrt(vec_y[0]*vec_y[0] + vec_y[1]*vec_y[1]);
-        if (0) {
-        // if (len < MEAS_NUM_Y_DIST) {
-            // Scale the vector up to MEAS_NUM_X_DIST lenght.
-            float factor = MEAS_NUM_Y_DIST / len;
-            vec_y[1] *= factor;
-            vec_y[1] *= factor;
-        } else {
-            // The vector is longer than MEAS_NUM_X_DIST. The X/Y axes are skewed.
-            // Verify the maximum skew?
-        }
-
-        // Fearlessly store the calibration values into the eeprom.
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
-        eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
-
-#if 0
-        SERIAL_ECHOLN("Calibration done.");
-        SERIAL_ECHO("Center: ");
-        SERIAL_ECHO(cntr[0]);
-        SERIAL_ECHO(",");
-        SERIAL_ECHO(cntr[1]);
-        SERIAL_ECHO(", x: ");
-        SERIAL_ECHO(vec_x[0]);
-        SERIAL_ECHO(",");
-        SERIAL_ECHO(vec_x[1]);
-        SERIAL_ECHO(", y: ");
-        SERIAL_ECHO(vec_y[0]);
-        SERIAL_ECHO(",");
-        SERIAL_ECHO(vec_y[1]);
-        SERIAL_ECHOLN("");
-#endif
-    }
-#endif
-
-    calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr);
+    calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
     world2machine_rotation_and_skew[0][0] = vec_x[0];
     world2machine_rotation_and_skew[1][0] = vec_x[1];
     world2machine_rotation_and_skew[0][1] = vec_y[0];
@@ -989,10 +936,12 @@ bool find_bed_offset_and_skew()
     eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
 #endif
 
+    // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
+    world2machine_update_current();
     return true;
 }
 
-bool improve_bed_offset_and_skew(int8_t method)
+bool improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level)
 {
     // Reusing the z_values memory for the measurement cache.
     // 7x7=49 floats, good for 16 (x,y,z) vectors.
@@ -1003,6 +952,7 @@ bool improve_bed_offset_and_skew(int8_t method)
     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];
@@ -1043,9 +993,11 @@ bool improve_bed_offset_and_skew(int8_t method)
             current_position[Y_AXIS] = Y_MIN_POS;
         go_to_current(homing_feedrate[X_AXIS]/60);
         // Find its Z position by running the normal vertical search.
-//        delay_keep_alive(3000);
+        if (verbosity_level >= 10)
+            delay_keep_alive(3000);
         find_bed_induction_sensor_point_z();
-//        delay_keep_alive(3000);
+        if (verbosity_level >= 10)
+            delay_keep_alive(3000);
         // Improve the point position by searching its center in a current plane.
         int8_t n_errors = 3;
         for (int8_t iter = 0; iter < 8; ) {
@@ -1073,49 +1025,30 @@ bool improve_bed_offset_and_skew(int8_t method)
                 go_to_current(homing_feedrate[Z_AXIS]);
             }
         }
-        delay_keep_alive(3000);
+        if (verbosity_level >= 10)
+            delay_keep_alive(3000);
     }
 
     // Average the last 4 measurements.
     for (int8_t i = 0; i < 18; ++ i)
         pts[i] *= (1.f/4.f);
 
-// Test the positions. Are the positions reproducible?
-#if 1
     enable_endstops(false);
     enable_z_endstop(false);
-    for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
-        // Go to the measurement point.
-        // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-        current_position[X_AXIS] = pts[mesh_point*2];
-        current_position[Y_AXIS] = pts[mesh_point*2+1];
-        go_to_current(homing_feedrate[X_AXIS]/60);
-        delay_keep_alive(3000);
+
+    if (verbosity_level >= 10) {
+        // Test the positions. Are the positions reproducible?
+        for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+            // Go to the measurement point.
+            // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+            current_position[X_AXIS] = pts[mesh_point*2];
+            current_position[Y_AXIS] = pts[mesh_point*2+1];
+            go_to_current(homing_feedrate[X_AXIS]/60);
+            delay_keep_alive(3000);
+        }
     }
-#endif
 
-#if 0
-    // Average the X and Y vectors. They may not be perpendicular, if the printer is built incorrectly.
-    // Average the center point.
-    cntr[0] *= 1.f/9.f;
-    cntr[1] *= 1.f/9.f;
-    // Average the X vector.
-    vec_x[0] = (pts[2 * 2 + 0] - pts[2 * 0 + 0] + pts[2 * 5 + 0] - pts[2 * 3 + 0] + pts[2 * 8 + 0] - pts[2 * 6 + 0]) / 6.f;
-    vec_x[1] = (pts[2 * 2 + 1] - pts[2 * 0 + 1] + pts[2 * 5 + 1] - pts[2 * 3 + 1] + pts[2 * 8 + 1] - pts[2 * 6 + 1]) / 6.f;
-    // Average the Y vector.
-    vec_y[0] = (pts[2 * 6 + 0] - pts[2 * 0 + 0] + pts[2 * 7 + 0] - pts[2 * 1 + 0] + pts[2 * 8 + 0] - pts[2 * 2 + 0]) / 6.f;
-    vec_y[1] = (pts[2 * 6 + 1] - pts[2 * 0 + 1] + pts[2 * 7 + 1] - pts[2 * 1 + 1] + pts[2 * 8 + 1] - pts[2 * 2 + 1]) / 6.f;
-#if 1
-    // Fearlessly store the calibration values into the eeprom.
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
-    eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
-#endif
-#else
-    calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr);
+    calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr, verbosity_level);
     world2machine_rotation_and_skew[0][0] = vec_x[0];
     world2machine_rotation_and_skew[1][0] = vec_x[1];
     world2machine_rotation_and_skew[0][1] = vec_y[0];
@@ -1131,40 +1064,25 @@ bool improve_bed_offset_and_skew(int8_t method)
     eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
     eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
 #endif
-#endif
 
-// Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
-#if 1
+    // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
+    world2machine_update_current();
+
     enable_endstops(false);
     enable_z_endstop(false);
-    delay_keep_alive(3000);
-    for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
-        // Go to the measurement point.
-        // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
-        current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
-        current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
-        go_to_current(homing_feedrate[X_AXIS]/60);
+
+    if (verbosity_level >= 10) {
+        // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
         delay_keep_alive(3000);
+        for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
+            // Go to the measurement point.
+            // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
+            current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
+            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);
+        }
     }
-#endif
-
-#if 0
-    // and let us know the result.
-    SERIAL_ECHOLN("Calibration done.");
-    SERIAL_ECHO("Center: ");
-    SERIAL_ECHO(cntr[0]);
-    SERIAL_ECHO(",");
-    SERIAL_ECHO(cntr[1]);
-    SERIAL_ECHO(", x: ");
-    SERIAL_ECHO(vec_x[0]);
-    SERIAL_ECHO(",");
-    SERIAL_ECHO(vec_x[1]);
-    SERIAL_ECHO(", y: ");
-    SERIAL_ECHO(vec_y[0]);
-    SERIAL_ECHO(",");
-    SERIAL_ECHO(vec_y[1]);
-    SERIAL_ECHOLN("");
-#endif
 
     enable_endstops(endstops_enabled);
     enable_z_endstop(endstop_z_enabled);

Firmware/mesh_bed_calibration.h

@@ -27,8 +27,8 @@ extern void world2machine_update_current();
 extern void find_bed_induction_sensor_point_z();
 extern bool find_bed_induction_sensor_point_xy();
 
-extern bool find_bed_offset_and_skew();
-extern bool improve_bed_offset_and_skew(int8_t method);
+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();
 
 #endif /* MESH_BED_CALIBRATION_H */

Firmware/ultralcd.cpp

@@ -1207,6 +1207,10 @@ bool lcd_calibrate_z_end_stop_manual()
     int8_t        cursor_pos;
     int8_t        enc_dif = 0;
 
+    // Don't know where we are. Let's claim we are Z=0, so the soft end stops will not be triggered when moving up.
+    current_position[Z_AXIS] = 0;
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+
     // Until confirmed by the confirmation dialog.
     for (;;) {
         previous_millis_cmd = millis();
@@ -1236,6 +1240,11 @@ bool lcd_calibrate_z_end_stop_manual()
                 current_position[Z_AXIS] += fabs(encoderPosition);
                 encoderPosition = 0;
                 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], manual_feedrate[Z_AXIS] / 60, active_extruder);
+                // Wait for the motors to stop.
+                st_synchronize();
+                // Claim we are at Z=0, so the soft end stop will not trigger.
+                current_position[Z_AXIS] = 0;
+                plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
             }
             if (lcd_clicked()) {
                 // Wait until the Z up movement is finished.
@@ -1307,6 +1316,22 @@ canceled:
     return false;
 }
 
+static void lcd_show_end_stops() {
+    lcd.setCursor(0, 0);
+    lcd_printPGM((PSTR("End stops diag")));
+    lcd.setCursor(0, 1);
+    lcd_printPGM((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("X1")) : (PSTR("X0")));
+    lcd.setCursor(0, 2);
+    lcd_printPGM((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("Y1")) : (PSTR("Y0")));
+    lcd.setCursor(0, 3);
+    lcd_printPGM((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("Z1")) : (PSTR("Z0")));
+}
+
+static void menu_show_end_stops() {
+    lcd_show_end_stops();
+    if (LCD_CLICKED) lcd_goto_menu(lcd_settings_menu);
+}
+
 // Lets the user move the Z carriage up to the end stoppers.
 // When done, it sets the current Z to Z_MAX_POS and returns true.
 // Otherwise the Z calibration is not changed and false is returned.
@@ -1314,17 +1339,10 @@ void lcd_diag_show_end_stops()
 {
     int enc_dif = encoderDiff;
     lcd_implementation_clear();
-    lcd.setCursor(0, 0);
-    lcd_printPGM((PSTR("End stops diag")));
     for (;;) {
         manage_heater();
         manage_inactivity(true);
-        lcd.setCursor(0, 1);
-        lcd_printPGM((READ(X_MIN_PIN) ^ X_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("X1")) : (PSTR("X0")));
-        lcd.setCursor(0, 2);
-        lcd_printPGM((READ(Y_MIN_PIN) ^ Y_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("Y1")) : (PSTR("Y0")));
-        lcd.setCursor(0, 3);
-        lcd_printPGM((READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING == 1) ? (PSTR("Z1")) : (PSTR("Z0")));
+        lcd_show_end_stops();
         if (lcd_clicked()) {
             while (lcd_clicked()) ;
             delay(10);
@@ -1529,12 +1547,6 @@ void lcd_mesh_bedleveling()
 }
 
 void lcd_mesh_calibration()
-{
-  enquecommand_P(PSTR("M46"));
-  lcd_return_to_status();
-}
-
-void lcd_mesh_calibration_reset()
 {
   enquecommand_P(PSTR("M45"));
   lcd_return_to_status();
@@ -1584,8 +1596,9 @@ static void lcd_settings_menu()
 	if (!isPrintPaused)
 	{
 		MENU_ITEM(submenu, MSG_SELFTEST, lcd_selftest);
+    MENU_ITEM(submenu, PSTR("Show end stops"), menu_show_end_stops);
     MENU_ITEM(submenu, MSG_CALIBRATE_BED, lcd_mesh_calibration);
-    MENU_ITEM(submenu, MSG_CALIBRATE_BED_RESET, lcd_mesh_calibration_reset);
+    MENU_ITEM(gcode, MSG_CALIBRATE_BED_RESET, PSTR("M44"));
 	}
   
 	END_MENU();