Merge pull request #6152 from thinkyhead/rc_cleanup_6150_etc

UBL cleanup, optimization

Marlin/G26_Mesh_Validation_Tool.cpp

@@ -35,7 +35,6 @@
   #include "temperature.h"
   #include "UBL.h"
   #include "ultralcd.h"
-//#include <avr/pgmspace.h>
 
   #define EXTRUSION_MULTIPLIER 1.0    // This is too much clutter for the main Configuration.h file  But
   #define RETRACTION_MULTIPLIER 1.0   // some user have expressed an interest in being able to customize
@@ -177,7 +176,7 @@
 
   /**
    * G26: Mesh Validation Pattern generation.
-   * 
+   *
    * Used to interactively edit UBL's Mesh by placing the
    * nozzle in a problem area and doing a G29 P4 R command.
    */
@@ -234,7 +233,7 @@
     move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], ooze_amount);
 
     ubl_has_control_of_lcd_panel = true; // Take control of the LCD Panel!
-//  debug_current_and_destination((char*)"Starting G26 Mesh Validation Pattern.");
+    //debug_current_and_destination((char*)"Starting G26 Mesh Validation Pattern.");
 
     /**
      * Declare and generate a sin() & cos() table to be used during the circle drawing.  This will lighten
@@ -250,16 +249,17 @@
 
       if (ubl_lcd_clicked()) {              // Check if the user wants to stop the Mesh Validation
         #if ENABLED(ULTRA_LCD)
-          lcd_setstatuspgm(PSTR("Mesh Validation Stopped."), (uint8_t) 99);
+          lcd_setstatuspgm(PSTR("Mesh Validation Stopped."), 99);
           lcd_quick_feedback();
         #endif
         while (!ubl_lcd_clicked()) {         // Wait until the user is done pressing the
           idle();                            // Encoder Wheel if that is why we are leaving
-          lcd_setstatuspgm(PSTR(" "), (uint8_t) 99);
+          lcd_reset_alert_level();
+          lcd_setstatuspgm(PSTR(""));
         }
-        while ( ubl_lcd_clicked()) {         // Wait until the user is done pressing the
+        while (ubl_lcd_clicked()) {          // Wait until the user is done pressing the
           idle();                            // Encoder Wheel if that is why we are leaving
-          lcd_setstatuspgm(PSTR("Unpress Wheel "), (uint8_t) 99);
+          lcd_setstatuspgm(PSTR("Unpress Wheel"), 99);
         }
         goto LEAVE;
       }
@@ -276,13 +276,16 @@
         // Let's do a couple of quick sanity checks.  We can pull this code out later if we never see it catch a problem
         #ifdef DELTA
           if (HYPOT2(circle_x, circle_y) > sq(DELTA_PRINTABLE_RADIUS)) {
-            SERIAL_PROTOCOLLNPGM("?Error: Attempt to print outside of DELTA_PRINTABLE_RADIUS.");
+            SERIAL_ERROR_START;
+            SERIAL_ERRORLNPGM("Attempt to print outside of DELTA_PRINTABLE_RADIUS.");
             goto LEAVE;
           }
         #endif
 
-        if (circle_x < X_MIN_POS || circle_x > X_MAX_POS || circle_y < Y_MIN_POS || circle_y > Y_MAX_POS) {
-          SERIAL_PROTOCOLLNPGM("?Error: Attempt to print off the bed.");
+        // TODO: Change this to use `position_is_reachable`
+        if (circle_x < (X_MIN_POS) || circle_x > (X_MAX_POS) || circle_y < (Y_MIN_POS) || circle_y > (Y_MAX_POS)) {
+          SERIAL_ERROR_START;
+          SERIAL_ERRORLNPGM("Attempt to print off the bed.");
           goto LEAVE;
         }
 
@@ -290,11 +293,10 @@
         yi = location.y_index;
 
         if (g26_debug_flag) {
-          SERIAL_ECHOPGM("   Doing circle at: (xi=");
-          SERIAL_ECHO(xi);
-          SERIAL_ECHOPGM(", yi=");
-          SERIAL_ECHO(yi);
-          SERIAL_ECHOLNPGM(")");
+          SERIAL_ECHOPAIR("   Doing circle at: (xi=", xi);
+          SERIAL_ECHOPAIR(", yi=", yi);
+          SERIAL_CHAR(')');
+          SERIAL_EOL;
         }
 
         start_angle = 0.0;    // assume it is going to be a full circle
@@ -344,56 +346,53 @@
             ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1);
           #endif
 
-//          if (g26_debug_flag) {
-//            char ccc, *cptr, seg_msg[50], seg_num[10];
-//            strcpy(seg_msg, "   segment: ");
-//            strcpy(seg_num, "    \n");
-//            cptr = (char*) "01234567890ABCDEF????????";
-//            ccc = cptr[tmp_div_30];
-//            seg_num[1] = ccc;
-//            strcat(seg_msg, seg_num);
-//            debug_current_and_destination(seg_msg);
-//          }
+          //if (g26_debug_flag) {
+          //  char ccc, *cptr, seg_msg[50], seg_num[10];
+          //  strcpy(seg_msg, "   segment: ");
+          //  strcpy(seg_num, "    \n");
+          //  cptr = (char*) "01234567890ABCDEF????????";
+          //  ccc = cptr[tmp_div_30];
+          //  seg_num[1] = ccc;
+          //  strcat(seg_msg, seg_num);
+          //  debug_current_and_destination(seg_msg);
+          //}
 
           print_line_from_here_to_there(x, y, layer_height, xe, ye, layer_height);
 
         }
-//      lcd_init_counter++;
-//      if (lcd_init_counter > 10) {
-//        lcd_init_counter = 0;
-//        lcd_init(); // Some people's LCD Displays are locking up.  This might help them
-//        ubl_has_control_of_lcd_panel = true;     // Make sure UBL still is controlling the LCD Panel
-//      }
-
-    // If the end point of the line is closer to the nozzle, we are going to
-//      debug_current_and_destination((char*)"Looking for lines to connect.");
+        //lcd_init_counter++;
+        //if (lcd_init_counter > 10) {
+        //  lcd_init_counter = 0;
+        //  lcd_init(); // Some people's LCD Displays are locking up.  This might help them
+        //  ubl_has_control_of_lcd_panel = true;     // Make sure UBL still is controlling the LCD Panel
+        //}
+
+        //debug_current_and_destination((char*)"Looking for lines to connect.");
         look_for_lines_to_connect();
-//      debug_current_and_destination((char*)"Done with line connect.");
+        //debug_current_and_destination((char*)"Done with line connect.");
       }
 
-//    debug_current_and_destination((char*)"Done with current circle.");
-
-    // If the end point of the line is closer to the nozzle, we are going to
-
+      //debug_current_and_destination((char*)"Done with current circle.");
     }
     while (location.x_index >= 0 && location.y_index >= 0);
 
     LEAVE:
-    lcd_setstatuspgm(PSTR("Leaving G26 "), (uint8_t) 99);
+    lcd_reset_alert_level();
+    lcd_setstatuspgm(PSTR("Leaving G26"));
 
     retract_filament();
-    destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;                             // Raise the nozzle
+    destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;
 
-//  debug_current_and_destination((char*)"ready to do Z-Raise.");
-    move_to( destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0);   // Raise the nozzle
-//  debug_current_and_destination((char*)"done doing Z-Raise.");
+    //debug_current_and_destination((char*)"ready to do Z-Raise.");
+    move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Raise the nozzle
+    //debug_current_and_destination((char*)"done doing Z-Raise.");
 
-    destination[X_AXIS] = x_pos;                                                  // Move back to the starting position
+    destination[X_AXIS] = x_pos;                                               // Move back to the starting position
     destination[Y_AXIS] = y_pos;
-    destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;                             // Keep the nozzle where it is
+    //destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;                        // Keep the nozzle where it is
 
     move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Move back to the starting position
-//  debug_current_and_destination((char*)"done doing X/Y move.");
+    //debug_current_and_destination((char*)"done doing X/Y move.");
 
     ubl_has_control_of_lcd_panel = false;     // Give back control of the LCD Panel!
 
@@ -481,16 +480,13 @@
               ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
 
               if (g26_debug_flag) {
-                SERIAL_ECHOPGM(" Connecting with horizontal line (sx=");
-                SERIAL_ECHO(sx);
-                SERIAL_ECHOPGM(", sy=");
-                SERIAL_ECHO(sy);
-                SERIAL_ECHOPGM(") -> (ex=");
-                SERIAL_ECHO(ex);
-                SERIAL_ECHOPGM(", ey=");
-                SERIAL_ECHO(ey);
-                SERIAL_ECHOLNPGM(")");
-//              debug_current_and_destination((char*)"Connecting horizontal line.");
+                SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
+                SERIAL_ECHOPAIR(", sy=", sy);
+                SERIAL_ECHOPAIR(") -> (ex=", ex);
+                SERIAL_ECHOPAIR(", ey=", ey);
+                SERIAL_CHAR(')');
+                SERIAL_EOL;
+                //debug_current_and_destination((char*)"Connecting horizontal line.");
               }
 
               print_line_from_here_to_there(sx, sy, layer_height, ex, ey, layer_height);
@@ -521,15 +517,12 @@
                 ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
 
                 if (g26_debug_flag) {
-                  SERIAL_ECHOPGM(" Connecting with vertical line (sx=");
-                  SERIAL_ECHO(sx);
-                  SERIAL_ECHOPGM(", sy=");
-                  SERIAL_ECHO(sy);
-                  SERIAL_ECHOPGM(") -> (ex=");
-                  SERIAL_ECHO(ex);
-                  SERIAL_ECHOPGM(", ey=");
-                  SERIAL_ECHO(ey);
-                  SERIAL_ECHOLNPGM(")");
+                  SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx);
+                  SERIAL_ECHOPAIR(", sy=", sy);
+                  SERIAL_ECHOPAIR(") -> (ex=", ex);
+                  SERIAL_ECHOPAIR(", ey=", ey);
+                  SERIAL_CHAR(')');
+                  SERIAL_EOL;
                   debug_current_and_destination((char*)"Connecting vertical line.");
                 }
                 print_line_from_here_to_there(sx, sy, layer_height, ex, ey, layer_height);
@@ -548,16 +541,10 @@
 
     bool has_xy_component = (x != current_position[X_AXIS] || y != current_position[Y_AXIS]); // Check if X or Y is involved in the movement.
 
-//  if (g26_debug_flag) {
-//    SERIAL_ECHOPAIR("in move_to()  has_xy_component:", (int)has_xy_component);
-//    SERIAL_EOL;
-//  }
+    //if (g26_debug_flag) SERIAL_ECHOLNPAIR("in move_to()  has_xy_component:", (int)has_xy_component);
 
     if (z != last_z) {
-//    if (g26_debug_flag) {
-//      SERIAL_ECHOPAIR("in move_to()  changing Z to ", (int)z);
-//      SERIAL_EOL;
-//    }
+      //if (g26_debug_flag) SERIAL_ECHOLNPAIR("in move_to()  changing Z to ", (int)z);
 
       last_z = z;
       feed_value = planner.max_feedrate_mm_s[Z_AXIS]/(3.0);  // Base the feed rate off of the configured Z_AXIS feed rate
@@ -572,30 +559,24 @@
       stepper.synchronize();
       set_destination_to_current();
 
-//    if (g26_debug_flag)
-//      debug_current_and_destination((char*)" in move_to() done with Z move");
+      //if (g26_debug_flag) debug_current_and_destination((char*)" in move_to() done with Z move");
     }
 
     // Check if X or Y is involved in the movement.
     // Yes: a 'normal' movement. No: a retract() or un_retract()
     feed_value = has_xy_component ? PLANNER_XY_FEEDRATE() / 10.0 : planner.max_feedrate_mm_s[E_AXIS] / 1.5;
 
-    if (g26_debug_flag) {
-      SERIAL_ECHOPAIR("in move_to() feed_value for XY:", feed_value);
-      SERIAL_EOL;
-    }
+    if (g26_debug_flag) SERIAL_ECHOLNPAIR("in move_to() feed_value for XY:", feed_value);
 
     destination[X_AXIS] = x;
     destination[Y_AXIS] = y;
     destination[E_AXIS] += e_delta;
 
-//  if (g26_debug_flag)
-//    debug_current_and_destination((char*)" in move_to() doing last move");
+    //if (g26_debug_flag) debug_current_and_destination((char*)" in move_to() doing last move");
 
     ubl_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feed_value, 0);
 
-//  if (g26_debug_flag)
-//    debug_current_and_destination((char*)" in move_to() after last move");
+    //if (g26_debug_flag) debug_current_and_destination((char*)" in move_to() after last move");
 
     stepper.synchronize();
     set_destination_to_current();
@@ -605,9 +586,9 @@
   void retract_filament() {
     if (!g26_retracted) { // Only retract if we are not already retracted!
       g26_retracted = true;
-//    if (g26_debug_flag) SERIAL_ECHOLNPGM(" Decided to do retract.");
+      //if (g26_debug_flag) SERIAL_ECHOLNPGM(" Decided to do retract.");
       move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], -1.0 * retraction_multiplier);
-//    if (g26_debug_flag) SERIAL_ECHOLNPGM(" Retraction done.");
+      //if (g26_debug_flag) SERIAL_ECHOLNPGM(" Retraction done.");
     }
   }
 
@@ -615,7 +596,7 @@
     if (g26_retracted) { // Only un-retract if we are retracted.
       move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 1.2 * retraction_multiplier);
       g26_retracted = false;
-//    if (g26_debug_flag) SERIAL_ECHOLNPGM(" unretract done.");
+      //if (g26_debug_flag) SERIAL_ECHOLNPGM(" unretract done.");
     }
   }
 
@@ -635,27 +616,24 @@
    * cases where the optimization comes into play.
    */
   void print_line_from_here_to_there( float sx, float sy, float sz, float ex, float ey, float ez) {
-    float dx, dy, dx_s, dy_s, dx_e, dy_e, dist_start, dist_end, Line_Length;
-
-    dx_s = current_position[X_AXIS] - sx;   // find our distance from the start of the actual line segment
-    dy_s = current_position[Y_AXIS] - sy;
-    dist_start = HYPOT2(dx_s, dy_s);        // We don't need to do a sqrt(), we can compare the distance^2
-                                            // to save computation time
-    dx_e = current_position[X_AXIS] - ex;   // find our distance from the end of the actual line segment
-    dy_e = current_position[Y_AXIS] - ey;
-    dist_end = HYPOT2(dx_e, dy_e);
-
-    dx = ex - sx;
-    dy = ey - sy;
-    Line_Length = HYPOT(dx, dy);
+    const float dx_s = current_position[X_AXIS] - sx,   // find our distance from the start of the actual line segment
+                dy_s = current_position[Y_AXIS] - sy,
+                dist_start = HYPOT2(dx_s, dy_s),        // We don't need to do a sqrt(), we can compare the distance^2
+                                                        // to save computation time
+                dx_e = current_position[X_AXIS] - ex,   // find our distance from the end of the actual line segment
+                dy_e = current_position[Y_AXIS] - ey,
+                dist_end = HYPOT2(dx_e, dy_e),
+
+                dx = ex - sx,
+                dy = ey - sy,
+                line_length = HYPOT(dx, dy);
 
     // If the end point of the line is closer to the nozzle, we are going to
     // flip the direction of this line.   We will print it from the end to the start.
     // On very small lines we don't do the optimization because it just isn't worth it.
     //
-    if (dist_end < dist_start && (SIZE_OF_INTERSECTION_CIRCLES) < abs(Line_Length)) {
-//    if (g26_debug_flag)
-//      SERIAL_ECHOLNPGM("  Reversing start and end of print_line_from_here_to_there()");
+    if (dist_end < dist_start && (SIZE_OF_INTERSECTION_CIRCLES) < abs(line_length)) {
+      //if (g26_debug_flag) SERIAL_ECHOLNPGM("  Reversing start and end of print_line_from_here_to_there()");
       print_line_from_here_to_there(ex, ey, ez, sx, sy, sz);
       return;
     }
@@ -664,26 +642,19 @@
 
     if (dist_start > 2.0) {
       retract_filament();
-//    if (g26_debug_flag)
-//      SERIAL_ECHOLNPGM("  filament retracted.");
+      //if (g26_debug_flag) SERIAL_ECHOLNPGM("  filament retracted.");
     }
-    // If the end point of the line is closer to the nozzle, we are going to
     move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion
 
-    // If the end point of the line is closer to the nozzle, we are going to
-
-    float e_pos_delta = Line_Length * g26_e_axis_feedrate * extrusion_multiplier;
+    const float e_pos_delta = line_length * g26_e_axis_feedrate * extrusion_multiplier;
 
     un_retract_filament();
 
-    // If the end point of the line is closer to the nozzle, we are going to
-//  if (g26_debug_flag) {
-//    SERIAL_ECHOLNPGM("  doing printing move.");
-//    debug_current_and_destination((char*)"doing final move_to() inside print_line_from_here_to_there()");
-//  }
+    //if (g26_debug_flag) {
+    //  SERIAL_ECHOLNPGM("  doing printing move.");
+    //  debug_current_and_destination((char*)"doing final move_to() inside print_line_from_here_to_there()");
+    //}
     move_to(ex, ey, ez, e_pos_delta);  // Get to the ending point with an appropriate amount of extrusion
-
-    // If the end point of the line is closer to the nozzle, we are going to
   }
 
   /**
@@ -820,6 +791,14 @@
     return UBL_OK;
   }
 
+  bool exit_from_g26() {
+    //strcpy(lcd_status_message, "Leaving G26"); // We can't do lcd_setstatus() without having it continue;
+    lcd_reset_alert_level();
+    lcd_setstatuspgm(PSTR("Leaving G26"));
+    while (ubl_lcd_clicked()) idle();
+    return UBL_ERR;
+  }
+
   /**
    * Turn on the bed and nozzle heat and
    * wait for them to get up to temperature.
@@ -828,24 +807,18 @@
     #if HAS_TEMP_BED
       #if ENABLED(ULTRA_LCD)
         if (bed_temp > 25) {
-          lcd_setstatuspgm(PSTR("G26 Heating Bed."), (uint8_t) 99);
+          lcd_setstatuspgm(PSTR("G26 Heating Bed."), 99);
           lcd_quick_feedback();
       #endif
           ubl_has_control_of_lcd_panel = true;
           thermalManager.setTargetBed(bed_temp);
           while (abs(thermalManager.degBed() - bed_temp) > 3) {
-            if (ubl_lcd_clicked()) {
-              strcpy(lcd_status_message, "Leaving G26");      // We can't do lcd_setstatus() without having it continue;
-              lcd_setstatuspgm(PSTR("Leaving G26"), (uint8_t) 99);      // Now we do it right.
-              while (ubl_lcd_clicked())                       // Debounce Encoder Wheel 
-                idle();
-              return UBL_ERR;
-            }
+            if (ubl_lcd_clicked()) return exit_from_g26();
             idle();
           }
       #if ENABLED(ULTRA_LCD)
         }
-        lcd_setstatuspgm(PSTR("G26 Heating Nozzle."), (uint8_t) 99);
+        lcd_setstatuspgm(PSTR("G26 Heating Nozzle."), 99);
         lcd_quick_feedback();
       #endif
     #endif
@@ -853,18 +826,13 @@
     // Start heating the nozzle and wait for it to reach temperature.
     thermalManager.setTargetHotend(hotend_temp, 0);
     while (abs(thermalManager.degHotend(0) - hotend_temp) > 3) {
-      if (ubl_lcd_clicked()) {
-        strcpy(lcd_status_message, "Leaving G26");          // We can't do lcd_setstatuspgm() without having it continue;
-        lcd_setstatuspgm(PSTR("Leaving G26"), (uint8_t) 99);          // Now we do it right.
-        while (ubl_lcd_clicked())                           // Debounce Encoder Wheel 
-          idle();
-        return UBL_ERR;
-      }
+      if (ubl_lcd_clicked()) return exit_from_g26();
       idle();
     }
 
     #if ENABLED(ULTRA_LCD)
-      lcd_setstatuspgm(PSTR(""), (uint8_t) 99);
+      lcd_reset_alert_level();
+      lcd_setstatuspgm(PSTR(""));
       lcd_quick_feedback();
     #endif
     return UBL_OK;
@@ -877,7 +845,7 @@
     float Total_Prime = 0.0;
 
     if (prime_flag == -1) {  // The user wants to control how much filament gets purged
-      lcd_setstatuspgm(PSTR("User-Controlled Prime"), (uint8_t) 99);
+      lcd_setstatuspgm(PSTR("User-Controlled Prime"), 99);
       chirp_at_user();
 
       set_destination_to_current();
@@ -894,7 +862,6 @@
         #endif
         ubl_line_to_destination(
           destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS],
-          //planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0, 0xFFFF, 0xFFFF);
           planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0
         );
 
@@ -906,26 +873,24 @@
         idle();
       }
 
-      strcpy(lcd_status_message, "Done Priming"); // We can't do lcd_setstatuspgm() without having it continue;
-                                                  // So...  We cheat to get a message up.
-      while (ubl_lcd_clicked())                   // Debounce Encoder Wheel 
-        idle();
+      while (ubl_lcd_clicked()) idle();           // Debounce Encoder Wheel
 
       #if ENABLED(ULTRA_LCD)
-        lcd_setstatuspgm(PSTR("Done Priming"), (uint8_t) 99); 
+        strcpy_P(lcd_status_message, PSTR("Done Priming")); // We can't do lcd_setstatuspgm() without having it continue;
+                                                            // So...  We cheat to get a message up.
+        lcd_setstatuspgm(PSTR("Done Priming"), 99);
         lcd_quick_feedback();
       #endif
     }
     else {
       #if ENABLED(ULTRA_LCD)
-        lcd_setstatuspgm(PSTR("Fixed Length Prime."), (uint8_t) 99);
+        lcd_setstatuspgm(PSTR("Fixed Length Prime."), 99);
         lcd_quick_feedback();
       #endif
       set_destination_to_current();
       destination[E_AXIS] += prime_length;
       ubl_line_to_destination(
         destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS],
-        //planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0, 0xFFFF, 0xFFFF);
         planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0
       );
       stepper.synchronize();

Marlin/M100_Free_Mem_Chk.cpp

@@ -76,32 +76,25 @@ void gcode_M100() {
       // We want to start and end the dump on a nice 16 byte boundry even though
       // the values we are using are not 16 byte aligned.
       //
-      SERIAL_ECHOPGM("\nbss_end : ");
-      prt_hex_word((unsigned int) ptr);
-      ptr = (char*)((unsigned long) ptr & 0xfff0);
+      SERIAL_ECHOPAIR("\nbss_end : ", hex_word((uint16_t)ptr));
+      ptr = (char*)((uint32_t)ptr & 0xfff0);
       sp = top_of_stack();
-      SERIAL_ECHOPGM("\nStack Pointer : ");
-      prt_hex_word((unsigned int) sp);
-      SERIAL_EOL;
-      sp = (char*)((unsigned long) sp | 0x000f);
+      SERIAL_ECHOLNPAIR("\nStack Pointer : ", hex_word((uint16_t)sp));
+      sp = (char*)((uint32_t)sp | 0x000f);
       n = sp - ptr;
       //
       // This is the main loop of the Dump command.
       //
       while (ptr < sp) {
-        prt_hex_word((unsigned int) ptr); // Print the address
+        print_hex_word((uint16_t)ptr); // Print the address
         SERIAL_CHAR(':');
         for (i = 0; i < 16; i++) {      // and 16 data bytes
-          prt_hex_byte(*(ptr + i));
+          print_hex_byte(*(ptr + i));
           SERIAL_CHAR(' ');
         }
         SERIAL_CHAR('|');         // now show where non 0xE5's are
-        for (i = 0; i < 16; i++) {
-          if (*(ptr + i) == (char)0xe5)
-            SERIAL_CHAR(' ');
-          else
-            SERIAL_CHAR('?');
-        }
+        for (i = 0; i < 16; i++)
+          SERIAL_CHAR((*(ptr + i) == (char)0xe5) ? ' ' : '?');
         SERIAL_EOL;
         ptr += 16;
       }
@@ -127,9 +120,7 @@ void gcode_M100() {
         j = how_many_E5s_are_here(ptr + i);
         if (j > 8) {
           SERIAL_ECHOPAIR("Found ", j);
-          SERIAL_ECHOPGM(" bytes free at 0x");
-          prt_hex_word((int) ptr + i);
-          SERIAL_EOL;
+          SERIAL_ECHOLNPAIR(" bytes free at 0x", hex_word((uint16_t)(ptr + i)));
           i += j;
           block_cnt++;
         }
@@ -164,8 +155,7 @@ void gcode_M100() {
       j = n / (x + 1);
       for (i = 1; i <= x; i++) {
         *(ptr + (i * j)) = i;
-        SERIAL_ECHOPGM("\nCorrupting address: 0x");
-        prt_hex_word((unsigned int)(ptr + (i * j)));
+        SERIAL_ECHOPAIR("\nCorrupting address: 0x", hex_word((uint16_t)(ptr + i * j)));
       }
       SERIAL_ECHOLNPGM("\n");
       return;

Marlin/Marlin.h

@@ -363,7 +363,10 @@ float code_value_temp_diff();
 #endif
 
 #if ENABLED(HOST_KEEPALIVE_FEATURE)
-  extern uint8_t host_keepalive_interval;
+  extern MarlinBusyState busy_state;
+  #define KEEPALIVE_STATE(n) do{ busy_state = n; }while(0)
+#else
+  #define KEEPALIVE_STATE(n) NOOP
 #endif
 
 #if FAN_COUNT > 0

Marlin/Marlin_main.cpp

@@ -655,11 +655,9 @@ static bool send_ok[BUFSIZE];
   static MarlinBusyState busy_state = NOT_BUSY;
   static millis_t next_busy_signal_ms = 0;
   uint8_t host_keepalive_interval = DEFAULT_KEEPALIVE_INTERVAL;
-  #define KEEPALIVE_STATE(n) do{ busy_state = n; }while(0)
 #else
-  #define host_keepalive() ;
-  #define KEEPALIVE_STATE(n) ;
-#endif // HOST_KEEPALIVE_FEATURE
+  #define host_keepalive() NOOP
+#endif
 
 static inline float pgm_read_any(const float *p) { return pgm_read_float_near(p); }
 static inline signed char pgm_read_any(const signed char *p) { return pgm_read_byte_near(p); }
@@ -1021,7 +1019,7 @@ inline void get_serial_commands() {
   // send "wait" to indicate Marlin is still waiting.
   #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0
     static millis_t last_command_time = 0;
-    millis_t ms = millis();
+    const millis_t ms = millis();
     if (commands_in_queue == 0 && !MYSERIAL.available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) {
       SERIAL_ECHOLNPGM(MSG_WAIT);
       last_command_time = ms;
@@ -4700,8 +4698,8 @@ inline void gcode_G92() {
 
     #endif
 
-    wait_for_user = true;
     KEEPALIVE_STATE(PAUSED_FOR_USER);
+    wait_for_user = true;
 
     stepper.synchronize();
     refresh_cmd_timeout();
@@ -5040,7 +5038,7 @@ inline void gcode_M42() {
       if (first_pin > NUM_DIGITAL_PINS - 1) return;
     }
 
-    bool ignore_protection = code_seen('I') ? code_value_bool() : false;
+    const bool ignore_protection = code_seen('I') ? code_value_bool() : false;
 
     // Watch until click, M108, or reset
     if (code_seen('W') && code_value_bool()) { // watch digital pins
@@ -6314,8 +6312,8 @@ inline void gcode_M121() { endstops.enable_globally(false); }
 
     #if DISABLED(SDSUPPORT)
       // Wait for lcd click or M108
-      wait_for_user = true;
       KEEPALIVE_STATE(PAUSED_FOR_USER);
+      wait_for_user = true;
       while (wait_for_user) idle();
       KEEPALIVE_STATE(IN_HANDLER);
 
@@ -7581,7 +7579,7 @@ inline void gcode_M503() {
     disable_e_steppers();
     safe_delay(100);
 
-    millis_t nozzle_timeout = millis() + (millis_t)(FILAMENT_CHANGE_NOZZLE_TIMEOUT) * 1000L;
+    const millis_t nozzle_timeout = millis() + (millis_t)(FILAMENT_CHANGE_NOZZLE_TIMEOUT) * 1000UL;
     bool nozzle_timed_out = false;
     float temps[4];
 
@@ -7596,9 +7594,10 @@ inline void gcode_M503() {
 
     HOTEND_LOOP() temps[e] = thermalManager.target_temperature[e]; // Save nozzle temps
 
+    KEEPALIVE_STATE(PAUSED_FOR_USER);
     wait_for_user = true;    // LCD click or M108 will clear this
     while (wait_for_user) {
-      millis_t current_ms = millis();
+
       if (nozzle_timed_out)
         lcd_filament_change_show_message(FILAMENT_CHANGE_MESSAGE_CLICK_TO_HEAT_NOZZLE);
 
@@ -7606,15 +7605,14 @@ inline void gcode_M503() {
         filament_change_beep();
       #endif
 
-      if (current_ms >= nozzle_timeout) {
-        if (!nozzle_timed_out) {
-          nozzle_timed_out = true; // on nozzle timeout remember the nozzles need to be reheated
-          HOTEND_LOOP() thermalManager.setTargetHotend(0, e); // Turn off all the nozzles
-          lcd_filament_change_show_message(FILAMENT_CHANGE_MESSAGE_CLICK_TO_HEAT_NOZZLE);
-        }
+      if (!nozzle_timed_out && ELAPSED(millis(), nozzle_timeout)) {
+        nozzle_timed_out = true; // on nozzle timeout remember the nozzles need to be reheated
+        HOTEND_LOOP() thermalManager.setTargetHotend(0, e); // Turn off all the nozzles
+        lcd_filament_change_show_message(FILAMENT_CHANGE_MESSAGE_CLICK_TO_HEAT_NOZZLE);
       }
       idle(true);
     }
+    KEEPALIVE_STATE(IN_HANDLER);
 
     if (nozzle_timed_out)      // Turn nozzles back on if they were turned off
       HOTEND_LOOP() thermalManager.setTargetHotend(temps[e], e);
@@ -7642,6 +7640,7 @@ inline void gcode_M503() {
       filament_change_beep(true);
     #endif
 
+    KEEPALIVE_STATE(PAUSED_FOR_USER);
     wait_for_user = true;    // LCD click or M108 will clear this
     while (wait_for_user && nozzle_timed_out) {
       #if HAS_BUZZER
@@ -7649,6 +7648,7 @@ inline void gcode_M503() {
       #endif
       idle(true);
     }
+    KEEPALIVE_STATE(IN_HANDLER);
 
     // Show "load" message
     lcd_filament_change_show_message(FILAMENT_CHANGE_MESSAGE_LOAD);
@@ -10160,9 +10160,9 @@ void prepare_move_to_destination() {
 #if HAS_CONTROLLERFAN
 
   void controllerFan() {
-    static millis_t lastMotorOn = 0; // Last time a motor was turned on
-    static millis_t nextMotorCheck = 0; // Last time the state was checked
-    millis_t ms = millis();
+    static millis_t lastMotorOn = 0, // Last time a motor was turned on
+                    nextMotorCheck = 0; // Last time the state was checked
+    const millis_t ms = millis();
     if (ELAPSED(ms, nextMotorCheck)) {
       nextMotorCheck = ms + 2500UL; // Not a time critical function, so only check every 2.5s
       if (X_ENABLE_READ == X_ENABLE_ON || Y_ENABLE_READ == Y_ENABLE_ON || Z_ENABLE_READ == Z_ENABLE_ON || thermalManager.soft_pwm_bed > 0
@@ -10495,7 +10495,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
 
   if (commands_in_queue < BUFSIZE) get_available_commands();
 
-  millis_t ms = millis();
+  const millis_t ms = millis();
 
   if (max_inactive_time && ELAPSED(ms, previous_cmd_ms + max_inactive_time)) {
     SERIAL_ERROR_START;
@@ -10709,7 +10709,7 @@ void kill(const char* lcd_msg) {
 
   thermalManager.disable_all_heaters();
   disable_all_steppers();
-            
+
   #if ENABLED(ULTRA_LCD)
     kill_screen(lcd_msg);
   #else
@@ -10718,7 +10718,7 @@ void kill(const char* lcd_msg) {
 
   _delay_ms(250); // Wait a short time
   cli(); // Stop interrupts
-            
+
   _delay_ms(250); //Wait to ensure all interrupts routines stopped
   thermalManager.disable_all_heaters(); //turn off heaters again
 

Marlin/UBL_Bed_Leveling.cpp

@@ -38,6 +38,25 @@
   void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); }
   bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); }
 
+  static void serial_echo_xy(const uint16_t x, const uint16_t y) {
+    SERIAL_CHAR('(');
+    SERIAL_ECHO(x);
+    SERIAL_CHAR(',');
+    SERIAL_ECHO(y);
+    SERIAL_CHAR(')');
+    safe_delay(10);
+  }
+
+  static void serial_echo_10x_spaces() {
+    for (uint8_t i = UBL_MESH_NUM_X_POINTS - 1; --i;) {
+      SERIAL_ECHOPGM("          ");
+      #if TX_BUFFER_SIZE > 0
+        MYSERIAL.flushTX();
+      #endif
+      safe_delay(10);
+    }
+  }
+
   /**
    * These variables used to be declared inside the unified_bed_leveling class. We are going to
    * still declare them within the .cpp file for bed leveling. But there is only one instance of
@@ -105,13 +124,10 @@
     }
 
     j = UBL_LAST_EEPROM_INDEX - (m + 1) * sizeof(z_values);
-    eeprom_read_block((void *)&z_values , (void *)j, sizeof(z_values));
+    eeprom_read_block((void *)&z_values, (void *)j, sizeof(z_values));
 
-    SERIAL_PROTOCOLPGM("Mesh loaded from slot ");
-    SERIAL_PROTOCOL(m);
-    SERIAL_PROTOCOLPGM("  at offset 0x");
-    prt_hex_word(j);
-    SERIAL_EOL;
+    SERIAL_PROTOCOLPAIR("Mesh loaded from slot ", m);
+    SERIAL_PROTOCOLLNPAIR(" at offset 0x", hex_word(j));
   }
 
   void unified_bed_leveling::store_mesh(const int16_t m) {
@@ -132,11 +148,8 @@
     j = UBL_LAST_EEPROM_INDEX - (m + 1) * sizeof(z_values);
     eeprom_write_block((const void *)&z_values, (void *)j, sizeof(z_values));
 
-    SERIAL_PROTOCOLPGM("Mesh saved in slot ");
-    SERIAL_PROTOCOL(m);
-    SERIAL_PROTOCOLPGM("  at offset 0x");
-    prt_hex_word(j);
-    SERIAL_EOL;
+    SERIAL_PROTOCOLPAIR("Mesh saved in slot ", m);
+    SERIAL_PROTOCOLLNPAIR(" at offset 0x", hex_word(j));
   }
 
   void unified_bed_leveling::reset() {
@@ -151,7 +164,7 @@
   }
 
   void unified_bed_leveling::invalidate() {
-    prt_hex_word((unsigned int)this);
+    print_hex_word((uint16_t)this);
     SERIAL_EOL;
 
     state.active = false;
@@ -162,125 +175,76 @@
   }
 
   void unified_bed_leveling::display_map(const int map_type) {
-    float f, current_xi, current_yi;
-    int8_t i, j;
-    UNUSED(map_type);
 
-    if (map_type==0) {
-      SERIAL_PROTOCOLLNPGM("\nBed Topography Report:\n");
+    const bool map0 = map_type == 0;
 
-      SERIAL_ECHOPAIR("(", 0);
-      SERIAL_ECHOPAIR(", ", UBL_MESH_NUM_Y_POINTS - 1);
-      SERIAL_ECHOPGM(")    ");
+    if (map0) {
+      SERIAL_PROTOCOLLNPGM("\nBed Topography Report:\n");
+      serial_echo_xy(0, UBL_MESH_NUM_Y_POINTS - 1);
+      SERIAL_ECHOPGM("    ");
     }
 
-    current_xi = ubl.get_cell_index_x(current_position[X_AXIS] + (MESH_X_DIST) / 2.0);
-    current_yi = ubl.get_cell_index_y(current_position[Y_AXIS] + (MESH_Y_DIST) / 2.0);
-
-    if (map_type==0) {
-      for (i = 0; i < UBL_MESH_NUM_X_POINTS - 1; i++) {
-        SERIAL_ECHOPGM("            ");
-        #if TX_BUFFER_SIZE>0
-          MYSERIAL.flushTX();
-        #endif
-        safe_delay(15);
-      }
-      
-      SERIAL_ECHOPAIR("(", UBL_MESH_NUM_X_POINTS - 1);
-      SERIAL_ECHOPAIR(",", UBL_MESH_NUM_Y_POINTS - 1);
-      SERIAL_ECHOLNPGM(")");
-
-      SERIAL_ECHOPAIR("(", UBL_MESH_MIN_X);
-      SERIAL_ECHOPAIR(",", UBL_MESH_MAX_Y);
-      SERIAL_CHAR(')');
-      safe_delay(15);
-
-      for (i = 0; i < UBL_MESH_NUM_X_POINTS - 1; i++) {
-        SERIAL_ECHOPGM("            ");
-        #if TX_BUFFER_SIZE>0
-          MYSERIAL.flushTX();
-        #endif
-        safe_delay(15);
-      }
-
-      SERIAL_ECHOPAIR("(", UBL_MESH_MAX_X);
-      SERIAL_ECHOPAIR(",", UBL_MESH_MAX_Y);
-      SERIAL_ECHOLNPGM(")");
-      safe_delay(15);
+    if (map0) {
+      serial_echo_10x_spaces();
+      serial_echo_xy(UBL_MESH_NUM_X_POINTS - 1, UBL_MESH_NUM_Y_POINTS - 1);
+      SERIAL_EOL;
+      serial_echo_xy(UBL_MESH_MIN_X, UBL_MESH_MIN_Y);
+      serial_echo_10x_spaces();
+      serial_echo_xy(UBL_MESH_MAX_X, UBL_MESH_MAX_Y);
+      SERIAL_EOL;
     }
 
-    for (j = UBL_MESH_NUM_Y_POINTS - 1; j >= 0; j--) {
-      for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
-        f = z_values[i][j];
+    const float current_xi = ubl.get_cell_index_x(current_position[X_AXIS] + (MESH_X_DIST) / 2.0),
+                current_yi = ubl.get_cell_index_y(current_position[Y_AXIS] + (MESH_Y_DIST) / 2.0);
+
+    for (uint8_t j = UBL_MESH_NUM_Y_POINTS - 1; j >= 0; j--) {
+      for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
+        const bool is_current = i == current_xi && j == current_yi;
 
         // is the nozzle here?  if so, mark the number
-        if (map_type==0) 
-          SERIAL_CHAR(i == current_xi && j == current_yi ? '[' : ' ');
-
-        if (isnan(f))
-          if (map_type==0) {
-            SERIAL_PROTOCOLPGM("    .    ");
-          } else 
-            SERIAL_PROTOCOLPGM("NAN");
+        if (map0)
+          SERIAL_CHAR(is_current ? '[' : ' ');
+
+        const float f = z_values[i][j];
+        if (isnan(f)) {
+          serialprintPGM(map0 ? PSTR("    .    ") : PSTR("NAN"));
+        }
         else {
           // if we don't do this, the columns won't line up nicely
-          if (f>=0.0 && map_type==0) SERIAL_CHAR(' ');
+          if (f >= 0.0 && map0) SERIAL_CHAR(' ');
           SERIAL_PROTOCOL_F(f, 3);
           idle();
         }
-        if (map_type!=0 && i<UBL_MESH_NUM_X_POINTS-1) 
-         SERIAL_PROTOCOLPGM(",");
+        if (!map0 && i < UBL_MESH_NUM_X_POINTS - 1)
+         SERIAL_CHAR(',');
 
-        #if TX_BUFFER_SIZE>0
+        #if TX_BUFFER_SIZE > 0
           MYSERIAL.flushTX();
         #endif
         safe_delay(15);
-        if (map_type==0) {
-          if (i == current_xi && j == current_yi) // is the nozzle here? if so, finish marking the number
-            SERIAL_CHAR(']');
-          else
-            SERIAL_PROTOCOL("  ");
+        if (map0) {
+          SERIAL_CHAR(is_current ? ']' : ' ');
           SERIAL_CHAR(' ');
         }
       }
       SERIAL_EOL;
-      if (j && map_type==0) { // we want the (0,0) up tight against the block of numbers
+      if (j && map0) { // we want the (0,0) up tight against the block of numbers
         SERIAL_CHAR(' ');
         SERIAL_EOL;
       }
     }
 
-    if (map_type==0) {
-      SERIAL_ECHOPAIR("(", int(UBL_MESH_MIN_X));
-      SERIAL_ECHOPAIR(",", int(UBL_MESH_MIN_Y));
-      SERIAL_ECHOPGM(")    ");
-
-      for (i = 0; i < UBL_MESH_NUM_X_POINTS - 1; i++)  {
-        SERIAL_ECHOPGM("            ");
-        #if TX_BUFFER_SIZE>0
-          MYSERIAL.flushTX();
-        #endif
-        safe_delay(15);
-      }
-      SERIAL_ECHOPAIR("(", int(UBL_MESH_MAX_X));
-      SERIAL_ECHOPAIR(",", int(UBL_MESH_MIN_Y));
-      SERIAL_CHAR(')');
+    if (map0) {
+      serial_echo_xy(UBL_MESH_MIN_X, UBL_MESH_MIN_Y);
+      SERIAL_ECHOPGM("    ");
+      serial_echo_10x_spaces();
+      serial_echo_xy(UBL_MESH_MAX_X, UBL_MESH_MIN_Y);
+      SERIAL_EOL;
+      serial_echo_xy(0, 0);
+      SERIAL_ECHOPGM("       ");
+      serial_echo_10x_spaces();
+      serial_echo_xy(UBL_MESH_NUM_X_POINTS - 1, 0);
       SERIAL_EOL;
-
-      SERIAL_ECHOPAIR("(", 0);
-      SERIAL_ECHOPAIR(",", 0);
-      SERIAL_ECHOPGM(")       ");
-
-      for (i = 0; i < UBL_MESH_NUM_X_POINTS - 1; i++) {
-        SERIAL_ECHOPGM("            ");
-        #if TX_BUFFER_SIZE>0
-          MYSERIAL.flushTX();
-        #endif
-        safe_delay(15);
-      }
-      SERIAL_ECHOPAIR("(", UBL_MESH_NUM_X_POINTS-1);
-      SERIAL_ECHOPAIR(",", 0);
-      SERIAL_ECHOLNPGM(")");
     }
   }
 

Marlin/UBL_G29.cpp

@@ -158,7 +158,7 @@
    *                    only done between probe points. You will need to press and hold the switch until the
    *                    Phase 1 command can detect it.)
    *
-   *   P2    Phase 2    Probe areas of the Mesh that can not be automatically handled. Phase 2 respects an H
+   *   P2    Phase 2    Probe areas of the Mesh that can't be automatically handled. Phase 2 respects an H
    *                    parameter to control the height between Mesh points. The default height for movement
    *                    between Mesh points is 5mm. A smaller number can be used to make this part of the
    *                    calibration less time consuming. You will be running the nozzle down until it just barely
@@ -303,25 +303,17 @@
   volatile int8_t ubl_encoderDiff = 0; // Volatile because it's changed by Temperature ISR button update
 
   // The simple parameter flags and values are 'static' so parameter parsing can be in a support routine.
-  static int g29_verbose_level = 0, phase_value = -1, repetition_cnt = 1,
-             storage_slot = 0, map_type = 0, test_pattern = 0, unlevel_value = -1;
-  static bool repeat_flag = UBL_OK, c_flag = false, x_flag = UBL_OK, y_flag = UBL_OK, statistics_flag = UBL_OK, business_card_mode = false;
-  static float x_pos = 0.0, y_pos = 0.0, height_value = 5.0, measured_z, card_thickness = 0.0, constant = 0.0;
+  static int g29_verbose_level, phase_value = -1, repetition_cnt,
+             storage_slot = 0, map_type; //unlevel_value = -1;
+  static bool repeat_flag, c_flag, x_flag, y_flag;
+  static float x_pos, y_pos, measured_z, card_thickness = 0.0, ubl_constant = 0.0;
 
   #if ENABLED(ULTRA_LCD)
     void lcd_setstatus(const char* message, bool persist);
   #endif
 
   void gcode_G29() {
-    float Z1, Z2, Z3;
-
-    g29_verbose_level = 0;  // These may change, but let's get some reasonable values into them.
-    repeat_flag       = UBL_OK;
-    repetition_cnt    = 1;
-    c_flag            = false;
-
     SERIAL_PROTOCOLLNPAIR("ubl_eeprom_start=", ubl_eeprom_start);
-
     if (ubl_eeprom_start < 0) {
       SERIAL_PROTOCOLLNPGM("?You need to enable your EEPROM and initialize it");
       SERIAL_PROTOCOLLNPGM("with M502, M500, M501 in that order.\n");
@@ -350,53 +342,46 @@
 
     if (code_seen('Q')) {
 
-      if (code_has_value()) test_pattern = code_value_int();
-
-      if (test_pattern < 0 || test_pattern > 4) {
-        SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-4)\n");
+      const int test_pattern = code_has_value() ? code_value_int() : -1;
+      if (test_pattern < 0 || test_pattern > 2) {
+        SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-2)\n");
         return;
       }
       SERIAL_PROTOCOLLNPGM("Loading test_pattern values.\n");
       switch (test_pattern) {
         case 0:
-          for (uint8_t x = 0; x < UBL_MESH_NUM_X_POINTS; x++) {         // Create a bowl shape. This is
-            for (uint8_t y = 0; y < UBL_MESH_NUM_Y_POINTS; y++) {       // similar to what a user would see with
-              Z1 = 0.5 * (UBL_MESH_NUM_X_POINTS) - x;                   // a poorly calibrated Delta.
-              Z2 = 0.5 * (UBL_MESH_NUM_Y_POINTS) - y;
-              z_values[x][y] += 2.0 * HYPOT(Z1, Z2);
+          for (uint8_t x = 0; x < UBL_MESH_NUM_X_POINTS; x++) {   // Create a bowl shape - similar to
+            for (uint8_t y = 0; y < UBL_MESH_NUM_Y_POINTS; y++) { // a poorly calibrated Delta.
+              const float p1 = 0.5 * (UBL_MESH_NUM_X_POINTS) - x,
+                          p2 = 0.5 * (UBL_MESH_NUM_Y_POINTS) - y;
+              z_values[x][y] += 2.0 * HYPOT(p1, p2);
             }
           }
-        break;
+          break;
         case 1:
-          for (uint8_t x = 0; x < UBL_MESH_NUM_X_POINTS; x++) {  // Create a diagonal line several Mesh
-            z_values[x][x] += 9.999;                             // cells thick that is raised
-            if (x < UBL_MESH_NUM_Y_POINTS - 1)
-              z_values[x][x + 1] += 9.999;                       // We want the altered line several mesh points thick
-            if (x > 0)
-              z_values[x][x - 1] += 9.999;                       // We want the altered line several mesh points thick
+          for (uint8_t x = 0; x < UBL_MESH_NUM_X_POINTS; x++) {  // Create a diagonal line several Mesh cells thick that is raised
+            z_values[x][x] += 9.999;
+            z_values[x][x + (x < UBL_MESH_NUM_Y_POINTS - 1) ? 1 : -1] += 9.999; // We want the altered line several mesh points thick
           }
           break;
         case 2:
-          // Allow the user to specify the height because 10mm is
-          // a little bit extreme in some cases.
+          // Allow the user to specify the height because 10mm is a little extreme in some cases.
           for (uint8_t x = (UBL_MESH_NUM_X_POINTS) / 3; x < 2 * (UBL_MESH_NUM_X_POINTS) / 3; x++)   // Create a rectangular raised area in
             for (uint8_t y = (UBL_MESH_NUM_Y_POINTS) / 3; y < 2 * (UBL_MESH_NUM_Y_POINTS) / 3; y++) // the center of the bed
-              z_values[x][y] += code_seen('C') ? constant : 9.99;
-          break;
-        case 3:
+              z_values[x][y] += code_seen('C') ? ubl_constant : 9.99;
           break;
       }
     }
 
-/*
+    /*
     if (code_seen('U')) {
       unlevel_value = code_value_int();
-//    if (unlevel_value < 0 || unlevel_value > 7) {
-//      SERIAL_PROTOCOLLNPGM("Invalid Unlevel value. (0-4)\n");
-//      return;
-//    }
+      //if (unlevel_value < 0 || unlevel_value > 7) {
+      //  SERIAL_PROTOCOLLNPGM("Invalid Unlevel value. (0-4)\n");
+      //  return;
+      //}
     }
-*/
+    //*/
 
     if (code_seen('P')) {
       phase_value = code_value_int();
@@ -430,9 +415,9 @@
                             code_seen('O') || code_seen('M'), code_seen('E'), code_seen('U'));
           break;
         //
-        // Manually Probe Mesh in areas that can not be reached by the probe
+        // Manually Probe Mesh in areas that can't be reached by the probe
         //
-        case 2:
+        case 2: {
           SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.\n");
           do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
           if (!x_flag && !y_flag) {      // use a good default location for the path
@@ -451,32 +436,34 @@
             y_pos = current_position[Y_AXIS];
           }
 
-          height_value = code_seen('H') && code_has_value() ? code_value_float() : Z_CLEARANCE_BETWEEN_PROBES;
+          const float height = code_seen('H') && code_has_value() ? code_value_float() : Z_CLEARANCE_BETWEEN_PROBES;
 
-          if ((business_card_mode = code_seen('B'))) {
-            card_thickness = code_has_value() ? code_value_float() : measure_business_card_thickness(height_value);
+          if (code_seen('B')) {
+            card_thickness = code_has_value() ? code_value_float() : measure_business_card_thickness(height);
 
             if (fabs(card_thickness) > 1.5) {
-              SERIAL_PROTOCOLLNPGM("?Error in Business Card measurment.\n");
+              SERIAL_PROTOCOLLNPGM("?Error in Business Card measurement.\n");
               return;
             }
           }
-          manually_probe_remaining_mesh(x_pos, y_pos, height_value, card_thickness, code_seen('O') || code_seen('M'));
-          break;
+          manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('O') || code_seen('M'));
+
+        } break;
+
         //
         // Populate invalid Mesh areas with a constant
         //
-        case 3:
-          height_value = 0.0; // Assume 0.0 until proven otherwise
-          if (code_seen('C')) height_value = constant;
+        case 3: {
+          const float height = code_seen('C') ? ubl_constant : 0.0;
           // If no repetition is specified, do the whole Mesh
           if (!repeat_flag) repetition_cnt = 9999;
           while (repetition_cnt--) {
             const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 0, NULL, false); // The '0' says we want to use the nozzle's position
             if (location.x_index < 0) break; // No more invalid Mesh Points to populate
-            z_values[location.x_index][location.y_index] = height_value;
+            z_values[location.x_index][location.y_index] = height;
           }
-          break;
+        } break;
+
         //
         // Fine Tune (Or Edit) the Mesh
         //
@@ -491,36 +478,56 @@
           break;
 
         case 10:
-          // Debug code... Pay no attention to this stuff
-          // it can be removed soon.
+          // [DEBUG] Pay no attention to this stuff. It can be removed soon.
           SERIAL_ECHO_START;
           SERIAL_ECHOLNPGM("Checking G29 has control of LCD Panel:");
-          wait_for_user = true;
+          KEEPALIVE_STATE(PAUSED_FOR_USER);
+          ubl_has_control_of_lcd_panel++;
           while (!ubl_lcd_clicked()) {
             safe_delay(250);
-            SERIAL_ECHO((int)ubl_encoderDiff);
-            ubl_encoderDiff = 0;
-            SERIAL_EOL;
+            if (ubl_encoderDiff) {
+              SERIAL_ECHOLN((int)ubl_encoderDiff);
+              ubl_encoderDiff = 0;
+            }
+          }
+          SERIAL_ECHOLNPGM("G29 giving back control of LCD Panel.");
+          ubl_has_control_of_lcd_panel = false;
+          KEEPALIVE_STATE(IN_HANDLER);
+          break;
+
+        case 11:
+          // [DEBUG] wait_for_user code. Pay no attention to this stuff. It can be removed soon.
+          SERIAL_ECHO_START;
+          SERIAL_ECHOLNPGM("Checking G29 has control of LCD Panel:");
+          KEEPALIVE_STATE(PAUSED_FOR_USER);
+          wait_for_user = true;
+          while (wait_for_user) {
+            safe_delay(250);
+            if (ubl_encoderDiff) {
+              SERIAL_ECHOLN((int)ubl_encoderDiff);
+              ubl_encoderDiff = 0;
+            }
           }
           SERIAL_ECHOLNPGM("G29 giving back control of LCD Panel.");
+          KEEPALIVE_STATE(IN_HANDLER);
           break;
       }
     }
 
     if (code_seen('T')) {
-      Z1 = probe_pt(ubl_3_point_1_X, ubl_3_point_1_Y, false /*Stow Flag*/, g29_verbose_level) + zprobe_zoffset;
-      Z2 = probe_pt(ubl_3_point_2_X, ubl_3_point_2_Y, false /*Stow Flag*/, g29_verbose_level) + zprobe_zoffset;
-      Z3 = probe_pt(ubl_3_point_3_X, ubl_3_point_3_Y, true  /*Stow Flag*/, g29_verbose_level) + zprobe_zoffset;
+      float z1 = probe_pt(ubl_3_point_1_X, ubl_3_point_1_Y, false /*Stow Flag*/, g29_verbose_level) + zprobe_zoffset,
+            z2 = probe_pt(ubl_3_point_2_X, ubl_3_point_2_Y, false /*Stow Flag*/, g29_verbose_level) + zprobe_zoffset,
+            z3 = probe_pt(ubl_3_point_3_X, ubl_3_point_3_Y, true  /*Stow Flag*/, g29_verbose_level) + zprobe_zoffset;
 
-      //  We need to adjust Z1, Z2, Z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
+      //  We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
       //  the Mesh is tilted!  (We need to compensate each probe point by what the Mesh says that location's height is)
 
-      Z1 -= ubl.get_z_correction(ubl_3_point_1_X, ubl_3_point_1_Y);
-      Z2 -= ubl.get_z_correction(ubl_3_point_2_X, ubl_3_point_2_Y);
-      Z3 -= ubl.get_z_correction(ubl_3_point_3_X, ubl_3_point_3_Y);
+      z1 -= ubl.get_z_correction(ubl_3_point_1_X, ubl_3_point_1_Y);
+      z2 -= ubl.get_z_correction(ubl_3_point_2_X, ubl_3_point_2_Y);
+      z3 -= ubl.get_z_correction(ubl_3_point_3_X, ubl_3_point_3_Y);
 
       do_blocking_move_to_xy((X_MAX_POS - (X_MIN_POS)) / 2.0, (Y_MAX_POS - (Y_MIN_POS)) / 2.0);
-      tilt_mesh_based_on_3pts(Z1, Z2, Z3);
+      tilt_mesh_based_on_3pts(z1, z2, z3);
     }
 
     //
@@ -610,13 +617,16 @@
         save_ubl_active_state_and_disable();
         //measured_z = probe_pt(x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER, ProbeDeployAndStow, g29_verbose_level);
 
-        ubl_has_control_of_lcd_panel = true;// Grab the LCD Hardware
+        ubl_has_control_of_lcd_panel++;     // Grab the LCD Hardware
         measured_z = 1.5;
         do_blocking_move_to_z(measured_z);  // Get close to the bed, but leave some space so we don't damage anything
                                             // The user is not going to be locking in a new Z-Offset very often so
                                             // it won't be that painful to spin the Encoder Wheel for 1.5mm
         lcd_implementation_clear();
         lcd_z_offset_edit_setup(measured_z);
+
+        KEEPALIVE_STATE(PAUSED_FOR_USER);
+
         do {
           measured_z = lcd_z_offset_edit();
           idle();
@@ -628,6 +638,8 @@
                                           // or here. So, until we are done looking for a long Encoder Wheel Press,
                                           // we need to take control of the panel
 
+        KEEPALIVE_STATE(IN_HANDLER);
+
         lcd_return_to_status();
 
         const millis_t nxt = millis() + 1500UL;
@@ -637,7 +649,6 @@
             SERIAL_PROTOCOLLNPGM("\nZ-Offset Adjustment Stopped.");
             do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
             lcd_setstatus("Z-Offset Stopped", true);
-            ubl_has_control_of_lcd_panel = false;
             restore_ubl_active_state_and_leave();
             goto LEAVE;
           }
@@ -702,14 +713,14 @@
       for (x = 0; x < UBL_MESH_NUM_X_POINTS; x++)
         for (y = 0; y < UBL_MESH_NUM_Y_POINTS; y++)
           if (!isnan(z_values[x][y]))
-            z_values[x][y] -= mean + constant;
+            z_values[x][y] -= mean + ubl_constant;
   }
 
   void shift_mesh_height() {
     for (uint8_t x = 0; x < UBL_MESH_NUM_X_POINTS; x++)
       for (uint8_t y = 0; y < UBL_MESH_NUM_Y_POINTS; y++)
         if (!isnan(z_values[x][y]))
-          z_values[x][y] += constant;
+          z_values[x][y] += ubl_constant;
   }
 
   /**
@@ -728,9 +739,7 @@
         SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.\n");
         lcd_quick_feedback();
         STOW_PROBE();
-        while (ubl_lcd_clicked() ) {
-          idle();
-        }
+        while (ubl_lcd_clicked()) idle();
         ubl_has_control_of_lcd_panel = false;
         restore_ubl_active_state_and_leave();
         safe_delay(50);  // Debounce the Encoder wheel
@@ -739,14 +748,18 @@
 
       location = find_closest_mesh_point_of_type(INVALID, lx, ly, 1, NULL, do_furthest );  // the '1' says we want the location to be relative to the probe
       if (location.x_index >= 0 && location.y_index >= 0) {
-        const float xProbe = ubl.map_x_index_to_bed_location(location.x_index),
-                    yProbe = ubl.map_y_index_to_bed_location(location.y_index);
-        if (xProbe < MIN_PROBE_X || xProbe > MAX_PROBE_X || yProbe < MIN_PROBE_Y || yProbe > MAX_PROBE_Y) {
-          SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed.");
+
+        const float rawx = ubl.map_x_index_to_bed_location(location.x_index),
+                    rawy = ubl.map_y_index_to_bed_location(location.y_index);
+
+        // TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
+        if (rawx < (MIN_PROBE_X) || rawx > (MAX_PROBE_X) || rawy < (MIN_PROBE_Y) || rawy > (MAX_PROBE_Y)) {
+          SERIAL_ERROR_START;
+          SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
           ubl_has_control_of_lcd_panel = false;
           goto LEAVE;
         }
-        const float measured_z = probe_pt(xProbe, yProbe, stow_probe, g29_verbose_level);
+        const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level);
         z_values[location.x_index][location.y_index] = measured_z + zprobe_zoffset;
       }
 
@@ -831,6 +844,7 @@
   }
 
   float use_encoder_wheel_to_measure_point() {
+    KEEPALIVE_STATE(PAUSED_FOR_USER);
     while (!ubl_lcd_clicked()) {     // we need the loop to move the nozzle based on the encoder wheel here!
       idle();
       if (ubl_encoderDiff) {
@@ -838,34 +852,35 @@
         ubl_encoderDiff = 0;
       }
     }
+    KEEPALIVE_STATE(IN_HANDLER);
     return current_position[Z_AXIS];
   }
 
-  float measure_business_card_thickness(const float &height_value) {
+  float measure_business_card_thickness(const float &in_height) {
 
     ubl_has_control_of_lcd_panel++;
     save_ubl_active_state_and_disable();   // we don't do bed level correction because we want the raw data when we probe
 
     SERIAL_PROTOCOLLNPGM("Place Shim Under Nozzle and Perform Measurement.");
-    do_blocking_move_to_z(height_value);
+    do_blocking_move_to_z(in_height);
     do_blocking_move_to_xy((float(X_MAX_POS) - float(X_MIN_POS)) / 2.0, (float(Y_MAX_POS) - float(Y_MIN_POS)) / 2.0);
       //, min( planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS])/2.0);
 
-    const float Z1 = use_encoder_wheel_to_measure_point();
+    const float z1 = use_encoder_wheel_to_measure_point();
     do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
     ubl_has_control_of_lcd_panel = false;
 
     SERIAL_PROTOCOLLNPGM("Remove Shim and Measure Bed Height.");
-    const float Z2 = use_encoder_wheel_to_measure_point();
+    const float z2 = use_encoder_wheel_to_measure_point();
     do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
 
     if (g29_verbose_level > 1) {
       SERIAL_PROTOCOLPGM("Business Card is: ");
-      SERIAL_PROTOCOL_F(abs(Z1 - Z2), 6);
+      SERIAL_PROTOCOL_F(abs(z1 - z2), 6);
       SERIAL_PROTOCOLLNPGM("mm thick.");
     }
     restore_ubl_active_state_and_leave();
-    return abs(Z1 - Z2);
+    return abs(z1 - z2);
   }
 
   void manually_probe_remaining_mesh(const float &lx, const float &ly, const float &z_clearance, const float &card_thickness, const bool do_ubl_mesh_map) {
@@ -881,21 +896,23 @@
       if (do_ubl_mesh_map) ubl.display_map(map_type);
 
       location = find_closest_mesh_point_of_type(INVALID, lx, ly, 0, NULL, false); // The '0' says we want to use the nozzle's position
-      // It doesn't matter if the probe can not reach the
-      // NAN location. This is a manual probe.
+      // It doesn't matter if the probe can't reach the NAN location. This is a manual probe.
       if (location.x_index < 0 && location.y_index < 0) continue;
 
-      const float xProbe = ubl.map_x_index_to_bed_location(location.x_index),
-                  yProbe = ubl.map_y_index_to_bed_location(location.y_index);
+      const float rawx = ubl.map_x_index_to_bed_location(location.x_index),
+                  rawy = ubl.map_y_index_to_bed_location(location.y_index);
 
-      // Modify to use if (position_is_reachable(pos[XYZ]))
-      if (xProbe < (X_MIN_POS) || xProbe > (X_MAX_POS) || yProbe < (Y_MIN_POS) || yProbe > (Y_MAX_POS)) {
-        SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed.");
+      // TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
+      if (rawx < (X_MIN_POS) || rawx > (X_MAX_POS) || rawy < (Y_MIN_POS) || rawy > (Y_MAX_POS)) {
+        SERIAL_ERROR_START;
+        SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
         ubl_has_control_of_lcd_panel = false;
         goto LEAVE;
       }
 
-      const float dx = xProbe - last_x,
+      const float xProbe = LOGICAL_X_POSITION(rawx),
+                  yProbe = LOGICAL_Y_POSITION(rawy),
+                  dx = xProbe - last_x,
                   dy = yProbe - last_y;
 
       if (HYPOT(dx, dy) < BIG_RAISE_NOT_NEEDED)
@@ -908,8 +925,10 @@
       last_x = xProbe;
       last_y = yProbe;
 
+      KEEPALIVE_STATE(PAUSED_FOR_USER);
       ubl_has_control_of_lcd_panel = true;
-      while (!ubl_lcd_clicked) {     // we need the loop to move the nozzle based on the encoder wheel here!
+
+      while (!ubl_lcd_clicked()) {     // we need the loop to move the nozzle based on the encoder wheel here!
         idle();
         if (ubl_encoderDiff) {
           do_blocking_move_to_z(current_position[Z_AXIS] + float(ubl_encoderDiff) / 100.0);
@@ -926,6 +945,7 @@
           lcd_quick_feedback();
           while (ubl_lcd_clicked()) idle();
           ubl_has_control_of_lcd_panel = false;
+          KEEPALIVE_STATE(IN_HANDLER);
           restore_ubl_active_state_and_leave();
           return;
         }
@@ -933,7 +953,7 @@
 
       z_values[location.x_index][location.y_index] = current_position[Z_AXIS] - card_thickness;
       if (g29_verbose_level > 2) {
-        SERIAL_PROTOCOL("Mesh Point Measured at: ");
+        SERIAL_PROTOCOLPGM("Mesh Point Measured at: ");
         SERIAL_PROTOCOL_F(z_values[location.x_index][location.y_index], 6);
         SERIAL_EOL;
       }
@@ -943,52 +963,40 @@
 
     LEAVE:
     restore_ubl_active_state_and_leave();
+    KEEPALIVE_STATE(IN_HANDLER);
     do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
     do_blocking_move_to_xy(lx, ly);
   }
 
   bool g29_parameter_parsing() {
-
     #if ENABLED(ULTRA_LCD)
       lcd_setstatus("Doing G29 UBL !", true);
       lcd_quick_feedback();
     #endif
 
-    x_pos = current_position[X_AXIS];
-    y_pos = current_position[Y_AXIS];
-    x_flag = y_flag = repeat_flag = false;
-    map_type = 0;
-    constant = 0.0;
-    repetition_cnt = 1;
-
-    if ((x_flag = code_seen('X'))) {
-      x_pos = code_value_float();
-      if (x_pos < X_MIN_POS || x_pos > X_MAX_POS) {
-        SERIAL_PROTOCOLLNPGM("Invalid X location specified.\n");
-        return UBL_ERR;
-      }
+    g29_verbose_level = code_seen('V') ? code_value_int() : 0;
+    if (g29_verbose_level < 0 || g29_verbose_level > 4) {
+      SERIAL_PROTOCOLLNPGM("Invalid Verbose Level specified. (0-4)\n");
+      return UBL_ERR;
     }
 
-    if ((y_flag = code_seen('Y'))) {
-      y_pos = code_value_float();
-      if (y_pos < Y_MIN_POS || y_pos > Y_MAX_POS) {
-        SERIAL_PROTOCOLLNPGM("Invalid Y location specified.\n");
-        return UBL_ERR;
-      }
+    x_flag = code_seen('X') && code_has_value();
+    x_pos = x_flag ? code_value_float() : current_position[X_AXIS];
+    if (x_pos < LOGICAL_X_POSITION(X_MIN_POS) || x_pos > LOGICAL_X_POSITION(X_MAX_POS)) {
+      SERIAL_PROTOCOLLNPGM("Invalid X location specified.\n");
+      return UBL_ERR;
     }
 
-    if (x_flag != y_flag) {
-      SERIAL_PROTOCOLLNPGM("Both X & Y locations must be specified.\n");
+    y_flag = code_seen('Y') && code_has_value();
+    y_pos = y_flag ? code_value_float() : current_position[Y_AXIS];
+    if (y_pos < LOGICAL_Y_POSITION(Y_MIN_POS) || y_pos > LOGICAL_Y_POSITION(Y_MAX_POS)) {
+      SERIAL_PROTOCOLLNPGM("Invalid Y location specified.\n");
       return UBL_ERR;
     }
 
-    g29_verbose_level = 0;
-    if (code_seen('V')) {
-      g29_verbose_level = code_value_int();
-      if (g29_verbose_level < 0 || g29_verbose_level > 4) {
-        SERIAL_PROTOCOLLNPGM("Invalid Verbose Level specified. (0-4)\n");
-        return UBL_ERR;
-      }
+    if (x_flag != y_flag) {
+      SERIAL_PROTOCOLLNPGM("Both X & Y locations must be specified.\n");
+      return UBL_ERR;
     }
 
     if (code_seen('A')) {     // Activate the Unified Bed Leveling System
@@ -997,8 +1005,8 @@
       ubl.store_state();
     }
 
-    if ((c_flag = code_seen('C') && code_has_value()))
-      constant = code_value_float();
+    c_flag = code_seen('C') && code_has_value();
+    ubl_constant = c_flag ? code_value_float() : 0.0;
 
     if (code_seen('D')) {     // Disable the Unified Bed Leveling System
       ubl.state.active = 0;
@@ -1018,29 +1026,28 @@
       }
     #endif
 
-    if ((repeat_flag = code_seen('R'))) {
-      repetition_cnt = code_has_value() ? code_value_int() : 9999;
-      if (repetition_cnt < 1) {
-        SERIAL_PROTOCOLLNPGM("Invalid Repetition count.\n");
-        return UBL_ERR;
-      }
+    repeat_flag = code_seen('R');
+    repetition_cnt = repeat_flag ? (code_has_value() ? code_value_int() : 9999) : 1;
+    if (repetition_cnt < 1) {
+      SERIAL_PROTOCOLLNPGM("Invalid Repetition count.\n");
+      return UBL_ERR;
     }
 
-    if (code_seen('O')) {     // Check if a map type was specified
-      map_type = code_value_int() ? code_has_value() : 0; 
-      if ( map_type<0 || map_type>1) {
-        SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
-        return UBL_ERR;
-      }
+    map_type = code_seen('O') && code_has_value() ? code_value_int() : 0;
+    if (map_type < 0 || map_type > 1) {
+      SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
+      return UBL_ERR;
     }
 
+    /*
     if (code_seen('M')) {     // Check if a map type was specified
-      map_type = code_value_int() ? code_has_value() : 0; 
-      if ( map_type<0 || map_type>1) {
+      map_type = code_has_value() ? code_value_int() : 0; 
+      if (map_type < 0 || map_type > 1) {
         SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
         return UBL_ERR;
       }
     }
+    //*/
 
     return UBL_OK;
   }
@@ -1054,20 +1061,15 @@
 
     SERIAL_PROTOCOL(str);
     SERIAL_PROTOCOL_F(f, 8);
-    SERIAL_PROTOCOL("  ");
+    SERIAL_PROTOCOLPGM("  ");
     ptr = (char*)&f;
-    for (uint8_t i = 0; i < 4; i++) {
-      SERIAL_PROTOCOL("  ");
-      prt_hex_byte(*ptr++);
-    }
-    SERIAL_PROTOCOL("  isnan()=");
-    SERIAL_PROTOCOL(isnan(f));
-    SERIAL_PROTOCOL("  isinf()=");
-    SERIAL_PROTOCOL(isinf(f));
+    for (uint8_t i = 0; i < 4; i++)
+      SERIAL_PROTOCOLPAIR("  ", hex_byte(*ptr++));
+    SERIAL_PROTOCOLPAIR("  isnan()=", isnan(f));
+    SERIAL_PROTOCOLPAIR("  isinf()=", isinf(f));
 
-    constexpr float g = INFINITY;
-    if (f == -g)
-      SERIAL_PROTOCOL("  Minus Infinity detected.");
+    if (f == -INFINITY)
+      SERIAL_PROTOCOLPGM("  Minus Infinity detected.");
 
     SERIAL_EOL;
   }
@@ -1104,7 +1106,6 @@
    */
   void g29_what_command() {
     const uint16_t k = E2END - ubl_eeprom_start;
-    statistics_flag++;
 
     SERIAL_PROTOCOLPGM("Unified Bed Leveling System Version 1.00 ");
     if (ubl.state.active)  
@@ -1117,8 +1118,7 @@
     if (ubl.state.eeprom_storage_slot == -1)
       SERIAL_PROTOCOLPGM("No Mesh Loaded.");
     else {
-      SERIAL_PROTOCOLPGM("Mesh: ");
-      prt_hex_word(ubl.state.eeprom_storage_slot);
+      SERIAL_PROTOCOLPAIR("Mesh ", ubl.state.eeprom_storage_slot);
       SERIAL_PROTOCOLPGM(" Loaded.");
     }
     SERIAL_EOL;
@@ -1136,7 +1136,7 @@
 
     SERIAL_PROTOCOLPGM("X-Axis Mesh Points at: ");
     for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
-      SERIAL_PROTOCOL_F( ubl.map_x_index_to_bed_location(i), 1);
+      SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(ubl.map_x_index_to_bed_location(i)), 1);
       SERIAL_PROTOCOLPGM("  ");
       safe_delay(50);
     }
@@ -1144,7 +1144,7 @@
 
     SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: ");
     for (uint8_t i = 0; i < UBL_MESH_NUM_Y_POINTS; i++) {
-      SERIAL_PROTOCOL_F( ubl.map_y_index_to_bed_location(i), 1);
+      SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(ubl.map_y_index_to_bed_location(i)), 1);
       SERIAL_PROTOCOLPGM("  ");
       safe_delay(50);
     }
@@ -1162,13 +1162,9 @@
     SERIAL_PROTOCOLLNPAIR("ubl_state_recursion_chk :", ubl_state_recursion_chk);
     SERIAL_EOL;
     safe_delay(50);
-    SERIAL_PROTOCOLPGM("Free EEPROM space starts at: 0x");
-    prt_hex_word(ubl_eeprom_start);
-    SERIAL_EOL;
+    SERIAL_PROTOCOLLNPAIR("Free EEPROM space starts at: 0x", hex_word(ubl_eeprom_start));
 
-    SERIAL_PROTOCOLPGM("end of EEPROM              : ");
-    prt_hex_word(E2END);
-    SERIAL_EOL;
+    SERIAL_PROTOCOLLNPAIR("end of EEPROM              : ", hex_word(E2END));
     safe_delay(50);
 
     SERIAL_PROTOCOLLNPAIR("sizeof(ubl) :  ", (int)sizeof(ubl));
@@ -1177,18 +1173,14 @@
     SERIAL_EOL;
     safe_delay(50);
 
-    SERIAL_PROTOCOLPGM("EEPROM free for UBL: 0x");
-    prt_hex_word(k);
-    SERIAL_EOL;
+    SERIAL_PROTOCOLLNPAIR("EEPROM free for UBL: 0x", hex_word(k));
     safe_delay(50);
 
-    SERIAL_PROTOCOLPGM("EEPROM can hold 0x");
-    prt_hex_word(k / sizeof(z_values));
+    SERIAL_PROTOCOLPAIR("EEPROM can hold ", k / sizeof(z_values));
     SERIAL_PROTOCOLLNPGM(" meshes.\n");
     safe_delay(50);
 
-    SERIAL_PROTOCOLPGM("sizeof(ubl.state) :");
-    prt_hex_word(sizeof(ubl.state));
+    SERIAL_PROTOCOLPAIR("sizeof(ubl.state) : ", (int)sizeof(ubl.state));
 
     SERIAL_PROTOCOLPAIR("\nUBL_MESH_NUM_X_POINTS  ", UBL_MESH_NUM_X_POINTS);
     SERIAL_PROTOCOLPAIR("\nUBL_MESH_NUM_Y_POINTS  ", UBL_MESH_NUM_Y_POINTS);
@@ -1222,12 +1214,12 @@
     SERIAL_ECHOLNPGM("EEPROM Dump:");
     for (uint16_t i = 0; i < E2END + 1; i += 16) {
       if (!(i & 0x3)) idle();
-      prt_hex_word(i);
+      print_hex_word(i);
       SERIAL_ECHOPGM(": ");
       for (uint16_t j = 0; j < 16; j++) {
         kkkk = i + j;
         eeprom_read_block(&cccc, (void *)kkkk, 1);
-        prt_hex_byte(cccc);
+        print_hex_byte(cccc);
         SERIAL_ECHO(' ');
       }
       SERIAL_EOL;
@@ -1259,9 +1251,8 @@
     eeprom_read_block((void *)&tmp_z_values, (void *)j, sizeof(tmp_z_values));
 
     SERIAL_ECHOPAIR("Subtracting Mesh ", storage_slot);
-    SERIAL_PROTOCOLPGM(" loaded from EEPROM address ");   // Soon, we can remove the extra clutter of printing
-    prt_hex_word(j);            // the address in the EEPROM where the Mesh is stored.
-    SERIAL_EOL;
+    SERIAL_PROTOCOLLNPAIR(" loaded from EEPROM address ", hex_word(j)); // Soon, we can remove the extra clutter of printing
+                                                                        // the address in the EEPROM where the Mesh is stored.
 
     for (uint8_t x = 0; x < UBL_MESH_NUM_X_POINTS; x++)
       for (uint8_t y = 0; y < UBL_MESH_NUM_Y_POINTS; y++)
@@ -1269,7 +1260,6 @@
   }
 
   mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16], bool far_flag) {
-    int i, j, k, l;
     float distance, closest = far_flag ? -99999.99 : 99999.99;
     mesh_index_pair return_val;
 
@@ -1282,8 +1272,8 @@
     const float px = lx - (probe_as_reference ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
                 py = ly - (probe_as_reference ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0);
 
-    for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
-      for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) {
+    for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
+      for (uint8_t j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) {
 
         if ( (type == INVALID && isnan(z_values[i][j]))  // Check to see if this location holds the right thing
           || (type == REAL && !isnan(z_values[i][j]))
@@ -1292,42 +1282,45 @@
 
           // We only get here if we found a Mesh Point of the specified type
 
-          const float mx = LOGICAL_X_POSITION(ubl.map_x_index_to_bed_location(i)), // Check if we can probe this mesh location
-                      my = LOGICAL_Y_POSITION(ubl.map_y_index_to_bed_location(j));
+          const float rawx = ubl.map_x_index_to_bed_location(i), // Check if we can probe this mesh location
+                      rawy = ubl.map_y_index_to_bed_location(j);
 
-          // If we are using the probe as the reference there are some locations we can't get to.
-          // We prune these out of the list and ignore them until the next Phase where we do the
-          // manual nozzle probing.
+          // If using the probe as the reference there are some unreachable locations.
+          // Prune them from the list and ignore them till the next Phase (manual nozzle probing).
 
           if (probe_as_reference &&
-            (mx < (MIN_PROBE_X) || mx > (MAX_PROBE_X) || my < (MIN_PROBE_Y) || my > (MAX_PROBE_Y))
+            (rawx < (MIN_PROBE_X) || rawx > (MAX_PROBE_X) || rawy < (MIN_PROBE_Y) || rawy > (MAX_PROBE_Y))
           ) continue;
 
-          // We can get to it. Let's see if it is the closest location to the nozzle.
+          // Unreachable. Check if it's the closest location to the nozzle.
           // Add in a weighting factor that considers the current location of the nozzle.
 
+          const float mx = LOGICAL_X_POSITION(rawx), // Check if we can probe this mesh location
+                      my = LOGICAL_Y_POSITION(rawy);
+
           distance = HYPOT(px - mx, py - my) + HYPOT(current_x - mx, current_y - my) * 0.1;
 
-	  if (far_flag) {                                    // If doing the far_flag action, we want to be as far as possible
-            for (k = 0; k < UBL_MESH_NUM_X_POINTS; k++) {    // from the starting point and from any other probed points.  We
-              for (l = 0; l < UBL_MESH_NUM_Y_POINTS; l++) {  // want the next point spread out and filling in any blank spaces
-                if ( !isnan(z_values[k][l])) {               // in the mesh.   So we add in some of the distance to every probed 
-                  distance += (i-k)*(i-k)*MESH_X_DIST*.05;   // point we can find.
-                  distance += (j-l)*(j-l)*MESH_Y_DIST*.05;
-		}
+          if (far_flag) {                                           // If doing the far_flag action, we want to be as far as possible
+            for (uint8_t k = 0; k < UBL_MESH_NUM_X_POINTS; k++) {   // from the starting point and from any other probed points.  We
+              for (uint8_t l = 0; l < UBL_MESH_NUM_Y_POINTS; l++) { // want the next point spread out and filling in any blank spaces
+                if (!isnan(z_values[k][l])) {                       // in the mesh. So we add in some of the distance to every probed
+                  distance += sq(i - k) * (MESH_X_DIST) * .05       // point we can find.
+                            + sq(j - l) * (MESH_Y_DIST) * .05;
+                }
               }
-	    }
-	  }
+            }
+          }
 
-          if ( (!far_flag&&(distance < closest)) || (far_flag&&(distance > closest)) ) {  // if far_flag, look for furthest away point
-            closest = distance;       // We found a closer location with
+          if (far_flag == (distance > closest) && distance != closest) {  // if far_flag, look for farthest point
+            closest = distance;       // We found a closer/farther location with
             return_val.x_index = i;   // the specified type of mesh value.
             return_val.y_index = j;
             return_val.distance = closest;
           }
         }
-      }
-    }
+      } // for j
+    } // for i
+
     return return_val;
   }
 
@@ -1356,27 +1349,30 @@
       bit_clear(not_done, location.x_index, location.y_index);  // Mark this location as 'adjusted' so we will find a
                                                                 // different location the next time through the loop
 
-      const float xProbe = ubl.map_x_index_to_bed_location(location.x_index),
-                  yProbe = ubl.map_y_index_to_bed_location(location.y_index);
-      if (xProbe < X_MIN_POS || xProbe > X_MAX_POS || yProbe < Y_MIN_POS || yProbe > Y_MAX_POS) { // In theory, we don't need this check.
-        SERIAL_PROTOCOLLNPGM("?Error: Attempt to edit off the bed.");                             // This really can't happen, but for now,
-        ubl_has_control_of_lcd_panel = false;                                                         // Let's do the check.
+      const float rawx = ubl.map_x_index_to_bed_location(location.x_index),
+                  rawy = ubl.map_y_index_to_bed_location(location.y_index);
+
+      // TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
+      if (rawx < (X_MIN_POS) || rawx > (X_MAX_POS) || rawy < (Y_MIN_POS) || rawy > (Y_MAX_POS)) { // In theory, we don't need this check.
+        SERIAL_ERROR_START;
+        SERIAL_ERRORLNPGM("Attempt to edit off the bed."); // This really can't happen, but do the check for now
+        ubl_has_control_of_lcd_panel = false;
         goto FINE_TUNE_EXIT;
       }
 
       do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);    // Move the nozzle to where we are going to edit
-      do_blocking_move_to_xy(xProbe, yProbe);
+      do_blocking_move_to_xy(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy));
       float new_z = z_values[location.x_index][location.y_index];
       
       round_off = (int32_t)(new_z * 1000.0);    // we chop off the last digits just to be clean. We are rounding to the
       new_z = float(round_off) / 1000.0;
 
+      KEEPALIVE_STATE(PAUSED_FOR_USER);
       ubl_has_control_of_lcd_panel = true;
 
       lcd_implementation_clear();
       lcd_mesh_edit_setup(new_z);
 
-      wait_for_user = true;
       do {
         new_z = lcd_mesh_edit();
         idle();
@@ -1393,13 +1389,12 @@
         idle();
         if (ELAPSED(millis(), nxt)) {
           lcd_return_to_status();
-//        SERIAL_PROTOCOLLNPGM("\nFine Tuning of Mesh Stopped.");
+          //SERIAL_PROTOCOLLNPGM("\nFine Tuning of Mesh Stopped.");
           do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
           lcd_setstatus("Mesh Editing Stopped", true);
 
           while (ubl_lcd_clicked()) idle();
 
-          ubl_has_control_of_lcd_panel = false;
           goto FINE_TUNE_EXIT;
         }
       }
@@ -1415,6 +1410,7 @@
     FINE_TUNE_EXIT:
 
     ubl_has_control_of_lcd_panel = false;
+    KEEPALIVE_STATE(IN_HANDLER);
 
     if (do_ubl_mesh_map) ubl.display_map(map_type);
     restore_ubl_active_state_and_leave();

Marlin/configuration_store.cpp

@@ -1235,20 +1235,17 @@ void Config_ResetDefault() {
         SERIAL_ECHOPAIR("EEPROM can hold ", (int)((UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values)));
         SERIAL_ECHOLNPGM(" meshes.\n");
 
-        SERIAL_ECHOPAIR("\nUBL_MESH_NUM_X_POINTS  ", UBL_MESH_NUM_X_POINTS);
-        SERIAL_ECHOPAIR("\nUBL_MESH_NUM_Y_POINTS  ", UBL_MESH_NUM_Y_POINTS);
+        SERIAL_ECHOLNPGM("UBL_MESH_NUM_X_POINTS  " STRINGIFY(UBL_MESH_NUM_X_POINTS));
+        SERIAL_ECHOLNPGM("UBL_MESH_NUM_Y_POINTS  " STRINGIFY(UBL_MESH_NUM_Y_POINTS));
 
-        SERIAL_ECHOPAIR("\nUBL_MESH_MIN_X         ", UBL_MESH_MIN_X);
-        SERIAL_ECHOPAIR("\nUBL_MESH_MIN_Y         ", UBL_MESH_MIN_Y);
+        SERIAL_ECHOLNPGM("UBL_MESH_MIN_X         " STRINGIFY(UBL_MESH_MIN_X));
+        SERIAL_ECHOLNPGM("UBL_MESH_MIN_Y         " STRINGIFY(UBL_MESH_MIN_Y));
 
-        SERIAL_ECHOPAIR("\nUBL_MESH_MAX_X         ", UBL_MESH_MAX_X);
-        SERIAL_ECHOPAIR("\nUBL_MESH_MAX_Y         ", UBL_MESH_MAX_Y);
+        SERIAL_ECHOLNPGM("UBL_MESH_MAX_X         " STRINGIFY(UBL_MESH_MAX_X));
+        SERIAL_ECHOLNPGM("UBL_MESH_MAX_Y         " STRINGIFY(UBL_MESH_MAX_Y));
 
-        SERIAL_ECHOPGM("\nMESH_X_DIST        ");
-        SERIAL_ECHO_F(MESH_X_DIST, 6);
-        SERIAL_ECHOPGM("\nMESH_Y_DIST        ");
-        SERIAL_ECHO_F(MESH_Y_DIST, 6);
-        SERIAL_EOL;
+        SERIAL_ECHOLNPGM("MESH_X_DIST        " STRINGIFY(MESH_X_DIST));
+        SERIAL_ECHOLNPGM("MESH_Y_DIST        " STRINGIFY(MESH_Y_DIST));
         SERIAL_EOL;
       }
 

Marlin/hex_print_routines.cpp

@@ -26,22 +26,25 @@
 
 #include "hex_print_routines.h"
 
-void prt_hex_nibble(uint8_t n) {
-  if (n <= 9) 
-    SERIAL_CHAR('0'+n);
-  else
-    SERIAL_CHAR('A' + n - 10);
-  delay(3);
-}
+static char _hex[5] = { 0 };
 
-void prt_hex_byte(uint8_t b) {
-  prt_hex_nibble((b & 0xF0) >> 4);
-  prt_hex_nibble(b & 0x0F);
+char* hex_byte(const uint8_t b) {
+  _hex[0] = hex_nybble(b >> 4);
+  _hex[1] = hex_nybble(b);
+  _hex[2] = '\0';
+  return _hex;
 }
 
-void prt_hex_word(uint16_t w) {
-  prt_hex_byte((w & 0xFF00) >> 8);
-  prt_hex_byte(w & 0x0FF);
+char* hex_word(const uint16_t w) {
+  _hex[0] = hex_nybble(w >> 12);
+  _hex[1] = hex_nybble(w >> 8);
+  _hex[2] = hex_nybble(w >> 4);
+  _hex[3] = hex_nybble(w);
+  return _hex;
 }
 
+void print_hex_nybble(const uint8_t n) { SERIAL_CHAR(hex_nybble(n));  }
+void print_hex_byte(const uint8_t b)   { SERIAL_ECHO(hex_byte(b)); }
+void print_hex_word(const uint16_t w)  { SERIAL_ECHO(hex_word(w)); }
+
 #endif // AUTO_BED_LEVELING_UBL || M100_FREE_MEMORY_WATCHER

Marlin/hex_print_routines.h

@@ -23,11 +23,23 @@
 #ifndef HEX_PRINT_ROUTINES_H
 #define HEX_PRINT_ROUTINES_H
 
+#include "MarlinConfig.h"
+
+#if ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(M100_FREE_MEMORY_WATCHER)
+
 //
-// 3 support routines to print hex numbers.  We can print a nibble, byte and word
+// Utility functions to create and print hex strings as nybble, byte, and word.
 //
-void prt_hex_nibble(uint8_t n);
-void prt_hex_byte(uint8_t b);
-void prt_hex_word(uint16_t w);
 
+inline char hex_nybble(const uint8_t n) {
+  return (n & 0xF) + ((n & 0xF) < 10 ? '0' : 'A' - 10);
+}
+char* hex_byte(const uint8_t b);
+char* hex_word(const uint16_t w);
+
+void print_hex_nybble(const uint8_t n);
+void print_hex_byte(const uint8_t b);
+void print_hex_word(const uint16_t w);
+
+#endif // AUTO_BED_LEVELING_UBL || M100_FREE_MEMORY_WATCHER
 #endif // HEX_PRINT_ROUTINES_H

Marlin/pins_RUMBA.h

@@ -100,7 +100,7 @@
 #endif
 
 #if TEMP_SENSOR_2 == -1
-  #define TEMP_2_PIN        7   // Analog Input (connector *K3* on RUMBA thermocouple ADD ON is used <-- this can not be used when TEMP_SENSOR_BED is defined as thermocouple)
+  #define TEMP_2_PIN        7   // Analog Input (connector *K3* on RUMBA thermocouple ADD ON is used <-- this can't be used when TEMP_SENSOR_BED is defined as thermocouple)
 #else
   #define TEMP_2_PIN       13   // Analog Input (default connector for thermistor *T2* on rumba board is used)
 #endif
@@ -109,7 +109,7 @@
 //#define TEMP_X_PIN         12   // Analog Input (default connector for thermistor *T3* on rumba board is used)
 
 #if TEMP_SENSOR_BED == -1
-  #define TEMP_BED_PIN      7   // Analog Input (connector *K3* on RUMBA thermocouple ADD ON is used <-- this can not be used when TEMP_SENSOR_2 is defined as thermocouple)
+  #define TEMP_BED_PIN      7   // Analog Input (connector *K3* on RUMBA thermocouple ADD ON is used <-- this can't be used when TEMP_SENSOR_2 is defined as thermocouple)
 #else
   #define TEMP_BED_PIN     11   // Analog Input (default connector for thermistor *THB* on rumba board is used)
 #endif

Marlin/stepper.cpp

@@ -1265,7 +1265,7 @@ void Stepper::report_positions() {
     #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
       uint32_t pulse_start;
     #endif
-    
+
     switch (axis) {
 
       case X_AXIS:

Marlin/ultralcd.cpp

@@ -859,65 +859,45 @@ void kill_screen(const char* lcd_msg) {
     static int ubl_encoderPosition = 0;
 
     static void _lcd_mesh_fine_tune(const char* msg) {
-//    static millis_t next_click = 0;             // We are going to accelerate the number speed when the wheel
-//                                                // turns fast.   But that isn't implemented yet
-      int16_t last_digit;
-      int32_t rounded;
-
       defer_return_to_status = true;
       if (ubl_encoderDiff) {
-        if ( ubl_encoderDiff > 0 ) 
-          ubl_encoderPosition = 1;
-        else {
-          ubl_encoderPosition = -1;
-        }
-
+        ubl_encoderPosition = (ubl_encoderDiff > 0) ? 1 : -1;
         ubl_encoderDiff = 0;
-//      next_click = millis();
 
-        mesh_edit_accumulator += ( (float) (ubl_encoderPosition)) * .005 / 2.0 ;
+        mesh_edit_accumulator += float(ubl_encoderPosition) * 0.005 / 2.0;
         mesh_edit_value = mesh_edit_accumulator;
         encoderPosition = 0;
         lcdDrawUpdate = LCDVIEW_REDRAW_NOW;
 
-        rounded = (int32_t)(mesh_edit_value * 1000.0);
-        last_digit = rounded % 5L; //10L;
-        rounded -= last_digit;
-        mesh_edit_value = float(rounded) / 1000.0;
+        const int32_t rounded = (int32_t)(mesh_edit_value * 1000.0);
+        mesh_edit_value = float(rounded - (rounded % 5L)) / 1000.0;
       }
 
       if (lcdDrawUpdate)
         lcd_implementation_drawedit(msg, ftostr43sign(mesh_edit_value));
     }
 
-
     void _lcd_mesh_edit_NOP() {
       defer_return_to_status = true;
     }
 
-
     void _lcd_mesh_edit() {
       _lcd_mesh_fine_tune(PSTR("Mesh Editor: "));
-      defer_return_to_status = true;
     }
 
     float lcd_mesh_edit() {
       lcd_goto_screen(_lcd_mesh_edit_NOP);
       _lcd_mesh_fine_tune(PSTR("Mesh Editor: "));
-      defer_return_to_status = true;
       return mesh_edit_value;
     }
 
     void lcd_mesh_edit_setup(float initial) {
       mesh_edit_value = mesh_edit_accumulator = initial;
       lcd_goto_screen(_lcd_mesh_edit_NOP);
-      mesh_edit_value = mesh_edit_accumulator = initial;
-      defer_return_to_status = true; 
     }
 
     void _lcd_z_offset_edit() {
       _lcd_mesh_fine_tune(PSTR("Z-Offset: "));
-      defer_return_to_status = true;
     }
 
     float lcd_z_offset_edit() {