diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 40128de997..ad4f82bd14 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -31,6 +31,10 @@
 #define TEST(n,b) (((n)&BIT(b))!=0)
 #define RADIANS(d) ((d)*M_PI/180.0)
 #define DEGREES(r) ((d)*180.0/M_PI)
+#define NOLESS(v,n) do{ if (v < n) v = n; }while(0)
+#define NOMORE(v,n) do{ if (v > n) v = n; }while(0)
+
+typedef unsigned long millis_t;
 
 // Arduino < 1.0.0 does not define this, so we need to do it ourselves
 #ifndef analogInputToDigitalPin
@@ -223,14 +227,14 @@ extern bool Running;
 inline bool IsRunning() { return  Running; }
 inline bool IsStopped() { return !Running; }
 
-bool enquecommand(const char *cmd); //put a single ASCII command at the end of the current buffer or return false when it is full
-void enquecommands_P(const char *cmd); //put one or many ASCII commands at the end of the current buffer, read from flash
+bool enqueuecommand(const char *cmd); //put a single ASCII command at the end of the current buffer or return false when it is full
+void enqueuecommands_P(const char *cmd); //put one or many ASCII commands at the end of the current buffer, read from flash
 
 void prepare_arc_move(char isclockwise);
 void clamp_to_software_endstops(float target[3]);
 
-extern unsigned long previous_millis_cmd;
-inline void refresh_cmd_timeout() { previous_millis_cmd = millis(); }
+extern millis_t previous_cmd_ms;
+inline void refresh_cmd_timeout() { previous_cmd_ms = millis(); }
 
 #ifdef FAST_PWM_FAN
   void setPwmFrequency(uint8_t pin, int val);
@@ -305,8 +309,8 @@ extern int fanSpeed;
   extern float retract_recover_length, retract_recover_length_swap, retract_recover_feedrate;
 #endif
 
-extern unsigned long starttime;
-extern unsigned long stoptime;
+extern millis_t starttime;
+extern millis_t stoptime;
 
 // Handling multiple extruders pins
 extern uint8_t active_extruder;
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index ddf8036de7..94c4c7bd84 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -244,11 +244,11 @@ static char *strchr_pointer; ///< A pointer to find chars in the command string
 const char* queued_commands_P= NULL; /* pointer to the current line in the active sequence of commands, or NULL when none */
 const int sensitive_pins[] = SENSITIVE_PINS; ///< Sensitive pin list for M42
 // Inactivity shutdown
-unsigned long previous_millis_cmd = 0;
-static unsigned long max_inactive_time = 0;
-static unsigned long stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME*1000l;
-unsigned long starttime = 0; ///< Print job start time
-unsigned long stoptime = 0;  ///< Print job stop time
+millis_t previous_cmd_ms = 0;
+static millis_t max_inactive_time = 0;
+static millis_t stepper_inactive_time = DEFAULT_STEPPER_DEACTIVE_TIME * 1000L;
+millis_t starttime = 0; ///< Print job start time
+millis_t stoptime = 0;  ///< Print job stop time
 static uint8_t target_extruder;
 bool CooldownNoWait = true;
 bool target_direction;
@@ -425,7 +425,7 @@ static bool drain_queued_commands_P() {
   char c;
   while((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command
   cmd[i] = '\0';
-  if (enquecommand(cmd)) {        // buffer was not full (else we will retry later)
+  if (enqueuecommand(cmd)) {        // buffer was not full (else we will retry later)
     if (c)
       queued_commands_P += i + 1; // move to next command
     else
@@ -437,7 +437,7 @@ static bool drain_queued_commands_P() {
 //Record one or many commands to run from program memory.
 //Aborts the current queue, if any.
 //Note: drain_queued_commands_P() must be called repeatedly to drain the commands afterwards
-void enquecommands_P(const char* pgcode) {
+void enqueuecommands_P(const char* pgcode) {
     queued_commands_P = pgcode;
     drain_queued_commands_P(); // first command executed asap (when possible)
 }
@@ -446,7 +446,7 @@ void enquecommands_P(const char* pgcode) {
 //that is really done in a non-safe way.
 //needs overworking someday
 //Returns false if it failed to do so
-bool enquecommand(const char *cmd)
+bool enqueuecommand(const char *cmd)
 {
   if(*cmd==';')
     return false;
@@ -666,33 +666,30 @@ void loop() {
   lcd_update();
 }
 
-void get_command()
-{
-  if (drain_queued_commands_P()) // priority is given to non-serial commands
-    return;
+void get_command() {
+
+  if (drain_queued_commands_P()) return; // priority is given to non-serial commands
   
-  while( MYSERIAL.available() > 0  && buflen < BUFSIZE) {
+  while (MYSERIAL.available() > 0 && buflen < BUFSIZE) {
     serial_char = MYSERIAL.read();
-    if(serial_char == '\n' ||
-       serial_char == '\r' ||
-       serial_count >= (MAX_CMD_SIZE - 1) )
-    {
+    if (serial_char == '\n' || serial_char == '\r' ||
+       serial_count >= (MAX_CMD_SIZE - 1)
+    ) {
       // end of line == end of comment
       comment_mode = false;
 
-      if(!serial_count) {
-        // short cut for empty lines
-        return;
-      }
-      cmdbuffer[bufindw][serial_count] = 0; //terminate string
+      if (!serial_count) return; // shortcut for empty lines
+
+      cmdbuffer[bufindw][serial_count] = 0; // terminate string
+
       #ifdef SDSUPPORT
-      fromsd[bufindw] = false;
-      #endif //!SDSUPPORT
-      if(strchr(cmdbuffer[bufindw], 'N') != NULL)
-      {
+        fromsd[bufindw] = false;
+      #endif
+
+      if (strchr(cmdbuffer[bufindw], 'N') != NULL) {
         strchr_pointer = strchr(cmdbuffer[bufindw], 'N');
         gcode_N = (strtol(strchr_pointer + 1, NULL, 10));
-        if(gcode_N != gcode_LastN+1 && (strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) ) {
+        if (gcode_N != gcode_LastN + 1 && strstr_P(cmdbuffer[bufindw], PSTR("M110")) == NULL) {
           SERIAL_ERROR_START;
           SERIAL_ERRORPGM(MSG_ERR_LINE_NO);
           SERIAL_ERRORLN(gcode_LastN);
@@ -702,14 +699,13 @@ void get_command()
           return;
         }
 
-        if(strchr(cmdbuffer[bufindw], '*') != NULL)
-        {
+        if (strchr(cmdbuffer[bufindw], '*') != NULL) {
           byte checksum = 0;
           byte count = 0;
-          while(cmdbuffer[bufindw][count] != '*') checksum = checksum^cmdbuffer[bufindw][count++];
+          while (cmdbuffer[bufindw][count] != '*') checksum ^= cmdbuffer[bufindw][count++];
           strchr_pointer = strchr(cmdbuffer[bufindw], '*');
 
-          if(strtol(strchr_pointer + 1, NULL, 10) != checksum) {
+          if (strtol(strchr_pointer + 1, NULL, 10) != checksum) {
             SERIAL_ERROR_START;
             SERIAL_ERRORPGM(MSG_ERR_CHECKSUM_MISMATCH);
             SERIAL_ERRORLN(gcode_LastN);
@@ -719,8 +715,7 @@ void get_command()
           }
           //if no errors, continue parsing
         }
-        else
-        {
+        else {
           SERIAL_ERROR_START;
           SERIAL_ERRORPGM(MSG_ERR_NO_CHECKSUM);
           SERIAL_ERRORLN(gcode_LastN);
@@ -732,10 +727,8 @@ void get_command()
         gcode_LastN = gcode_N;
         //if no errors, continue parsing
       }
-      else  // if we don't receive 'N' but still see '*'
-      {
-        if((strchr(cmdbuffer[bufindw], '*') != NULL))
-        {
+      else {  // if we don't receive 'N' but still see '*'
+        if ((strchr(cmdbuffer[bufindw], '*') != NULL)) {
           SERIAL_ERROR_START;
           SERIAL_ERRORPGM(MSG_ERR_NO_LINENUMBER_WITH_CHECKSUM);
           SERIAL_ERRORLN(gcode_LastN);
@@ -743,111 +736,99 @@ void get_command()
           return;
         }
       }
-      if((strchr(cmdbuffer[bufindw], 'G') != NULL)){
+
+      if (strchr(cmdbuffer[bufindw], 'G') != NULL) {
         strchr_pointer = strchr(cmdbuffer[bufindw], 'G');
-        switch(strtol(strchr_pointer + 1, NULL, 10)){
-        case 0:
-        case 1:
-        case 2:
-        case 3:
-          if (IsStopped()) {
-            SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
-            LCD_MESSAGEPGM(MSG_STOPPED);
-          }
-          break;
-        default:
-          break;
+        switch (strtol(strchr_pointer + 1, NULL, 10)) {
+          case 0:
+          case 1:
+          case 2:
+          case 3:
+            if (IsStopped()) {
+              SERIAL_ERRORLNPGM(MSG_ERR_STOPPED);
+              LCD_MESSAGEPGM(MSG_STOPPED);
+            }
+            break;
+          default:
+            break;
         }
-
       }
 
-      //If command was e-stop process now
-      if(strcmp(cmdbuffer[bufindw], "M112") == 0)
-        kill();
+      // If command was e-stop process now
+      if (strcmp(cmdbuffer[bufindw], "M112") == 0) kill();
 
-      bufindw = (bufindw + 1)%BUFSIZE;
+      bufindw = (bufindw + 1) % BUFSIZE;
       buflen += 1;
 
       serial_count = 0; //clear buffer
     }
-    else if(serial_char == '\\') {  //Handle escapes
-       
-        if(MYSERIAL.available() > 0  && buflen < BUFSIZE) {
-            // if we have one more character, copy it over
-            serial_char = MYSERIAL.read();
-            cmdbuffer[bufindw][serial_count++] = serial_char;
-        }
-
-        //otherwise do nothing        
+    else if (serial_char == '\\') {  // Handle escapes
+      if (MYSERIAL.available() > 0  && buflen < BUFSIZE) {
+        // if we have one more character, copy it over
+        serial_char = MYSERIAL.read();
+        cmdbuffer[bufindw][serial_count++] = serial_char;
+      }
+      // otherwise do nothing
     }
     else { // its not a newline, carriage return or escape char
-        if(serial_char == ';') comment_mode = true;
-        if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+      if (serial_char == ';') comment_mode = true;
+      if (!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
     }
   }
-  #ifdef SDSUPPORT
-  if(!card.sdprinting || serial_count!=0){
-    return;
-  }
 
-  //'#' stops reading from SD to the buffer prematurely, so procedural macro calls are possible
-  // if it occurs, stop_buffering is triggered and the buffer is ran dry.
-  // this character _can_ occur in serial com, due to checksums. however, no checksums are used in SD printing
-
-  static bool stop_buffering=false;
-  if(buflen==0) stop_buffering=false;
-
-  while( !card.eof()  && buflen < BUFSIZE && !stop_buffering) {
-    int16_t n=card.get();
-    serial_char = (char)n;
-    if(serial_char == '\n' ||
-       serial_char == '\r' ||
-       (serial_char == '#' && comment_mode == false) ||
-       (serial_char == ':' && comment_mode == false) ||
-       serial_count >= (MAX_CMD_SIZE - 1)||n==-1)
-    {
-      if(card.eof()){
-        SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
-        stoptime=millis();
-        char time[30];
-        unsigned long t=(stoptime-starttime)/1000;
-        int hours, minutes;
-        minutes=(t/60)%60;
-        hours=t/60/60;
-        sprintf_P(time, PSTR("%i "MSG_END_HOUR" %i "MSG_END_MINUTE),hours, minutes);
-        SERIAL_ECHO_START;
-        SERIAL_ECHOLN(time);
-        lcd_setstatus(time, true);
-        card.printingHasFinished();
-        card.checkautostart(true);
+  #ifdef SDSUPPORT
 
-      }
-      if(serial_char=='#')
-        stop_buffering=true;
+    if (!card.sdprinting || serial_count) return;
+
+    // '#' stops reading from SD to the buffer prematurely, so procedural macro calls are possible
+    // if it occurs, stop_buffering is triggered and the buffer is ran dry.
+    // this character _can_ occur in serial com, due to checksums. however, no checksums are used in SD printing
+
+    static bool stop_buffering = false;
+    if (buflen == 0) stop_buffering = false;
+
+    while (!card.eof() && buflen < BUFSIZE && !stop_buffering) {
+      int16_t n = card.get();
+      serial_char = (char)n;
+      if (serial_char == '\n' || serial_char == '\r' ||
+          ((serial_char == '#' || serial_char == ':') && !comment_mode) ||
+          serial_count >= (MAX_CMD_SIZE - 1) || n == -1
+      ) {
+        if (card.eof()) {
+          SERIAL_PROTOCOLLNPGM(MSG_FILE_PRINTED);
+          stoptime = millis();
+          char time[30];
+          millis_t t = (stoptime - starttime) / 1000;
+          int hours = t / 60 / 60, minutes = (t / 60) % 60;
+          sprintf_P(time, PSTR("%i " MSG_END_HOUR " %i " MSG_END_MINUTE), hours, minutes);
+          SERIAL_ECHO_START;
+          SERIAL_ECHOLN(time);
+          lcd_setstatus(time, true);
+          card.printingHasFinished();
+          card.checkautostart(true);
+        }
+        if (serial_char == '#') stop_buffering = true;
 
-      if(!serial_count)
-      {
-        comment_mode = false; //for new command
-        return; //if empty line
-      }
-      cmdbuffer[bufindw][serial_count] = 0; //terminate string
-//      if(!comment_mode){
+        if (!serial_count) {
+          comment_mode = false; //for new command
+          return; //if empty line
+        }
+        cmdbuffer[bufindw][serial_count] = 0; //terminate string
+        // if (!comment_mode) {
         fromsd[bufindw] = true;
         buflen += 1;
         bufindw = (bufindw + 1)%BUFSIZE;
-//      }
-      comment_mode = false; //for new command
-      serial_count = 0; //clear buffer
-    }
-    else
-    {
-      if(serial_char == ';') comment_mode = true;
-      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+        // }
+        comment_mode = false; //for new command
+        serial_count = 0; //clear buffer
+      }
+      else {
+        if (serial_char == ';') comment_mode = true;
+        if (!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
+      }
     }
-  }
-
-  #endif //SDSUPPORT
 
+  #endif // SDSUPPORT
 }
 
 float code_has_value() {
@@ -923,7 +904,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
   static float inactive_extruder_x_pos = X2_MAX_POS; // used in mode 0 & 1
   static bool active_extruder_parked = false; // used in mode 1 & 2
   static float raised_parked_position[NUM_AXIS]; // used in mode 1
-  static unsigned long delayed_move_time = 0; // used in mode 1
+  static millis_t delayed_move_time = 0; // used in mode 1
   static float duplicate_extruder_x_offset = DEFAULT_DUPLICATION_X_OFFSET; // used in mode 2
   static float duplicate_extruder_temp_offset = 0; // used in mode 2
   bool extruder_duplication_enabled = false; // used in mode 2
@@ -1111,7 +1092,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       // move down slowly until you find the bed
       feedrate = homing_feedrate[Z_AXIS] / 4;
       destination[Z_AXIS] = -10;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       st_synchronize();
       endstops_hit_on_purpose(); // clear endstop hit flags
       
@@ -1157,7 +1138,8 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
   }
 
   /**
-   * 
+   *  Plan a move to (X, Y, Z) and set the current_position
+   *  The final current_position may not be the one that was requested
    */
   static void do_blocking_move_to(float x, float y, float z) {
     float oldFeedRate = feedrate;
@@ -1169,7 +1151,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       destination[X_AXIS] = x;
       destination[Y_AXIS] = y;
       destination[Z_AXIS] = z;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       st_synchronize();
 
     #else
@@ -1233,17 +1215,17 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       destination[X_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_X;
       destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Y;
       destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_DEPLOY_Z;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
 
       // Home X to touch the belt
       feedrate = homing_feedrate[X_AXIS]/10;
       destination[X_AXIS] = 0;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       // Home Y for safety
       feedrate = homing_feedrate[X_AXIS]/2;
       destination[Y_AXIS] = 0;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       st_synchronize();
 
@@ -1275,7 +1257,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       if (servo_endstops[Z_AXIS] >= 0) {
 
         #if Z_RAISE_AFTER_PROBING > 0
-          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING);
+          do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING); // this also updates current_position
           st_synchronize();
         #endif
 
@@ -1296,29 +1278,29 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
       // Move up for safety
       feedrate = homing_feedrate[X_AXIS];
       destination[Z_AXIS] = current_position[Z_AXIS] + Z_RAISE_AFTER_PROBING;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
 
       // Move to the start position to initiate retraction
       destination[X_AXIS] = Z_PROBE_ALLEN_KEY_STOW_X;
       destination[Y_AXIS] = Z_PROBE_ALLEN_KEY_STOW_Y;
       destination[Z_AXIS] = Z_PROBE_ALLEN_KEY_STOW_Z;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
 
       // Move the nozzle down to push the probe into retracted position
       feedrate = homing_feedrate[Z_AXIS]/10;
       destination[Z_AXIS] = current_position[Z_AXIS] - Z_PROBE_ALLEN_KEY_STOW_DEPTH;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       // Move up for safety
       feedrate = homing_feedrate[Z_AXIS]/2;
       destination[Z_AXIS] = current_position[Z_AXIS] + Z_PROBE_ALLEN_KEY_STOW_DEPTH * 2;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       // Home XY for safety
       feedrate = homing_feedrate[X_AXIS]/2;
       destination[X_AXIS] = 0;
       destination[Y_AXIS] = 0;
-      prepare_move_raw();
+      prepare_move_raw(); // this will also set_current_to_destination
       
       st_synchronize();
 
@@ -1352,8 +1334,8 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
   // Probe bed height at position (x,y), returns the measured z value
   static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeDeployAndStow, int verbose_level=1) {
     // move to right place
-    do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
-    do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
+    do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before); // this also updates current_position
+    do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]); // this also updates current_position
 
     #if !defined(Z_PROBE_SLED) && !defined(Z_PROBE_ALLEN_KEY)
       if (retract_action & ProbeDeploy) deploy_z_probe();
@@ -1364,7 +1346,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
 
     #if Z_RAISE_BETWEEN_PROBINGS > 0
       if (retract_action == ProbeStay) {
-        do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
+        do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS); // this also updates current_position
         st_synchronize();
       }
     #endif
@@ -1643,12 +1625,12 @@ static void homeaxis(AxisEnum axis) {
     }
 
     if (dock) {
-      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, current_position[Y_AXIS], current_position[Z_AXIS]);
+      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, current_position[Y_AXIS], current_position[Z_AXIS]); // this also updates current_position
       digitalWrite(SERVO0_PIN, LOW); // turn off magnet
     } else {
       float z_loc = current_position[Z_AXIS];
       if (z_loc < Z_RAISE_BEFORE_PROBING + 5) z_loc = Z_RAISE_BEFORE_PROBING;
-      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc);
+      do_blocking_move_to(X_MAX_POS + SLED_DOCKING_OFFSET + offset, Y_PROBE_OFFSET_FROM_EXTRUDER, z_loc); // this also updates current_position
       digitalWrite(SERVO0_PIN, HIGH); // turn on magnet
     }
   }
@@ -1700,7 +1682,7 @@ inline void gcode_G2_G3(bool clockwise) {
  * G4: Dwell S<seconds> or P<milliseconds>
  */
 inline void gcode_G4() {
-  unsigned long codenum = 0;
+  millis_t codenum = 0;
 
   LCD_MESSAGEPGM(MSG_DWELL);
 
@@ -1709,7 +1691,7 @@ inline void gcode_G4() {
 
   st_synchronize();
   refresh_cmd_timeout();
-  codenum += previous_millis_cmd;  // keep track of when we started waiting
+  codenum += previous_cmd_ms;  // keep track of when we started waiting
   while (millis() < codenum) {
     manage_heater();
     manage_inactivity();
@@ -2096,7 +2078,7 @@ inline void gcode_G28() {
       case MeshStart:
         mbl.reset();
         probe_point = 0;
-        enquecommands_P(PSTR("G28\nG29 S2"));
+        enqueuecommands_P(PSTR("G28\nG29 S2"));
         break;
 
       case MeshNext:
@@ -2135,7 +2117,7 @@ inline void gcode_G28() {
           SERIAL_PROTOCOLLNPGM("Mesh probing done.");
           probe_point = -1;
           mbl.active = 1;
-          enquecommands_P(PSTR("G28"));
+          enqueuecommands_P(PSTR("G28"));
         }
         break;
 
@@ -2517,7 +2499,7 @@ inline void gcode_G28() {
     #endif
 
     #ifdef Z_PROBE_END_SCRIPT
-      enquecommands_P(PSTR(Z_PROBE_END_SCRIPT));
+      enqueuecommands_P(PSTR(Z_PROBE_END_SCRIPT));
       st_synchronize();
     #endif
   }
@@ -2579,7 +2561,7 @@ inline void gcode_G92() {
   inline void gcode_M0_M1() {
     char *src = strchr_pointer + 2;
 
-    unsigned long codenum = 0;
+    millis_t codenum = 0;
     bool hasP = false, hasS = false;
     if (code_seen('P')) {
       codenum = code_value_short(); // milliseconds to wait
@@ -2605,7 +2587,7 @@ inline void gcode_G92() {
     st_synchronize();
     refresh_cmd_timeout();
     if (codenum > 0) {
-      codenum += previous_millis_cmd;  // keep track of when we started waiting
+      codenum += previous_cmd_ms;  // keep track of when we started waiting
       while(millis() < codenum && !lcd_clicked()) {
         manage_heater();
         manage_inactivity();
@@ -2747,7 +2729,7 @@ inline void gcode_M17() {
  */
 inline void gcode_M31() {
   stoptime = millis();
-  unsigned long t = (stoptime - starttime) / 1000;
+  millis_t t = (stoptime - starttime) / 1000;
   int min = t / 60, sec = t % 60;
   char time[30];
   sprintf_P(time, PSTR("%i min, %i sec"), min, sec);
@@ -2980,11 +2962,11 @@ inline void gcode_M42() {
     if (deploy_probe_for_each_reading) stow_z_probe();
 
     for (uint8_t n=0; n < n_samples; n++) {
-
-      do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Make sure we are at the probe location
+      // Make sure we are at the probe location
+      do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // this also updates current_position
 
       if (n_legs) {
-        unsigned long ms = millis();
+        millis_t ms = millis();
         double radius = ms % (X_MAX_LENGTH / 4),       // limit how far out to go
                theta = RADIANS(ms % 360L);
         float dir = (ms & 0x0001) ? 1 : -1;            // clockwise or counter clockwise
@@ -3011,11 +2993,12 @@ inline void gcode_M42() {
             SERIAL_EOL;
           }
 
-          do_blocking_move_to(X_current, Y_current, Z_current);
+          do_blocking_move_to(X_current, Y_current, Z_current); // this also updates current_position
 
         } // n_legs loop
 
-        do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // Go back to the probe location
+        // Go back to the probe location
+        do_blocking_move_to(X_probe_location, Y_probe_location, Z_start_location); // this also updates current_position
 
       } // n_legs
 
@@ -3221,7 +3204,7 @@ inline void gcode_M109() {
 
   setWatch();
 
-  unsigned long timetemp = millis();
+  millis_t temp_ms = millis();
 
   /* See if we are heating up or cooling down */
   target_direction = isHeatingHotend(target_extruder); // true if heating, false if cooling
@@ -3229,26 +3212,26 @@ inline void gcode_M109() {
   cancel_heatup = false;
 
   #ifdef TEMP_RESIDENCY_TIME
-    long residencyStart = -1;
+    long residency_start_ms = -1;
     /* continue to loop until we have reached the target temp
       _and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
-    while((!cancel_heatup)&&((residencyStart == -1) ||
-          (residencyStart >= 0 && (((unsigned int) (millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL)))) )
+    while((!cancel_heatup)&&((residency_start_ms == -1) ||
+          (residency_start_ms >= 0 && (((unsigned int) (millis() - residency_start_ms)) < (TEMP_RESIDENCY_TIME * 1000UL)))) )
   #else
     while ( target_direction ? (isHeatingHotend(target_extruder)) : (isCoolingHotend(target_extruder)&&(CooldownNoWait==false)) )
   #endif //TEMP_RESIDENCY_TIME
 
     { // while loop
-      if (millis() > timetemp + 1000UL) { //Print temp & remaining time every 1s while waiting
+      if (millis() > temp_ms + 1000UL) { //Print temp & remaining time every 1s while waiting
         SERIAL_PROTOCOLPGM("T:");
         SERIAL_PROTOCOL_F(degHotend(target_extruder),1);
         SERIAL_PROTOCOLPGM(" E:");
         SERIAL_PROTOCOL((int)target_extruder);
         #ifdef TEMP_RESIDENCY_TIME
           SERIAL_PROTOCOLPGM(" W:");
-          if (residencyStart > -1) {
-            timetemp = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residencyStart)) / 1000UL;
-            SERIAL_PROTOCOLLN( timetemp );
+          if (residency_start_ms > -1) {
+            temp_ms = ((TEMP_RESIDENCY_TIME * 1000UL) - (millis() - residency_start_ms)) / 1000UL;
+            SERIAL_PROTOCOLLN(temp_ms);
           }
           else {
             SERIAL_PROTOCOLLNPGM("?");
@@ -3256,7 +3239,7 @@ inline void gcode_M109() {
         #else
           SERIAL_EOL;
         #endif
-        timetemp = millis();
+        temp_ms = millis();
       }
       manage_heater();
       manage_inactivity();
@@ -3264,18 +3247,18 @@ inline void gcode_M109() {
       #ifdef TEMP_RESIDENCY_TIME
         // start/restart the TEMP_RESIDENCY_TIME timer whenever we reach target temp for the first time
         // or when current temp falls outside the hysteresis after target temp was reached
-        if ((residencyStart == -1 &&  target_direction && (degHotend(target_extruder) >= (degTargetHotend(target_extruder)-TEMP_WINDOW))) ||
-            (residencyStart == -1 && !target_direction && (degHotend(target_extruder) <= (degTargetHotend(target_extruder)+TEMP_WINDOW))) ||
-            (residencyStart > -1 && labs(degHotend(target_extruder) - degTargetHotend(target_extruder)) > TEMP_HYSTERESIS) )
+        if ((residency_start_ms == -1 &&  target_direction && (degHotend(target_extruder) >= (degTargetHotend(target_extruder)-TEMP_WINDOW))) ||
+            (residency_start_ms == -1 && !target_direction && (degHotend(target_extruder) <= (degTargetHotend(target_extruder)+TEMP_WINDOW))) ||
+            (residency_start_ms > -1 && labs(degHotend(target_extruder) - degTargetHotend(target_extruder)) > TEMP_HYSTERESIS) )
         {
-          residencyStart = millis();
+          residency_start_ms = millis();
         }
       #endif //TEMP_RESIDENCY_TIME
     }
 
   LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
   refresh_cmd_timeout();
-  starttime = previous_millis_cmd;
+  starttime = previous_cmd_ms;
 }
 
 #if HAS_TEMP_BED
@@ -3290,15 +3273,15 @@ inline void gcode_M109() {
     if (CooldownNoWait || code_seen('R'))
       setTargetBed(code_value());
 
-    unsigned long timetemp = millis();
+    millis_t temp_ms = millis();
     
     cancel_heatup = false;
     target_direction = isHeatingBed(); // true if heating, false if cooling
 
     while ( (target_direction)&&(!cancel_heatup) ? (isHeatingBed()) : (isCoolingBed()&&(CooldownNoWait==false)) ) {
-      unsigned long ms = millis();
-      if (ms > timetemp + 1000UL) { //Print Temp Reading every 1 second while heating up.
-        timetemp = ms;
+      millis_t ms = millis();
+      if (ms > temp_ms + 1000UL) { //Print Temp Reading every 1 second while heating up.
+        temp_ms = ms;
         float tt = degHotend(active_extruder);
         SERIAL_PROTOCOLPGM("T:");
         SERIAL_PROTOCOL(tt);
@@ -3974,14 +3957,14 @@ inline void gcode_M226() {
 
 #endif // NUM_SERVOS > 0
 
-#if defined(LARGE_FLASH) && (BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER))
+#if BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER)
 
   /**
    * M300: Play beep sound S<frequency Hz> P<duration ms>
    */
   inline void gcode_M300() {
-    int beepS = code_seen('S') ? code_value() : 110;
-    int beepP = code_seen('P') ? code_value() : 1000;
+    uint16_t beepS = code_seen('S') ? code_value_short() : 110;
+    uint32_t beepP = code_seen('P') ? code_value_long() : 1000;
     if (beepS > 0) {
       #if BEEPER > 0
         tone(BEEPER, beepS);
@@ -3998,7 +3981,7 @@ inline void gcode_M226() {
     }
   }
 
-#endif // LARGE_FLASH && (BEEPER>0 || ULTRALCD || LCD_USE_I2C_BUZZER)
+#endif // BEEPER>0 || ULTRALCD || LCD_USE_I2C_BUZZER
 
 #ifdef PIDTEMP
 
@@ -4472,24 +4455,10 @@ inline void gcode_M503() {
     LCD_ALERTMESSAGEPGM(MSG_FILAMENTCHANGE);
     uint8_t cnt = 0;
     while (!lcd_clicked()) {
-      cnt++;
+      if (++cnt == 0) lcd_quick_feedback(); // every 256th frame till the lcd is clicked
       manage_heater();
       manage_inactivity(true);
       lcd_update();
-      if (cnt == 0) {
-        #if BEEPER > 0
-          OUT_WRITE(BEEPER,HIGH);
-          delay(3);
-          WRITE(BEEPER,LOW);
-          delay(3);
-        #else
-          #if !defined(LCD_FEEDBACK_FREQUENCY_HZ) || !defined(LCD_FEEDBACK_FREQUENCY_DURATION_MS)
-            lcd_buzz(1000/6, 100);
-          #else
-            lcd_buzz(LCD_FEEDBACK_FREQUENCY_DURATION_MS, LCD_FEEDBACK_FREQUENCY_HZ);
-          #endif
-        #endif
-      }
     } // while(!lcd_clicked)
 
     //return to normal
@@ -5078,11 +5047,11 @@ void process_commands() {
           break;
       #endif // NUM_SERVOS > 0
 
-      #if defined(LARGE_FLASH) && (BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER))
+      #if BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER)
         case 300: // M300 - Play beep tone
           gcode_M300();
           break;
-      #endif // LARGE_FLASH && (BEEPER>0 || ULTRALCD || LCD_USE_I2C_BUZZER)
+      #endif // BEEPER > 0 || ULTRALCD || LCD_USE_I2C_BUZZER
 
       #ifdef PIDTEMP
         case 301: // M301
@@ -5289,25 +5258,23 @@ void get_arc_coordinates() {
   offset[1] = code_seen('J') ? code_value() : 0;
 }
 
-void clamp_to_software_endstops(float target[3])
-{
+void clamp_to_software_endstops(float target[3]) {
   if (min_software_endstops) {
-    if (target[X_AXIS] < min_pos[X_AXIS]) target[X_AXIS] = min_pos[X_AXIS];
-    if (target[Y_AXIS] < min_pos[Y_AXIS]) target[Y_AXIS] = min_pos[Y_AXIS];
+    NOLESS(target[X_AXIS], min_pos[X_AXIS]);
+    NOLESS(target[Y_AXIS], min_pos[Y_AXIS]);
     
     float negative_z_offset = 0;
     #ifdef ENABLE_AUTO_BED_LEVELING
-      if (Z_PROBE_OFFSET_FROM_EXTRUDER < 0) negative_z_offset = negative_z_offset + Z_PROBE_OFFSET_FROM_EXTRUDER;
-      if (home_offset[Z_AXIS] < 0) negative_z_offset = negative_z_offset + home_offset[Z_AXIS];
+      if (Z_PROBE_OFFSET_FROM_EXTRUDER < 0) negative_z_offset += Z_PROBE_OFFSET_FROM_EXTRUDER;
+      if (home_offset[Z_AXIS] < 0) negative_z_offset += home_offset[Z_AXIS];
     #endif
-    
-    if (target[Z_AXIS] < min_pos[Z_AXIS]+negative_z_offset) target[Z_AXIS] = min_pos[Z_AXIS]+negative_z_offset;
+    NOLESS(target[Z_AXIS], min_pos[Z_AXIS] + negative_z_offset);
   }
 
   if (max_software_endstops) {
-    if (target[X_AXIS] > max_pos[X_AXIS]) target[X_AXIS] = max_pos[X_AXIS];
-    if (target[Y_AXIS] > max_pos[Y_AXIS]) target[Y_AXIS] = max_pos[Y_AXIS];
-    if (target[Z_AXIS] > max_pos[Z_AXIS]) target[Z_AXIS] = max_pos[Z_AXIS];
+    NOMORE(target[X_AXIS], max_pos[X_AXIS]);
+    NOMORE(target[Y_AXIS], max_pos[Y_AXIS]);
+    NOMORE(target[Z_AXIS], max_pos[Z_AXIS]);
   }
 }
 
@@ -5522,7 +5489,7 @@ void prepare_move() {
       //SERIAL_ECHOPGM("delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
       //SERIAL_ECHOPGM("delta[Z_AXIS]="); SERIAL_ECHOLN(delta[Z_AXIS]);
 
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedmultiply/100.0, active_extruder);
     }
 
   #endif // SCARA
@@ -5549,7 +5516,7 @@ void prepare_move() {
       #ifdef ENABLE_AUTO_BED_LEVELING
         adjust_delta(destination);
       #endif
-      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+      plan_buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], feedrate/60*feedmultiply/100.0, active_extruder);
     }
 
   #endif // DELTA
@@ -5573,7 +5540,7 @@ void prepare_move() {
           // (so it can be used as the start of the next non-travel move)
           if (delayed_move_time != 0xFFFFFFFFUL) {
             set_current_to_destination();
-            if (destination[Z_AXIS] > raised_parked_position[Z_AXIS]) raised_parked_position[Z_AXIS] = destination[Z_AXIS];
+            NOLESS(raised_parked_position[Z_AXIS], destination[Z_AXIS]);
             delayed_move_time = millis();
             return;
           }
@@ -5621,11 +5588,11 @@ void prepare_arc_move(char isclockwise) {
 
 #if HAS_CONTROLLERFAN
 
-unsigned long lastMotor = 0; // Last time a motor was turned on
-unsigned long lastMotorCheck = 0; // Last time the state was checked
+millis_t lastMotor = 0; // Last time a motor was turned on
+millis_t lastMotorCheck = 0; // Last time the state was checked
 
 void controllerFan() {
-  uint32_t ms = millis();
+  millis_t ms = millis();
   if (ms >= lastMotorCheck + 2500) { // Not a time critical function, so we only check every 2500ms
     lastMotorCheck = ms;
     if (X_ENABLE_READ == X_ENABLE_ON || Y_ENABLE_READ == Y_ENABLE_ON || Z_ENABLE_READ == Z_ENABLE_ON || soft_pwm_bed > 0
@@ -5732,36 +5699,28 @@ void calculate_delta(float cartesian[3]){
 #endif
 
 #ifdef TEMP_STAT_LEDS
-static bool blue_led = false;
-static bool red_led = false;
-static uint32_t stat_update = 0;
-
-void handle_status_leds(void) {
-  float max_temp = 0.0;
-  if(millis() > stat_update) {
-    stat_update += 500; // Update every 0.5s
-    for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder) {
-       max_temp = max(max_temp, degHotend(cur_extruder));
-       max_temp = max(max_temp, degTargetHotend(cur_extruder));
-    }
-    #if HAS_TEMP_BED
-      max_temp = max(max_temp, degTargetBed());
-      max_temp = max(max_temp, degBed());
-    #endif
-    if((max_temp > 55.0) && (red_led == false)) {
-      digitalWrite(STAT_LED_RED, 1);
-      digitalWrite(STAT_LED_BLUE, 0);
-      red_led = true;
-      blue_led = false;
-    }
-    if((max_temp < 54.0) && (blue_led == false)) {
-      digitalWrite(STAT_LED_RED, 0);
-      digitalWrite(STAT_LED_BLUE, 1);
-      red_led = false;
-      blue_led = true;
+
+  static bool red_led = false;
+  static millis_t next_status_led_update_ms = 0;
+
+  void handle_status_leds(void) {
+    float max_temp = 0.0;
+    if (millis() > next_status_led_update_ms) {
+      next_status_led_update_ms += 500; // Update every 0.5s
+      for (int8_t cur_extruder = 0; cur_extruder < EXTRUDERS; ++cur_extruder)
+         max_temp = max(max(max_temp, degHotend(cur_extruder)), degTargetHotend(cur_extruder));
+      #if HAS_TEMP_BED
+        max_temp = max(max(max_temp, degTargetBed()), degBed());
+      #endif
+      bool new_led = (max_temp > 55.0) ? true : (max_temp < 54.0) ? false : red_led;
+      if (new_led != red_led) {
+        red_led = new_led;
+        digitalWrite(STAT_LED_RED, new_led ? HIGH : LOW);
+        digitalWrite(STAT_LED_BLUE, new_led ? LOW : HIGH);
+      }
     }
   }
-}
+
 #endif
 
 void enable_all_steppers() {
@@ -5805,11 +5764,11 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
 
   if (buflen < BUFSIZE - 1) get_command();
 
-  unsigned long ms = millis();
+  millis_t ms = millis();
 
-  if (max_inactive_time && ms > previous_millis_cmd + max_inactive_time) kill();
+  if (max_inactive_time && ms > previous_cmd_ms + max_inactive_time) kill();
 
-  if (stepper_inactive_time && ms > previous_millis_cmd + stepper_inactive_time
+  if (stepper_inactive_time && ms > previous_cmd_ms + stepper_inactive_time
       && !ignore_stepper_queue && !blocks_queued())
     disable_all_steppers();
 
@@ -5845,7 +5804,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
     const int HOME_DEBOUNCE_DELAY = 750;
     if (!READ(HOME_PIN)) {
       if (!homeDebounceCount) {
-        enquecommands_P(PSTR("G28"));
+        enqueuecommands_P(PSTR("G28"));
         LCD_ALERTMESSAGEPGM(MSG_AUTO_HOME);
       }
       if (homeDebounceCount < HOME_DEBOUNCE_DELAY)
@@ -5860,7 +5819,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
   #endif
 
   #ifdef EXTRUDER_RUNOUT_PREVENT
-    if (ms > previous_millis_cmd + EXTRUDER_RUNOUT_SECONDS * 1000)
+    if (ms > previous_cmd_ms + EXTRUDER_RUNOUT_SECONDS * 1000)
     if (degHotend(active_extruder) > EXTRUDER_RUNOUT_MINTEMP) {
       bool oldstatus;
       switch(active_extruder) {
@@ -5894,7 +5853,7 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
       current_position[E_AXIS] = oldepos;
       destination[E_AXIS] = oldedes;
       plan_set_e_position(oldepos);
-      previous_millis_cmd = ms; // refresh_cmd_timeout()
+      previous_cmd_ms = ms; // refresh_cmd_timeout()
       st_synchronize();
       switch(active_extruder) {
         case 0:
@@ -5964,7 +5923,7 @@ void kill()
    {
       if filrunoutEnqued == false {
          filrunoutEnqued = true;
-         enquecommand("M600");
+         enqueuecommand("M600");
       }
    }
 #endif
diff --git a/Marlin/cardreader.cpp b/Marlin/cardreader.cpp
index 877b72b927..639b4f244a 100644
--- a/Marlin/cardreader.cpp
+++ b/Marlin/cardreader.cpp
@@ -25,7 +25,7 @@ CardReader::CardReader() {
     OUT_WRITE(SDPOWER, HIGH);
   #endif //SDPOWER
 
-  autostart_atmillis = millis() + 5000;
+  next_autostart_ms = millis() + 5000;
 }
 
 char *createFilename(char *buffer, const dir_t &p) { //buffer > 12characters
@@ -397,7 +397,7 @@ void CardReader::write_command(char *buf) {
 }
 
 void CardReader::checkautostart(bool force) {
-  if (!force && (!autostart_stilltocheck || autostart_atmillis < millis()))
+  if (!force && (!autostart_stilltocheck || next_autostart_ms < millis()))
     return;
 
   autostart_stilltocheck = false;
@@ -421,8 +421,8 @@ void CardReader::checkautostart(bool force) {
     if (p.name[9] != '~' && strncmp((char*)p.name, autoname, 5) == 0) {
       char cmd[30];
       sprintf_P(cmd, PSTR("M23 %s"), autoname);
-      enquecommand(cmd);
-      enquecommands_P(PSTR("M24"));
+      enqueuecommand(cmd);
+      enqueuecommands_P(PSTR("M24"));
       found = true;
     }
   }
@@ -508,7 +508,7 @@ void CardReader::printingHasFinished() {
     sdprinting = false;
     if (SD_FINISHED_STEPPERRELEASE) {
       //finishAndDisableSteppers();
-      enquecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
+      enqueuecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
     }
     autotempShutdown();
   }
diff --git a/Marlin/cardreader.h b/Marlin/cardreader.h
index b55d09a65a..03d4303e30 100644
--- a/Marlin/cardreader.h
+++ b/Marlin/cardreader.h
@@ -62,7 +62,7 @@ private:
   uint32_t filespos[SD_PROCEDURE_DEPTH];
   char filenames[SD_PROCEDURE_DEPTH][MAXPATHNAMELENGTH];
   uint32_t filesize;
-  unsigned long autostart_atmillis;
+  millis_t next_autostart_ms;
   uint32_t sdpos;
 
   bool autostart_stilltocheck; //the sd start is delayed, because otherwise the serial cannot answer fast enought to make contact with the hostsoftware.
diff --git a/Marlin/dogm_lcd_implementation.h b/Marlin/dogm_lcd_implementation.h
index fd52d645ed..546703bfb0 100644
--- a/Marlin/dogm_lcd_implementation.h
+++ b/Marlin/dogm_lcd_implementation.h
@@ -350,7 +350,7 @@ static void lcd_implementation_status_screen() {
   #ifndef FILAMENT_LCD_DISPLAY
     lcd_print(lcd_status_message);
   #else
-    if (millis() < message_millis + 5000) {  //Display both Status message line and Filament display on the last line
+    if (millis() < previous_lcd_status_ms + 5000) {  //Display both Status message line and Filament display on the last line
       lcd_print(lcd_status_message);
     }
     else {
diff --git a/Marlin/example_configurations/Hephestos/Configuration.h b/Marlin/example_configurations/Hephestos/Configuration.h
index 02504e2839..a83c6e911d 100644
--- a/Marlin/example_configurations/Hephestos/Configuration.h
+++ b/Marlin/example_configurations/Hephestos/Configuration.h
@@ -58,9 +58,7 @@ Here are some standard links for getting your machine calibrated:
 
 // The following define selects which electronics board you have.
 // Please choose the name from boards.h that matches your setup
-#ifndef MOTHERBOARD
-  #define MOTHERBOARD BOARD_HEPHESTOS
-#endif
+#define MOTHERBOARD BOARD_HEPHESTOS
 
 // Optional custom name for your RepStrap or other custom machine
 // Displayed in the LCD "Ready" message
diff --git a/Marlin/example_configurations/WITBOX/Configuration.h b/Marlin/example_configurations/WITBOX/Configuration.h
index eaa1a8b9e5..dda21b7759 100644
--- a/Marlin/example_configurations/WITBOX/Configuration.h
+++ b/Marlin/example_configurations/WITBOX/Configuration.h
@@ -58,9 +58,7 @@ Here are some standard links for getting your machine calibrated:
 
 // The following define selects which electronics board you have.
 // Please choose the name from boards.h that matches your setup
-#ifndef MOTHERBOARD
-  #define MOTHERBOARD BOARD_WITBOX
-#endif
+#define MOTHERBOARD BOARD_WITBOX
 
 // Optional custom name for your RepStrap or other custom machine
 // Displayed in the LCD "Ready" message
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 49c090881c..a452428d78 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -60,13 +60,13 @@
 
 #ifdef MESH_BED_LEVELING
   #include "mesh_bed_leveling.h"
-#endif  // MESH_BED_LEVELING
+#endif
 
 //===========================================================================
 //============================= public variables ============================
 //===========================================================================
 
-unsigned long minsegmenttime;
+millis_t minsegmenttime;
 float max_feedrate[NUM_AXIS]; // Max speeds in mm per minute
 float axis_steps_per_unit[NUM_AXIS];
 unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 to override by software
@@ -159,8 +159,8 @@ void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exi
   unsigned long final_rate = ceil(block->nominal_rate * exit_factor); // (step/min)
 
   // Limit minimal step rate (Otherwise the timer will overflow.)
-  if (initial_rate < 120) initial_rate = 120;
-  if (final_rate < 120) final_rate = 120;
+  NOLESS(initial_rate, 120);
+  NOLESS(final_rate, 120);
 
   long acceleration = block->acceleration_st;
   int32_t accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
@@ -382,9 +382,11 @@ void plan_init() {
     }
 
     float t = autotemp_min + high * autotemp_factor;
-    if (t < autotemp_min) t = autotemp_min;
-    if (t > autotemp_max) t = autotemp_max;
-    if (oldt > t) t = AUTOTEMP_OLDWEIGHT * oldt + (1 - AUTOTEMP_OLDWEIGHT) * t;
+    t = constrain(t, autotemp_min, autotemp_max);
+    if (oldt > t) {
+      t *= (1 - AUTOTEMP_OLDWEIGHT);
+      t += AUTOTEMP_OLDWEIGHT * oldt;
+    }
     oldt = t;
     setTargetHotend0(t);
   }
@@ -426,7 +428,7 @@ void check_axes_activity() {
 
   #if HAS_FAN
     #ifdef FAN_KICKSTART_TIME
-      static unsigned long fan_kick_end;
+      static millis_t fan_kick_end;
       if (tail_fan_speed) {
         if (fan_kick_end == 0) {
           // Just starting up fan - run at full power.
@@ -651,10 +653,10 @@ float junction_deviation = 0.1;
     }
   }
 
-  if (block->steps[E_AXIS]) {
-    if (feed_rate < minimumfeedrate) feed_rate = minimumfeedrate;
-  }
-  else if (feed_rate < mintravelfeedrate) feed_rate = mintravelfeedrate;
+  if (block->steps[E_AXIS])
+    NOLESS(feed_rate, minimumfeedrate);
+  else
+    NOLESS(feed_rate, mintravelfeedrate);
 
   /**
    * This part of the code calculates the total length of the movement. 
diff --git a/Marlin/planner.h b/Marlin/planner.h
index c617d6d481..d96aa8c112 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -115,7 +115,7 @@ FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block
 
 void plan_set_e_position(const float &e);
 
-extern unsigned long minsegmenttime;
+extern millis_t minsegmenttime;
 extern float max_feedrate[NUM_AXIS]; // set the max speeds
 extern float axis_steps_per_unit[NUM_AXIS];
 extern unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 to override by software
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 1f28b3af95..2ad555327d 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -400,7 +400,7 @@ ISR(TIMER1_COMPA_vect) {
     current_block = NULL;
     plan_discard_current_block();
     #ifdef SD_FINISHED_RELEASECOMMAND
-      if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enquecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
+      if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueuecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
     #endif
     cleaning_buffer_counter--;
     OCR1A = 200;
@@ -718,7 +718,7 @@ ISR(TIMER1_COMPA_vect) {
     // Calculate new timer value
     unsigned short timer;
     unsigned short step_rate;
-    if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
+    if (step_events_completed <= (unsigned long)current_block->accelerate_until) {
 
       MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
       acc_step_rate += current_block->initial_rate;
@@ -742,7 +742,7 @@ ISR(TIMER1_COMPA_vect) {
 
       #endif
     }
-    else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
+    else if (step_events_completed > (unsigned long)current_block->decelerate_after) {
       MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
 
       if (step_rate > acc_step_rate) { // Check step_rate stays positive
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index b4dffbfe16..9c9d6c5e08 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -77,14 +77,14 @@ unsigned char soft_pwm_bed;
 #define HAS_BED_THERMAL_PROTECTION (defined(THERMAL_RUNAWAY_PROTECTION_BED_PERIOD) && THERMAL_RUNAWAY_PROTECTION_BED_PERIOD > 0 && TEMP_SENSOR_BED != 0)
 #if HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
   enum TRState { TRReset, TRInactive, TRFirstHeating, TRStable, TRRunaway };
-  void thermal_runaway_protection(TRState *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc);
+  void thermal_runaway_protection(TRState *state, millis_t *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc);
   #if HAS_HEATER_THERMAL_PROTECTION
     static TRState thermal_runaway_state_machine[4] = { TRReset, TRReset, TRReset, TRReset };
-    static unsigned long thermal_runaway_timer[4]; // = {0,0,0,0};
+    static millis_t thermal_runaway_timer[4]; // = {0,0,0,0};
   #endif
   #if HAS_BED_THERMAL_PROTECTION
     static TRState thermal_runaway_bed_state_machine = TRReset;
-    static unsigned long thermal_runaway_bed_timer;
+    static millis_t thermal_runaway_bed_timer;
   #endif
 #endif
 
@@ -118,7 +118,7 @@ static volatile bool temp_meas_ready = false;
   static float temp_iState_min_bed;
   static float temp_iState_max_bed;
 #else //PIDTEMPBED
-  static unsigned long  previous_millis_bed_heater;
+  static millis_t  next_bed_check_ms;
 #endif //PIDTEMPBED
   static unsigned char soft_pwm[EXTRUDERS];
 
@@ -126,7 +126,7 @@ static volatile bool temp_meas_ready = false;
   static unsigned char soft_pwm_fan;
 #endif
 #if HAS_AUTO_FAN
-  static unsigned long extruder_autofan_last_check;
+  static millis_t next_auto_fan_check_ms;
 #endif  
 
 #ifdef PIDTEMP
@@ -171,7 +171,7 @@ static void updateTemperaturesFromRawValues();
 
 #ifdef WATCH_TEMP_PERIOD
   int watch_start_temp[EXTRUDERS] = { 0 };
-  unsigned long watchmillis[EXTRUDERS] = { 0 };
+  millis_t watchmillis[EXTRUDERS] = { 0 };
 #endif //WATCH_TEMP_PERIOD
 
 #ifndef SOFT_PWM_SCALE
@@ -196,7 +196,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
   int cycles = 0;
   bool heating = true;
 
-  unsigned long temp_millis = millis(), t1 = temp_millis, t2 = temp_millis;
+  millis_t temp_ms = millis(), t1 = temp_ms, t2 = temp_ms;
   long t_high = 0, t_low = 0;
 
   long bias, d;
@@ -205,7 +205,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
   float max = 0, min = 10000;
 
   #if HAS_AUTO_FAN
-        unsigned long extruder_autofan_last_check = temp_millis;
+    millis_t next_auto_fan_check_ms = temp_ms + 2500;
   #endif
 
   if (extruder >= EXTRUDERS
@@ -229,7 +229,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
   // PID Tuning loop
   for (;;) {
 
-    unsigned long ms = millis();
+    millis_t ms = millis();
 
     if (temp_meas_ready) { // temp sample ready
       updateTemperaturesFromRawValues();
@@ -240,9 +240,9 @@ void PID_autotune(float temp, int extruder, int ncycles)
       min = min(min, input);
 
       #if HAS_AUTO_FAN
-        if (ms > extruder_autofan_last_check + 2500) {
+        if (ms > next_auto_fan_check_ms) {
           checkExtruderAutoFans();
-          extruder_autofan_last_check = ms;
+          next_auto_fan_check_ms = ms + 2500;
         }
       #endif
 
@@ -317,7 +317,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
       return;
     }
     // Every 2 seconds...
-    if (ms > temp_millis + 2000) {
+    if (ms > temp_ms + 2000) {
       int p;
       if (extruder < 0) {
         p = soft_pwm_bed;
@@ -332,7 +332,7 @@ void PID_autotune(float temp, int extruder, int ncycles)
       SERIAL_PROTOCOLPGM(MSG_AT);
       SERIAL_PROTOCOLLN(p);
 
-      temp_millis = ms;
+      temp_ms = ms;
     } // every 2 seconds
     // Over 2 minutes?
     if (((ms - t1) + (ms - t2)) > (10L*60L*1000L*2L)) {
@@ -592,7 +592,7 @@ void manage_heater() {
   #endif //HEATER_0_USES_MAX6675
 
   #if defined(WATCH_TEMP_PERIOD) || !defined(PIDTEMPBED) || HAS_AUTO_FAN
-    unsigned long ms = millis();
+    millis_t ms = millis();
   #endif
 
   // Loop through all extruders
@@ -631,16 +631,16 @@ void manage_heater() {
   } // Extruders Loop
 
   #if HAS_AUTO_FAN
-    if (ms > extruder_autofan_last_check + 2500) { // only need to check fan state very infrequently
+    if (ms > next_auto_fan_check_ms) { // only need to check fan state very infrequently
       checkExtruderAutoFans();
-      extruder_autofan_last_check = ms;
+      next_auto_fan_check_ms = ms + 2500;
     }
   #endif       
   
   #ifndef PIDTEMPBED
-    if (ms < previous_millis_bed_heater + BED_CHECK_INTERVAL) return;
-    previous_millis_bed_heater = ms;
-  #endif //PIDTEMPBED
+    if (ms < next_bed_check_ms) return;
+    next_bed_check_ms = ms + BED_CHECK_INTERVAL;
+  #endif
 
   #if TEMP_SENSOR_BED != 0
   
@@ -992,7 +992,7 @@ void tp_init()
 
 void setWatch() {
   #ifdef WATCH_TEMP_PERIOD
-    unsigned long ms = millis();
+    millis_t ms = millis();
     for (int e = 0; e < EXTRUDERS; e++) {
       if (degHotend(e) < degTargetHotend(e) - (WATCH_TEMP_INCREASE * 2)) {
         watch_start_temp[e] = degHotend(e);
@@ -1004,7 +1004,7 @@ void setWatch() {
 
 #if HAS_HEATER_THERMAL_PROTECTION || HAS_BED_THERMAL_PROTECTION
 
-  void thermal_runaway_protection(TRState *state, unsigned long *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc) {
+  void thermal_runaway_protection(TRState *state, millis_t *timer, float temperature, float target_temperature, int heater_id, int period_seconds, int hysteresis_degc) {
 
     static float tr_target_temperature[EXTRUDERS+1] = { 0.0 };
 
@@ -1109,16 +1109,18 @@ void disable_heater() {
 
 #ifdef HEATER_0_USES_MAX6675
   #define MAX6675_HEAT_INTERVAL 250u
-  unsigned long max6675_previous_millis = MAX6675_HEAT_INTERVAL;
+  static millis_t next_max6675_ms = 0;
   int max6675_temp = 2000;
 
   static int read_max6675() {
 
-    unsigned long ms = millis();
-    if (ms < max6675_previous_millis + MAX6675_HEAT_INTERVAL)
+    millis_t ms = millis();
+
+    if (ms < next_max6675_ms)
       return max6675_temp;
     
-    max6675_previous_millis = ms;
+    next_max6675_ms = ms + MAX6675_HEAT_INTERVAL;
+
     max6675_temp = 0;
 
     #ifdef PRR
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index bdd15776d1..293f819c92 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -22,7 +22,7 @@ int absPreheatHPBTemp;
 int absPreheatFanSpeed;
 
 #ifdef FILAMENT_LCD_DISPLAY
-  unsigned long message_millis = 0;
+  millis_t previous_lcd_status_ms = 0;
 #endif
 
 /* !Configuration settings */
@@ -77,8 +77,6 @@ static void lcd_status_screen();
     static void lcd_level_bed();
   #endif
 
-  static void lcd_quick_feedback();//Cause an LCD refresh, and give the user visual or audible feedback that something has happened
-
   /* Different types of actions that can be used in menu items. */
   static void menu_action_back(menuFunc_t data);
   static void menu_action_submenu(menuFunc_t data);
@@ -220,7 +218,7 @@ static void lcd_status_screen();
     volatile uint8_t slow_buttons; // Bits of the pressed buttons.
   #endif
   uint8_t currentMenuViewOffset;              /* scroll offset in the current menu */
-  uint32_t blocking_enc;
+  millis_t next_button_update_ms;
   uint8_t lastEncoderBits;
   uint32_t encoderPosition;
   #if (SDCARDDETECT > 0)
@@ -230,7 +228,7 @@ static void lcd_status_screen();
 #endif // ULTIPANEL
 
 menuFunc_t currentMenu = lcd_status_screen; /* function pointer to the currently active menu */
-uint32_t lcd_next_update_millis;
+millis_t next_lcd_update_ms;
 uint8_t lcd_status_update_delay;
 bool ignore_click = false;
 bool wait_for_unclick;
@@ -267,7 +265,7 @@ static void lcd_status_screen() {
 	encoderRateMultiplierEnabled = false;
 
   #ifdef LCD_PROGRESS_BAR
-    unsigned long ms = millis();
+    millis_t ms = millis();
     #ifndef PROGRESS_MSG_ONCE
       if (ms > progressBarTick + PROGRESS_BAR_MSG_TIME + PROGRESS_BAR_BAR_TIME) {
         progressBarTick = ms;
@@ -324,7 +322,7 @@ static void lcd_status_screen() {
         #endif
       );
       #ifdef FILAMENT_LCD_DISPLAY
-        message_millis = millis();  // get status message to show up for a while
+        previous_lcd_status_ms = millis();  // get status message to show up for a while
       #endif
     }
 
@@ -433,7 +431,7 @@ void lcd_set_home_offsets() {
   plan_set_position(0.0, 0.0, 0.0, current_position[E_AXIS]);
 
   // Audio feedback
-  enquecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
+  enqueuecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
   lcd_return_to_status();
 }
 
@@ -1114,15 +1112,15 @@ menu_edit_type(unsigned long, long5, ftostr5, 0.01)
     lcd_move_y();
   }
   static void reprapworld_keypad_move_home() {
-    enquecommands_P((PSTR("G28"))); // move all axis home
+    enqueuecommands_P((PSTR("G28"))); // move all axis home
   }
 #endif //REPRAPWORLD_KEYPAD
 
 /** End of menus **/
 
-static void lcd_quick_feedback() {
+void lcd_quick_feedback() {
   lcdDrawUpdate = 2;
-  blocking_enc = millis() + 500;
+  next_button_update_ms = millis() + 500;
     
   #ifdef LCD_USE_I2C_BUZZER
     #ifndef LCD_FEEDBACK_FREQUENCY_HZ
@@ -1140,15 +1138,15 @@ static void lcd_quick_feedback() {
     #ifndef LCD_FEEDBACK_FREQUENCY_DURATION_MS
       #define LCD_FEEDBACK_FREQUENCY_DURATION_MS 2
     #endif
-    const unsigned int delay = 1000000 / LCD_FEEDBACK_FREQUENCY_HZ / 2;
-    int i = LCD_FEEDBACK_FREQUENCY_DURATION_MS * LCD_FEEDBACK_FREQUENCY_HZ / 1000;
+    const uint16_t delay = 1000000 / LCD_FEEDBACK_FREQUENCY_HZ / 2;
+    uint16_t i = LCD_FEEDBACK_FREQUENCY_DURATION_MS * LCD_FEEDBACK_FREQUENCY_HZ / 1000;
     while (i--) {
       WRITE(BEEPER,HIGH);
       delayMicroseconds(delay);
       WRITE(BEEPER,LOW);
       delayMicroseconds(delay);
     }
-    const int j = max(10000 - LCD_FEEDBACK_FREQUENCY_DURATION_MS * 1000, 0);
+    const uint16_t j = max(10000 - LCD_FEEDBACK_FREQUENCY_DURATION_MS * 1000, 0);
     if (j) delayMicroseconds(j);
   #endif
 }
@@ -1156,15 +1154,15 @@ static void lcd_quick_feedback() {
 /** Menu action functions **/
 static void menu_action_back(menuFunc_t data) { lcd_goto_menu(data); }
 static void menu_action_submenu(menuFunc_t data) { lcd_goto_menu(data); }
-static void menu_action_gcode(const char* pgcode) { enquecommands_P(pgcode); }
+static void menu_action_gcode(const char* pgcode) { enqueuecommands_P(pgcode); }
 static void menu_action_function(menuFunc_t data) { (*data)(); }
 static void menu_action_sdfile(const char* filename, char* longFilename) {
   char cmd[30];
   char* c;
   sprintf_P(cmd, PSTR("M23 %s"), filename);
   for(c = &cmd[4]; *c; c++) *c = tolower(*c);
-  enquecommand(cmd);
-  enquecommands_P(PSTR("M24"));
+  enqueuecommand(cmd);
+  enqueuecommands_P(PSTR("M24"));
   lcd_return_to_status();
 }
 static void menu_action_sddirectory(const char* filename, char* longFilename) {
@@ -1252,7 +1250,7 @@ int lcd_strlen_P(const char *s) {
 
 void lcd_update() {
   #ifdef ULTIPANEL
-    static unsigned long timeoutToStatus = 0;
+    static millis_t return_to_status_ms = 0;
   #endif
 
   #ifdef LCD_HAS_SLOW_BUTTONS
@@ -1282,8 +1280,8 @@ void lcd_update() {
     }
   #endif//CARDINSERTED
   
-  uint32_t ms = millis();
-  if (ms > lcd_next_update_millis) {
+  millis_t ms = millis();
+  if (ms > next_lcd_update_ms) {
 
     #ifdef ULTIPANEL
 
@@ -1335,7 +1333,7 @@ void lcd_update() {
           encoderPosition += (encoderDiff * encoderMultiplier) / ENCODER_PULSES_PER_STEP;
           encoderDiff = 0;
         }
-        timeoutToStatus = ms + LCD_TIMEOUT_TO_STATUS;
+        return_to_status_ms = ms + LCD_TIMEOUT_TO_STATUS;
         lcdDrawUpdate = 1;
       }
     #endif //ULTIPANEL
@@ -1371,20 +1369,24 @@ void lcd_update() {
     #endif
 
     #ifdef ULTIPANEL
+
+      // Return to Status Screen after a timeout
       if (currentMenu != lcd_status_screen &&
-        #if defined(MANUAL_BED_LEVELING)
-          currentMenu != _lcd_level_bed && 
-          currentMenu != _lcd_level_bed_homing && 
-        #endif  // MANUAL_BED_LEVELING
-          millis() > timeoutToStatus) {
+        #ifdef MANUAL_BED_LEVELING
+          currentMenu != _lcd_level_bed &&
+          currentMenu != _lcd_level_bed_homing &&
+        #endif
+        millis() > return_to_status_ms
+      ) {
         lcd_return_to_status();
         lcdDrawUpdate = 2;
       }
-    #endif //ULTIPANEL
+
+    #endif // ULTIPANEL
 
     if (lcdDrawUpdate == 2) lcd_implementation_clear();
     if (lcdDrawUpdate) lcdDrawUpdate--;
-    lcd_next_update_millis = millis() + LCD_UPDATE_INTERVAL;
+    next_lcd_update_ms = millis() + LCD_UPDATE_INTERVAL;
   }
 }
 
@@ -1403,7 +1405,7 @@ void lcd_finishstatus(bool persist=false) {
   lcdDrawUpdate = 2;
 
   #ifdef FILAMENT_LCD_DISPLAY
-    message_millis = millis();  //get status message to show up for a while
+    previous_lcd_status_ms = millis();  //get status message to show up for a while
   #endif
 }
 
@@ -1473,7 +1475,7 @@ void lcd_buttons_update() {
     if (READ(BTN_EN1) == 0) newbutton |= EN_A;
     if (READ(BTN_EN2) == 0) newbutton |= EN_B;
     #if BTN_ENC > 0
-      if (millis() > blocking_enc && READ(BTN_ENC) == 0) newbutton |= EN_C;
+      if (millis() > next_button_update_ms && READ(BTN_ENC) == 0) newbutton |= EN_C;
     #endif
     buttons = newbutton;
     #ifdef LCD_HAS_SLOW_BUTTONS
@@ -1815,7 +1817,7 @@ char *ftostr52(const float &x) {
           current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
           line_to_current();
           mbl.active = 1;
-          enquecommands_P(PSTR("G28"));
+          enqueuecommands_P(PSTR("G28"));
           lcd_return_to_status();
         } else {
           current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
@@ -1857,7 +1859,7 @@ char *ftostr52(const float &x) {
     axis_known_position[Y_AXIS] = false;
     axis_known_position[Z_AXIS] = false;
     mbl.reset();
-    enquecommands_P(PSTR("G28"));
+    enqueuecommands_P(PSTR("G28"));
     lcdDrawUpdate = 2;
     lcd_goto_menu(_lcd_level_bed_homing);
   }
diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h
index fe4f597680..b8d5cba8ad 100644
--- a/Marlin/ultralcd.h
+++ b/Marlin/ultralcd.h
@@ -49,10 +49,11 @@
   extern bool cancel_heatup;
   
   #ifdef FILAMENT_LCD_DISPLAY
-    extern unsigned long message_millis;
+    extern millis_t previous_lcd_status_ms;
   #endif
 
   void lcd_buzz(long duration,uint16_t freq);
+  void lcd_quick_feedback(); // Audible feedback for a button click - could also be visual
   bool lcd_clicked();
 
   void lcd_ignore_click(bool b=true);
diff --git a/Marlin/ultralcd_implementation_hitachi_HD44780.h b/Marlin/ultralcd_implementation_hitachi_HD44780.h
index aa348018ef..2601575e3b 100644
--- a/Marlin/ultralcd_implementation_hitachi_HD44780.h
+++ b/Marlin/ultralcd_implementation_hitachi_HD44780.h
@@ -610,7 +610,7 @@ static void lcd_implementation_status_screen() {
 
     // Show Filament Diameter and Volumetric Multiplier %
     // After allowing lcd_status_message to show for 5 seconds
-    if (millis() >= message_millis + 5000) {
+    if (millis() >= previous_lcd_status_ms + 5000) {
       lcd_printPGM(PSTR("Dia "));
       lcd.print(ftostr12ns(filament_width_meas));
       lcd_printPGM(PSTR(" V"));
@@ -724,46 +724,45 @@ static void lcd_implementation_drawmenu_sddirectory(bool sel, uint8_t row, const
 #define lcd_implementation_drawmenu_function(sel, row, pstr, data) lcd_implementation_drawmenu_generic(sel, row, pstr, '>', ' ')
 
 #ifdef LCD_HAS_STATUS_INDICATORS
-static void lcd_implementation_update_indicators()
-{
-  #if defined(LCD_I2C_PANELOLU2) || defined(LCD_I2C_VIKI)
-    //set the LEDS - referred to as backlights by the LiquidTWI2 library 
-    static uint8_t ledsprev = 0;
-    uint8_t leds = 0;
-    if (target_temperature_bed > 0) leds |= LED_A;
-    if (target_temperature[0] > 0) leds |= LED_B;
-    if (fanSpeed) leds |= LED_C;
-    #if EXTRUDERS > 1  
-      if (target_temperature[1] > 0) leds |= LED_C;
+
+  static void lcd_implementation_update_indicators() {
+    #if defined(LCD_I2C_PANELOLU2) || defined(LCD_I2C_VIKI)
+      //set the LEDS - referred to as backlights by the LiquidTWI2 library 
+      static uint8_t ledsprev = 0;
+      uint8_t leds = 0;
+      if (target_temperature_bed > 0) leds |= LED_A;
+      if (target_temperature[0] > 0) leds |= LED_B;
+      if (fanSpeed) leds |= LED_C;
+      #if EXTRUDERS > 1  
+        if (target_temperature[1] > 0) leds |= LED_C;
+      #endif
+      if (leds != ledsprev) {
+        lcd.setBacklight(leds);
+        ledsprev = leds;
+      }
     #endif
-    if (leds != ledsprev) {
-      lcd.setBacklight(leds);
-      ledsprev = leds;
-    }
-  #endif
-}
-#endif
+  }
+
+#endif // LCD_HAS_STATUS_INDICATORS
 
 #ifdef LCD_HAS_SLOW_BUTTONS
-extern uint32_t blocking_enc;
 
-static uint8_t lcd_implementation_read_slow_buttons()
-{
-  #ifdef LCD_I2C_TYPE_MCP23017
-  uint8_t slow_buttons;
-    // Reading these buttons this is likely to be too slow to call inside interrupt context
-    // so they are called during normal lcd_update
-    slow_buttons = lcd.readButtons() << B_I2C_BTN_OFFSET; 
-    #if defined(LCD_I2C_VIKI)
-    if(slow_buttons & (B_MI|B_RI)) { //LCD clicked
-       if(blocking_enc > millis()) {
-         slow_buttons &= ~(B_MI|B_RI); // Disable LCD clicked buttons if screen is updated
-       }
-    }
+  extern millis_t next_button_update_ms;
+
+  static uint8_t lcd_implementation_read_slow_buttons() {
+    #ifdef LCD_I2C_TYPE_MCP23017
+      uint8_t slow_buttons;
+      // Reading these buttons this is likely to be too slow to call inside interrupt context
+      // so they are called during normal lcd_update
+      slow_buttons = lcd.readButtons() << B_I2C_BTN_OFFSET; 
+      #ifdef LCD_I2C_VIKI
+        if ((slow_buttons & (B_MI|B_RI)) && millis() < next_button_update_ms) // LCD clicked
+          slow_buttons &= ~(B_MI|B_RI); // Disable LCD clicked buttons if screen is updated
+      #endif
+      return slow_buttons;
     #endif
-    return slow_buttons; 
-  #endif
-}
-#endif
+  }
+
+#endif // LCD_HAS_SLOW_BUTTONS
 
 #endif //__ULTRALCD_IMPLEMENTATION_HITACHI_HD44780_H