From 6f89db11f02a61d79728bc586f2c629db93d055d Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 11:51:29 -0500
Subject: [PATCH 01/13] Allow lcd_setstatusPGM to reset the alert level

---
 Marlin/G26_Mesh_Validation_Tool.cpp | 30 +++++++++++++----------------
 Marlin/ubl_G29.cpp                  |  2 +-
 Marlin/ultralcd.cpp                 | 11 ++++++-----
 Marlin/ultralcd.h                   |  6 +++---
 4 files changed, 23 insertions(+), 26 deletions(-)

diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp
index a17adda10f..56f2059bb8 100644
--- a/Marlin/G26_Mesh_Validation_Tool.cpp
+++ b/Marlin/G26_Mesh_Validation_Tool.cpp
@@ -131,7 +131,7 @@
   void set_destination_to_current();
   void set_current_to_destination();
   void prepare_move_to_destination();
-  void lcd_setstatuspgm(const char* const message, const uint8_t level);
+  void lcd_setstatusPGM(const char* const message, const int8_t level);
   void sync_plan_position_e();
   void chirp_at_user();
 
@@ -181,18 +181,17 @@
     safe_delay(10);                       // Wait for click to settle
 
     #if ENABLED(ULTRA_LCD)
-      lcd_setstatuspgm(PSTR("Mesh Validation Stopped."), 99);
+      lcd_setstatusPGM(PSTR("Mesh Validation Stopped."), 99);
       lcd_quick_feedback();
     #endif
-    lcd_reset_alert_level();
 
     while (!ubl_lcd_clicked()) idle();    // Wait for button release
 
     // If the button is suddenly pressed again,
     // ask the user to resolve the issue
-    lcd_setstatuspgm(PSTR("Release button"), 99); // will never appear...
+    lcd_setstatusPGM(PSTR("Release button"), 99); // will never appear...
     while (ubl_lcd_clicked()) idle();             // unless this loop happens
-    lcd_setstatuspgm(PSTR(""));
+    lcd_setstatusPGM(PSTR(""), -1);
 
     return true;
   }
@@ -351,8 +350,7 @@
     } while (--g26_repeats && location.x_index >= 0 && location.y_index >= 0);
 
     LEAVE:
-    lcd_reset_alert_level();
-    lcd_setstatuspgm(PSTR("Leaving G26"));
+    lcd_setstatusPGM(PSTR("Leaving G26"), -1);
 
     retract_filament(destination);
     destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;
@@ -726,8 +724,7 @@
   }
 
   bool unified_bed_leveling::exit_from_g26() {
-    lcd_reset_alert_level();
-    lcd_setstatuspgm(PSTR("Leaving G26"));
+    lcd_setstatusPGM(PSTR("Leaving G26"), -1);
     while (ubl_lcd_clicked()) idle();
     return UBL_ERR;
   }
@@ -741,7 +738,7 @@
     #if HAS_TEMP_BED
       #if ENABLED(ULTRA_LCD)
         if (g26_bed_temp > 25) {
-          lcd_setstatuspgm(PSTR("G26 Heating Bed."), 99);
+          lcd_setstatusPGM(PSTR("G26 Heating Bed."), 99);
           lcd_quick_feedback();
       #endif
           has_control_of_lcd_panel = true;
@@ -757,7 +754,7 @@
           }
       #if ENABLED(ULTRA_LCD)
         }
-        lcd_setstatuspgm(PSTR("G26 Heating Nozzle."), 99);
+        lcd_setstatusPGM(PSTR("G26 Heating Nozzle."), 99);
         lcd_quick_feedback();
       #endif
     #endif
@@ -774,8 +771,7 @@
     }
 
     #if ENABLED(ULTRA_LCD)
-      lcd_reset_alert_level();
-      lcd_setstatuspgm(PSTR(""));
+      lcd_setstatusPGM(PSTR(""), -1);
       lcd_quick_feedback();
     #endif
 
@@ -792,7 +788,7 @@
 
       has_control_of_lcd_panel = true;
 
-      lcd_setstatuspgm(PSTR("User-Controlled Prime"), 99);
+      lcd_setstatusPGM(PSTR("User-Controlled Prime"), 99);
       chirp_at_user();
 
       set_destination_to_current();
@@ -819,9 +815,9 @@
       while (ubl_lcd_clicked()) idle();           // Debounce Encoder Wheel
 
       #if ENABLED(ULTRA_LCD)
-        strcpy_P(lcd_status_message, PSTR("Done Priming")); // We can't do lcd_setstatuspgm() without having it continue;
+        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_setstatusPGM(PSTR("Done Priming"), 99);
         lcd_quick_feedback();
       #endif
 
@@ -830,7 +826,7 @@
     }
     else {
       #if ENABLED(ULTRA_LCD)
-        lcd_setstatuspgm(PSTR("Fixed Length Prime."), 99);
+        lcd_setstatusPGM(PSTR("Fixed Length Prime."), 99);
         lcd_quick_feedback();
       #endif
       set_destination_to_current();
diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp
index c9abfbd5cf..c4f77dceb4 100644
--- a/Marlin/ubl_G29.cpp
+++ b/Marlin/ubl_G29.cpp
@@ -58,7 +58,7 @@
   typedef void (*screenFunc_t)();
   extern void lcd_goto_screen(screenFunc_t screen, const uint32_t encoder = 0);
   extern void lcd_setstatus(const char* message, const bool persist);
-  extern void lcd_setstatuspgm(const char* message, const uint8_t level);
+  extern void lcd_setstatusPGM(const char* message, const int8_t level);
 
   int    unified_bed_leveling::g29_verbose_level,
          unified_bed_leveling::g29_phase_value,
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 4e21ba0792..340178835f 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -680,7 +680,7 @@ void kill_screen(const char* lcd_msg) {
       #if ENABLED(PARK_HEAD_ON_PAUSE)
         enqueue_and_echo_commands_P(PSTR("M125"));
       #endif
-      lcd_setstatuspgm(PSTR(MSG_PRINT_PAUSED), true);
+      lcd_setstatusPGM(PSTR(MSG_PRINT_PAUSED), -1);
     }
 
     void lcd_sdcard_resume() {
@@ -690,7 +690,7 @@ void kill_screen(const char* lcd_msg) {
         card.startFileprint();
         print_job_timer.start();
       #endif
-      lcd_setstatuspgm(PSTR(""), true);
+      lcd_setstatusPGM(PSTR(""), -1);
     }
 
     void lcd_sdcard_stop() {
@@ -703,7 +703,7 @@ void kill_screen(const char* lcd_msg) {
         for (uint8_t i = 0; i < FAN_COUNT; i++) fanSpeeds[i] = 0;
       #endif
       wait_for_heatup = false;
-      LCD_MESSAGEPGM(MSG_PRINT_ABORTED);
+      lcd_setstatusPGM(PSTR(MSG_PRINT_PAUSED), -1);
     }
 
   #endif // SDSUPPORT
@@ -4095,7 +4095,8 @@ void lcd_setstatus(const char * const message, const bool persist) {
   lcd_finishstatus(persist);
 }
 
-void lcd_setstatuspgm(const char * const message, const uint8_t level) {
+void lcd_setstatusPGM(const char * const message, int8_t level) {
+  if (level < 0) level = lcd_status_message_level = 0;
   if (level < lcd_status_message_level) return;
   lcd_status_message_level = level;
   strncpy_P(lcd_status_message, message, 3 * (LCD_WIDTH));
@@ -4113,7 +4114,7 @@ void lcd_status_printf_P(const uint8_t level, const char * const fmt, ...) {
 }
 
 void lcd_setalertstatuspgm(const char * const message) {
-  lcd_setstatuspgm(message, 1);
+  lcd_setstatusPGM(message, 1);
   #if ENABLED(ULTIPANEL)
     lcd_return_to_status();
   #endif
diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h
index c6a11ee0bf..36f3b7d656 100644
--- a/Marlin/ultralcd.h
+++ b/Marlin/ultralcd.h
@@ -38,7 +38,7 @@
   void lcd_init();
   bool lcd_hasstatus();
   void lcd_setstatus(const char* message, const bool persist=false);
-  void lcd_setstatuspgm(const char* message, const uint8_t level=0);
+  void lcd_setstatusPGM(const char* message, const int8_t level=0);
   void lcd_status_printf_P(const uint8_t level, const char * const fmt, ...);
   void lcd_setalertstatuspgm(const char* message);
   void lcd_reset_alert_level();
@@ -61,7 +61,7 @@
     void bootscreen();
   #endif
 
-  #define LCD_MESSAGEPGM(x) lcd_setstatuspgm(PSTR(x))
+  #define LCD_MESSAGEPGM(x) lcd_setstatusPGM(PSTR(x))
   #define LCD_ALERTMESSAGEPGM(x) lcd_setalertstatuspgm(PSTR(x))
 
   #define LCD_UPDATE_INTERVAL 100
@@ -153,7 +153,7 @@
   inline void lcd_init() {}
   inline bool lcd_hasstatus() { return false; }
   inline void lcd_setstatus(const char* const message, const bool persist=false) { UNUSED(message); UNUSED(persist); }
-  inline void lcd_setstatuspgm(const char* const message, const uint8_t level=0) { UNUSED(message); UNUSED(level); }
+  inline void lcd_setstatusPGM(const char* const message, const int8_t level=0) { UNUSED(message); UNUSED(level); }
   inline void lcd_status_printf_P(const uint8_t level, const char * const fmt, ...) { UNUSED(level); UNUSED(fmt); }
   inline void lcd_buttons_update() {}
   inline void lcd_reset_alert_level() {}

From 194f8b2f55ec4d7298de9e60793a313f59ef816e Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 11:52:02 -0500
Subject: [PATCH 02/13] Start at Z=0 in manual probing

---
 Marlin/Marlin_main.cpp | 2 +-
 Marlin/ultralcd.cpp    | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 1d3c282ff6..8335d0672d 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -3839,7 +3839,7 @@ void home_all_axes() { gcode_G28(true); }
 
     #if MANUAL_PROBE_HEIGHT > 0
       feedrate_mm_s = homing_feedrate_mm_s[Z_AXIS];
-      current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS) + 0.2; // just slightly over the bed
+      current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS); // just slightly over the bed
       line_to_current_position();
     #endif
 
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 340178835f..a8c39f4d95 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -1432,7 +1432,7 @@ void kill_screen(const char* lcd_msg) {
         current_position[Y_AXIS] = LOGICAL_Y_POSITION(y);
         planner.buffer_line_kinematic(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder);
         #if MANUAL_PROBE_HEIGHT > 0
-          current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS) + 0.2;
+          current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS);
           line_to_current(Z_AXIS);
         #endif
         lcd_synchronize();

From eb39d6e3e24245e474189e003a8d3acc635ca8da Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 11:52:42 -0500
Subject: [PATCH 03/13] Don't draw progress bar until it has 1%

---
 Marlin/ultralcd_impl_HD44780.h | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/Marlin/ultralcd_impl_HD44780.h b/Marlin/ultralcd_impl_HD44780.h
index b735baa8fc..d650890d50 100644
--- a/Marlin/ultralcd_impl_HD44780.h
+++ b/Marlin/ultralcd_impl_HD44780.h
@@ -806,8 +806,10 @@ static void lcd_implementation_status_screen() {
 
     // Draw the progress bar if the message has shown long enough
     // or if there is no message set.
-    if (card.isFileOpen() && (ELAPSED(millis(), progress_bar_ms + PROGRESS_BAR_MSG_TIME) || !lcd_status_message[0]))
-      return lcd_draw_progress_bar(card.percentDone());
+    if (card.isFileOpen() && (ELAPSED(millis(), progress_bar_ms + PROGRESS_BAR_MSG_TIME) || !lcd_status_message[0])) {
+      const uint8_t percent = card.percentDone();
+      if (percent) return lcd_draw_progress_bar(percent);
+    }
 
   #elif ENABLED(FILAMENT_LCD_DISPLAY) && ENABLED(SDSUPPORT)
 

From 78d8c598e1be6473c5d71feada47b9d3edef006a Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 12:19:02 -0500
Subject: [PATCH 04/13] Fix lcd_synchronize with message

---
 Marlin/ultralcd.cpp | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index a8c39f4d95..c6978d6f03 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -481,21 +481,28 @@ uint16_t max_display_update_time = 0;
   /**
    * Show "Moving..." till moves are done, then revert to previous display.
    */
-  inline void lcd_synchronize(const char * const msg=NULL) {
+  static const char moving[] PROGMEM = MSG_MOVING;
+  static const char *sync_message = moving;
+
+  void _lcd_synchronize() {
     static bool no_reentry = false;
-    const static char moving[] PROGMEM = MSG_MOVING;
-    lcd_implementation_drawmenu_static(LCD_HEIGHT >= 4 ? 1 : 0, msg ? msg : moving);
+    if (lcdDrawUpdate) lcd_implementation_drawmenu_static(LCD_HEIGHT >= 4 ? 1 : 0, sync_message);
     if (no_reentry) return;
 
     // Make this the current handler till all moves are done
     no_reentry = true;
     screenFunc_t old_screen = currentScreen;
-    lcd_goto_screen(lcd_synchronize);
+    lcd_goto_screen(_lcd_synchronize);
     stepper.synchronize();
     no_reentry = false;
     lcd_goto_screen(old_screen);
   }
 
+  void lcd_synchronize(const char * const msg=NULL) {
+    sync_message = msg ? msg : moving;
+    _lcd_synchronize();
+  }
+
   void lcd_return_to_status() { lcd_goto_screen(lcd_status_screen); }
 
   void lcd_save_previous_screen() {

From f1e33afda4128aeb17f7ed937d1100fe28ad9ea5 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 12:19:41 -0500
Subject: [PATCH 05/13] Return to Bed Leveling menu when done

---
 Marlin/ultralcd.cpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index c6978d6f03..07bc183b06 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -1504,7 +1504,8 @@ void kill_screen(const char* lcd_msg) {
 
           #endif
 
-          lcd_return_to_status();
+          lcd_goto_previous_menu(); // Return to the last clicked item ("Level Bed")
+
           //LCD_MESSAGEPGM(MSG_LEVEL_BED_DONE);
           lcd_completion_feedback();
         }
@@ -1627,8 +1628,8 @@ void kill_screen(const char* lcd_msg) {
     /**
      * Step 1: Bed Level entry-point
      *  - Cancel
-     *  - Level Bed >
      *  - Leveling On/Off (if there is leveling data)
+     *  - Level Bed >
      *  - Fade Height (Req: ENABLE_LEVELING_FADE_HEIGHT)
      *  - Mesh Z Offset (Req: MESH_BED_LEVELING)
      *  - Z Probe Offset (Req: HAS_BED_PROBE, Opt: BABYSTEP_ZPROBE_OFFSET)
@@ -1638,8 +1639,8 @@ void kill_screen(const char* lcd_msg) {
     void lcd_level_bed() {
       START_MENU();
       MENU_BACK(MSG_PREPARE);
-      MENU_ITEM(submenu, MSG_LEVEL_BED, _lcd_level_bed_continue);
-      if (leveling_is_valid()) {      // Leveling data exists? Show more options.
+
+      if (leveling_is_valid()) {
         _level_state = leveling_is_active();
         MENU_ITEM_EDIT_CALLBACK(bool, MSG_BED_LEVELING, &_level_state, _lcd_toggle_bed_leveling);
       }
@@ -1660,6 +1661,8 @@ void kill_screen(const char* lcd_msg) {
         MENU_ITEM_EDIT_CALLBACK(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX, lcd_refresh_zprobe_zoffset);
       #endif
 
+      MENU_ITEM(submenu, MSG_LEVEL_BED, _lcd_level_bed_continue);
+
       #if ENABLED(EEPROM_SETTINGS)
         MENU_ITEM(function, MSG_LOAD_EEPROM, lcd_load_settings);
         MENU_ITEM(function, MSG_STORE_EEPROM, lcd_store_settings);

From 50ab9c2e04bcb378c1574cd8134cdf0a483530c0 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 12:52:56 -0500
Subject: [PATCH 06/13] General cleanup of spacing, comments

---
 Marlin/Marlin.h        |  4 ++--
 Marlin/Marlin_main.cpp | 18 +++++++++---------
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index c18f5db53e..c2e84e6281 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -217,8 +217,8 @@ extern bool volumetric_enabled;
 extern int flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder
 extern float filament_size[EXTRUDERS]; // cross-sectional area of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder.
 extern float volumetric_multiplier[EXTRUDERS]; // reciprocal of cross-sectional area of filament (in square millimeters), stored this way to reduce computational burden in planner
-extern bool axis_known_position[XYZ]; // axis[n].is_known
-extern bool axis_homed[XYZ]; // axis[n].is_homed
+extern bool axis_known_position[XYZ];
+extern bool axis_homed[XYZ];
 extern volatile bool wait_for_heatup;
 
 #if HAS_RESUME_CONTINUE
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 8335d0672d..b26f3ec278 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1743,15 +1743,15 @@ static void clean_up_after_endstop_or_probe_move() {
 #if HAS_PROBING_PROCEDURE || HOTENDS > 1 || ENABLED(Z_PROBE_ALLEN_KEY) || ENABLED(Z_PROBE_SLED) || ENABLED(NOZZLE_CLEAN_FEATURE) || ENABLED(NOZZLE_PARK_FEATURE) || ENABLED(DELTA_AUTO_CALIBRATION)
 
   bool axis_unhomed_error(const bool x/*=true*/, const bool y/*=true*/, const bool z/*=true*/) {
-#if ENABLED(HOME_AFTER_DEACTIVATE)
-    const bool xx = x && !axis_known_position[X_AXIS],
-               yy = y && !axis_known_position[Y_AXIS],
-               zz = z && !axis_known_position[Z_AXIS];
-#else
-    const bool xx = x && !axis_homed[X_AXIS],
-               yy = y && !axis_homed[Y_AXIS],
-               zz = z && !axis_homed[Z_AXIS];
-#endif
+    #if ENABLED(HOME_AFTER_DEACTIVATE)
+      const bool xx = x && !axis_known_position[X_AXIS],
+                 yy = y && !axis_known_position[Y_AXIS],
+                 zz = z && !axis_known_position[Z_AXIS];
+    #else
+      const bool xx = x && !axis_homed[X_AXIS],
+                 yy = y && !axis_homed[Y_AXIS],
+                 zz = z && !axis_homed[Z_AXIS];
+    #endif
     if (xx || yy || zz) {
       SERIAL_ECHO_START;
       SERIAL_ECHOPGM(MSG_HOME " ");

From b2d3c8aedd458cd63090922bd8b86bba24bd1ed9 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 15:15:13 -0500
Subject: [PATCH 07/13] Have G28 do a refresh of the display

---
 Marlin/Marlin_main.cpp | 2 ++
 Marlin/ultralcd.h      | 4 ++++
 2 files changed, 6 insertions(+)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index b26f3ec278..b742533c43 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -3798,6 +3798,8 @@ inline void gcode_G28(const bool always_home_all) {
     tool_change(old_tool_index, 0, true);
   #endif
 
+  lcd_refresh();
+
   report_current_position();
 
   #if ENABLED(DEBUG_LEVELING_FEATURE)
diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h
index 36f3b7d656..13da79e8a5 100644
--- a/Marlin/ultralcd.h
+++ b/Marlin/ultralcd.h
@@ -46,6 +46,9 @@
   void kill_screen(const char* lcd_msg);
   bool lcd_detected(void);
 
+  extern uint8_t lcdDrawUpdate;
+  inline void lcd_refresh() { lcdDrawUpdate = LCDVIEW_CLEAR_CALL_REDRAW; }
+
   #if HAS_BUZZER
     void lcd_buzz(long duration, uint16_t freq);
   #endif
@@ -158,6 +161,7 @@
   inline void lcd_buttons_update() {}
   inline void lcd_reset_alert_level() {}
   inline bool lcd_detected() { return true; }
+  inline void lcd_refresh() {}
 
   #define LCD_MESSAGEPGM(x) NOOP
   #define LCD_ALERTMESSAGEPGM(x) NOOP

From 2d715691384c5f1f54d9ea3433b9c061c218b342 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 15:44:20 -0500
Subject: [PATCH 08/13] Fix spacing in JSON output

---
 Marlin/Marlin_main.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index b742533c43..bab6f8057c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2467,7 +2467,7 @@ static void clean_up_after_endstop_or_probe_move() {
     #endif
     for (uint8_t y = 0; y < sy; y++) {
       #ifdef SCAD_MESH_OUTPUT
-        SERIAL_PROTOCOLLNPGM(" [");           // open sub-array
+        SERIAL_PROTOCOLPGM(" [");           // open sub-array
       #else
         if (y < 10) SERIAL_PROTOCOLCHAR(' ');
         SERIAL_PROTOCOL((int)y);
@@ -2501,7 +2501,7 @@ static void clean_up_after_endstop_or_probe_move() {
       SERIAL_EOL;
     }
     #ifdef SCAD_MESH_OUTPUT
-      SERIAL_PROTOCOLPGM("\n];");                     // close 2D array
+      SERIAL_PROTOCOLPGM("];");                       // close 2D array
     #endif
     SERIAL_EOL;
   }

From 4cece2d72e283d7c21be158cdde43b2c4495cdbb Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 15:59:55 -0500
Subject: [PATCH 09/13] Use NAN for G29 W omitted parameters

---
 Marlin/Marlin_main.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index bab6f8057c..6003b78816 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -4233,19 +4233,19 @@ void home_all_axes() { gcode_G28(true); }
             return;
           }
 
-          const float z = parser.seen('Z') && parser.has_value() ? parser.value_float() : 99999;
-          if (!WITHIN(z, -10, 10)) {
+          const float z = parser.seen('Z') && parser.has_value() ? parser.value_float() : NAN;
+          if (!isnan(z) || !WITHIN(z, -10, 10)) {
             SERIAL_ERROR_START;
             SERIAL_ERRORLNPGM("Bad Z value");
             return;
           }
 
-          const float x = parser.seen('X') && parser.has_value() ? parser.value_float() : 99999,
-                      y = parser.seen('Y') && parser.has_value() ? parser.value_float() : 99999;
+          const float x = parser.seen('X') && parser.has_value() ? parser.value_float() : NAN,
+                      y = parser.seen('Y') && parser.has_value() ? parser.value_float() : NAN;
           int8_t i = parser.seen('I') && parser.has_value() ? parser.value_byte() : -1,
                  j = parser.seen('J') && parser.has_value() ? parser.value_byte() : -1;
 
-          if (x < 99998 && y < 99998) {
+          if (!isnan(x) && !isnan(y)) {
             // Get nearest i / j from x / y
             i = (x - LOGICAL_X_POSITION(bilinear_start[X_AXIS]) + 0.5 * xGridSpacing) / xGridSpacing;
             j = (y - LOGICAL_Y_POSITION(bilinear_start[Y_AXIS]) + 0.5 * yGridSpacing) / yGridSpacing;

From c6a7adc29359b135e77e0f0f2c171ab1dd3c99b3 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 16:00:57 -0500
Subject: [PATCH 10/13] Patch G29 A and Q arguments to bypass setup

---
 Marlin/Marlin_main.cpp | 28 ++++++++++++++++++++++------
 1 file changed, 22 insertions(+), 6 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 6003b78816..158b552d36 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -4134,8 +4134,14 @@ void home_all_axes() { gcode_G28(true); }
       #endif
     #endif
 
+    #if ENABLED(PROBE_MANUALLY)
+      const bool seenA = parser.seen('A'), seenQ = parser.seen('Q'), no_action = seenA || seenQ;
+    #endif
+
     #if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY)
       const bool faux = parser.seen('C') && parser.value_bool();
+    #elif ENABLED(PROBE_MANUALLY)
+      const bool faux = no_action;
     #else
       bool constexpr faux = false;
     #endif
@@ -4281,7 +4287,11 @@ void home_all_axes() { gcode_G28(true); }
         return;
       }
 
-      dryrun = parser.seen('D') && parser.value_bool();
+      dryrun = (parser.seen('D') && parser.value_bool())
+        #if ENABLED(PROBE_MANUALLY)
+          || no_action
+        #endif
+      ;
 
       #if ENABLED(AUTO_BED_LEVELING_LINEAR)
 
@@ -4426,16 +4436,14 @@ void home_all_axes() { gcode_G28(true); }
 
     #if ENABLED(PROBE_MANUALLY)
 
-      const bool seenA = parser.seen('A'), seenQ = parser.seen('Q');
-
       // For manual probing, get the next index to probe now.
       // On the first probe this will be incremented to 0.
-      if (!seenA && !seenQ) {
+      if (!no_action) {
         ++abl_probe_index;
         g29_in_progress = true;
       }
 
-      // Abort current G29 procedure, go back to ABLStart
+      // Abort current G29 procedure, go back to idle state
       if (seenA && g29_in_progress) {
         SERIAL_PROTOCOLLNPGM("Manual G29 aborted");
         #if HAS_SOFTWARE_ENDSTOPS
@@ -4459,7 +4467,7 @@ void home_all_axes() { gcode_G28(true); }
           SERIAL_PROTOCOLLNPGM("idle");
       }
 
-      if (seenA || seenQ) return;
+      if (no_action) return;
 
       if (abl_probe_index == 0) {
         // For the initial G29 save software endstop state
@@ -4484,6 +4492,14 @@ void home_all_axes() { gcode_G28(true); }
 
           z_values[xCount][yCount] = measured_z + zoffset;
 
+          #if ENABLED(DEBUG_LEVELING_FEATURE)
+            if (DEBUGGING(LEVELING)) {
+              SERIAL_PROTOCOLPAIR("Save X", xCount);
+              SERIAL_PROTOCOLPAIR(" Y", yCount);
+              SERIAL_PROTOCOLLNPAIR(" Z", measured_z + zoffset);
+            }
+          #endif
+
         #elif ENABLED(AUTO_BED_LEVELING_3POINT)
 
           points[i].z = measured_z;

From fc2eaab7f368aca6699a3c28ebb55a1ffeb0b7a6 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 12:47:35 -0500
Subject: [PATCH 11/13] Show home option in level bed menu

---
 Marlin/ultralcd.cpp | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 07bc183b06..fc3d278817 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -1636,11 +1636,13 @@ void kill_screen(const char* lcd_msg) {
      *  - Load Settings (Req: EEPROM_SETTINGS)
      *  - Save Settings (Req: EEPROM_SETTINGS)
      */
-    void lcd_level_bed() {
+    void lcd_bed_leveling() {
       START_MENU();
       MENU_BACK(MSG_PREPARE);
 
-      if (leveling_is_valid()) {
+      if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS]))
+        MENU_ITEM(gcode, MSG_AUTO_HOME, PSTR("G28"));
+      else if (leveling_is_valid()) {
         _level_state = leveling_is_active();
         MENU_ITEM_EDIT_CALLBACK(bool, MSG_BED_LEVELING, &_level_state, _lcd_toggle_bed_leveling);
       }
@@ -1650,7 +1652,9 @@ void kill_screen(const char* lcd_msg) {
         MENU_MULTIPLIER_ITEM_EDIT_CALLBACK(float62, MSG_Z_FADE_HEIGHT, &planner.z_fade_height, 0.0, 100.0, _lcd_set_z_fade_height);
       #endif
 
-      // Manual bed leveling, Bed Z:
+      //
+      // MBL Z Offset
+      //
       #if ENABLED(MESH_BED_LEVELING)
         MENU_ITEM_EDIT(float43, MSG_BED_Z, &mbl.z_offset, -1, 1);
       #endif
@@ -2073,7 +2077,7 @@ void kill_screen(const char* lcd_msg) {
       #if ENABLED(PROBE_MANUALLY)
         if (!g29_in_progress)
       #endif
-      MENU_ITEM(submenu, MSG_BED_LEVELING, lcd_level_bed);
+      MENU_ITEM(submenu, MSG_BED_LEVELING, lcd_bed_leveling);
     #endif
 
     #if HAS_M206_COMMAND

From 9677f3f2f559b47b670ea276210da588c8c6568c Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 16:01:17 -0500
Subject: [PATCH 12/13] Patch up LCD Bed Leveling menu

---
 Marlin/Marlin_main.cpp |   2 +-
 Marlin/ultralcd.cpp    | 122 +++++++++++++++++++++++++----------------
 2 files changed, 77 insertions(+), 47 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 158b552d36..c62fd7db7c 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -4460,7 +4460,7 @@ void home_all_axes() { gcode_G28(true); }
       if (verbose_level || seenQ) {
         SERIAL_PROTOCOLPGM("Manual G29 ");
         if (g29_in_progress) {
-          SERIAL_PROTOCOLPAIR("point ", abl_probe_index + 1);
+          SERIAL_PROTOCOLPAIR("point ", min(abl_probe_index + 1, abl2));
           SERIAL_PROTOCOLLNPAIR(" of ", abl2);
         }
         else
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index fc3d278817..b8d3380e84 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -484,6 +484,11 @@ uint16_t max_display_update_time = 0;
   static const char moving[] PROGMEM = MSG_MOVING;
   static const char *sync_message = moving;
 
+  //
+  // Display the synchronize screen until moves are
+  // finished, and don't return to the caller until
+  // done. ** This blocks the command queue! **
+  //
   void _lcd_synchronize() {
     static bool no_reentry = false;
     if (lcdDrawUpdate) lcd_implementation_drawmenu_static(LCD_HEIGHT >= 4 ? 1 : 0, sync_message);
@@ -498,6 +503,8 @@ uint16_t max_display_update_time = 0;
     lcd_goto_screen(old_screen);
   }
 
+  // Display the synchronize screen with a custom message
+  // ** This blocks the command queue! **
   void lcd_synchronize(const char * const msg=NULL) {
     sync_message = msg ? msg : moving;
     _lcd_synchronize();
@@ -1427,6 +1434,26 @@ void kill_screen(const char* lcd_msg) {
       #endif
     );
 
+    //
+    // Raise Z to the "manual probe height"
+    // Don't return until done.
+    // ** This blocks the command queue! **
+    //
+    void _lcd_after_probing() {
+      #if MANUAL_PROBE_HEIGHT > 0
+        current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS) + MANUAL_PROBE_HEIGHT;
+        line_to_current(Z_AXIS);
+      #endif
+      // Display "Done" screen and wait for moves to complete
+      #if MANUAL_PROBE_HEIGHT > 0 || ENABLED(MESH_BED_LEVELING)
+        lcd_synchronize(PSTR(MSG_LEVEL_BED_DONE));
+      #endif
+      lcd_goto_previous_menu();
+      lcd_completion_feedback();
+      defer_return_to_status = false;
+      //LCD_MESSAGEPGM(MSG_LEVEL_BED_DONE);
+    }
+
     #if ENABLED(MESH_BED_LEVELING)
 
       // Utility to go to the next mesh point
@@ -1445,12 +1472,22 @@ void kill_screen(const char* lcd_msg) {
         lcd_synchronize();
       }
 
-    #endif // MESH_BED_LEVELING
+    #elif ENABLED(PROBE_MANUALLY)
 
-    void _lcd_level_bed_done() {
-      if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR(MSG_LEVEL_BED_DONE));
-      lcdDrawUpdate = LCDVIEW_CALL_REDRAW_NEXT;
-    }
+      bool lcd_wait_for_move;
+
+      //
+      // Bed leveling is done. Wait for G29 to complete.
+      // A flag is used so that this can release control
+      // and allow the command queue to be processed.
+      //
+      void _lcd_level_bed_done() {
+        if (!lcd_wait_for_move) _lcd_after_probing();
+        if (lcdDrawUpdate) lcd_implementation_drawedit(PSTR(MSG_LEVEL_BED_DONE));
+        lcdDrawUpdate = LCDVIEW_CALL_REDRAW_NEXT;
+      }
+
+    #endif
 
     void _lcd_level_goto_next_point();
 
@@ -1462,60 +1499,55 @@ void kill_screen(const char* lcd_msg) {
 
       if (lcd_clicked) {
 
-        // Use a hook to set the probe point z
+        //
+        // Save the current Z position
+        //
+
         #if ENABLED(MESH_BED_LEVELING)
 
+          //
           // MBL records the position but doesn't move to the next one
+          //
+
           mbl.set_zigzag_z(manual_probe_index, current_position[Z_AXIS]);
 
-        #elif ENABLED(PROBE_MANUALLY)
-
-          // The last G29 will record but not move
-          if (manual_probe_index == total_probe_points - 1)
-            enqueue_and_echo_commands_P(PSTR("G29 V1"));
-
         #endif
 
         // If done...
         if (++manual_probe_index >= total_probe_points) {
 
-          // Say "Done!"
-          lcd_goto_screen(_lcd_level_bed_done);
+          #if ENABLED(PROBE_MANUALLY)
 
-          // Raise Z to the "manual probe height"
-          #if MANUAL_PROBE_HEIGHT > 0
-            current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS) + MANUAL_PROBE_HEIGHT;
-            line_to_current(Z_AXIS);
-          #endif
+            //
+            // The last G29 will record and enable but not move.
+            // Since G29 is deferred, 
+            //
+            lcd_wait_for_move = true;
+            enqueue_and_echo_commands_P(PSTR("G29 V1"));
+            lcd_goto_screen(_lcd_level_bed_done);
 
-          #if MANUAL_PROBE_HEIGHT > 0 || ENABLED(MESH_BED_LEVELING)
-            lcd_synchronize(PSTR(MSG_LEVEL_BED_DONE));
-          #endif
+          #elif ENABLED(MESH_BED_LEVELING)
 
-          // Enable leveling, if needed
-          #if ENABLED(MESH_BED_LEVELING)
+            _lcd_after_probing();
 
             mbl.set_has_mesh(true);
             mesh_probing_done();
 
-          #elif ENABLED(PROBE_MANUALLY)
-
-            // ABL will be enabled due to "G29".
-
           #endif
 
-          lcd_goto_previous_menu(); // Return to the last clicked item ("Level Bed")
-
-          //LCD_MESSAGEPGM(MSG_LEVEL_BED_DONE);
-          lcd_completion_feedback();
         }
-        else
+        else {
+          // MESH_BED_LEVELING: Z already stored, just move
+          //    PROBE_MANUALLY: Send G29 to record Z, then move
           _lcd_level_goto_next_point();
+        }
 
         return;
       }
 
+      //
       // Encoder knob or keypad buttons adjust the Z position
+      //
       if (encoderPosition) {
         refresh_cmd_timeout();
         current_position[Z_AXIS] += float((int32_t)encoderPosition) * (MBL_Z_STEP);
@@ -1526,8 +1558,9 @@ void kill_screen(const char* lcd_msg) {
         encoderPosition = 0;
       }
 
-      // Update on first display, then only on updates to Z position
-      // Show message above on clicks instead
+      //
+      // Draw on first display, then only on Z change
+      //
       if (lcdDrawUpdate) {
         const float v = current_position[Z_AXIS];
         lcd_implementation_drawedit(PSTR(MSG_MOVE_Z), ftostr43sign(v + (v < 0 ? -0.0001 : 0.0001), '+'));
@@ -1538,10 +1571,6 @@ void kill_screen(const char* lcd_msg) {
      * Step 6: Display "Next point: 1 / 9" while waiting for move to finish
      */
 
-    #if ENABLED(PROBE_MANUALLY)
-      bool lcd_wait_for_move;
-    #endif
-
     void _lcd_level_bed_moving() {
       if (lcdDrawUpdate) {
         char msg[10];
@@ -1578,7 +1607,7 @@ void kill_screen(const char* lcd_msg) {
 
       #elif ENABLED(PROBE_MANUALLY)
 
-        // G29 will signal when it's done
+        // G29 Records Z, moves, and signals when it pauses
         lcd_wait_for_move = true;
         enqueue_and_echo_commands_P(PSTR("G29 V1"));
 
@@ -1628,13 +1657,14 @@ void kill_screen(const char* lcd_msg) {
     /**
      * Step 1: Bed Level entry-point
      *  - Cancel
-     *  - Leveling On/Off (if there is leveling data)
+     *  - Auto Home       (if homing needed)
+     *  - Leveling On/Off (if data exists, and homed)
      *  - Level Bed >
-     *  - Fade Height (Req: ENABLE_LEVELING_FADE_HEIGHT)
-     *  - Mesh Z Offset (Req: MESH_BED_LEVELING)
-     *  - Z Probe Offset (Req: HAS_BED_PROBE, Opt: BABYSTEP_ZPROBE_OFFSET)
-     *  - Load Settings (Req: EEPROM_SETTINGS)
-     *  - Save Settings (Req: EEPROM_SETTINGS)
+     *  - Fade Height     (Req: ENABLE_LEVELING_FADE_HEIGHT)
+     *  - Mesh Z Offset   (Req: MESH_BED_LEVELING)
+     *  - Z Probe Offset  (Req: HAS_BED_PROBE, Opt: BABYSTEP_ZPROBE_OFFSET)
+     *  - Load Settings   (Req: EEPROM_SETTINGS)
+     *  - Save Settings   (Req: EEPROM_SETTINGS)
      */
     void lcd_bed_leveling() {
       START_MENU();

From 51587c4b43d9b6cad4307873d9a89b9aa4a505c4 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 29 May 2017 17:48:08 -0500
Subject: [PATCH 13/13] Rename Feedrate to Velocity to match prior naming

---
 Marlin/language_en.h |  4 ++--
 Marlin/ultralcd.cpp  | 15 ++++++++-------
 2 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/Marlin/language_en.h b/Marlin/language_en.h
index 32690ebde2..8de63f4361 100644
--- a/Marlin/language_en.h
+++ b/Marlin/language_en.h
@@ -394,8 +394,8 @@
 #ifndef MSG_VE_JERK
   #define MSG_VE_JERK                         _UxGT("Ve-jerk")
 #endif
-#ifndef MSG_FEEDRATE
-  #define MSG_FEEDRATE                        _UxGT("Feedrate")
+#ifndef MSG_VELOCITY
+  #define MSG_VELOCITY                        _UxGT("Velocity")
 #endif
 #ifndef MSG_VMAX
   #define MSG_VMAX                            _UxGT("Vmax ")
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index b8d3380e84..67dc2c6af9 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -2870,8 +2870,8 @@ void kill_screen(const char* lcd_msg) {
     static void lcd_refresh_zprobe_zoffset() { refresh_zprobe_zoffset(); }
   #endif
 
-  // M203 / M205 Feedrates
-  void lcd_control_motion_feedrate_menu() {
+  // M203 / M205 Velocity options
+  void lcd_control_motion_velocity_menu() {
     START_MENU();
     MENU_BACK(MSG_MOTION);
 
@@ -3001,18 +3001,19 @@ void kill_screen(const char* lcd_msg) {
       MENU_ITEM_EDIT_CALLBACK(float32, MSG_ZPROBE_ZOFFSET, &zprobe_zoffset, Z_PROBE_OFFSET_RANGE_MIN, Z_PROBE_OFFSET_RANGE_MAX, lcd_refresh_zprobe_zoffset);
     #endif
 
-    // M203 / M205 Feedrate items
-    MENU_ITEM(submenu, MSG_FEEDRATE, lcd_control_motion_feedrate_menu);
+    // M203 / M205 - Feedrate items
+    MENU_ITEM(submenu, MSG_VELOCITY, lcd_control_motion_velocity_menu);
 
-    // M201 Acceleration items
+    // M201 - Acceleration items
     MENU_ITEM(submenu, MSG_ACCELERATION, lcd_control_motion_acceleration_menu);
 
-    // M205 Max Jerk
+    // M205 - Max Jerk
     MENU_ITEM(submenu, MSG_JERK, lcd_control_motion_jerk_menu);
 
-    // M92 Steps Per mm
+    // M92 - Steps Per mm
     MENU_ITEM(submenu, MSG_STEPS_PER_MM, lcd_control_motion_steps_per_mm_menu);
 
+    // M540 S - Abort on endstop hit when SD printing
     #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
       MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &stepper.abort_on_endstop_hit);
     #endif