Merge remote-tracking branch 'upstream/dev' into dev

Documentation/changelog.md

+### Version 4.2.81
+* Fix serial protocol for Repetier Host
+* Bug fix
+
 ### Version 4.2.8
 * Add board folder with files of various board containing the pins
 * Add End time on Graphics display when SD print

MK/Configuration_Basic.h

@@ -9,7 +9,6 @@
  * - Extruders number
  * - Thermistor type
  * - Temperature limits
- * - UI Language
  *
  * Mechanisms-settings can be found in Configuration_Xxxxxx.h (where Xxxxxx can be: Cartesian - Delta - Core - Scara)
  * Feature-settings can be found in Configuration_Feature.h
@@ -244,34 +243,4 @@
 #define GUM_PREHEAT_FAN_SPEED   255   // Insert Value between 0 and 255
 /*****************************************************************************************************/
 
-
-/***********************************************************************
- *************************** UI Language  ******************************
- ***********************************************************************
- *                                                                     *
- * Select the language that you prefer and change LANGUAGE_CHOICE      *
- *                                                                     *
- * 1  English                                                          *
- * 2  Polish                                                           *
- * 3  French                                                           *
- * 4  German                                                           *
- * 5  Spanish                                                          *
- * 6  Russian                                                          *
- * 7  Italian                                                          *
- * 8  Portuguese                                                       *
- * 9  Finnish                                                          *
- * 10 Aragonese                                                        *
- * 11 Dutch                                                            *
- * 12 Danish                                                           *
- * 13 Catalan                                                          *
- * 14 Basque-Euskera                                                   *
- * 15 Portuguese (Brazil)                                              *
- * 16 Bulgarian                                                        *
- * 17 Japanese                                                         *
- * 18 Japanese utf                                                     *
- * 19 Chinese                                                          *
- *                                                                     *
- ***********************************************************************/
-#define LANGUAGE_CHOICE 1
-/***********************************************************************/
 #endif

MK/Configuration_Delta.h

@@ -142,9 +142,6 @@
 // Precision for G30 delta autocalibration function
 #define AUTOCALIBRATION_PRECISION 0.1 // mm
 
-// Precision probe. Number of probe for the mean
-#define PROBE_COUNT 3
-
 // Z-Probe variables
 // Offsets to the probe relative to the extruder tip (Hotend - Probe)
 // X and Y offsets MUST be INTEGERS
@@ -163,13 +160,13 @@
 #define Z_PROBE_OFFSET_FROM_EXTRUDER -1     // Z offset: -below [of the nozzle] (always negative!)
 
 // Start and end location values are used to deploy/retract the probe (will move from start to end and back again)
-#define Z_PROBE_DEPLOY_START_LOCATION {0, 0, 20}   // X, Y, Z, E start location for z-probe deployment sequence
-#define Z_PROBE_DEPLOY_END_LOCATION {0, 0, 20}     // X, Y, Z, E end location for z-probe deployment sequence
-#define Z_PROBE_RETRACT_START_LOCATION {0, 0, 20}  // X, Y, Z, E start location for z-probe retract sequence
-#define Z_PROBE_RETRACT_END_LOCATION {0, 0, 20}    // X, Y, Z, E end location for z-probe retract sequence
+#define Z_PROBE_DEPLOY_START_LOCATION {0, 0, 30}   // X, Y, Z, E start location for z-probe deployment sequence
+#define Z_PROBE_DEPLOY_END_LOCATION {0, 0, 30}     // X, Y, Z, E end location for z-probe deployment sequence
+#define Z_PROBE_RETRACT_START_LOCATION {0, 0, 30}  // X, Y, Z, E start location for z-probe retract sequence
+#define Z_PROBE_RETRACT_END_LOCATION {0, 0, 30}    // X, Y, Z, E end location for z-probe retract sequence
 
 // How much the nozzle will be raised when travelling from between next probing points
-#define Z_RAISE_BETWEEN_PROBINGS 5
+#define Z_RAISE_BETWEEN_PROBINGS 30
 
 // Define the grid for bed level AUTO BED LEVELING GRID POINTS X AUTO BED LEVELING GRID POINTS.
 #define AUTO_BED_LEVELING_GRID_POINTS 9

MK/Configuration_Feature.h

@@ -53,6 +53,7 @@
  * ADDON FEATURES:
  * - EEPROM
  * - SDCARD
+ * - LCD Language
  * - LCD
  * - Canon RC-1 Remote
  * - Camera trigger
@@ -310,13 +311,21 @@
 #define THERMAL_PROTECTION_PERIOD    40     // Seconds
 #define THERMAL_PROTECTION_HYSTERESIS 4     // Degrees Celsius
 
-// Whenever an M104 or M109 increases the target temperature the firmware will wait for the
-// WATCH TEMP PERIOD to expire, and if the temperature hasn't increased by WATCH TEMP INCREASE
-// degrees, the machine is halted, requiring a hard reset. This test restarts with any M104/M109,
-//but only if the current temperature is far enough below the target for a reliable test.
-#define WATCH_TEMP_PERIOD  16               // Seconds
-#define WATCH_TEMP_INCREASE 4               // Degrees Celsius
+/**
+ * Whenever an M104 or M109 increases the target temperature the firmware will wait for the
+ * WATCH TEMP PERIOD to expire, and if the temperature hasn't increased by WATCH TEMP INCREASE
+ * degrees, the machine is halted, requiring a hard reset. This test restarts with any M104/M109,
+ * but only if the current temperature is far enough below the target for a reliable test.
+ *
+ * If you get false positives for "Heating failed" increase WATCH TEMP PERIOD and/or decrease WATCH TEMP INCREASE
+ * WATCH TEMP INCREASE should not be below 2.
+ */
+#define WATCH_TEMP_PERIOD  20               // Seconds
+#define WATCH_TEMP_INCREASE 2               // Degrees Celsius
 
+/**
+ * Thermal Protection parameters for the bed are just as above for hotends.
+ */
 //#define THERMAL_PROTECTION_BED 
 
 #define THERMAL_PROTECTION_BED_PERIOD    20 // Seconds
@@ -325,7 +334,7 @@
 
 //#define THERMAL_PROTECTION_COOLER
 
-#define THERMAL_PROTECTION_BED_COOLER    30 // Seconds
+#define THERMAL_PROTECTION_COOLER_PERIOD    30 // Seconds
 #define THERMAL_PROTECTION_COOLER_HYSTERESIS 3 // Degree Celsius
 
 // Using M141 to set cooling temperature the firmware will wait for the WATCH_COOLER_TEMP_PERIOD 
@@ -338,8 +347,17 @@
 #define WATCH_TEMP_COOLER_PERIOD 60          // Seconds
 #define WATCH_TEMP_COOLER_DECREASE 1         // Degree Celsius
 
-
-
+/**
+ * Whenever an M140 or M190 increases the target temperature the firmware will wait for the
+ * WATCH BED TEMP PERIOD to expire, and if the temperature hasn't increased by WATCH BED TEMP INCREASE
+ * degrees, the machine is halted, requiring a hard reset. This test restarts with any M140/M190,
+ * but only if the current temperature is far enough below the target for a reliable test.
+ *
+ * If you get too many "Heating failed" errors, increase WATCH BED TEMP PERIOD and/or decrease
+ * WATCH BED TEMP INCREASE. (WATCH BED TEMP INCREASE should not be below 2.)
+ */
+#define WATCH_BED_TEMP_PERIOD  60           // Seconds
+#define WATCH_BED_TEMP_INCREASE 2           // Degrees Celsius
 /********************************************************************************/
 
 
@@ -624,9 +642,21 @@
  *****************************************************************************************/
 //#define ADVANCE
 
-#define EXTRUDER_ADVANCE_K .0
+#define EXTRUDER_ADVANCE_K 0.0
 #define D_FILAMENT 1.75
-#define STEPS_PER_CUBIC_MM_E 0.85
+/*****************************************************************************************/
+
+
+/*****************************************************************************************
+ ****************** Extruder Advance Linear Pressure Control *****************************
+ *****************************************************************************************
+ *                                                                                       *
+ * Assumption: advance = k * (delta velocity)                                            *
+ * K=0 means advance disabled. A good value for a gregs wade extruder will be around K=75*
+ *                                                                                       *
+ *****************************************************************************************/
+//#define ADVANCE_LPC
+#define ADVANCE_LPC_K 75
 /*****************************************************************************************/
 
 
@@ -1074,6 +1104,7 @@
 /**************************************************************************/
 
 
+
 //===========================================================================
 //============================= ADDON FEATURES ==============================
 //===========================================================================
@@ -1130,9 +1161,25 @@
 /*****************************************************************************************/
 
 
-/************************************************************************************************
- ********************************************* LCD **********************************************
- ************************************************************************************************/
+/***********************************************************************
+ *************************** LCD Language ******************************
+ ***********************************************************************
+ *                                                                     *
+ * Here you may choose the language used by MK or MK4due               *
+ * on the LCD menus, the following                                     *
+ *                                                                     *
+ * list of languages are available:                                    *
+ *    en, pl, fr, de, es, ru, it, pt, fi, an, nl, ca, eu               *
+ *    pt-br, bg, kana, kana_utf8, cn                                   *
+ *                                                                     *
+ ***********************************************************************/
+#define LCD_LANGUAGE en
+/***********************************************************************/
+
+
+/***********************************************************************
+ ******************************* LCD ***********************************
+ ***********************************************************************/
 
 //Charset type
 //Choose ONE of these 3 charsets. This has to match your hardware.

MK/Configuration_Store.cpp

-#include "base.h"
+/**
+ * MK & MK4due 3D Printer Firmware
+ *
+ * Based on Marlin, Sprinter and grbl
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
 
 /**
  * configuration_store.cpp
@@ -15,6 +35,7 @@
  *       either sets a Sane Default, or results in No Change to the existing value.
  *
  */
+#include "base.h"
 
 #define EEPROM_VERSION "MKV429"
 
@@ -665,8 +686,8 @@ void Config_ResetDefault() {
       ECHO_LM(CFG, "Maximum feedrates (mm/s):");
     }
     ECHO_SMV(CFG, "  M203 X", max_feedrate[X_AXIS]);
-    ECHO_MV(" Y", max_feedrate[Y_AXIS] ); 
-    ECHO_MV(" Z", max_feedrate[Z_AXIS] ); 
+    ECHO_MV(" Y", max_feedrate[Y_AXIS] );
+    ECHO_MV(" Z", max_feedrate[Z_AXIS] );
     ECHO_EMV(" E", max_feedrate[E_AXIS]);
     #if EXTRUDERS > 1
       for (short i = 1; i < EXTRUDERS; i++) {

MK/Configuration_Store.h

+/**
+ * MK & MK4due 3D Printer Firmware
+ *
+ * Based on Marlin, Sprinter and grbl
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
 #ifndef CONFIGURATION_STORE_H
-#define CONFIGURATION_STORE_H
+  #define CONFIGURATION_STORE_H
 
-void Config_ResetDefault();
-void ConfigSD_ResetDefault();
+  void Config_ResetDefault();
+  void ConfigSD_ResetDefault();
 
-#if DISABLED(DISABLE_M503)
-void Config_PrintSettings(bool forReplay = false);
-void ConfigSD_PrintSettings(bool forReplay = false);
-#else
-FORCE_INLINE void Config_PrintSettings(bool forReplay = false) {}
-FORCE_INLINE void ConfigSD_PrintSettings(bool forReplay = false) {}
-#endif
+  #if DISABLED(DISABLE_M503)
+    void Config_PrintSettings(bool forReplay = false);
+    void ConfigSD_PrintSettings(bool forReplay = false);
+  #else
+    FORCE_INLINE void Config_PrintSettings(bool forReplay = false) {}
+    FORCE_INLINE void ConfigSD_PrintSettings(bool forReplay = false) {}
+  #endif
 
-#if ENABLED(EEPROM_SETTINGS)
-void Config_StoreSettings();
-void Config_RetrieveSettings();
-#else
-FORCE_INLINE void Config_StoreSettings() {}
-FORCE_INLINE void Config_RetrieveSettings() { Config_ResetDefault(); Config_PrintSettings(); }
-#endif
+  #if ENABLED(EEPROM_SETTINGS)
+    void Config_StoreSettings();
+    void Config_RetrieveSettings();
+  #else
+    FORCE_INLINE void Config_StoreSettings() {}
+    FORCE_INLINE void Config_RetrieveSettings() { Config_ResetDefault(); Config_PrintSettings(); }
+  #endif
 
-#if ENABLED(SDSUPPORT) && ENABLED(SD_SETTINGS)
-#define CFG_SD_MAX_KEY_LEN    3+1         // icrease this if you add key name longer than the actual value.
-#define CFG_SD_MAX_VALUE_LEN  10+1        // this should be enought for int, long and float if you need to retrive strings increase this carefully
-//(11 = strlen("4294967295")+1) (4294967295 = (2^32)-1) (32 = the num of bits of the bigger basic data scructor used)
-//If yuou need to save string increase this to strlen("YOUR LONGER STRING")+1
-void ConfigSD_StoreSettings();
-void ConfigSD_RetrieveSettings(bool addValue = false);
-int ConfigSD_KeyIndex(char *key);
-#else
-FORCE_INLINE void ConfigSD_RetrieveSettings() {  ConfigSD_ResetDefault(); }
-#endif
+  #if ENABLED(SDSUPPORT) && ENABLED(SD_SETTINGS)
+    #define CFG_SD_MAX_KEY_LEN    3+1         // icrease this if you add key name longer than the actual value.
+    #define CFG_SD_MAX_VALUE_LEN  10+1        // this should be enought for int, long and float if you need to retrive strings increase this carefully
+    //(11 = strlen("4294967295")+1) (4294967295 = (2^32)-1) (32 = the num of bits of the bigger basic data scructor used)
+    //If yuou need to save string increase this to strlen("YOUR LONGER STRING")+1
+    void ConfigSD_StoreSettings();
+    void ConfigSD_RetrieveSettings(bool addValue = false);
+    int ConfigSD_KeyIndex(char *key);
+  #else
+    FORCE_INLINE void ConfigSD_RetrieveSettings() {  ConfigSD_ResetDefault(); }
+  #endif
 
 #endif //CONFIGURATION_STORE_H

MK/Configuration_Version.h

+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
 #ifndef CONFIGURATION_VERSION_H
   #define CONFIGURATION_VERSION_H
 
   #define FIRMWARE_NAME "MK"
-  #define SHORT_BUILD_VERSION "4.2.8_dev"
+  #define SHORT_BUILD_VERSION "4.2.81_dev"
   #define BUILD_VERSION FIRMWARE_NAME "_" SHORT_BUILD_VERSION
   #define STRING_DISTRIBUTION_DATE __DATE__ " " __TIME__    // build date and time
   // It might also be appropriate to define a location where additional information can be found

MK/base.h

@@ -59,6 +59,7 @@
 #include "module/motion/planner.h"
 #include "module/motion/stepper_indirection.h"
 #include "module/motion/stepper.h"
+#include "module/motion/endstops.h"
 #include "module/motion/vector_3.h"
 #include "module/motion/qr_solve.h"
 #include "module/motion/cartesian_correction.h"

MK/module/MK_Main.cpp

@@ -120,7 +120,6 @@ Stopwatch print_job_timer = Stopwatch();
 
 static uint8_t target_extruder;
 
-bool no_wait_for_cooling = true;
 bool software_endstops = true;
 
 unsigned long printer_usage_seconds;
@@ -225,9 +224,9 @@ double printer_usage_filament;
   static float adj_t1_Radius = 0;
   static float adj_t2_Radius = 0;
   static float adj_t3_Radius = 0;
-  static float z_offset;
   static float bed_level_c, bed_level_x, bed_level_y, bed_level_z;
   static float bed_level_ox, bed_level_oy, bed_level_oz;
+  static float bed_safe_z;
   static int loopcount;
   static bool home_all_axis = true;
 #else
@@ -347,8 +346,6 @@ void stop();
 void get_available_commands();
 void process_next_command();
 
-inline void refresh_cmd_timeout() { previous_cmd_ms = millis(); }
-
 void delay_ms(millis_t ms) {
   ms += millis();
   while (millis() < ms) idle();
@@ -356,8 +353,29 @@ void delay_ms(millis_t ms) {
 
 void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
 
+void gcode_M114();
+
 static void report_current_position();
 
+void print_xyz(const char* prefix, const float x, const float y, const float z) {
+  ECHO_T(prefix);
+  ECHO_MV(": (", x);
+  ECHO_MV(", ", y);
+  ECHO_MV(", ", z);
+  ECHO_M(")");
+  ECHO_E;
+}
+
+void print_xyz(const char* prefix, const float xyz[]) {
+  print_xyz(prefix, xyz[X_AXIS], xyz[Y_AXIS], xyz[Z_AXIS]);
+}
+#if ENABLED(AUTO_BED_LEVELING_FEATURE)
+  void print_xyz(const char* prefix, const vector_3 &xyz) {
+    print_xyz(prefix, xyz.x, xyz.y, xyz.z);
+  }
+#endif
+#define DEBUG_POS(PREFIX, VAR) do{ ECHO_SM(INFO,PREFIX); print_xyz(" > " STRINGIFY(VAR), VAR); }while(0)
+
 #if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
   float extrude_min_temp = EXTRUDE_MINTEMP;
 #endif
@@ -797,7 +815,7 @@ void loop() {
     commands_in_queue--;
     cmd_queue_index_r = (cmd_queue_index_r + 1) % BUFSIZE;
   }
-  checkHitEndstops();
+  endstops.report_state();
   idle();
 }
 
@@ -1144,21 +1162,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir,  HOME_DIR);
 
 #endif //DUAL_X_CARRIAGE
 
-void print_xyz(const char* prefix, const float x, const float y, const float z) {
-  ECHO_T(prefix);
-  ECHO_MV(": (", x);
-  ECHO_MV(", ", y);
-  ECHO_MV(", ", z);
-  ECHO_M(")");
-  ECHO_E;
-}
 
-void print_xyz(const char* prefix, const float xyz[]) {
-  print_xyz(prefix, xyz[X_AXIS], xyz[Y_AXIS], xyz[Z_AXIS]);
-}
-
-#define DEBUG_POS(PREFIX, VAR) do{ ECHO_SM(INFO,PREFIX); print_xyz(" > " STRINGIFY(VAR), VAR); }while(0)
-  
 /**
  * Software endstops can be used to monitor the open end of
  * an axis that has a hardware endstop on the other end. Or
@@ -1346,17 +1350,17 @@ static void setup_for_endstop_move() {
   saved_feedrate_multiplier = feedrate_multiplier;
   feedrate_multiplier = 100;
   refresh_cmd_timeout();
-  enable_endstops(true);
+  endstops.enable();
 }
 
 static void clean_up_after_endstop_move() {
   #if ENABLED(ENDSTOPS_ONLY_FOR_HOMING)
-    enable_endstops(false);
+    if (DEBUGGING(INFO)) ECHO_LM(INFO, "clean_up_after_endstop_move > ENDSTOPS_ONLY_FOR_HOMING > endstops.not_homing()");
   #endif
+  endstops.not_homing();
   feedrate = saved_feedrate;
   feedrate_multiplier = saved_feedrate_multiplier;
   refresh_cmd_timeout();
-  endstops_hit_on_purpose(); // clear endstop hit flags
 }
 
 static void axis_unhomed_error() {
@@ -1364,20 +1368,31 @@ static void axis_unhomed_error() {
   ECHO_LM(ER, MSG_POSITION_UNKNOWN);
 }
 
-#if MECH(CARTESIAN) || MECH(COREXY) || MECH(COREYX) || MECH(COREXZ) || MECH(COREZX) || MECH(SCARA)
+/**
+ *  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;
 
-  /**
-   *  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;
-    feedrate = homing_feedrate[Z_AXIS];
+  if (DEBUGGING(INFO)) {
+    ECHO_S(INFO);
+    print_xyz("do_blocking_move_to", x, y, z);
+  }
 
-    if (DEBUGGING(INFO)) {
-      ECHO_S(INFO);
-      print_xyz("do_blocking_move_to", x, y, z);
-    }
+  #if MECH(DELTA)
+
+    feedrate = AUTOCAL_TRAVELRATE * 60;
+
+    destination[X_AXIS] = x;
+    destination[Y_AXIS] = y;
+    destination[Z_AXIS] = z;
+    prepare_move_raw(); // this will also set_current_to_destination
+    st_synchronize();
+
+  #else
+
+    feedrate = homing_feedrate[Z_AXIS];
 
     current_position[Z_AXIS] = z;
     line_to_current_position();
@@ -1390,15 +1405,18 @@ static void axis_unhomed_error() {
     line_to_current_position();
     st_synchronize();
 
-    feedrate = oldFeedRate;
-  }
+  #endif
 
-  inline void do_blocking_move_to_xy(float x, float y) { do_blocking_move_to(x, y, current_position[Z_AXIS]); }
-  inline void do_blocking_move_to_x(float x) { do_blocking_move_to(x, current_position[Y_AXIS], current_position[Z_AXIS]); }
-  inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z); }
+  feedrate = oldFeedRate;
+}
 
-  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+inline void do_blocking_move_to_xy(float x, float y) { do_blocking_move_to(x, y, current_position[Z_AXIS]); }
+inline void do_blocking_move_to_x(float x) { do_blocking_move_to(x, current_position[Y_AXIS], current_position[Z_AXIS]); }
+inline void do_blocking_move_to_z(float z) { do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z); }
+
+#if MECH(CARTESIAN) || MECH(COREXY) || MECH(COREYX) || MECH(COREXZ) || MECH(COREZX) || MECH(SCARA)
 
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
     #if ENABLED(AUTO_BED_LEVELING_GRID)
       static void set_bed_level_equation_lsq(double *plane_equation_coefficients) {
         if (DEBUGGING(INFO)) {
@@ -1460,6 +1478,12 @@ static void axis_unhomed_error() {
 
     static void run_z_probe() {
 
+      /**
+       * To prevent stepper_inactive_time from running out and
+       * EXTRUDER_RUNOUT_PREVENT from extruding
+       */
+      refresh_cmd_timeout();
+
       plan_bed_level_matrix.set_to_identity();
       feedrate = homing_feedrate[Z_AXIS];
 
@@ -1476,7 +1500,7 @@ static void axis_unhomed_error() {
       zPosition += home_bump_mm(Z_AXIS);
       line_to_z(zPosition);
       st_synchronize();
-      endstops_hit_on_purpose(); // clear endstop hit flags
+      endstops.hit_on_purpose(); // clear endstop hit flags
 
       // move back down slowly to find bed
       set_homing_bump_feedrate(Z_AXIS);
@@ -1484,7 +1508,7 @@ static void axis_unhomed_error() {
       zPosition -= home_bump_mm(Z_AXIS) * 2;
       line_to_z(zPosition);
       st_synchronize();
-      endstops_hit_on_purpose(); // clear endstop hit flags
+      endstops.hit_on_purpose(); // clear endstop hit flags
 
       // Get the current stepper position after bumping an endstop
       current_position[Z_AXIS] = st_get_axis_position_mm(Z_AXIS);
@@ -1498,16 +1522,22 @@ static void axis_unhomed_error() {
       if (DEBUGGING(INFO))
         DEBUG_POS("deploy_z_probe", current_position);
 
+      if (endstops.z_probe_enabled) return;
+
       #if HAS(SERVO_ENDSTOPS)
         // Engage Z Servo endstop if enabled
         if (servo_endstop_id[Z_AXIS] >= 0) servo[servo_endstop_id[Z_AXIS]].move(servo_endstop_angle[Z_AXIS][0]);
       #endif
+
+      endstops.enable_z_probe();
     }
 
     static void stow_z_probe(bool doRaise = true) {
       if (DEBUGGING(INFO))
         DEBUG_POS("stow_z_probe", current_position);
 
+      if (!endstops.z_probe_enabled) return;
+
       #if HAS(SERVO_ENDSTOPS)
         // Retract Z Servo endstop if enabled
         if (servo_endstop_id[Z_AXIS] >= 0) {
@@ -1527,6 +1557,8 @@ static void axis_unhomed_error() {
           servo[servo_endstop_id[Z_AXIS]].move(servo_endstop_angle[Z_AXIS][1]);
         }
       #endif
+
+      endstops.enable_z_probe(false);
     }
 
     enum ProbeAction {
@@ -1588,7 +1620,11 @@ static void axis_unhomed_error() {
     #if HAS(SERVO_ENDSTOPS) && HASNT(Z_PROBE_SLED)
       void raise_z_for_servo() {
         float zpos = current_position[Z_AXIS], z_dest = Z_RAISE_BEFORE_PROBING;
-        z_dest += TEST(axis_was_homed, Z_AXIS) ? zprobe_zoffset : zpos;
+        /**
+         * The zprobe_zoffset is negative any switch below the nozzle, so
+         * multiply by Z_HOME_DIR (-1) to move enough away from bed for the probe
+         */
+        z_dest += axis_homed[Z_AXIS] ? zprobe_zoffset * Z_HOME_DIR : zpos;
         if (zpos < z_dest) do_blocking_move_to_z(z_dest); // also updates current_position
       }
     #endif
@@ -1652,14 +1688,14 @@ static void axis_unhomed_error() {
       current_position[axis] = 0;
       sync_plan_position();
 
-      enable_endstops(false); // Disable endstops while moving away
+      endstops.enable(false); // Disable endstops while moving away
 
       // Move away from the endstop by the axis HOME_BUMP_MM
       destination[axis] = -home_bump_mm(axis) * axis_home_dir;
       line_to_destination();
       st_synchronize();
 
-      enable_endstops(true); // Enable endstops for next homing move
+      endstops.enable(); // Enable endstops for next homing move
 
       // Slow down the feedrate for the next move
       set_homing_bump_feedrate(axis);
@@ -1706,7 +1742,7 @@ static void axis_unhomed_error() {
 
       destination[axis] = current_position[axis];
       feedrate = 0.0;
-      endstops_hit_on_purpose(); // clear endstop hit flags
+      endstops.hit_on_purpose(); // clear endstop hit flags
       axis_known_position[axis] = true;
       axis_homed[axis] = true;
 
@@ -1769,14 +1805,14 @@ static void axis_unhomed_error() {
       current_position[axis] = 0;
       sync_plan_position();
 
-      enable_endstops(false); // Disable endstops while moving away
+      endstops.enable(false); // Disable endstops while moving away
 
       // Move away from the endstop by the axis HOME_BUMP_MM
       destination[axis] = -home_bump_mm(axis) * axis_home_dir;
       line_to_destination();
       st_synchronize();
 
-      enable_endstops(true); // Enable endstops for next homing move
+      endstops.enable(); // Enable endstops for next homing move
 
       // Slow down the feedrate for the next move
       set_homing_bump_feedrate(axis);
@@ -1788,7 +1824,7 @@ static void axis_unhomed_error() {
 
       // retrace by the amount specified in endstop_adj
       if (endstop_adj[axis] * axis_home_dir < 0) {
-        enable_endstops(false); // Disable Endstops while moving away
+        endstops.enable(false); // Disable Endstops while moving away
         sync_plan_position();
         destination[axis] = endstop_adj[axis];
         if (DEBUGGING(INFO)) {
@@ -1797,7 +1833,7 @@ static void axis_unhomed_error() {
         }
         line_to_destination();
         st_synchronize();
-        enable_endstops(true); // Enable Endstops for next homing move
+        endstops.enable(); // Enable Endstops for next homing move
       }
 
       if (DEBUGGING(INFO)) ECHO_LMV(INFO, " > endstop_adj * axis_home_dir = ", endstop_adj[axis] * axis_home_dir);
@@ -1811,7 +1847,7 @@ static void axis_unhomed_error() {
 
       destination[axis] = current_position[axis];
       feedrate = 0.0;
-      endstops_hit_on_purpose(); // clear Endstop hit flags
+      endstops.hit_on_purpose(); // clear Endstop hit flags
       axis_known_position[axis] = true;
       axis_homed[axis] = true;
     }
@@ -1823,9 +1859,9 @@ static void axis_unhomed_error() {
     base_max_pos[Z_AXIS]  = sw_endstop_max[Z_AXIS];
     base_home_pos[Z_AXIS] = sw_endstop_max[Z_AXIS];
 
-    delta_diagonal_rod_1 = pow(delta_diagonal_rod + diagrod_adj[0], 2);
-    delta_diagonal_rod_2 = pow(delta_diagonal_rod + diagrod_adj[1], 2);
-    delta_diagonal_rod_3 = pow(delta_diagonal_rod + diagrod_adj[2], 2);
+    delta_diagonal_rod_1 = sq(delta_diagonal_rod + diagrod_adj[0]);
+    delta_diagonal_rod_2 = sq(delta_diagonal_rod + diagrod_adj[1]);
+    delta_diagonal_rod_3 = sq(delta_diagonal_rod + diagrod_adj[2]);
 
     // Effective X/Y positions of the three vertical towers.
     delta_tower1_x = (delta_radius + tower_adj[3]) * cos((210 + tower_adj[0]) * M_PI/180); // front left tower
@@ -1836,7 +1872,7 @@ static void axis_unhomed_error() {
     delta_tower3_y = (delta_radius + tower_adj[5]) * sin((90 + tower_adj[2]) * M_PI/180); 
   }
 
-  bool Equal_AB(const float A, const float B, const float prec = 0.001) {
+  bool Equal_AB(const float A, const float B, const float prec = ac_prec) {
     if (abs(A - B) <= prec) return true;
     return false;
   }
@@ -1886,7 +1922,7 @@ static void axis_unhomed_error() {
   }
 
   // Reset calibration results to zero.
-  void reset_bed_level() {
+  static void reset_bed_level() {
     if (DEBUGGING(INFO)) ECHO_LM(INFO, "reset_bed_level");
     for (int y = 0; y < AUTO_BED_LEVELING_GRID_POINTS; y++) {
       for (int x = 0; x < AUTO_BED_LEVELING_GRID_POINTS; x++) {
@@ -1895,76 +1931,147 @@ static void axis_unhomed_error() {
     }
   }
 
-  void deploy_z_probe() {
+  static void deploy_z_probe() {
+
+    if (endstops.z_probe_enabled) return;
+
     #if HAS(SERVO_ENDSTOPS)
-      feedrate = homing_feedrate[X_AXIS];
-      destination[X_AXIS] = z_probe_deploy_start_location[X_AXIS];
-      destination[Y_AXIS] = z_probe_deploy_start_location[Y_AXIS];
-      destination[Z_AXIS] = z_probe_deploy_start_location[Z_AXIS];
-      prepare_move_raw();
-      st_synchronize();
+      feedrate = homing_feedrate[Z_AXIS];
+      do_blocking_move_to_z(  z_probe_deploy_start_location[Z_AXIS]);
+      do_blocking_move_to_xy( z_probe_deploy_start_location[X_AXIS],
+                              z_probe_deploy_start_location[Y_AXIS]);
 
       // Engage Z Servo endstop if enabled
       if (servo_endstop_id[Z_AXIS] >= 0)
         servo[servo_endstop_id[Z_AXIS]].move(servo_endstop_angle[Z_AXIS][0]);
     #else
-      feedrate = homing_feedrate[X_AXIS];
-      destination[X_AXIS] = z_probe_deploy_start_location[X_AXIS];
-      destination[Y_AXIS] = z_probe_deploy_start_location[Y_AXIS];
-      destination[Z_AXIS] = z_probe_deploy_start_location[Z_AXIS];
-      prepare_move_raw();
-
-      feedrate = homing_feedrate[X_AXIS]/10;
-      destination[X_AXIS] = z_probe_deploy_end_location[X_AXIS];
-      destination[Y_AXIS] = z_probe_deploy_end_location[Y_AXIS];
-      destination[Z_AXIS] = z_probe_deploy_end_location[Z_AXIS];
-      prepare_move_raw();
-
-      feedrate = homing_feedrate[X_AXIS];
-      destination[X_AXIS] = z_probe_deploy_start_location[X_AXIS];
-      destination[Y_AXIS] = z_probe_deploy_start_location[Y_AXIS];
-      destination[Z_AXIS] = z_probe_deploy_start_location[Z_AXIS];
-      prepare_move_raw();
-      st_synchronize();
+      feedrate = homing_feedrate[Z_AXIS];
+      do_blocking_move_to_z(  z_probe_deploy_start_location[Z_AXIS]);
+      do_blocking_move_to_xy( z_probe_deploy_start_location[X_AXIS],
+                              z_probe_deploy_start_location[Y_AXIS]);
+
+      feedrate = homing_feedrate[Z_AXIS]/10;
+      do_blocking_move_to(z_probe_deploy_end_location[X_AXIS],
+                          z_probe_deploy_end_location[Y_AXIS],
+                          z_probe_deploy_end_location[Z_AXIS]);
+
+      feedrate = homing_feedrate[Z_AXIS];
+      
+      do_blocking_move_to(z_probe_deploy_start_location[X_AXIS],
+                          z_probe_deploy_start_location[Y_AXIS],
+                          z_probe_deploy_start_location[Z_AXIS]);
     #endif
+
+    endstops.enable_z_probe();
+    sync_plan_position_delta();
   }
 
-  void retract_z_probe() {
-    #if HAS(SERVO_ENDSTOPS)
-      feedrate = homing_feedrate[X_AXIS];
-      destination[X_AXIS] = z_probe_retract_start_location[X_AXIS];
-      destination[Y_AXIS] = z_probe_retract_start_location[Y_AXIS];
-      destination[Z_AXIS] = z_probe_retract_start_location[Z_AXIS];
-      prepare_move_raw();
-      st_synchronize();
+  static void retract_z_probe() {
+
+    if (!endstops.z_probe_enabled) return;
 
+    #if HAS(SERVO_ENDSTOPS)
       // Retract Z Servo endstop if enabled
       if (servo_endstop_id[Z_AXIS] >= 0)
         servo[servo_endstop_id[Z_AXIS]].move(servo_endstop_angle[Z_AXIS][1]);
+
+      feedrate = homing_feedrate[Z_AXIS];
+      do_blocking_move_to(z_probe_retract_start_location[X_AXIS],
+                          z_probe_retract_start_location[Y_AXIS],
+                          z_probe_retract_start_location[Z_AXIS]);
     #else
-      feedrate = homing_feedrate[X_AXIS];
-      destination[X_AXIS] = z_probe_retract_start_location[X_AXIS];
-      destination[Y_AXIS] = z_probe_retract_start_location[Y_AXIS];
-      destination[Z_AXIS] = z_probe_retract_start_location[Z_AXIS];
-      prepare_move_raw();
+      feedrate = homing_feedrate[Z_AXIS];
+      do_blocking_move_to(z_probe_retract_start_location[X_AXIS],
+                          z_probe_retract_start_location[Y_AXIS],
+                          z_probe_retract_start_location[Z_AXIS]);
 
       // Move the nozzle below the print surface to push the probe up.
       feedrate = homing_feedrate[Z_AXIS]/10;
-      destination[X_AXIS] = z_probe_retract_end_location[X_AXIS];
-      destination[Y_AXIS] = z_probe_retract_end_location[Y_AXIS];
-      destination[Z_AXIS] = z_probe_retract_end_location[Z_AXIS];
-      prepare_move_raw();
+      do_blocking_move_to(z_probe_retract_end_location[X_AXIS],
+                          z_probe_retract_end_location[Y_AXIS],
+                          z_probe_retract_end_location[Z_AXIS]);
 
       feedrate = homing_feedrate[Z_AXIS];
-      destination[X_AXIS] = z_probe_retract_start_location[X_AXIS];
-      destination[Y_AXIS] = z_probe_retract_start_location[Y_AXIS];
-      destination[Z_AXIS] = z_probe_retract_start_location[Z_AXIS];
-      prepare_move_raw();
-      st_synchronize();
+      do_blocking_move_to(z_probe_retract_start_location[X_AXIS],
+                          z_probe_retract_start_location[Y_AXIS],
+                          z_probe_retract_start_location[Z_AXIS]);
     #endif
+
+    endstops.enable_z_probe(false);
+    sync_plan_position_delta();
   }
 
-  void apply_endstop_adjustment(float x_endstop, float y_endstop, float z_endstop) {
+  static void run_z_probe() {
+    refresh_cmd_timeout();
+
+    endstops.enable();
+    float start_z = current_position[Z_AXIS];
+    long start_steps = st_get_position(Z_AXIS);
+
+    feedrate = AUTOCAL_PROBERATE * 60;
+    destination[Z_AXIS] = -20;
+    prepare_move_raw();
+    st_synchronize();
+    endstops.hit_on_purpose(); // clear endstop hit flags
+
+    endstops.enable(false);
+    long stop_steps = st_get_position(Z_AXIS);
+    float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
+    current_position[Z_AXIS] = mm;
+    sync_plan_position_delta();
+  }
+
+  // Probe bed height at position (x,y), returns the measured z value
+  static float probe_bed(float x, float y) {
+
+    // Move Z up to the bed_safe_z
+    do_blocking_move_to_z(bed_safe_z);
+
+    float Dx = x - z_probe_offset[X_AXIS];
+    NOLESS(Dx, X_MIN_POS);
+    NOMORE(Dx, X_MAX_POS);
+    float Dy = y - z_probe_offset[Y_AXIS];
+    NOLESS(Dy, Y_MIN_POS);
+    NOMORE(Dy, Y_MAX_POS);
+
+    if (DEBUGGING(INFO)) {
+      ECHO_LM(INFO, "probe_bed >>>");
+      DEBUG_POS("", current_position);
+      ECHO_SMV(INFO, " > do_blocking_move_to_xy ", Dx);
+      ECHO_EMV(", ", Dy);
+    }
+
+    // this also updates current_position
+    do_blocking_move_to_xy(Dx, Dy);
+
+    run_z_probe();
+    float probe_z = current_position[Z_AXIS] + z_probe_offset[Z_AXIS];
+
+    if (DEBUGGING(INFO)) {
+      ECHO_SM(INFO, "Bed probe heights: ");
+      if (probe_z >= 0) ECHO_M(" ");
+      ECHO_EV(probe_z, 4);
+    }
+
+    bed_safe_z = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
+    return probe_z;
+  }
+
+  static void bed_probe_all() {
+    // Initial throwaway probe.. used to stabilize probe
+    bed_level_c = probe_bed(0.0, 0.0);
+
+    // Probe all bed positions & store carriage positions
+    bed_level_z = probe_bed(0.0, bed_radius);
+    bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius);
+    bed_level_x = probe_bed(-SIN_60 * bed_radius, -COS_60 * bed_radius);
+    bed_level_oz = probe_bed(0.0, -bed_radius);
+    bed_level_y = probe_bed(SIN_60 * bed_radius, -COS_60 * bed_radius);
+    bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius);
+    bed_level_c = probe_bed(0.0, 0.0);
+  }
+
+  static void apply_endstop_adjustment(float x_endstop, float y_endstop, float z_endstop) {
     memcpy(saved_endstop_adj, endstop_adj, sizeof(saved_endstop_adj));
     endstop_adj[X_AXIS] += x_endstop;
     endstop_adj[Y_AXIS] += y_endstop;
@@ -1975,7 +2082,7 @@ static void axis_unhomed_error() {
     st_synchronize();
   }
 
-  void adj_endstops() {
+  static void adj_endstops() {
     boolean x_done = false;
     boolean y_done = false;
     boolean z_done = false;
@@ -2082,9 +2189,9 @@ static void axis_unhomed_error() {
     xy_equal = false;
     xz_equal = false;
     yz_equal = false;
-    if (Equal_AB(x_diff, y_diff, ac_prec)) xy_equal = true;
-    if (Equal_AB(x_diff, z_diff, ac_prec)) xz_equal = true;
-    if (Equal_AB(y_diff, z_diff, ac_prec)) yz_equal = true;
+    if (Equal_AB(x_diff, y_diff)) xy_equal = true;
+    if (Equal_AB(x_diff, z_diff)) xz_equal = true;
+    if (Equal_AB(y_diff, z_diff)) yz_equal = true;
 
     ECHO_SM(DB, "xy_equal = ");
     if (xy_equal == true) ECHO_EM("true"); else ECHO_EM("false");
@@ -2102,7 +2209,7 @@ static void axis_unhomed_error() {
 
     ECHO_LMV(DB, "Opp Range = ", high_opp - low_opp, 5);
 
-    if (Equal_AB(high_opp, low_opp, ac_prec)) {
+    if (Equal_AB(high_opp, low_opp)) {
       ECHO_LM(DB, "Opposite Points within Limits - Adjustment not required");
       t1_err = false;
       t2_err = false;
@@ -2164,7 +2271,6 @@ static void axis_unhomed_error() {
 
   bool adj_deltaradius() { 
     float adj_r;
-    float prev_c;
     uint8_t c_nochange_count = 0;
     float nochange_r;
 
@@ -2176,15 +2282,16 @@ static void axis_unhomed_error() {
     }
     else {
       ECHO_LM(DB, "Adjusting Delta Radius");
+      ECHO_LMV(DB, "Bed level center = ", bed_level_c);
+
       // set initial direction and magnitude for delta radius adjustment
-      adj_r = 0.5;
-      if (bed_level_c > 0) adj_r = -0.5;
+      adj_r = 0.2;
+      if (bed_level_c > 0) adj_r = -0.2;
 
       do {
         delta_radius += adj_r;
         set_delta_constants();
 
-        prev_c = bed_level_c;
         bed_level_c = probe_bed(0.0, 0.0);
 
         //Show progress
@@ -2193,24 +2300,16 @@ static void axis_unhomed_error() {
         ECHO_EMV(") c:", bed_level_c, 4);
 
         //Adjust delta radius
-        if (((adj_r > 0) and (bed_level_c < prev_c)) or ((adj_r < 0) and (bed_level_c > prev_c))) adj_r = -(adj_r / 2);
+        if (bed_level_c < 0) adj_r =  (abs(adj_r) / 2);
+        if (bed_level_c > 0) adj_r = -(abs(adj_r) / 2);
 
-        //Count iterations with no change to c probe point
-        if (Equal_AB(bed_level_c, prev_c)) c_nochange_count ++;
-        if (c_nochange_count == 1) nochange_r = delta_radius;
-
-      } while(((bed_level_c < -ac_prec) or (bed_level_c > ac_prec)) and (c_nochange_count < 3));
-
-      if (c_nochange_count > 0) {
-        delta_radius = nochange_r;
-        set_delta_constants();
-      }
+      } while(bed_level_c < -ac_prec or bed_level_c > ac_prec);
 
       return true;
     }
   }
 
-  void adj_tower_radius(int tower) {
+  static void adj_tower_radius(int tower) {
     boolean done,t1_done,t2_done,t3_done;
     int nochange_count;
     float target, prev_target, prev_bed_level;
@@ -2268,8 +2367,8 @@ static void axis_unhomed_error() {
         temp = (bed_level_ox - target) / 2;
         adj_target = target + temp;
         if (((bed_level_ox < adj_target) and (adj_t1_Radius > 0)) or ((bed_level_ox > adj_target) and (adj_t1_Radius < 0))) adj_t1_Radius = -(adj_t1_Radius / 2);
-        if (Equal_AB(bed_level_ox, adj_target)) t1_done = true;
-        if (Equal_AB(bed_level_ox, prev_bed_level) and Equal_AB(adj_target, prev_target)) nochange_count ++;
+        if (Equal_AB(bed_level_ox, adj_target, ac_prec / 2)) t1_done = true;
+        if (Equal_AB(bed_level_ox, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++;
         if (nochange_count > 1) {
           ECHO_LM(DB, "Stuck in Loop.. Exiting");
           t1_done = true;
@@ -2295,8 +2394,8 @@ static void axis_unhomed_error() {
         temp = (bed_level_oy - target) / 2;
         adj_target = target + temp;
         if (((bed_level_oy < adj_target) and (adj_t2_Radius > 0)) or ((bed_level_oy > adj_target) and (adj_t2_Radius < 0))) adj_t2_Radius = -(adj_t2_Radius / 2);
-        if (Equal_AB(bed_level_oy, adj_target)) t2_done = true;
-        if (Equal_AB(bed_level_oy, prev_bed_level) and Equal_AB(adj_target, prev_target)) nochange_count ++;
+        if (Equal_AB(bed_level_oy, adj_target, ac_prec / 2)) t2_done = true;
+        if (Equal_AB(bed_level_oy, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++;
         if (nochange_count > 1) {
           ECHO_LM(DB, "Stuck in Loop.. Exiting");
           t2_done = true;
@@ -2322,8 +2421,8 @@ static void axis_unhomed_error() {
         temp = (bed_level_oz - target) / 2;
         adj_target = target + temp;
         if (((bed_level_oz < adj_target) and (adj_t3_Radius > 0)) or ((bed_level_oz > adj_target) and (adj_t3_Radius < 0))) adj_t3_Radius = -(adj_t3_Radius / 2);
-        if (Equal_AB(bed_level_oz, adj_target)) t3_done = true;
-        if (Equal_AB(bed_level_oz, prev_bed_level) and Equal_AB(adj_target, prev_target)) nochange_count ++;
+        if (Equal_AB(bed_level_oz, adj_target, ac_prec / 2)) t3_done = true;
+        if (Equal_AB(bed_level_oz, prev_bed_level, ac_prec / 2) and Equal_AB(adj_target, prev_target, ac_prec / 2)) nochange_count ++;
         if (nochange_count > 1) {
           ECHO_LM(DB, "Stuck in Loop.. Exiting");
           t3_done = true;
@@ -2336,7 +2435,7 @@ static void axis_unhomed_error() {
     } while ((t1_done == false) or (t2_done == false) or (t3_done == false));
   }
 
-  void adj_tower_delta(int tower) {
+  static void adj_tower_delta(int tower) {
     float adj_val = 0;
     float adj_mag = 0.2;
     float adj_prv;
@@ -2436,43 +2535,7 @@ static void axis_unhomed_error() {
     return (delta_diagonal_rod - prev_diag_rod);
   }
 
-  float z_probe() {
-    feedrate = AUTOCAL_TRAVELRATE * 60;
-    prepare_move(true);
-    st_synchronize();
-
-    enable_endstops(true);
-    float start_z = current_position[Z_AXIS];
-    long start_steps = st_get_position(Z_AXIS);
-
-    feedrate = AUTOCAL_PROBERATE * 60;
-    destination[Z_AXIS] = -20;
-    prepare_move_raw();
-    st_synchronize();
-    endstops_hit_on_purpose();
-
-    enable_endstops(false);
-    long stop_steps = st_get_position(Z_AXIS);
-
-    /*
-    if (DEBUGGING(INFO)) {
-      ECHO_LMV(INFO, "start_z = ", start_z);
-      ECHO_LMV(INFO, "start_steps = ", start_steps);
-      ECHO_LMV(INFO, "stop_steps = ", stop_steps);
-    }
-    */
-
-    float mm = start_z - float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
-    current_position[Z_AXIS] = mm;
-    sync_plan_position_delta();
-
-    destination[Z_AXIS] = mm + Z_RAISE_BETWEEN_PROBINGS;
-    prepare_move_raw();
-    st_synchronize();
-    return mm;
-  }
-
-  void calibrate_print_surface(float z_offset) {
+  static void calibrate_print_surface() {
     float probe_bed_z, probe_z, probe_h, probe_l;
     int probe_count, auto_bed_leveling_grid_points = AUTO_BED_LEVELING_GRID_POINTS;
 
@@ -2527,54 +2590,7 @@ static void axis_unhomed_error() {
     print_bed_level();
   }
 
-  float probe_bed(float x, float y) {
-    //Probe bed at specified location and return z height of bed
-    uint8_t probe_count = PROBE_COUNT;
-    float probe_z, probe_bed_array[probe_count], probe_bed_mean = 0;
-
-    destination[X_AXIS] = x - z_probe_offset[X_AXIS];
-    NOLESS(destination[X_AXIS], X_MIN_POS);
-    NOMORE(destination[X_AXIS], X_MAX_POS);
-    destination[Y_AXIS] = y - z_probe_offset[Y_AXIS];
-    NOLESS(destination[Y_AXIS], Y_MIN_POS);
-    NOMORE(destination[Y_AXIS], Y_MAX_POS);
-
-    for(int i = 0; i < probe_count; i++) {
-      probe_bed_array[i] = z_probe() + z_probe_offset[Z_AXIS];
-      probe_bed_mean += probe_bed_array[i];
-    }
-
-    probe_z = probe_bed_mean / probe_count;
-
-    if (DEBUGGING(INFO)) {
-      ECHO_SM(INFO, "Bed probe heights: ");
-      for(int i = 0; i < probe_count; i++) {
-        if (probe_bed_array[i] >= 0) ECHO_M(" ");
-        ECHO_VM(probe_bed_array[i], " ", 4);
-      }
-      ECHO_M("mean");
-      if (probe_z >= 0) ECHO_M(" ");
-      ECHO_EV(probe_z, 4);
-    }
-
-    return probe_z;
-  }
-
-  void bed_probe_all() {
-    // Initial throwaway probe.. used to stabilize probe
-    bed_level_c = probe_bed(0.0, 0.0);
-
-    // Probe all bed positions & store carriage positions
-    bed_level_z = probe_bed(0.0, bed_radius);
-    bed_level_oy = probe_bed(-SIN_60 * bed_radius, COS_60 * bed_radius);
-    bed_level_x = probe_bed(-SIN_60 * bed_radius, -COS_60 * bed_radius);
-    bed_level_oz = probe_bed(0.0, -bed_radius);
-    bed_level_y = probe_bed(SIN_60 * bed_radius, -COS_60 * bed_radius);
-    bed_level_ox = probe_bed(SIN_60 * bed_radius, COS_60 * bed_radius);
-    bed_level_c = probe_bed(0.0, 0.0);
-  }
-
-  void calibration_report() {
+  static void calibration_report() {
     // Display Report
     ECHO_LM(DB, "|\tZ-Tower\t\t\tEndstop Offsets");
 
@@ -2619,13 +2635,13 @@ static void axis_unhomed_error() {
     ECHO_E;
   }
 
-  void home_delta_axis() {
+  static void home_delta_axis() {
     saved_feedrate = feedrate;
     saved_feedrate_multiplier = feedrate_multiplier;
     feedrate_multiplier = 100;
     refresh_cmd_timeout();
 
-    enable_endstops(true);
+    endstops.enable();
 
     set_destination_to_current();
 
@@ -2640,7 +2656,7 @@ static void axis_unhomed_error() {
     feedrate = 1.732 * homing_feedrate[X_AXIS];
     line_to_destination();
     st_synchronize();
-    endstops_hit_on_purpose(); // clear endstop hit flags
+    endstops.hit_on_purpose(); // clear endstop hit flags
 
     // Destination reached
     set_current_to_destination();
@@ -2653,16 +2669,16 @@ static void axis_unhomed_error() {
     sync_plan_position_delta();
 
     #if ENABLED(ENDSTOPS_ONLY_FOR_HOMING)
-      enable_endstops(false);
+      endstops.enable(false);
     #endif
 
     feedrate = saved_feedrate;
     feedrate_multiplier = saved_feedrate_multiplier;
     refresh_cmd_timeout();
-    endstops_hit_on_purpose(); // clear endstop hit flags
+    endstops.hit_on_purpose(); // clear endstop hit flags
   }
 
-  void prepare_move_raw() {
+  static void prepare_move_raw() {
     if (DEBUGGING(INFO))
       DEBUG_POS("prepare_move_raw", destination);
 
@@ -2688,7 +2704,7 @@ static void axis_unhomed_error() {
   }
 
   // Adjust print surface height by linear interpolation over the bed_level array.
-  void adjust_delta(float cartesian[3]) {
+  static void adjust_delta(float cartesian[3]) {
     if (delta_grid_spacing[0] == 0 || delta_grid_spacing[1] == 0) return; // G29 not done!
 
     int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
@@ -2798,12 +2814,12 @@ static void axis_unhomed_error() {
     if (retracting) {
       feedrate = retract_feedrate * 60;
       current_position[E_AXIS] += (swapping ? retract_length_swap : retract_length) / volumetric_multiplier[active_extruder];
-      plan_set_e_position(current_position[E_AXIS]);
+      sync_plan_position_e();
       prepare_move();
 
       if (retract_zlift > 0.01) {
         current_position[Z_AXIS] -= retract_zlift;
-        #if MECH(DELTA) || MECH(SCARA)
+        #if MECH(DELTA)
           sync_plan_position_delta();
         #else
           sync_plan_position();
@@ -2814,12 +2830,11 @@ static void axis_unhomed_error() {
     else {
       if (retract_zlift > 0.01) {
         current_position[Z_AXIS] += retract_zlift;
-        #if MECH(DELTA) || MECH(SCARA)
+        #if MECH(DELTA)
           sync_plan_position_delta();
         #else
           sync_plan_position();
         #endif
-        //prepare_move();
       }
 
       feedrate = retract_recover_feedrate * 60;
@@ -2832,8 +2847,8 @@ static void axis_unhomed_error() {
     feedrate = oldFeedrate;
     retracted[active_extruder] = retracting;
 
-  } // retract()
-#endif //FWRETRACT
+  }
+#endif // FWRETRACT
 
 #if HAS(Z_PROBE_SLED)
 
@@ -2931,18 +2946,10 @@ static void axis_unhomed_error() {
   }
 #endif
 
-inline void wait_heater() {
-  bool wants_to_cool = isCoolingHotend(target_extruder);
-
-  // Exit if S<lower>, continue if S<higher>, R<lower>, or R<higher>
-  if (no_wait_for_cooling && wants_to_cool) return;
-
-  // Prevents a wait-forever situation if R is misused i.e. M109 R0
-  // Try to calculate a ballpark safe margin by halving EXTRUDE_MINTEMP
-  if (wants_to_cool && degTargetHotend(target_extruder) < (EXTRUDE_MINTEMP)/2) return;
+inline void wait_heater(bool no_wait_for_cooling = true) {
 
   #if ENABLED(TEMP_RESIDENCY_TIME)
-    long residency_start_ms = -1;
+    millis_t residency_start_ms = -1;
     // Loop until the temperature has stabilized
     #define TEMP_CONDITIONS (!residency_start_ms || PENDING(now, residency_start_ms + (TEMP_RESIDENCY_TIME) * 1000UL))
   #else
@@ -2950,13 +2957,18 @@ inline void wait_heater() {
     #define TEMP_CONDITIONS (wants_to_cool ? isCoolingHotend(target_extruder) : isHeatingHotend(target_extruder))
   #endif // TEMP_RESIDENCY_TIME
 
+  float theTarget = -1;
+  bool wants_to_cool;
   cancel_heatup = false;
   millis_t now, next_temp_ms = 0;
+
+  KEEPALIVE_STATE(NOT_BUSY);
+
   do {
     now = millis();
     if (ELAPSED(now, next_temp_ms)) { //Print temp & remaining time every 1s while waiting
       next_temp_ms = now + 1000UL;
-      #if HAS(TEMP_0) || HAS(TEMP_BED) || ENABLED(HEATER_0_USES_MAX6675)
+      #if HAS(TEMP_HOTEND) || HAS(TEMP_BED)
         print_heaterstates();
       #endif
       #if TEMP_RESIDENCY_TIME > 0
@@ -2973,11 +2985,24 @@ inline void wait_heater() {
       #endif
     }
 
+    // Target temperature might be changed during the loop
+    if (theTarget != degTargetHotend(target_extruder)) {
+      wants_to_cool = isCoolingHotend(target_extruder);
+      theTarget = degTargetHotend(target_extruder);
+
+      // Exit if S<lower>, continue if S<higher>, R<lower>, or R<higher>
+      if (no_wait_for_cooling && wants_to_cool) break;
+
+      // Prevent a wait-forever situation if R is misused i.e. M109 R0
+      // Try to calculate a ballpark safe margin by halving EXTRUDE_MINTEMP
+      if (wants_to_cool && theTarget < (EXTRUDE_MINTEMP) / 2) break;
+    }
+
     idle();
     refresh_cmd_timeout(); // to prevent stepper_inactive_time from running out
 
     #if TEMP_RESIDENCY_TIME > 0
-      float temp_diff = fabs(degTargetHotend(target_extruder) - degHotend(target_extruder));
+      float temp_diff = fabs(theTarget - degHotend(target_extruder));
 
       if (!residency_start_ms) {
         // Start the TEMP_RESIDENCY_TIME timer when we reach target temp for the first time.
@@ -2992,13 +3017,10 @@ inline void wait_heater() {
   } while(!cancel_heatup && TEMP_CONDITIONS);
 
   LCD_MESSAGEPGM(MSG_HEATING_COMPLETE);
+  KEEPALIVE_STATE(IN_HANDLER);
 }
 
-inline void wait_bed() {
-  bool wants_to_cool = isCoolingBed();
-
-  // Exit if the temperature is above target and not waiting for cooling
-  if (no_wait_for_cooling && wants_to_cool) return;
+inline void wait_bed(bool no_wait_for_cooling = true) {
 
   #if TEMP_BED_RESIDENCY_TIME > 0
     millis_t residency_start_ms = 0;
@@ -3009,9 +3031,13 @@ inline void wait_bed() {
     #define TEMP_BED_CONDITIONS (wants_to_cool ? isCoolingBed() : isHeatingBed())
   #endif // TEMP_BED_RESIDENCY_TIME > 0
 
+  float theTarget = -1;
+  bool wants_to_cool;
   cancel_heatup = false;
   millis_t now, next_temp_ms = 0;
 
+  KEEPALIVE_STATE(NOT_BUSY);
+
   // Wait for temperature to come close enough
   do {
     now = millis();
@@ -3032,11 +3058,24 @@ inline void wait_bed() {
       #endif
     }
 
+    // Target temperature might be changed during the loop
+    if (theTarget != degTargetBed()) {
+      wants_to_cool = isCoolingBed();
+      theTarget = degTargetBed();
+
+      // Exit if S<lower>, continue if S<higher>, R<lower>, or R<higher>
+      if (no_wait_for_cooling && wants_to_cool) break;
+
+      // Prevent a wait-forever situation if R is misused i.e. M190 R0
+      // Simply don't wait to cool a bed under 30C
+      if (wants_to_cool && theTarget < 30) break;
+    }
+
     idle();
     refresh_cmd_timeout(); // to prevent stepper_inactive_time from running out
 
     #if TEMP_BED_RESIDENCY_TIME > 0
-      float temp_diff = fabs(degBed() - degTargetBed());
+      float temp_diff = fabs(theTarget - degTargetBed());
 
       if (!residency_start_ms) {
         // Start the TEMP_BED_RESIDENCY_TIME timer when we reach target temp for the first time.
@@ -3050,6 +3089,7 @@ inline void wait_bed() {
 
   } while (!cancel_heatup && TEMP_BED_CONDITIONS);
   LCD_MESSAGEPGM(MSG_BED_DONE);
+  KEEPALIVE_STATE(IN_HANDLER);
 }
 
 
@@ -3485,7 +3525,7 @@ inline void gcode_G28() {
     feedrate = 1.732 * homing_feedrate[X_AXIS];
     line_to_destination();
     st_synchronize();
-    endstops_hit_on_purpose(); // clear endstop hit flags
+    endstops.hit_on_purpose(); // clear endstop hit flags
 
     // Destination reached
     for (int i = X_AXIS; i <= Z_AXIS; i++) current_position[i] = destination[i];
@@ -3564,7 +3604,7 @@ inline void gcode_G28() {
         line_to_destination();
         feedrate = 0.0;
         st_synchronize();
-        endstops_hit_on_purpose(); // clear endstop hit flags
+        endstops.hit_on_purpose(); // clear endstop hit flags
 
         current_position[X_AXIS] = destination[X_AXIS];
         current_position[Y_AXIS] = destination[Y_AXIS];
@@ -3633,7 +3673,7 @@ inline void gcode_G28() {
             feedrate_multiplier = 100;
             refresh_cmd_timeout();
 
-            enable_endstops(true);
+            endstops.enable();
             for(uint8_t i = 0; i < NUM_AXIS; i++) {
               destination[i] = current_position[i];
             }
@@ -3649,13 +3689,13 @@ inline void gcode_G28() {
             sync_plan_position();
 
             #if ENABLED(ENDSTOPS_ONLY_FOR_HOMING)
-              enable_endstops(false);
+              endstops.enable(false);
             #endif
 
             feedrate = saved_feedrate;
             feedrate_multiplier = saved_feedrate_multiplier;
             refresh_cmd_timeout();
-            endstops_hit_on_purpose(); // clear endstop hit flags
+            endstops.hit_on_purpose(); // clear endstop hit flags
 
             sync_plan_position();
 
@@ -4336,7 +4376,8 @@ inline void gcode_G28() {
       home_delta_axis();
 
     deploy_z_probe();
-    calibrate_print_surface(z_probe_offset[Z_AXIS] + (code_seen(axis_codes[Z_AXIS]) ? code_value() : 0.0));
+    bed_safe_z = current_position[Z_AXIS];
+    calibrate_print_surface();
     retract_z_probe();
     clean_up_after_endstop_move();
 
@@ -4371,6 +4412,7 @@ inline void gcode_G28() {
     if (!axis_homed[X_AXIS] || !axis_homed[Y_AXIS] || !axis_homed[Z_AXIS])
       home_delta_axis();
     deploy_z_probe();
+    bed_safe_z = current_position[Z_AXIS];
 
     if (code_seen('X') and code_seen('Y')) {
       // Probe specified X,Y point
@@ -5088,7 +5130,7 @@ inline void gcode_M42() {
 
     if (deploy_probe_for_each_reading) stow_z_probe();
 
-    for (uint8_t n=0; n < n_samples; n++) {
+    for (uint8_t n = 0; n < n_samples; n++) {
       // 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
 
@@ -5580,7 +5622,7 @@ inline void gcode_M92() {
 #endif
 
 /**
- * M104: Set hot end temperature
+ * M104: Set hotend temperature
  */
 inline void gcode_M104() {
   if (setTargetedExtruder(104)) return;
@@ -5659,9 +5701,7 @@ inline void gcode_M109() {
     if (target_extruder != active_extruder) return;
   #endif
 
-  LCD_MESSAGEPGM(MSG_HEATING);
-
-  no_wait_for_cooling = code_seen('S');
+  bool no_wait_for_cooling = code_seen('S');
   if (no_wait_for_cooling || code_seen('R')) {
     float temp = code_value();
     setTargetHotend(temp, target_extruder);
@@ -5675,7 +5715,7 @@ inline void gcode_M109() {
      * stand by mode, for instance in a dual extruder setup, without affecting
      * the running print timer.
      */
-    if (temp <= (EXTRUDE_MINTEMP)/2) {
+    if (temp <= (EXTRUDE_MINTEMP) / 2) {
       print_job_timer.stop();
       LCD_MESSAGEPGM(WELCOME_MSG);
     }
@@ -5696,7 +5736,7 @@ inline void gcode_M109() {
     if (code_seen('B')) autotemp_max = code_value();
   #endif
 
-  wait_heater();
+  wait_heater(no_wait_for_cooling);
 }
 
 /**
@@ -5774,50 +5814,17 @@ inline void gcode_M115() {
 /**
  * M119: Output endstop states to serial output
  */
-inline void gcode_M119() {
-  ECHO_LM(DB, SERIAL_M119_REPORT);
-  #if HAS(X_MIN)
-    ECHO_EMT(SERIAL_X_MIN, ((READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(X_MAX)
-    ECHO_EMT(SERIAL_X_MAX, ((READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(Y_MIN)
-    ECHO_EMT(SERIAL_Y_MIN, ((READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(Y_MAX)
-    ECHO_EMT(SERIAL_Y_MAX, ((READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(Z_MIN)
-    ECHO_EMT(SERIAL_Z_MIN, ((READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(Z_MAX)
-    ECHO_EMT(SERIAL_Z_MAX, ((READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(Z2_MAX)
-    ECHO_EMT(SERIAL_Z2_MAX, ((READ(Z2_MAX_PIN)^Z2_MAX_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(Z_PROBE)
-    ECHO_EMT(SERIAL_Z_PROBE, ((READ(Z_PROBE_PIN)^Z_PROBE_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(E_MIN)
-    ECHO_EMT(SERIAL_E_MIN, ((READ(E_MIN_PIN)^E_MIN_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  #if HAS(FILRUNOUT)
-    ECHO_EMT(SERIAL_FILRUNOUT_PIN, ((READ(FILRUNOUT_PIN)^FILRUNOUT_PIN_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
-  #endif
-  ECHO_E;
-}
+inline void gcode_M119() { endstops.M119(); }
 
 /**
- * M120: Enable endstops
+ * M120: Enable endstops and set non-homing endstop state to "enabled"
  */
-inline void gcode_M120() { enable_endstops(true); }
+inline void gcode_M120() { endstops.enable_globally(true); }
 
 /**
- * M121: Disable endstops
+ * M121: Disable endstops and set non-homing endstop state to "disabled"
  */
-inline void gcode_M121() { enable_endstops(false); }
+inline void gcode_M121() { endstops.enable_globally(false); }
 
 /**
  * M122: Disable or enable software endstops
@@ -6016,10 +6023,10 @@ inline void gcode_M140() {
     if (DEBUGGING(DRYRUN)) return;
 
     LCD_MESSAGEPGM(MSG_BED_HEATING);
-    no_wait_for_cooling = code_seen('S');
+    bool no_wait_for_cooling = code_seen('S');
     if (no_wait_for_cooling || code_seen('R')) setTargetBed(code_value());
 
-    wait_bed();
+    wait_bed(no_wait_for_cooling);
   }
 #endif // HAS(TEMP_BED)
 
@@ -6611,7 +6618,7 @@ inline void gcode_M226() {
    * M363: SCARA calibration: Move to cal-position PsiB (90 deg calibration - steps per degree)
    */
   inline bool gcode_M363() {
-    ECHO_LM(DB,"Cal: Psi 90 ");
+    ECHO_LM(DB, "Cal: Psi 90 ");
     return SCARA_move_to_cal(50, 90);
   }
 
@@ -7281,11 +7288,9 @@ inline void gcode_M503() {
       enable_all_steppers(); // Enable all stepper
       for(uint8_t e = 0; e < HOTENDS; e++) {
         setTargetHotend(old_target_temperature[e], e);
-        no_wait_for_cooling = true;
         wait_heater();
       }
       setTargetBed(old_target_temperature_bed);
-      no_wait_for_cooling = true;
       wait_bed();
       sleep = false;
       beep = true;
@@ -7530,6 +7535,17 @@ inline void gcode_M503() {
   }
 #endif
 
+#if ENABLED(ADVANCE_LPC)
+  /**
+   * M905: Set advance factor
+   */
+  inline void gcode_M905() {
+    st_synchronize();
+    if (code_seen('K')) extruder_advance_k = code_value();
+    ECHO_LMV(DB, "Advance factor = ", extruder_advance_k);
+  }
+#endif
+
 #if MB(ALLIGATOR)
   /**
    * M906: Set motor currents
@@ -7618,6 +7634,7 @@ inline void gcode_M999() {
  */
 inline void gcode_T(uint8_t tmp_extruder) {
   bool good_extruder = false;
+  float stored_feedrate = feedrate;
 
   #if ENABLED(COLOR_MIXING_EXTRUDER) && MIXING_VIRTUAL_TOOLS > 1
 
@@ -7636,21 +7653,18 @@ inline void gcode_T(uint8_t tmp_extruder) {
       good_extruder = true;
       target_extruder = tmp_extruder;
 
-      #if ENABLED(DONDOLO)
-        bool make_move = true;
-      #else
-        bool make_move = false;
-      #endif
-
       if (code_seen('F')) {
-
-        #if EXTRUDERS > 1
-          make_move = true;
-        #endif
-
         float next_feedrate = code_value();
-        if (next_feedrate > 0.0) feedrate = next_feedrate;
+        if (next_feedrate > 0.0) stored_feedrate = feedrate = next_feedrate;
+      }
+      else {
+        #if ENABLED(XY_TRAVEL_SPEED)
+          feedrate = XY_TRAVEL_SPEED;
+        #else
+          feedrate = min(max_feedrate[X_AXIS], max_feedrate[Y_AXIS]);
+        #endif
       }
+
       #if EXTRUDERS > 1
         #if ENABLED(NPR2)
           if(target_extruder != old_color)
@@ -7892,7 +7906,7 @@ inline void gcode_T(uint8_t tmp_extruder) {
             sync_plan_position();
           #endif // !DELTA
           // Move to the old position if 'F' was in the parameters
-          if (make_move && IsRunning()) prepare_move();
+          if (IsRunning()) prepare_move();
         }
 
         #if ENABLED(EXT_SOLENOID)
@@ -7905,6 +7919,8 @@ inline void gcode_T(uint8_t tmp_extruder) {
     }
   #endif // !COLOR_MIXING_EXTRUDER
 
+  feedrate = stored_feedrate;
+
   if (!good_extruder) {
     ECHO_SMV(DB, "T", (int)tmp_extruder);
     ECHO_EM(" " SERIAL_INVALID_EXTRUDER);
@@ -8099,12 +8115,12 @@ void process_next_command() {
         case 48: // M48 Z-Probe repeatability
           gcode_M48(); break;
       #endif
-      
+
       #if HAS(POWER_CONSUMPTION_SENSOR)
         case 70: // M70 - Power consumption sensor calibration
           gcode_M70(); break;
       #endif
-      
+
       case 75: // Start print timer
         gcode_M75(); break;
 
@@ -8178,14 +8194,14 @@ void process_next_command() {
       case 112: //  M112 Emergency Stop
         gcode_M112(); break;
 
-      case 114: // M114 Report current position
-        gcode_M114(); break;
-
       #if ENABLED(HOST_KEEPALIVE_FEATURE)
         case 113: // M113: Set Host Keepalive interval
           gcode_M113(); break;
       #endif
 
+      case 114: // M114 Report current position
+        gcode_M114(); break;
+
       case 115: // M115 Report capabilities
         gcode_M115(); break;
 
@@ -8402,7 +8418,7 @@ void process_next_command() {
       #endif
 
       #if ENABLED(FILAMENTCHANGEENABLE)
-        case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
+        case 600: // Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
           gcode_M600(); break;
       #endif
 
@@ -8431,6 +8447,11 @@ void process_next_command() {
           gcode_M666(); break;
       #endif
 
+      #if ENABLED(ADVANCE_LPC)
+        case 905: // M905 Set advance factor.
+          gcode_M905(); break;
+      #endif
+
       #if MB(ALLIGATOR)
         case 906: // M906 Set motor currents XYZ T0-4 E
           gcode_M906(); break;
@@ -8568,7 +8589,7 @@ static void report_current_position() {
 
 #if MECH(DELTA) || MECH(SCARA)
 
-  inline bool prepare_move_delta(float target[NUM_AXIS], const bool delta_probe) {
+  inline bool prepare_move_delta(float target[NUM_AXIS]) {
     float difference[NUM_AXIS];
     float addDistance[NUM_AXIS];
     float fractions[NUM_AXIS];
@@ -8622,15 +8643,11 @@ static void report_current_position() {
       #endif
 
       calculate_delta(target);
-      if (!delta_probe) adjust_delta(target);
+      adjust_delta(target);
 
       if (DEBUGGING(DEBUG)) {
-        ECHO_LMV(DEB, "target[X_AXIS]=", target[X_AXIS]);
-        ECHO_LMV(DEB, "target[Y_AXIS]=", target[Y_AXIS]);
-        ECHO_LMV(DEB, "target[Z_AXIS]=", target[Z_AXIS]);
-        ECHO_LMV(DEB, "delta[TOWER_1]=", delta[TOWER_1]);
-        ECHO_LMV(DEB, "delta[TOWER_2]=", delta[TOWER_2]);
-        ECHO_LMV(DEB, "delta[TOWER_3]=", delta[TOWER_3]);
+        DEBUG_POS("prepare_move_delta", target);
+        DEBUG_POS("prepare_move_delta", delta);
       }
 
       plan_buffer_line(delta[TOWER_1], delta[TOWER_2], delta[TOWER_3], target[E_AXIS], frfm, active_extruder, active_driver);
@@ -8641,7 +8658,7 @@ static void report_current_position() {
 #endif // DELTA || SCARA
 
 #if MECH(SCARA)
-  inline bool prepare_move_scara(float target[NUM_AXIS]) { return prepare_move_delta(target, false); }
+  inline bool prepare_move_scara(float target[NUM_AXIS]) { return prepare_move_delta(target); }
 #endif
 
 #if ENABLED(DUAL_X_CARRIAGE)
@@ -8715,11 +8732,7 @@ static void report_current_position() {
  * (This may call plan_buffer_line several times to put
  *  smaller moves into the planner for DELTA or SCARA.)
  */
-void prepare_move(
-  #if MECH(DELTA)
-    const bool delta_probe /*= false*/
-  #endif
-) {
+void prepare_move() {
   clamp_to_software_endstops(destination);
   refresh_cmd_timeout();
 
@@ -8730,14 +8743,11 @@ void prepare_move(
   #if MECH(SCARA)
     if (!prepare_move_scara(destination)) return;
   #elif MECH(DELTA)
-    if (!prepare_move_delta(destination, delta_probe)) return;
-  #endif
-
-  #if ENABLED(DUAL_X_CARRIAGE)
-    if (!prepare_move_dual_x_carriage()) return;
-  #endif
-
-  #if MECH(CARTESIAN) || MECH(COREXY) || MECH(COREYX) || MECH(COREXZ) || MECH(COREZX)
+    if (!prepare_move_delta(destination)) return;
+  #else
+    #if ENABLED(DUAL_X_CARRIAGE)
+      if (!prepare_move_dual_x_carriage()) return;
+    #endif
     if (!prepare_move_cartesian()) return;
   #endif
 

MK/module/MK_Main.h

@@ -36,23 +36,26 @@ void idle(
 
 void manage_inactivity(bool ignore_stepper_queue = false);
 
+#if ENABLED(DUAL_X_CARRIAGE)
+  extern bool extruder_duplication_enabled;
+#endif
+
 void FlushSerialRequestResend();
 void ok_to_send();
 
 #if MECH(DELTA)
-  float probe_bed(float x, float y);
-  void set_delta_constants();
-  void adj_tower_delta(int tower);
-  void adj_tower_radius(int tower);
-  void home_delta_axis();
-  void calibration_report();
-  void bed_probe_all();
   void set_delta_constants();
   void calculate_delta(float cartesian[3]);
-  void adjust_delta(float cartesian[3]);
-  void adj_endstops();
-  void reset_bed_level();
-  void prepare_move_raw();
+  static float probe_bed(float x, float y);
+  static void adj_tower_delta(int tower);
+  static void adj_tower_radius(int tower);
+  static void home_delta_axis();
+  static void calibration_report();
+  static void bed_probe_all();
+  static void adjust_delta(float cartesian[3]);
+  static void adj_endstops();
+  static void reset_bed_level();
+  static void prepare_move_raw();
   extern float delta[3];
   extern float delta_tmp[3];
   extern float delta_tower1_x, delta_tower1_y;
@@ -71,11 +74,7 @@ void ok_to_send();
   void calculate_SCARA_forward_Transform(float f_scara[3]);
 #endif
 
-void prepare_move(
-  #if MECH(DELTA)
-    const bool delta_probe = false
-  #endif
-);
+void prepare_move();
 
 void kill(const char *);
 void Stop();
@@ -113,7 +112,7 @@ void prepare_arc_move(char isclockwise);
 void clamp_to_software_endstops(float target[3]);
 
 extern millis_t previous_cmd_ms;
-void refresh_cmd_timeout();
+inline void refresh_cmd_timeout() { previous_cmd_ms = millis(); }
 
 extern void delay_ms(millis_t ms);
 
@@ -140,6 +139,10 @@ extern bool axis_known_position[3];
 extern bool axis_homed[3];
 extern float zprobe_zoffset;
 
+#if ENABLED(ADVANCE_LPC)
+  extern int extruder_advance_k;
+#endif
+
 #if HEATER_USES_AD595
   extern float ad595_offset[HOTENDS];
   extern float ad595_gain[HOTENDS];

MK/module/conditionals.h

@@ -368,6 +368,17 @@
   #define Y_MAX_LENGTH (Y_MAX_POS - (Y_MIN_POS))
   #define Z_MAX_LENGTH (Z_MAX_POS - (Z_MIN_POS))
 
+  /**
+   * CoreXY or CoreYX or CoreXZ or CoreZX
+   */
+  #if MECH(COREXY) || MECH(COREYX)
+    #define CORE_AXIS_2 B_AXIS
+    #define CORE_AXIS_3 Z_AXIS
+  #elif MECH(COREXZ) || MECH(COREZX)
+    #define CORE_AXIS_2 C_AXIS
+    #define CORE_AXIS_3 Y_AXIS
+  #endif
+
   /**
    * SCARA
    */
@@ -505,7 +516,7 @@
    */
   #if ENABLED(ADVANCE)
     #define EXTRUSION_AREA (0.25 * (D_FILAMENT) * (D_FILAMENT) * M_PI)
-    #define STEPS_PER_CUBIC_MM_E (axis_steps_per_unit[E_AXIS + active_extruder] / EXTRUSION_AREA)
+    #define STEPS_PER_CUBIC_MM_E (axis_steps_per_unit[E_AXIS + active_extruder] / (EXTRUSION_AREA))
   #endif
 
   /**
@@ -754,6 +765,10 @@
 
   #define HAS_DIGIPOTSS (PIN_EXISTS(DIGIPOTSS))
 
+  #define HAS_TEMP_HOTEND (HAS_TEMP_0 || ENABLED(HEATER_0_USES_MAX6675))
+
+  #define HAS_THERMALLY_PROTECTED_BED (HAS_TEMP_BED && HAS_HEATER_BED && ENABLED(THERMAL_PROTECTION_BED))
+
   /**
    * Shorthand for filament sensor and power sensor for ultralcd.cpp, dogm_lcd_implementation.h, ultralcd_implementation_hitachi_HD44780.h
    */

MK/module/language/language.h

@@ -23,32 +23,9 @@
 #ifndef LANGUAGE_H
 #define LANGUAGE_H
 
-// NOTE: IF YOU CHANGE LANGUAGE FILES OR MERGE A FILE WITH CHANGES
-//
-//   ==> ALWAYS TRY TO COMPILE MARLIN WITH/WITHOUT "ULTIPANEL" / "ULTRALCD" / "SDSUPPORT" #define IN "Configuration_Basic.h"
-//   ==> ALSO TRY ALL AVAILABLE "LANGUAGE_CHOICE" OPTIONS
-// See also documentation/LCDLanguageFont.md
-
-// Languages
-// 1  English  // Language base
-// 2  Polish
-// 3  French
-// 4  German
-// 5  Spanish
-// 6  Russian
-// 7  Italian
-// 8  Portuguese
-// 9  Finnish
-// 10 Aragonese
-// 11 Dutch
-// 12 Danish
-// 13 Catalan
-// 14 Basque-Euskera
-// 15 Portuguese (Brazil)
-// 16 Bulgarian
-// 17 Japanese
-// 18 Japanese utf
-// 19 Chinese
+#ifndef LCD_LANGUAGE
+  #define LCD_LANGUAGE en
+#endif
 
 #define PROTOCOL_VERSION "2.0"
 
@@ -223,7 +200,7 @@
 #define SERIAL_HEATER_BED                       "bed"
 #define SERIAL_STOPPED_HEATER                   ", system stopped! Heater_ID: "
 #define SERIAL_STOPPED_COOLER                   "system stopped! Cooler"
-//#define SERIAL_REDUNDANCY                     "Heater switched off. Temperature difference between temp sensors is too high !"
+#define SERIAL_REDUNDANCY                       "Heater switched off. Temperature difference between temp sensors is too high !"
 #define SERIAL_T_HEATING_FAILED                 "Heating failed"
 #define SERIAL_T_THERMAL_RUNAWAY                "Thermal Runaway"
 #define SERIAL_T_MAXTEMP                        "MAXTEMP triggered"
@@ -253,45 +230,13 @@
 #define SERIAL_BED_LEVELLING_Y                  " Y: "
 #define SERIAL_BED_LEVELLING_Z                  " Z: "
 
-#if   LANGUAGE_CHOICE == 1  // English
-  #include "language_en.h"
-#elif LANGUAGE_CHOICE == 2  // Polish
-  #include "language_pl.h"
-#elif LANGUAGE_CHOICE == 3  // French
-  #include "language_fr.h"
-#elif LANGUAGE_CHOICE == 4  // German
-  #include "language_de.h"
-#elif LANGUAGE_CHOICE == 5  // Spanish
-  #include "language_es.h"
-#elif LANGUAGE_CHOICE == 6  // Russian
-	#define MAPPER_D0D1       // For Cyrillic
-  #include "language_ru.h"
-#elif LANGUAGE_CHOICE == 7  // Italian
-  #include "language_it.h"
-#elif LANGUAGE_CHOICE == 8  // Portuguese
-  #include "language_pt.h"
-#elif LANGUAGE_CHOICE == 9  // Finnish
-  #include "language_fi.h"
-#elif LANGUAGE_CHOICE == 10 // Aragonese
-  #include "language_an.h"
-#elif LANGUAGE_CHOICE == 11 // Dutch
-  #include "language_nl.h"
-#elif LANGUAGE_CHOICE == 12 // Danish
-  #include "language_da.h"
-#elif LANGUAGE_CHOICE == 13 // Catalan
-  #include "language_ca.h"
-#elif LANGUAGE_CHOICE == 14 // Basque-Euskera
-  #include "language_eu.h"
-#elif LANGUAGE_CHOICE == 15 // Portuguese - Brasil
-  #include "language_pt-br.h"
-#elif LANGUAGE_CHOICE == 16 // Bulgarian
-  #include "language_bg.h"
-#elif LANGUAGE_CHOICE == 17 // Japanese
-  #include "language_kana.h"
-#elif LANGUAGE_CHOICE == 18 // Japanese utf
-  #include "language_kana_utf8.h"
-#elif LANGUAGE_CHOICE == 19 // Chinese
-  #include "language_cn.h"
-#endif
+// LCD Menu Messages
+
+#define LANGUAGE_INCL_(M) STRINGIFY_(language_##M.h)
+#define LANGUAGE_INCL(M) LANGUAGE_INCL_(M)
+#define INCLUDE_LANGUAGE LANGUAGE_INCL(LCD_LANGUAGE)
+
+#include "language_en.h"
+#include INCLUDE_LANGUAGE
 
 #endif //__LANGUAGE_H

MK/module/language/language_an.h

@@ -74,6 +74,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_bg.h

@@ -74,6 +74,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_ca.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_cn.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_da.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_de.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_en.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_es.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_eu.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_fi.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_fr.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_it.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"
@@ -151,13 +152,13 @@
 #define MSG_BABYSTEP_Z                      MSG_BABYSTEP " " MSG_Z
 #define MSG_ENDSTOP_ABORT                   "Finecorsa abort."
 #define MSG_HEATING_FAILED_LCD              "Riscaldamento fallito"
-#define MSG_ERR_REDUNDANT_TEMP              "REDUNDANT TEMP ERROR"
-#define MSG_THERMAL_RUNAWAY                 "THERMAL RUNAWAY"
+#define MSG_ERR_REDUNDANT_TEMP              "Err: TEMP RIDONDANTI"
+#define MSG_THERMAL_RUNAWAY                 "TEMP FUORI CONTROLLO"
 #define MSG_AD595                           "AD595 Offset & Gain"
-#define MSG_ERR_MAXTEMP                     "MAXTEMP ERROR"
-#define MSG_ERR_MINTEMP                     "MINTEMP ERROR"
-#define MSG_ERR_MAXTEMP_BED                 "MAXTEMP BED ERROR"
-#define MSG_ERR_MINTEMP_BED                 "MINTEMP BED ERROR"
+#define MSG_ERR_MAXTEMP                     "Err: TEMP MASSIMA"
+#define MSG_ERR_MINTEMP                     "Err: TEMP MINIMA"
+#define MSG_ERR_MAXTEMP_BED                 "Err: TEMP MASSIMA PIATTO"
+#define MSG_ERR_MINTEMP_BED                 "Err: TEMP MINIMA PIATTO"
 #define MSG_END_DAY                         "giorni"
 #define MSG_END_HOUR                        "ore"
 #define MSG_END_MINUTE                      "minuti"

MK/module/language/language_kana.h

@@ -75,6 +75,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_kana_utf8.h

@@ -78,6 +78,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_nl.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_pl.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_pt-br.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_pt.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/language/language_ru.h

@@ -73,6 +73,7 @@
 #define MSG_PID_P                           "PID-P"
 #define MSG_PID_I                           "PID-I"
 #define MSG_PID_D                           "PID-D"
+#define MSG_PID_C                           "PID-C"
 #define MSG_H1                              " H1"
 #define MSG_H2                              " H2"
 #define MSG_H3                              " H3"

MK/module/lcd/ultralcd.cpp

@@ -2723,10 +2723,17 @@ char* ftostr32sp(const float& x) {
   return conv;
 }
 
-// Convert int to lj string with +123.0 format
-char* itostr31(const int& x) {
-  conv[0] = x >= 0 ? '+' : '-';
-  int xx = abs(x);
+// Convert signed int to lj string with +012 / -012 format
+char* itostr3sign(const int& x) {
+  int xx;
+  if (x >= 0) {
+    conv[0] = '+';
+    xx = x;
+  }
+  else {
+    conv[0] = '-';
+    xx = -x;
+  }
   conv[1] = (xx / 100) % 10 + '0';
   conv[2] = (xx / 10) % 10 + '0';
   conv[3] = xx % 10 + '0';

MK/module/lcd/ultralcd.h

@@ -163,7 +163,7 @@
   #endif // NEWPANEL
 
   char* itostr2(const uint8_t& x);
-  char* itostr31(const int& xx);
+  char* itostr3sign(const int& x);
   char* itostr3(const int& xx);
   char* itostr3left(const int& xx);
   char* itostr4(const int& xx);
@@ -195,8 +195,8 @@
   FORCE_INLINE void lcd_reset_alert_level() {}
   FORCE_INLINE bool lcd_detected(void) { return true; }
 
-  #define LCD_MESSAGEPGM(x) do{}while(0)
-  #define LCD_ALERTMESSAGEPGM(x) do{}while(0)
+  #define LCD_MESSAGEPGM(x) NOOP
+  #define LCD_ALERTMESSAGEPGM(x) NOOP
 
 #endif //ULTRA_LCD
 

MK/module/lcd/ultralcd_implementation_hitachi_HD44780.h

@@ -474,7 +474,7 @@ char lcd_printPGM(const char* str) {
   return n;
 }
 
-char lcd_print(char* str) {
+char lcd_print(const char* str) {
   char c, n = 0;
   unsigned char i = 0;
   while ((c = str[i++])) n += charset_mapper(c);

MK/module/macros.h

@@ -76,4 +76,6 @@
 
 #define NOOP do{}while(0)
 
+#define _AXIS(AXIS) AXIS ##_AXIS
+
 #endif //__MACROS_H

MK/module/motion/endstops.cpp

+/**
+ * MK & MK4due 3D Printer Firmware
+ *
+ * Based on Marlin, Sprinter and grbl
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * endstops.cpp - A singleton object to manage endstops
+ */
+
+#include "../../base.h"
+#include "endstops.h"
+
+// TEST_ENDSTOP: test the old and the current status of an endstop
+#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits & old_endstop_bits, ENDSTOP))
+
+Endstops endstops;
+
+Endstops::Endstops() {
+  enable_globally(
+    #if ENABLED(ENDSTOPS_ONLY_FOR_HOMING)
+      true
+    #else
+      false
+    #endif
+  );
+  enable(true);
+  #if ENABLED(HAS_Z_PROBE)
+    enable_z_probe(false);
+  #endif
+} // Endstops::Endstops
+
+void Endstops::init() {
+
+  #if HAS(X_MIN)
+    SET_INPUT(X_MIN_PIN);
+    #if ENABLED(ENDSTOPPULLUP_XMIN)
+      PULLUP(X_MIN_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(Y_MIN)
+    SET_INPUT(Y_MIN_PIN);
+    #if ENABLED(ENDSTOPPULLUP_YMIN)
+      PULLUP(Y_MIN_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(Z_MIN)
+    SET_INPUT(Z_MIN_PIN);
+    #if ENABLED(ENDSTOPPULLUP_ZMIN)
+      PULLUP(Z_MIN_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(Z2_MIN)
+    SET_INPUT(Z2_MIN_PIN);
+    #if ENABLED(ENDSTOPPULLUP_Z2MIN)
+      PULLUP(Z2_MIN_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(E_MIN)
+    SET_INPUT(E_MIN_PIN);
+    #if ENABLED(ENDSTOPPULLUP_EMIN)
+      PULLUP(E_MIN_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(X_MAX)
+    SET_INPUT(X_MAX_PIN);
+    #if ENABLED(ENDSTOPPULLUP_XMAX)
+      PULLUP(X_MAX_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(Y_MAX)
+    SET_INPUT(Y_MAX_PIN);
+    #if ENABLED(ENDSTOPPULLUP_YMAX)
+      PULLUP(Y_MAX_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(Z_MAX)
+    SET_INPUT(Z_MAX_PIN);
+    #if ENABLED(ENDSTOPPULLUP_ZMAX)
+      PULLUP(Z_MAX_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(Z2_MAX)
+    SET_INPUT(Z2_MAX_PIN);
+    #if ENABLED(ENDSTOPPULLUP_Z2MAX)
+      PULLUP(Z2_MAX_PIN, HIGH);
+    #endif
+  #endif
+
+  #if HAS(Z_PROBE) // Check for Z_PROBE_ENDSTOP so we don't pull a pin high unless it's to be used.
+    SET_INPUT(Z_PROBE_PIN);
+    #if ENABLED(ENDSTOPPULLUP_ZPROBE)
+      PULLUP(Z_PROBE_PIN, HIGH);
+    #endif
+  #endif
+
+} // Endstops::init
+
+void Endstops::report_state() {
+  if (endstop_hit_bits) {
+    #if ENABLED(ULTRA_LCD)
+      char chrX = ' ', chrY = ' ', chrZ = ' ', chrP = ' ';
+      #define _SET_STOP_CHAR(A,C) (chr## A = C)
+    #else
+      #define _SET_STOP_CHAR(A,C) ;
+    #endif
+
+    #define _ENDSTOP_HIT_ECHO(A,C) do{ \
+      ECHO_MV(" " STRINGIFY(A) ":", triggered_position_mm(A ##_AXIS)); \
+      _SET_STOP_CHAR(A,C); }while(0)
+
+    #define _ENDSTOP_HIT_TEST(A,C) \
+      if (TEST(endstop_hit_bits, A ##_MIN) || TEST(endstop_hit_bits, A ##_MAX)) \
+        _ENDSTOP_HIT_ECHO(A,C)
+
+    ECHO_SM(ER, SERIAL_ENDSTOPS_HIT);
+    _ENDSTOP_HIT_TEST(X, 'X');
+    _ENDSTOP_HIT_TEST(Y, 'Y');
+    _ENDSTOP_HIT_TEST(Z, 'Z');
+
+    #if ENABLED(Z_PROBE_ENDSTOP)
+      #define P_AXIS Z_AXIS
+      if (TEST(endstop_hit_bits, Z_PROBE)) _ENDSTOP_HIT_ECHO(P, 'P');
+    #endif
+    ECHO_E;
+
+    #if ENABLED(ULTRA_LCD)
+      char msg[3 * strlen(MSG_ENDSTOPS_HIT) + 8 + 1]; // Room for a UTF 8 string
+      sprintf_P(msg, PSTR(MSG_ENDSTOPS_HIT " %c %c %c %c"), chrX, chrY, chrZ, chrP);
+      lcd_setstatus(msg);
+    #endif
+
+    hit_on_purpose();
+
+    #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && ENABLED(SDSUPPORT)
+      if (abort_on_endstop_hit) {
+        card.sdprinting = false;
+        card.closefile();
+        quickStop();
+        disable_all_heaters(); // switch off all heaters.
+      }
+    #endif
+  }
+} // Endstops::report_state
+
+void Endstops::M119() {
+  ECHO_LM(DB, SERIAL_M119_REPORT);
+  #if HAS(X_MIN)
+    ECHO_EMT(SERIAL_X_MIN, ((READ(X_MIN_PIN)^X_MIN_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(X_MAX)
+    ECHO_EMT(SERIAL_X_MAX, ((READ(X_MAX_PIN)^X_MAX_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(Y_MIN)
+    ECHO_EMT(SERIAL_Y_MIN, ((READ(Y_MIN_PIN)^Y_MIN_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(Y_MAX)
+    ECHO_EMT(SERIAL_Y_MAX, ((READ(Y_MAX_PIN)^Y_MAX_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(Z_MIN)
+    ECHO_EMT(SERIAL_Z_MIN, ((READ(Z_MIN_PIN)^Z_MIN_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(Z_MAX)
+    ECHO_EMT(SERIAL_Z_MAX, ((READ(Z_MAX_PIN)^Z_MAX_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(Z2_MAX)
+    ECHO_EMT(SERIAL_Z2_MAX, ((READ(Z2_MAX_PIN)^Z2_MAX_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(Z_PROBE)
+    ECHO_EMT(SERIAL_Z_PROBE, ((READ(Z_PROBE_PIN)^Z_PROBE_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(E_MIN)
+    ECHO_EMT(SERIAL_E_MIN, ((READ(E_MIN_PIN)^E_MIN_ENDSTOP_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+  #if HAS(FILRUNOUT)
+    ECHO_EMT(SERIAL_FILRUNOUT_PIN, ((READ(FILRUNOUT_PIN)^FILRUNOUT_PIN_INVERTING)?SERIAL_ENDSTOP_HIT:SERIAL_ENDSTOP_OPEN));
+  #endif
+} // Endstops::M119
+
+#if ENABLED(Z_DUAL_ENDSTOPS)
+
+  // Pass the result of the endstop test
+  void Endstops::test_dual_z_endstops(EndstopEnum es1, EndstopEnum es2) {
+    byte z_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for Z, bit 1 for Z2
+    if (current_block->steps[Z_AXIS] > 0) {
+      endstop_triggered(Z_AXIS);
+      SBI(endstop_hit_bits, Z_MIN);
+      if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
+        kill_current_block();
+    }
+  }
+
+#endif
+
+// Check endstops - Called from ISR!
+void Endstops::update() {
+
+  #define _ENDSTOP_PIN(AXIS, MINMAX) AXIS ##_## MINMAX ##_PIN
+  #define _ENDSTOP_INVERTING(AXIS, MINMAX) AXIS ##_## MINMAX ##_ENDSTOP_INVERTING
+  #define _ENDSTOP_HIT(AXIS) SBI(endstop_hit_bits, _ENDSTOP(AXIS, MIN))
+  #define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX
+
+  // UPDATE_ENDSTOP_BIT: set the current endstop bits for an endstop to its status
+  #define UPDATE_ENDSTOP_BIT(AXIS, MINMAX) SET_BIT(current_endstop_bits, _ENDSTOP(AXIS, MINMAX), (READ(_ENDSTOP_PIN(AXIS, MINMAX)) != _ENDSTOP_INVERTING(AXIS, MINMAX)))
+  // COPY_BIT: copy the value of COPY_BIT to BIT in bits
+  #define COPY_BIT(bits, COPY_BIT, BIT) SET_BIT(bits, BIT, TEST(bits, COPY_BIT))
+
+  #define UPDATE_ENDSTOP(AXIS,MINMAX) do { \
+      UPDATE_ENDSTOP_BIT(AXIS, MINMAX); \
+      if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX)) && current_block->steps[_AXIS(AXIS)] > 0) { \
+        _ENDSTOP_HIT(AXIS); \
+        endstop_triggered(_AXIS(AXIS)); \
+      } \
+    } while(0)
+
+  #if MECH(COREXY) || MECH(COREYX)|| MECH(COREXZ) || MECH(COREZX)
+    // Head direction in -X axis for CoreXY and CoreXZ bots.
+    // If Delta1 == -Delta2, the movement is only in Y or Z axis
+    if ((current_block->steps[A_AXIS] != current_block->steps[CORE_AXIS_2]) || (motor_direction(A_AXIS) == motor_direction(CORE_AXIS_2))) {
+      if (motor_direction(X_HEAD))
+  #else
+    if (motor_direction(X_AXIS))   // stepping along -X axis (regular Cartesian bot)
+  #endif
+      { // -direction
+        #if ENABLED(DUAL_X_CARRIAGE)
+          // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
+          if ((current_block->active_driver == 0 && X_HOME_DIR == -1) || (current_block->active_driver != 0 && X2_HOME_DIR == -1))
+        #endif
+          {
+            #if HAS(X_MIN)
+              UPDATE_ENDSTOP(X, MIN);
+            #endif
+          }
+      }
+      else { // +direction
+        #if ENABLED(DUAL_X_CARRIAGE)
+          // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
+          if ((current_block->active_driver == 0 && X_HOME_DIR == 1) || (current_block->active_driver != 0 && X2_HOME_DIR == 1))
+        #endif
+          {
+            #if HAS(X_MAX)
+              UPDATE_ENDSTOP(X, MAX);
+            #endif
+          }
+      }
+  #if MECH(COREXY) || MECH(COREYX) || MECH(COREXZ) || MECH(COREZX)
+    }
+  #endif
+
+  #if MECH(COREXY) || MECH(COREYX)
+    // Head direction in -Y axis for CoreXY bots.
+    // If DeltaX == DeltaY, the movement is only in X axis
+    if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (motor_direction(A_AXIS) != motor_direction(B_AXIS))) {
+      if (motor_direction(Y_HEAD))
+  #else
+      if (motor_direction(Y_AXIS))   // -direction
+  #endif
+      { // -direction
+        #if HAS(Y_MIN)
+          UPDATE_ENDSTOP(Y, MIN);
+        #endif
+      }
+      else { // +direction
+        #if HAS(Y_MAX)
+          UPDATE_ENDSTOP(Y, MAX);
+        #endif
+      }
+  #if MECH(COREXY) || MECH(COREYX)
+    }
+  #endif
+
+  #if MECH(COREXZ) || MECH(COREZX)
+    // Head direction in -Z axis for CoreXZ bots.
+    // If DeltaX == DeltaZ, the movement is only in X axis
+    if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (motor_direction(A_AXIS) != motor_direction(C_AXIS))) {
+      if (motor_direction(Z_HEAD))
+  #else
+      if (motor_direction(Z_AXIS))
+  #endif
+      { // z -direction
+        #if HAS(Z_MIN)
+
+          #if ENABLED(Z_DUAL_ENDSTOPS)
+            UPDATE_ENDSTOP_BIT(Z, MIN);
+            #if HAS_Z2_MIN
+              UPDATE_ENDSTOP_BIT(Z2, MIN);
+            #else
+              COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
+            #endif
+
+            test_dual_z_endstops(Z_MIN, Z2_MIN);
+
+          #else // !Z_DUAL_ENDSTOPS
+
+            UPDATE_ENDSTOP(Z, MIN);
+
+          #endif // !Z_DUAL_ENDSTOPS
+        #endif // HAS_Z_MIN
+
+        #if ENABLED(Z_PROBE_ENDSTOP)
+          if (z_probe_enabled) {
+            UPDATE_ENDSTOP(Z, PROBE);
+            if (TEST_ENDSTOP(Z_PROBE)) SBI(endstop_hit_bits, Z_PROBE);
+          }
+        #endif
+      }
+      else { // z +direction
+        #if HAS(Z_MAX)
+
+          #if ENABLED(Z_DUAL_ENDSTOPS)
+
+            UPDATE_ENDSTOP_BIT(Z, MAX);
+            #if HAS_Z2_MAX
+              UPDATE_ENDSTOP_BIT(Z2, MAX);
+            #else
+              COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
+            #endif
+
+            test_dual_z_endstops(Z_MAX, Z2_MAX);
+
+          #else // !Z_DUAL_ENDSTOPS
+
+            UPDATE_ENDSTOP(Z, MAX);
+
+          #endif // !Z_DUAL_ENDSTOPS
+        #endif // Z_MAX_PIN
+      }
+  #if MECH(COREXZ) || MECH(COREZX)
+    }
+  #endif
+
+  #if ENABLED(NPR2)
+    UPDATE_ENDSTOP(E, MIN);
+  #endif
+
+  old_endstop_bits = current_endstop_bits;
+
+} // Endstops::update()

MK/module/motion/endstops.h

+/**
+ * MK & MK4due 3D Printer Firmware
+ *
+ * Based on Marlin, Sprinter and grbl
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ *  endstops.h - manages endstops
+ */
+
+#ifndef ENDSTOPS_H
+#define ENDSTOPS_H
+
+enum EndstopEnum {X_MIN = 0, Y_MIN = 1, Z_MIN = 2, Z_PROBE = 3, X_MAX = 4, Y_MAX = 5, Z_MAX = 6, Z2_MIN = 7, Z2_MAX = 8, E_MIN = 9};
+
+class Endstops {
+
+  public:
+
+    volatile char endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_PROBE as BIT value
+
+    #if ENABLED(Z_DUAL_ENDSTOPS)
+      uint16_t current_endstop_bits = 0,
+                   old_endstop_bits = 0;
+    #else
+      byte current_endstop_bits = 0,
+               old_endstop_bits = 0;
+    #endif
+        
+
+    bool enabled = true;
+    bool enabled_globally =
+      #if ENABLED(ENDSTOPS_ONLY_FOR_HOMING)
+        false
+      #else
+        true
+      #endif
+    ;
+
+    Endstops();
+
+    /**
+     * Initialize the endstop pins
+     */
+    void init();
+
+    /**
+     * Update the endstops bits from the pins
+     */
+    void update();
+
+    /**
+     * Print an error message reporting the position when the endstops were last hit.
+     */
+    void report_state(); //call from somewhere to create an serial error message with the locations the endstops where hit, in case they were triggered
+
+    /**
+     * Report endstop positions in response to M119
+     */
+    void M119();
+
+    // Enable / disable endstop checking globally
+    FORCE_INLINE void enable_globally(bool onoff = true) { enabled_globally = enabled = onoff; }
+
+    // Enable / disable endstop checking
+    FORCE_INLINE void enable(bool onoff = true) { enabled = onoff; }
+
+    // Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
+    FORCE_INLINE void not_homing() { enabled = enabled_globally; }
+
+    // Clear endstops (i.e., they were hit intentionally) to suppress the report
+    FORCE_INLINE void hit_on_purpose() { endstop_hit_bits = 0; }
+
+    // Enable / disable endstop z-probe checking
+    #if ENABLED(HAS_Z_PROBE)
+      volatile bool z_probe_enabled = false;
+      FORCE_INLINE void enable_z_probe(bool onoff = true) { z_probe_enabled = onoff; }
+    #endif
+
+  private:
+
+    #if ENABLED(Z_DUAL_ENDSTOPS)
+      void test_dual_z_endstops(EndstopEnum es1, EndstopEnum es2);
+    #endif
+};
+
+extern Endstops endstops;
+
+#endif // ENDSTOPS_H

MK/module/motion/planner.cpp

@@ -1037,7 +1037,7 @@ float junction_deviation = 0.1;
 
   // Compute and limit the acceleration rate for the trapezoid generator.
   float steps_per_mm = block->step_event_count / block->millimeters;
-  unsigned long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
+  long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
   if (bsx == 0 && bsy == 0 && bsz == 0) {
     block->acceleration_st = ceil(retract_acceleration[extruder] * steps_per_mm); // convert to: acceleration steps/sec^2
   }
@@ -1171,7 +1171,20 @@ float junction_deviation = 0.1;
     ECHO_SMV(OK, "advance :", block->advance/256);
     ECHO_EMV("advance rate :", block->advance_rate/256);
     */
-  #endif // ADVANCE
+  #elif ENABLED(ADVANCE_LPC) // ADVANCE_LPC
+    // bse == allsteps: A problem occurs when there's a very tiny move before a retract.
+    // In this case, the retract and the move will be executed together.
+    // This leads to an enormous number of advance steps due to a huge e_acceleration.
+    // The math is correct, but you don't want a retract move done with advance!
+    // So this situation is filtered out here.
+    if (!bse || (!bsx && !bsy && !bsz) || extruder_advance_k == 0 || bse == allsteps) {
+      block->use_advance_lead = false;
+    }
+    else {
+      block->use_advance_lead = true;
+      block->e_speed_multiplier8 = (block->steps[E_AXIS] << 8) / block->step_event_count;
+    }
+  #endif
 
   calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
 

MK/module/motion/planner.h

@@ -49,25 +49,32 @@
 // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
 // the source g-code and may never actually be reached if acceleration management is active.
 typedef struct {
+
+  unsigned char active_driver;              // Selects the active driver
+
   // Fields used by the bresenham algorithm for tracing the line
   unsigned long steps[NUM_AXIS];                      // Step count along each axis
+  unsigned long step_event_count;           // The number of step events required to complete this block
 
   #if ENABLED(COLOR_MIXING_EXTRUDER)
     unsigned long mix_event_count[DRIVER_EXTRUDERS];  // Step count for each stepper in a mixing extruder
   #endif
 
-  unsigned long step_event_count;           // The number of step events required to complete this block
   long accelerate_until;                    // The index of the step event on which to stop acceleration
   long decelerate_after;                    // The index of the step event on which to start decelerating
   long acceleration_rate;                   // The acceleration rate used for acceleration calculation
+
   unsigned char direction_bits;             // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
-  unsigned char active_driver;              // Selects the active driver
 
+  // Advance extrusion
   #if ENABLED(ADVANCE)
     long advance_rate;
     volatile long initial_advance;
     volatile long final_advance;
     float advance;
+  #elif ENABLED(ADVANCE_LPC)
+    bool use_advance_lead;
+    int e_speed_multiplier8;
   #endif
 
   // Fields used by the motion planner to manage acceleration

MK/module/motion/stepper.cpp

@@ -12,16 +12,35 @@
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * stepper.cpp - A singleton object to execute motion plans using stepper motors
+ *
+ * Derived from Grbl
+ * Copyright (c) 2009-2011 Simen Svale Skogsrud
+ *
+ * Grbl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Grbl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
  *
+ * You should have received a copy of the GNU General Public License
+ * along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
  */
 
 /**
- * stepper.cpp - stepper motor driver: executes motion plans using stepper motors
  * The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
  * and Philipp Tiefenbacher.
  */
@@ -46,8 +65,8 @@ block_t* current_block;  // A pointer to the block currently being traced
 //static makes it impossible to be called from outside of this file by extern.!
 
 // Variables used by The Stepper Driver Interrupt
-static unsigned char out_bits = 0;        // The next stepping-bits to be output
-static unsigned int cleaning_buffer_counter;
+static unsigned char last_direction_bits = 0;  // The next stepping-bits to be output
+static unsigned int cleaning_buffer_counter = 0;
 
 #ifdef LASER
 static long counter_l;
@@ -67,12 +86,24 @@ static int counter_raster;
 
 // Counter variables for the Bresenham line tracer
 static long counter_X, counter_Y, counter_Z, counter_E;
-volatile static unsigned long step_events_completed; // The number of step events executed in the current block
+volatile unsigned long step_events_completed; // The number of step events executed in the current block
+volatile long endstops_trigsteps[3];
+volatile long endstops_stepsTotal, endstops_stepsDone;
 
-#if ENABLED(ADVANCE)
-  static long advance_rate, advance, final_advance = 0;
-  static long old_advance = 0;
-  static long e_steps[6];
+#if ENABLED(ADVANCE) || ENABLED(ADVANCE_LPC)
+  unsigned char old_OCR0A;
+  #if ENABLED(ADVANCE)
+    static long advance_rate, advance, final_advance = 0;
+    static long old_advance = 0;
+    static long e_steps[6];
+  #elif ENABLED(ADVANCE_LPC)
+    int extruder_advance_k = ADVANCE_LPC_K;
+    volatile int e_steps[EXTRUDERS] = ARRAY_BY_EXTRUDERS(0);
+    volatile unsigned char eISR_Rate = 200; // Keep the ISR at a low rate until needed
+    static int final_estep_rate;
+    static int current_estep_rate[EXTRUDERS]; // Actual extruder speed [steps/s]
+    static int current_adv_steps[EXTRUDERS];
+  #endif
 #endif
 
 static long acceleration_time, deceleration_time;
@@ -82,25 +113,6 @@ static uint8_t step_loops;
 static uint8_t step_loops_nominal;
 static unsigned short OCR1A_nominal;
 
-volatile long endstops_trigsteps[3] = { 0 };
-volatile long endstops_stepsTotal, endstops_stepsDone;
-static volatile char endstop_hit_bits = 0; // use X_MIN, Y_MIN, Z_MIN and Z_PROBE as BIT value
-
-#if ENABLED(Z_DUAL_ENDSTOPS) || ENABLED(NPR2)
-  static uint16_t
-#else
-  static byte
-#endif
-    old_endstop_bits = 0; // use X_MIN, X_MAX... Z_MAX, Z_PROBE, Z2_MIN, Z2_MAX, E_MIN
-
-#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-  #if ENABLED(ABORT_ON_ENDSTOP_HIT_INIT)
-    bool abort_on_endstop_hit = ABORT_ON_ENDSTOP_HIT_INIT;
-  #else
-    bool abort_on_endstop_hit = false;
-  #endif
-#endif
-
 #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
   int motor_current_setting[3] = DEFAULT_PWM_MOTOR_CURRENT;
 #endif
@@ -267,273 +279,23 @@ volatile signed char count_direction[NUM_AXIS] = { 1 };
 #define ENABLE_STEPPER_DRIVER_INTERRUPT()  SBI(TIMSK1, OCIE1A)
 #define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
 
-void endstops_hit_on_purpose() {
-  endstop_hit_bits = 0;
-}
-
-void checkHitEndstops() {
-  if (endstop_hit_bits) {
-
-    #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-      if (abort_on_endstop_hit)
-        ECHO_SM(ER, SERIAL_ENDSTOPS_HIT);
-      else
-        ECHO_SM(DB, SERIAL_ENDSTOPS_HIT);
-    #else
-      ECHO_SM(DB, SERIAL_ENDSTOPS_HIT);
-    #endif
-
-    if (TEST(endstop_hit_bits, X_MIN)) {
-      ECHO_MV(SERIAL_ENDSTOP_X, (float)endstops_trigsteps[X_AXIS] / axis_steps_per_unit[X_AXIS]);
-      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_XS);
-    }
-    if (TEST(endstop_hit_bits, Y_MIN)) {
-      ECHO_MV(SERIAL_ENDSTOP_Y, (float)endstops_trigsteps[Y_AXIS] / axis_steps_per_unit[Y_AXIS]);
-      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_YS);
-    }
-    if (TEST(endstop_hit_bits, Z_MIN)) {
-      ECHO_MV(SERIAL_ENDSTOP_Z, (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
-      LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_ZS);
-    }
-    #if ENABLED(Z_PROBE_ENDSTOP)
-      if (TEST(endstop_hit_bits, Z_PROBE)) {
-        ECHO_MV(SERIAL_ENDSTOP_PROBE, (float)endstops_trigsteps[Z_AXIS] / axis_steps_per_unit[Z_AXIS]);
-        LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_ZPS);
-      }
-    #endif
-    #if ENABLED(NPR2)
-      if (TEST(endstop_hit_bits, E_MIN)) {
-        ECHO_MV(SERIAL_ENDSTOP_E, (float)endstops_trigsteps[E_AXIS] / axis_steps_per_unit[E_AXIS]);
-        LCD_MESSAGEPGM(MSG_ENDSTOPS_HIT MSG_ENDSTOP_ES);
-      }
-    #endif
-    ECHO_E;
-
-    #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-      if (abort_on_endstop_hit && !(endstop_hit_bits & _BV(Z_PROBE)) && !(endstop_hit_bits & _BV(E_MIN))) {
-        #if ENABLED(SDSUPPORT)
-          card.sdprinting = false;
-          card.closeFile();
-        #endif
-        for (int i = 0; i < 3; i++) axis_known_position[i] = true; // not homed anymore
-        quickStop(); // kill the planner buffer
-        Stop();      // restart by M999
-      }
-    #endif
-
-    endstops_hit_on_purpose();
-  }
-}
-
-#if MECH(COREXY) || MECH(COREYX)
-  #define CORE_AXIS_2 B_AXIS
-#elif MECH(COREXZ) || MECH(COREZX)
-  #define CORE_AXIS_2 C_AXIS
-#endif
-
-void enable_endstops(bool check) { check_endstops = check; }
-
-// Check endstops - Called from ISR!
-inline void update_endstops() {
-
-  #if ENABLED(Z_DUAL_ENDSTOPS) || ENABLED(NPR2)
-    uint16_t
-  #else
-    byte
-  #endif
-      current_endstop_bits = 0;
-
-  #define _ENDSTOP_PIN(AXIS, MINMAX) AXIS ##_## MINMAX ##_PIN
-  #define _ENDSTOP_INVERTING(AXIS, MINMAX) AXIS ##_## MINMAX ##_ENDSTOP_INVERTING
-  #define _AXIS(AXIS) AXIS ##_AXIS
-  #define _ENDSTOP_HIT(AXIS) SBI(endstop_hit_bits, _ENDSTOP(AXIS, MIN))
-  #define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX
-
-  // SET_ENDSTOP_BIT: set the current endstop bits for an endstop to its status
-  #define SET_ENDSTOP_BIT(AXIS, MINMAX) SET_BIT(current_endstop_bits, _ENDSTOP(AXIS, MINMAX), (READ(_ENDSTOP_PIN(AXIS, MINMAX)) != _ENDSTOP_INVERTING(AXIS, MINMAX)))
-  // COPY_BIT: copy the value of COPY_BIT to BIT in bits
-  #define COPY_BIT(bits, COPY_BIT, BIT) SET_BIT(bits, BIT, TEST(bits, COPY_BIT))
-  // TEST_ENDSTOP: test the old and the current status of an endstop
-  #define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits, ENDSTOP) && TEST(old_endstop_bits, ENDSTOP))
-
-  #if MECH(COREXY) || MECH(COREYX)|| MECH(COREXZ) || MECH(COREZX)
-
-    #define _SET_TRIGSTEPS(AXIS) do { \
-        float axis_pos = count_position[_AXIS(AXIS)]; \
-        if (_AXIS(AXIS) == A_AXIS) \
-          axis_pos = (axis_pos + count_position[CORE_AXIS_2]) / 2; \
-        else if (_AXIS(AXIS) == CORE_AXIS_2) \
-          axis_pos = (count_position[A_AXIS] - axis_pos) / 2; \
-        endstops_trigsteps[_AXIS(AXIS)] = axis_pos; \
-      } while(0)
-
-  #else
-
-    #define _SET_TRIGSTEPS(AXIS) endstops_trigsteps[_AXIS(AXIS)] = count_position[_AXIS(AXIS)]
-
-  #endif // COREXY || COREYX || COREXZ || COREZX
-
-  #define UPDATE_ENDSTOP(AXIS,MINMAX) do { \
-      SET_ENDSTOP_BIT(AXIS, MINMAX); \
-      if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX)) && current_block->steps[_AXIS(AXIS)] > 0) { \
-        _SET_TRIGSTEPS(AXIS); \
-        _ENDSTOP_HIT(AXIS); \
-        step_events_completed = current_block->step_event_count; \
-      } \
-    } while(0)
-
-  #if MECH(COREXY) || MECH(COREYX)|| MECH(COREXZ) || MECH(COREZX)
-    // Head direction in -X axis for CoreXY and CoreXZ bots.
-    // If Delta1 == -Delta2, the movement is only in Y or Z axis
-    if ((current_block->steps[A_AXIS] != current_block->steps[CORE_AXIS_2]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, CORE_AXIS_2))) {
-      if (TEST(out_bits, X_HEAD))
-  #else
-    if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular Cartesian bot)
-  #endif
-      { // -direction
-        #if ENABLED(DUAL_X_CARRIAGE)
-          // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
-          if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) || (current_block->active_extruder != 0 && X2_HOME_DIR == -1))
-        #endif
-          {
-            #if HAS(X_MIN)
-              UPDATE_ENDSTOP(X, MIN);
-            #endif
-          }
-      }
-      else { // +direction
-        #if ENABLED(DUAL_X_CARRIAGE)
-          // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
-          if ((current_block->active_extruder == 0 && X_HOME_DIR == 1) || (current_block->active_extruder != 0 && X2_HOME_DIR == 1))
-        #endif
-          {
-            #if HAS(X_MAX)
-              UPDATE_ENDSTOP(X, MAX);
-            #endif
-          }
-      }
-  #if MECH(COREXY) || MECH(COREYX)|| MECH(COREXZ) || MECH(COREZX)
-    }
-  #endif
-
-  #if MECH(COREXY) || MECH(COREYX)
-    // Head direction in -Y axis for CoreXY bots.
-    // If DeltaX == DeltaY, the movement is only in X axis
-    if ((current_block->steps[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS))) {
-      if (TEST(out_bits, Y_HEAD))
-  #else
-      if (TEST(out_bits, Y_AXIS))   // -direction
-  #endif
-      { // -direction
-        #if HAS(Y_MIN)
-          UPDATE_ENDSTOP(Y, MIN);
-        #endif
-      }
-      else { // +direction
-        #if HAS(Y_MAX)
-          UPDATE_ENDSTOP(Y, MAX);
-        #endif
-      }
-  #if MECH(COREXY) || MECH(COREYX)
-    }
-  #endif
-
-  #if MECH(COREXZ) || MECH(COREZX)
-    // Head direction in -Z axis for CoreXZ bots.
-    // If DeltaX == DeltaZ, the movement is only in X axis
-    if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, C_AXIS))) {
-      if (TEST(out_bits, Z_HEAD))
-  #else
-      if (TEST(out_bits, Z_AXIS))
-  #endif
-      { // z -direction
-        #if HAS(Z_MIN)
-
-          #if ENABLED(Z_DUAL_ENDSTOPS)
-            SET_ENDSTOP_BIT(Z, MIN);
-            #if HAS(Z2_MIN)
-              SET_ENDSTOP_BIT(Z2, MIN);
-            #else
-              COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
-            #endif
-
-            byte z_test = TEST_ENDSTOP(Z_MIN) | (TEST_ENDSTOP(Z2_MIN) << 1); // bit 0 for Z, bit 1 for Z2
-
-            if (z_test && current_block->steps[Z_AXIS] > 0) { // z_test = Z_MIN || Z2_MIN
-              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              SBI(endstop_hit_bits, Z_MIN);
-              if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
-                step_events_completed = current_block->step_event_count;
-            }
-          #else // !Z_DUAL_ENDSTOPS
-
-            UPDATE_ENDSTOP(Z, MIN);
-
-          #endif // !Z_DUAL_ENDSTOPS
-        #endif // Z_MIN_PIN
-
-        #if ENABLED(Z_PROBE_ENDSTOP)
-          UPDATE_ENDSTOP(Z, PROBE);
-
-          if (TEST_ENDSTOP(Z_PROBE)) {
-            endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-            SBI(endstop_hit_bits, Z_PROBE);
-          }
-        #endif
-      }
-      else { // z +direction
-        #if HAS(Z_MAX)
-
-          #if ENABLED(Z_DUAL_ENDSTOPS)
-
-            SET_ENDSTOP_BIT(Z, MAX);
-            #if HAS(Z2_MAX)
-              SET_ENDSTOP_BIT(Z2, MAX);
-            #else
-              COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
-            #endif
-
-            byte z_test = TEST_ENDSTOP(Z_MAX) | (TEST_ENDSTOP(Z2_MAX) << 1); // bit 0 for Z, bit 1 for Z2
-
-            if (z_test && current_block->steps[Z_AXIS] > 0) {  // t_test = Z_MAX || Z2_MAX
-              endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
-              SBI(endstop_hit_bits, Z_MIN);
-              if (!performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
-                step_events_completed = current_block->step_event_count;
-            }
-
-          #else // !Z_DUAL_ENDSTOPS
-
-            UPDATE_ENDSTOP(Z, MAX);
-
-          #endif // !Z_DUAL_ENDSTOPS
-        #endif // Z_MAX_PIN
-      }
-  #if MECH(COREXZ) || MECH(COREZX)
-    }
-  #endif
-  #if ENABLED(NPR2)
-    UPDATE_ENDSTOP(E, MIN);
-  #endif
-  old_endstop_bits = current_endstop_bits;
-}
-
-//         __________________________
-//        /|                        |\     _________________         ^
-//       / |                        | \   /|               |\        |
-//      /  |                        |  \ / |               | \       s
-//     /   |                        |   |  |               |  \      p
-//    /    |                        |   |  |               |   \     e
-//   +-----+------------------------+---+--+---------------+----+    e
-//   |               BLOCK 1            |      BLOCK 2          |    d
-//
-//                           time ----->
-//
-//  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
-//  first block->accelerate_until step_events_completed, then keeps going at constant speed until
-//  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
-//  The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far.
-
+/**
+ *         __________________________
+ *        /|                        |\     _________________         ^
+ *       / |                        | \   /|               |\        |
+ *      /  |                        |  \ / |               | \       s
+ *     /   |                        |   |  |               |  \      p
+ *    /    |                        |   |  |               |   \     e
+ *   +-----+------------------------+---+--+---------------+----+    e
+ *   |               BLOCK 1            |      BLOCK 2          |    d
+ *
+ *                           time ----->
+ *
+ *  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
+ *  first block->accelerate_until step_events_completed, then keeps going at constant speed until
+ *  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
+ *  The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far.
+ */
 void st_wake_up() {
   //  TCNT1 = 0;
   ENABLE_STEPPER_DRIVER_INTERRUPT();
@@ -545,11 +307,11 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
   NOMORE(step_rate, MAX_STEP_FREQUENCY);
 
   if(step_rate > (2 * DOUBLE_STEP_FREQUENCY)) { // If steprate > 2*DOUBLE_STEP_FREQUENCY >> step 4 times
-    step_rate = (step_rate >> 2) & 0x3fff;
+    step_rate = (step_rate >> 2);
     step_loops = 4;
   }
   else if(step_rate > DOUBLE_STEP_FREQUENCY) { // If steprate > DOUBLE_STEP_FREQUENCY >> step 2 times
-    step_rate = (step_rate >> 1) & 0x7fff;
+    step_rate = (step_rate >> 1);
     step_loops = 2;
   }
   else {
@@ -589,7 +351,7 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
 void set_stepper_direction(bool onlye) {
 
   #define SET_STEP_DIR(AXIS) \
-    if (TEST(out_bits, AXIS ##_AXIS)) { \
+    if (motor_direction(AXIS ##_AXIS)) { \
       AXIS ##_APPLY_DIR(INVERT_## AXIS ##_DIR, false); \
       count_direction[AXIS ##_AXIS] = -1; \
     } \
@@ -605,7 +367,7 @@ void set_stepper_direction(bool onlye) {
   }
 
   #if DISABLED(ADVANCE)
-    if (TEST(out_bits, E_AXIS)) {
+    if (motor_direction(E_AXIS)) {
       REV_E_DIR();
       count_direction[E_AXIS] = -1;
     }
@@ -620,8 +382,11 @@ void set_stepper_direction(bool onlye) {
 // block begins.
 FORCE_INLINE void trapezoid_generator_reset() {
 
-  if (current_block->direction_bits != out_bits) {
-    out_bits = current_block->direction_bits;
+  static int8_t last_driver = -1;
+
+  if (current_block->direction_bits != last_direction_bits || current_block->active_driver != last_driver) {
+    last_direction_bits = current_block->direction_bits;
+    last_driver = current_block->active_driver;
     set_stepper_direction();
   }
 
@@ -640,6 +405,13 @@ FORCE_INLINE void trapezoid_generator_reset() {
   acc_step_rate = current_block->initial_rate;
   acceleration_time = calc_timer(acc_step_rate);
   OCR1A = acceleration_time;
+
+  #if ENABLED(ADVANCE_LPC)
+    if (current_block->use_advance_lead) {
+      current_estep_rate[current_block->active_driver] = ((unsigned long)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
+      final_estep_rate = (current_block->nominal_rate * current_block->e_speed_multiplier8) >> 8;
+    }
+  #endif
 }
 
 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
@@ -713,7 +485,11 @@ ISR(TIMER1_COMPA_vect) {
   if (current_block != NULL) {
 
     // Update endstops state, if enabled
-    if (check_endstops) update_endstops();
+    #if ENABLED(Z_PROBE_ENDSTOP)
+      if (endstops.enabled || endstops.z_probe_enabled) endstops.update();
+    #else
+      if (endstops.enabled) endstops.update();
+    #endif
 
     // Continuous firing of the laser during a move happens here, PPM and raster happen further down
     #if ENABLED(LASER)
@@ -737,12 +513,12 @@ ISR(TIMER1_COMPA_vect) {
           counter_E -= current_block->step_event_count;
           #if DISABLED(COLOR_MIXING_EXTRUDER)
             // Don't step E for mixing extruder
-            e_steps[current_block->active_driver] += TEST(out_bits, E_AXIS) ? -1 : 1;
+            e_steps[current_block->active_driver] += motor_direction(E_AXIS) ? -1 : 1;
           #endif
         }
 
         #if ENABLED(COLOR_MIXING_EXTRUDER)
-          long dir = TEST(out_bits, E_AXIS) ? -1 : 1;
+          long dir = motor_direction(E_AXIS) ? -1 : 1;
           for (uint8_t j = 0; j < DRIVER_EXTRUDERS; j++) {
             counter_m[j] += current_block->steps[E_AXIS];
             if (counter_m[j] > 0) {
@@ -751,7 +527,21 @@ ISR(TIMER1_COMPA_vect) {
             }
           }
         #endif // !COLOR_MIXING_EXTRUDER
-      #endif // ADVANCE
+      #elif ENABLED(ADVANCE_LPC) // ADVANCE_LPC
+        counter_E += current_block->steps[E_AXIS];
+        if (counter_E > 0) {
+          counter_E -= current_block->step_event_count;
+          count_position[E_AXIS] += count_direction[E_AXIS];
+          e_steps[current_block->active_driver] += motor_direction(E_AXIS) ? -1 : 1;
+        }
+
+        if (current_block->use_advance_lead) {
+          int delta_adv_steps; // Maybe a char would be enough?
+          delta_adv_steps = (((long)extruder_advance_k * current_estep_rate[current_block->active_driver]) >> 9) - current_adv_steps[current_block->active_driver];
+          e_steps[current_block->active_driver] += delta_adv_steps;
+          current_adv_steps[current_block->active_driver] += delta_adv_steps;
+        }
+      #endif
 
       #define _COUNTER(AXIS) counter_## AXIS
       #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
@@ -785,7 +575,7 @@ ISR(TIMER1_COMPA_vect) {
       STEP_START(X);
       STEP_START(Y);
       STEP_START(Z);
-      #if DISABLED(ADVANCE)
+      #if DISABLED(ADVANCE) && DISABLED(ADVANCE_LPC)
         STEP_START(E);
         #if ENABLED(COLOR_MIXING_EXTRUDER)
           STEP_START_MIXING;
@@ -799,7 +589,7 @@ ISR(TIMER1_COMPA_vect) {
       STEP_END(X);
       STEP_END(Y);
       STEP_END(Z);
-      #if DISABLED(ADVANCE)
+      #if DISABLED(ADVANCE) && DISABLED(ADVANCE_LPC)
         STEP_END(E);
         #if ENABLED(COLOR_MIXING_EXTRUDER)
           STEP_END_MIXING;
@@ -840,9 +630,14 @@ ISR(TIMER1_COMPA_vect) {
       step_events_completed++;
       if (step_events_completed >= current_block->step_event_count) break;
     }
+
+    #if ENABLED(ADVANCE_LPC)
+      // If we have esteps to execute, fire the next ISR "now"
+      if (e_steps[current_block->active_driver]) OCR0A = TCNT0 + 2;
+    #endif
+
     // Calculate new timer value
-    unsigned short timer;
-    unsigned short step_rate;
+    unsigned short timer, step_rate;
     if (step_events_completed <= (unsigned long)current_block->accelerate_until) {
 
       MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
@@ -871,8 +666,14 @@ ISR(TIMER1_COMPA_vect) {
         #endif
 
         old_advance = advance >> 8;
+      #elif ENABLED(ADVANCE_LPC) // ADVANCE_LPC
+        if (current_block->use_advance_lead)
+          current_estep_rate[current_block->active_driver] = ((unsigned long)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
+      #endif
 
-      #endif // ADVANCE
+      #if ENABLED(ADVANCE) || ENABLED(ADVANCE_LPC)
+        eISR_Rate = (timer >> 2) / e_steps[current_block->active_driver];
+      #endif
     }
     else if (step_events_completed > (unsigned long)current_block->decelerate_after) {
       MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
@@ -906,9 +707,22 @@ ISR(TIMER1_COMPA_vect) {
         #endif
 
         old_advance = advance_whole;
-      #endif //ADVANCE
+      #elif ENABLED(ADVANCE_LPC) // ADVANCE_LPC
+        if (current_block->use_advance_lead)
+          current_estep_rate[current_block->active_driver] = ((unsigned long)step_rate * current_block->e_speed_multiplier8) >> 8;
+      #endif
+
+      #if ENABLED(ADVANCE) || ENABLED(ADVANCE_LPC)
+        eISR_Rate = (timer >> 2) / e_steps[current_block->active_driver];
+      #endif
     }
     else {
+      #if ENABLED(ADVANCE_LPC)
+        if (current_block->use_advance_lead)
+          current_estep_rate[current_block->active_driver] = final_estep_rate;
+        eISR_Rate = (OCR1A_nominal >> 2) / e_steps[current_block->active_driver];
+      #endif
+
       OCR1A = OCR1A_nominal;
       // ensure we're running at the correct step rate, even if we just came off an acceleration
       step_loops = step_loops_nominal;
@@ -924,12 +738,13 @@ ISR(TIMER1_COMPA_vect) {
   }
 }
 
-#if ENABLED(ADVANCE)
-  unsigned char old_OCR0A;
+#if ENABLED(ADVANCE) || ENABLED(ADVANCE_LPC)
+
   // Timer interrupt for E. e_steps is set in the main routine;
   // Timer 0 is shared with millies
   ISR(TIMER0_COMPA_vect) {
-    old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
+
+    old_OCR0A += eISR_Rate;
     OCR0A = old_OCR0A;
 
     #define STEP_E_ONCE(INDEX) \
@@ -946,27 +761,25 @@ ISR(TIMER1_COMPA_vect) {
         E## INDEX ##_STEP_WRITE(!INVERT_E_STEP_PIN); \
       }
 
-    // Step all E steppers that have steps, up to 4 steps per interrupt
-    for (uint8_t i = 0; i < 4; i++) {
-      STEP_E_ONCE(0);
-      #if DRIVER_EXTRUDERS > 1
-        STEP_E_ONCE(1);
-        #if DRIVER_EXTRUDERS > 2
-          STEP_E_ONCE(2);
-          #if DRIVER_EXTRUDERS > 3
-            STEP_E_ONCE(3);
-            #if DRIVER_EXTRUDERS > 4
-              STEP_E_ONCE(4);
-              #if DRIVER_EXTRUDERS > 5
-                STEP_E_ONCE(5);
-              #endif // DRIVER_EXTRUDERS > 5
-            #endif // DRIVER_EXTRUDERS > 4
-          #endif // DRIVER_EXTRUDERS > 3
-        #endif // DRIVER_EXTRUDERS > 2
-      #endif // DRIVER_EXTRUDERS > 1
-    }
+    // Step all E steppers that have steps
+    STEP_E_ONCE(0);
+    #if EXTRUDERS > 1
+      STEP_E_ONCE(1);
+      #if EXTRUDERS > 2
+        STEP_E_ONCE(2);
+        #if EXTRUDERS > 3
+          STEP_E_ONCE(3);
+          #if EXTRUDERS > 4
+            STEP_E_ONCE(4);
+            #if EXTRUDERS > 5
+              STEP_E_ONCE(5);
+            #endif
+          #endif
+        #endif
+      #endif
+    #endif
   }
-#endif // ADVANCE
+#endif
 
 void st_init() {
   digipot_init(); //Initialize Digipot Motor Current
@@ -1092,77 +905,10 @@ void st_init() {
     OUT_WRITE_RELE(E1E3_CHOICE_PIN, LOW);
   #endif
 
-  //endstops and pullups
-
-  #if HAS(X_MIN)
-    SET_INPUT(X_MIN_PIN);
-    #if ENABLED(ENDSTOPPULLUP_XMIN)
-      PULLUP(X_MIN_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(Y_MIN)
-    SET_INPUT(Y_MIN_PIN);
-    #if ENABLED(ENDSTOPPULLUP_YMIN)
-      PULLUP(Y_MIN_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(Z_MIN)
-    SET_INPUT(Z_MIN_PIN);
-    #if ENABLED(ENDSTOPPULLUP_ZMIN)
-      PULLUP(Z_MIN_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(Z2_MIN)
-    SET_INPUT(Z2_MIN_PIN);
-    #if ENABLED(ENDSTOPPULLUP_Z2MIN)
-      PULLUP(Z2_MIN_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(E_MIN)
-    SET_INPUT(E_MIN_PIN);
-    #if ENABLED(ENDSTOPPULLUP_EMIN)
-      PULLUP(E_MIN_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(X_MAX)
-    SET_INPUT(X_MAX_PIN);
-    #if ENABLED(ENDSTOPPULLUP_XMAX)
-      PULLUP(X_MAX_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(Y_MAX)
-    SET_INPUT(Y_MAX_PIN);
-    #if ENABLED(ENDSTOPPULLUP_YMAX)
-      PULLUP(Y_MAX_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(Z_MAX)
-    SET_INPUT(Z_MAX_PIN);
-    #if ENABLED(ENDSTOPPULLUP_ZMAX)
-      PULLUP(Z_MAX_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(Z2_MAX)
-    SET_INPUT(Z2_MAX_PIN);
-    #if ENABLED(ENDSTOPPULLUP_Z2MAX)
-      PULLUP(Z2_MAX_PIN, HIGH);
-    #endif
-  #endif
-
-  #if HAS(Z_PROBE) // Check for Z_PROBE_ENDSTOP so we don't pull a pin high unless it's to be used.
-    SET_INPUT(Z_PROBE_PIN);
-    #if ENABLED(ENDSTOPPULLUP_ZPROBE)
-      PULLUP(Z_PROBE_PIN, HIGH);
-    #endif
-  #endif
+  //
+  // Init endstops and pullups here
+  //
+  endstops.init();
 
   #define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
   #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
@@ -1235,19 +981,28 @@ void st_init() {
   TCNT1 = 0;
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 
-  #if ENABLED(ADVANCE)
+  #if ENABLED(ADVANCE) || ENABLED(ADVANCE_LPC)
+    #if ENABLED(ADVANCE)
+      e_steps[0] = e_steps[1] = e_steps[2] = e_steps[3] = e_steps[4] = e_steps[5] = 0;
+    #elif ENABLED(ADVANCE_LPC)
+      for (uint8_t i = 0; i < EXTRUDERS; i++) {
+        e_steps[i] = 0;
+        current_adv_steps[i] = 0;
+      }
+    #endif
+      
     #if defined(TCCR0A) && defined(WGM01)
       CBI(TCCR0A, WGM01);
       CBI(TCCR0A, WGM00);
     #endif
-    e_steps[0] = e_steps[1] = e_steps[2] = e_steps[3] = e_steps[4] = e_steps[5] = 0;
     SBI(TIMSK0, OCIE0A);
-  #endif //ADVANCE
 
-  enable_endstops(true); // Start with endstops active. After homing they can be disabled
+  #endif // ADVANCE or ADVANCE_LPC
+
+  endstops.enable(true); // Start with endstops active. After homing they can be disabled
   sei();
 
-  set_stepper_direction(); // Init directions to out_bits = 0
+  set_stepper_direction(); // Init directions to last_direction_bits = 0
 }
 
 
@@ -1366,6 +1121,26 @@ void quickStop() {
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
+void endstop_triggered(AxisEnum axis) {
+
+  #if MECH(COREXY) || MECH(COREYX) || MECH(COREXZ) || MECH(COREZX)
+
+    float axis_pos = count_position[axis];
+    if (axis == A_AXIS)
+      axis_pos = (axis_pos + count_position[CORE_AXIS_2]) / 2;
+    else if (axis == CORE_AXIS_2)
+      axis_pos = (count_position[A_AXIS] - axis_pos) / 2;
+    endstops_trigsteps[axis] = axis_pos;
+
+  #else // ! COREXY || COREYX || COREXZ || COREZX
+
+    endstops_trigsteps[axis] = count_position[axis];
+
+  #endif // ! COREXY || COREYX || COREXZ || COREZX
+
+  kill_current_block();
+}
+
 void report_positions() {
   CRITICAL_SECTION_START;
   long xpos = count_position[X_AXIS],
@@ -1403,6 +1178,16 @@ void report_positions() {
   ECHO_E;
 }
 
+void kill_current_block() {
+  step_events_completed = current_block->step_event_count;
+}
+
+float triggered_position_mm(AxisEnum axis) {
+  return endstops_trigsteps[axis] / axis_steps_per_unit[axis];
+}
+
+bool motor_direction(AxisEnum axis) { return TEST(last_direction_bits, axis); }
+
 #if ENABLED(NPR2)
   void colorstep(long csteps,const bool direction) {
     enable_e1();

MK/module/motion/stepper.h

@@ -34,11 +34,14 @@
    * A_AXIS and C_AXIS are used by COREXZ or COREZX printers
    * X_HEAD and Y_HEAD and Z_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots.
    */
-  enum AxisEnum {X_AXIS=0, A_AXIS=0, Y_AXIS=1, B_AXIS=1, Z_AXIS=2, C_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5, Z_HEAD=5};
-  enum EndstopEnum {X_MIN=0, Y_MIN=1, Z_MIN=2, Z_PROBE=3, X_MAX=4, Y_MAX=5, Z_MAX=6, Z2_MIN=7, Z2_MAX=8, E_MIN=9};
-  
+  enum AxisEnum {X_AXIS = 0, A_AXIS = 0, Y_AXIS = 1, B_AXIS = 1, Z_AXIS = 2, C_AXIS = 2, E_AXIS = 3, X_HEAD = 4, Y_HEAD = 5, Z_HEAD = 5};
+
   #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-    extern bool abort_on_endstop_hit;
+    #if ENABLED(ABORT_ON_ENDSTOP_HIT_INIT)
+      bool abort_on_endstop_hit = ABORT_ON_ENDSTOP_HIT_INIT;
+    #else
+      bool abort_on_endstop_hit = false;
+    #endif
   #endif
 
   // Initialize and start the stepper motor subsystem
@@ -69,10 +72,20 @@
   //
   void report_positions();
 
-  void checkHitEndstops(); //call from somewhere to create an serial error message with the locations the endstops where hit, in case they were triggered
-  void endstops_hit_on_purpose(); //avoid creation of the message, i.e. after homing and before a routine call of checkHitEndstops();
+  //
+  // Handle a triggered endstop
+  //
+  void endstop_triggered(AxisEnum axis);
 
-  void enable_endstops(bool check); // Enable/disable endstop checking
+  //
+  // Triggered position of an axis in mm (not core-savvy)
+  //
+  float triggered_position_mm(AxisEnum axis);
+
+  //
+  // The direction of a single motor
+  //
+  bool motor_direction(AxisEnum axis);
 
   void checkStepperErrors(); //Print errors detected by the stepper
 
@@ -91,6 +104,7 @@
   void digipot_current(uint8_t driver, int current);
   void microstep_init();
   void microstep_readings();
+  void kill_current_block();
 
   #if ENABLED(Z_DUAL_ENDSTOPS)
     void In_Homing_Process(bool state);

MK/module/motion/stepper_indirection.h

@@ -232,7 +232,6 @@
     #define NORM_E_DIR()    { switch(current_block->active_driver) { case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; }}
     #define REV_E_DIR()     { switch(current_block->active_driver) { case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; }}
   #else
-    extern bool extruder_duplication_enabled;
     #define E_STEP_WRITE(v) { if(extruder_duplication_enabled) { E0_STEP_WRITE(v); E1_STEP_WRITE(v); } else if(current_block->active_driver == 1) { E1_STEP_WRITE(v); } else { E0_STEP_WRITE(v); }}
     #define NORM_E_DIR()    { if(extruder_duplication_enabled) { E0_DIR_WRITE(!INVERT_E0_DIR); E1_DIR_WRITE(!INVERT_E1_DIR); } else if(current_block->active_driver == 1) { E1_DIR_WRITE(!INVERT_E1_DIR); } else { E0_DIR_WRITE(!INVERT_E0_DIR); }}
     #define REV_E_DIR()     { if(extruder_duplication_enabled) { E0_DIR_WRITE( INVERT_E0_DIR); E1_DIR_WRITE( INVERT_E1_DIR); } else if(current_block->active_driver == 1) { E1_DIR_WRITE( INVERT_E1_DIR); } else { E0_DIR_WRITE( INVERT_E0_DIR); }}

MK/module/sanitycheck.h

@@ -174,8 +174,8 @@
   #endif
 
   // Language
-  #if DISABLED(LANGUAGE_CHOICE)
-    #error DEPENDENCY ERROR: Missing setting LANGUAGE_CHOICE
+  #if DISABLED(LCD_LANGUAGE)
+    #error DEPENDENCY ERROR: Missing setting LCD_LANGUAGE
   #endif
 
   /// FEATURE
@@ -415,6 +415,12 @@
     #endif
   #endif
 
+  /**
+   * Advance Extrusion
+   */
+  #if ENABLED(ADVANCE) && ENABLED(ADVANCE_LPC)
+    #error You can enable ADVANCE or ADVANCE_LPC, but not both.
+  #endif
   #if ENABLED(ADVANCE)
     #if DISABLED(EXTRUDER_ADVANCE_K)
       #error DEPENDENCY ERROR: Missing setting EXTRUDER_ADVANCE_K

MK/module/stopwatch/stopwatch.cpp

@@ -29,29 +29,29 @@ Stopwatch::Stopwatch() {
 
 void Stopwatch::stop() {
   #if ENABLED(DEBUG_STOPWATCH)
-    debug(PSTR("stop"));
+    Stopwatch::debug(PSTR("stop"));
   #endif
 
   if (!this->isRunning()) return;
 
-  this->status = STPWTCH_STOPPED;
+  this->state = STPWTCH_STOPPED;
   this->stopTimestamp = millis();
 }
 
 void Stopwatch::pause() {
   #if ENABLED(DEBUG_STOPWATCH)
-    debug(PSTR("pause"));
+    Stopwatch::debug(PSTR("pause"));
   #endif
 
   if (!this->isRunning()) return;
 
-  this->status = STPWTCH_PAUSED;
+  this->state = STPWTCH_PAUSED;
   this->stopTimestamp = millis();
 }
 
 void Stopwatch::start() {
   #if ENABLED(DEBUG_STOPWATCH)
-    debug(PSTR("start"));
+    Stopwatch::debug(PSTR("start"));
   #endif
 
   if (this->isRunning()) return;
@@ -59,27 +59,27 @@ void Stopwatch::start() {
   if (this->isPaused()) this->accumulator = this->duration();
   else this->reset();
 
-  this->status = STPWTCH_RUNNING;
+  this->state = STPWTCH_RUNNING;
   this->startTimestamp = millis();
 }
 
 void Stopwatch::reset() {
   #if ENABLED(DEBUG_STOPWATCH)
-    debug(PSTR("reset"));
+    Stopwatch::debug(PSTR("reset"));
   #endif
 
-  this->status = STPWTCH_STOPPED;
+  this->state = STPWTCH_STOPPED;
   this->startTimestamp = 0;
   this->stopTimestamp = 0;
   this->accumulator = 0;
 }
 
 bool Stopwatch::isRunning() {
-  return (this->status == STPWTCH_RUNNING) ? true : false;
+  return (this->state == STPWTCH_RUNNING) ? true : false;
 }
 
 bool Stopwatch::isPaused() {
-  return (this->status == STPWTCH_PAUSED) ? true : false;
+  return (this->state == STPWTCH_PAUSED) ? true : false;
 }
 
 uint16_t Stopwatch::duration() {

MK/module/stopwatch/stopwatch.h

@@ -26,7 +26,7 @@
   // Print debug messages with M111 S2 (Uses 156 bytes of PROGMEM)
   //#define DEBUG_STOPWATCH
 
-  enum StopwatchStatus {
+  enum StopwatchState {
     STPWTCH_STOPPED,
     STPWTCH_RUNNING,
     STPWTCH_PAUSED
@@ -39,7 +39,7 @@
    */
   class Stopwatch {
     private:
-      StopwatchStatus status;
+      StopwatchState state;
       uint16_t accumulator;
       uint32_t startTimestamp;
       uint32_t stopTimestamp;

MK/module/temperature/temperature.cpp

@@ -113,8 +113,8 @@ unsigned char soft_pwm_cooler;
   enum TRState { TRReset, TRInactive, TRFirstRunnig, TRStable, TRRunaway };
   void thermal_runaway_protection(TRState* state, millis_t* timer, float temperature, float target_temperature, int temp_controller_id, int period_seconds, int hysteresis_degc);
   #if ENABLED(THERMAL_PROTECTION_HOTENDS)
-    static TRState thermal_runaway_state_machine[4] = { TRReset, TRReset, TRReset, TRReset };
-    static millis_t thermal_runaway_timer[4]; // = {0,0,0,0};
+    static TRState thermal_runaway_state_machine[HOTENDS] = { TRReset };
+    static millis_t thermal_runaway_timer[HOTENDS] = { 0 };
   #endif
   #if ENABLED(THERMAL_PROTECTION_BED) && TEMP_SENSOR_BED != 0
     static TRState thermal_runaway_bed_state_machine = TRReset;
@@ -125,6 +125,7 @@ unsigned char soft_pwm_cooler;
     static millis_t thermal_runaway_cooler_timer;
   #endif
 #endif
+
 #if HAS(POWER_CONSUMPTION_SENSOR)
   int current_raw_powconsumption = 0;  //Holds measured power consumption
   static unsigned long raw_powconsumption_value = 0;
@@ -245,6 +246,11 @@ static void updateTemperaturesFromRawValues();
   millis_t watch_cooler_next_ms = 0;
 #endif
 
+#if ENABLED(THERMAL_PROTECTION_BED)
+  int watch_target_bed_temp = 0;
+  millis_t watch_bed_next_ms = 0;
+#endif
+
 #if DISABLED(SOFT_PWM_SCALE)
   #define SOFT_PWM_SCALE 0
 #endif
@@ -346,12 +352,12 @@ void autotempShutdown() {
         #if HAS(AUTO_FAN)
           if (ms > next_auto_fan_check_ms) {
             checkExtruderAutoFans();
-            next_auto_fan_check_ms = ms + 2500;
+            next_auto_fan_check_ms = ms + 2500UL;
           }
         #endif
 
         if (running && ((input > temp && temp_controller >= -1) || (input < temp && temp_controller < -1))) {
-          if (ms > t2 + 5000) {
+          if (ms > t2 + 5000UL) {
             runnig = false;
 				if (temp_controller < -1)
 				  soft_pwm_cooler = (bias - d) >> 1;
@@ -369,7 +375,7 @@ void autotempShutdown() {
         }
 
         if (!running && ((input < temp && temp_controller >= -1) || (input > temp && temp_controller < -1))) {
-          if (ms > t1 + 5000) {
+          if (ms > t1 + 5000UL) {
             running = true;
             t2 = ms;
             t_low = t2 - t1;
@@ -833,7 +839,7 @@ void manage_temp_controller() {
     if (ct < max(HEATER_0_MINTEMP, 0.01)) min_temp_error(0);
   #endif
 
-  #if ENABLED(THERMAL_PROTECTION_HOTENDS) || DISABLED(PIDTEMPBED) || DISABLED(PIDTEMPCOOLER) || HAS(AUTO_FAN)
+  #if ENABLED(THERMAL_PROTECTION_HOTENDS) || ENABLED(THERMAL_PROTECTION_BED) || ENABLED(THERMAL_PROTECTION_COOLER) || DISABLED(PIDTEMPBED) || DISABLED(PIDTEMPCOOLER) || HAS(AUTO_FAN)
     millis_t ms = millis();
   #endif
 
@@ -867,9 +873,27 @@ void manage_temp_controller() {
 
     #endif // THERMAL_PROTECTION_HOTENDS
 
+    // Check if the temperature is failing to increase
+    #if ENABLED(THERMAL_PROTECTION_BED)
+
+      // Is it time to check the bed?
+      if (watch_bed_next_ms && ELAPSED(ms, watch_bed_next_ms)) {
+        // Has it failed to increase enough?
+        if (degBed() < watch_target_bed_temp) {
+          // Stop!
+          _temp_error(-1, PSTR(SERIAL_T_HEATING_FAILED), PSTR(MSG_HEATING_FAILED_LCD));
+        }
+        else {
+          // Start again if the target is still far off
+          start_watching_bed();
+        }
+      }
+
+    #endif // THERMAL_PROTECTION_HOTENDS
+
     #if ENABLED(TEMP_SENSOR_1_AS_REDUNDANT)
       if (fabs(current_temperature[0] - redundant_temperature) > MAX_REDUNDANT_TEMP_SENSOR_DIFF) {
-        _temp_error(0, PSTR(MSG_EXTRUDER_SWITCHED_OFF), PSTR(MSG_ERR_REDUNDANT_TEMP));
+        _temp_error(0, PSTR(SERIAL_REDUNDANCY), PSTR(MSG_ERR_REDUNDANT_TEMP));
       }
     #endif
 
@@ -930,7 +954,7 @@ void manage_temp_controller() {
         soft_pwm_bed = 0;
         WRITE_HEATER_BED(LOW);
       }
-    #else // BED_LIMIT_SWITCHING
+    #else // !PIDTEMPBED && !BED_LIMIT_SWITCHING
       // Check if temperature is within the correct range
       if (current_temperature_bed > BED_MINTEMP && current_temperature_bed < BED_MAXTEMP) {
         soft_pwm_bed = current_temperature_bed < target_temperature_bed ? MAX_BED_POWER >> 1 : 0;
@@ -1487,6 +1511,22 @@ void tp_init() {
   }
 #endif
 
+#if ENABLED(THERMAL_PROTECTION_BED)
+  /**
+   * Start Heating Sanity Check for bed that are below
+   * their target temperature by a configurable margin.
+   * This is called when the temperature is set. (M140, M190)
+   */
+  void start_watching_bed() {
+    if (degBed() < degTargetBed() - (WATCH_BED_TEMP_INCREASE + TEMP_BED_HYSTERESIS + 1)) {
+      watch_target_bed_temp = degBed() + WATCH_BED_TEMP_INCREASE;
+      watch_bed_next_ms = millis() + (WATCH_BED_TEMP_PERIOD) * 1000UL;
+    }
+    else
+      watch_bed_next_ms = 0;
+  }
+#endif
+
 
 #if ENABLED(THERMAL_PROTECTION_HOTENDS) || ENABLED(THERMAL_PROTECTION_BED) || ENABLED(THERMAL_PROTECTION_COOLER) 
 
@@ -1538,14 +1578,14 @@ void tp_init() {
         	 if (temperature >= tr_target_temperature[temp_controller_index] - hysteresis_degc)
             *timer = millis();
           // If the timer goes too long without a reset, trigger shutdown
-          else if (millis() > *timer + period_seconds * 1000UL)
+          else if (ELAPSED(millis() > *timer + period_seconds * 1000UL))
             *state = TRRunaway;
         }
         else { // COOLERS
 		    if (temperature <= tr_target_temperature[temp_controller_index] + hysteresis_degc)
 			   *timer = millis();
           // If the timer goes too long without a reset, trigger shutdown
-          else if (millis() > *timer + period_seconds * 1000UL)
+          else if (ELAPSED(millis() > *timer + period_seconds * 1000UL))
             *state = TRRunaway;
         }
         break;
@@ -1561,6 +1601,9 @@ void disable_all_heaters() {
   for (int i = 0; i < HOTENDS; i++) setTargetHotend(0, i);
   setTargetBed(0);
 
+  // If all heaters go down then for sure our print job has stopped
+  print_job_timer.stop();
+
   #define DISABLE_HEATER(NR) { \
     target_temperature[NR] = 0; \
     soft_pwm[NR] = 0; \
@@ -1607,59 +1650,61 @@ void disable_all_coolers() {
 }
 
 #if ENABLED(HEATER_0_USES_MAX6675)
+
   #define MAX6675_HEAT_INTERVAL 250u
-  static millis_t next_max6675_ms = 0;
-  int max6675_temp = 2000;
 
-  static int read_max6675() {
+  #if ENABLED(MAX6675_IS_MAX31855)
+    uint32_t max6675_temp = 2000;
+    #define MAX6675_ERROR_MASK 7
+    #define MAX6675_DISCARD_BITS 18
+  #else
+    uint16_t max6675_temp = 2000;
+    #define MAX6675_ERROR_MASK 4
+    #define MAX6675_DISCARD_BITS 3
+  #endif
 
-    millis_t ms = millis();
+  int Temperature::read_max6675() {
 
-    if (ms < next_max6675_ms)
-      return max6675_temp;
+    static millis_t next_max6675_ms = 0;
 
-    next_max6675_ms = ms + MAX6675_HEAT_INTERVAL;
+    millis_t ms = millis();
 
-    max6675_temp = 0;
+    if (PENDING(ms, next_max6675_ms)) return (int)max6675_temp;
 
-    #ifdef PRR
-      CBI(PRR, PRSPI);
-    #elif defined(PRR0)
-      CBI(PRR0, PRSPI);
-    #endif
+    next_max6675_ms = ms + MAX6675_HEAT_INTERVAL;
 
+    CBI(
+      #ifdef PRR
+        PRR
+      #elif defined(PRR0)
+        PRR0
+      #endif
+        , PRSPI);
     SPCR = _BV(MSTR) | _BV(SPE) | _BV(SPR0);
 
-    // enable TT_MAX6675
-    WRITE(MAX6675_SS, 0);
+    WRITE(MAX6675_SS, 0); // enable TT_MAX6675
 
     // ensure 100ns delay - a bit extra is fine
-    asm("nop"); // 50ns on 20Mhz, 62.5ns on 16Mhz
-    asm("nop"); // 50ns on 20Mhz, 62.5ns on 16Mhz
-
-    // read MSB
-    SPDR = 0;
-    for (; !TEST(SPSR, SPIF););
-    max6675_temp = SPDR;
-    max6675_temp <<= 8;
-
-    // read LSB
-    SPDR = 0;
-    for (; !TEST(SPSR, SPIF););
-    max6675_temp |= SPDR;
-
-    // disable TT_MAX6675
-    WRITE(MAX6675_SS, 1);
-
-    if (max6675_temp & 4) {
-      // thermocouple open
-      max6675_temp = 4000;
-    }
-    else {
-      max6675_temp = max6675_temp >> 3;
+    asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
+    asm("nop");//50ns on 20Mhz, 62.5ns on 16Mhz
+
+    // Read a big-endian temperature value
+    max6675_temp = 0;
+    for (uint8_t i = sizeof(max6675_temp); i--;) {
+      SPDR = 0;
+      for (;!TEST(SPSR, SPIF););
+      max6675_temp |= SPDR;
+      if (i > 0) max6675_temp <<= 8; // shift left if not the last byte
     }
 
-    return max6675_temp;
+    WRITE(MAX6675_SS, 1); // disable TT_MAX6675
+
+    if (max6675_temp & MAX6675_ERROR_MASK)
+      max6675_temp = 4000; // thermocouple open
+    else
+      max6675_temp >>= MAX6675_DISCARD_BITS;
+
+    return (int)max6675_temp;
   }
 
 #endif // HEATER_0_USES_MAX6675

MK/module/temperature/temperature.h

@@ -138,7 +138,11 @@ FORCE_INLINE float degTargetCooler() { return target_temperature_cooler; }
 #endif
 
 #if ENABLED(THERMAL_PROTECTION_COOLERS)
-  void start_watching_cooler(void);
+  void start_watching_cooler();
+#endif
+
+#if ENABLED(THERMAL_PROTECTION_BED)
+  void start_watching_bed();
 #endif
 
 FORCE_INLINE void setTargetHotend(const float& celsius, uint8_t hotend) {
@@ -147,8 +151,21 @@ FORCE_INLINE void setTargetHotend(const float& celsius, uint8_t hotend) {
     start_watching_heater(HOTEND_ARG);
   #endif
 }
-FORCE_INLINE void setTargetBed(const float& celsius) { target_temperature_bed = celsius; }
-FORCE_INLINE void setTargetCooler(const float& celsius) { target_temperature_cooler = celsius; }
+
+FORCE_INLINE void setTargetBed(const float& celsius) {
+  target_temperature_bed = celsius;
+  #if ENABLED(THERMAL_PROTECTION_BED)
+    start_watching_bed();
+  #endif
+}
+
+FORCE_INLINE void setTargetCooler(const float& celsius) {
+  target_temperature_cooler = celsius;
+  #if ENABLED(THERMAL_PROTECTION_COOLER) && ENABLED(THERMAL_PROTECTION_COOLER_WATCHDOG)
+    start_watching_cooler();
+  #endif
+}
+
 
 FORCE_INLINE bool isHeatingHotend(uint8_t hotend) { return target_temperature[HOTEND_ARG] > current_temperature[HOTEND_ARG]; }
 FORCE_INLINE bool isHeatingBed() { return target_temperature_bed > current_temperature_bed; }

README.md

 <img align="right" src="Documentation/Logo/MarlinKimbra%20Logo%20GitHub.png" />
 # MarlinKimbra 3D Printer Firmware for Arduino
-## Version 4.2.8 dev
+## Version 4.2.81 dev
 
 ### Special thanks
 * all Marlin8bit-developers.