Browse Source

Merge branch 'MK3' into MK3_NEW_SD_COMPILATION

Alex Voinea 4 years ago
parent
commit
53e130fc6d

+ 3 - 3
Firmware/Configuration.h

@@ -16,8 +16,8 @@ extern uint16_t nPrinterType;
 extern PGM_P sPrinterName;
 
 // Firmware version
-#define FW_VERSION "3.8.1"
-#define FW_COMMIT_NR   2869
+#define FW_VERSION "3.9.0-RC1"
+#define FW_COMMIT_NR   3272
 // FW_VERSION_UNKNOWN means this is an unofficial build.
 // The firmware should only be checked into github with this symbol.
 #define FW_DEV_VERSION FW_VERSION_UNKNOWN
@@ -424,7 +424,7 @@ your extruder heater takes 2 minutes to hit the target on heating.
 #define DEFAULT_XJERK                10       // (mm/sec)
 #define DEFAULT_YJERK                10       // (mm/sec)
 #define DEFAULT_ZJERK                 0.4     // (mm/sec)
-#define DEFAULT_EJERK                 2.5     // (mm/sec)
+#define DEFAULT_EJERK                 4.5     // (mm/sec)
 
 //===========================================================================
 //=============================Additional Features===========================

+ 2 - 2
Firmware/ConfigurationStore.cpp

@@ -165,8 +165,8 @@ void Config_PrintSettings(uint8_t level)
 #endif
 	if (level >= 10) {
 #ifdef LIN_ADVANCE
-		printf_P(PSTR("%SLinear advance settings:\n   M900 K%.2f   E/D = %.2f\n"),
-			echomagic, extruder_advance_k, advance_ed_ratio);
+		printf_P(PSTR("%SLinear advance settings:%S   M900 K%.2f\n"),
+                 echomagic, echomagic, extruder_advance_K);
 #endif //LIN_ADVANCE
 	}
 }

+ 20 - 34
Firmware/Configuration_adv.h

@@ -270,43 +270,29 @@
 #endif
 
 /**
-    * Implementation of linear pressure control
-    *
-    * Assumption: advance = k * (delta velocity)
-    * K=0 means advance disabled.
-    * See Marlin documentation for calibration instructions.
-    */
+ * Linear Pressure Control v1.5
+ *
+ * Assumption: advance [steps] = k * (delta velocity [steps/s])
+ * K=0 means advance disabled.
+ *
+ * NOTE: K values for LIN_ADVANCE 1.5 differs from earlier versions!
+ *
+ * Set K around 0.22 for 3mm PLA Direct Drive with ~6.5cm between the drive gear and heatbreak.
+ * Larger K values will be needed for flexible filament and greater distances.
+ * If this algorithm produces a higher speed offset than the extruder can handle (compared to E jerk)
+ * print acceleration will be reduced during the affected moves to keep within the limit.
+ *
+ * See http://marlinfw.org/docs/features/lin_advance.html for full instructions.
+ * Mention @Sebastianv650 on GitHub to alert the author of any issues.
+ */
 #define LIN_ADVANCE
 
 #ifdef LIN_ADVANCE
-  #define LIN_ADVANCE_K 0 //Try around 45 for PLA, around 25 for ABS.
-
- /**
-        * Some Slicers produce Gcode with randomly jumping extrusion widths occasionally.
-        * For example within a 0.4mm perimeter it may produce a single segment of 0.05mm width.
-        * While this is harmless for normal printing (the fluid nature of the filament will
-        * close this very, very tiny gap), it throws off the LIN_ADVANCE pressure adaption.
-        *
-        * For this case LIN_ADVANCE_E_D_RATIO can be used to set the extrusion:distance ratio
-        * to a fixed value. Note that using a fixed ratio will lead to wrong nozzle pressures
-        * if the slicer is using variable widths or layer heights within one print!
-        *
-        * This option sets the default E:D ratio at startup. Use `M900` to override this value.
-        *
-        * Example: `M900 W0.4 H0.2 D1.75`, where:
-        *   - W is the extrusion width in mm
-        *   - H is the layer height in mm
-        *   - D is the filament diameter in mm
-        *
-        * Example: `M900 R0.0458` to set the ratio directly.
-        *
-        * Set to 0 to auto-detect the ratio based on given Gcode G1 print moves.
-        *
-        * Slic3r (including Prusa Slic3r) produces Gcode compatible with the automatic mode.
-        * Cura (as of this writing) may produce Gcode incompatible with the automatic mode.
-        */
-#define LIN_ADVANCE_E_D_RATIO 0 // The calculated ratio (or 0) according to the formula W * H / ((D / 2) ^ 2 * PI)
-                                // Example: 0.4 * 0.2 / ((1.75 / 2) ^ 2 * PI) = 0.033260135
+  #define LIN_ADVANCE_K 0  // Unit: mm compression per 1mm/s extruder speed
+  //#define LA_NOCOMPAT    // Disable Linear Advance 1.0 compatibility
+  //#define LA_LIVE_K      // Allow adjusting K in the Tune menu
+  //#define LA_DEBUG       // If enabled, this will generate debug information output over USB.
+  //#define LA_DEBUG_LOGIC // @wavexx: setup logic channels for isr debugging
 #endif
 
 // Arc interpretation settings:

+ 144 - 0
Firmware/Dcodes.cpp

@@ -99,6 +99,18 @@ void print_mem(uint32_t address, uint16_t count, uint8_t type, uint8_t countperl
 
 #ifdef DEBUG_DCODE3
 #define EEPROM_SIZE 0x1000
+    /*!
+    *
+    ### D3 - Read/Write EEPROM <a href="https://reprap.org/wiki/G-code#D3:_Read.2FWrite_EEPROM">D3: Read/Write EEPROM</a>
+    This command can be used without any additional parameters. It will read the entire eeprom.
+      
+          D3 [ A | C | X ]
+      
+      - `A` - Address (0x0000-0x0fff)
+      - `C` - Count (0x0001-0x1000)
+      - `X` - Data
+    *
+    */
 void dcode_3()
 {
 	DBG(_N("D3 - Read/Write EEPROM\n"));
@@ -176,6 +188,14 @@ extern float axis_steps_per_unit[NUM_AXIS];
 #endif //0
 #define LOG(args...)
 
+    /*!
+    *
+    ### D-1 - Endless Loop <a href="https://reprap.org/wiki/G-code#G28:_Move_to_Origin_.28Home.29">D-1: Endless Loop</a>
+      
+          D-1
+      
+    *
+    */
 void dcode__1()
 {
 	printf_P(PSTR("D-1 - Endless loop\n"));
@@ -185,6 +205,15 @@ void dcode__1()
 
 #ifdef DEBUG_DCODES
 
+    /*!
+    *
+    ### D0 - Reset <a href="https://reprap.org/wiki/G-code#D0:_Reset">D0: Reset</a>
+      
+          D0 [ B ]
+      
+      - `B` - Bootloader
+    *
+    */
 void dcode_0()
 {
 	if (*(strchr_pointer + 1) == 0) return;
@@ -203,6 +232,14 @@ void dcode_0()
 	}
 }
 
+    /*!
+    *
+    ### D1 - Clear EEPROM and RESET <a href="https://reprap.org/wiki/G-code#D1:_Clear_EEPROM_and_RESET">D1: Clear EEPROM and RESET</a>
+      
+          D1
+      
+    *
+    */
 void dcode_1()
 {
 	LOG("D1 - Clear EEPROM and RESET\n");
@@ -213,6 +250,18 @@ void dcode_1()
 	while(1);
 }
 
+    /*!
+    *
+    ### D2 - Read/Write RAM <a href="https://reprap.org/wiki/G-code#D2:_Read.2FWrite_RAM">D2: Read/Write RAM</a>
+    This command can be used without any additional parameters. It will read the entire RAM.
+      
+          D2 [ A | C | X ]
+      
+      - `A` - Address (0x0000-0x1fff)
+      - `C` - Count (0x0001-0x2000)
+      - `X` - Data
+    *
+    */
 void dcode_2()
 {
 	LOG("D2 - Read/Write RAM\n");
@@ -256,6 +305,19 @@ void dcode_2()
 	}*/
 }
 
+    /*!
+    *
+    ### D4 - Read/Write PIN <a href="https://reprap.org/wiki/G-code#D4:_Read.2FWrite_PIN">D4: Read/Write PIN</a>
+    
+    To read the digital value of a pin you need only to define the pin number.
+      
+          D4 [ P | F | V ]
+      
+      - `P` - Pin (0-255)
+      - `F` - Function in/out (0/1)
+      - `V` - Value (0/1)
+    *
+    */
 void dcode_4()
 {
 	LOG("D4 - Read/Write PIN\n");
@@ -288,6 +350,19 @@ void dcode_4()
 
 #ifdef DEBUG_DCODE5
 
+    /*!
+    *
+    ### D5 - Read/Write FLASH <a href="https://reprap.org/wiki/G-code#D5:_Read.2FWrite_FLASH">D5: Read/Write Flash</a>
+    This command can be used without any additional parameters. It will read the 1kb FLASH.
+      
+          D5 [ A | C | X | E ]
+      
+      - `A` - Address (0x00000-0x3ffff)
+      - `C` - Count (0x0001-0x2000)
+      - `X` - Data
+      - `E` - Erase
+    *
+    */
 void dcode_5()
 {
 	printf_P(PSTR("D5 - Read/Write FLASH\n"));
@@ -351,11 +426,25 @@ void dcode_5()
 
 #ifdef DEBUG_DCODES
 
+    /*!
+    *
+    ### D6 - Read/Write external FLASH <a href="https://reprap.org/wiki/G-code#D6:_Read.2FWrite_external_FLASH">D6: Read/Write external Flash</a>
+    
+    Reserved
+   *
+   */
 void dcode_6()
 {
 	LOG("D6 - Read/Write external FLASH\n");
 }
 
+    /*!
+    *
+    ### D7 - Read/Write Bootloader <a href="https://reprap.org/wiki/G-code#D7:_Read.2FWrite_Bootloader">D7: Read/Write Bootloader</a>
+    
+    Reserved
+   *
+   */
 void dcode_7()
 {
 	LOG("D7 - Read/Write Bootloader\n");
@@ -371,6 +460,18 @@ void dcode_7()
 */
 }
 
+    /*!
+    *
+    ### D8 - Read/Write PINDA <a href="https://reprap.org/wiki/G-code#D8:_Read.2FWrite_PINDA">D8: Read/Write PINDA</a>
+      
+          D8 [ ? | ! | P | Z ]
+      
+      - `?` - Read PINDA temperature shift values
+      - `!` - Reset PINDA temperature shift values to default
+      - `P` - Pinda temperature [C]
+      - `Z` - Z Offset [mm]
+    *
+    */
 void dcode_8()
 {
 	printf_P(PSTR("D8 - Read/Write PINDA\n"));
@@ -412,6 +513,23 @@ void dcode_8()
 	printf_P(PSTR("temp_pinda=%d offset_z=%d.%03d\n"), (int)temp_pinda, (int)offset_z, ((int)(1000 * offset_z) % 1000));
 }
 
+    /*!
+    *
+    ### D9 - Read ADC <a href="https://reprap.org/wiki/G-code#D9:_Read.2FWrite_ADC">D9: Read ADC</a>
+      
+          D9 [ I | V ]
+      
+      - `I` - ADC channel index 
+         - `0` - Heater 0 temperature
+         - `1` - Heater 1 temperature
+         - `2` - Bed temperature
+         - `3` - PINDA temperature
+         - `4` - PWR voltage
+         - `5` - Ambient temperature
+         - `6` - BED voltage
+      - `V` Value to be written as simulated
+    *
+    */
 const char* dcode_9_ADC_name(uint8_t i)
 {
 	switch (i)
@@ -485,12 +603,24 @@ void dcode_9()
 	}
 }
 
+    /*!
+    *
+    ### D10 - Set XYZ calibration = OK <a href="https://reprap.org/wiki/G-code#D10:_Set_XYZ_calibration_.3D_OK">D10: Set XYZ calibration = OK</a>
+    
+   *
+   */
 void dcode_10()
 {//Tell the printer that XYZ calibration went OK
 	LOG("D10 - XYZ calibration = OK\n");
 	calibration_status_store(CALIBRATION_STATUS_LIVE_ADJUST); 
 }
 
+    /*!
+    *
+    ### D12 - Time <a href="https://reprap.org/wiki/G-code#D12:_Time">D12: Time</a>
+    
+   *
+   */
 void dcode_12()
 {//Time
 	LOG("D12 - Time\n");
@@ -636,6 +766,20 @@ void dcode_2130()
 #endif //TMC2130
 
 #ifdef PAT9125
+    /*!
+    *
+    ### D9125 - PAT9125 filament sensor <a href="https://reprap.org/wiki/G-code#D9:_Read.2FWrite_ADC">D9125: PAT9125 filament sensor</a>
+      
+          D9125 [ ? | ! | R | X | Y | L ]
+      
+      - `?` - Print values
+      - `!` - Print values
+      - `R` - Resolution. Not active in code
+      - `X` - X values
+      - `Y` - Y values
+      - `L` - Activate filament sensor log
+    *
+    */
 void dcode_9125()
 {
 	LOG("D9125 - PAT9125\n");

+ 20 - 21
Firmware/Marlin.h

@@ -146,40 +146,39 @@ void manage_inactivity(bool ignore_stepper_queue=false);
 #if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1 
 	#if defined(Z_AXIS_ALWAYS_ON)
 		  #ifdef Z_DUAL_STEPPER_DRIVERS
-			#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
-			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+			#define  poweron_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
+			#define poweroff_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
 		  #else
-			#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
-			#define  disable_z() {}
+			#define  poweron_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
+			#define poweroff_z() {}
 		  #endif
 	#else
 		#ifdef Z_DUAL_STEPPER_DRIVERS
-			#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
-			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+			#define  poweron_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
+			#define poweroff_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
 		#else
-			#define  enable_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
-			#define disable_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
+			#define  poweron_z() WRITE(Z_ENABLE_PIN, Z_ENABLE_ON)
+			#define poweroff_z() { WRITE(Z_ENABLE_PIN,!Z_ENABLE_ON); axis_known_position[Z_AXIS] = false; }
 		#endif
 	#endif
 #else
-  #define enable_z() {}
-  #define disable_z() {}
+    #define  poweron_z() {}
+    #define poweroff_z() {}
 #endif
 
-#ifdef PSU_Delta
+#ifndef PSU_Delta
+    #define  enable_z()  poweron_z()
+    #define disable_z() poweroff_z()
+#else
     void init_force_z();
     void check_force_z();
-    #undef disable_z
-    #define disable_z() disable_force_z()
-    void disable_force_z();
-    #undef enable_z
-    #define enable_z() enable_force_z()
     void enable_force_z();
+    void disable_force_z();
+    #define  enable_z()  enable_force_z()
+    #define disable_z() disable_force_z()
 #endif // PSU_Delta
 
 
-
-
 //#if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
 //#ifdef Z_DUAL_STEPPER_DRIVERS
 //#define  enable_z() { WRITE(Z_ENABLE_PIN, Z_ENABLE_ON); WRITE(Z2_ENABLE_PIN, Z_ENABLE_ON); }
@@ -308,6 +307,7 @@ extern float max_pos[3];
 extern bool axis_known_position[3];
 extern int fanSpeed;
 extern int8_t lcd_change_fil_state;
+extern float default_retraction;
 
 #ifdef TMC2130
 void homeaxis(int axis, uint8_t cnt = 1, uint8_t* pstep = 0);
@@ -444,9 +444,8 @@ void setup_uvlo_interrupt();
 void setup_fan_interrupt();
 #endif
 
-//extern void recover_machine_state_after_power_panic();
-extern void recover_machine_state_after_power_panic(bool bTiny);
-extern void restore_print_from_eeprom();
+extern bool recover_machine_state_after_power_panic();
+extern void restore_print_from_eeprom(bool mbl_was_active);
 extern void position_menu();
 
 extern void print_world_coordinates();

+ 1791 - 844
Firmware/Marlin_main.cpp

@@ -83,6 +83,9 @@
 #include "Dcodes.h"
 #include "AutoDeplete.h"
 
+#ifndef LA_NOCOMPAT
+#include "la10compat.h"
+#endif
 
 #ifdef SWSPI
 #include "swspi.h"
@@ -357,9 +360,6 @@ unsigned long starttime=0;
 unsigned long stoptime=0;
 unsigned long _usb_timer = 0;
 
-bool extruder_under_pressure = true;
-
-
 bool Stopped=false;
 
 #if NUM_SERVOS > 0
@@ -384,7 +384,6 @@ static uint16_t saved_feedrate2 = 0; //!< Default feedrate (truncated from float
 static int saved_feedmultiply2 = 0;
 static uint8_t saved_active_extruder = 0;
 static float saved_extruder_temperature = 0.0; //!< Active extruder temperature
-static bool saved_extruder_under_pressure = false;
 static bool saved_extruder_relative_mode = false;
 static int saved_fanSpeed = 0; //!< Print fan speed
 //! @}
@@ -641,6 +640,9 @@ void failstats_reset_print()
 	eeprom_update_byte((uint8_t *)EEPROM_POWER_COUNT, 0);
 	eeprom_update_byte((uint8_t *)EEPROM_MMU_FAIL, 0);
 	eeprom_update_byte((uint8_t *)EEPROM_MMU_LOAD_FAIL, 0);
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+    fsensor_softfail = 0;
+#endif
 }
 
 
@@ -1299,10 +1301,6 @@ void setup()
 
 	st_init();    // Initialize stepper, this enables interrupts!
   
-#ifdef UVLO_SUPPORT
-    setup_uvlo_interrupt();
-#endif //UVLO_SUPPORT
-
 #ifdef TMC2130
 	tmc2130_mode = silentMode?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
 	update_mode_profile();
@@ -1328,29 +1326,12 @@ void setup()
 	SET_OUTPUT(CONTROLLERFAN_PIN); //Set pin used for driver cooling fan
 #endif
 
-
 	setup_homepin();
 
-#ifdef TMC2130
-
-  if (1) {
-    // try to run to zero phase before powering the Z motor.    
-    // Move in negative direction
-    WRITE(Z_DIR_PIN,INVERT_Z_DIR);
-    // Round the current micro-micro steps to micro steps.
-    for (uint16_t phase = (tmc2130_rd_MSCNT(Z_AXIS) + 8) >> 4; phase > 0; -- phase) {
-      // Until the phase counter is reset to zero.
-      WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
-      _delay(2);
-      WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
-      _delay(2);
-    }
-  }
-#endif //TMC2130
-
-#if defined(Z_AXIS_ALWAYS_ON) && !defined(PSU_Delta)
-	enable_z();
+#if defined(Z_AXIS_ALWAYS_ON)
+    enable_z();
 #endif
+
 	farm_mode = eeprom_read_byte((uint8_t*)EEPROM_FARM_MODE);
 	EEPROM_read_B(EEPROM_FARM_NUMBER, &farm_no);
 	if ((farm_mode == 0xFF && farm_no == 0) || (farm_no == static_cast<int>(0xFFFF))) farm_mode = false; //if farm_mode has not been stored to eeprom yet and farm number is set to zero or EEPROM is fresh, deactivate farm mode
@@ -1614,12 +1595,14 @@ void setup()
               lcd_update(2); 
               lcd_setstatuspgm(_T(WELCOME_MSG)); 
           } 
-           
       }
-
-	   
   }
+
+  // Only arm the uvlo interrupt _after_ a recovering print has been initialized and
+  // the entire state machine initialized.
+  setup_uvlo_interrupt();
 #endif //UVLO_SUPPORT
+
   fCheckModeInit();
   fSetMmuMode(mmu_enabled);
   KEEPALIVE_STATE(NOT_BUSY);
@@ -2062,35 +2045,36 @@ static float probe_pt(float x, float y, float z_before) {
 
 #ifdef LIN_ADVANCE
    /**
-    * M900: Set and/or Get advance K factor and WH/D ratio
+    * M900: Set and/or Get advance K factor
     *
     *  K<factor>                  Set advance K factor
-    *  R<ratio>                   Set ratio directly (overrides WH/D)
-    *  W<width> H<height> D<diam> Set ratio from WH/D
     */
 inline void gcode_M900() {
-    st_synchronize();
-    
-    const float newK = code_seen('K') ? code_value_float() : -1;
-    if (newK >= 0) extruder_advance_k = newK;
-    
-    float newR = code_seen('R') ? code_value_float() : -1;
-    if (newR < 0) {
-        const float newD = code_seen('D') ? code_value_float() : -1,
-        newW = code_seen('W') ? code_value_float() : -1,
-        newH = code_seen('H') ? code_value_float() : -1;
-        if (newD >= 0 && newW >= 0 && newH >= 0)
-            newR = newD ? (newW * newH) / (sq(newD * 0.5) * M_PI) : 0;
+    float newK = code_seen('K') ? code_value_float() : -2;
+#ifdef LA_NOCOMPAT
+    if (newK >= 0 && newK < 10)
+        extruder_advance_K = newK;
+    else
+        SERIAL_ECHOLNPGM("K out of allowed range!");
+#else
+    if (newK == 0)
+        extruder_advance_K = 0;
+    else if (newK == -1)
+        la10c_reset();
+    else
+    {
+        newK = la10c_value(newK);
+        if (newK < 0)
+            SERIAL_ECHOLNPGM("K out of allowed range!");
+        else
+            extruder_advance_K = newK;
     }
-    if (newR >= 0) advance_ed_ratio = newR;
-    
+#endif
+
     SERIAL_ECHO_START;
     SERIAL_ECHOPGM("Advance K=");
-    SERIAL_ECHOLN(extruder_advance_k);
-    SERIAL_ECHOPGM(" E/D=");
-    const float ratio = advance_ed_ratio;
-    if (ratio) SERIAL_ECHOLN(ratio); else SERIAL_ECHOLNPGM("Auto");
-    }
+    SERIAL_ECHOLN(extruder_advance_K);
+}
 #endif // LIN_ADVANCE
 
 bool check_commands() {
@@ -3356,6 +3340,49 @@ static void gcode_PRUSA_BadRAMBoFanTest(){
 #endif
 }
 
+
+// G92 - Set current position to coordinates given
+static void gcode_G92()
+{
+    bool codes[NUM_AXIS];
+    float values[NUM_AXIS];
+
+    // Check which axes need to be set
+    for(uint8_t i = 0; i < NUM_AXIS; ++i)
+    {
+        codes[i] = code_seen(axis_codes[i]);
+        if(codes[i])
+            values[i] = code_value();
+    }
+
+    if((codes[E_AXIS] && values[E_AXIS] == 0) &&
+       (!codes[X_AXIS] && !codes[Y_AXIS] && !codes[Z_AXIS]))
+    {
+        // As a special optimization, when _just_ clearing the E position
+        // we schedule a flag asynchronously along with the next block to
+        // reset the starting E position instead of stopping the planner
+        current_position[E_AXIS] = 0;
+        plan_reset_next_e();
+    }
+    else
+    {
+        // In any other case we're forced to synchronize
+        st_synchronize();
+        for(uint8_t i = 0; i < 3; ++i)
+        {
+            if(codes[i])
+                current_position[i] = values[i] + cs.add_homing[i];
+        }
+        if(codes[E_AXIS])
+            current_position[E_AXIS] = values[E_AXIS];
+
+        // Set all at once
+        plan_set_position(current_position[X_AXIS], current_position[Y_AXIS],
+                          current_position[Z_AXIS], current_position[E_AXIS]);
+    }
+}
+
+
 #ifdef BACKLASH_X
 extern uint8_t st_backlash_x;
 #endif //BACKLASH_X
@@ -3367,8 +3394,7 @@ extern uint8_t st_backlash_y;
 
 //! @brief Parse and process commands
 //!
-//! look here for descriptions of G-codes: http://linuxcnc.org/handbook/gcode/g-code.html
-//! http://objects.reprap.org/wiki/Mendel_User_Manual:_RepRapGCodes
+//! look here for descriptions of G-codes: https://reprap.org/wiki/G-code
 //!
 //!
 //! Implemented Codes 
@@ -3387,7 +3413,7 @@ extern uint8_t st_backlash_y;
 //!@n G4  - Dwell S<seconds> or P<milliseconds>
 //!@n G10 - retract filament according to settings of M207
 //!@n G11 - retract recover filament according to settings of M208
-//!@n G28 - Home all Axis
+//!@n G28 - Home all Axes
 //!@n G29 - Detailed Z-Probe, probes the bed at 3 or more points.  Will fail if you haven't homed yet.
 //!@n G30 - Single Z Probe, probes bed at current XY location.
 //!@n G31 - Dock sled (Z_PROBE_SLED only)
@@ -3504,7 +3530,11 @@ extern uint8_t st_backlash_y;
 
 /** \ingroup GCodes */
 
-//! _This is a list of currently implemented G Codes in Prusa firmware (dynamically generated from doxygen)_
+//! _This is a list of currently implemented G Codes in Prusa firmware (dynamically generated from doxygen)._ 
+/**
+They are shown in order of appearance in the code.
+There are reasons why some G Codes aren't in numerical order.
+*/
 
 
 void process_commands()
@@ -3548,7 +3578,25 @@ void process_commands()
   float tmp_motor_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
   int8_t SilentMode;
 #endif
+  /*!
+  
+  ---------------------------------------------------------------------------------
+  ### M117 - Display Message <a href="https://reprap.org/wiki/G-code#M117:_Display_Message">M117: Display Message</a>
+  This causes the given message to be shown in the status line on an attached LCD.
+  It is processed early as to allow printing messages that contain G, M, N or T.
   
+  ---------------------------------------------------------------------------------
+  ### Special internal commands
+  These are used by internal functions to process certain actions in the right order. Some of these are also usable by the user.
+  They are processed early as the commands are complex (strings).
+  These are only available on the MK3(S) as these require TMC2130 drivers:
+    - CRASH DETECTED
+    - CRASH RECOVER
+    - CRASH_CANCEL
+    - TMC_SET_WAVE
+    - TMC_SET_STEP
+    - TMC_SET_CHOP
+ */
   if (code_seen("M117")) { //moved to highest priority place to be able to to print strings which includes "G", "PRUSA" and "^"
 	  starpos = (strchr(strchr_pointer + 5, '*'));
 	  if (starpos != NULL)
@@ -3560,7 +3608,7 @@ void process_commands()
 	else if (strncmp_P(CMDBUFFER_CURRENT_STRING, PSTR("CRASH_"), 6) == 0)
 	{
 
-    //! ### CRASH_DETECTED - TMC2130
+    // ### CRASH_DETECTED - TMC2130
     // ---------------------------------
 	  if(code_seen("CRASH_DETECTED"))
 	  {
@@ -3570,12 +3618,12 @@ void process_commands()
 		  crashdet_detected(mask);
 	  }
 
-    //! ### CRASH_RECOVER - TMC2130
+    // ### CRASH_RECOVER - TMC2130
     // ----------------------------------
 	  else if(code_seen("CRASH_RECOVER"))
 		  crashdet_recover();
 
-    //! ### CRASH_CANCEL - TMC2130
+    // ### CRASH_CANCEL - TMC2130
     // ----------------------------------
 	  else if(code_seen("CRASH_CANCEL"))
 		  crashdet_cancel();
@@ -3583,7 +3631,7 @@ void process_commands()
 	else if (strncmp_P(CMDBUFFER_CURRENT_STRING, PSTR("TMC_"), 4) == 0)
 	{
     
-    //! ### TMC_SET_WAVE_ 
+    // ### TMC_SET_WAVE_ 
     // --------------------
 		if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_WAVE_"), 9) == 0)
 		{
@@ -3596,7 +3644,7 @@ void process_commands()
 			}
 		}
     
-    //! ### TMC_SET_STEP_
+    // ### TMC_SET_STEP_
     //  ------------------
 		else if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_STEP_"), 9) == 0)
 		{
@@ -3610,7 +3658,7 @@ void process_commands()
 			}
 		}
 
-    //! ### TMC_SET_CHOP_
+    // ### TMC_SET_CHOP_
     //  -------------------
 		else if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_CHOP_"), 9) == 0)
 		{
@@ -3665,13 +3713,14 @@ void process_commands()
 #endif //TMC2130
   else if(code_seen("PRUSA")){ 
     /*!
-    *
-    ### PRUSA - Internal command set
+    ---------------------------------------------------------------------------------
+    ### PRUSA - Internal command set <a href="https://reprap.org/wiki/G-code#G98:_Activate_farm_mode">G98: Activate farm mode - Notes</a>
     
     Set of internal PRUSA commands
-      
-          PRUSA [ Ping | PRN | FAN | fn | thx | uvlo | MMURES | RESET | fv | M28 | SN | Fir | Rev | Lang | Lz | Beat | FR ]
-      
+    #### Usage
+         PRUSA [ Ping | PRN | FAN | fn | thx | uvlo | MMURES | RESET | fv | M28 | SN | Fir | Rev | Lang | Lz | Beat | FR ]
+    
+    #### Parameters
       - `Ping` 
       - `PRN` - Prints revision of the printer
       - `FAN` - Prints fan details
@@ -3692,7 +3741,6 @@ void process_commands()
       - `nozzle set <diameter>` - set nozzle diameter (farm mode only), e.g. `PRUSA nozzle set 0.4`
       - `nozzle D<diameter>` - check the nozzle diameter (farm mode only), works like M862.1 P, e.g. `PRUSA nozzle D0.4`
       - `nozzle` - prints nozzle diameter (farm mode only), works like M862.1 P, e.g. `PRUSA nozzle`
-    *
     */
 
 
@@ -3707,7 +3755,7 @@ void process_commands()
 
         } else if( code_seen("FANPINTST") ){
             gcode_PRUSA_BadRAMBoFanTest();
-        }else if (code_seen("FAN")) { //! PRUSA FAN
+        }else if (code_seen("FAN")) { // PRUSA FAN
 			printf_P(_N("E0:%d RPM\nPRN0:%d RPM\n"), 60*fan_speed[0], 60*fan_speed[1]);
 		}else if (code_seen("fn")) { // PRUSA fn
 		  if (farm_mode) {
@@ -3791,6 +3839,17 @@ void process_commands()
     } else if(code_seen("FR")) { // PRUSA FR
         // Factory full reset
         factory_reset(0);
+    } else if(code_seen("MBL")) { // PRUSA MBL
+        // Change the MBL status without changing the logical Z position.
+        if(code_seen("V")) {
+            bool value = code_value_short();
+            st_synchronize();
+            if(value != mbl.active) {
+                mbl.active = value;
+                // Use plan_set_z_position to reset the physical values
+                plan_set_z_position(current_position[Z_AXIS]);
+            }
+        }
 
 //-//
 /*
@@ -3876,8 +3935,24 @@ eeprom_update_word((uint16_t*)EEPROM_NOZZLE_DIAMETER_uM,0xFFFF);
     switch (gcode_in_progress)
     {
 
-    //! ### G0, G1 - Coordinated movement X Y Z E
-    // --------------------------------------      
+    /*!
+    ---------------------------------------------------------------------------------
+	 # G Codes
+	### G0, G1 - Coordinated movement X Y Z E <a href="https://reprap.org/wiki/G-code#G0_.26_G1:_Move">G0 & G1: Move</a> 
+	In Prusa Firmware G0 and G1 are the same.
+	#### Usage
+	
+	      G0 [ X | Y | Z | E | F | S ]
+		  G1 [ X | Y | Z | E | F | S ]
+	
+	#### Parameters
+	  - `X` - The position to move to on the X axis
+	  - `Y` - The position to move to on the Y axis
+	  - `Z` - The position to move to on the Z axis
+	  - `E` - The amount to extrude between the starting point and ending point
+	  - `F` - The feedrate per minute of the move between the starting point and ending point (if supplied)
+	  
+    */
     case 0: // G0 -> G1
     case 1: // G1
       if(Stopped == false) {
@@ -4087,8 +4162,25 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       }
       break;
 
-    //! ### G2 - CW ARC
-    // ------------------------------     
+    /*!
+	### G2, G3 - Controlled Arc Move <a href="https://reprap.org/wiki/G-code#G2_.26_G3:_Controlled_Arc_Move">G2 & G3: Controlled Arc Move</a>
+	
+    These commands don't propperly work with MBL enabled. The compensation only happens at the end of the move, so avoid long arcs.
+    
+	#### Usage
+	
+	      G2 [ X | Y | I | E | F ] (Clockwise Arc)
+		  G3 [ X | Y | I | E | F ] (Counter-Clockwise Arc)
+	
+	#### Parameters
+	  - `X` - The position to move to on the X axis
+	  - `Y` - The position to move to on the Y axis
+	  - `I` - The point in X space from the current X position to maintain a constant distance from
+	  - `J` - The point in Y space from the current Y position to maintain a constant distance from
+	  - `E` - The amount to extrude between the starting point and ending point
+	  - `F` - The feedrate per minute of the move between the starting point and ending point (if supplied)
+	
+    */
     case 2: 
       if(Stopped == false) {
         get_arc_coordinates();
@@ -4096,8 +4188,6 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       }
       break;
  
-
-    //! ### G3  - CCW ARC
     // -------------------------------
     case 3: 
       if(Stopped == false) {
@@ -4107,8 +4197,19 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       break;
 
 
-    //! ### G4 - Dwell
-    // -------------------------------
+    /*!
+	### G4 - Dwell <a href="https://reprap.org/wiki/G-code#G4:_Dwell">G4: Dwell</a>
+	Pause the machine for a period of time.
+	
+	#### Usage
+	
+	    G4 [ P | S ]
+	
+	#### Parameters
+	  - `P` - Time to wait, in milliseconds
+	  - `S` - Time to wait, in seconds
+	
+    */
     case 4: 
       codenum = 0;
       if(code_seen('P')) codenum = code_value(); // milliseconds to wait
@@ -4126,8 +4227,10 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       #ifdef FWRETRACT
       
 
-    //! ### G10 Retract
-    // ------------------------------
+    /*!
+	### G10 - Retract <a href="https://reprap.org/wiki/G-code#G10:_Retract">G10: Retract</a>
+	Retracts filament according to settings of `M207`
+    */
     case 10: 
        #if EXTRUDERS > 1
         retracted_swap[active_extruder]=(code_seen('S') && code_value_long() == 1); // checks for swap retract argument
@@ -4138,8 +4241,10 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       break;
       
 
-    //! ### G11 - Retract recover
-    // ----------------------------- 
+    /*!
+	### G11 - Retract recover <a href="https://reprap.org/wiki/G-code#G11:_Unretract">G11: Unretract</a>
+	Unretracts/recovers filament according to settings of `M208`
+    */
     case 11: 
        #if EXTRUDERS > 1
         retract(false,retracted_swap[active_extruder]);
@@ -4150,8 +4255,20 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       #endif //FWRETRACT
     
 
-    //! ### G28 - Home all Axis one at a time
-    // --------------------------------------------
+    /*!
+    ### G28 - Home all Axes one at a time <a href="https://reprap.org/wiki/G-code#G28:_Move_to_Origin_.28Home.29">G28: Move to Origin (Home)</a>
+    Using `G28` without any parameters will perfom homing of all axes AND mesh bed leveling, while `G28 W` will just home all axes (no mesh bed leveling).
+    #### Usage
+	
+         G28 [ X | Y | Z | W | C ]
+    
+	#### Parameters
+     - `X` - Flag to go back to the X axis origin
+     - `Y` - Flag to go back to the Y axis origin
+     - `Z` - Flag to go back to the Z axis origin
+     - `W` - Suppress mesh bed leveling if `X`, `Y` or `Z` are not provided
+     - `C` - Calibrate X and Y origin (home) - Only on MK3/s
+	*/
     case 28: 
     {
       long home_x_value = 0;
@@ -4183,8 +4300,12 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 #ifdef ENABLE_AUTO_BED_LEVELING
     
 
-    //! ### G29 - Detailed Z-Probe
-    // --------------------------------    
+    /*!
+	### G29 - Detailed Z-Probe <a href="https://reprap.org/wiki/G-code#G29:_Detailed_Z-Probe">G29: Detailed Z-Probe</a>
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+	
+	See `G81`
+    */
     case 29: 
         {
             #if Z_MIN_PIN == -1
@@ -4329,8 +4450,10 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
         break;
 #ifndef Z_PROBE_SLED
 
-    //! ### G30 - Single Z Probe
-    // ------------------------------------        
+    /*!
+	### G30 - Single Z Probe <a href="https://reprap.org/wiki/G-code#G30:_Single_Z-Probe">G30: Single Z-Probe</a>
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+    */
     case 30: 
         {
             st_synchronize();
@@ -4354,15 +4477,19 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
         break;
 #else
 
-    //! ### G31 - Dock the sled
-    // ---------------------------
+    /*!
+	### G31 - Dock the sled <a href="https://reprap.org/wiki/G-code#G31:_Dock_Z_Probe_sled">G31: Dock Z Probe sled</a>
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+    */
     case 31: 
         dock_sled(true);
         break;
 
 
-    //! ### G32 - Undock the sled
-    // ----------------------------     
+    /*!
+	### G32 - Undock the sled <a href="https://reprap.org/wiki/G-code#G32:_Undock_Z_Probe_sled">G32: Undock Z Probe sled</a>
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+    */
     case 32: 
         dock_sled(false);
         break;
@@ -4371,8 +4498,11 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
             
 #ifdef MESH_BED_LEVELING
 
-    //! ### G30 - Single Z Probe
-    // ----------------------------    
+    /*!
+	### G30 - Single Z Probe <a href="https://reprap.org/wiki/G-code#G30:_Single_Z-Probe">G30: Single Z-Probe</a>
+    Sensor must be over the bed.
+    The maximum travel distance before an error is triggered is 10mm.
+    */
     case 30: 
         {
             st_synchronize();
@@ -4389,8 +4519,10 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
         }
         break;
 	
-  //! ### G75 - Print temperature interpolation
-  // ---------------------------------------------
+  /*!
+  ### G75 - Print temperature interpolation <a href="https://reprap.org/wiki/G-code#G75:_Print_temperature_interpolation">G75: Print temperature interpolation</a>
+  Show/print PINDA temperature interpolating.
+  */
 	case 75:
 	{
 		for (int i = 40; i <= 110; i++)
@@ -4398,9 +4530,26 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 	}
 	break;
 
-  //! ### G76 - PINDA probe temperature calibration
-  // ------------------------------------------------
-	case 76: 
+  /*!
+  ### G76 - PINDA probe temperature calibration <a href="https://reprap.org/wiki/G-code#G76:_PINDA_probe_temperature_calibration">G76: PINDA probe temperature calibration</a>
+  This G-code is used to calibrate the temperature drift of the PINDA (inductive Sensor).
+  
+  The PINDAv2 sensor has a built-in thermistor which has the advantage that the calibration can be done once for all materials.
+  
+  The Original i3 Prusa MK2/s uses PINDAv1 and this calibration improves the temperature drift, but not as good as the PINDAv2.
+
+  #### Example
+  
+  ```
+  G76
+  
+  echo PINDA probe calibration start
+  echo start temperature: 35.0°
+  echo ...
+  echo PINDA temperature -- Z shift (mm): 0.---
+  ```
+  */
+  case 76: 
 	{
 #ifdef PINDA_THERMISTOR
 		if (true)
@@ -4657,11 +4806,28 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 	break;
 
 
-	//! ### G80 - Mesh-based Z probe 
-  // -----------------------------------
+    /*!
+    ### G80 - Mesh-based Z probe <a href="https://reprap.org/wiki/G-code#G80:_Mesh-based_Z_probe">G80: Mesh-based Z probe</a>
+    Default 3x3 grid can be changed on MK2.5/s and MK3/s to 7x7 grid.
+    #### Usage
+	  
+          G80 [ N | R | V | L | R | F | B ]
+      
+	#### Parameters
+      - `N` - Number of mesh points on x axis. Default is 3. Valid values are 3 and 7.
+      - `R` - Probe retries. Default 3 max. 10
+      - `V` - Verbosity level 1=low, 10=mid, 20=high. It only can be used if the firmware has been compiled with SUPPORT_VERBOSITY active.
+      
+      Using the following parameters enables additional "manual" bed leveling correction. Valid values are -100 microns to 100 microns.
+    #### Additional Parameters
+      - `L` - Left Bed Level correct value in um.
+      - `R` - Right Bed Level correct value in um.
+      - `F` - Front Bed Level correct value in um.
+      - `B` - Back Bed Level correct value in um.
+    */
   
 	/*
-  * Probes a grid and produces a mesh to compensate for variable bed height
+    * Probes a grid and produces a mesh to compensate for variable bed height
 	* The S0 report the points as below
 	*  +----> X-axis
 	*  |
@@ -4677,6 +4843,11 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 	case_G80:
 	{
 		mesh_bed_leveling_flag = true;
+#ifndef LA_NOCOMPAT
+        // When printing via USB there's no clear boundary between prints. Abuse MBL to indicate
+        // the beginning of a new print, allowing a new autodetected setting just after G80.
+        la10c_reset();
+#endif
 #ifndef PINDA_THERMISTOR
         static bool run = false; // thermistor-less PINDA temperature compensation is running
 #endif // ndef PINDA_THERMISTOR
@@ -4694,13 +4865,8 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 			// We don't know where we are! HOME!
 			// Push the commands to the front of the message queue in the reverse order!
 			// There shall be always enough space reserved for these commands.
-			if (lcd_commands_type != LcdCommands::StopPrint) {
-				repeatcommand_front(); // repeat G80 with all its parameters
-				enquecommand_front_P((PSTR("G28 W0")));
-			}
-			else {
-				mesh_bed_leveling_flag = false;
-			}
+			repeatcommand_front(); // repeat G80 with all its parameters
+			enquecommand_front_P((PSTR("G28 W0")));
 			break;
 		} 
 		
@@ -4730,23 +4896,14 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 #ifndef PINDA_THERMISTOR
 		if (run == false && temp_cal_active == true && calibration_status_pinda() == true && target_temperature_bed >= 50)
 		{
-			if (lcd_commands_type != LcdCommands::StopPrint) {
-				temp_compensation_start();
-				run = true;
-				repeatcommand_front(); // repeat G80 with all its parameters
-				enquecommand_front_P((PSTR("G28 W0")));
-			}
-			else {
-				mesh_bed_leveling_flag = false;
-			}
+			temp_compensation_start();
+			run = true;
+			repeatcommand_front(); // repeat G80 with all its parameters
+			enquecommand_front_P((PSTR("G28 W0")));
 			break;
 		}
         run = false;
 #endif //PINDA_THERMISTOR
-		if (lcd_commands_type == LcdCommands::StopPrint) {
-			mesh_bed_leveling_flag = false;
-			break;
-		}
 		// Save custom message state, set a new custom message state to display: Calibrating point 9.
 		CustomMsg custom_message_type_old = custom_message_type;
 		unsigned int custom_message_state_old = custom_message_state;
@@ -5086,12 +5243,10 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 	}
 	break;
 
-        //! ### G81 - Mesh bed leveling status
-        // -----------------------------------------
-
-         /*
-         * Prints mesh bed leveling status and bed profile if activated
-         */
+        /*!
+		### G81 - Mesh bed leveling status <a href="https://reprap.org/wiki/G-code#G81:_Mesh_bed_leveling_status">G81: Mesh bed leveling status</a>
+		Prints mesh bed leveling status and bed profile if activated.
+        */
         case 81:
             if (mbl.active) {
                 SERIAL_PROTOCOLPGM("Num X,Y: ");
@@ -5114,12 +5269,12 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
             break;
             
 #if 0
-        /*
-         * G82: Single Z probe at current location
-         *
-         * WARNING! USE WITH CAUTION! If you'll try to probe where is no leveling pad, nasty things can happen!
-         *
-         */
+        /*!
+        ### G82: Single Z probe at current location - Not active <a href="https://reprap.org/wiki/G-code#G82:_Single_Z_probe_at_current_location">G82: Single Z probe at current location</a>
+        
+        WARNING! USE WITH CAUTION! If you'll try to probe where is no leveling pad, nasty things can happen!
+		In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+		*/
         case 82:
             SERIAL_PROTOCOLLNPGM("Finding bed ");
             int l_feedmultiply = setup_for_endstop_move();
@@ -5130,9 +5285,10 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
             SERIAL_PROTOCOLPGM("\n");
             break;
 
-            /*
-             * G83: Prusa3D specific: Babystep in Z and store to EEPROM
-             */
+        /*!
+        ### G83: Babystep in Z and store to EEPROM - Not active <a href="https://reprap.org/wiki/G-code#G83:_Babystep_in_Z_and_store_to_EEPROM">G83: Babystep in Z and store to EEPROM</a>
+		In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+		*/
         case 83:
         {
             int babystepz = code_seen('S') ? code_value() : 0;
@@ -5155,50 +5311,50 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
             
         }
         break;
-            /*
-             * G84: Prusa3D specific: UNDO Babystep Z (move Z axis back)
-             */
+        /*!
+        ### G84: UNDO Babystep Z (move Z axis back) - Not active <a href="https://reprap.org/wiki/G-code#G84:_UNDO_Babystep_Z_.28move_Z_axis_back.29">G84: UNDO Babystep Z (move Z axis back)</a>
+		In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+		*/
         case 84:
             babystepsTodoZsubtract(babystepLoadZ);
             // babystepLoadZ = 0;
             break;
             
-            /*
-             * G85: Prusa3D specific: Pick best babystep
-             */
+        /*!
+        ### G85: Pick best babystep - Not active <a href="https://reprap.org/wiki/G-code#G85:_Pick_best_babystep">G85: Pick best babystep</a>
+		In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+		*/
         case 85:
             lcd_pick_babystep();
             break;
 #endif
             
-        /**
-         * ### G86 - Disable babystep correction after home
-         *
-         * This G-code will be performed at the start of a calibration script.
-         * (Prusa3D specific)
-         */
+        /*!
+        ### G86 - Disable babystep correction after home <a href="https://reprap.org/wiki/G-code#G86:_Disable_babystep_correction_after_home">G86: Disable babystep correction after home</a>
+        
+        This G-code will be performed at the start of a calibration script.
+        (Prusa3D specific)
+        */
         case 86:
             calibration_status_store(CALIBRATION_STATUS_LIVE_ADJUST);
             break;
            
 
-        /**
-         * ### G87 - Enable babystep correction after home
-         * 
-         *
-         * This G-code will be performed at the end of a calibration script.
-         * (Prusa3D specific)
-         */
+        /*!
+        ### G87 - Enable babystep correction after home <a href="https://reprap.org/wiki/G-code#G87:_Enable_babystep_correction_after_home">G87: Enable babystep correction after home</a>
+        
+		This G-code will be performed at the end of a calibration script.
+        (Prusa3D specific)
+        */
         case 87:
 			calibration_status_store(CALIBRATION_STATUS_CALIBRATED);
             break;
 
-
-        /**
-         * ### G88 - Reserved
-         *
-         * Currently has no effect. 
-         */
+        /*!
+        ### G88 - Reserved <a href="https://reprap.org/wiki/G-code#G88:_Reserved">G88: Reserved</a>
+        
+        Currently has no effect. 
+        */
 
         // Prusa3D specific: Don't know what it is for, it is in V2Calibration.gcode
 
@@ -5208,44 +5364,56 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 
 #endif  // ENABLE_MESH_BED_LEVELING
             
-    //! ### G90 - Switch off relative mode
-    // -------------------------------
+
+    /*!
+	### G90 - Switch off relative mode <a href="https://reprap.org/wiki/G-code#G90:_Set_to_Absolute_Positioning">G90: Set to Absolute Positioning</a>
+	All coordinates from now on are absolute relative to the origin of the machine. E axis is also switched to absolute mode.
+    */
     case 90: {
         for(uint8_t i = 0; i != NUM_AXIS; ++i)
             axis_relative_modes[i] = false;
     }
     break;
 
-    //! ### G91 - Switch on relative mode
-    // -------------------------------
+    /*!
+	### G91 - Switch on relative mode <a href="https://reprap.org/wiki/G-code#G91:_Set_to_Relative_Positioning">G91: Set to Relative Positioning</a>
+    All coordinates from now on are relative to the last position. E axis is also switched to relative mode.
+	*/
     case 91: {
         for(uint8_t i = 0; i != NUM_AXIS; ++i)
             axis_relative_modes[i] = true;
     }
     break;
 
-    //! ### G92 - Set position
-    // -----------------------------
-    case 92:
-      if(!code_seen(axis_codes[E_AXIS]))
-        st_synchronize();
-      for(int8_t i=0; i < NUM_AXIS; i++) {
-        if(code_seen(axis_codes[i])) {
-           if(i == E_AXIS) {
-             current_position[i] = code_value();
-             plan_set_e_position(current_position[E_AXIS]);
-           }
-           else {
-		current_position[i] = code_value()+cs.add_homing[i];
-            plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-           }
-        }
-      }
-      break;
-
+    /*!
+	### G92 - Set position <a href="https://reprap.org/wiki/G-code#G92:_Set_Position">G92: Set Position</a>
+    
+    It is used for setting the current position of each axis. The parameters are always absolute to the origin.
+    If a parameter is omitted, that axis will not be affected.
+    If `X`, `Y`, or `Z` axis are specified, the move afterwards might stutter because of Mesh Bed Leveling. `E` axis is not affected if the target position is 0 (`G92 E0`).
+	A G92 without coordinates will reset all axes to zero on some firmware. This is not the case for Prusa-Firmware!
+    
+    #### Usage
+	
+	      G92 [ X | Y | Z | E ]
+	
+	#### Parameters
+	  - `X` - new X axis position
+	  - `Y` - new Y axis position
+	  - `Z` - new Z axis position
+	  - `E` - new extruder position
+	
+    */
+    case 92: {
+        gcode_G92();
+    }
+    break;
 
-  //! ### G98 - Activate farm mode
-  // -----------------------------------    
+    /*!
+    ### G98 - Activate farm mode <a href="https://reprap.org/wiki/G-code#G98:_Activate_farm_mode">G98: Activate farm mode</a>
+	Enable Prusa-specific Farm functions and g-code.
+    See Internal Prusa commands.
+    */
 	case 98:
 		farm_mode = 1;
 		PingTime = _millis();
@@ -5256,8 +5424,9 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
           fCheckModeInit();                       // alternatively invoke printer reset
 		break;
 
-  //! ### G99 - Deactivate farm mode
-  // -------------------------------------
+    /*! ### G99 - Deactivate farm mode <a href="https://reprap.org/wiki/G-code#G99:_Deactivate_farm_mode">G99: Deactivate farm mode</a>
+ 	Disables Prusa-specific Farm functions and g-code.
+   */
 	case 99:
 		farm_mode = 0;
 		lcd_printer_connected();
@@ -5271,9 +5440,15 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 //	printf_P(_N("END G-CODE=%u\n"), gcode_in_progress);
 	gcode_in_progress = 0;
   } // end if(code_seen('G'))
+  /*!
+  ### End of G-Codes
+  */
 
-
-  //! ---------------------------------------------------------------------------------
+  /*!
+  ---------------------------------------------------------------------------------
+  # M Commands
+  
+  */
 
   else if(code_seen('M'))
   {
@@ -5293,8 +5468,9 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
     switch(mcode_in_progress)
     {
 
-    //! ### M0, M1 - Stop the printer
-    // ---------------------------------------------------------------
+    /*!
+	### M0, M1 - Stop the printer <a href="https://reprap.org/wiki/G-code#M0:_Stop_or_Unconditional_stop">M0: Stop or Unconditional stop</a>
+    */
     case 0: // M0 - Unconditional stop - Wait for user button press on LCD
     case 1: // M1 - Conditional stop - Wait for user button press on LCD
     {
@@ -5343,8 +5519,9 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
     }
     break;
 
-    //! ### M17 - Enable axes
-    // ---------------------------------
+    /*!
+	### M17 - Enable all axes <a href="https://reprap.org/wiki/G-code#M17:_Enable.2FPower_all_stepper_motors">M17: Enable/Power all stepper motors</a>
+    */
     case 17:
         LCD_MESSAGERPGM(_i("No move."));////MSG_NO_MOVE
         enable_x();
@@ -5357,28 +5534,36 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 
 #ifdef SDSUPPORT
 
-    //! ### M20 - SD Card file list
-    // -----------------------------------
+    /*!
+	### M20 - SD Card file list <a href="https://reprap.org/wiki/G-code#M20:_List_SD_card">M20: List SD card</a>
+    */
     case 20:
       SERIAL_PROTOCOLLNRPGM(_N("Begin file list"));////MSG_BEGIN_FILE_LIST
       card.ls();
       SERIAL_PROTOCOLLNRPGM(_N("End file list"));////MSG_END_FILE_LIST
       break;
 
-    //! ### M21 - Init SD card
-    // ------------------------------------
+    /*!
+	### M21 - Init SD card <a href="https://reprap.org/wiki/G-code#M21:_Initialize_SD_card">M21: Initialize SD card</a>
+    */
     case 21:
       card.initsd();
       break;
 
-    //! ### M22 - Release SD card
-    // -----------------------------------
+    /*!
+	### M22 - Release SD card <a href="https://reprap.org/wiki/G-code#M22:_Release_SD_card">M22: Release SD card</a>
+    */
     case 22: 
       card.release();
       break;
 
-    //! ### M23 - Select file
-    // -----------------------------------
+    /*!
+	### M23 - Select file <a href="https://reprap.org/wiki/G-code#M23:_Select_SD_file">M23: Select SD file</a>
+    #### Usage
+    
+        M23 [filename]
+    
+    */
     case 23: 
       starpos = (strchr(strchr_pointer + 4,'*'));
 	  if(starpos!=NULL)
@@ -5386,24 +5571,40 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       card.openFile(strchr_pointer + 4,true);
       break;
 
-    //! ### M24 - Start/resume SD print
-    // ----------------------------------
+    /*!
+	### M24 - Start SD print <a href="https://reprap.org/wiki/G-code#M24:_Start.2Fresume_SD_print">M24: Start/resume SD print</a>
+    */
     case 24:
 	  if (isPrintPaused)
           lcd_resume_print();
       else
       {
-          failstats_reset_print();
+          if (!card.get_sdpos())
+          {
+              // A new print has started from scratch, reset stats
+              failstats_reset_print();
+#ifndef LA_NOCOMPAT
+              la10c_reset();
+#endif
+          }
+
           card.startFileprint();
           starttime=_millis();
       }
 	  break;
 
-    //! ### M26 S\<index\> - Set SD index
-    //! Set position in SD card file to index in bytes.
-    //! This command is expected to be called after M23 and before M24.
-    //! Otherwise effect of this command is undefined.
-    // ----------------------------------
+    /*!
+	### M26 - Set SD index <a href="https://reprap.org/wiki/G-code#M26:_Set_SD_position">M26: Set SD position</a>
+    Set position in SD card file to index in bytes.
+    This command is expected to be called after M23 and before M24.
+    Otherwise effect of this command is undefined.
+    #### Usage
+	
+	      M26 [ S ]
+	
+	#### Parameters
+	  - `S` - Index in bytes
+    */
     case 26: 
       if(card.cardOK && code_seen('S')) {
         long index = code_value_long();
@@ -5414,14 +5615,16 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       }
       break;
 
-    //! ### M27 - Get SD status
-    // ----------------------------------
+    /*!
+	### M27 - Get SD status <a href="https://reprap.org/wiki/G-code#M27:_Report_SD_print_status">M27: Report SD print status</a>
+    */
     case 27:
       card.getStatus();
       break;
 
-    //! ### M28 - Start SD write
-    // ---------------------------------  
+    /*!
+	### M28 - Start SD write <a href="https://reprap.org/wiki/G-code#M28:_Begin_write_to_SD_card">M28: Begin write to SD card</a>
+    */
     case 28: 
       starpos = (strchr(strchr_pointer + 4,'*'));
       if(starpos != NULL){
@@ -5432,16 +5635,21 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       card.openFile(strchr_pointer+4,false);
       break;
 
-    //! ### M29 - Stop SD write
-    // -------------------------------------
-    //! Currently has no effect.
+    /*! ### M29 - Stop SD write <a href="https://reprap.org/wiki/G-code#M29:_Stop_writing_to_SD_card">M29: Stop writing to SD card</a>
+	Stops writing to the SD file signaling the end of the uploaded file. It is processed very early and it's not written to the card.
+    */
     case 29:
       //processed in write to file routine above
       //card,saving = false;
       break;
 
-    //! ### M30 - Delete file  <filename> 
-    // ----------------------------------
+    /*!
+	### M30 - Delete file <a href="https://reprap.org/wiki/G-code#M30:_Delete_a_file_on_the_SD_card">M30: Delete a file on the SD card</a>
+    #### Usage
+    
+        M30 [filename]
+    
+    */
     case 30:
       if (card.cardOK){
         card.closefile();
@@ -5455,8 +5663,10 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       }
       break;
 
-    //! ### M32 - Select file and start SD print
-    // ------------------------------------
+    /*!
+	### M32 - Select file and start SD print <a href="https://reprap.org/wiki/G-code#M32:_Select_file_and_start_SD_print">M32: Select file and start SD print</a>
+	@todo What are the parameters P and S for in M32?
+    */
     case 32:
     {
       if(card.sdprinting) {
@@ -5489,12 +5699,27 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
             card.setIndex(code_value_long());
         card.startFileprint();
         if(!call_procedure)
-          starttime=_millis(); //procedure calls count as normal print time.
+        {
+            if(!card.get_sdpos())
+            {
+                // A new print has started from scratch, reset stats
+                failstats_reset_print();
+#ifndef LA_NOCOMPAT
+                la10c_reset();
+#endif
+            }
+            starttime=_millis(); // procedure calls count as normal print time.
+        }
       }
     } break;
 
-    //! ### M982 - Start SD write
-    // ---------------------------------
+    /*!
+	### M928 - Start SD logging <a href="https://reprap.org/wiki/G-code#M928:_Start_SD_logging">M928: Start SD logging</a>
+    #### Usage
+    
+        M928 [filename]
+    
+    */
     case 928: 
       starpos = (strchr(strchr_pointer + 5,'*'));
       if(starpos != NULL){
@@ -5507,8 +5732,9 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 
 #endif //SDSUPPORT
 
-    //! ### M31 - Report current print time
-    // --------------------------------------------------
+    /*!
+	### M31 - Report current print time <a href="https://reprap.org/wiki/G-code#M31:_Output_time_since_last_M109_or_SD_card_start_to_serial">M31: Output time since last M109 or SD card start to serial</a>
+    */
     case 31: //M31 take time since the start of the SD print or an M109 command
       {
       stoptime=_millis();
@@ -5525,8 +5751,17 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       }
       break;
 
-    //! ### M42 - Set pin state
-    // -----------------------------
+    /*!
+	### M42 - Set pin state <a href="https://reprap.org/wiki/G-code#M42:_Switch_I.2FO_pin">M42: Switch I/O pin</a>
+    #### Usage
+    
+        M42 [ P | S ]
+        
+    #### Parameters
+    - `P` - Pin number.
+    - `S` - Pin value. If the pin is analog, values are from 0 to 255. If the pin is digital, values are from 0 to 1.
+    
+    */
     case 42:
       if (code_seen('S'))
       {
@@ -5556,8 +5791,9 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
      break;
 
 
-    //! ### M44 - Reset the bed skew and offset calibration (Prusa specific)
-    // --------------------------------------------------------------------
+    /*!
+	### M44 - Reset the bed skew and offset calibration <a href="https://reprap.org/wiki/G-code#M44:_Reset_the_bed_skew_and_offset_calibration">M44: Reset the bed skew and offset calibration</a>
+    */
     case 44: // M44: Prusa3D: Reset the bed skew and offset calibration.
 
 		// Reset the baby step value and the baby step applied flag.
@@ -5572,8 +5808,15 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
         world2machine_revert_to_uncorrected();
         break;
 
-    //! ### M45 - Bed skew and offset with manual Z up (Prusa specific)
-    // ------------------------------------------------------
+    /*!
+	### M45 - Bed skew and offset with manual Z up <a href="https://reprap.org/wiki/G-code#M45:_Bed_skew_and_offset_with_manual_Z_up">M45: Bed skew and offset with manual Z up</a>
+	#### Usage
+    
+        M45 [ V ]
+    #### Parameters
+	- `V` - Verbosity level 1, 10 and 20 (low, mid, high). Only when SUPPORT_VERBOSITY is defined. Optional.
+    - `Z` - If it is provided, only Z calibration will run. Otherwise full calibration is executed.
+    */
     case 45: // M45: Prusa3D: bed skew and offset with manual Z up
     {
 		int8_t verbosity_level = 0;
@@ -5590,8 +5833,11 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
     }
 	break;
 
+    /*!
+	### M46 - Show the assigned IP address <a href="https://reprap.org/wiki/G-code#M46:_Show_the_assigned_IP_address">M46: Show the assigned IP address.</a>
+    */
     /*
-    case 46:
+     case 46:
     {
         // M46: Prusa3D: Show the assigned IP address.
         uint8_t ip[4];
@@ -5613,8 +5859,9 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
     }
     */
 
-    //! ### M47 - Show end stops dialog on the display (Prusa specific)
-    // ---------------------------------------------------- 
+    /*!
+	### M47 - Show end stops dialog on the display <a href="https://reprap.org/wiki/G-code#M47:_Show_end_stops_dialog_on_the_display">M47: Show end stops dialog on the display</a>
+    */
     case 47:
         
 		KEEPALIVE_STATE(PAUSED_FOR_USER);
@@ -5662,22 +5909,25 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 #ifdef ENABLE_AUTO_BED_LEVELING
 #ifdef Z_PROBE_REPEATABILITY_TEST 
 
-    //! ### M48 - Z-Probe repeatability measurement function.
-    // ------------------------------------------------------
-    //!
-    //! _Usage:_
-    //!    
-    //!     M48 <n #_samples> <X X_position_for_samples> <Y Y_position_for_samples> <V Verbose_Level> <L legs_of_movement_prior_to_doing_probe>
-    //! 
-    //! This function assumes the bed has been homed.  Specifically, that a G28 command
-    //! as been issued prior to invoking the M48 Z-Probe repeatability measurement function.
-    //! Any information generated by a prior G29 Bed leveling command will be lost and need to be
-    //! regenerated.
-    //!
-    //! The number of samples will default to 10 if not specified.  You can use upper or lower case
-    //! letters for any of the options EXCEPT n.  n must be in lower case because Marlin uses a capital
-    //! N for its communication protocol and will get horribly confused if you send it a capital N.
-    //!
+    /*!
+	### M48 - Z-Probe repeatability measurement function <a href="https://reprap.org/wiki/G-code#M48:_Measure_Z-Probe_repeatability">M48: Measure Z-Probe repeatability</a>
+    
+     This function assumes the bed has been homed.  Specifically, that a G28 command as been issued prior to invoking the M48 Z-Probe repeatability measurement function. Any information generated by a prior G29 Bed leveling command will be lost and needs to be regenerated.
+     
+     The number of samples will default to 10 if not specified.  You can use upper or lower case letters for any of the options EXCEPT n.  n must be in lower case because Marlin uses a capital N for its communication protocol and will get horribly confused if you send it a capital N.
+     @todo Why would you check for both uppercase and lowercase? Seems wasteful.
+	 
+     #### Usage
+     
+	     M48 [ n | X | Y | V | L ]
+     
+     #### Parameters
+       - `n` - Number of samples. Valid values 4-50
+	   - `X` - X position for samples
+	   - `Y` - Y position for samples
+	   - `V` - Verbose level. Valid values 1-4
+	   - `L` - Legs of movementprior to doing probe. Valid values 1-15
+    */
     case 48: // M48 Z-Probe repeatability
         {
             #if Z_MIN_PIN == -1
@@ -5916,12 +6166,18 @@ Sigma_Exit:
 #endif		// Z_PROBE_REPEATABILITY_TEST 
 #endif		// ENABLE_AUTO_BED_LEVELING
 
-  //! ### M73 - Set/get print progress 
-  // -------------------------------------
-  //! _Usage:_
-  //! 
-  //!     M73 P<percent> R<time_remaining> Q<percent_silent> S<time_remaining_silent>
-  //! 
+	/*!
+	### M73 - Set/get print progress <a href="https://reprap.org/wiki/G-code#M73:_Set.2FGet_build_percentage">M73: Set/Get build percentage</a>
+	#### Usage
+    
+	    M73 [ P | R | Q | S ]
+    
+	#### Parameters
+    - `P` - Percent in normal mode
+    - `R` - Time remaining in normal mode
+    - `Q` - Percent in silent mode
+    - `S` - Time in silent mode
+   */
 	case 73: //M73 show percent done and time remaining
 		if(code_seen('P')) print_percent_done_normal = code_value();
 		if(code_seen('R')) print_time_remaining_normal = code_value();
@@ -5935,8 +6191,15 @@ Sigma_Exit:
 		}
 		break;
 
-    //! ### M104 - Set hotend temperature
-    // -----------------------------------------
+    /*!
+	### M104 - Set hotend temperature <a href="https://reprap.org/wiki/G-code#M104:_Set_Extruder_Temperature">M104: Set Extruder Temperature</a>
+	#### Usage
+    
+	    M104 [ S ]
+    
+	#### Parameters
+       - `S` - Target temperature
+    */
     case 104: // M104
     {
           uint8_t extruder;
@@ -5950,24 +6213,48 @@ Sigma_Exit:
           break;
     }
 
-    //! ### M112 - Emergency stop
-    // -----------------------------------------
+    /*!
+	### M112 - Emergency stop <a href="https://reprap.org/wiki/G-code#M112:_Full_.28Emergency.29_Stop">M112: Full (Emergency) Stop</a>
+    It is processed much earlier as to bypass the cmdqueue.
+    */
     case 112: 
       kill(MSG_M112_KILL, 3);
       break;
 
-    //! ### M140 - Set bed temperature
-    // -----------------------------------------
+    /*!
+	### M140 - Set bed temperature <a href="https://reprap.org/wiki/G-code#M140:_Set_Bed_Temperature_.28Fast.29">M140: Set Bed Temperature (Fast)</a>
+    #### Usage
+    
+	    M140 [ S ]
+    
+	#### Parameters
+       - `S` - Target temperature
+    */
     case 140: 
       if (code_seen('S')) setTargetBed(code_value());
       break;
 
-    //! ### M105 - Report temperatures
-    // ----------------------------------------- 
-    case 105:
-    {
-      uint8_t extruder;
-      if(setTargetedHotend(105, extruder)){
+    /*!
+	### M105 - Report temperatures <a href="https://reprap.org/wiki/G-code#M105:_Get_Extruder_Temperature">M105: Get Extruder Temperature</a>
+	Prints temperatures:
+	
+	  - `T:`  - Hotend (actual / target)
+	  - `B:`  - Bed (actual / target)
+	  - `Tx:` - x Tool (actual / target)
+	  - `@:`  - Hotend power
+	  - `B@:` - Bed power
+	  - `P:`  - PINDAv2 actual (only MK2.5/s and MK3/s)
+	  - `A:`  - Ambient actual (only MK3/s)
+	
+	_Example:_
+	
+	    ok T:20.2 /0.0 B:19.1 /0.0 T0:20.2 /0.0 @:0 B@:0 P:19.8 A:26.4
+	
+    */
+    case 105:
+    {
+      uint8_t extruder;
+      if(setTargetedHotend(105, extruder)){
         break;
         }
       #if defined(TEMP_0_PIN) && TEMP_0_PIN > -1
@@ -6059,16 +6346,22 @@ Sigma_Exit:
       break;
     }
 
-    //! ### M109 - Wait for extruder temperature
-    //! Parameters (not mandatory):
-    //! * S \<temp\> set extruder temperature
-    //! * R \<temp\> set extruder temperature
-    //!
-    //! Parameters S and R are treated identically.
-    //! Command always waits for both cool down and heat up.
-    //! If no parameters are supplied waits for previously
-    //! set extruder temperature.
-    // -------------------------------------------------
+    /*!
+	### M109 - Wait for extruder temperature <a href="https://reprap.org/wiki/G-code#M109:_Set_Extruder_Temperature_and_Wait">M109: Set Extruder Temperature and Wait</a>
+    #### Usage
+    
+	    M104 [ B | R | S ]
+    
+    #### Parameters (not mandatory)
+     
+	  - `S` - Set extruder temperature
+      - `R` - Set extruder temperature
+	  - `B` - Set max. extruder temperature, while `S` is min. temperature. Not active in default, only if AUTOTEMP is defined in source code.
+    
+    Parameters S and R are treated identically.
+    Command always waits for both cool down and heat up.
+    If no parameters are supplied waits for previously set extruder temperature.
+    */
     case 109:
     {
       uint8_t extruder;
@@ -6118,12 +6411,19 @@ Sigma_Exit:
       }
       break;
 
-    //! ### M190 - Wait for bed temperature
-    //! Parameters (not mandatory):
-    //! * S \<temp\> set extruder temperature and wait for heating
-    //! * R \<temp\> set extruder temperature and wait for heating or cooling
-    //!
-    //! If no parameter is supplied, waits for heating or cooling to previously set temperature.
+    /*!
+	### M190 - Wait for bed temperature <a href="https://reprap.org/wiki/G-code#M190:_Wait_for_bed_temperature_to_reach_target_temp">M190: Wait for bed temperature to reach target temp</a>
+    #### Usage
+    
+        M190 [ R | S ]
+    
+    #### Parameters (not mandatory)
+    
+	  - `S` - Set extruder temperature and wait for heating
+      - `R` - Set extruder temperature and wait for heating or cooling
+    
+    If no parameter is supplied, waits for heating or cooling to previously set temperature.
+	*/
     case 190: 
     #if defined(TEMP_BED_PIN) && TEMP_BED_PIN > -1
     {
@@ -6178,8 +6478,15 @@ Sigma_Exit:
 
     #if defined(FAN_PIN) && FAN_PIN > -1
 
-      //! ### M106 - Set fan speed
-      // -------------------------------------------
+      /*!
+	  ### M106 - Set fan speed <a href="https://reprap.org/wiki/G-code#M106:_Fan_On">M106: Fan On</a>
+      #### Usage
+      
+        M106 [ S ]
+        
+      #### Parameters
+      - `S` - Specifies the duty cycle of the print fan. Allowed values are 0-255. If it's omitted, a value of 255 is used.
+      */
       case 106: // M106 Sxxx Fan On S<speed> 0 .. 255
         if (code_seen('S')){
            fanSpeed=constrain(code_value(),0,255);
@@ -6189,8 +6496,9 @@ Sigma_Exit:
         }
         break;
 
-      //! ### M107 - Fan off
-      // -------------------------------
+      /*!
+	  ### M107 - Fan off <a href="https://reprap.org/wiki/G-code#M107:_Fan_Off">M107: Fan Off</a>
+      */
       case 107:
         fanSpeed = 0;
         break;
@@ -6198,8 +6506,10 @@ Sigma_Exit:
 
     #if defined(PS_ON_PIN) && PS_ON_PIN > -1
 
-      //! ### M80 - Turn on the Power Supply
-      // -------------------------------
+      /*!
+	  ### M80 - Turn on the Power Supply <a href="https://reprap.org/wiki/G-code#M80:_ATX_Power_On">M80: ATX Power On</a>
+      Only works if the firmware is compiled with PS_ON_PIN defined.
+      */
       case 80:
         SET_OUTPUT(PS_ON_PIN); //GND
         WRITE(PS_ON_PIN, PS_ON_AWAKE);
@@ -6217,8 +6527,10 @@ Sigma_Exit:
           lcd_update(0);
         break;
 
-      //! ### M81 - Turn off Power Supply
-      // --------------------------------------
+      /*!
+	  ### M81 - Turn off Power Supply <a href="https://reprap.org/wiki/G-code#M81:_ATX_Power_Off">M81: ATX Power Off</a>
+      Only works if the firmware is compiled with PS_ON_PIN defined.
+      */
       case 81: 
         disable_heater();
         st_synchronize();
@@ -6241,24 +6553,40 @@ Sigma_Exit:
 	  break;
     #endif
 
-    //! ### M82 - Set E axis to absolute mode
-    // ---------------------------------------
+    /*!
+	### M82 - Set E axis to absolute mode <a href="https://reprap.org/wiki/G-code#M82:_Set_extruder_to_absolute_mode">M82: Set extruder to absolute mode</a>
+	Makes the extruder interpret extrusion as absolute positions.
+    */
     case 82:
       axis_relative_modes[E_AXIS] = false;
       break;
 
-    //! ### M83 - Set E axis to relative mode
-    // ---------------------------------------  
+    /*!
+	### M83 - Set E axis to relative mode <a href="https://reprap.org/wiki/G-code#M83:_Set_extruder_to_relative_mode">M83: Set extruder to relative mode</a>
+	Makes the extruder interpret extrusion values as relative positions.
+    */
     case 83:
       axis_relative_modes[E_AXIS] = true;
       break;
 
-    //! ### M84, M18 - Disable steppers
-    //---------------------------------------
-    //! This command can be used to set the stepper inactivity timeout (`S`) or to disable steppers (`X`,`Y`,`Z`,`E`)
-    //! 
-    //!     M84 [E<flag>] [S<seconds>] [X<flag>] [Y<flag>] [Z<flag>]  
-    //! 
+    /*!
+	### M84 - Disable steppers <a href="https://reprap.org/wiki/G-code#M84:_Stop_idle_hold">M84: Stop idle hold</a>
+    This command can be used to set the stepper inactivity timeout (`S`) or to disable steppers (`X`,`Y`,`Z`,`E`)
+	This command can be used without any additional parameters. In that case all steppers are disabled.
+    
+    The file completeness check uses this parameter to detect an incomplete file. It has to be present at the end of a file with no parameters.
+	
+        M84 [ S | X | Y | Z | E ]
+	
+	  - `S` - Seconds
+	  - `X` - X axis
+	  - `Y` - Y axis
+	  - `Z` - Z axis
+	  - `E` - Exruder
+
+	### M18 - Disable steppers <a href="https://reprap.org/wiki/G-code#M18:_Disable_all_stepper_motors">M18: Disable all stepper motors</a>
+	Equal to M84 (compatibility)
+    */
     case 18: //compatibility
     case 84: // M84
       if(code_seen('S')){
@@ -6295,8 +6623,15 @@ Sigma_Exit:
 	  snmm_filaments_used = 0;
       break;
 
-    //! ### M85 - Set max inactive time
-    // ---------------------------------------
+    /*!
+	### M85 - Set max inactive time <a href="https://reprap.org/wiki/G-code#M85:_Set_Inactivity_Shutdown_Timer">M85: Set Inactivity Shutdown Timer</a>
+    #### Usage
+    
+        M85 [ S ]
+    
+    #### Parameters
+    - `S` - specifies the time in seconds. If a value of 0 is specified, the timer is disabled.
+    */
     case 85: // M85
       if(code_seen('S')) {
         max_inactive_time = code_value() * 1000;
@@ -6304,13 +6639,16 @@ Sigma_Exit:
       break;
 #ifdef SAFETYTIMER
 
-  //! ### M86 - Set safety timer expiration time
-  //!
-  //! _Usage:_
-  //!     M86 S<seconds>
-  //! 
-  //! Sets the safety timer expiration time in seconds. M86 S0 will disable safety timer.
-  //! When safety timer expires, heatbed and nozzle target temperatures are set to zero.
+    /*!
+    ### M86 - Set safety timer expiration time <a href="https://reprap.org/wiki/G-code#M86:_Set_Safety_Timer_expiration_time">M86: Set Safety Timer expiration time</a>	
+    When safety timer expires, heatbed and nozzle target temperatures are set to zero.
+    #### Usage
+    
+        M86 [ S ]
+    
+    #### Parameters
+    - `S` - specifies the time in seconds. If a value of 0 is specified, the timer is disabled.
+    */
 	case 86: 
 	  if (code_seen('S')) {
 	    safetytimer_inactive_time = code_value() * 1000;
@@ -6319,15 +6657,25 @@ Sigma_Exit:
 	  break;
 #endif
 
-    //! ### M92 Set Axis steps-per-unit
-    // ---------------------------------------
-    //! Same syntax as G92
+    /*!
+	### M92 Set Axis steps-per-unit <a href="https://reprap.org/wiki/G-code#M92:_Set_axis_steps_per_unit">M92: Set axis_steps_per_unit</a>
+	Allows programming of steps per unit (usually mm) for motor drives. These values are reset to firmware defaults on power on, unless saved to EEPROM if available (M500 in Marlin)
+	#### Usage
+    
+	    M92 [ X | Y | Z | E ]
+	
+    #### Parameters
+	- `X` - Steps per unit for the X drive
+	- `Y` - Steps per unit for the Y drive
+	- `Z` - Steps per unit for the Z drive
+	- `E` - Steps per unit for the extruder drive
+    */
     case 92:
       for(int8_t i=0; i < NUM_AXIS; i++)
       {
         if(code_seen(axis_codes[i]))
         {
-          if(i == 3) { // E
+          if(i == E_AXIS) { // E
             float value = code_value();
             if(value < 20.0) {
               float factor = cs.axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab.
@@ -6336,6 +6684,9 @@ Sigma_Exit:
               axis_steps_per_sqr_second[i] *= factor;
             }
             cs.axis_steps_per_unit[i] = value;
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+            fsensor_set_axis_steps_per_unit(value);
+#endif
           }
           else {
             cs.axis_steps_per_unit[i] = code_value();
@@ -6344,15 +6695,31 @@ Sigma_Exit:
       }
       break;
 
-    //! ### M110 - Set Line number
-    // ---------------------------------------
+    /*!
+	### M110 - Set Line number <a href="https://reprap.org/wiki/G-code#M110:_Set_Current_Line_Number">M110: Set Current Line Number</a>
+	Sets the line number in G-code
+	#### Usage
+    
+	    M110 [ N ]
+	
+    #### Parameters
+	- `N` - Line number
+    */
     case 110:
       if (code_seen('N'))
 	    gcode_LastN = code_value_long();
     break;
 
-  //! ### M113 - Get or set host keep-alive interval
-  // ------------------------------------------ 
+    /*!
+    ### M113 - Get or set host keep-alive interval <a href="https://reprap.org/wiki/G-code#M113:_Host_Keepalive">M113: Host Keepalive</a>
+    During some lengthy processes, such as G29, Marlin may appear to the host to have “gone away.” The “host keepalive” feature will send messages to the host when Marlin is busy or waiting for user response so the host won’t try to reconnect (or disconnect).
+    #### Usage
+    
+        M113 [ S ]
+	
+    #### Parameters
+	- `S` - Seconds. Default is 2 seconds between "busy" messages
+    */
 	case 113:
 		if (code_seen('S')) {
 			host_keepalive_interval = (uint8_t)code_value_short();
@@ -6365,16 +6732,34 @@ Sigma_Exit:
 		}
 		break;
 
-    //! ### M115 - Firmware info
-    // --------------------------------------
-    //! Print the firmware info and capabilities
-    //! 
-    //!     M115 [V] [U<version>] 
-    //! 
-    //! Without any arguments, prints Prusa firmware version number, machine type, extruder count and UUID.
-    //! `M115 U` Checks the firmware version provided. If the firmware version provided by the U code is higher than the currently running firmware,
-    //!  pause the print for 30s and ask the user to upgrade the firmware. 
-    case 115: // M115
+    /*!
+	### M115 - Firmware info <a href="https://reprap.org/wiki/G-code#M115:_Get_Firmware_Version_and_Capabilities">M115: Get Firmware Version and Capabilities</a>
+    Print the firmware info and capabilities
+    Without any arguments, prints Prusa firmware version number, machine type, extruder count and UUID.
+    `M115 U` Checks the firmware version provided. If the firmware version provided by the U code is higher than the currently running firmware, it will pause the print for 30s and ask the user to upgrade the firmware.
+	
+	_Examples:_
+	
+	`M115` results:
+	
+	`FIRMWARE_NAME:Prusa-Firmware 3.8.1 based on Marlin FIRMWARE_URL:https://github.com/prusa3d/Prusa-Firmware PROTOCOL_VERSION:1.0 MACHINE_TYPE:Prusa i3 MK3S EXTRUDER_COUNT:1 UUID:00000000-0000-0000-0000-000000000000`
+	
+	`M115 V` results:
+	
+	`3.8.1`
+	
+	`M115 U3.8.2-RC1` results on LCD display for 30s or user interaction:
+	
+	`New firmware version available: 3.8.2-RC1 Please upgrade.`
+    #### Usage
+    
+        M115 [ V | U ]
+	
+    #### Parameters
+	- V - Report current installed firmware version
+	- U - Firmware version provided by G-code to be compared to current one.  
+	*/
+	case 115: // M115
       if (code_seen('V')) {
           // Report the Prusa version number.
           SERIAL_PROTOCOLLNRPGM(FW_VERSION_STR_P());
@@ -6396,16 +6781,13 @@ Sigma_Exit:
       }
       break;
 
-    //! ### M114 - Get current position
-    // -------------------------------------
+    /*!
+	### M114 - Get current position <a href="https://reprap.org/wiki/G-code#M114:_Get_Current_Position">M114: Get Current Position</a>
+    */
     case 114:
 		gcode_M114();
       break;
 
-
-
-      //! ### M117 - Set LCD Message
-      // -------------------------------------- 
       
       /*
         M117 moved up to get the high priority
@@ -6417,20 +6799,24 @@ Sigma_Exit:
       lcd_setstatus(strchr_pointer + 5);
       break;*/
 
-    //! ### M120 - Disable endstops
-    // ----------------------------------------
+    /*!
+	### M120 - Enable endstops <a href="https://reprap.org/wiki/G-code#M120:_Enable_endstop_detection">M120: Enable endstop detection</a>
+    */
     case 120:
       enable_endstops(false) ;
       break;
 
-    //! ### M121 - Enable endstops
-    // ----------------------------------------
+    /*!
+	### M121 - Disable endstops <a href="https://reprap.org/wiki/G-code#M121:_Disable_endstop_detection">M121: Disable endstop detection</a>
+    */
     case 121:
       enable_endstops(true) ;
       break;
 
-    //! ### M119 - Get endstop states
-    // ----------------------------------------
+    /*!
+	### M119 - Get endstop states <a href="https://reprap.org/wiki/G-code#M119:_Get_Endstop_Status">M119: Get Endstop Status</a>
+	Returns the current state of the configured X, Y, Z endstops. Takes into account any 'inverted endstop' settings, so one can confirm that the machine is interpreting the endstops correctly.
+    */
     case 119:
     SERIAL_PROTOCOLRPGM(_N("Reporting endstop status"));////MSG_M119_REPORT
     SERIAL_PROTOCOLLN("");
@@ -6489,12 +6875,22 @@ Sigma_Exit:
         SERIAL_PROTOCOLLN("");
       #endif
       break;
-      //TODO: update for all axis, use for loop
+      //!@todo update for all axes, use for loop
     
-    #ifdef BLINKM
 
-    //! ### M150 - Set RGB(W) Color
-    // -------------------------------------------
+    #ifdef BLINKM
+    /*!
+	### M150 - Set RGB(W) Color <a href="https://reprap.org/wiki/G-code#M150:_Set_LED_color">M150: Set LED color</a>
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code by defining BLINKM and its dependencies.
+    #### Usage
+    
+        M150 [ R | U | B ]
+    
+    #### Parameters
+    - `R` - Red color value
+    - `U` - Green color value. It is NOT `G`!
+    - `B` - Blue color value
+    */
     case 150:
       {
         byte red;
@@ -6510,8 +6906,16 @@ Sigma_Exit:
       break;
     #endif //BLINKM
 
-    //! ### M200 - Set filament diameter
-    // ----------------------------------------
+    /*!
+	### M200 - Set filament diameter <a href="https://reprap.org/wiki/G-code#M200:_Set_filament_diameter">M200: Set filament diameter</a>
+	#### Usage
+    
+	    M200 [ D | T ]
+	
+    #### Parameters
+	  - `D` - Diameter in mm
+	  - `T` - Number of extruder (MMUs)
+    */
     case 200: // M200 D<millimeters> set filament diameter and set E axis units to cubic millimeters (use S0 to set back to millimeters).
       {
 
@@ -6536,10 +6940,10 @@ Sigma_Exit:
 			// make sure all extruders have some sane value for the filament size
 			cs.filament_size[0] = (cs.filament_size[0] == 0.0 ? DEFAULT_NOMINAL_FILAMENT_DIA : cs.filament_size[0]);
             #if EXTRUDERS > 1
-			cs.filament_size[1] = (cs.filament_size[1] == 0.0 ? DEFAULT_NOMINAL_FILAMENT_DIA : cs.filament_size[1]);
-            #if EXTRUDERS > 2
-			cs.filament_size[2] = (cs.filament_size[2] == 0.0 ? DEFAULT_NOMINAL_FILAMENT_DIA : cs.filament_size[2]);
-            #endif
+				cs.filament_size[1] = (cs.filament_size[1] == 0.0 ? DEFAULT_NOMINAL_FILAMENT_DIA : cs.filament_size[1]);
+				#if EXTRUDERS > 2
+					cs.filament_size[2] = (cs.filament_size[2] == 0.0 ? DEFAULT_NOMINAL_FILAMENT_DIA : cs.filament_size[2]);
+				#endif
             #endif
 			cs.volumetric_enabled = true;
 		  }
@@ -6551,8 +6955,10 @@ Sigma_Exit:
       }
       break;
 
-    //! ### M201 - Set Print Max Acceleration
-    // -------------------------------------------
+    /*!
+	### M201 - Set Print Max Acceleration <a href="https://reprap.org/wiki/G-code#M201:_Set_max_printing_acceleration">M201: Set max printing acceleration</a>
+    For each axis individually.
+    */
     case 201:
 		for (int8_t i = 0; i < NUM_AXIS; i++)
 		{
@@ -6586,8 +6992,10 @@ Sigma_Exit:
       break;
     #endif
 
-    //! ### M203 - Set Max Feedrate
-    // ---------------------------------------
+    /*!
+	### M203 - Set Max Feedrate <a href="https://reprap.org/wiki/G-code#M203:_Set_maximum_feedrate">M203: Set maximum feedrate</a>
+    For each axis individually.
+    */
     case 203: // M203 max feedrate mm/sec
 		for (int8_t i = 0; i < NUM_AXIS; i++)
 		{
@@ -6612,15 +7020,28 @@ Sigma_Exit:
 		}
 		break;
 
-    //! ### M204 - Acceleration settings
-    // ------------------------------------------
-    //! Supporting old format: 
-    //!
-    //!         M204 S[normal moves] T[filmanent only moves]
-    //!
-    //! and new format:        
-    //!
-    //!         M204 P[printing moves] R[filmanent only moves] T[travel moves] (as of now T is ignored)
+    /*!
+	### M204 - Acceleration settings <a href="https://reprap.org/wiki/G-code#M204:_Set_default_acceleration">M204: Set default acceleration</a>
+
+    #### Old format:
+    ##### Usage
+    
+        M204 [ S | T ]
+        
+    ##### Parameters
+    - `S` - normal moves
+    - `T` - filmanent only moves
+    
+    #### New format:
+    ##### Usage
+    
+        M204 [ P | R | T ]
+    
+    ##### Parameters
+    - `P` - printing moves
+    - `R` - filmanent only moves
+    - `T` - travel moves (as of now T is ignored)
+	*/
     case 204:
       {
         if(code_seen('S')) {
@@ -6639,23 +7060,30 @@ Sigma_Exit:
             cs.retract_acceleration = code_value();
           if(code_seen('T')) {
             // Interpret the T value as the travel acceleration in the new Marlin format.
-            //FIXME Prusa3D firmware currently does not support travel acceleration value independent from the extruding acceleration value.
+            /*!
+            @todo Prusa3D firmware currently does not support travel acceleration value independent from the extruding acceleration value.
+            */
             // travel_acceleration = code_value();
           }
         }
       }
       break;
 
-    //! ### M205 - Set advanced settings
-    // --------------------------------------------- 
-    //! Set some advanced settings related to movement.
-    //!
-    //!          M205 [S] [T] [B] [X] [Y] [Z] [E]
     /*!
+	### M205 - Set advanced settings <a href="https://reprap.org/wiki/G-code#M205:_Advanced_settings">M205: Advanced settings</a>
+    Set some advanced settings related to movement.
+    #### Usage
+    
+        M205 [ S | T | B | X | Y | Z | E ]
+        
+    #### Parameters
     - `S` - Minimum feedrate for print moves (unit/s)
     - `T` - Minimum feedrate for travel moves (units/s)
     - `B` - Minimum segment time (us)
-    - `X` - Maximum X jerk (units/s), similarly for other axes
+    - `X` - Maximum X jerk (units/s)
+    - `Y` - Maximum Y jerk (units/s)
+    - `Z` - Maximum Z jerk (units/s)
+    - `E` - Maximum E jerk (units/s)
     */
     case 205: 
     {
@@ -6665,14 +7093,31 @@ Sigma_Exit:
       if(code_seen('X')) cs.max_jerk[X_AXIS] = cs.max_jerk[Y_AXIS] = code_value();
       if(code_seen('Y')) cs.max_jerk[Y_AXIS] = code_value();
       if(code_seen('Z')) cs.max_jerk[Z_AXIS] = code_value();
-      if(code_seen('E')) cs.max_jerk[E_AXIS] = code_value();
-		if (cs.max_jerk[X_AXIS] > DEFAULT_XJERK) cs.max_jerk[X_AXIS] = DEFAULT_XJERK;
-		if (cs.max_jerk[Y_AXIS] > DEFAULT_YJERK) cs.max_jerk[Y_AXIS] = DEFAULT_YJERK;
+      if(code_seen('E'))
+      {
+          float e = code_value();
+#ifndef LA_NOCOMPAT
+
+          e = la10c_jerk(e);
+#endif
+          cs.max_jerk[E_AXIS] = e;
+      }
+      if (cs.max_jerk[X_AXIS] > DEFAULT_XJERK) cs.max_jerk[X_AXIS] = DEFAULT_XJERK;
+      if (cs.max_jerk[Y_AXIS] > DEFAULT_YJERK) cs.max_jerk[Y_AXIS] = DEFAULT_YJERK;
     }
     break;
 
-    //! ### M206 - Set additional homing offsets
-    // ----------------------------------------------
+    /*!
+	### M206 - Set additional homing offsets <a href="https://reprap.org/wiki/G-code#M206:_Offset_axes">M206: Offset axes</a>
+    #### Usage
+    
+        M206 [ X | Y | Z ]
+    
+    #### Parameters
+    - `X` - X axis offset
+    - `Y` - Y axis offset
+    - `Z` - Z axis offset
+	*/
     case 206:
       for(int8_t i=0; i < 3; i++)
       {
@@ -6681,8 +7126,17 @@ Sigma_Exit:
       break;
     #ifdef FWRETRACT
 
-    //! ### M207 - Set firmware retraction
-    // --------------------------------------------------
+    /*!
+	### M207 - Set firmware retraction <a href="https://reprap.org/wiki/G-code#M207:_Set_retract_length">M207: Set retract length</a>
+	#### Usage
+    
+        M207 [ S | F | Z ]
+    
+    #### Parameters
+    - `S` - positive length to retract, in mm
+    - `F` - retraction feedrate, in mm/min
+    - `Z` - additional zlift/hop
+    */
     case 207: //M207 - set retract length S[positive mm] F[feedrate mm/min] Z[additional zlift/hop]
     {
       if(code_seen('S'))
@@ -6699,8 +7153,16 @@ Sigma_Exit:
       }
     }break;
 
-    //! ### M208 - Set retract recover length
-    // --------------------------------------------
+    /*!
+	### M208 - Set retract recover length <a href="https://reprap.org/wiki/G-code#M208:_Set_unretract_length">M208: Set unretract length</a>
+	#### Usage
+    
+        M208 [ S | F ]
+    
+    #### Parameters
+    - `S` - positive length surplus to the M207 Snnn, in mm
+    - `F` - feedrate, in mm/sec
+    */
     case 208: // M208 - set retract recover length S[positive mm surplus to the M207 S*] F[feedrate mm/min]
     {
       if(code_seen('S'))
@@ -6713,8 +7175,16 @@ Sigma_Exit:
       }
     }break;
 
-    //! ### M209 - Enable/disable automatict retract
-    // ---------------------------------------------
+    /*!
+	### M209 - Enable/disable automatict retract <a href="https://reprap.org/wiki/G-code#M209:_Enable_automatic_retract">M209: Enable automatic retract</a>
+	This boolean value S 1=true or 0=false enables automatic retract detect if the slicer did not support G10/G11: every normal extrude-only move will be classified as retract depending on the direction.
+    #### Usage
+    
+        M209 [ S ]
+        
+    #### Parameters
+    - `S` - 1=true or 0=false
+    */
     case 209: // M209 - S<1=true/0=false> enable automatic retract detect if the slicer did not support G10/11: every normal extrude-only move will be classified as retract depending on the direction.
     {
       if(code_seen('S'))
@@ -6756,8 +7226,17 @@ Sigma_Exit:
     #endif // FWRETRACT
     #if EXTRUDERS > 1
 
-    // ### M218 - Set hotend offset
-    // ----------------------------------------
+    /*!
+	### M218 - Set hotend offset <a href="https://reprap.org/wiki/G-code#M218:_Set_Hotend_Offset">M218: Set Hotend Offset</a>
+	In Prusa Firmware this G-code is only active if `EXTRUDERS` is higher then 1 in the source code. On Original i3 Prusa MK2/s MK2.5/s MK3/s it is not active.
+    #### Usage
+    
+        M218 [ X | Y ]
+        
+    #### Parameters
+    - `X` - X offset
+    - `Y` - Y offset
+    */
     case 218: // M218 - set hotend offset (in mm), T<extruder_number> X<offset_on_X> Y<offset_on_Y>
     {
       uint8_t extruder;
@@ -6785,8 +7264,17 @@ Sigma_Exit:
     }break;
     #endif
 
-    //! ### M220 Set feedrate percentage
-    // -----------------------------------------------
+    /*!
+	### M220 Set feedrate percentage <a href="https://reprap.org/wiki/G-code#M220:_Set_speed_factor_override_percentage">M220: Set speed factor override percentage</a>
+	#### Usage
+    
+        M220 [ B | S | R ]
+    
+    #### Parameters
+    - `B` - Backup current speed factor
+	- `S` - Speed factor override percentage (0..100 or higher)
+	- `R` - Restore previous speed factor
+    */
     case 220: // M220 S<factor in percent>- set speed factor override percentage
     {
       if (code_seen('B')) //backup current speed factor
@@ -6803,8 +7291,16 @@ Sigma_Exit:
     }
     break;
 
-    //! ### M221 - Set extrude factor override percentage
-    // ----------------------------------------------------
+    /*!
+	### M221 - Set extrude factor override percentage <a href="https://reprap.org/wiki/G-code#M221:_Set_extrude_factor_override_percentage">M221: Set extrude factor override percentage</a>
+	#### Usage
+    
+        M221 [ S | T ]
+    
+    #### Parameters
+	- `S` - Extrude factor override percentage (0..100 or higher), default 100%
+	- `T` - Extruder drive number (Prusa Firmware only), default 0 if not set.
+    */
     case 221: // M221 S<factor in percent>- set extrude factor override percentage
     {
       if(code_seen('S'))
@@ -6827,8 +7323,17 @@ Sigma_Exit:
     }
     break;
 
-  //! ### M226 - Wait for Pin state
-  // ------------------------------------------
+    /*!
+    ### M226 - Wait for Pin state <a href="https://reprap.org/wiki/G-code#M226:_Wait_for_pin_state">M226: Wait for pin state</a>
+    Wait until the specified pin reaches the state required
+    #### Usage
+    
+        M226 [ P | S ]
+    
+    #### Parameters
+    - `P` - pin number
+    - `S` - pin state
+    */
 	case 226: // M226 P<pin number> S<pin state>- Wait until the specified pin reaches the state required
 	{
       if(code_seen('P')){
@@ -6883,8 +7388,17 @@ Sigma_Exit:
 
     #if NUM_SERVOS > 0
 
-    //! ### M280 - Set/Get servo position
-    // --------------------------------------------
+    /*!
+	### M280 - Set/Get servo position <a href="https://reprap.org/wiki/G-code#M280:_Set_servo_position">M280: Set servo position</a>
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+    #### Usage
+    
+        M280 [ P | S ]
+    
+    #### Parameters
+    - `P` - Servo index (id)
+    - `S` - Target position
+    */
     case 280: // M280 - set servo position absolute. P: servo index, S: angle or microseconds
       {
         int servo_index = -1;
@@ -6924,8 +7438,17 @@ Sigma_Exit:
 
     #if (LARGE_FLASH == true && ( BEEPER > 0 || defined(ULTRALCD) || defined(LCD_USE_I2C_BUZZER)))
     
-    //! ### M300 - Play tone
-    // -----------------------
+    /*!
+	### M300 - Play tone <a href="https://reprap.org/wiki/G-code#M300:_Play_beep_sound">M300: Play beep sound</a>
+	In Prusa Firmware the defaults are `100Hz` and `1000ms`, so that `M300` without parameters will beep for a second.
+    #### Usage
+    
+        M300 [ S | P ]
+    
+    #### Parameters
+    - `S` - frequency in Hz. Not all firmware versions support this parameter
+    - `P` - duration in milliseconds
+    */
     case 300: // M300
     {
       int beepS = code_seen('S') ? code_value() : 110;
@@ -6946,8 +7469,20 @@ Sigma_Exit:
 
     #ifdef PIDTEMP
 
-    //! ### M301 - Set hotend PID
-    // ---------------------------------------
+    /*!
+	### M301 - Set hotend PID <a href="https://reprap.org/wiki/G-code#M301:_Set_PID_parameters">M301: Set PID parameters</a>
+	Sets Proportional (P), Integral (I) and Derivative (D) values for hot end.
+    See also <a href="https://reprap.org/wiki/PID_Tuning">PID Tuning.</a>
+    #### Usage
+    
+        M301 [ P | I | D | C ]
+    
+    #### Parameters
+    - `P` - proportional (Kp)
+    - `I` - integral (Ki)
+    - `D` - derivative (Kd)
+    - `C` - heating power=Kc*(e_speed0)  
+    */
     case 301:
       {
         if(code_seen('P')) cs.Kp = code_value();
@@ -6977,8 +7512,19 @@ Sigma_Exit:
     #endif //PIDTEMP
     #ifdef PIDTEMPBED
 
-    //! ### M304 - Set bed PID 
-    // --------------------------------------
+    /*!
+	### M304 - Set bed PID  <a href="https://reprap.org/wiki/G-code#M304:_Set_PID_parameters_-_Bed">M304: Set PID parameters - Bed</a>
+	Sets Proportional (P), Integral (I) and Derivative (D) values for bed.
+    See also <a href="https://reprap.org/wiki/PID_Tuning">PID Tuning.</a>
+    #### Usage
+    
+        M304 [ P | I | D ]
+    
+    #### Parameters
+    - `P` - proportional (Kp)
+    - `I` - integral (Ki)
+    - `D` - derivative (Kd)
+    */
     case 304:
       {
         if(code_seen('P')) cs.bedKp = code_value();
@@ -6998,8 +7544,13 @@ Sigma_Exit:
       break;
     #endif //PIDTEMP
 
-    //! ### M240 - Trigger camera
-    // --------------------------------------------
+    /*!
+	### M240 - Trigger camera <a href="https://reprap.org/wiki/G-code#M240:_Trigger_camera">M240: Trigger camera</a>
+	
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code.
+	
+	You need to (re)define and assign `CHDK` or `PHOTOGRAPH_PIN` the correct pin number to be able to use the feature.
+    */
     case 240: // M240  Triggers a camera by emulating a Canon RC-1 : http://www.doc-diy.net/photo/rc-1_hacked/
      {
      	#ifdef CHDK
@@ -7033,8 +7584,16 @@ Sigma_Exit:
     break;
     #ifdef PREVENT_DANGEROUS_EXTRUDE
 
-    //! ### M302 - Allow cold extrude, or set minimum extrude temperature
-    // -------------------------------------------------------------------
+    /*!
+	### M302 - Allow cold extrude, or set minimum extrude temperature <a href="https://reprap.org/wiki/G-code#M302:_Allow_cold_extrudes">M302: Allow cold extrudes</a>
+    This tells the printer to allow movement of the extruder motor above a certain temperature, or if disabled, to allow extruder movement when the hotend is below a safe printing temperature.
+    #### Usage
+    
+        M302 [ S ]
+    
+    #### Parameters
+    - `S` - Cold extrude minimum temperature
+    */
     case 302:
     {
 	  float temp = .0;
@@ -7044,8 +7603,18 @@ Sigma_Exit:
     break;
 	#endif
 
-    //! ### M303 - PID autotune
-    // -------------------------------------
+    /*!
+	### M303 - PID autotune <a href="https://reprap.org/wiki/G-code#M303:_Run_PID_tuning">M303: Run PID tuning</a>
+    PID Tuning refers to a control algorithm used in some repraps to tune heating behavior for hot ends and heated beds. This command generates Proportional (Kp), Integral (Ki), and Derivative (Kd) values for the hotend or bed. Send the appropriate code and wait for the output to update the firmware values.
+    #### Usage
+    
+        M303 [ E | S | C ]
+    
+    #### Parameters
+      - `E` - Extruder, default `E0`. Use `E-1` to calibrate the bed PID
+      - `S` - Target temperature, default `210°C` for hotend, 70 for bed
+      - `C` - Cycles, default `5`
+	*/
     case 303:
     {
       float temp = 150.0;
@@ -7060,17 +7629,30 @@ Sigma_Exit:
     }
     break;
     
-    //! ### M400 - Wait for all moves to finish
-    // -----------------------------------------
+    /*!
+	### M400 - Wait for all moves to finish <a href="https://reprap.org/wiki/G-code#M400:_Wait_for_current_moves_to_finish">M400: Wait for current moves to finish</a>
+	Finishes all current moves and and thus clears the buffer.
+    Equivalent to `G4` with no parameters.
+    */
     case 400:
     {
       st_synchronize();
     }
     break;
 
-  //! ### M403 - Set filament type (material) for particular extruder and notify the MMU
-	// ----------------------------------------------
-  case 403:
+    /*!
+	### M403 - Set filament type (material) for particular extruder and notify the MMU <a href="https://reprap.org/wiki/G-code#M403:_Set_filament_type_.28material.29_for_particular_extruder_and_notify_the_MMU.">M403 - Set filament type (material) for particular extruder and notify the MMU</a>
+    Currently three different materials are needed (default, flex and PVA).  
+    And storing this information for different load/unload profiles etc. in the future firmware does not have to wait for "ok" from MMU.
+    #### Usage
+    
+        M403 [ E | F ]
+    
+    #### Parameters
+    - `E` - Extruder number. 0-indexed.
+    - `F` - Filament type
+	*/
+    case 403:
 	{
 		// currently three different materials are needed (default, flex and PVA)
 		// add storing this information for different load/unload profiles etc. in the future
@@ -7086,40 +7668,51 @@ Sigma_Exit:
 	}
 	break;
 
-    //! ### M500 - Store settings in EEPROM
-    // -----------------------------------------
+    /*!
+	### M500 - Store settings in EEPROM <a href="https://reprap.org/wiki/G-code#M500:_Store_parameters_in_non-volatile_storage">M500: Store parameters in non-volatile storage</a>
+	Save current parameters to EEPROM.
+    */
     case 500:
     {
         Config_StoreSettings();
     }
     break;
 
-    //! ### M501 - Read settings from EEPROM
-    // ----------------------------------------
+    /*!
+	### M501 - Read settings from EEPROM <a href="https://reprap.org/wiki/G-code#M501:_Read_parameters_from_EEPROM">M501: Read parameters from EEPROM</a>
+	Set the active parameters to those stored in the EEPROM. This is useful to revert parameters after experimenting with them.
+    */
     case 501:
     {
         Config_RetrieveSettings();
     }
     break;
 
-    //! ### M502 - Revert all settings to factory default
-    // -------------------------------------------------
+    /*!
+	### M502 - Revert all settings to factory default <a href="https://reprap.org/wiki/G-code#M502:_Restore_Default_Settings">M502: Restore Default Settings</a>
+	This command resets all tunable parameters to their default values, as set in the firmware's configuration files. This doesn't reset any parameters stored in the EEPROM, so it must be followed by M500 to write the default settings.
+    */
     case 502:
     {
         Config_ResetDefault();
     }
     break;
 
-    //! ### M503 - Repport all settings currently in memory
-    // -------------------------------------------------
+    /*!
+	### M503 - Repport all settings currently in memory <a href="https://reprap.org/wiki/G-code#M503:_Report_Current_Settings">M503: Report Current Settings</a>
+	This command asks the firmware to reply with the current print settings as set in memory. Settings will differ from EEPROM contents if changed since the last load / save. The reply output includes the G-Code commands to produce each setting. For example, Steps-Per-Unit values are displayed as an M92 command.
+    */
     case 503:
     {
         Config_PrintSettings();
     }
     break;
 
-    //! ### M509 - Force language selection
-    // ------------------------------------------------
+    /*!
+	### M509 - Force language selection <a href="https://reprap.org/wiki/G-code#M509:_Force_language_selection">M509: Force language selection</a>
+	Resets the language to English.
+	Only on Original Prusa i3 MK2.5/s and MK3/s with multiple languages.
+	*/
     case 509:
     {
 		lang_reset();
@@ -7129,8 +7722,16 @@ Sigma_Exit:
     break;
     #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
 
-    //! ### M540 - Abort print on endstop hit (enable/disable)
-    // -----------------------------------------------------
+    /*!
+	### M540 - Abort print on endstop hit (enable/disable) <a href="https://reprap.org/wiki/G-code#M540_in_Marlin:_Enable.2FDisable_.22Stop_SD_Print_on_Endstop_Hit.22">M540 in Marlin: Enable/Disable "Stop SD Print on Endstop Hit"</a>
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code. You must define `ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED`.
+    #### Usage
+    
+        M540 [ S ]
+    
+    #### Parameters
+    - `S` - disabled=0, enabled=1
+	*/
     case 540:
     {
         if(code_seen('S')) abort_on_endstop_hit = code_value() > 0;
@@ -7138,6 +7739,17 @@ Sigma_Exit:
     break;
     #endif
 
+	/*!
+	### M851 - Set Z-Probe Offset <a href="https://reprap.org/wiki/G-code#M851:_Set_Z-Probe_Offset">M851: Set Z-Probe Offset"</a>
+    Sets the Z-probe Z offset. This offset is used to determine the actual Z position of the nozzle when using a probe to home Z with G28. This value may also be used by G81 (Prusa) / G29 (Marlin) to apply correction to the Z position.
+	This value represents the distance from nozzle to the bed surface at the point where the probe is triggered. This value will be negative for typical switch probes, inductive probes, and setups where the nozzle makes a circuit with a raised metal contact. This setting will be greater than zero on machines where the nozzle itself is used as the probe, pressing down on the bed to press a switch. (This is a common setup on delta machines.)
+    #### Usage
+    
+        M851 [ Z ]
+    
+    #### Parameters
+    - `Z` - Z offset probe to nozzle.
+	*/
     #ifdef CUSTOM_M_CODE_SET_Z_PROBE_OFFSET
     case CUSTOM_M_CODE_SET_Z_PROBE_OFFSET:
     {
@@ -7176,8 +7788,21 @@ Sigma_Exit:
 
     #ifdef FILAMENTCHANGEENABLE
 
-    //! ### M600 - Initiate Filament change procedure
-    // --------------------------------------
+    /*!
+	### M600 - Initiate Filament change procedure <a href="https://reprap.org/wiki/G-code#M600:_Filament_change_pause">M600: Filament change pause</a>
+    Initiates Filament change, it is also used during Filament Runout Sensor process.
+	If the `M600` is triggered under 25mm it will do a Z-lift of 25mm to prevent a filament blob.
+    #### Usage
+    
+        M600 [ X | Y | Z | E | L | AUTO ]
+      
+    - `X`    - X position, default 211
+    - `Y`    - Y position, default 0
+    - `Z`    - relative lift Z, default 2.
+    - `E`    - initial retract, default -2
+    - `L`    - later retract distance for removal, default -80
+    - `AUTO` - Automatically (only with MMU)
+    */
     case 600: //Pause for filament change X[pos] Y[pos] Z[relative lift] E[initial retract] L[later retract distance for removal]
 	{
 		st_synchronize();
@@ -7253,10 +7878,15 @@ Sigma_Exit:
     break;
     #endif //FILAMENTCHANGEENABLE
 
-  //! ### M25 - Pause SD print
-  //! ### M601 - Pause print
-  //! ### M125 - Pause print (TODO: not implemented)
-  // -------------------------------
+    /*!
+    ### M601 - Pause print <a href="https://reprap.org/wiki/G-code#M601:_Pause_print">M601: Pause print</a>
+    */
+    /*!
+    ### M125 - Pause print (TODO: not implemented)
+    */
+    /*!
+    ### M25 - Pause SD print <a href="https://reprap.org/wiki/G-code#M25:_Pause_SD_print">M25: Pause SD print</a>
+    */
 	case 25:
 	case 601:
 	{
@@ -7269,30 +7899,34 @@ Sigma_Exit:
 	}
 	break;
 
-  //! ### M602 - Resume print
-  // -------------------------------
+    /*!
+	### M602 - Resume print <a href="https://reprap.org/wiki/G-code#M602:_Resume_print">M602: Resume print</a>
+    */
 	case 602: {
 	  if (isPrintPaused)
           lcd_resume_print();
 	}
 	break;
 
-  //! ### M603 - Stop print
-  // -------------------------------
+    /*!
+    ### M603 - Stop print <a href="https://reprap.org/wiki/G-code#M603:_Stop_print">M603: Stop print</a>
+    */
 	case 603: {
 		lcd_print_stop();
 	}
 	break;
 
 #ifdef PINDA_THERMISTOR
-  //! ### M860 - Wait for extruder temperature (PINDA)
-  // --------------------------------------------------------------
-  /*! 
-      Wait for PINDA thermistor to reach target temperature
-      
-                M860 [S<target_temperature>]
-      
-  */
+    /*!
+	### M860 - Wait for extruder temperature (PINDA) <a href="https://reprap.org/wiki/G-code#M860_Wait_for_Probe_Temperature">M860 Wait for Probe Temperature</a>
+    Wait for PINDA thermistor to reach target temperature
+    #### Usage
+    
+        M860 [ S ]
+    
+    #### Parameters
+    - `S` - Target temperature
+    */
 	case 860: 
 	{
 		int set_target_pinda = 0;
@@ -7337,17 +7971,20 @@ Sigma_Exit:
 		break;
 	}
  
-  //! ### M861 - Set/Get PINDA temperature compensation offsets
-  // -----------------------------------------------------------
-  /*!
-      
-        M861 [ ? | ! | Z | S<microsteps> [I<table_index>] ]
-      
-      - `?` - Print current EEPROM offset values
-      - `!` - Set factory default values
-      - `Z` - Set all values to 0 (effectively disabling PINDA temperature compensation)
-      - `S<microsteps>` `I<table_index>` - Set compensation ustep value S for compensation table index I
-  */
+    /*!
+    ### M861 - Set/Get PINDA temperature compensation offsets <a href="https://reprap.org/wiki/G-code#M861_Set_Probe_Thermal_Compensation">M861 Set Probe Thermal Compensation</a>
+    Set compensation ustep value `S` for compensation table index `I`.
+    #### Usage
+    
+        M861 [ ? | ! | Z | S | I ]
+    
+    #### Parameters
+    - `?` - Print current EEPROM offset values
+    - `!` - Set factory default values
+    - `Z` - Set all values to 0 (effectively disabling PINDA temperature compensation)
+    - `S` - Microsteps
+    - `I` - Table index
+    */
 	case 861:
 		if (code_seen('?')) { // ? - Print out current EEPROM offset values
 			uint8_t cal_status = calibration_status_pinda();
@@ -7420,38 +8057,40 @@ Sigma_Exit:
 
 #endif //PINDA_THERMISTOR
    
-    //! ### M862 - Print checking
-    // ----------------------------------------------
-    /*!     
-        Checks the parameters of the printer and gcode and performs compatibility check
-          - M862.1 { P<nozzle_diameter> | Q }
-          - M862.2 { P<model_code> | Q }
-          - M862.3 { P"<model_name>" | Q }
-          - M862.4 { P<fw_version> | Q }
-          - M862.5 { P<gcode_level> | Q }
-
-        When run with P<> argument, the check is performed against the input value.
-        When run with Q argument, the current value is shown.
-		  
-        M862.3 accepts text identifiers of printer types too.
-        The syntax of M862.3 is (note the quotes around the type):
-		
-                M862.3 P "MK3S"
-		  
-        Accepted printer type identifiers and their numeric counterparts:
-          - MK1         (100)
-          - MK2         (200)       
-          - MK2MM       (201)     
-          - MK2S        (202)      
-          - MK2SMM      (203)    
-          - MK2.5       (250)     
-          - MK2.5MMU2   (20250) 
-          - MK2.5S      (252)    
-          - MK2.5SMMU2S (20252)
-          - MK3         (300)
-          - MK3MMU2     (20300)
-          - MK3S        (302)
-          - MK3SMMU2S	(20302)
+    /*!
+	### M862 - Print checking <a href="https://reprap.org/wiki/G-code#M862:_Print_checking">M862: Print checking</a>
+    Checks the parameters of the printer and gcode and performs compatibility check
+	
+      - M862.1 { P<nozzle_diameter> | Q } 0.25/0.40/0.60
+      - M862.2 { P<model_code> | Q }
+      - M862.3 { P"<model_name>" | Q }
+      - M862.4 { P<fw_version> | Q }
+      - M862.5 { P<gcode_level> | Q }
+    
+    When run with P<> argument, the check is performed against the input value.
+    When run with Q argument, the current value is shown.
+	
+    M862.3 accepts text identifiers of printer types too.
+    The syntax of M862.3 is (note the quotes around the type):
+	  
+          M862.3 P "MK3S"
+	  
+    Accepted printer type identifiers and their numeric counterparts:
+	
+      - MK1         (100)
+      - MK2         (200)       
+      - MK2MM       (201)     
+      - MK2S        (202)      
+      - MK2SMM      (203)    
+      - MK2.5       (250)     
+      - MK2.5MMU2   (20250) 
+      - MK2.5S      (252)    
+      - MK2.5SMMU2S (20252)
+      - MK3         (300)
+      - MK3MMU2     (20300)
+      - MK3S        (302)
+      - MK3SMMU2S   (20302)
+	
     */
     case 862: // M862: print checking
           float nDummy;
@@ -7513,19 +8152,44 @@ Sigma_Exit:
     break;
 
 #ifdef LIN_ADVANCE
-    //! ### M900 - Set Linear advance options
-    // ----------------------------------------------
+    /*!
+	### M900 - Set Linear advance options <a href="https://reprap.org/wiki/G-code#M900_Set_Linear_Advance_Scaling_Factors">M900 Set Linear Advance Scaling Factors</a>
+	Sets the advance extrusion factors for Linear Advance. If any of the R, W, H, or D parameters are set to zero the ratio will be computed dynamically during printing.
+	#### Usage
+    
+        M900 [ K | R | W | H | D]
+    
+    #### Parameters
+    - `K` -  Advance K factor
+    - `R` - Set ratio directly (overrides WH/D)
+    - `W` - Width
+    - `H` - Height
+    - `D` - Diameter Set ratio from WH/D
+    */
     case 900:
         gcode_M900();
     break;
 #endif
 
-    //! ### M907 - Set digital trimpot motor current in mA using axis codes
-    // ---------------------------------------------------------------
+    /*!
+	### M907 - Set digital trimpot motor current in mA using axis codes <a href="https://reprap.org/wiki/G-code#M907:_Set_digital_trimpot_motor">M907: Set digital trimpot motor</a>
+	Set digital trimpot motor current using axis codes (X, Y, Z, E, B, S).
+	#### Usage
+    
+        M907 [ X | Y | Z | E | B | S ]
+	
+    #### Parameters
+    - `X` - X motor driver
+    - `Y` - Y motor driver
+    - `Z` - Z motor driver
+    - `E` - Extruder motor driver
+    - `B` - Second Extruder motor driver
+    - `S` - All motors
+    */
     case 907:
     {
 #ifdef TMC2130
-        //! See tmc2130_cur2val() for translation to 0 .. 63 range
+        // See tmc2130_cur2val() for translation to 0 .. 63 range
         for (int i = 0; i < NUM_AXIS; i++)
 			if(code_seen(axis_codes[i]))
 			{
@@ -7555,8 +8219,17 @@ Sigma_Exit:
     }
     break;
 
-    //! ### M908 - Control digital trimpot directly
-    // ---------------------------------------------------------
+    /*!
+	### M908 - Control digital trimpot directly <a href="https://reprap.org/wiki/G-code#M908:_Control_digital_trimpot_directly">M908: Control digital trimpot directly</a>
+	In Prusa Firmware this G-code is deactivated by default, must be turned on in the source code. Not usable on Prusa printers.
+    #### Usage
+    
+        M908 [ P | S ]
+    
+    #### Parameters
+    - `P` - channel
+    - `S` - current
+    */
     case 908:
     {
       #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
@@ -7570,16 +8243,30 @@ Sigma_Exit:
 
 #ifdef TMC2130_SERVICE_CODES_M910_M918
 
-  //! ### M910 - TMC2130 init
-  // -----------------------------------------------
+    /*!
+	### M910 - TMC2130 init <a href="https://reprap.org/wiki/G-code#M910:_TMC2130_init">M910: TMC2130 init</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+	
+    */
 	case 910:
     {
 		tmc2130_init();
     }
     break;
 
-  //! ### M911 - Set TMC2130 holding currents
-  // -------------------------------------------------
+    /*!
+    ### M911 - Set TMC2130 holding currents <a href="https://reprap.org/wiki/G-code#M911:_Set_TMC2130_holding_currents">M911: Set TMC2130 holding currents</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+    #### Usage
+    
+        M911 [ X | Y | Z | E ]
+    
+    #### Parameters
+    - `X` - X stepper driver holding current value
+    - `Y` - Y stepper driver holding current value
+    - `Z` - Z stepper driver holding current value
+    - `E` - Extruder stepper driver holding current value
+    */
 	case 911: 
     {
 		if (code_seen('X')) tmc2130_set_current_h(0, code_value());
@@ -7589,8 +8276,19 @@ Sigma_Exit:
     }
     break;
 
-  //! ### M912 - Set TMC2130 running currents
-  // -----------------------------------------------
+    /*!
+	### M912 - Set TMC2130 running currents <a href="https://reprap.org/wiki/G-code#M912:_Set_TMC2130_running_currents">M912: Set TMC2130 running currents</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+    #### Usage
+    
+        M912 [ X | Y | Z | E ]
+    
+    #### Parameters
+    - `X` - X stepper driver running current value
+    - `Y` - Y stepper driver running current value
+    - `Z` - Z stepper driver running current value
+    - `E` - Extruder stepper driver running current value
+    */
 	case 912: 
     {
 		if (code_seen('X')) tmc2130_set_current_r(0, code_value());
@@ -7600,17 +8298,22 @@ Sigma_Exit:
     }
     break;
 
-  //! ### M913 - Print TMC2130 currents
-  // -----------------------------
+    /*!
+	### M913 - Print TMC2130 currents <a href="https://reprap.org/wiki/G-code#M913:_Print_TMC2130_currents">M913: Print TMC2130 currents</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+	Shows TMC2130 currents.
+    */
 	case 913:
     {
 		tmc2130_print_currents();
     }
     break;
 
-  //! ### M914 - Set TMC2130 normal mode
-  // ------------------------------
-        case 914:
+    /*!
+	### M914 - Set TMC2130 normal mode <a href="https://reprap.org/wiki/G-code#M914:_Set_TMC2130_normal_mode">M914: Set TMC2130 normal mode</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+    */
+    case 914:
     {
 		tmc2130_mode = TMC2130_MODE_NORMAL;
 		update_mode_profile();
@@ -7618,9 +8321,11 @@ Sigma_Exit:
     }
     break;
 
-  //! ### M95 - Set TMC2130 silent mode
-  // ------------------------------
-        case 915:
+    /*!
+	### M915 - Set TMC2130 silent mode <a href="https://reprap.org/wiki/G-code#M915:_Set_TMC2130_silent_mode">M915: Set TMC2130 silent mode</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+    */
+    case 915:
     {
 		tmc2130_mode = TMC2130_MODE_SILENT;
 		update_mode_profile();
@@ -7628,8 +8333,19 @@ Sigma_Exit:
     }
     break;
 
-  //! ### M916 - Set TMC2130 Stallguard sensitivity threshold
-  // -------------------------------------------------------
+    /*!
+	### M916 - Set TMC2130 Stallguard sensitivity threshold <a href="https://reprap.org/wiki/G-code#M916:_Set_TMC2130_Stallguard_sensitivity_threshold">M916: Set TMC2130 Stallguard sensitivity threshold</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+    #### Usage
+    
+        M916 [ X | Y | Z | E ]
+    
+    #### Parameters
+    - `X` - X stepper driver stallguard sensitivity threshold value
+    - `Y` - Y stepper driver stallguard sensitivity threshold value
+    - `Z` - Z stepper driver stallguard sensitivity threshold value
+    - `E` - Extruder stepper driver stallguard sensitivity threshold value
+    */
 	case 916:
     {
 		if (code_seen('X')) tmc2130_sg_thr[X_AXIS] = code_value();
@@ -7641,8 +8357,19 @@ Sigma_Exit:
     }
     break;
 
-  //! ### M917 - Set TMC2130 PWM amplitude offset (pwm_ampl)
-  // --------------------------------------------------------------
+    /*!
+	### M917 - Set TMC2130 PWM amplitude offset (pwm_ampl) <a href="https://reprap.org/wiki/G-code#M917:_Set_TMC2130_PWM_amplitude_offset_.28pwm_ampl.29">M917: Set TMC2130 PWM amplitude offset (pwm_ampl)</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+    #### Usage
+    
+        M917 [ X | Y | Z | E ]
+    
+    #### Parameters
+    - `X` - X stepper driver PWM amplitude offset value
+    - `Y` - Y stepper driver PWM amplitude offset value
+    - `Z` - Z stepper driver PWM amplitude offset value
+    - `E` - Extruder stepper driver PWM amplitude offset value
+    */
 	case 917:
     {
 		if (code_seen('X')) tmc2130_set_pwm_ampl(0, code_value());
@@ -7652,8 +8379,19 @@ Sigma_Exit:
     }
     break;
 
-  //! ### M918 - Set TMC2130 PWM amplitude gradient (pwm_grad)
-  // -------------------------------------------------------------
+    /*!
+	### M918 - Set TMC2130 PWM amplitude gradient (pwm_grad) <a href="https://reprap.org/wiki/G-code#M918:_Set_TMC2130_PWM_amplitude_gradient_.28pwm_grad.29">M918: Set TMC2130 PWM amplitude gradient (pwm_grad)</a>
+	Not active in default, only if `TMC2130_SERVICE_CODES_M910_M918` is defined in source code.
+    #### Usage
+    
+        M918 [ X | Y | Z | E ]
+    
+    #### Parameters
+    - `X` - X stepper driver PWM amplitude gradient value
+    - `Y` - Y stepper driver PWM amplitude gradient value
+    - `Z` - Z stepper driver PWM amplitude gradient value
+    - `E` - Extruder stepper driver PWM amplitude gradient value
+    */
 	case 918:
     {
 		if (code_seen('X')) tmc2130_set_pwm_grad(0, code_value());
@@ -7665,9 +8403,24 @@ Sigma_Exit:
 
 #endif //TMC2130_SERVICE_CODES_M910_M918
 
-    //! ### M350 - Set microstepping mode
-    // ---------------------------------------------------
-    //!  Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
+    /*!
+	### M350 - Set microstepping mode <a href="https://reprap.org/wiki/G-code#M350:_Set_microstepping_mode">M350: Set microstepping mode</a>
+    Printers with TMC2130 drivers have `X`, `Y`, `Z` and `E` as options. The steps-per-unit value is updated accordingly. Not all resolutions are valid!
+    Printers without TMC2130 drivers also have `B` and `S` options. In this case, the steps-per-unit value in not changed!
+    #### Usage
+    
+        M350 [ X | Y | Z | E | B | S ]
+    
+    #### Parameters
+    - `X` - X new resolution
+    - `Y` - Y new resolution
+    - `Z` - Z new resolution
+    - `E` - E new resolution
+    
+    Only valid for MK2.5(S) or printers without TMC2130 drivers
+    - `B` - Second extruder new resolution
+    - `S` - All axes new resolution
+    */
     case 350: 
     {
 	#ifdef TMC2130
@@ -7681,7 +8434,6 @@ Sigma_Exit:
 				res_valid |= (i == E_AXIS) && ((res_new == 64) || (res_new == 128)); // resolutions valid for E only
 				if (res_valid)
 				{
-					
 					st_synchronize();
 					uint16_t res = tmc2130_get_res(i);
 					tmc2130_set_res(i, res_new);
@@ -7698,6 +8450,10 @@ Sigma_Exit:
 						cs.axis_steps_per_unit[i] /= fac;
 						position[i] /= fac;
 					}
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+                    if (i == E_AXIS)
+                        fsensor_set_axis_steps_per_unit(cs.axis_steps_per_unit[i]);
+#endif
 				}
 			}
 		}
@@ -7712,11 +8468,21 @@ Sigma_Exit:
     }
     break;
 
-    //! ### M351 - Toggle Microstep Pins
-    // -----------------------------------
-    //! Toggle MS1 MS2 pins directly, S# determines MS1 or MS2, X# sets the pin high/low.
-    //!
-    //!         M351 [B<0|1>] [E<0|1>] S<1|2> [X<0|1>] [Y<0|1>] [Z<0|1>] 
+    /*!
+	### M351 - Toggle Microstep Pins <a href="https://reprap.org/wiki/G-code#M351:_Toggle_MS1_MS2_pins_directly">M351: Toggle MS1 MS2 pins directly</a>
+    Toggle MS1 MS2 pins directly.
+    #### Usage
+    
+        M351 [B<0|1>] [E<0|1>] S<1|2> [X<0|1>] [Y<0|1>] [Z<0|1>]
+    
+    #### Parameters
+    - `X` - Update X axis
+    - `Y` - Update Y axis
+    - `Z` - Update Z axis
+    - `E` - Update E axis
+    - `S` - which MSx pin to toggle
+    - `B` - new pin value
+    */
     case 351:
     {
       #if defined(X_MS1_PIN) && X_MS1_PIN > -1
@@ -7736,8 +8502,10 @@ Sigma_Exit:
     }
     break;
 
-  //! ### M701 - Load filament
-  // -------------------------
+  /*!
+  ### M701 - Load filament <a href="https://reprap.org/wiki/G-code#M701:_Load_filament">M701: Load filament</a>
+  
+  */
 	case 701:
 	{
 		if (mmu_enabled && code_seen('E'))
@@ -7746,16 +8514,17 @@ Sigma_Exit:
 	}
 	break;
 
-  //! ### M702 - Unload filament
-  // ------------------------
-  /*! 
-      
-          M702 [U C]
-      
-      - `U` Unload all filaments used in current print
-      - `C` Unload just current filament
-      - without any parameters unload all filaments
-  */
+    /*!
+    ### M702 - Unload filament <a href="https://reprap.org/wiki/G-code#M702:_Unload_filament">G32: Undock Z Probe sled</a>
+    #### Usage
+    
+        M702 [ U | C ]
+    
+    #### Parameters
+    - `U` - Unload all filaments used in current print
+    - `C` - Unload just current filament
+    - without any parameters unload all filaments
+    */
 	case 702:
 	{
 #ifdef SNMM
@@ -7778,14 +8547,19 @@ Sigma_Exit:
 	}
 	break;
 
-    //! ### M999 - Restart after being stopped
-    // ------------------------------------
+    /*!
+	### M999 - Restart after being stopped <a href="https://reprap.org/wiki/G-code#M999:_Restart_after_being_stopped_by_error">M999: Restart after being stopped by error</a>
+    @todo Usually doesn't work. Should be fixed or removed. Most of the time, if `Stopped` it set, the print fails and is unrecoverable.
+    */
     case 999:
       Stopped = false;
       lcd_reset_alert_level();
       gcode_LastN = Stopped_gcode_LastN;
       FlushSerialRequestResend();
     break;
+	/*!
+	#### End of M-Commands
+    */
 	default: 
 		printf_P(PSTR("Unknown M code: %s \n"), cmdbuffer + bufindr + CMDHDRSIZE);
     }
@@ -7794,19 +8568,16 @@ Sigma_Exit:
 	}
   }
   // end if(code_seen('M')) (end of M codes)
-
-  //! -----------------------------------------------------------------------------------------
-  //! T Codes
-  //! 
-  //! T<extruder nr.> - select extruder in case of multi extruder printer
-  //! select filament in case of MMU_V2
-  //! if extruder is "?", open menu to let the user select extruder/filament
-  //!
-  //!  For MMU_V2:
-  //! @n T<n> Gcode to extrude at least 38.10 mm at feedrate 19.02 mm/s must follow immediately to load to extruder wheels.
-  //! @n T? Gcode to extrude shouldn't have to follow, load to extruder wheels is done automatically
-  //! @n Tx Same as T?, except nozzle doesn't have to be preheated. Tc must be placed after extruder nozzle is preheated to finish filament load.
-  //! @n Tc Load to nozzle after filament was prepared by Tc and extruder nozzle is already heated.
+  /*!
+  -----------------------------------------------------------------------------------------
+  # T Codes
+  T<extruder nr.> - select extruder in case of multi extruder printer. select filament in case of MMU_V2.
+  #### For MMU_V2:
+  T<n> Gcode to extrude at least 38.10 mm at feedrate 19.02 mm/s must follow immediately to load to extruder wheels.
+  @n T? Gcode to extrude shouldn't have to follow, load to extruder wheels is done automatically
+  @n Tx Same as T?, except nozzle doesn't have to be preheated. Tc must be placed after extruder nozzle is preheated to finish filament load.
+  @n Tc Load to nozzle after filament was prepared by Tc and extruder nozzle is already heated.
+  */
   else if(code_seen('T'))
   {
       int index;
@@ -7895,15 +8666,8 @@ Sigma_Exit:
           else
           {
 #ifdef SNMM
-
-#ifdef LIN_ADVANCE
-              if (mmu_extruder != tmp_extruder)
-                  clear_current_adv_vars(); //Check if the selected extruder is not the active one and reset LIN_ADVANCE variables if so.
-#endif
-
               mmu_extruder = tmp_extruder;
 
-
               _delay(100);
 
               disable_e0();
@@ -7990,95 +8754,197 @@ Sigma_Exit:
           }
       }
   } // end if(code_seen('T')) (end of T codes)
+  /*!
+  #### End of T-Codes
+  */
 
-  //! ----------------------------------------------------------------------------------------------
-
+  /**
+  *---------------------------------------------------------------------------------
+  *# D codes
+  */
   else if (code_seen('D')) // D codes (debug)
   {
     switch((int)code_value())
     {
 
-  //! ### D-1 - Endless loop
-  // -------------------
+    /*!
+    ### D-1 - Endless Loop <a href="https://reprap.org/wiki/G-code#D-1:_Endless_Loop">D-1: Endless Loop</a>
+    */
 	case -1:
 		dcode__1(); break;
 #ifdef DEBUG_DCODES
 
-  //! ### D0 - Reset
-  // -------------- 
+    /*!
+    ### D0 - Reset <a href="https://reprap.org/wiki/G-code#D0:_Reset">D0: Reset</a>
+    #### Usage
+    
+        D0 [ B ]
+    
+    #### Parameters
+    - `B` - Bootloader
+    */
 	case 0:
 		dcode_0(); break;
 
-  //! ### D1 - Clear EEPROM
-  // ------------------
+    /*!
+    *
+    ### D1 - Clear EEPROM and RESET <a href="https://reprap.org/wiki/G-code#D1:_Clear_EEPROM_and_RESET">D1: Clear EEPROM and RESET</a>
+      
+          D1
+      
+    *
+    */
 	case 1:
 		dcode_1(); break;
 
-  //! ### D2 - Read/Write RAM
-  // --------------------
+    /*!
+    ### D2 - Read/Write RAM <a href="https://reprap.org/wiki/G-code#D2:_Read.2FWrite_RAM">D3: Read/Write RAM</a>
+    This command can be used without any additional parameters. It will read the entire RAM.
+    #### Usage
+    
+        D3 [ A | C | X ]
+    
+    #### Parameters
+    - `A` - Address (0x0000-0x1fff)
+    - `C` - Count (0x0001-0x2000)
+    - `X` - Data
+    */
 	case 2:
 		dcode_2(); break;
 #endif //DEBUG_DCODES
 #ifdef DEBUG_DCODE3
 
-  //! ### D3 - Read/Write EEPROM
-  // -----------------------
+    /*!
+    ### D3 - Read/Write EEPROM <a href="https://reprap.org/wiki/G-code#D3:_Read.2FWrite_EEPROM">D3: Read/Write EEPROM</a>
+    This command can be used without any additional parameters. It will read the entire eeprom.
+    #### Usage
+    
+        D3 [ A | C | X ]
+    
+    #### Parameters
+    - `A` - Address (0x0000-0x0fff)
+    - `C` - Count (0x0001-0x1000)
+    - `X` - Data
+    */
 	case 3:
 		dcode_3(); break;
 #endif //DEBUG_DCODE3
 #ifdef DEBUG_DCODES
 
-  //! ### D4 - Read/Write PIN
-  // ---------------------
+    /*!
+    
+    ### D4 - Read/Write PIN <a href="https://reprap.org/wiki/G-code#D4:_Read.2FWrite_PIN">D4: Read/Write PIN</a>
+    To read the digital value of a pin you need only to define the pin number.
+    #### Usage
+    
+        D4 [ P | F | V ]
+    
+    #### Parameters
+    - `P` - Pin (0-255)
+    - `F` - Function in/out (0/1)
+    - `V` - Value (0/1)
+    */
 	case 4:
 		dcode_4(); break;
 #endif //DEBUG_DCODES
 #ifdef DEBUG_DCODE5
 
-  //! ### D5 - Read/Write FLASH
-  // ------------------------
+    /*!
+    ### D5 - Read/Write FLASH <a href="https://reprap.org/wiki/G-code#D5:_Read.2FWrite_FLASH">D5: Read/Write Flash</a>
+    This command can be used without any additional parameters. It will read the 1kb FLASH.
+    #### Usage
+    
+        D3 [ A | C | X | E ]
+    
+    #### Parameters
+    - `A` - Address (0x00000-0x3ffff)
+    - `C` - Count (0x0001-0x2000)
+    - `X` - Data
+    - `E` - Erase
+    */
 	case 5:
 		dcode_5(); break;
 		break;
 #endif //DEBUG_DCODE5
 #ifdef DEBUG_DCODES
 
-  //! ### D6 - Read/Write external FLASH
-  // ---------------------------------------
+    /*!
+    ### D6 - Read/Write external FLASH <a href="https://reprap.org/wiki/G-code#D6:_Read.2FWrite_external_FLASH">D6: Read/Write external Flash</a>
+    Reserved
+    */
 	case 6:
 		dcode_6(); break;
 
-  //! ### D7 - Read/Write Bootloader
-  // -------------------------------
+    /*!
+    ### D7 - Read/Write Bootloader <a href="https://reprap.org/wiki/G-code#D7:_Read.2FWrite_Bootloader">D7: Read/Write Bootloader</a>
+    Reserved
+    */
 	case 7:
 		dcode_7(); break;
 
-  //! ### D8 - Read/Write PINDA
-  // ---------------------------
+    /*!
+    ### D8 - Read/Write PINDA <a href="https://reprap.org/wiki/G-code#D8:_Read.2FWrite_PINDA">D8: Read/Write PINDA</a>
+    #### Usage
+    
+        D8 [ ? | ! | P | Z ]
+    
+    #### Parameters
+    - `?` - Read PINDA temperature shift values
+    - `!` - Reset PINDA temperature shift values to default
+    - `P` - Pinda temperature [C]
+    - `Z` - Z Offset [mm]
+    */
 	case 8:
 		dcode_8(); break;
 
-  // ### D9 - Read/Write ADC
-  // ------------------------
+    /*!
+    ### D9 - Read ADC <a href="https://reprap.org/wiki/G-code#D9:_Read.2FWrite_ADC">D9: Read ADC</a>
+    #### Usage
+    
+        D9 [ I | V ]
+    
+    #### Parameters
+    - `I` - ADC channel index 
+        - `0` - Heater 0 temperature
+        - `1` - Heater 1 temperature
+        - `2` - Bed temperature
+        - `3` - PINDA temperature
+        - `4` - PWR voltage
+        - `5` - Ambient temperature
+        - `6` - BED voltage
+    - `V` Value to be written as simulated
+    */
 	case 9:
 		dcode_9(); break;
 
-  //! ### D10 - XYZ calibration = OK
-  // ------------------------------
+    /*!
+    ### D10 - Set XYZ calibration = OK <a href="https://reprap.org/wiki/G-code#D10:_Set_XYZ_calibration_.3D_OK">D10: Set XYZ calibration = OK</a>
+    */
 	case 10:
 		dcode_10(); break;
+
+    /*!
+    ### D12 - Time <a href="https://reprap.org/wiki/G-code#D12:_Time">D12: Time</a>
+    Writes the actual time in the log file.
+    */
+
 #endif //DEBUG_DCODES
 #ifdef HEATBED_ANALYSIS
 
-  //! ### D80 - Bed check
-  // ---------------------
-  /*! 
-    - `E` - dimension x
-    - `F` - dimention y
-    - `G` - points_x
-    - `H` - points_y
-    - `I` - offset_x
-    - `J` - offset_y
+    /*!
+    ### D80 - Bed check <a href="https://reprap.org/wiki/G-code#D80:_Bed_check">D80: Bed check</a>
+    This command will log data to SD card file "mesh.txt".
+    #### Usage
+    
+        D80 [ E | F | G | H | I | J ]
+    
+    #### Parameters
+    - `E` - Dimension X (default 40)
+    - `F` - Dimention Y (default 40)
+    - `G` - Points X (default 40)
+    - `H` - Points Y (default 40)
+    - `I` - Offset X (default 74)
+    - `J` - Offset Y (default 34)
   */
 	case 80:
 	{
@@ -8104,15 +8970,20 @@ Sigma_Exit:
  		bed_check(dimension_x,dimension_y,points_x,points_y,offset_x,offset_y);
 	}break;
 
-  //! ### D81 - Bed analysis
-  // -----------------------------
-  /*! 
-    - `E` - dimension x
-    - `F` - dimention y
-    - `G` - points_x
-    - `H` - points_y
-    - `I` - offset_x
-    - `J` - offset_y
+    /*!
+    ### D81 - Bed analysis <a href="https://reprap.org/wiki/G-code#D81:_Bed_analysis">D80: Bed analysis</a>
+    This command will log data to SD card file "wldsd.txt".
+    #### Usage
+    
+        D81 [ E | F | G | H | I | J ]
+    
+    #### Parameters
+    - `E` - Dimension X (default 40)
+    - `F` - Dimention Y (default 40)
+    - `G` - Points X (default 40)
+    - `H` - Points Y (default 40)
+    - `I` - Offset X (default 74)
+    - `J` - Offset Y (default 34)
   */
 	case 81:
 	{
@@ -8137,8 +9008,9 @@ Sigma_Exit:
 #endif //HEATBED_ANALYSIS
 #ifdef DEBUG_DCODES
 
-  //! ### D106 print measured fan speed for different pwm values
-  // --------------------------------------------------------------
+    /*!
+    ### D106 - Print measured fan speed for different pwm values <a href="https://reprap.org/wiki/G-code#D106:_Print_measured_fan_speed_for_different_pwm_values">D106: Print measured fan speed for different pwm values</a>
+    */
 	case 106:
 	{
 		for (int i = 255; i > 0; i = i - 5) {
@@ -8153,42 +9025,61 @@ Sigma_Exit:
 	}break;
 
 #ifdef TMC2130
-  //! ### D2130 - TMC2130 Trinamic stepper controller
-  // ---------------------------
-
-  
-  /*!
-  
-  
-        D2130<axis><command>[subcommand][value]
-
-   - <command>:
-       - '0' current off
-       - '1' current on
-       - '+' single step
-       - * value sereval steps
-       - '-' dtto oposite direction
-       - '?' read register
-       - * "mres"
-       - * "step"
-       - * "mscnt"
-       - * "mscuract"
-       - * "wave"
-       - '!' set register
-       - * "mres"
-       - * "step"
-       - * "wave"
-       - '@' home calibrate axis
-
-    Example:
-
-    D2130E?wave ...        print extruder microstep linearity compensation curve
-
-    D2130E!wave0 ...       disable extruder linearity compensation curve, (sine curve is used)
-
-    D2130E!wave220 ...    (sin(x))^1.1 extruder microstep compensation curve used
-  */
-
+    /*!
+    ### D2130 - Trinamic stepper controller <a href="https://reprap.org/wiki/G-code#D2130:_Trinamic_stepper_controller">D2130: Trinamic stepper controller</a>
+    @todo Please review by owner of the code. RepRap Wiki Gcode needs to be updated after review of owner as well.
+    
+    #### Usage
+    
+        D2130 [ Axis | Command | Subcommand | Value ]
+    
+    #### Parameters
+    - Axis
+      - `X` - X stepper driver
+      - `Y` - Y stepper driver
+      - `Z` - Z stepper driver
+      - `E` - Extruder stepper driver
+    - Commands
+      - `0`   - Current off
+      - `1`   - Current on
+      - `+`   - Single step
+      - `-`   - Single step oposite direction
+      - `NNN` - Value sereval steps
+      - `?`   - Read register
+      - Subcommands for read register
+        - `mres`     - Micro step resolution. More information in datasheet '5.5.2 CHOPCONF – Chopper Configuration'
+        - `step`     - Step
+        - `mscnt`    - Microstep counter. More information in datasheet '5.5 Motor Driver Registers'
+        - `mscuract` - Actual microstep current for motor. More information in datasheet '5.5 Motor Driver Registers'
+        - `wave`     - Microstep linearity compensation curve
+      - `!`   - Set register
+      - Subcommands for set register
+        - `mres`     - Micro step resolution
+        - `step`     - Step
+        - `wave`     - Microstep linearity compensation curve
+        - Values for set register
+          - `0, 180 --> 250` - Off
+          - `0.9 --> 1.25`   - Valid values (recommended is 1.1)
+      - `@`   - Home calibrate axis
+    
+    Examples:
+      
+          D2130E?wave
+      
+      Print extruder microstep linearity compensation curve
+      
+          D2130E!wave0
+      
+      Disable extruder linearity compensation curve, (sine curve is used)
+      
+          D2130E!wave220
+      
+      (sin(x))^1.1 extruder microstep compensation curve used
+    
+    Notes:
+      For more information see https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC2130_datasheet.pdf
+    *
+	*/
 
 	case 2130:
 		dcode_2130(); break;
@@ -8196,8 +9087,20 @@ Sigma_Exit:
 
 #if (defined (FILAMENT_SENSOR) && defined(PAT9125))
 
-        //! ### D9125 - FILAMENT_SENSOR
-        // ---------------------------------
+    /*!
+    ### D9125 - PAT9125 filament sensor <a href="https://reprap.org/wiki/G-code#D9:_Read.2FWrite_ADC">D9125: PAT9125 filament sensor</a>
+    #### Usage
+    
+        D9125 [ ? | ! | R | X | Y | L ]
+    
+    #### Parameters
+    - `?` - Print values
+    - `!` - Print values
+    - `R` - Resolution. Not active in code
+    - `X` - X values
+    - `Y` - Y values
+    - `L` - Activate filament sensor log
+    */
 	case 9125:
 		dcode_9125(); break;
 #endif //FILAMENT_SENSOR
@@ -8217,7 +9120,9 @@ Sigma_Exit:
   ClearToSend();
 }
 
-
+/*!
+#### End of D-Codes
+*/
 
   /** @defgroup GCodes G-Code List 
   */
@@ -8612,7 +9517,8 @@ if(0)
 #ifdef PAT9125
 				fsensor_autoload_check_stop();
 #endif //PAT9125
-				fsensor_update();
+                if (fsensor_enabled && !saved_printing)
+                    fsensor_update();
 			}
 		}
 	}
@@ -8717,7 +9623,7 @@ void kill(const char *full_screen_message, unsigned char id)
   disable_x();
 //  SERIAL_ECHOLNPGM("kill - disable Y");
   disable_y();
-  disable_z();
+  poweroff_z();
   disable_e0();
   disable_e1();
   disable_e2();
@@ -8954,6 +9860,7 @@ static void wait_for_heater(long codenum, uint8_t extruder) {
 	residencyStart = -1;
 	/* continue to loop until we have reached the target temp
 	_and_ until TEMP_RESIDENCY_TIME hasn't passed since we reached it */
+    cancel_heatup = false;
 	while ((!cancel_heatup) && ((residencyStart == -1) ||
 		(residencyStart >= 0 && (((unsigned int)(_millis() - residencyStart)) < (TEMP_RESIDENCY_TIME * 1000UL))))) {
 #else
@@ -9577,10 +10484,6 @@ void long_pause() //long pause print
     // Stop heaters
     setAllTargetHotends(0);
 
-	//retract
-	current_position[E_AXIS] -= default_retraction;
-	plan_buffer_line_curposXYZE(400, active_extruder);
-
 	//lift z
 	current_position[Z_AXIS] += Z_PAUSE_LIFT;
 	if (current_position[Z_AXIS] > Z_MAX_POS) current_position[Z_AXIS] = Z_MAX_POS;
@@ -9607,6 +10510,16 @@ void serialecho_temperatures() {
 }
 
 #ifdef UVLO_SUPPORT
+void uvlo_drain_reset()
+{
+    // burn all that residual power
+    wdt_enable(WDTO_1S);
+    WRITE(BEEPER,HIGH);
+    lcd_clear();
+    lcd_puts_at_P(0, 1, MSG_POWERPANIC_DETECTED);
+    while(1);
+}
+
 
 void uvlo_()
 {
@@ -9628,16 +10541,11 @@ void uvlo_()
 	tmc2130_set_current_r(E_AXIS, 20);
 #endif //TMC2130
 
-
-    // Indicate that the interrupt has been triggered.
-	//	SERIAL_ECHOLNPGM("UVLO");
-
-    // Read out the current Z motor microstep counter. This will be later used
-    // for reaching the zero full step before powering off.
-    uint16_t z_microsteps = 0;
-#ifdef TMC2130
-	z_microsteps = tmc2130_rd_MSCNT(Z_TMC2130_CS);
-#endif //TMC2130
+    // Stop all heaters
+    uint8_t saved_target_temperature_bed = target_temperature_bed;
+    uint16_t saved_target_temperature_ext = target_temperature[active_extruder];
+    setAllTargetHotends(0);
+    setTargetBed(0);
 
     // Calculate the file position, from which to resume this print.
     long sd_position = sdpos_atomic; //atomic sd position of last command added in queue
@@ -9651,7 +10559,7 @@ void uvlo_()
 
     // save the global state at planning time
     uint16_t feedrate_bckp;
-    if (blocks_queued())
+    if (current_block)
     {
         memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
         feedrate_bckp = current_block->gcode_feedrate;
@@ -9662,75 +10570,81 @@ void uvlo_()
         feedrate_bckp = feedrate;
     }
 
+    // From this point on and up to the print recovery, Z should not move during X/Y travels and
+    // should be controlled precisely. Reset the MBL status before planner_abort_hard in order to
+    // get the physical Z for further manipulation.
+    bool mbl_was_active = mbl.active;
+    mbl.active = false;
+
     // After this call, the planner queue is emptied and the current_position is set to a current logical coordinate.
     // The logical coordinate will likely differ from the machine coordinate if the skew calibration and mesh bed leveling
     // are in action.
     planner_abort_hard();
 
-	// Store the current extruder position.
-	eeprom_update_float((float*)(EEPROM_UVLO_CURRENT_POSITION_E), st_get_position_mm(E_AXIS));
+    // Store the print logical Z position, which we need to recover (a slight error here would be
+    // recovered on the next Gcode instruction, while a physical location error would not)
+    float logical_z = current_position[Z_AXIS];
+    if(mbl_was_active) logical_z -= mbl.get_z(st_get_position_mm(X_AXIS), st_get_position_mm(Y_AXIS));
+    eeprom_update_float((float*)EEPROM_UVLO_CURRENT_POSITION_Z, logical_z);
+
+    // Store the print E position before we lose track
+	eeprom_update_float((float*)(EEPROM_UVLO_CURRENT_POSITION_E), current_position[E_AXIS]);
 	eeprom_update_byte((uint8_t*)EEPROM_UVLO_E_ABS, axis_relative_modes[3]?0:1);
-    // Clean the input command queue.
+
+    // Clean the input command queue, inhibit serial processing using saved_printing
     cmdqueue_reset();
     card.sdprinting = false;
-//    card.closefile();    
-    // Enable stepper driver interrupt to move Z axis.
-    // This should be fine as the planner and command queues are empty and the SD card printing is disabled.
-    //FIXME one may want to disable serial lines at this point of time to avoid interfering with the command queue,
-    // though it should not happen that the command queue is touched as the plan_buffer_line always succeed without blocking.
-		sei();
-		plan_buffer_line(
-      current_position[X_AXIS], 
-      current_position[Y_AXIS], 
-      current_position[Z_AXIS], 
-      current_position[E_AXIS] - default_retraction,
-      95, active_extruder);
-    
-        st_synchronize();
-        disable_e0();
-    
-    plan_buffer_line(
-      current_position[X_AXIS],
-      current_position[Y_AXIS],
-      current_position[Z_AXIS] + UVLO_Z_AXIS_SHIFT + float((1024 - z_microsteps + 7) >> 4) / cs.axis_steps_per_unit[Z_AXIS],
-      current_position[E_AXIS] - default_retraction,
-      40, active_extruder);
+    saved_printing = true;
+
+    // Enable stepper driver interrupt to move Z axis. This should be fine as the planner and
+    // command queues are empty, SD card printing is disabled, usb is inhibited.
+    sei();
+
+    // Retract
+    current_position[E_AXIS] -= default_retraction;
+    plan_buffer_line_curposXYZE(95, active_extruder);
     st_synchronize();
     disable_e0();
 
-    plan_buffer_line(
-      current_position[X_AXIS],
-      current_position[Y_AXIS],
-      current_position[Z_AXIS] + UVLO_Z_AXIS_SHIFT + float((1024 - z_microsteps + 7) >> 4) / cs.axis_steps_per_unit[Z_AXIS],
-      current_position[E_AXIS] - default_retraction,
-      40, active_extruder);
+    // Read out the current Z motor microstep counter to move the axis up towards
+    // a full step before powering off. NOTE: we need to ensure to schedule more
+    // than "dropsegments" steps in order to move (this is always the case here
+    // due to UVLO_Z_AXIS_SHIFT being used)
+    uint16_t z_res = tmc2130_get_res(Z_AXIS);
+    uint16_t z_microsteps = tmc2130_rd_MSCNT(Z_AXIS);
+    current_position[Z_AXIS] += float(1024 - z_microsteps)
+                                / (z_res * cs.axis_steps_per_unit[Z_AXIS])
+                                + UVLO_Z_AXIS_SHIFT;
+    plan_buffer_line_curposXYZE(homing_feedrate[Z_AXIS]/60, active_extruder);
     st_synchronize();
+    poweroff_z();
 
-    disable_e0();
-    // Move Z up to the next 0th full step.
     // Write the file position.
     eeprom_update_dword((uint32_t*)(EEPROM_FILE_POSITION), sd_position);
+
     // Store the mesh bed leveling offsets. This is 2*7*7=98 bytes, which takes 98*3.4us=333us in worst case.
     for (int8_t mesh_point = 0; mesh_point < MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS; ++ mesh_point) {
       uint8_t ix = mesh_point % MESH_NUM_X_POINTS; // from 0 to MESH_NUM_X_POINTS - 1
       uint8_t iy = mesh_point / MESH_NUM_X_POINTS;
       // Scale the z value to 1u resolution.
-      int16_t v = mbl.active ? int16_t(floor(mbl.z_values[iy][ix] * 1000.f + 0.5f)) : 0;
+      int16_t v = mbl_was_active ? int16_t(floor(mbl.z_values[iy][ix] * 1000.f + 0.5f)) : 0;
       eeprom_update_word((uint16_t*)(EEPROM_UVLO_MESH_BED_LEVELING_FULL +2*mesh_point), *reinterpret_cast<uint16_t*>(&v));
     }
-    // Read out the current Z motor microstep counter. This will be later used
-    // for reaching the zero full step before powering off.
+
+    // Write the _final_ Z position and motor microstep counter (unused).
+    eeprom_update_float((float*)EEPROM_UVLO_TINY_CURRENT_POSITION_Z, current_position[Z_AXIS]);
+    z_microsteps = tmc2130_rd_MSCNT(Z_AXIS);
     eeprom_update_word((uint16_t*)(EEPROM_UVLO_Z_MICROSTEPS), z_microsteps);
-    // Store the current position.
 
+    // Store the current position.
     eeprom_update_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 0), current_position[X_AXIS]);
     eeprom_update_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 4), current_position[Y_AXIS]);
-    eeprom_update_float((float*)EEPROM_UVLO_CURRENT_POSITION_Z , current_position[Z_AXIS]);
+
     // Store the current feed rate, temperatures, fan speed and extruder multipliers (flow rates)
 	eeprom_update_word((uint16_t*)EEPROM_UVLO_FEEDRATE, feedrate_bckp);
-    EEPROM_save_B(EEPROM_UVLO_FEEDMULTIPLY, &feedmultiply);
-    eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_HOTEND, target_temperature[active_extruder]);
-    eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_BED, target_temperature_bed);
+    eeprom_update_word((uint16_t*)EEPROM_UVLO_FEEDMULTIPLY, feedmultiply);
+    eeprom_update_word((uint16_t*)EEPROM_UVLO_TARGET_HOTEND, saved_target_temperature_ext);
+    eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_BED, saved_target_temperature_bed);
     eeprom_update_byte((uint8_t*)EEPROM_UVLO_FAN_SPEED, fanSpeed);
 	eeprom_update_float((float*)(EEPROM_EXTRUDER_MULTIPLIER_0), extruder_multiplier[0]);
 #if EXTRUDERS > 1
@@ -9740,80 +10654,102 @@ void uvlo_()
 #endif
 #endif
 	eeprom_update_word((uint16_t*)(EEPROM_EXTRUDEMULTIPLY), (uint16_t)extrudemultiply);
+
     // Store the saved target
     eeprom_update_float((float*)(EEPROM_UVLO_SAVED_TARGET+0*4), saved_target[X_AXIS]);
     eeprom_update_float((float*)(EEPROM_UVLO_SAVED_TARGET+1*4), saved_target[Y_AXIS]);
     eeprom_update_float((float*)(EEPROM_UVLO_SAVED_TARGET+2*4), saved_target[Z_AXIS]);
     eeprom_update_float((float*)(EEPROM_UVLO_SAVED_TARGET+3*4), saved_target[E_AXIS]);
 
+#ifdef LIN_ADVANCE
+	eeprom_update_float((float*)(EEPROM_UVLO_LA_K), extruder_advance_K);
+#endif
+
     // Finaly store the "power outage" flag.
 	if(sd_print) eeprom_update_byte((uint8_t*)EEPROM_UVLO, 1);
 
-    st_synchronize();
-    printf_P(_N("stps%d\n"), tmc2130_rd_MSCNT(Z_AXIS));
-    
     // Increment power failure counter
 	eeprom_update_byte((uint8_t*)EEPROM_POWER_COUNT, eeprom_read_byte((uint8_t*)EEPROM_POWER_COUNT) + 1);
 	eeprom_update_word((uint16_t*)EEPROM_POWER_COUNT_TOT, eeprom_read_word((uint16_t*)EEPROM_POWER_COUNT_TOT) + 1);
-      printf_P(_N("UVLO - end %d\n"), _millis() - time_start);
 
-#if 0
-    // Move the print head to the side of the print until all the power stored in the power supply capacitors is depleted.
+    printf_P(_N("UVLO - end %d\n"), _millis() - time_start);
+    WRITE(BEEPER,HIGH);
+
+    // All is set: with all the juice left, try to move extruder away to detach the nozzle completely from the print
+    poweron_z();
     current_position[X_AXIS] = (current_position[X_AXIS] < 0.5f * (X_MIN_POS + X_MAX_POS)) ? X_MIN_POS : X_MAX_POS;
     plan_buffer_line_curposXYZE(500, active_extruder);
     st_synchronize();
-#endif
-wdt_enable(WDTO_500MS);
-WRITE(BEEPER,HIGH);
-while(1)
-     ;
+
+    wdt_enable(WDTO_1S);
+    while(1);
 }
 
 
 void uvlo_tiny()
 {
-uint16_t z_microsteps=0;
+    unsigned long time_start = _millis();
 
-// Conserve power as soon as possible.
-disable_x();
-disable_y();
-disable_e0();
-    
-#ifdef TMC2130
-tmc2130_set_current_h(Z_AXIS, 20);
-tmc2130_set_current_r(Z_AXIS, 20);
-#endif //TMC2130
+    // Conserve power as soon as possible.
+    disable_x();
+    disable_y();
+    disable_e0();
 
-// Read out the current Z motor microstep counter
 #ifdef TMC2130
-z_microsteps=tmc2130_rd_MSCNT(Z_TMC2130_CS);
+    tmc2130_set_current_h(Z_AXIS, 20);
+    tmc2130_set_current_r(Z_AXIS, 20);
 #endif //TMC2130
-planner_abort_hard();
 
-//save current position only in case, where the printer is moving on Z axis, which is only when EEPROM_UVLO is 1
-//EEPROM_UVLO is 1 after normal uvlo or after recover_print(), when the extruder is moving on Z axis after rehome
-if(eeprom_read_byte((uint8_t*)EEPROM_UVLO)!=2){
-  eeprom_update_float((float*)(EEPROM_UVLO_TINY_CURRENT_POSITION_Z), current_position[Z_AXIS]);
-  eeprom_update_word((uint16_t*)(EEPROM_UVLO_TINY_Z_MICROSTEPS),z_microsteps);
-}
+    // Stop all heaters
+    setAllTargetHotends(0);
+    setTargetBed(0);
 
-//after multiple power panics current Z axis is unknow
-//in this case we set EEPROM_UVLO_TINY_CURRENT_POSITION_Z to last know position which is EEPROM_UVLO_CURRENT_POSITION_Z 
-if(eeprom_read_float((float*)EEPROM_UVLO_TINY_CURRENT_POSITION_Z) < 0.001f){
-  eeprom_update_float((float*)(EEPROM_UVLO_TINY_CURRENT_POSITION_Z), eeprom_read_float((float*)EEPROM_UVLO_CURRENT_POSITION_Z));
-  eeprom_update_word((uint16_t*)(EEPROM_UVLO_TINY_Z_MICROSTEPS), eeprom_read_word((uint16_t*)EEPROM_UVLO_Z_MICROSTEPS));
-}
+    // When power is interrupted on the _first_ recovery an attempt can be made to raise the
+    // extruder, causing the Z position to change. Similarly, when recovering, the Z position is
+    // lowered. In such cases we cannot just save Z, we need to re-align the steppers to a fullstep.
+    // Disable MBL (if not already) to work with physical coordinates.
+    mbl.active = false;
+    planner_abort_hard();
 
-// Finaly store the "power outage" flag.
-eeprom_update_byte((uint8_t*)EEPROM_UVLO,2);
+    // Allow for small roundoffs to be ignored
+    if(abs(current_position[Z_AXIS] - eeprom_read_float((float*)(EEPROM_UVLO_TINY_CURRENT_POSITION_Z))) >= 1.f/cs.axis_steps_per_unit[Z_AXIS])
+    {
+        // Clean the input command queue, inhibit serial processing using saved_printing
+        cmdqueue_reset();
+        card.sdprinting = false;
+        saved_printing = true;
+
+        // Enable stepper driver interrupt to move Z axis. This should be fine as the planner and
+        // command queues are empty, SD card printing is disabled, usb is inhibited.
+        sei();
+
+        // The axis was moved: adjust Z as done on a regular UVLO.
+        uint16_t z_res = tmc2130_get_res(Z_AXIS);
+        uint16_t z_microsteps = tmc2130_rd_MSCNT(Z_AXIS);
+        current_position[Z_AXIS] += float(1024 - z_microsteps)
+                                    / (z_res * cs.axis_steps_per_unit[Z_AXIS])
+                                    + UVLO_TINY_Z_AXIS_SHIFT;
+        plan_buffer_line_curposXYZE(homing_feedrate[Z_AXIS]/60, active_extruder);
+        st_synchronize();
+        poweroff_z();
+
+        // Update Z position
+        eeprom_update_float((float*)(EEPROM_UVLO_TINY_CURRENT_POSITION_Z), current_position[Z_AXIS]);
+
+        // Update the _final_ Z motor microstep counter (unused).
+        z_microsteps = tmc2130_rd_MSCNT(Z_AXIS);
+        eeprom_update_word((uint16_t*)(EEPROM_UVLO_Z_MICROSTEPS), z_microsteps);
+    }
+
+    // Update the the "power outage" flag.
+    eeprom_update_byte((uint8_t*)EEPROM_UVLO,2);
+
+    // Increment power failure counter
+    eeprom_update_byte((uint8_t*)EEPROM_POWER_COUNT, eeprom_read_byte((uint8_t*)EEPROM_POWER_COUNT) + 1);
+    eeprom_update_word((uint16_t*)EEPROM_POWER_COUNT_TOT, eeprom_read_word((uint16_t*)EEPROM_POWER_COUNT_TOT) + 1);
 
-// Increment power failure counter
-eeprom_update_byte((uint8_t*)EEPROM_POWER_COUNT, eeprom_read_byte((uint8_t*)EEPROM_POWER_COUNT) + 1);
-eeprom_update_word((uint16_t*)EEPROM_POWER_COUNT_TOT, eeprom_read_word((uint16_t*)EEPROM_POWER_COUNT_TOT) + 1);
-wdt_enable(WDTO_500MS);
-WRITE(BEEPER,HIGH);
-while(1)
-     ;
+    printf_P(_N("UVLO_TINY - end %d\n"), _millis() - time_start);
+    uvlo_drain_reset();
 }
 #endif //UVLO_SUPPORT
 
@@ -9860,12 +10796,19 @@ void setup_uvlo_interrupt() {
 	DDRE &= ~(1 << 4); //input pin
 	PORTE &= ~(1 << 4); //no internal pull-up
 
-						//sensing falling edge
+    // sensing falling edge
 	EICRB |= (1 << 0);
 	EICRB &= ~(1 << 1);
 
-	//enable INT4 interrupt
+	// enable INT4 interrupt
 	EIMSK |= (1 << 4);
+
+    // check if power was lost before we armed the interrupt
+    if(!(PINE & (1 << 4)) && eeprom_read_byte((uint8_t*)EEPROM_UVLO))
+    {
+        SERIAL_ECHOLNPGM("INT4");
+        uvlo_drain_reset();
+    }
 }
 
 ISR(INT4_vect) {
@@ -9882,44 +10825,55 @@ void recover_print(uint8_t automatic) {
 	lcd_update(2);
   lcd_setstatuspgm(_i("Recovering print    "));////MSG_RECOVERING_PRINT c=20 r=1
 
-      bool bTiny=(eeprom_read_byte((uint8_t*)EEPROM_UVLO)==2);
-      recover_machine_state_after_power_panic(bTiny); //recover position, temperatures and extrude_multipliers
-  // Lift the print head, so one may remove the excess priming material.
-      if(!bTiny&&(current_position[Z_AXIS]<25))
-          enquecommand_P(PSTR("G1 Z25 F800"));
+  // Recover position, temperatures and extrude_multipliers
+  bool mbl_was_active = recover_machine_state_after_power_panic();
+
+  // Lift the print head 25mm, first to avoid collisions with oozed material with the print,
+  // and second also so one may remove the excess priming material.
+  if(eeprom_read_byte((uint8_t*)EEPROM_UVLO) == 1)
+  {
+      sprintf_P(cmd, PSTR("G1 Z%.3f F800"), current_position[Z_AXIS] + 25);
+      enquecommand(cmd);
+  }
 
-  // Home X and Y axes. Homing just X and Y shall not touch the babystep and the world2machine transformation status.
+  // Home X and Y axes. Homing just X and Y shall not touch the babystep and the world2machine
+  // transformation status. G28 will not touch Z when MBL is off.
 	enquecommand_P(PSTR("G28 X Y"));
   // Set the target bed and nozzle temperatures and wait.
-	sprintf_P(cmd, PSTR("M109 S%d"), target_temperature[active_extruder]);
+	sprintf_P(cmd, PSTR("M104 S%d"), target_temperature[active_extruder]);
 	enquecommand(cmd);
 	sprintf_P(cmd, PSTR("M190 S%d"), target_temperature_bed);
 	enquecommand(cmd);
+	sprintf_P(cmd, PSTR("M109 S%d"), target_temperature[active_extruder]);
+	enquecommand(cmd);
+
 	enquecommand_P(PSTR("M83")); //E axis relative mode
-	//enquecommand_P(PSTR("G1 E5 F120")); //Extrude some filament to stabilize pessure
-    // If not automatically recoreverd (long power loss), extrude extra filament to stabilize 
-    if(automatic == 0){ 
-        enquecommand_P(PSTR("G1 E5 F120")); //Extrude some filament to stabilize pessure 
-    } 
-	enquecommand_P(PSTR("G1 E"  STRINGIFY(-default_retraction)" F480"));
+
+    // If not automatically recoreverd (long power loss)
+    if(automatic == 0){
+        //Extrude some filament to stabilize the pressure
+        enquecommand_P(PSTR("G1 E5 F120"));
+        // Retract to be consistent with a short pause
+        sprintf_P(cmd, PSTR("G1 E%-0.3f F2700"), default_retraction);
+        enquecommand(cmd);
+    }
 
 	printf_P(_N("After waiting for temp:\nCurrent pos X_AXIS:%.3f\nCurrent pos Y_AXIS:%.3f\n"), current_position[X_AXIS], current_position[Y_AXIS]);
 
   // Restart the print.
-	restore_print_from_eeprom();
+  restore_print_from_eeprom(mbl_was_active);
   printf_P(_N("Current pos Z_AXIS:%.3f\nCurrent pos E_AXIS:%.3f\n"), current_position[Z_AXIS], current_position[E_AXIS]);
 }
 
-void recover_machine_state_after_power_panic(bool bTiny)
+bool recover_machine_state_after_power_panic()
 {
-  char cmd[30];
-  // 1) Recover the logical cordinates at the time of the power panic.
-  // The logical XY coordinates are needed to recover the machine Z coordinate corrected by the mesh bed leveling.
-  current_position[X_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 0));
-  current_position[Y_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 4));
-
-  // 2) Restore the mesh bed leveling offsets. This is 2*7*7=98 bytes, which takes 98*3.4us=333us in worst case.
-  mbl.active = false;
+  // 1) Preset some dummy values for the XY axes
+  current_position[X_AXIS] = 0;
+  current_position[Y_AXIS] = 0;
+
+  // 2) Restore the mesh bed leveling offsets, but not the MBL status.
+  // This is 2*7*7=98 bytes, which takes 98*3.4us=333us in worst case.
+  bool mbl_was_active = false;
   for (int8_t mesh_point = 0; mesh_point < MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS; ++ mesh_point) {
     uint8_t ix = mesh_point % MESH_NUM_X_POINTS; // from 0 to MESH_NUM_X_POINTS - 1
     uint8_t iy = mesh_point / MESH_NUM_X_POINTS;
@@ -9927,32 +10881,16 @@ void recover_machine_state_after_power_panic(bool bTiny)
     int16_t v;
     eeprom_read_block(&v, (void*)(EEPROM_UVLO_MESH_BED_LEVELING_FULL+2*mesh_point), 2);
     if (v != 0)
-      mbl.active = true;
+      mbl_was_active = true;
     mbl.z_values[iy][ix] = float(v) * 0.001f;
   }
 
-  // Recover the logical coordinate of the Z axis at the time of the power panic.
+  // Recover the physical coordinate of the Z axis at the time of the power panic.
   // The current position after power panic is moved to the next closest 0th full step.
-  if(bTiny){    
-    current_position[Z_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_TINY_CURRENT_POSITION_Z))
-     + float((1024 - eeprom_read_word((uint16_t*)(EEPROM_UVLO_TINY_Z_MICROSTEPS)) 
-    + 7) >> 4) / cs.axis_steps_per_unit[Z_AXIS];
-
-    //after multiple power panics the print is slightly in the air so get it little bit down. 
-    //Not exactly sure why is this happening, but it has something to do with bed leveling and world2machine coordinates 
-    current_position[Z_AXIS] -= 0.4*mbl.get_z(current_position[X_AXIS], current_position[Y_AXIS]); 
-  }
-  else{
-    current_position[Z_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION_Z)) + 
-    UVLO_Z_AXIS_SHIFT + float((1024 - eeprom_read_word((uint16_t*)(EEPROM_UVLO_Z_MICROSTEPS)) 
-    + 7) >> 4) / cs.axis_steps_per_unit[Z_AXIS];
-  }
-  if (eeprom_read_byte((uint8_t*)EEPROM_UVLO_E_ABS)) {
-	  current_position[E_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION_E));
-	  sprintf_P(cmd, PSTR("G92 E"));
-	  dtostrf(current_position[E_AXIS], 6, 3, cmd + strlen(cmd));
-	  enquecommand(cmd);
-  }
+  current_position[Z_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_TINY_CURRENT_POSITION_Z));
+
+  // Recover last E axis position
+  current_position[E_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION_E));
 
   memcpy(destination, current_position, sizeof(destination));
 
@@ -9968,20 +10906,16 @@ void recover_machine_state_after_power_panic(bool bTiny)
   // The baby stepping value is used to reset the physical Z axis when rehoming the Z axis.
   babystep_load();
 
-  // 5) Set the physical positions from the logical positions using the world2machine transformation and the active bed leveling.
+  // 5) Set the physical positions from the logical positions using the world2machine transformation
+  // This is only done to inizialize Z/E axes with physical locations, since X/Y are unknown.
   plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 
-  // 6) Power up the motors, mark their positions as known.
-  //FIXME Verfiy, whether the X and Y axes should be powered up here, as they will later be re-homed anyway.
-  axis_known_position[X_AXIS] = true; enable_x();
-  axis_known_position[Y_AXIS] = true; enable_y();
-  axis_known_position[Z_AXIS] = true; enable_z();
-  
-  SERIAL_ECHOPGM("recover_machine_state_after_power_panic, initial ");
-  print_physical_coordinates();
+  // 6) Power up the Z motors, mark their positions as known.
+  axis_known_position[Z_AXIS] = true;
+  enable_z();
 
   // 7) Recover the target temperatures.
-  target_temperature[active_extruder] = eeprom_read_byte((uint8_t*)EEPROM_UVLO_TARGET_HOTEND);
+  target_temperature[active_extruder] = eeprom_read_word((uint16_t*)EEPROM_UVLO_TARGET_HOTEND);
   target_temperature_bed = eeprom_read_byte((uint8_t*)EEPROM_UVLO_TARGET_BED);
 
   // 8) Recover extruder multipilers
@@ -9999,9 +10933,15 @@ void recover_machine_state_after_power_panic(bool bTiny)
   saved_target[Y_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_SAVED_TARGET+1*4));
   saved_target[Z_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_SAVED_TARGET+2*4));
   saved_target[E_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_SAVED_TARGET+3*4));
+
+#ifdef LIN_ADVANCE
+  extruder_advance_K = eeprom_read_float((float*)EEPROM_UVLO_LA_K);
+#endif
+
+  return mbl_was_active;
 }
 
-void restore_print_from_eeprom() {
+void restore_print_from_eeprom(bool mbl_was_active) {
 	int feedrate_rec;
 	int feedmultiply_rec;
 	uint8_t fan_speed_rec;
@@ -10012,7 +10952,7 @@ void restore_print_from_eeprom() {
 
 	fan_speed_rec = eeprom_read_byte((uint8_t*)EEPROM_UVLO_FAN_SPEED);
     feedrate_rec = eeprom_read_word((uint16_t*)EEPROM_UVLO_FEEDRATE);
-	EEPROM_read_B(EEPROM_UVLO_FEEDMULTIPLY, &feedmultiply_rec);
+    feedmultiply_rec = eeprom_read_word((uint16_t*)EEPROM_UVLO_FEEDMULTIPLY);
 	SERIAL_ECHOPGM("Feedrate:");
 	MYSERIAL.print(feedrate_rec);
 	SERIAL_ECHOPGM(", feedmultiply:");
@@ -10042,30 +10982,38 @@ void restore_print_from_eeprom() {
 	enquecommand(cmd);
 	uint32_t position = eeprom_read_dword((uint32_t*)(EEPROM_FILE_POSITION));
 	SERIAL_ECHOPGM("Position read from eeprom:");
-	MYSERIAL.println(position);	
-  // E axis relative mode.
-	enquecommand_P(PSTR("M83"));
-  // Move to the XY print position in logical coordinates, where the print has been killed.
-	strcpy_P(cmd, PSTR("G1 X")); strcat(cmd, ftostr32(eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 0))));
-	strcat_P(cmd, PSTR(" Y"));   strcat(cmd, ftostr32(eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 4))));
-	strcat_P(cmd, PSTR(" F2000"));
+	MYSERIAL.println(position);
+
+    // Move to the XY print position in logical coordinates, where the print has been killed, but
+    // without shifting Z along the way. This requires performing the move without mbl.
+	sprintf_P(cmd, PSTR("G1 X%f Y%f F3000"),
+              eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 0)),
+              eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 4)));
 	enquecommand(cmd);
-  //moving on Z axis ahead, set EEPROM_UVLO to 1, so normal uvlo can fire
-  eeprom_update_byte((uint8_t*)EEPROM_UVLO,1);
-  // Move the Z axis down to the print, in logical coordinates.
-	strcpy_P(cmd, PSTR("G1 Z")); strcat(cmd, ftostr32(eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION_Z))));
+
+    // Enable MBL and switch to logical positioning
+    if (mbl_was_active)
+        enquecommand_P(PSTR("PRUSA MBL V1"));
+
+    // Move the Z axis down to the print, in logical coordinates.
+    sprintf_P(cmd, PSTR("G1 Z%f"), eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION_Z)));
 	enquecommand(cmd);
+
   // Unretract.
-	enquecommand_P(PSTR("G1 E"  STRINGIFY(2*default_retraction)" F480"));
+    sprintf_P(cmd, PSTR("G1 E%0.3f F2700"), default_retraction);
+    enquecommand(cmd);
+  // Recover final E axis position and mode
+    float pos_e = eeprom_read_float((float*)(EEPROM_UVLO_CURRENT_POSITION_E));
+    sprintf_P(cmd, PSTR("G92 E"));
+    dtostrf(pos_e, 6, 3, cmd + strlen(cmd));
+    enquecommand(cmd);
+    if (eeprom_read_byte((uint8_t*)EEPROM_UVLO_E_ABS))
+        enquecommand_P(PSTR("M82")); //E axis abslute mode
   // Set the feedrates saved at the power panic.
 	sprintf_P(cmd, PSTR("G1 F%d"), feedrate_rec);
 	enquecommand(cmd);
 	sprintf_P(cmd, PSTR("M220 S%d"), feedmultiply_rec);
 	enquecommand(cmd);
-	if (eeprom_read_byte((uint8_t*)EEPROM_UVLO_E_ABS))
-	{
-	  enquecommand_P(PSTR("M82")); //E axis abslute mode
-	}
   // Set the fan speed saved at the power panic.
 	strcpy_P(cmd, PSTR("M106 S"));
 	strcat(cmd, itostr3(int(fan_speed_rec)));
@@ -10214,7 +11162,7 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
 #endif
 
   // save the global state at planning time
-  if (blocks_queued())
+  if (current_block)
   {
       memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
       saved_feedrate2 = current_block->gcode_feedrate;
@@ -10230,8 +11178,6 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
     saved_feedmultiply2 = feedmultiply; //save feedmultiply
 	saved_active_extruder = active_extruder; //save active_extruder
 	saved_extruder_temperature = degTargetHotend(active_extruder);
-
-	saved_extruder_under_pressure = extruder_under_pressure; //extruder under pressure flag - currently unused
 	saved_extruder_relative_mode = axis_relative_modes[E_AXIS];
 	saved_fanSpeed = fanSpeed;
 	cmdqueue_reset(); //empty cmdqueue
@@ -10248,23 +11194,29 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
     // move away from the print.
     char buf[48];
 
-	// First unretract (relative extrusion)
-	if(!saved_extruder_relative_mode){
-		enquecommand(PSTR("M83"), true);
-	}
-	//retract 45mm/s
-	// A single sprintf may not be faster, but is definitely 20B shorter
-	// than a sequence of commands building the string piece by piece
-	// A snprintf would have been a safer call, but since it is not used
-	// in the whole program, its implementation would bring more bytes to the total size
-	// The behavior of dtostrf 8,3 should be roughly the same as %-0.3
-	sprintf_P(buf, PSTR("G1 E%-0.3f F2700"), e_move);
-	enquecommand(buf, false);
-
-	// Then lift Z axis
-	sprintf_P(buf, PSTR("G1 Z%-0.3f F%-0.3f"), saved_pos[Z_AXIS] + z_move, homing_feedrate[Z_AXIS]); 
-    // At this point the command queue is empty.
-    enquecommand(buf, false);
+    if(e_move)
+    {
+        // First unretract (relative extrusion)
+        if(!saved_extruder_relative_mode){
+            enquecommand(PSTR("M83"), true);
+        }
+        //retract 45mm/s
+        // A single sprintf may not be faster, but is definitely 20B shorter
+        // than a sequence of commands building the string piece by piece
+        // A snprintf would have been a safer call, but since it is not used
+        // in the whole program, its implementation would bring more bytes to the total size
+        // The behavior of dtostrf 8,3 should be roughly the same as %-0.3
+        sprintf_P(buf, PSTR("G1 E%-0.3f F2700"), e_move);
+        enquecommand(buf, false);
+    }
+
+    if(z_move)
+    {
+        // Then lift Z axis
+        sprintf_P(buf, PSTR("G1 Z%-0.3f F%-0.3f"), saved_pos[Z_AXIS] + z_move, homing_feedrate[Z_AXIS]);
+        enquecommand(buf, false);
+    }
+
     // If this call is invoked from the main Arduino loop() function, let the caller know that the command
     // in the command queue is not the original command, but a new one, so it should not be removed from the queue.
     repeatcommand_front();
@@ -10318,12 +11270,10 @@ void restore_print_from_ram_and_continue(float e_move)
 
 	//first move print head in XY to the saved position:
 	plan_buffer_line(saved_pos[X_AXIS], saved_pos[Y_AXIS], current_position[Z_AXIS], saved_pos[E_AXIS] - e_move, homing_feedrate[Z_AXIS]/13, active_extruder);
-	st_synchronize();
 	//then move Z
 	plan_buffer_line(saved_pos[X_AXIS], saved_pos[Y_AXIS], saved_pos[Z_AXIS], saved_pos[E_AXIS] - e_move, homing_feedrate[Z_AXIS]/13, active_extruder);
-	st_synchronize();
 	//and finaly unretract (35mm/s)
-	plan_buffer_line(saved_pos[X_AXIS], saved_pos[Y_AXIS], saved_pos[Z_AXIS], saved_pos[E_AXIS], 35, active_extruder);
+	plan_buffer_line(saved_pos[X_AXIS], saved_pos[Y_AXIS], saved_pos[Z_AXIS], saved_pos[E_AXIS], FILAMENTCHANGE_RFEED, active_extruder);
 	st_synchronize();
 
   #ifdef FANCHECK
@@ -10360,6 +11310,7 @@ void restore_print_from_ram_and_continue(float e_move)
 // Cancel the state related to a currently saved print
 void cancel_saved_printing()
 {
+    eeprom_update_byte((uint8_t*)EEPROM_UVLO, 0);
     saved_target[0] = SAVED_TARGET_UNSET;
     saved_printing_type = PRINTING_TYPE_NONE;
     saved_printing = false;
@@ -10671,8 +11622,6 @@ if(!(bEnableForce_z||eeprom_read_byte((uint8_t*)EEPROM_SILENT)))
 
 void disable_force_z()
 {
-    uint16_t z_microsteps=0;
-
     if(!bEnableForce_z) return;   // motor already disabled (may be ;-p )
 
     bEnableForce_z=false;
@@ -10683,8 +11632,6 @@ void disable_force_z()
     update_mode_profile();
     tmc2130_init(true);
 #endif // TMC2130
-
-    axis_known_position[Z_AXIS]=false;
 }
 
 

+ 2 - 1
Firmware/config.h

@@ -34,7 +34,8 @@
 //#define PAT9125_I2C_ADDR  0x79  //ID=HI
 //#define PAT9125_I2C_ADDR  0x73  //ID=NC
 #define PAT9125_XRES      0
-#define PAT9125_YRES      240
+#define PAT9125_YRES      240                   // maximum resolution (5*X cpi)
+#define PAT9124_YRES_MM   (5*PAT9125_YRES/25.4) // counts per mm
 
 //SM4 configuration
 #define SM4_DEFDELAY      500       //default step delay [us]

+ 7 - 8
Firmware/eeprom.h

@@ -72,14 +72,14 @@ static_assert(sizeof(Sheets) == EEPROM_SHEETS_SIZEOF, "Sizeof(Sheets) is not EEP
 #define EEPROM_FILENAME (EEPROM_UVLO_CURRENT_POSITION - 8) //8chars to store filename without extension
 #define EEPROM_FILE_POSITION (EEPROM_FILENAME - 4) //32 bit for uint32_t file position 
 #define EEPROM_UVLO_CURRENT_POSITION_Z	(EEPROM_FILE_POSITION - 4) //float for current position in Z
-#define EEPROM_UVLO_TARGET_HOTEND		(EEPROM_UVLO_CURRENT_POSITION_Z - 1)
-#define EEPROM_UVLO_TARGET_BED			(EEPROM_UVLO_TARGET_HOTEND - 1)
+#define EEPROM_UVLO_UNUSED_001		(EEPROM_UVLO_CURRENT_POSITION_Z - 1) // uint8_t (unused)
+#define EEPROM_UVLO_TARGET_BED			(EEPROM_UVLO_UNUSED_001 - 1)
 #define EEPROM_UVLO_FEEDRATE			(EEPROM_UVLO_TARGET_BED - 2) //uint16_t
 #define EEPROM_UVLO_FAN_SPEED			(EEPROM_UVLO_FEEDRATE - 1) 
 #define EEPROM_FAN_CHECK_ENABLED		(EEPROM_UVLO_FAN_SPEED - 1)
 #define EEPROM_UVLO_MESH_BED_LEVELING     (EEPROM_FAN_CHECK_ENABLED - 9*2)
 
-#define EEPROM_UVLO_Z_MICROSTEPS     (EEPROM_UVLO_MESH_BED_LEVELING - 2)
+#define EEPROM_UVLO_Z_MICROSTEPS     (EEPROM_UVLO_MESH_BED_LEVELING - 2) // uint16_t (could be removed)
 #define EEPROM_UVLO_E_ABS            (EEPROM_UVLO_Z_MICROSTEPS - 1)
 #define EEPROM_UVLO_CURRENT_POSITION_E	(EEPROM_UVLO_E_ABS - 4)                 //float for current position in E
 
@@ -165,13 +165,11 @@ static_assert(sizeof(Sheets) == EEPROM_SHEETS_SIZEOF, "Sizeof(Sheets) is not EEP
 #define EEPROM_EXTRUDER_MULTIPLIER_2 (EEPROM_EXTRUDER_MULTIPLIER_1 - 4)                        //float
 #define EEPROM_EXTRUDEMULTIPLY (EEPROM_EXTRUDER_MULTIPLIER_2 - 2)                              // uint16
 
-//
 #define EEPROM_UVLO_TINY_CURRENT_POSITION_Z (EEPROM_EXTRUDEMULTIPLY-4) // float
-#define EEPROM_UVLO_TINY_Z_MICROSTEPS (EEPROM_UVLO_TINY_CURRENT_POSITION_Z-2) // uint16
+#define EEPROM_UVLO_TARGET_HOTEND (EEPROM_UVLO_TINY_CURRENT_POSITION_Z-2) // uint16
 
 // Sound Mode
-//#define EEPROM_SOUND_MODE (EEPROM_EXTRUDEMULTIPLY-1) // uint8
-#define EEPROM_SOUND_MODE (EEPROM_UVLO_TINY_Z_MICROSTEPS-1) // uint8
+#define EEPROM_SOUND_MODE (EEPROM_UVLO_TARGET_HOTEND-1) // uint8
 #define EEPROM_AUTO_DEPLETE (EEPROM_SOUND_MODE-1) //bool
 
 #define EEPROM_FSENS_OQ_MEASS_ENABLED (EEPROM_AUTO_DEPLETE - 1) //bool
@@ -212,9 +210,10 @@ static Sheets * const EEPROM_Sheets_base = (Sheets*)(EEPROM_SHEETS_BASE);
 #define EEPROM_BACKLIGHT_MODE (EEPROM_BACKLIGHT_LEVEL_LOW-1) // uint8
 #define EEPROM_BACKLIGHT_TIMEOUT (EEPROM_BACKLIGHT_MODE-2) // uint16
 
+#define EEPROM_UVLO_LA_K (EEPROM_BACKLIGHT_TIMEOUT-4) // float
 
 //This is supposed to point to last item to allow EEPROM overrun check. Please update when adding new items.
-#define EEPROM_LAST_ITEM EEPROM_BACKLIGHT_TIMEOUT
+#define EEPROM_LAST_ITEM EEPROM_UVLO_LA_K
 // !!!!!
 // !!!!! this is end of EEPROM section ... all updates MUST BE inserted before this mark !!!!!
 // !!!!!

+ 172 - 135
Firmware/fsensor.cpp

@@ -6,12 +6,9 @@
 #include <avr/pgmspace.h>
 #include "pat9125.h"
 #include "stepper.h"
-#include "planner.h"
-#include "fastio.h"
 #include "io_atmega2560.h"
 #include "cmdqueue.h"
 #include "ultralcd.h"
-#include "ConfigurationStore.h"
 #include "mmu.h"
 #include "cardreader.h"
 
@@ -21,19 +18,19 @@
 
 //! @name Basic parameters
 //! @{
-#define FSENSOR_CHUNK_LEN    0.64F  //!< filament sensor chunk length 0.64mm
-#define FSENSOR_ERR_MAX         17  //!< filament sensor maximum error count for runout detection
+#define FSENSOR_CHUNK_LEN      1.25 //!< filament sensor chunk length (mm)
+#define FSENSOR_ERR_MAX           4 //!< filament sensor maximum error/chunk count for runout detection
+
+#define FSENSOR_SOFTERR_CMAX      3 //!< number of contiguous soft failures before a triggering a runout
+#define FSENSOR_SOFTERR_DELTA 30000 //!< maximum interval (ms) to consider soft failures contiguous
 //! @}
 
 //! @name Optical quality measurement parameters
 //! @{
-#define FSENSOR_OQ_MAX_ES      6    //!< maximum error sum while loading (length ~64mm = 100chunks)
-#define FSENSOR_OQ_MAX_EM      2    //!< maximum error counter value while loading
-#define FSENSOR_OQ_MIN_YD      2    //!< minimum yd per chunk (applied to avg value)
-#define FSENSOR_OQ_MAX_YD      200  //!< maximum yd per chunk (applied to avg value)
-#define FSENSOR_OQ_MAX_PD      4    //!< maximum positive deviation (= yd_max/yd_avg)
-#define FSENSOR_OQ_MAX_ND      5    //!< maximum negative deviation (= yd_avg/yd_min)
-#define FSENSOR_OQ_MAX_SH      13   //!< maximum shutter value
+#define FSENSOR_OQ_MAX_ES      2    //!< maximum sum of error blocks during filament recheck
+#define FSENSOR_OQ_MIN_YD      2    //!< minimum yd sum during filament check (counts per inch)
+#define FSENSOR_OQ_MIN_BR      80   //!< minimum brightness value
+#define FSENSOR_OQ_MAX_SH      10   //!< maximum shutter value
 //! @}
 
 const char ERRMSG_PAT9125_NOT_RESP[] PROGMEM = "PAT9125 not responding (%d)!\n";
@@ -47,25 +44,30 @@ const char ERRMSG_PAT9125_NOT_RESP[] PROGMEM = "PAT9125 not responding (%d)!\n";
 #define FSENSOR_INT_PIN_PCMSK_BIT PCINT13         // PinChange Interrupt / PinChange Enable Mask @ PJ4
 #define FSENSOR_INT_PIN_PCICR_BIT PCIE1           // PinChange Interrupt Enable / Flag @ PJ4
 
-//uint8_t fsensor_int_pin = FSENSOR_INT_PIN;
-uint8_t fsensor_int_pin_old = 0;
-int16_t fsensor_chunk_len = 0;
-
 //! enabled = initialized and sampled every chunk event
 bool fsensor_enabled = true;
 //! runout watching is done in fsensor_update (called from main loop)
 bool fsensor_watch_runout = true;
 //! not responding - is set if any communication error occurred during initialization or readout
 bool fsensor_not_responding = false;
+
+#ifdef PAT9125
+uint8_t fsensor_int_pin_old = 0;
+//! optical checking "chunk lenght" (already in steps)
+int16_t fsensor_chunk_len = 0;
 //! enable/disable quality meassurement
 bool fsensor_oq_meassure_enabled = false;
-
 //! number of errors, updated in ISR
 uint8_t fsensor_err_cnt = 0;
 //! variable for accumulating step count (updated callbacks from stepper and ISR)
 int16_t fsensor_st_cnt = 0;
-//! last dy value from pat9125 sensor (used in ISR)
-int16_t fsensor_dy_old = 0;
+//! count of total sensor "soft" failures (filament status checks)
+uint8_t fsensor_softfail = 0;
+//! timestamp of last soft failure
+unsigned long fsensor_softfail_last = 0;
+//! count of soft failures within the configured time
+uint8_t fsensor_softfail_ccnt = 0;
+#endif
 
 //! log flag: 0=log disabled, 1=log enabled
 uint8_t fsensor_log = 1;
@@ -78,6 +80,8 @@ uint8_t fsensor_log = 1;
 bool fsensor_autoload_enabled = true;
 //! autoload watching enable/disable flag
 bool fsensor_watch_autoload = false;
+
+#ifdef PAT9125
 //
 uint16_t fsensor_autoload_y;
 //
@@ -87,6 +91,7 @@ uint32_t fsensor_autoload_last_millis;
 //
 uint8_t fsensor_autoload_sum;
 //! @}
+#endif
 
 
 //! @name filament optical quality measurement variables
@@ -124,14 +129,34 @@ unsigned long nIRsensorLastTime;
 void fsensor_stop_and_save_print(void)
 {
     printf_P(PSTR("fsensor_stop_and_save_print\n"));
-    stop_and_save_print_to_ram(0, 0); //XYZE - no change
+    stop_and_save_print_to_ram(0, 0);
+    fsensor_watch_runout = false;
+}
+
+#ifdef PAT9125
+// Reset all internal counters to zero, including stepper callbacks
+void fsensor_reset_err_cnt()
+{
+    fsensor_err_cnt = 0;
+    pat9125_y = 0;
+    st_reset_fsensor();
 }
 
+void fsensor_set_axis_steps_per_unit(float u)
+{
+    fsensor_chunk_len = (int16_t)(FSENSOR_CHUNK_LEN * u);
+}
+#endif
+
+
 void fsensor_restore_print_and_continue(void)
 {
     printf_P(PSTR("fsensor_restore_print_and_continue\n"));
-	fsensor_err_cnt = 0;
-    restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
+    fsensor_watch_runout = true;
+#ifdef PAT9125
+    fsensor_reset_err_cnt();
+#endif
+    restore_print_from_ram_and_continue(0);
 }
 
 // fsensor_checkpoint_print cuts the current print job at the current position,
@@ -155,7 +180,7 @@ void fsensor_init(void)
 #ifdef PAT9125
 	uint8_t oq_meassure_enabled = eeprom_read_byte((uint8_t*)EEPROM_FSENS_OQ_MEASS_ENABLED);
 	fsensor_oq_meassure_enabled = (oq_meassure_enabled == 1)?true:false;
-	fsensor_chunk_len = (int16_t)(FSENSOR_CHUNK_LEN * cs.axis_steps_per_unit[E_AXIS]);
+    fsensor_set_axis_steps_per_unit(cs.axis_steps_per_unit[E_AXIS]);
 
 	if (!pat9125)
 	{
@@ -195,8 +220,7 @@ bool fsensor_enable(bool bUpdateEEPROM)
 		fsensor_enabled = pat9125 ? true : false;
 		fsensor_watch_runout = true;
 		fsensor_oq_meassure = false;
-		fsensor_err_cnt = 0;
-		fsensor_dy_old = 0;
+        fsensor_reset_err_cnt();
 		eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, fsensor_enabled ? 0x01 : 0x00);
 		FSensorStateMenu = fsensor_enabled ? 1 : 0;
 	}
@@ -261,7 +285,7 @@ void fsensor_autoload_check_start(void)
 	if (!fsensor_enabled) return;
 	if (!fsensor_autoload_enabled) return;
 	if (fsensor_watch_autoload) return;
-	if (!pat9125_update_y()) //update sensor
+	if (!pat9125_update()) //update sensor
 	{
 		fsensor_disable();
 		fsensor_not_responding = true;
@@ -276,12 +300,11 @@ void fsensor_autoload_check_start(void)
 	fsensor_autoload_last_millis = _millis();
 	fsensor_watch_runout = false;
 	fsensor_watch_autoload = true;
-	fsensor_err_cnt = 0;
 }
 
+
 void fsensor_autoload_check_stop(void)
 {
-
 //	puts_P(_N("fsensor_autoload_check_stop\n"));
 	if (!fsensor_enabled) return;
 //	puts_P(_N("fsensor_autoload_check_stop 1\n"));
@@ -292,7 +315,7 @@ void fsensor_autoload_check_stop(void)
 	fsensor_autoload_sum = 0;
 	fsensor_watch_autoload = false;
 	fsensor_watch_runout = true;
-	fsensor_err_cnt = 0;
+    fsensor_reset_err_cnt();
 }
 #endif //PAT9125
 
@@ -357,6 +380,7 @@ bool fsensor_check_autoload(void)
 	return false;
 }
 
+#ifdef PAT9125
 void fsensor_oq_meassure_set(bool State)
 {
 	fsensor_oq_meassure_enabled = State;
@@ -374,12 +398,11 @@ void fsensor_oq_meassure_start(uint8_t skip)
 	fsensor_oq_yd_sum = 0;
 	fsensor_oq_er_sum = 0;
 	fsensor_oq_er_max = 0;
-	fsensor_oq_yd_min = FSENSOR_OQ_MAX_YD;
+	fsensor_oq_yd_min = INT16_MAX;
 	fsensor_oq_yd_max = 0;
 	fsensor_oq_sh_sum = 0;
 	pat9125_update();
 	pat9125_y = 0;
-	fsensor_watch_runout = false;
 	fsensor_oq_meassure = true;
 }
 
@@ -391,10 +414,9 @@ void fsensor_oq_meassure_stop(void)
 	printf_P(_N(" st_sum=%u yd_sum=%u er_sum=%u er_max=%hhu\n"), fsensor_oq_st_sum, fsensor_oq_yd_sum, fsensor_oq_er_sum, fsensor_oq_er_max);
 	printf_P(_N(" yd_min=%u yd_max=%u yd_avg=%u sh_avg=%u\n"), fsensor_oq_yd_min, fsensor_oq_yd_max, (uint16_t)((uint32_t)fsensor_oq_yd_sum * fsensor_chunk_len / fsensor_oq_st_sum), (uint16_t)(fsensor_oq_sh_sum / fsensor_oq_samples));
 	fsensor_oq_meassure = false;
-	fsensor_watch_runout = true;
-	fsensor_err_cnt = 0;
 }
 
+#ifdef FSENSOR_QUALITY
 const char _OK[] PROGMEM = "OK";
 const char _NG[] PROGMEM = "NG!";
 
@@ -430,18 +452,24 @@ bool fsensor_oq_result(void)
 	printf_P(_N("fsensor_oq_result %S\n"), (res?_OK:_NG));
 	return res;
 }
-#ifdef PAT9125
+#endif //FSENSOR_QUALITY
+
 ISR(FSENSOR_INT_PIN_VECT)
 {
 	if (mmu_enabled || ir_sensor_detected) return;
 	if (!((fsensor_int_pin_old ^ FSENSOR_INT_PIN_PIN_REG) & FSENSOR_INT_PIN_MASK)) return;
 	fsensor_int_pin_old = FSENSOR_INT_PIN_PIN_REG;
+
+    // prevent isr re-entry
 	static bool _lock = false;
 	if (_lock) return;
 	_lock = true;
+
+    // fetch fsensor_st_cnt atomically
 	int st_cnt = fsensor_st_cnt;
 	fsensor_st_cnt = 0;
 	sei();
+
 	uint8_t old_err_cnt = fsensor_err_cnt;
 	uint8_t pat9125_res = fsensor_oq_meassure?pat9125_update():pat9125_update_y();
 	if (!pat9125_res)
@@ -450,56 +478,71 @@ ISR(FSENSOR_INT_PIN_VECT)
 		fsensor_not_responding = true;
 		printf_P(ERRMSG_PAT9125_NOT_RESP, 1);
 	}
+
 	if (st_cnt != 0)
-	{ //movement
-		if (st_cnt > 0) //positive movement
-		{
-			if (pat9125_y < 0)
-			{
-				if (fsensor_err_cnt)
-					fsensor_err_cnt += 2;
-				else
-					fsensor_err_cnt++;
-			}
-			else if (pat9125_y > 0)
-			{
-				if (fsensor_err_cnt)
-					fsensor_err_cnt--;
-			}
-			else //(pat9125_y == 0)
-				if (((fsensor_dy_old <= 0) || (fsensor_err_cnt)) && (st_cnt > (fsensor_chunk_len >> 1)))
-					fsensor_err_cnt++;
-			if (fsensor_oq_meassure)
-			{
-				if (fsensor_oq_skipchunk)
-				{
-					fsensor_oq_skipchunk--;
-					fsensor_err_cnt = 0;
-				}
-				else
-				{
-					if (st_cnt == fsensor_chunk_len)
-					{
-						if (pat9125_y > 0) if (fsensor_oq_yd_min > pat9125_y) fsensor_oq_yd_min = (fsensor_oq_yd_min + pat9125_y) / 2;
-						if (pat9125_y >= 0) if (fsensor_oq_yd_max < pat9125_y) fsensor_oq_yd_max = (fsensor_oq_yd_max + pat9125_y) / 2;
-					}
-					fsensor_oq_samples++;
-					fsensor_oq_st_sum += st_cnt;
-					if (pat9125_y > 0) fsensor_oq_yd_sum += pat9125_y;
-					if (fsensor_err_cnt > old_err_cnt)
-						fsensor_oq_er_sum += (fsensor_err_cnt - old_err_cnt);
-					if (fsensor_oq_er_max < fsensor_err_cnt)
-						fsensor_oq_er_max = fsensor_err_cnt;
-					fsensor_oq_sh_sum += pat9125_s;
-				}
-			}
-		}
-		else //negative movement
-		{
-		}
-	}
-	else
-	{ //no movement
+	{
+        // movement was planned, check for sensor movement
+        int8_t st_dir = st_cnt >= 0;
+        int8_t pat9125_dir = pat9125_y >= 0;
+
+        if (pat9125_y == 0)
+        {
+            if (st_dir)
+            {
+                // no movement detected: we might be within a blind sensor range,
+                // update the frame and shutter parameters we didn't earlier
+                if (!fsensor_oq_meassure)
+                    pat9125_update_bs();
+
+                // increment the error count only if underexposed: filament likely missing
+                if ((pat9125_b < FSENSOR_OQ_MIN_BR) && (pat9125_s > FSENSOR_OQ_MAX_SH))
+                {
+                    // check for a dark frame (<30% avg brightness) with long exposure
+                    ++fsensor_err_cnt;
+                }
+                else
+                {
+                    // good frame, filament likely present
+                    if(fsensor_err_cnt) --fsensor_err_cnt;
+                }
+            }
+        }
+        else if (pat9125_dir != st_dir)
+        {
+            // detected direction opposite of motor movement
+            if (st_dir) ++fsensor_err_cnt;
+        }
+        else if (pat9125_dir == st_dir)
+        {
+            // direction agreeing with planned movement
+            if (fsensor_err_cnt) --fsensor_err_cnt;
+        }
+
+        if (st_dir && fsensor_oq_meassure)
+        {
+            // extruding with quality assessment
+            if (fsensor_oq_skipchunk)
+            {
+                fsensor_oq_skipchunk--;
+                fsensor_err_cnt = 0;
+            }
+            else
+            {
+                if (st_cnt == fsensor_chunk_len)
+                {
+                    if (pat9125_y > 0) if (fsensor_oq_yd_min > pat9125_y) fsensor_oq_yd_min = (fsensor_oq_yd_min + pat9125_y) / 2;
+                    if (pat9125_y >= 0) if (fsensor_oq_yd_max < pat9125_y) fsensor_oq_yd_max = (fsensor_oq_yd_max + pat9125_y) / 2;
+                }
+                fsensor_oq_samples++;
+                fsensor_oq_st_sum += st_cnt;
+                if (pat9125_y > 0) fsensor_oq_yd_sum += pat9125_y;
+                if (fsensor_err_cnt > old_err_cnt)
+                    fsensor_oq_er_sum += (fsensor_err_cnt - old_err_cnt);
+                if (fsensor_oq_er_max < fsensor_err_cnt)
+                    fsensor_oq_er_max = fsensor_err_cnt;
+                fsensor_oq_sh_sum += pat9125_s;
+            }
+        }
 	}
 
 #ifdef DEBUG_FSENSOR_LOG
@@ -510,9 +553,7 @@ ISR(FSENSOR_INT_PIN_VECT)
 	}
 #endif //DEBUG_FSENSOR_LOG
 
-	fsensor_dy_old = pat9125_y;
 	pat9125_y = 0;
-
 	_lock = false;
 	return;
 }
@@ -532,31 +573,16 @@ void fsensor_setup_interrupt(void)
      PCICR |= bit(FSENSOR_INT_PIN_PCICR_BIT);     // enable corresponding PinChangeInterrupt (set of pins)
 }
 
-#endif //PAT9125
-
-void fsensor_st_block_begin(block_t* bl)
+void fsensor_st_block_chunk(int cnt)
 {
 	if (!fsensor_enabled) return;
-	if (((fsensor_st_cnt > 0) && (bl->direction_bits & 0x8)) || 
-		((fsensor_st_cnt < 0) && !(bl->direction_bits & 0x8)))
-	{
-// !!! bit toggling (PINxn <- 1) (for PinChangeInterrupt) does not work for some MCU pins
-		if (PIN_GET(FSENSOR_INT_PIN)) {PIN_VAL(FSENSOR_INT_PIN, LOW);}
-		else {PIN_VAL(FSENSOR_INT_PIN, HIGH);}
-	}
-}
+	fsensor_st_cnt += cnt;
 
-void fsensor_st_block_chunk(block_t* bl, int cnt)
-{
-	if (!fsensor_enabled) return;
-	fsensor_st_cnt += (bl->direction_bits & 0x8)?-cnt:cnt;
-	if ((fsensor_st_cnt >= fsensor_chunk_len) || (fsensor_st_cnt <= -fsensor_chunk_len))
-	{
-// !!! bit toggling (PINxn <- 1) (for PinChangeInterrupt) does not work for some MCU pins
-		if (PIN_GET(FSENSOR_INT_PIN)) {PIN_VAL(FSENSOR_INT_PIN, LOW);}
-		else {PIN_VAL(FSENSOR_INT_PIN, HIGH);}
-	}
+    // !!! bit toggling (PINxn <- 1) (for PinChangeInterrupt) does not work for some MCU pins
+    if (PIN_GET(FSENSOR_INT_PIN)) {PIN_VAL(FSENSOR_INT_PIN, LOW);}
+    else {PIN_VAL(FSENSOR_INT_PIN, HIGH);}
 }
+#endif //PAT9125
 
 
 //! Common code for enqueing M600 and supplemental codes into the command queue.
@@ -576,50 +602,61 @@ void fsensor_enque_M600(){
 void fsensor_update(void)
 {
 #ifdef PAT9125
-		if (fsensor_enabled && fsensor_watch_runout && (fsensor_err_cnt > FSENSOR_ERR_MAX))
+		if (fsensor_watch_runout && (fsensor_err_cnt > FSENSOR_ERR_MAX))
 		{
+			fsensor_stop_and_save_print();
+            KEEPALIVE_STATE(IN_HANDLER);
+
 			bool autoload_enabled_tmp = fsensor_autoload_enabled;
 			fsensor_autoload_enabled = false;
 			bool oq_meassure_enabled_tmp = fsensor_oq_meassure_enabled;
 			fsensor_oq_meassure_enabled = true;
 
-			fsensor_stop_and_save_print();
+            // move the nozzle away while checking the filament
+            current_position[Z_AXIS] += 0.8;
+            if(current_position[Z_AXIS] > Z_MAX_POS) current_position[Z_AXIS] = Z_MAX_POS;
+            plan_buffer_line_curposXYZE(max_feedrate[Z_AXIS], active_extruder);
+            st_synchronize();
 
-			fsensor_err_cnt = 0;
+            // check the filament in isolation
+            fsensor_reset_err_cnt();
 			fsensor_oq_meassure_start(0);
-
-			enquecommand_front_P((PSTR("G1 E-3 F200")));
-			process_commands();
-			KEEPALIVE_STATE(IN_HANDLER);
-			cmdqueue_pop_front();
-			st_synchronize();
-
-			enquecommand_front_P((PSTR("G1 E3 F200")));
-			process_commands();
-			KEEPALIVE_STATE(IN_HANDLER);
-			cmdqueue_pop_front();
-			st_synchronize();
-
-			uint8_t err_cnt = fsensor_err_cnt;
+            float e_tmp = current_position[E_AXIS];
+            current_position[E_AXIS] -= 3;
+            plan_buffer_line_curposXYZE(250/60, active_extruder);
+            current_position[E_AXIS] = e_tmp;
+            plan_buffer_line_curposXYZE(200/60, active_extruder);
+            st_synchronize();
 			fsensor_oq_meassure_stop();
 
 			bool err = false;
-			err |= (err_cnt > 1);
-
-			err |= (fsensor_oq_er_sum > 2);
-			err |= (fsensor_oq_yd_sum < (4 * FSENSOR_OQ_MIN_YD));
+			err |= (fsensor_err_cnt > 0);                   // final error count is non-zero
+			err |= (fsensor_oq_er_sum > FSENSOR_OQ_MAX_ES); // total error count is above limit
+			err |= (fsensor_oq_yd_sum < FSENSOR_OQ_MIN_YD); // total measured distance is below limit
 
             fsensor_restore_print_and_continue();
-			fsensor_autoload_enabled = autoload_enabled_tmp;
-			fsensor_oq_meassure_enabled = oq_meassure_enabled_tmp;
-
-			if (!err)
-				printf_P(PSTR("fsensor_err_cnt = 0\n"));
-			else
-				fsensor_enque_M600();
+            fsensor_autoload_enabled = autoload_enabled_tmp;
+            fsensor_oq_meassure_enabled = oq_meassure_enabled_tmp;
+
+            unsigned long now = _millis();
+            if (!err && (now - fsensor_softfail_last) > FSENSOR_SOFTERR_DELTA)
+                fsensor_softfail_ccnt = 0;
+            if (!err && fsensor_softfail_ccnt <= FSENSOR_SOFTERR_CMAX)
+            {
+                printf_P(PSTR("fsensor_err_cnt = 0\n"));
+                ++fsensor_softfail;
+                ++fsensor_softfail_ccnt;
+                fsensor_softfail_last = now;
+            }
+            else
+            {
+                fsensor_softfail_ccnt = 0;
+                fsensor_softfail_last = 0;
+                fsensor_enque_M600();
+            }
 		}
 #else //PAT9125
-		if (CHECK_FSENSOR && fsensor_enabled && ir_sensor_detected)
+		if (CHECK_FSENSOR && ir_sensor_detected)
         {
                if(digitalRead(IR_SENSOR_PIN))
                {                                  // IR_SENSOR_PIN ~ H

+ 21 - 9
Firmware/fsensor.h

@@ -6,15 +6,16 @@
 #include "config.h"
 
 
-//! minimum meassured chunk length in steps
-extern int16_t fsensor_chunk_len;
 // enable/disable flag
 extern bool fsensor_enabled;
 // not responding flag
 extern bool fsensor_not_responding;
-//enable/disable quality meassurement
-extern bool fsensor_oq_meassure_enabled;
-
+#ifdef PAT9125
+// optical checking "chunk lenght" (already in steps)
+extern int16_t fsensor_chunk_len;
+// count of soft failures
+extern uint8_t fsensor_softfail;
+#endif
 
 //! @name save restore printing
 //! @{
@@ -28,6 +29,11 @@ extern void fsensor_checkpoint_print(void);
 //! initialize
 extern void fsensor_init(void);
 
+#ifdef PAT9125
+//! update axis resolution
+extern void fsensor_set_axis_steps_per_unit(float u);
+#endif
+
 //! @name enable/disable
 //! @{
 extern bool fsensor_enable(bool bUpdateEEPROM=true);
@@ -52,21 +58,27 @@ extern void fsensor_autoload_check_stop(void);
 extern bool fsensor_check_autoload(void);
 //! @}
 
+#ifdef PAT9125
 //! @name optical quality measurement support
 //! @{
+extern bool fsensor_oq_meassure_enabled;
 extern void fsensor_oq_meassure_set(bool State);
 extern void fsensor_oq_meassure_start(uint8_t skip);
 extern void fsensor_oq_meassure_stop(void);
 extern bool fsensor_oq_result(void);
 //! @}
 
-
-#include "planner.h"
 //! @name callbacks from stepper
 //! @{
-extern void fsensor_st_block_begin(block_t* bl);
-extern void fsensor_st_block_chunk(block_t* bl, int cnt);
+extern void fsensor_st_block_chunk(int cnt);
+
+// There's really nothing to do in block_begin: the stepper ISR likely has
+// called us already at the end of the last block, making this integration
+// redundant. LA1.5 might not always do that during a coasting move, so attempt
+// to drain fsensor_st_cnt anyway at the beginning of the new block.
+#define fsensor_st_block_begin(rev) fsensor_st_block_chunk(0)
 //! @}
+#endif //PAT9125
 
 
 #if IR_SENSOR_ANALOG

+ 10 - 0
Firmware/heatbed_pwm.cpp

@@ -45,6 +45,12 @@
 // If there are any change requirements in the future, the signal must be checked with an osciloscope again,
 // ad-hoc changes may completely screw things up!
 
+// 2020-01-29 update: we are introducing a new option to the automaton that will allow us to force the output state
+// to either full ON or OFF. This is so that interference during the MBL probing is minimal.
+// To accomplish this goal we use bedPWMDisabled. It is only supposed to be used for brief periods of time as to
+// not make the bed temperature too unstable. Also, careful consideration should be used when using this
+// option as leaving this enabled will also keep the bed output in the state it stopped in.
+
 ///! Definition off finite automaton states
 enum class States : uint8_t {
 	ZERO_START = 0,///< entry point of the automaton - reads the soft_pwm_bed value for the next whole PWM cycle
@@ -61,6 +67,8 @@ enum class States : uint8_t {
 ///! Inner states of the finite automaton
 static States state = States::ZERO_START;
 
+bool bedPWMDisabled = 0;
+
 ///! Fast PWM counter is used in the RISE and FALL states (62.5kHz)
 static uint8_t slowCounter = 0;
 ///! Slow PWM counter is used in the ZERO and ONE states (62.5kHz/8 or 64)
@@ -93,6 +101,7 @@ ISR(TIMER0_OVF_vect)          // timer compare interrupt service routine
 {
 	switch(state){
 	case States::ZERO_START:
+		if (bedPWMDisabled) return; // stay in the OFF state and do not change the output pin
 		pwm = soft_pwm_bed << 1;// expecting soft_pwm_bed to be 7bit!
 		if( pwm != 0 ){
 			state = States::ZERO;     // do nothing, let it tick once again after the 30Hz period
@@ -136,6 +145,7 @@ ISR(TIMER0_OVF_vect)          // timer compare interrupt service routine
 		break;
 	case States::ONE:             // state ONE - we'll either stay in ONE or change to FALL
 		OCR0B = 255;
+		if (bedPWMDisabled) return; // stay in the ON state and do not change the output pin
 		slowCounter += slowInc;   // this does software timer_clk/256 or less
 		if( slowCounter < pwm ){
 			return;

+ 88 - 0
Firmware/la10compat.cpp

@@ -0,0 +1,88 @@
+#include "la10compat.h"
+#include "Marlin.h"
+
+
+static LA10C_MODE la10c_mode = LA10C_UNKNOWN; // Current LA compatibility mode
+static float la10c_orig_jerk = 0;             // Unadjusted/saved e-jerk
+
+
+LA10C_MODE la10c_mode_get()
+{
+    return la10c_mode;
+}
+
+
+void la10c_mode_change(LA10C_MODE mode)
+{
+    if(mode == la10c_mode) return;
+
+    // always restore to the last unadjusted E-jerk value
+    if(la10c_orig_jerk)
+        cs.max_jerk[E_AXIS] = la10c_orig_jerk;
+
+    SERIAL_ECHOPGM("LA10C: Linear Advance mode: ");
+    switch(mode)
+    {
+    case LA10C_UNKNOWN: SERIAL_ECHOLNPGM("UNKNOWN"); break;
+    case LA10C_LA15:    SERIAL_ECHOLNPGM("1.5"); break;
+    case LA10C_LA10:    SERIAL_ECHOLNPGM("1.0"); break;
+    }
+    la10c_mode = mode;
+
+    // adjust the E-jerk if needed
+    cs.max_jerk[E_AXIS] = la10c_jerk(cs.max_jerk[E_AXIS]);
+}
+
+
+// Approximate a LA10 value to a LA15 equivalent.
+static float la10c_convert(float k)
+{
+    float new_K = k * 0.004 - 0.06;
+    return (new_K < 0? 0: new_K);
+}
+
+
+float la10c_value(float k)
+{
+    if(la10c_mode == LA10C_UNKNOWN)
+    {
+        // do not autodetect until a valid value is seen
+        if(k == 0)
+            return 0;
+        else if(k < 0)
+            return -1;
+
+        la10c_mode_change(k < 10? LA10C_LA15: LA10C_LA10);
+    }
+
+    if(la10c_mode == LA10C_LA15)
+        return (k >= 0 && k < 10? k: -1);
+    else
+        return (k >= 0? la10c_convert(k): -1);
+}
+
+
+float la10c_jerk(float j)
+{
+    la10c_orig_jerk = j;
+
+    if(la10c_mode != LA10C_LA10)
+        return j;
+
+    // check for a compatible range of values prior to convert (be sure that
+    // a higher E-jerk would still be compatible wrt the E accell range)
+    if(j < 4.5 && cs.max_acceleration_units_per_sq_second_normal[E_AXIS] < 2000)
+        return j;
+
+    // bring low E-jerk values into equivalent LA 1.5 values by
+    // flattening the response in the (1-4.5) range using a piecewise
+    // function. Is it truly worth to preserve the difference between
+    // 1.5/2.5 E-jerk for LA1.0? Probably not, but we try nonetheless.
+    j = j < 1.0? j * 3.625:
+        j < 4.5? j * 0.25 + 3.375:
+        j;
+
+    SERIAL_ECHOPGM("LA10C: Adjusted E-Jerk: ");
+    SERIAL_ECHOLN(j);
+    return j;
+}

+ 46 - 0
Firmware/la10compat.h

@@ -0,0 +1,46 @@
+// la10compat: LA10->LA15 conversion
+//
+// When the current mode is UNKNOWN autodetection is active and any K<10
+// will set the mode to LA15, LA10 is set otherwise. When LA10
+// compatbility mode is active the K factor is converted to a LA15
+// equivalent (that is, the return value is always a LA15 value).
+//
+// E-jerk<2 is also bumped in LA10 mode to restore the the printing speed
+// to values comparable to existing settings.
+//
+// Once the interpretation mode has been set it is kept until the mode
+// is explicitly reset. This is done to handle transparent fallback for
+// old firmware revisions in combination with the following gcode
+// sequence:
+//
+//   M900 K0.01 ; set LA15 value (interpreted by any firmware)
+//   M900 K10   ; set LA10 value (ignored by LA15 firmware)
+//
+// A LA15 firmware without this module will only parse the first
+// correctly, rejecting the second. A LA10 FW will parse both, but keep
+// the last value. Since the LA15 value, if present, corresponds to the
+// truth value, the compatibility stub needs to "lock" onto the first
+// seen value for the current print.
+//
+// The mode needs to be carefully reset for each print in order for
+// diffent versions of M900 to be interpreted independently.
+
+#pragma once
+
+enum __attribute__((packed)) LA10C_MODE
+{
+    LA10C_UNKNOWN = 0,
+    LA10C_LA15    = 1,
+    LA10C_LA10    = 2
+};
+
+// Explicitly set/get/reset the interpretation mode for la10c_value()
+void la10c_mode_change(LA10C_MODE mode);
+LA10C_MODE la10c_mode_get();
+static inline void la10c_reset() { la10c_mode_change(LA10C_UNKNOWN); }
+
+// Return a LA15 K value according to the supplied value and mode
+float la10c_value(float k);
+
+// Return an updated LA15 E-jerk value according to the current mode
+float la10c_jerk(float j);

+ 2 - 2
Firmware/menu.cpp

@@ -103,7 +103,7 @@ void menu_back(void)
 menu_back(1);
 }
 
-static void menu_back_no_reset(void)
+void menu_back_no_reset(void)
 {
 	if (menu_depth > 0)
 	{
@@ -146,7 +146,7 @@ void menu_submenu(menu_func_t submenu)
 	}
 }
 
-static void menu_submenu_no_reset(menu_func_t submenu)
+void menu_submenu_no_reset(menu_func_t submenu)
 {
 	if (menu_depth < MENU_DEPTH_MAX)
 	{

+ 2 - 1
Firmware/menu.h

@@ -76,6 +76,7 @@ void menu_start(void);
 extern void menu_end(void);
 
 extern void menu_back(void);
+extern void menu_back_no_reset(void);
 extern void menu_back(uint8_t nLevel);
 
 extern void menu_back_scroll(int scrollback);
@@ -85,6 +86,7 @@ extern void menu_back_if_clicked(void);
 extern void menu_back_if_clicked_fb(void);
 
 extern void menu_submenu(menu_func_t submenu);
+extern void menu_submenu_no_reset(menu_func_t submenu);
 
 extern void menu_submenu_scroll(menu_func_t submenu);
 
@@ -135,7 +137,6 @@ extern const char menu_fmt_int3[];
 extern const char menu_fmt_float31[];
 extern const char menu_fmt_float13[];
 
-
 extern void menu_draw_float31(const char* str, float val);
 
 extern void menu_draw_float13(const char* str, float val);

+ 4 - 0
Firmware/mesh_bed_calibration.cpp

@@ -6,6 +6,7 @@
 #include "mesh_bed_leveling.h"
 #include "stepper.h"
 #include "ultralcd.h"
+#include "temperature.h"
 
 #ifdef TMC2130
 #include "tmc2130.h"
@@ -946,6 +947,7 @@ inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, i
         )
 {
 	bool high_deviation_occured = false; 
+    bedPWMDisabled = 1;
 #ifdef TMC2130
 	FORCE_HIGH_POWER_START;
 #endif
@@ -1044,6 +1046,7 @@ inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, i
 #ifdef TMC2130
 	FORCE_HIGH_POWER_END;
 #endif
+    bedPWMDisabled = 0;
 	return true;
 
 error:
@@ -1053,6 +1056,7 @@ error:
 #ifdef TMC2130
 	FORCE_HIGH_POWER_END;
 #endif
+    bedPWMDisabled = 0;
 	return false;
 }
 

+ 2 - 0
Firmware/messages.c

@@ -168,3 +168,5 @@ const char MSG_OCTOPRINT_CANCEL[] PROGMEM_N1 = "// action:cancel"; ////
 const char MSG_FANCHECK_EXTRUDER[] PROGMEM_N1 = "Err: EXTR. FAN ERROR"; ////c=20
 const char MSG_FANCHECK_PRINT[] PROGMEM_N1 = "Err: PRINT FAN ERROR"; ////c=20
 const char MSG_M112_KILL[] PROGMEM_N1 = "M112 called. Emergency Stop."; ////c=20
+const char MSG_ADVANCE_K[] PROGMEM_N1 = "Advance K:"; ////c=13
+const char MSG_POWERPANIC_DETECTED[] PROGMEM_N1 = "POWER PANIC DETECTED"; ////c=20

+ 2 - 0
Firmware/messages.h

@@ -169,6 +169,8 @@ extern const char MSG_OCTOPRINT_CANCEL[];
 extern const char MSG_FANCHECK_EXTRUDER[];
 extern const char MSG_FANCHECK_PRINT[];
 extern const char MSG_M112_KILL[];
+extern const char MSG_ADVANCE_K[];
+extern const char MSG_POWERPANIC_DETECTED[];
 
 #if defined(__cplusplus)
 }

+ 2 - 1
Firmware/mmu.cpp

@@ -1547,7 +1547,7 @@ void mmu_continue_loading(bool blocking)
     };
     Ls state = Ls::Enter;
 
-    const uint_least8_t max_retry = 2;
+    const uint_least8_t max_retry = 3;
     uint_least8_t retry = 0;
 
     while (!success)
@@ -1589,6 +1589,7 @@ void mmu_continue_loading(bool blocking)
             if (blocking)
             {
                 marlin_wait_for_click();
+                st_synchronize();
                 restore_print_from_ram_and_continue(0);
                 state = Ls::Retry;
             }

+ 9 - 14
Firmware/pat9125.c

@@ -183,9 +183,9 @@ uint8_t pat9125_update(void)
 		if (pat9125_PID1 == 0xff) return 0;
 		if (ucMotion & 0x80)
 		{
-			uint8_t ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
-			uint8_t ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
-			uint8_t ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
+			uint16_t ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
+			uint16_t ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
+			uint16_t ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
 			if (pat9125_PID1 == 0xff) return 0;
 			int16_t iDX = ucXL | ((ucXYH << 4) & 0xf00);
 			int16_t iDY = ucYL | ((ucXYH << 8) & 0xf00);
@@ -207,8 +207,8 @@ uint8_t pat9125_update_y(void)
 		if (pat9125_PID1 == 0xff) return 0;
 		if (ucMotion & 0x80)
 		{
-			uint8_t ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
-			uint8_t ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
+			uint16_t ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
+			uint16_t ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
 			if (pat9125_PID1 == 0xff) return 0;
 			int16_t iDY = ucYL | ((ucXYH << 8) & 0xf00);
 			if (iDY & 0x800) iDY -= 4096;
@@ -219,18 +219,13 @@ uint8_t pat9125_update_y(void)
 	return 0;
 }
 
-uint8_t pat9125_update_y2(void)
+uint8_t pat9125_update_bs(void)
 {
 	if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
 	{
-		uint8_t ucMotion = pat9125_rd_reg(PAT9125_MOTION);
-		if (pat9125_PID1 == 0xff) return 0; //NOACK error
-		if (ucMotion & 0x80)
-		{
-			int8_t dy = pat9125_rd_reg(PAT9125_DELTA_YL);
-			if (pat9125_PID1 == 0xff) return 0; //NOACK error
-			pat9125_y -= dy; //negative number, because direction switching does not work
-		}
+		pat9125_b = pat9125_rd_reg(PAT9125_FRAME);
+		pat9125_s = pat9125_rd_reg(PAT9125_SHUTTER);
+		if (pat9125_PID1 == 0xff) return 0;
 		return 1;
 	}
 	return 0;

+ 3 - 3
Firmware/pat9125.h

@@ -19,9 +19,9 @@ extern uint8_t pat9125_b;
 extern uint8_t pat9125_s;
 
 extern uint8_t pat9125_init(void);
-extern uint8_t pat9125_update(void);
-extern uint8_t pat9125_update_y(void);
-extern uint8_t pat9125_update_y2(void);
+extern uint8_t pat9125_update(void);    // update all sensor data
+extern uint8_t pat9125_update_y(void);  // update _y only
+extern uint8_t pat9125_update_bs(void); // update _b/_s only
 
 
 #if defined(__cplusplus)

+ 153 - 67
Firmware/planner.cpp

@@ -126,11 +126,14 @@ float extrude_min_temp=EXTRUDE_MINTEMP;
 #endif
 
 #ifdef LIN_ADVANCE
-    float extruder_advance_k = LIN_ADVANCE_K,
-    advance_ed_ratio = LIN_ADVANCE_E_D_RATIO,
-    position_float[NUM_AXIS] = { 0 };
+float extruder_advance_K = LIN_ADVANCE_K;
+float position_float[NUM_AXIS];
 #endif
 
+// Request the next block to start at zero E count
+static bool plan_reset_next_e_queue;
+static bool plan_reset_next_e_sched;
+
 // Returns the index of the next block in the ring buffer
 // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
 static inline int8_t next_block_index(int8_t block_index) {
@@ -262,6 +265,13 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
     }
   }
 
+#ifdef LIN_ADVANCE
+  uint16_t final_adv_steps = 0;
+  if (block->use_advance_lead) {
+      final_adv_steps = exit_speed * block->adv_comp;
+  }
+#endif
+
   CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
   // This block locks the interrupts globally for 4.38 us,
   // which corresponds to a maximum repeat frequency of 228.57 kHz.
@@ -272,6 +282,9 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
     block->decelerate_after = accelerate_steps+plateau_steps;
     block->initial_rate = initial_rate;
     block->final_rate = final_rate;
+#ifdef LIN_ADVANCE
+    block->final_adv_steps = final_adv_steps;
+#endif
   }
   CRITICAL_SECTION_END;
 }
@@ -424,14 +437,16 @@ void plan_init() {
   block_buffer_head = 0;
   block_buffer_tail = 0;
   memset(position, 0, sizeof(position)); // clear position
-#ifdef LIN_ADVANCE
-  memset(position_float, 0, sizeof(position)); // clear position
-#endif
+  #ifdef LIN_ADVANCE
+  memset(position_float, 0, sizeof(position_float)); // clear position
+  #endif
   previous_speed[0] = 0.0;
   previous_speed[1] = 0.0;
   previous_speed[2] = 0.0;
   previous_speed[3] = 0.0;
   previous_nominal_speed = 0.0;
+  plan_reset_next_e_queue = false;
+  plan_reset_next_e_sched = false;
 }
 
 
@@ -639,7 +654,9 @@ void planner_abort_hard()
     // Apply inverse world correction matrix.
     machine2world(current_position[X_AXIS], current_position[Y_AXIS]);
     memcpy(destination, current_position, sizeof(destination));
-
+#ifdef LIN_ADVANCE
+    memcpy(position_float, current_position, sizeof(position_float));
+#endif
     // Resets planner junction speeds. Assumes start from rest.
     previous_nominal_speed = 0.0;
     previous_speed[0] = 0.0;
@@ -647,6 +664,9 @@ void planner_abort_hard()
     previous_speed[2] = 0.0;
     previous_speed[3] = 0.0;
 
+    plan_reset_next_e_queue = false;
+    plan_reset_next_e_sched = false;
+
     // Relay to planner wait routine, that the current line shall be canceled.
     waiting_inside_plan_buffer_line_print_aborted = true;
 }
@@ -710,6 +730,20 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
   // Save the global feedrate at scheduling time
   block->gcode_feedrate = feedrate;
 
+  // Reset the starting E position when requested
+  if (plan_reset_next_e_queue)
+  {
+      position[E_AXIS] = 0;
+#ifdef LIN_ADVANCE
+      position_float[E_AXIS] = 0;
+#endif
+
+      // the block might still be discarded later, but we need to ensure the lower-level
+      // count_position is also reset correctly for consistent results!
+      plan_reset_next_e_queue = false;
+      plan_reset_next_e_sched = true;
+  }
+
 #ifdef ENABLE_AUTO_BED_LEVELING
   apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #endif // ENABLE_AUTO_BED_LEVELING
@@ -775,21 +809,15 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
 #endif // ENABLE_MESH_BED_LEVELING
   target[E_AXIS] = lround(e*cs.axis_steps_per_unit[E_AXIS]);
   
-#ifdef LIN_ADVANCE
-    const float mm_D_float = sqrt(sq(x - position_float[X_AXIS]) + sq(y - position_float[Y_AXIS]));
-    float de_float = e - position_float[E_AXIS];
-#endif
-    
   #ifdef PREVENT_DANGEROUS_EXTRUDE
   if(target[E_AXIS]!=position[E_AXIS])
   {
     if(degHotend(active_extruder)<extrude_min_temp)
     {
       position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
-#ifdef LIN_ADVANCE
+      #ifdef LIN_ADVANCE
       position_float[E_AXIS] = e;
-      de_float = 0;
-#endif
+      #endif
       SERIAL_ECHO_START;
       SERIAL_ECHOLNRPGM(_n(" cold extrusion prevented"));////MSG_ERR_COLD_EXTRUDE_STOP
     }
@@ -798,10 +826,9 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
     if(labs(target[E_AXIS]-position[E_AXIS])>cs.axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
     {
       position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
-#ifdef LIN_ADVANCE
-        position_float[E_AXIS] = e;
-        de_float = 0;
-#endif
+      #ifdef LIN_ADVANCE
+      position_float[E_AXIS] = e;
+      #endif
       SERIAL_ECHO_START;
       SERIAL_ECHOLNRPGM(_n(" too long extrusion prevented"));////MSG_ERR_LONG_EXTRUDE_STOP
     }
@@ -1012,6 +1039,9 @@ Having the real displacement of the head, we can calculate the total movement le
     block->nominal_rate *= speed_factor;
   }
 
+#ifdef LIN_ADVANCE
+  float e_D_ratio = 0;
+#endif
   // Compute and limit the acceleration rate for the trapezoid generator.  
   // block->step_event_count ... event count of the fastest axis
   // block->millimeters ... Euclidian length of the XYZ movement or the E length, if no XYZ movement.
@@ -1019,10 +1049,51 @@ Having the real displacement of the head, we can calculate the total movement le
   if(block->steps_x.wide == 0 && block->steps_y.wide == 0 && block->steps_z.wide == 0)
   {
     block->acceleration_st = ceil(cs.retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
+    #ifdef LIN_ADVANCE
+    block->use_advance_lead = false;
+    #endif
   }
   else
   {
     block->acceleration_st = ceil(cs.acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
+
+    #ifdef LIN_ADVANCE
+    /**
+     * Use LIN_ADVANCE within this block if all these are true:
+     *
+     * block->steps_e           : This is a print move, because we checked for X, Y, Z steps before.
+     * extruder_advance_K       : There is an advance factor set.
+     * delta_mm[E_AXIS] > 0     : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
+     * |delta_mm[Z_AXIS]| < 0.5 : Z is only moved for leveling (_not_ for priming)
+     */
+    block->use_advance_lead = block->steps_e.wide
+                              && extruder_advance_K
+                              && delta_mm[E_AXIS] > 0
+                              && abs(delta_mm[Z_AXIS]) < 0.5;
+    if (block->use_advance_lead) {
+        e_D_ratio = (e - position_float[E_AXIS]) /
+                    sqrt(sq(x - position_float[X_AXIS])
+                         + sq(y - position_float[Y_AXIS])
+                         + sq(z - position_float[Z_AXIS]));
+
+        // Check for unusual high e_D ratio to detect if a retract move was combined with the last
+        // print move due to min. steps per segment. Never execute this with advance! This assumes
+        // no one will use a retract length of 0mm < retr_length < ~0.2mm and no one will print
+        // 100mm wide lines using 3mm filament or 35mm wide lines using 1.75mm filament.
+        if (e_D_ratio > 3.0)
+            block->use_advance_lead = false;
+        else {
+            const uint32_t max_accel_steps_per_s2 = cs.max_jerk[E_AXIS] / (extruder_advance_K * e_D_ratio) * steps_per_mm;
+            if (block->acceleration_st > max_accel_steps_per_s2) {
+                block->acceleration_st = max_accel_steps_per_s2;
+                #ifdef LA_DEBUG
+                SERIAL_ECHOLNPGM("LA: Block acceleration limited due to max E-jerk");
+                #endif
+            }
+        }
+    }
+    #endif
+
     // Limit acceleration per axis
     //FIXME Vojtech: One shall rather limit a projection of the acceleration vector instead of using the limit.
     if(((float)block->acceleration_st * (float)block->steps_x.wide / (float)block->step_event_count.wide) > axis_steps_per_sqr_second[X_AXIS])
@@ -1055,6 +1126,47 @@ Having the real displacement of the head, we can calculate the total movement le
 
   block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
 
+#ifdef LIN_ADVANCE
+  if (block->use_advance_lead) {
+      // the nominal speed doesn't change past this point: calculate the compression ratio for the
+      // segment and the required advance steps
+      block->adv_comp = extruder_advance_K * e_D_ratio * cs.axis_steps_per_unit[E_AXIS];
+      block->max_adv_steps = block->nominal_speed * block->adv_comp;
+
+      // to save more space we avoid another copy of calc_timer and go through slow division, but we
+      // still need to replicate the *exact* same step grouping policy (see below)
+      float advance_speed = (extruder_advance_K * e_D_ratio * block->acceleration * cs.axis_steps_per_unit[E_AXIS]);
+      if (advance_speed > MAX_STEP_FREQUENCY) advance_speed = MAX_STEP_FREQUENCY;
+      float advance_rate = (F_CPU / 8.0) / advance_speed;
+      if (advance_speed > 20000) {
+          block->advance_rate = advance_rate * 4;
+          block->advance_step_loops = 4;
+      }
+      else if (advance_speed > 10000) {
+          block->advance_rate = advance_rate * 2;
+          block->advance_step_loops = 2;
+      }
+      else
+      {
+          // never overflow the internal accumulator with very low rates
+          if (advance_rate < UINT16_MAX)
+              block->advance_rate = advance_rate;
+          else
+              block->advance_rate = UINT16_MAX;
+          block->advance_step_loops = 1;
+      }
+
+      #ifdef LA_DEBUG
+      if (block->advance_step_loops > 2)
+          // @wavexx: we should really check for the difference between step_loops and
+          //          advance_step_loops instead. A difference of more than 1 will lead
+          //          to uneven speed and *should* be adjusted here by furthermore
+          //          reducing the speed.
+          SERIAL_ECHOLNPGM("LA: More than 2 steps per eISR loop executed.");
+      #endif
+  }
+#endif
+
   // Start with a safe speed.
   // Safe speed is the speed, from which the machine may halt to stop immediately.
   float safe_speed = block->nominal_speed;
@@ -1083,6 +1195,13 @@ Having the real displacement of the head, we can calculate the total movement le
   // Reset the block flag.
   block->flag = 0;
 
+  if (plan_reset_next_e_sched)
+  {
+      // finally propagate a pending reset
+      block->flag |= BLOCK_FLAG_E_RESET;
+      plan_reset_next_e_sched = false;
+  }
+
   // Initial limit on the segment entry velocity.
   float vmax_junction;
 
@@ -1171,37 +1290,6 @@ Having the real displacement of the head, we can calculate the total movement le
   previous_nominal_speed = block->nominal_speed;
   previous_safe_speed = safe_speed;
 
-#ifdef LIN_ADVANCE
-
-    //
-    // Use LIN_ADVANCE for blocks if all these are true:
-    //
-    // esteps                                          : We have E steps todo (a printing move)
-    //
-    // block->steps[X_AXIS] || block->steps[Y_AXIS]    : We have a movement in XY direction (i.e., not retract / prime).
-    //
-    // extruder_advance_k                              : There is an advance factor set.
-    //
-    // block->steps[E_AXIS] != block->step_event_count : A problem occurs if the move before a retract is too small.
-    //                                                   In that case, the retract and move will be executed together.
-    //                                                   This leads to too many advance steps due to a huge e_acceleration.
-    //                                                   The math is good, but we must avoid retract moves with advance!
-    // de_float > 0.0                                  : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
-    //
-    block->use_advance_lead =  block->steps_e.wide
-                           && (block->steps_x.wide || block->steps_y.wide)
-                           && extruder_advance_k
-                           && (uint32_t)block->steps_e.wide != block->step_event_count.wide
-                           && de_float > 0.0;
-    if (block->use_advance_lead)
-        block->abs_adv_steps_multiplier8 = lround(
-                          extruder_advance_k
-                          * ((advance_ed_ratio < 0.000001) ? de_float / mm_D_float : advance_ed_ratio) // Use the fixed ratio, if set
-                          * (block->nominal_speed / (float)block->nominal_rate)
-                          * cs.axis_steps_per_unit[E_AXIS] * 256.0
-                          );
-#endif
-    
   // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated.
   block->speed_factor = block->nominal_rate / block->nominal_speed;
   calculate_trapezoid_for_block(block, block->entry_speed, safe_speed);
@@ -1215,12 +1303,12 @@ Having the real displacement of the head, we can calculate the total movement le
   // Update position
   memcpy(position, target, sizeof(target)); // position[] = target[]
 
-#ifdef LIN_ADVANCE
+  #ifdef LIN_ADVANCE
   position_float[X_AXIS] = x;
   position_float[Y_AXIS] = y;
   position_float[Z_AXIS] = z;
   position_float[E_AXIS] = e;
-#endif
+  #endif
     
   // Recalculate the trapezoids to maximize speed at the segment transitions while respecting
   // the machine limits (maximum acceleration and maximum jerk).
@@ -1264,14 +1352,7 @@ void plan_set_position(float x, float y, float z, const float &e)
     apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #endif // ENABLE_AUTO_BED_LEVELING
 
-    // Apply the machine correction matrix.
-    if (world2machine_correction_mode != WORLD2MACHINE_CORRECTION_NONE)
-    {
-        float tmpx = x;
-        float tmpy = y;
-        x = world2machine_rotation_and_skew[0][0] * tmpx + world2machine_rotation_and_skew[0][1] * tmpy + world2machine_shift[0];
-        y = world2machine_rotation_and_skew[1][0] * tmpx + world2machine_rotation_and_skew[1][1] * tmpy + world2machine_shift[1];
-    }
+    world2machine(x, y);
 
   position[X_AXIS] = lround(x*cs.axis_steps_per_unit[X_AXIS]);
   position[Y_AXIS] = lround(y*cs.axis_steps_per_unit[Y_AXIS]);
@@ -1283,12 +1364,12 @@ void plan_set_position(float x, float y, float z, const float &e)
   position[Z_AXIS] = lround(z*cs.axis_steps_per_unit[Z_AXIS]);
 #endif // ENABLE_MESH_BED_LEVELING
   position[E_AXIS] = lround(e*cs.axis_steps_per_unit[E_AXIS]);
-#ifdef LIN_ADVANCE
+  #ifdef LIN_ADVANCE
   position_float[X_AXIS] = x;
   position_float[Y_AXIS] = y;
   position_float[Z_AXIS] = z;
   position_float[E_AXIS] = e;
-#endif
+  #endif
   st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
   previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
   previous_speed[0] = 0.0;
@@ -1300,11 +1381,11 @@ void plan_set_position(float x, float y, float z, const float &e)
 // Only useful in the bed leveling routine, when the mesh bed leveling is off.
 void plan_set_z_position(const float &z)
 {
-	#ifdef LIN_ADVANCE
-	position_float[Z_AXIS] = z;
-	#endif
-    position[Z_AXIS] = lround(z*cs.axis_steps_per_unit[Z_AXIS]);
-    st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
+  #ifdef LIN_ADVANCE
+  position_float[Z_AXIS] = z;
+  #endif
+  position[Z_AXIS] = lround(z*cs.axis_steps_per_unit[Z_AXIS]);
+  st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]);
 }
 
 void plan_set_e_position(const float &e)
@@ -1316,6 +1397,11 @@ void plan_set_e_position(const float &e)
   st_set_e_position(position[E_AXIS]);
 }
 
+void plan_reset_next_e()
+{
+    plan_reset_next_e_queue = true;
+}
+
 #ifdef PREVENT_DANGEROUS_EXTRUDE
 void set_extrude_min_temp(float temp)
 {

+ 13 - 4
Firmware/planner.h

@@ -44,6 +44,8 @@ enum BlockFlag {
     // than 32767, therefore the DDA algorithm may run with 16bit resolution only.
     // In addition, the stepper routine will not do any end stop checking for higher performance.
     BLOCK_FLAG_DDA_LOWRES = 8,
+    // Block starts with Zeroed E counter
+    BLOCK_FLAG_E_RESET = 16,
 };
 
 union dda_isteps_t
@@ -110,10 +112,14 @@ typedef struct {
 
   // Pre-calculated division for the calculate_trapezoid_for_block() routine to run faster.
   float speed_factor;
-    
+
 #ifdef LIN_ADVANCE
-  bool use_advance_lead;
-  unsigned long abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float
+  bool use_advance_lead;            // Whether the current block uses LA
+  uint16_t advance_rate,            // Step-rate for extruder speed
+           max_adv_steps,           // max. advance steps to get cruising speed pressure (not always nominal_speed!)
+           final_adv_steps;         // advance steps due to exit speed
+  uint8_t advance_step_loops;       // Number of stepper ticks for each advance isr
+  float adv_comp;                   // Precomputed E compression factor
 #endif
 
   // Save/recovery state data
@@ -123,7 +129,7 @@ typedef struct {
 } block_t;
 
 #ifdef LIN_ADVANCE
-  extern float extruder_advance_k, advance_ed_ratio;
+extern float extruder_advance_K;    // Linear-advance K factor
 #endif
 
 #ifdef ENABLE_AUTO_BED_LEVELING
@@ -164,6 +170,9 @@ void plan_set_position(float x, float y, float z, const float &e);
 void plan_set_z_position(const float &z);
 void plan_set_e_position(const float &e);
 
+// Reset the E position to zero at the start of the next segment
+void plan_reset_next_e();
+
 extern bool e_active();
 
 void check_axes_activity();

+ 147 - 0
Firmware/speed_lookuptable.cpp

@@ -0,0 +1,147 @@
+#include "speed_lookuptable.h"
+
+#if F_CPU == 16000000
+
+const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {\
+{ 62500, 55556}, { 6944, 3268}, { 3676, 1176}, { 2500, 607}, { 1893, 369}, { 1524, 249}, { 1275, 179}, { 1096, 135}, 
+{ 961, 105}, { 856, 85}, { 771, 69}, { 702, 58}, { 644, 49}, { 595, 42}, { 553, 37}, { 516, 32}, 
+{ 484, 28}, { 456, 25}, { 431, 23}, { 408, 20}, { 388, 19}, { 369, 16}, { 353, 16}, { 337, 14}, 
+{ 323, 13}, { 310, 11}, { 299, 11}, { 288, 11}, { 277, 9}, { 268, 9}, { 259, 8}, { 251, 8}, 
+{ 243, 8}, { 235, 7}, { 228, 6}, { 222, 6}, { 216, 6}, { 210, 6}, { 204, 5}, { 199, 5}, 
+{ 194, 5}, { 189, 4}, { 185, 4}, { 181, 4}, { 177, 4}, { 173, 4}, { 169, 4}, { 165, 3}, 
+{ 162, 3}, { 159, 4}, { 155, 3}, { 152, 3}, { 149, 2}, { 147, 3}, { 144, 3}, { 141, 2}, 
+{ 139, 3}, { 136, 2}, { 134, 2}, { 132, 3}, { 129, 2}, { 127, 2}, { 125, 2}, { 123, 2}, 
+{ 121, 2}, { 119, 1}, { 118, 2}, { 116, 2}, { 114, 1}, { 113, 2}, { 111, 2}, { 109, 1}, 
+{ 108, 2}, { 106, 1}, { 105, 2}, { 103, 1}, { 102, 1}, { 101, 1}, { 100, 2}, { 98, 1}, 
+{ 97, 1}, { 96, 1}, { 95, 2}, { 93, 1}, { 92, 1}, { 91, 1}, { 90, 1}, { 89, 1}, 
+{ 88, 1}, { 87, 1}, { 86, 1}, { 85, 1}, { 84, 1}, { 83, 0}, { 83, 1}, { 82, 1}, 
+{ 81, 1}, { 80, 1}, { 79, 1}, { 78, 0}, { 78, 1}, { 77, 1}, { 76, 1}, { 75, 0}, 
+{ 75, 1}, { 74, 1}, { 73, 1}, { 72, 0}, { 72, 1}, { 71, 1}, { 70, 0}, { 70, 1}, 
+{ 69, 0}, { 69, 1}, { 68, 1}, { 67, 0}, { 67, 1}, { 66, 0}, { 66, 1}, { 65, 0}, 
+{ 65, 1}, { 64, 1}, { 63, 0}, { 63, 1}, { 62, 0}, { 62, 1}, { 61, 0}, { 61, 1}, 
+{ 60, 0}, { 60, 0}, { 60, 1}, { 59, 0}, { 59, 1}, { 58, 0}, { 58, 1}, { 57, 0}, 
+{ 57, 1}, { 56, 0}, { 56, 0}, { 56, 1}, { 55, 0}, { 55, 1}, { 54, 0}, { 54, 0}, 
+{ 54, 1}, { 53, 0}, { 53, 0}, { 53, 1}, { 52, 0}, { 52, 0}, { 52, 1}, { 51, 0}, 
+{ 51, 0}, { 51, 1}, { 50, 0}, { 50, 0}, { 50, 1}, { 49, 0}, { 49, 0}, { 49, 1}, 
+{ 48, 0}, { 48, 0}, { 48, 1}, { 47, 0}, { 47, 0}, { 47, 0}, { 47, 1}, { 46, 0}, 
+{ 46, 0}, { 46, 1}, { 45, 0}, { 45, 0}, { 45, 0}, { 45, 1}, { 44, 0}, { 44, 0}, 
+{ 44, 0}, { 44, 1}, { 43, 0}, { 43, 0}, { 43, 0}, { 43, 1}, { 42, 0}, { 42, 0}, 
+{ 42, 0}, { 42, 1}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 1}, { 40, 0}, 
+{ 40, 0}, { 40, 0}, { 40, 0}, { 40, 1}, { 39, 0}, { 39, 0}, { 39, 0}, { 39, 0}, 
+{ 39, 1}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 1}, { 37, 0}, { 37, 0}, 
+{ 37, 0}, { 37, 0}, { 37, 0}, { 37, 1}, { 36, 0}, { 36, 0}, { 36, 0}, { 36, 0}, 
+{ 36, 1}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 1}, 
+{ 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 1}, { 33, 0}, { 33, 0}, 
+{ 33, 0}, { 33, 0}, { 33, 0}, { 33, 0}, { 33, 1}, { 32, 0}, { 32, 0}, { 32, 0}, 
+{ 32, 0}, { 32, 0}, { 32, 0}, { 32, 0}, { 32, 1}, { 31, 0}, { 31, 0}, { 31, 0}, 
+{ 31, 0}, { 31, 0}, { 31, 0}, { 31, 1}, { 30, 0}, { 30, 0}, { 30, 0}, { 30, 0}
+};
+
+const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {\
+{ 62500, 12500}, { 50000, 8334}, { 41666, 5952}, { 35714, 4464}, { 31250, 3473}, { 27777, 2777}, { 25000, 2273}, { 22727, 1894}, 
+{ 20833, 1603}, { 19230, 1373}, { 17857, 1191}, { 16666, 1041}, { 15625, 920}, { 14705, 817}, { 13888, 731}, { 13157, 657}, 
+{ 12500, 596}, { 11904, 541}, { 11363, 494}, { 10869, 453}, { 10416, 416}, { 10000, 385}, { 9615, 356}, { 9259, 331}, 
+{ 8928, 308}, { 8620, 287}, { 8333, 269}, { 8064, 252}, { 7812, 237}, { 7575, 223}, { 7352, 210}, { 7142, 198}, 
+{ 6944, 188}, { 6756, 178}, { 6578, 168}, { 6410, 160}, { 6250, 153}, { 6097, 145}, { 5952, 139}, { 5813, 132}, 
+{ 5681, 126}, { 5555, 121}, { 5434, 115}, { 5319, 111}, { 5208, 106}, { 5102, 102}, { 5000, 99}, { 4901, 94}, 
+{ 4807, 91}, { 4716, 87}, { 4629, 84}, { 4545, 81}, { 4464, 79}, { 4385, 75}, { 4310, 73}, { 4237, 71}, 
+{ 4166, 68}, { 4098, 66}, { 4032, 64}, { 3968, 62}, { 3906, 60}, { 3846, 59}, { 3787, 56}, { 3731, 55}, 
+{ 3676, 53}, { 3623, 52}, { 3571, 50}, { 3521, 49}, { 3472, 48}, { 3424, 46}, { 3378, 45}, { 3333, 44}, 
+{ 3289, 43}, { 3246, 41}, { 3205, 41}, { 3164, 39}, { 3125, 39}, { 3086, 38}, { 3048, 36}, { 3012, 36}, 
+{ 2976, 35}, { 2941, 35}, { 2906, 33}, { 2873, 33}, { 2840, 32}, { 2808, 31}, { 2777, 30}, { 2747, 30}, 
+{ 2717, 29}, { 2688, 29}, { 2659, 28}, { 2631, 27}, { 2604, 27}, { 2577, 26}, { 2551, 26}, { 2525, 25}, 
+{ 2500, 25}, { 2475, 25}, { 2450, 23}, { 2427, 24}, { 2403, 23}, { 2380, 22}, { 2358, 22}, { 2336, 22}, 
+{ 2314, 21}, { 2293, 21}, { 2272, 20}, { 2252, 20}, { 2232, 20}, { 2212, 20}, { 2192, 19}, { 2173, 18}, 
+{ 2155, 19}, { 2136, 18}, { 2118, 18}, { 2100, 17}, { 2083, 17}, { 2066, 17}, { 2049, 17}, { 2032, 16}, 
+{ 2016, 16}, { 2000, 16}, { 1984, 16}, { 1968, 15}, { 1953, 16}, { 1937, 14}, { 1923, 15}, { 1908, 15}, 
+{ 1893, 14}, { 1879, 14}, { 1865, 14}, { 1851, 13}, { 1838, 14}, { 1824, 13}, { 1811, 13}, { 1798, 13}, 
+{ 1785, 12}, { 1773, 13}, { 1760, 12}, { 1748, 12}, { 1736, 12}, { 1724, 12}, { 1712, 12}, { 1700, 11}, 
+{ 1689, 12}, { 1677, 11}, { 1666, 11}, { 1655, 11}, { 1644, 11}, { 1633, 10}, { 1623, 11}, { 1612, 10}, 
+{ 1602, 10}, { 1592, 10}, { 1582, 10}, { 1572, 10}, { 1562, 10}, { 1552, 9}, { 1543, 10}, { 1533, 9}, 
+{ 1524, 9}, { 1515, 9}, { 1506, 9}, { 1497, 9}, { 1488, 9}, { 1479, 9}, { 1470, 9}, { 1461, 8}, 
+{ 1453, 8}, { 1445, 9}, { 1436, 8}, { 1428, 8}, { 1420, 8}, { 1412, 8}, { 1404, 8}, { 1396, 8}, 
+{ 1388, 7}, { 1381, 8}, { 1373, 7}, { 1366, 8}, { 1358, 7}, { 1351, 7}, { 1344, 8}, { 1336, 7}, 
+{ 1329, 7}, { 1322, 7}, { 1315, 7}, { 1308, 6}, { 1302, 7}, { 1295, 7}, { 1288, 6}, { 1282, 7}, 
+{ 1275, 6}, { 1269, 7}, { 1262, 6}, { 1256, 6}, { 1250, 7}, { 1243, 6}, { 1237, 6}, { 1231, 6}, 
+{ 1225, 6}, { 1219, 6}, { 1213, 6}, { 1207, 6}, { 1201, 5}, { 1196, 6}, { 1190, 6}, { 1184, 5}, 
+{ 1179, 6}, { 1173, 5}, { 1168, 6}, { 1162, 5}, { 1157, 5}, { 1152, 6}, { 1146, 5}, { 1141, 5}, 
+{ 1136, 5}, { 1131, 5}, { 1126, 5}, { 1121, 5}, { 1116, 5}, { 1111, 5}, { 1106, 5}, { 1101, 5}, 
+{ 1096, 5}, { 1091, 5}, { 1086, 4}, { 1082, 5}, { 1077, 5}, { 1072, 4}, { 1068, 5}, { 1063, 4}, 
+{ 1059, 5}, { 1054, 4}, { 1050, 4}, { 1046, 5}, { 1041, 4}, { 1037, 4}, { 1033, 5}, { 1028, 4}, 
+{ 1024, 4}, { 1020, 4}, { 1016, 4}, { 1012, 4}, { 1008, 4}, { 1004, 4}, { 1000, 4}, { 996, 4}, 
+{ 992, 4}, { 988, 4}, { 984, 4}, { 980, 4}, { 976, 4}, { 972, 4}, { 968, 3}, { 965, 3}
+};
+
+#elif F_CPU == 20000000
+
+const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {
+   {62500, 54055}, {8445, 3917}, {4528, 1434}, {3094, 745}, {2349, 456}, {1893, 307}, {1586, 222}, {1364, 167},
+   {1197, 131}, {1066, 105}, {961, 86}, {875, 72}, {803, 61}, {742, 53}, {689, 45}, {644, 40},
+   {604, 35}, {569, 32}, {537, 28}, {509, 25}, {484, 23}, {461, 21}, {440, 19}, {421, 17},
+   {404, 16}, {388, 15}, {373, 14}, {359, 13}, {346, 12}, {334, 11}, {323, 10}, {313, 10},
+   {303, 9}, {294, 9}, {285, 8}, {277, 7}, {270, 8}, {262, 7}, {255, 6}, {249, 6},
+   {243, 6}, {237, 6}, {231, 5}, {226, 5}, {221, 5}, {216, 5}, {211, 4}, {207, 5},
+   {202, 4}, {198, 4}, {194, 4}, {190, 3}, {187, 4}, {183, 3}, {180, 3}, {177, 4},
+   {173, 3}, {170, 3}, {167, 2}, {165, 3}, {162, 3}, {159, 2}, {157, 3}, {154, 2},
+   {152, 3}, {149, 2}, {147, 2}, {145, 2}, {143, 2}, {141, 2}, {139, 2}, {137, 2},
+   {135, 2}, {133, 2}, {131, 2}, {129, 1}, {128, 2}, {126, 2}, {124, 1}, {123, 2},
+   {121, 1}, {120, 2}, {118, 1}, {117, 1}, {116, 2}, {114, 1}, {113, 1}, {112, 2},
+   {110, 1}, {109, 1}, {108, 1}, {107, 2}, {105, 1}, {104, 1}, {103, 1}, {102, 1},
+   {101, 1}, {100, 1}, {99, 1}, {98, 1}, {97, 1}, {96, 1}, {95, 1}, {94, 1},
+   {93, 1}, {92, 1}, {91, 0}, {91, 1}, {90, 1}, {89, 1}, {88, 1}, {87, 0},
+   {87, 1}, {86, 1}, {85, 1}, {84, 0}, {84, 1}, {83, 1}, {82, 1}, {81, 0},
+   {81, 1}, {80, 1}, {79, 0}, {79, 1}, {78, 0}, {78, 1}, {77, 1}, {76, 0},
+   {76, 1}, {75, 0}, {75, 1}, {74, 1}, {73, 0}, {73, 1}, {72, 0}, {72, 1},
+   {71, 0}, {71, 1}, {70, 0}, {70, 1}, {69, 0}, {69, 1}, {68, 0}, {68, 1},
+   {67, 0}, {67, 1}, {66, 0}, {66, 1}, {65, 0}, {65, 0}, {65, 1}, {64, 0},
+   {64, 1}, {63, 0}, {63, 1}, {62, 0}, {62, 0}, {62, 1}, {61, 0}, {61, 1},
+   {60, 0}, {60, 0}, {60, 1}, {59, 0}, {59, 0}, {59, 1}, {58, 0}, {58, 0},
+   {58, 1}, {57, 0}, {57, 0}, {57, 1}, {56, 0}, {56, 0}, {56, 1}, {55, 0},
+   {55, 0}, {55, 1}, {54, 0}, {54, 0}, {54, 1}, {53, 0}, {53, 0}, {53, 0},
+   {53, 1}, {52, 0}, {52, 0}, {52, 1}, {51, 0}, {51, 0}, {51, 0}, {51, 1},
+   {50, 0}, {50, 0}, {50, 0}, {50, 1}, {49, 0}, {49, 0}, {49, 0}, {49, 1},
+   {48, 0}, {48, 0}, {48, 0}, {48, 1}, {47, 0}, {47, 0}, {47, 0}, {47, 1},
+   {46, 0}, {46, 0}, {46, 0}, {46, 0}, {46, 1}, {45, 0}, {45, 0}, {45, 0},
+   {45, 1}, {44, 0}, {44, 0}, {44, 0}, {44, 0}, {44, 1}, {43, 0}, {43, 0},
+   {43, 0}, {43, 0}, {43, 1}, {42, 0}, {42, 0}, {42, 0}, {42, 0}, {42, 0},
+   {42, 1}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 1}, {40, 0},
+   {40, 0}, {40, 0}, {40, 0}, {40, 1}, {39, 0}, {39, 0}, {39, 0}, {39, 0},
+   {39, 0}, {39, 0}, {39, 1}, {38, 0}, {38, 0}, {38, 0}, {38, 0}, {38, 0},
+};
+
+const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {
+   {62500, 10417}, {52083, 7441}, {44642, 5580}, {39062, 4340}, {34722, 3472}, {31250, 2841}, {28409, 2368}, {26041, 2003},
+   {24038, 1717}, {22321, 1488}, {20833, 1302}, {19531, 1149}, {18382, 1021}, {17361, 914}, {16447, 822}, {15625, 745},
+   {14880, 676}, {14204, 618}, {13586, 566}, {13020, 520}, {12500, 481}, {12019, 445}, {11574, 414}, {11160, 385},
+   {10775, 359}, {10416, 336}, {10080, 315}, {9765, 296}, {9469, 278}, {9191, 263}, {8928, 248}, {8680, 235},
+   {8445, 222}, {8223, 211}, {8012, 200}, {7812, 191}, {7621, 181}, {7440, 173}, {7267, 165}, {7102, 158},
+   {6944, 151}, {6793, 145}, {6648, 138}, {6510, 133}, {6377, 127}, {6250, 123}, {6127, 118}, {6009, 113},
+   {5896, 109}, {5787, 106}, {5681, 101}, {5580, 98}, {5482, 95}, {5387, 91}, {5296, 88}, {5208, 86},
+   {5122, 82}, {5040, 80}, {4960, 78}, {4882, 75}, {4807, 73}, {4734, 70}, {4664, 69}, {4595, 67},
+   {4528, 64}, {4464, 63}, {4401, 61}, {4340, 60}, {4280, 58}, {4222, 56}, {4166, 55}, {4111, 53},
+   {4058, 52}, {4006, 51}, {3955, 49}, {3906, 48}, {3858, 48}, {3810, 45}, {3765, 45}, {3720, 44},
+   {3676, 43}, {3633, 42}, {3591, 40}, {3551, 40}, {3511, 39}, {3472, 38}, {3434, 38}, {3396, 36},
+   {3360, 36}, {3324, 35}, {3289, 34}, {3255, 34}, {3221, 33}, {3188, 32}, {3156, 31}, {3125, 31},
+   {3094, 31}, {3063, 30}, {3033, 29}, {3004, 28}, {2976, 28}, {2948, 28}, {2920, 27}, {2893, 27},
+   {2866, 26}, {2840, 25}, {2815, 25}, {2790, 25}, {2765, 24}, {2741, 24}, {2717, 24}, {2693, 23},
+   {2670, 22}, {2648, 22}, {2626, 22}, {2604, 22}, {2582, 21}, {2561, 21}, {2540, 20}, {2520, 20},
+   {2500, 20}, {2480, 20}, {2460, 19}, {2441, 19}, {2422, 19}, {2403, 18}, {2385, 18}, {2367, 18},
+   {2349, 17}, {2332, 18}, {2314, 17}, {2297, 16}, {2281, 17}, {2264, 16}, {2248, 16}, {2232, 16},
+   {2216, 16}, {2200, 15}, {2185, 15}, {2170, 15}, {2155, 15}, {2140, 15}, {2125, 14}, {2111, 14},
+   {2097, 14}, {2083, 14}, {2069, 14}, {2055, 13}, {2042, 13}, {2029, 13}, {2016, 13}, {2003, 13},
+   {1990, 13}, {1977, 12}, {1965, 12}, {1953, 13}, {1940, 11}, {1929, 12}, {1917, 12}, {1905, 12},
+   {1893, 11}, {1882, 11}, {1871, 11}, {1860, 11}, {1849, 11}, {1838, 11}, {1827, 11}, {1816, 10},
+   {1806, 11}, {1795, 10}, {1785, 10}, {1775, 10}, {1765, 10}, {1755, 10}, {1745, 9}, {1736, 10},
+   {1726, 9}, {1717, 10}, {1707, 9}, {1698, 9}, {1689, 9}, {1680, 9}, {1671, 9}, {1662, 9},
+   {1653, 9}, {1644, 8}, {1636, 9}, {1627, 8}, {1619, 9}, {1610, 8}, {1602, 8}, {1594, 8},
+   {1586, 8}, {1578, 8}, {1570, 8}, {1562, 8}, {1554, 7}, {1547, 8}, {1539, 8}, {1531, 7},
+   {1524, 8}, {1516, 7}, {1509, 7}, {1502, 7}, {1495, 7}, {1488, 7}, {1481, 7}, {1474, 7},
+   {1467, 7}, {1460, 7}, {1453, 7}, {1446, 6}, {1440, 7}, {1433, 7}, {1426, 6}, {1420, 6},
+   {1414, 7}, {1407, 6}, {1401, 6}, {1395, 7}, {1388, 6}, {1382, 6}, {1376, 6}, {1370, 6},
+   {1364, 6}, {1358, 6}, {1352, 6}, {1346, 5}, {1341, 6}, {1335, 6}, {1329, 5}, {1324, 6},
+   {1318, 5}, {1313, 6}, {1307, 5}, {1302, 6}, {1296, 5}, {1291, 5}, {1286, 6}, {1280, 5},
+   {1275, 5}, {1270, 5}, {1265, 5}, {1260, 5}, {1255, 5}, {1250, 5}, {1245, 5}, {1240, 5},
+   {1235, 5}, {1230, 5}, {1225, 5}, {1220, 5}, {1215, 4}, {1211, 5}, {1206, 5}, {1201, 5},
+};
+
+#endif

+ 112 - 139
Firmware/speed_lookuptable.h

@@ -3,150 +3,123 @@
 
 #include "Marlin.h"
 
-#if F_CPU == 16000000
+extern const uint16_t speed_lookuptable_fast[256][2] PROGMEM;
+extern const uint16_t speed_lookuptable_slow[256][2] PROGMEM;
 
-const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {\
-{ 62500, 55556}, { 6944, 3268}, { 3676, 1176}, { 2500, 607}, { 1893, 369}, { 1524, 249}, { 1275, 179}, { 1096, 135}, 
-{ 961, 105}, { 856, 85}, { 771, 69}, { 702, 58}, { 644, 49}, { 595, 42}, { 553, 37}, { 516, 32}, 
-{ 484, 28}, { 456, 25}, { 431, 23}, { 408, 20}, { 388, 19}, { 369, 16}, { 353, 16}, { 337, 14}, 
-{ 323, 13}, { 310, 11}, { 299, 11}, { 288, 11}, { 277, 9}, { 268, 9}, { 259, 8}, { 251, 8}, 
-{ 243, 8}, { 235, 7}, { 228, 6}, { 222, 6}, { 216, 6}, { 210, 6}, { 204, 5}, { 199, 5}, 
-{ 194, 5}, { 189, 4}, { 185, 4}, { 181, 4}, { 177, 4}, { 173, 4}, { 169, 4}, { 165, 3}, 
-{ 162, 3}, { 159, 4}, { 155, 3}, { 152, 3}, { 149, 2}, { 147, 3}, { 144, 3}, { 141, 2}, 
-{ 139, 3}, { 136, 2}, { 134, 2}, { 132, 3}, { 129, 2}, { 127, 2}, { 125, 2}, { 123, 2}, 
-{ 121, 2}, { 119, 1}, { 118, 2}, { 116, 2}, { 114, 1}, { 113, 2}, { 111, 2}, { 109, 1}, 
-{ 108, 2}, { 106, 1}, { 105, 2}, { 103, 1}, { 102, 1}, { 101, 1}, { 100, 2}, { 98, 1}, 
-{ 97, 1}, { 96, 1}, { 95, 2}, { 93, 1}, { 92, 1}, { 91, 1}, { 90, 1}, { 89, 1}, 
-{ 88, 1}, { 87, 1}, { 86, 1}, { 85, 1}, { 84, 1}, { 83, 0}, { 83, 1}, { 82, 1}, 
-{ 81, 1}, { 80, 1}, { 79, 1}, { 78, 0}, { 78, 1}, { 77, 1}, { 76, 1}, { 75, 0}, 
-{ 75, 1}, { 74, 1}, { 73, 1}, { 72, 0}, { 72, 1}, { 71, 1}, { 70, 0}, { 70, 1}, 
-{ 69, 0}, { 69, 1}, { 68, 1}, { 67, 0}, { 67, 1}, { 66, 0}, { 66, 1}, { 65, 0}, 
-{ 65, 1}, { 64, 1}, { 63, 0}, { 63, 1}, { 62, 0}, { 62, 1}, { 61, 0}, { 61, 1}, 
-{ 60, 0}, { 60, 0}, { 60, 1}, { 59, 0}, { 59, 1}, { 58, 0}, { 58, 1}, { 57, 0}, 
-{ 57, 1}, { 56, 0}, { 56, 0}, { 56, 1}, { 55, 0}, { 55, 1}, { 54, 0}, { 54, 0}, 
-{ 54, 1}, { 53, 0}, { 53, 0}, { 53, 1}, { 52, 0}, { 52, 0}, { 52, 1}, { 51, 0}, 
-{ 51, 0}, { 51, 1}, { 50, 0}, { 50, 0}, { 50, 1}, { 49, 0}, { 49, 0}, { 49, 1}, 
-{ 48, 0}, { 48, 0}, { 48, 1}, { 47, 0}, { 47, 0}, { 47, 0}, { 47, 1}, { 46, 0}, 
-{ 46, 0}, { 46, 1}, { 45, 0}, { 45, 0}, { 45, 0}, { 45, 1}, { 44, 0}, { 44, 0}, 
-{ 44, 0}, { 44, 1}, { 43, 0}, { 43, 0}, { 43, 0}, { 43, 1}, { 42, 0}, { 42, 0}, 
-{ 42, 0}, { 42, 1}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 0}, { 41, 1}, { 40, 0}, 
-{ 40, 0}, { 40, 0}, { 40, 0}, { 40, 1}, { 39, 0}, { 39, 0}, { 39, 0}, { 39, 0}, 
-{ 39, 1}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 0}, { 38, 1}, { 37, 0}, { 37, 0}, 
-{ 37, 0}, { 37, 0}, { 37, 0}, { 37, 1}, { 36, 0}, { 36, 0}, { 36, 0}, { 36, 0}, 
-{ 36, 1}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 0}, { 35, 1}, 
-{ 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 0}, { 34, 1}, { 33, 0}, { 33, 0}, 
-{ 33, 0}, { 33, 0}, { 33, 0}, { 33, 0}, { 33, 1}, { 32, 0}, { 32, 0}, { 32, 0}, 
-{ 32, 0}, { 32, 0}, { 32, 0}, { 32, 0}, { 32, 1}, { 31, 0}, { 31, 0}, { 31, 0}, 
-{ 31, 0}, { 31, 0}, { 31, 0}, { 31, 1}, { 30, 0}, { 30, 0}, { 30, 0}, { 30, 0}
-};
+#ifndef _NO_ASM
 
-const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {\
-{ 62500, 12500}, { 50000, 8334}, { 41666, 5952}, { 35714, 4464}, { 31250, 3473}, { 27777, 2777}, { 25000, 2273}, { 22727, 1894}, 
-{ 20833, 1603}, { 19230, 1373}, { 17857, 1191}, { 16666, 1041}, { 15625, 920}, { 14705, 817}, { 13888, 731}, { 13157, 657}, 
-{ 12500, 596}, { 11904, 541}, { 11363, 494}, { 10869, 453}, { 10416, 416}, { 10000, 385}, { 9615, 356}, { 9259, 331}, 
-{ 8928, 308}, { 8620, 287}, { 8333, 269}, { 8064, 252}, { 7812, 237}, { 7575, 223}, { 7352, 210}, { 7142, 198}, 
-{ 6944, 188}, { 6756, 178}, { 6578, 168}, { 6410, 160}, { 6250, 153}, { 6097, 145}, { 5952, 139}, { 5813, 132}, 
-{ 5681, 126}, { 5555, 121}, { 5434, 115}, { 5319, 111}, { 5208, 106}, { 5102, 102}, { 5000, 99}, { 4901, 94}, 
-{ 4807, 91}, { 4716, 87}, { 4629, 84}, { 4545, 81}, { 4464, 79}, { 4385, 75}, { 4310, 73}, { 4237, 71}, 
-{ 4166, 68}, { 4098, 66}, { 4032, 64}, { 3968, 62}, { 3906, 60}, { 3846, 59}, { 3787, 56}, { 3731, 55}, 
-{ 3676, 53}, { 3623, 52}, { 3571, 50}, { 3521, 49}, { 3472, 48}, { 3424, 46}, { 3378, 45}, { 3333, 44}, 
-{ 3289, 43}, { 3246, 41}, { 3205, 41}, { 3164, 39}, { 3125, 39}, { 3086, 38}, { 3048, 36}, { 3012, 36}, 
-{ 2976, 35}, { 2941, 35}, { 2906, 33}, { 2873, 33}, { 2840, 32}, { 2808, 31}, { 2777, 30}, { 2747, 30}, 
-{ 2717, 29}, { 2688, 29}, { 2659, 28}, { 2631, 27}, { 2604, 27}, { 2577, 26}, { 2551, 26}, { 2525, 25}, 
-{ 2500, 25}, { 2475, 25}, { 2450, 23}, { 2427, 24}, { 2403, 23}, { 2380, 22}, { 2358, 22}, { 2336, 22}, 
-{ 2314, 21}, { 2293, 21}, { 2272, 20}, { 2252, 20}, { 2232, 20}, { 2212, 20}, { 2192, 19}, { 2173, 18}, 
-{ 2155, 19}, { 2136, 18}, { 2118, 18}, { 2100, 17}, { 2083, 17}, { 2066, 17}, { 2049, 17}, { 2032, 16}, 
-{ 2016, 16}, { 2000, 16}, { 1984, 16}, { 1968, 15}, { 1953, 16}, { 1937, 14}, { 1923, 15}, { 1908, 15}, 
-{ 1893, 14}, { 1879, 14}, { 1865, 14}, { 1851, 13}, { 1838, 14}, { 1824, 13}, { 1811, 13}, { 1798, 13}, 
-{ 1785, 12}, { 1773, 13}, { 1760, 12}, { 1748, 12}, { 1736, 12}, { 1724, 12}, { 1712, 12}, { 1700, 11}, 
-{ 1689, 12}, { 1677, 11}, { 1666, 11}, { 1655, 11}, { 1644, 11}, { 1633, 10}, { 1623, 11}, { 1612, 10}, 
-{ 1602, 10}, { 1592, 10}, { 1582, 10}, { 1572, 10}, { 1562, 10}, { 1552, 9}, { 1543, 10}, { 1533, 9}, 
-{ 1524, 9}, { 1515, 9}, { 1506, 9}, { 1497, 9}, { 1488, 9}, { 1479, 9}, { 1470, 9}, { 1461, 8}, 
-{ 1453, 8}, { 1445, 9}, { 1436, 8}, { 1428, 8}, { 1420, 8}, { 1412, 8}, { 1404, 8}, { 1396, 8}, 
-{ 1388, 7}, { 1381, 8}, { 1373, 7}, { 1366, 8}, { 1358, 7}, { 1351, 7}, { 1344, 8}, { 1336, 7}, 
-{ 1329, 7}, { 1322, 7}, { 1315, 7}, { 1308, 6}, { 1302, 7}, { 1295, 7}, { 1288, 6}, { 1282, 7}, 
-{ 1275, 6}, { 1269, 7}, { 1262, 6}, { 1256, 6}, { 1250, 7}, { 1243, 6}, { 1237, 6}, { 1231, 6}, 
-{ 1225, 6}, { 1219, 6}, { 1213, 6}, { 1207, 6}, { 1201, 5}, { 1196, 6}, { 1190, 6}, { 1184, 5}, 
-{ 1179, 6}, { 1173, 5}, { 1168, 6}, { 1162, 5}, { 1157, 5}, { 1152, 6}, { 1146, 5}, { 1141, 5}, 
-{ 1136, 5}, { 1131, 5}, { 1126, 5}, { 1121, 5}, { 1116, 5}, { 1111, 5}, { 1106, 5}, { 1101, 5}, 
-{ 1096, 5}, { 1091, 5}, { 1086, 4}, { 1082, 5}, { 1077, 5}, { 1072, 4}, { 1068, 5}, { 1063, 4}, 
-{ 1059, 5}, { 1054, 4}, { 1050, 4}, { 1046, 5}, { 1041, 4}, { 1037, 4}, { 1033, 5}, { 1028, 4}, 
-{ 1024, 4}, { 1020, 4}, { 1016, 4}, { 1012, 4}, { 1008, 4}, { 1004, 4}, { 1000, 4}, { 996, 4}, 
-{ 992, 4}, { 988, 4}, { 984, 4}, { 980, 4}, { 976, 4}, { 972, 4}, { 968, 3}, { 965, 3}
-};
+// intRes = intIn1 * intIn2 >> 16
+// uses:
+// r26 to store 0
+// r27 to store the byte 1 of the 24 bit result
+#define MultiU16X8toH16(intRes, charIn1, intIn2) \
+asm volatile ( \
+"clr r26 \n\t" \
+"mul %A1, %B2 \n\t" \
+"movw %A0, r0 \n\t" \
+"mul %A1, %A2 \n\t" \
+"add %A0, r1 \n\t" \
+"adc %B0, r26 \n\t" \
+"lsr r0 \n\t" \
+"adc %A0, r26 \n\t" \
+"adc %B0, r26 \n\t" \
+"clr r1 \n\t" \
+: \
+"=&r" (intRes) \
+: \
+"d" (charIn1), \
+"d" (intIn2) \
+: \
+"r26" \
+)
 
-#elif F_CPU == 20000000
+// intRes = longIn1 * longIn2 >> 24
+// uses:
+// r26 to store 0
+// r27 to store the byte 1 of the 48bit result
+#define MultiU24X24toH16(intRes, longIn1, longIn2) \
+asm volatile ( \
+"clr r26 \n\t" \
+"mul %A1, %B2 \n\t" \
+"mov r27, r1 \n\t" \
+"mul %B1, %C2 \n\t" \
+"movw %A0, r0 \n\t" \
+"mul %C1, %C2 \n\t" \
+"add %B0, r0 \n\t" \
+"mul %C1, %B2 \n\t" \
+"add %A0, r0 \n\t" \
+"adc %B0, r1 \n\t" \
+"mul %A1, %C2 \n\t" \
+"add r27, r0 \n\t" \
+"adc %A0, r1 \n\t" \
+"adc %B0, r26 \n\t" \
+"mul %B1, %B2 \n\t" \
+"add r27, r0 \n\t" \
+"adc %A0, r1 \n\t" \
+"adc %B0, r26 \n\t" \
+"mul %C1, %A2 \n\t" \
+"add r27, r0 \n\t" \
+"adc %A0, r1 \n\t" \
+"adc %B0, r26 \n\t" \
+"mul %B1, %A2 \n\t" \
+"add r27, r1 \n\t" \
+"adc %A0, r26 \n\t" \
+"adc %B0, r26 \n\t" \
+"lsr r27 \n\t" \
+"adc %A0, r26 \n\t" \
+"adc %B0, r26 \n\t" \
+"clr r1 \n\t" \
+: \
+"=&r" (intRes) \
+: \
+"d" (longIn1), \
+"d" (longIn2) \
+: \
+"r26" , "r27" \
+)
 
-const uint16_t speed_lookuptable_fast[256][2] PROGMEM = {
-   {62500, 54055}, {8445, 3917}, {4528, 1434}, {3094, 745}, {2349, 456}, {1893, 307}, {1586, 222}, {1364, 167},
-   {1197, 131}, {1066, 105}, {961, 86}, {875, 72}, {803, 61}, {742, 53}, {689, 45}, {644, 40},
-   {604, 35}, {569, 32}, {537, 28}, {509, 25}, {484, 23}, {461, 21}, {440, 19}, {421, 17},
-   {404, 16}, {388, 15}, {373, 14}, {359, 13}, {346, 12}, {334, 11}, {323, 10}, {313, 10},
-   {303, 9}, {294, 9}, {285, 8}, {277, 7}, {270, 8}, {262, 7}, {255, 6}, {249, 6},
-   {243, 6}, {237, 6}, {231, 5}, {226, 5}, {221, 5}, {216, 5}, {211, 4}, {207, 5},
-   {202, 4}, {198, 4}, {194, 4}, {190, 3}, {187, 4}, {183, 3}, {180, 3}, {177, 4},
-   {173, 3}, {170, 3}, {167, 2}, {165, 3}, {162, 3}, {159, 2}, {157, 3}, {154, 2},
-   {152, 3}, {149, 2}, {147, 2}, {145, 2}, {143, 2}, {141, 2}, {139, 2}, {137, 2},
-   {135, 2}, {133, 2}, {131, 2}, {129, 1}, {128, 2}, {126, 2}, {124, 1}, {123, 2},
-   {121, 1}, {120, 2}, {118, 1}, {117, 1}, {116, 2}, {114, 1}, {113, 1}, {112, 2},
-   {110, 1}, {109, 1}, {108, 1}, {107, 2}, {105, 1}, {104, 1}, {103, 1}, {102, 1},
-   {101, 1}, {100, 1}, {99, 1}, {98, 1}, {97, 1}, {96, 1}, {95, 1}, {94, 1},
-   {93, 1}, {92, 1}, {91, 0}, {91, 1}, {90, 1}, {89, 1}, {88, 1}, {87, 0},
-   {87, 1}, {86, 1}, {85, 1}, {84, 0}, {84, 1}, {83, 1}, {82, 1}, {81, 0},
-   {81, 1}, {80, 1}, {79, 0}, {79, 1}, {78, 0}, {78, 1}, {77, 1}, {76, 0},
-   {76, 1}, {75, 0}, {75, 1}, {74, 1}, {73, 0}, {73, 1}, {72, 0}, {72, 1},
-   {71, 0}, {71, 1}, {70, 0}, {70, 1}, {69, 0}, {69, 1}, {68, 0}, {68, 1},
-   {67, 0}, {67, 1}, {66, 0}, {66, 1}, {65, 0}, {65, 0}, {65, 1}, {64, 0},
-   {64, 1}, {63, 0}, {63, 1}, {62, 0}, {62, 0}, {62, 1}, {61, 0}, {61, 1},
-   {60, 0}, {60, 0}, {60, 1}, {59, 0}, {59, 0}, {59, 1}, {58, 0}, {58, 0},
-   {58, 1}, {57, 0}, {57, 0}, {57, 1}, {56, 0}, {56, 0}, {56, 1}, {55, 0},
-   {55, 0}, {55, 1}, {54, 0}, {54, 0}, {54, 1}, {53, 0}, {53, 0}, {53, 0},
-   {53, 1}, {52, 0}, {52, 0}, {52, 1}, {51, 0}, {51, 0}, {51, 0}, {51, 1},
-   {50, 0}, {50, 0}, {50, 0}, {50, 1}, {49, 0}, {49, 0}, {49, 0}, {49, 1},
-   {48, 0}, {48, 0}, {48, 0}, {48, 1}, {47, 0}, {47, 0}, {47, 0}, {47, 1},
-   {46, 0}, {46, 0}, {46, 0}, {46, 0}, {46, 1}, {45, 0}, {45, 0}, {45, 0},
-   {45, 1}, {44, 0}, {44, 0}, {44, 0}, {44, 0}, {44, 1}, {43, 0}, {43, 0},
-   {43, 0}, {43, 0}, {43, 1}, {42, 0}, {42, 0}, {42, 0}, {42, 0}, {42, 0},
-   {42, 1}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 0}, {41, 1}, {40, 0},
-   {40, 0}, {40, 0}, {40, 0}, {40, 1}, {39, 0}, {39, 0}, {39, 0}, {39, 0},
-   {39, 0}, {39, 0}, {39, 1}, {38, 0}, {38, 0}, {38, 0}, {38, 0}, {38, 0},
-};
+#else //_NO_ASM
 
-const uint16_t speed_lookuptable_slow[256][2] PROGMEM = {
-   {62500, 10417}, {52083, 7441}, {44642, 5580}, {39062, 4340}, {34722, 3472}, {31250, 2841}, {28409, 2368}, {26041, 2003},
-   {24038, 1717}, {22321, 1488}, {20833, 1302}, {19531, 1149}, {18382, 1021}, {17361, 914}, {16447, 822}, {15625, 745},
-   {14880, 676}, {14204, 618}, {13586, 566}, {13020, 520}, {12500, 481}, {12019, 445}, {11574, 414}, {11160, 385},
-   {10775, 359}, {10416, 336}, {10080, 315}, {9765, 296}, {9469, 278}, {9191, 263}, {8928, 248}, {8680, 235},
-   {8445, 222}, {8223, 211}, {8012, 200}, {7812, 191}, {7621, 181}, {7440, 173}, {7267, 165}, {7102, 158},
-   {6944, 151}, {6793, 145}, {6648, 138}, {6510, 133}, {6377, 127}, {6250, 123}, {6127, 118}, {6009, 113},
-   {5896, 109}, {5787, 106}, {5681, 101}, {5580, 98}, {5482, 95}, {5387, 91}, {5296, 88}, {5208, 86},
-   {5122, 82}, {5040, 80}, {4960, 78}, {4882, 75}, {4807, 73}, {4734, 70}, {4664, 69}, {4595, 67},
-   {4528, 64}, {4464, 63}, {4401, 61}, {4340, 60}, {4280, 58}, {4222, 56}, {4166, 55}, {4111, 53},
-   {4058, 52}, {4006, 51}, {3955, 49}, {3906, 48}, {3858, 48}, {3810, 45}, {3765, 45}, {3720, 44},
-   {3676, 43}, {3633, 42}, {3591, 40}, {3551, 40}, {3511, 39}, {3472, 38}, {3434, 38}, {3396, 36},
-   {3360, 36}, {3324, 35}, {3289, 34}, {3255, 34}, {3221, 33}, {3188, 32}, {3156, 31}, {3125, 31},
-   {3094, 31}, {3063, 30}, {3033, 29}, {3004, 28}, {2976, 28}, {2948, 28}, {2920, 27}, {2893, 27},
-   {2866, 26}, {2840, 25}, {2815, 25}, {2790, 25}, {2765, 24}, {2741, 24}, {2717, 24}, {2693, 23},
-   {2670, 22}, {2648, 22}, {2626, 22}, {2604, 22}, {2582, 21}, {2561, 21}, {2540, 20}, {2520, 20},
-   {2500, 20}, {2480, 20}, {2460, 19}, {2441, 19}, {2422, 19}, {2403, 18}, {2385, 18}, {2367, 18},
-   {2349, 17}, {2332, 18}, {2314, 17}, {2297, 16}, {2281, 17}, {2264, 16}, {2248, 16}, {2232, 16},
-   {2216, 16}, {2200, 15}, {2185, 15}, {2170, 15}, {2155, 15}, {2140, 15}, {2125, 14}, {2111, 14},
-   {2097, 14}, {2083, 14}, {2069, 14}, {2055, 13}, {2042, 13}, {2029, 13}, {2016, 13}, {2003, 13},
-   {1990, 13}, {1977, 12}, {1965, 12}, {1953, 13}, {1940, 11}, {1929, 12}, {1917, 12}, {1905, 12},
-   {1893, 11}, {1882, 11}, {1871, 11}, {1860, 11}, {1849, 11}, {1838, 11}, {1827, 11}, {1816, 10},
-   {1806, 11}, {1795, 10}, {1785, 10}, {1775, 10}, {1765, 10}, {1755, 10}, {1745, 9}, {1736, 10},
-   {1726, 9}, {1717, 10}, {1707, 9}, {1698, 9}, {1689, 9}, {1680, 9}, {1671, 9}, {1662, 9},
-   {1653, 9}, {1644, 8}, {1636, 9}, {1627, 8}, {1619, 9}, {1610, 8}, {1602, 8}, {1594, 8},
-   {1586, 8}, {1578, 8}, {1570, 8}, {1562, 8}, {1554, 7}, {1547, 8}, {1539, 8}, {1531, 7},
-   {1524, 8}, {1516, 7}, {1509, 7}, {1502, 7}, {1495, 7}, {1488, 7}, {1481, 7}, {1474, 7},
-   {1467, 7}, {1460, 7}, {1453, 7}, {1446, 6}, {1440, 7}, {1433, 7}, {1426, 6}, {1420, 6},
-   {1414, 7}, {1407, 6}, {1401, 6}, {1395, 7}, {1388, 6}, {1382, 6}, {1376, 6}, {1370, 6},
-   {1364, 6}, {1358, 6}, {1352, 6}, {1346, 5}, {1341, 6}, {1335, 6}, {1329, 5}, {1324, 6},
-   {1318, 5}, {1313, 6}, {1307, 5}, {1302, 6}, {1296, 5}, {1291, 5}, {1286, 6}, {1280, 5},
-   {1275, 5}, {1270, 5}, {1265, 5}, {1260, 5}, {1255, 5}, {1250, 5}, {1245, 5}, {1240, 5},
-   {1235, 5}, {1230, 5}, {1225, 5}, {1220, 5}, {1215, 4}, {1211, 5}, {1206, 5}, {1201, 5},
-};
+// NOTE: currently not implemented
+void MultiU16X8toH16(unsigned short& intRes, unsigned char& charIn1, unsigned short& intIn2);
+void MultiU24X24toH16(uint16_t& intRes, int32_t& longIn1, long& longIn2);
 
-#endif
+#endif //_NO_ASM
+
+
+FORCE_INLINE unsigned short calc_timer(uint16_t step_rate, uint8_t& step_loops) {
+  unsigned short timer;
+  if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
+
+  if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
+    step_rate = (step_rate >> 2)&0x3fff;
+    step_loops = 4;
+  }
+  else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
+    step_rate = (step_rate >> 1)&0x7fff;
+    step_loops = 2;
+  }
+  else {
+    step_loops = 1;
+  }
+
+  if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
+  step_rate -= (F_CPU/500000); // Correct for minimal speed
+  if(step_rate >= (8*256)){ // higher step rate
+    unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
+    unsigned char tmp_step_rate = (step_rate & 0x00ff);
+    unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
+    MultiU16X8toH16(timer, tmp_step_rate, gain);
+    timer = (unsigned short)pgm_read_word_near(table_address) - timer;
+  }
+  else { // lower step rates
+    unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
+    table_address += ((step_rate)>>1) & 0xfffc;
+    timer = (unsigned short)pgm_read_word_near(table_address);
+    timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
+  }
+  if(timer < 100) { timer = 100; }//(20kHz this should never happen)////MSG_STEPPER_TOO_HIGH c=0 r=0
+  return timer;
+}
 
 #endif

+ 300 - 384
Firmware/stepper.cpp

@@ -36,9 +36,9 @@
 #include "tmc2130.h"
 #endif //TMC2130
 
-#ifdef FILAMENT_SENSOR
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
 #include "fsensor.h"
-int fsensor_counter = 0; //counter for e-steps
+int fsensor_counter; //counter for e-steps
 #endif //FILAMENT_SENSOR
 
 #include "mmu.h"
@@ -113,23 +113,30 @@ volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
 volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
 
 #ifdef LIN_ADVANCE
+  void advance_isr_scheduler();
+  void advance_isr();
 
-  static uint16_t nextMainISR = 0;
+  static const uint16_t ADV_NEVER      = 0xFFFF;
+  static const uint8_t  ADV_INIT       = 0b01;
+  static const uint8_t  ADV_DECELERATE = 0b10;
+
+  static uint16_t nextMainISR;
+  static uint16_t nextAdvanceISR;
+
+  static uint16_t main_Rate;
   static uint16_t eISR_Rate;
+  static uint16_t eISR_Err;
+
+  static uint16_t current_adv_steps;
+  static uint16_t final_adv_steps;
+  static uint16_t max_adv_steps;
+  static uint32_t LA_decelerate_after;
 
-  // Extrusion steps to be executed by the stepper.
-  // If set to non zero, the timer ISR routine will tick the Linear Advance extruder ticks first.
-  // If e_steps is zero, then the timer ISR routine will perform the usual DDA step.
-  static volatile int16_t e_steps = 0;
-  // How many extruder steps shall be ticked at a single ISR invocation?
-  static uint8_t          estep_loops;
-  // The current speed of the extruder, scaled by the linear advance constant, so it has the same measure
-  // as current_adv_steps.
-  static int              current_estep_rate;
-  // The current pretension of filament expressed in extruder micro steps.
-  static int              current_adv_steps;
-
-  #define _NEXT_ISR(T)    nextMainISR = T
+  static int8_t e_steps;
+  static uint8_t e_step_loops;
+  static int8_t LA_phase;
+
+  #define _NEXT_ISR(T)    main_Rate = nextMainISR = T
 #else
   #define _NEXT_ISR(T)    OCR1A = T
 #endif
@@ -143,92 +150,6 @@ extern uint16_t stepper_timer_overflow_last;
 //=============================functions         ============================
 //===========================================================================
 
-#ifndef _NO_ASM
-
-// intRes = intIn1 * intIn2 >> 16
-// uses:
-// r26 to store 0
-// r27 to store the byte 1 of the 24 bit result
-#define MultiU16X8toH16(intRes, charIn1, intIn2) \
-asm volatile ( \
-"clr r26 \n\t" \
-"mul %A1, %B2 \n\t" \
-"movw %A0, r0 \n\t" \
-"mul %A1, %A2 \n\t" \
-"add %A0, r1 \n\t" \
-"adc %B0, r26 \n\t" \
-"lsr r0 \n\t" \
-"adc %A0, r26 \n\t" \
-"adc %B0, r26 \n\t" \
-"clr r1 \n\t" \
-: \
-"=&r" (intRes) \
-: \
-"d" (charIn1), \
-"d" (intIn2) \
-: \
-"r26" \
-)
-
-// intRes = longIn1 * longIn2 >> 24
-// uses:
-// r26 to store 0
-// r27 to store the byte 1 of the 48bit result
-#define MultiU24X24toH16(intRes, longIn1, longIn2) \
-asm volatile ( \
-"clr r26 \n\t" \
-"mul %A1, %B2 \n\t" \
-"mov r27, r1 \n\t" \
-"mul %B1, %C2 \n\t" \
-"movw %A0, r0 \n\t" \
-"mul %C1, %C2 \n\t" \
-"add %B0, r0 \n\t" \
-"mul %C1, %B2 \n\t" \
-"add %A0, r0 \n\t" \
-"adc %B0, r1 \n\t" \
-"mul %A1, %C2 \n\t" \
-"add r27, r0 \n\t" \
-"adc %A0, r1 \n\t" \
-"adc %B0, r26 \n\t" \
-"mul %B1, %B2 \n\t" \
-"add r27, r0 \n\t" \
-"adc %A0, r1 \n\t" \
-"adc %B0, r26 \n\t" \
-"mul %C1, %A2 \n\t" \
-"add r27, r0 \n\t" \
-"adc %A0, r1 \n\t" \
-"adc %B0, r26 \n\t" \
-"mul %B1, %A2 \n\t" \
-"add r27, r1 \n\t" \
-"adc %A0, r26 \n\t" \
-"adc %B0, r26 \n\t" \
-"lsr r27 \n\t" \
-"adc %A0, r26 \n\t" \
-"adc %B0, r26 \n\t" \
-"clr r1 \n\t" \
-: \
-"=&r" (intRes) \
-: \
-"d" (longIn1), \
-"d" (longIn2) \
-: \
-"r26" , "r27" \
-)
-
-#else //_NO_ASM
-
-void MultiU16X8toH16(unsigned short& intRes, unsigned char& charIn1, unsigned short& intIn2)
-{
-}
-
-void MultiU24X24toH16(uint16_t& intRes, int32_t& longIn1, long& longIn2)
-{
-}
-
-#endif //_NO_ASM
-
-// Some useful constants
-
 void checkHitEndstops()
 {
  if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
@@ -316,42 +237,6 @@ void invert_z_endstop(bool endstop_invert)
 //  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
 //  The slope of acceleration is calculated with the leib ramp alghorithm.
 
-FORCE_INLINE unsigned short calc_timer(uint16_t step_rate) {
-  unsigned short timer;
-  if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
-
-  if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
-    step_rate = (step_rate >> 2)&0x3fff;
-    step_loops = 4;
-  }
-  else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
-    step_rate = (step_rate >> 1)&0x7fff;
-    step_loops = 2;
-  }
-  else {
-    step_loops = 1;
-  }
-//    step_loops = 1;
-
-  if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
-  step_rate -= (F_CPU/500000); // Correct for minimal speed
-  if(step_rate >= (8*256)){ // higher step rate
-    unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
-    unsigned char tmp_step_rate = (step_rate & 0x00ff);
-    unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
-    MultiU16X8toH16(timer, tmp_step_rate, gain);
-    timer = (unsigned short)pgm_read_word_near(table_address) - timer;
-  }
-  else { // lower step rates
-    unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
-    table_address += ((step_rate)>>1) & 0xfffc;
-    timer = (unsigned short)pgm_read_word_near(table_address);
-    timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
-  }
-  if(timer < 100) { timer = 100; MYSERIAL.print(_N("Steprate too high: ")); MYSERIAL.println(step_rate); }//(20kHz this should never happen)////MSG_STEPPER_TOO_HIGH
-  return timer;
-}
-
 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
 // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
 ISR(TIMER1_COMPA_vect) {
@@ -361,53 +246,10 @@ ISR(TIMER1_COMPA_vect) {
 #endif //DEBUG_STACK_MONITOR
 
 #ifdef LIN_ADVANCE
-  // If there are any e_steps planned, tick them.
-  bool run_main_isr = false;
-  if (e_steps) {
-    //WRITE_NC(LOGIC_ANALYZER_CH7, true);
-	uint8_t cnt = 0;
-    for (uint8_t i = estep_loops; e_steps && i --;) {
-        WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
-        -- e_steps;
-		cnt++;
-        WRITE_NC(E0_STEP_PIN, INVERT_E_STEP_PIN);
-    }
-#ifdef FILAMENT_SENSOR
-		if (READ(E0_DIR_PIN) == INVERT_E0_DIR)
-		{
-			if (count_direction[E_AXIS] == 1)
-				fsensor_counter -= cnt;
-			else
-				fsensor_counter += cnt;
-		}
-		else
-		{
-			if (count_direction[E_AXIS] == 1)
-				fsensor_counter += cnt;
-			else
-				fsensor_counter -= cnt;
-		}
-#endif //FILAMENT_SENSOR
-    if (e_steps) {
-      // Plan another Linear Advance tick.
-      OCR1A = eISR_Rate;
-      nextMainISR -= eISR_Rate;
-    } else if (! (nextMainISR & 0x8000) || nextMainISR < 16) {
-      // The timer did not overflow and it is big enough, so it makes sense to plan it.
-      OCR1A = nextMainISR;
-    } else {
-      // The timer has overflown, or it is too small. Run the main ISR just after the Linear Advance routine
-      // in the current interrupt tick.
-      run_main_isr = true;
-      //FIXME pick the serial line.
-    }
-    //WRITE_NC(LOGIC_ANALYZER_CH7, false);
-  } else
-    run_main_isr = true;
-
-  if (run_main_isr)
-#endif
+    advance_isr_scheduler();
+#else
     isr();
+#endif
 
   // Don't run the ISR faster than possible
   // Is there a 8us time left before the next interrupt triggers?
@@ -493,10 +335,6 @@ FORCE_INLINE void stepper_next_block()
 	}
 #endif
 
-#ifdef FILAMENT_SENSOR
-	fsensor_counter = 0;
-	fsensor_st_block_begin(current_block);
-#endif //FILAMENT_SENSOR
     // The busy flag is set by the plan_get_current_block() call.
     // current_block->busy = true;
     // Initializes the trapezoid generator from the current block. Called whenever a new
@@ -507,10 +345,26 @@ FORCE_INLINE void stepper_next_block()
     // state is reached.
     step_loops_nominal = 0;
     acc_step_rate = uint16_t(current_block->initial_rate);
-    acceleration_time = calc_timer(acc_step_rate);
+    acceleration_time = calc_timer(acc_step_rate, step_loops);
+
 #ifdef LIN_ADVANCE
-    current_estep_rate = ((unsigned long)acc_step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
-#endif /* LIN_ADVANCE */
+    if (current_block->use_advance_lead) {
+        LA_decelerate_after = current_block->decelerate_after;
+        final_adv_steps = current_block->final_adv_steps;
+        max_adv_steps = current_block->max_adv_steps;
+        e_step_loops = current_block->advance_step_loops;
+    } else {
+        e_steps = 0;
+        e_step_loops = 1;
+        current_adv_steps = 0;
+    }
+    nextAdvanceISR = ADV_NEVER;
+    LA_phase = -1;
+#endif
+
+    if (current_block->flag & BLOCK_FLAG_E_RESET) {
+        count_position[E_AXIS] = 0;
+    }
 
     if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) {
       counter_x.lo = -(current_block->step_event_count.lo >> 1);
@@ -567,9 +421,23 @@ FORCE_INLINE void stepper_next_block()
 #endif /* LIN_ADVANCE */
       count_direction[E_AXIS] = 1;
     }
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+    fsensor_st_block_begin(count_direction[E_AXIS] < 0);
+#endif //FILAMENT_SENSOR
   }
   else {
-    OCR1A = 2000; // 1kHz.
+      _NEXT_ISR(2000); // 1kHz.
+
+#ifdef LIN_ADVANCE
+      // reset LA state when there's no block
+      nextAdvanceISR = ADV_NEVER;
+      e_steps = 0;
+
+      // incrementally lose pressure to give a chance for
+      // a new LA block to be scheduled and recover
+      if(current_adv_steps)
+          --current_adv_steps;
+#endif
   }
   //WRITE_NC(LOGIC_ANALYZER_CH2, false);
 }
@@ -779,10 +647,10 @@ FORCE_INLINE void stepper_tick_lowres()
       counter_e.lo -= current_block->step_event_count.lo;
       count_position[E_AXIS] += count_direction[E_AXIS];
 #ifdef LIN_ADVANCE
-      ++ e_steps;
+      e_steps += count_direction[E_AXIS];
 #else
 	#ifdef FILAMENT_SENSOR
-	  ++ fsensor_counter;
+	  fsensor_counter += count_direction[E_AXIS];
 	#endif //FILAMENT_SENSOR
       WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
 #endif
@@ -841,11 +709,11 @@ FORCE_INLINE void stepper_tick_highres()
       counter_e.wide -= current_block->step_event_count.wide;
       count_position[E_AXIS]+=count_direction[E_AXIS];
 #ifdef LIN_ADVANCE
-      ++ e_steps;
+      e_steps += count_direction[E_AXIS];
 #else
-  #ifdef FILAMENT_SENSOR
-      ++ fsensor_counter;
-  #endif //FILAMENT_SENSOR
+    #ifdef FILAMENT_SENSOR
+      fsensor_counter += count_direction[E_AXIS];
+    #endif //FILAMENT_SENSOR
       WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
 #endif
     }
@@ -854,8 +722,53 @@ FORCE_INLINE void stepper_tick_highres()
   }
 }
 
-// 50us delay
-#define LIN_ADV_FIRST_TICK_DELAY 100
+
+#ifdef LIN_ADVANCE
+// @wavexx: fast uint16_t division for small dividends<5
+//          q/3 based on "Hacker's delight" formula
+FORCE_INLINE uint16_t fastdiv(uint16_t q, uint8_t d)
+{
+    if(d != 3) return q >> (d / 2);
+    else return ((uint32_t)0xAAAB * q) >> 17;
+}
+
+FORCE_INLINE void advance_spread(uint16_t timer)
+{
+    if(eISR_Err > timer)
+    {
+        // advance-step skipped
+        eISR_Err -= timer;
+        eISR_Rate = timer;
+        nextAdvanceISR = timer;
+        return;
+    }
+
+    // at least one step
+    uint8_t ticks = 1;
+    uint32_t block = current_block->advance_rate;
+    uint16_t max_t = timer - eISR_Err;
+    while (block < max_t)
+    {
+        ++ticks;
+        block += current_block->advance_rate;
+    }
+    if (block > timer)
+        eISR_Err += block - timer;
+    else
+        eISR_Err -= timer - block;
+
+    if (ticks <= 4)
+        eISR_Rate = fastdiv(timer, ticks);
+    else
+    {
+        // >4 ticks are still possible on slow moves
+        eISR_Rate = timer / ticks;
+    }
+
+    nextAdvanceISR = eISR_Rate / 2;
+}
+#endif
+
 
 FORCE_INLINE void isr() {
   //WRITE_NC(LOGIC_ANALYZER_CH0, true);
@@ -868,81 +781,18 @@ FORCE_INLINE void isr() {
   if (current_block != NULL) 
   {
     stepper_check_endstops();
-#ifdef LIN_ADVANCE
-      e_steps = 0;
-#endif /* LIN_ADVANCE */
     if (current_block->flag & BLOCK_FLAG_DDA_LOWRES)
       stepper_tick_lowres();
     else
       stepper_tick_highres();
 
+
 #ifdef LIN_ADVANCE
-      if (out_bits&(1<<E_AXIS))
-        // Move in negative direction.
-        e_steps = - e_steps;
-      if (current_block->use_advance_lead) {
-        //int esteps_inc = 0;
-        //esteps_inc = current_estep_rate - current_adv_steps;
-        //e_steps += esteps_inc;
-        e_steps += current_estep_rate - current_adv_steps;
-#if 0
-        if (abs(esteps_inc) > 4) {
-          LOGIC_ANALYZER_SERIAL_TX_WRITE(esteps_inc);
-          if (esteps_inc < -511 || esteps_inc > 511)
-            LOGIC_ANALYZER_SERIAL_TX_WRITE(esteps_inc >> 9);
-        }
-#endif
-        current_adv_steps = current_estep_rate;
-      }
-      // If we have esteps to execute, step some of them now.
-      if (e_steps) {
-        //WRITE_NC(LOGIC_ANALYZER_CH7, true);
-        // Set the step direction.
-		bool neg = e_steps < 0;
-        {
-          bool dir =
-        #ifdef SNMM
-            (neg == (mmu_extruder & 1))
-        #else
-            neg
-        #endif
-            ? INVERT_E0_DIR : !INVERT_E0_DIR; //If we have SNMM, reverse every second extruder.
-          WRITE_NC(E0_DIR_PIN, dir);
-          if (neg)
-            // Flip the e_steps counter to be always positive.
-            e_steps = - e_steps;
-        }
-        // Tick min(step_loops, abs(e_steps)).
-        estep_loops = (e_steps & 0x0ff00) ? 4 : e_steps;
-        if (step_loops < estep_loops)
-          estep_loops = step_loops;
-#ifdef FILAMENT_SENSOR
-		if (READ(E0_DIR_PIN) == INVERT_E0_DIR)
-		{
-			if (count_direction[E_AXIS] == 1)
-				fsensor_counter -= estep_loops;
-			else
-				fsensor_counter += estep_loops;
-		}
-		else
-		{
-			if (count_direction[E_AXIS] == 1)
-				fsensor_counter += estep_loops;
-			else
-				fsensor_counter -= estep_loops;
-		}
-#endif //FILAMENT_SENSOR
-        do {
-          WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
-          -- e_steps;
-          WRITE_NC(E0_STEP_PIN, INVERT_E_STEP_PIN);
-        } while (-- estep_loops != 0);
-        //WRITE_NC(LOGIC_ANALYZER_CH7, false);
-        MSerial.checkRx(); // Check for serial chars.
-      }
+    if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
+    uint8_t la_state = 0;
 #endif
 
-    // Calculare new timer value
+    // Calculate new timer value
     // 13.38-14.63us for steady state,
     // 25.12us for acceleration / deceleration.
     {
@@ -955,14 +805,15 @@ FORCE_INLINE void isr() {
         if(acc_step_rate > uint16_t(current_block->nominal_rate))
           acc_step_rate = current_block->nominal_rate;
         // step_rate to timer interval
-        uint16_t timer = calc_timer(acc_step_rate);
+        uint16_t timer = calc_timer(acc_step_rate, step_loops);
         _NEXT_ISR(timer);
         acceleration_time += timer;
-  #ifdef LIN_ADVANCE
-        if (current_block->use_advance_lead)
-          // int32_t = (uint16_t * uint32_t) >> 17
-          current_estep_rate = ((uint32_t)acc_step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
-  #endif
+#ifdef LIN_ADVANCE
+        if (current_block->use_advance_lead) {
+            if (step_events_completed.wide <= (unsigned long int)step_loops)
+                la_state = ADV_INIT;
+        }
+#endif
       }
       else if (step_events_completed.wide > (unsigned long int)current_block->decelerate_after) {
         uint16_t step_rate;
@@ -973,24 +824,23 @@ FORCE_INLINE void isr() {
           step_rate = uint16_t(current_block->final_rate);
         }
         // Step_rate to timer interval.
-        uint16_t timer = calc_timer(step_rate);
+        uint16_t timer = calc_timer(step_rate, step_loops);
         _NEXT_ISR(timer);
         deceleration_time += timer;
-  #ifdef LIN_ADVANCE
-        if (current_block->use_advance_lead)
-          current_estep_rate = ((uint32_t)step_rate * current_block->abs_adv_steps_multiplier8) >> 17;
-  #endif
+#ifdef LIN_ADVANCE
+        if (current_block->use_advance_lead) {
+            la_state = ADV_DECELERATE;
+            if (step_events_completed.wide <= (unsigned long int)current_block->decelerate_after + step_loops)
+                la_state |= ADV_INIT;
+        }
+#endif
       }
       else {
         if (! step_loops_nominal) {
           // Calculation of the steady state timer rate has been delayed to the 1st tick of the steady state to lower
           // the initial interrupt blocking.
-          OCR1A_nominal = calc_timer(uint16_t(current_block->nominal_rate));
+          OCR1A_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
           step_loops_nominal = step_loops;
-  #ifdef LIN_ADVANCE
-          if (current_block->use_advance_lead)
-            current_estep_rate = (current_block->nominal_rate * current_block->abs_adv_steps_multiplier8) >> 17;
-  #endif
         }
         _NEXT_ISR(OCR1A_nominal);
       }
@@ -998,110 +848,40 @@ FORCE_INLINE void isr() {
     }
 
 #ifdef LIN_ADVANCE
-    if (e_steps && current_block->use_advance_lead) {
-      //WRITE_NC(LOGIC_ANALYZER_CH7, true);
-      MSerial.checkRx(); // Check for serial chars.
-      // Some of the E steps were not ticked yet. Plan additional interrupts.
-      uint16_t now = TCNT1;
-      // Plan the first linear advance interrupt after 50us from now.
-      uint16_t to_go = nextMainISR - now - LIN_ADV_FIRST_TICK_DELAY;
-      eISR_Rate = 0;
-      if ((to_go & 0x8000) == 0) {
-        // The to_go number is not negative.
-        // Count the number of 7812,5 ticks, that fit into to_go 2MHz ticks.
-        uint8_t ticks = to_go >> 8;
-        if (ticks == 1) {
-          // Avoid running the following loop for a very short interval.
-          estep_loops = 255;
-          eISR_Rate = 1;
-        } else if ((e_steps & 0x0ff00) == 0) {
-          // e_steps <= 0x0ff
-          if (uint8_t(e_steps) <= ticks) {
-            // Spread the e_steps along the whole go_to interval.
-            eISR_Rate = to_go / uint8_t(e_steps);
-            estep_loops = 1;
-          } else if (ticks != 0) {
-            // At least one tick fits into the to_go interval. Calculate the e-step grouping.
-            uint8_t e = uint8_t(e_steps) >> 1;
-            estep_loops = 2;
-            while (e > ticks) {
-              e >>= 1;
-              estep_loops <<= 1;
+    // avoid multiple instances or function calls to advance_spread
+    if (la_state & ADV_INIT) eISR_Err = current_block->advance_rate / 4;
+    if (la_state & ADV_INIT || nextAdvanceISR != ADV_NEVER) {
+        advance_spread(main_Rate);
+        if (la_state & ADV_DECELERATE) {
+            if (step_loops == e_step_loops)
+                LA_phase = (eISR_Rate > main_Rate);
+            else {
+                // avoid overflow through division. warning: we need to _guarantee_ step_loops
+                // and e_step_loops are <= 4 due to fastdiv's limit
+                LA_phase = (fastdiv(eISR_Rate, step_loops) > fastdiv(main_Rate, e_step_loops));
             }
-            // Now the estep_loops contains the number of loops of power of 2, that will be sufficient
-            // to squeeze enough of Linear Advance ticks until nextMainISR.
-            // Calculate the tick rate.
-            eISR_Rate = to_go / ticks;          
-          }
-        } else {
-          // This is an exterme case with too many e_steps inserted by the linear advance.
-          // At least one tick fits into the to_go interval. Calculate the e-step grouping.
-          estep_loops = 2;
-          uint16_t e = e_steps >> 1;
-          while (e & 0x0ff00) {
-            e >>= 1;
-            estep_loops <<= 1;
-          }
-          while (uint8_t(e) > ticks) {
-            e >>= 1;
-            estep_loops <<= 1;
-          }
-          // Now the estep_loops contains the number of loops of power of 2, that will be sufficient
-          // to squeeze enough of Linear Advance ticks until nextMainISR.
-          // Calculate the tick rate.
-          eISR_Rate = to_go / ticks;
         }
-      }
-      if (eISR_Rate == 0) {
-        // There is not enough time to fit even a single additional tick.
-        // Tick all the extruder ticks now.
-        MSerial.checkRx(); // Check for serial chars.
-#ifdef FILAMENT_SENSOR
-		if (READ(E0_DIR_PIN) == INVERT_E0_DIR)
-		{
-			if (count_direction[E_AXIS] == 1)
-				fsensor_counter -= e_steps;
-			else
-				fsensor_counter += e_steps;
-		}
-		else
-		{
-			if (count_direction[E_AXIS] == 1)
-				fsensor_counter += e_steps;
-			else
-				fsensor_counter -= e_steps;
-		}
-#endif //FILAMENT_SENSOR
-        do {
-          WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
-          -- e_steps;
-          WRITE_NC(E0_STEP_PIN, INVERT_E_STEP_PIN);
-        } while (e_steps);
-        OCR1A = nextMainISR;
-      } else {
-        // Tick the 1st Linear Advance interrupt after 50us from now.
-        nextMainISR -= LIN_ADV_FIRST_TICK_DELAY;
-        OCR1A = now + LIN_ADV_FIRST_TICK_DELAY;
-      }
-      //WRITE_NC(LOGIC_ANALYZER_CH7, false);
-    } else
-      OCR1A = nextMainISR;
+    }
+
+    // Check for serial chars. This executes roughtly inbetween 50-60% of the total runtime of the
+    // entire isr, making this spot a much better choice than checking during esteps
+    MSerial.checkRx();
 #endif
 
     // If current block is finished, reset pointer
     if (step_events_completed.wide >= current_block->step_event_count.wide) {
-#ifdef FILAMENT_SENSOR
-		fsensor_st_block_chunk(current_block, fsensor_counter);
+#if !defined(LIN_ADVANCE) && defined(FILAMENT_SENSOR)
+		fsensor_st_block_chunk(fsensor_counter);
 		fsensor_counter = 0;
 #endif //FILAMENT_SENSOR
 
       current_block = NULL;
       plan_discard_current_block();
     }
-#ifdef FILAMENT_SENSOR
-  	else if ((fsensor_counter >= fsensor_chunk_len))
+#if !defined(LIN_ADVANCE) && defined(FILAMENT_SENSOR)
+	else if ((abs(fsensor_counter) >= fsensor_chunk_len))
   	{
-      fsensor_st_block_chunk(current_block, fsensor_counter);
+      fsensor_st_block_chunk(fsensor_counter);
   	  fsensor_counter = 0;
   	}
 #endif //FILAMENT_SENSOR
@@ -1115,12 +895,105 @@ FORCE_INLINE void isr() {
 }
 
 #ifdef LIN_ADVANCE
-
-void clear_current_adv_vars() {
-  e_steps = 0; //Should be already 0 at an filament change event, but just to be sure..
-  current_adv_steps = 0;
+// Timer interrupt for E. e_steps is set in the main routine.
+
+FORCE_INLINE void advance_isr() {
+    if (step_events_completed.wide > LA_decelerate_after && current_adv_steps > final_adv_steps) {
+        // decompression
+        e_steps -= e_step_loops;
+        if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
+        if(current_adv_steps > e_step_loops)
+            current_adv_steps -= e_step_loops;
+        else
+            current_adv_steps = 0;
+        nextAdvanceISR = eISR_Rate;
+    }
+    else if (step_events_completed.wide < LA_decelerate_after && current_adv_steps < max_adv_steps) {
+        // compression
+        e_steps += e_step_loops;
+        if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
+        current_adv_steps += e_step_loops;
+        nextAdvanceISR = eISR_Rate;
+    }
+    else {
+        // advance steps completed
+        nextAdvanceISR = ADV_NEVER;
+        LA_phase = -1;
+        e_step_loops = 1;
+    }
 }
 
+FORCE_INLINE void advance_isr_scheduler() {
+    // Integrate the final timer value, accounting for scheduling adjustments
+    if(nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
+    {
+        if(nextAdvanceISR > OCR1A)
+            nextAdvanceISR -= OCR1A;
+        else
+            nextAdvanceISR = 0;
+    }
+    if(nextMainISR > OCR1A)
+        nextMainISR -= OCR1A;
+    else
+        nextMainISR = 0;
+
+    // Run main stepping ISR if flagged
+    if (!nextMainISR)
+    {
+#ifdef LA_DEBUG_LOGIC
+        WRITE_NC(LOGIC_ANALYZER_CH0, true);
+#endif
+        isr();
+#ifdef LA_DEBUG_LOGIC
+        WRITE_NC(LOGIC_ANALYZER_CH0, false);
+#endif
+    }
+
+    // Run the next advance isr if triggered
+    bool eisr = !nextAdvanceISR;
+    if (eisr)
+    {
+#ifdef LA_DEBUG_LOGIC
+        WRITE_NC(LOGIC_ANALYZER_CH1, true);
+#endif
+        advance_isr();
+#ifdef LA_DEBUG_LOGIC
+        WRITE_NC(LOGIC_ANALYZER_CH1, false);
+#endif
+    }
+
+    // Tick E steps if any
+    if (e_steps && (LA_phase < 0 || LA_phase == eisr)) {
+        uint8_t max_ticks = (eisr? e_step_loops: step_loops);
+        max_ticks = min(abs(e_steps), max_ticks);
+        bool rev = (e_steps < 0);
+        do
+        {
+            WRITE_NC(E0_STEP_PIN, !INVERT_E_STEP_PIN);
+            e_steps += (rev? 1: -1);
+            WRITE_NC(E0_STEP_PIN, INVERT_E_STEP_PIN);
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+            fsensor_counter += (rev? -1: 1);
+#endif
+        }
+        while(--max_ticks);
+
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+        if (abs(fsensor_counter) >= fsensor_chunk_len)
+        {
+            fsensor_st_block_chunk(fsensor_counter);
+            fsensor_counter = 0;
+        }
+#endif
+    }
+
+    // Schedule the next closest tick, ignoring advance if scheduled too
+    // soon in order to avoid skewing the regular stepper acceleration
+    if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + TCNT1 + 40) < nextMainISR)
+        OCR1A = nextAdvanceISR;
+    else
+        OCR1A = nextMainISR;
+}
 #endif // LIN_ADVANCE
 
 void st_init()
@@ -1347,18 +1220,49 @@ void st_init()
   // Plan the first interrupt after 8ms from now.
   OCR1A = 0x4000;
   TCNT1 = 0;
-  ENABLE_STEPPER_DRIVER_INTERRUPT();
 
 #ifdef LIN_ADVANCE
-    e_steps = 0;
-    current_adv_steps = 0;
+#ifdef LA_DEBUG_LOGIC
+  LOGIC_ANALYZER_CH0_ENABLE;
+  LOGIC_ANALYZER_CH1_ENABLE;
+  WRITE_NC(LOGIC_ANALYZER_CH0, false);
+  WRITE_NC(LOGIC_ANALYZER_CH1, false);
 #endif
-    
+
+  // Initialize state for the linear advance scheduler
+  nextMainISR = 0;
+  nextAdvanceISR = ADV_NEVER;
+  main_Rate = ADV_NEVER;
+  e_steps = 0;
+  e_step_loops = 1;
+  LA_phase = -1;
+  current_adv_steps = 0;
+#endif
+
   enable_endstops(true); // Start with endstops active. After homing they can be disabled
+
+  ENABLE_STEPPER_DRIVER_INTERRUPT();
   sei();
 }
 
 
+void st_reset_timer()
+{
+  // Clear a possible pending interrupt on OCR1A overflow.
+  TIFR1 |= 1 << OCF1A;
+  // Reset the counter.
+  TCNT1 = 0;
+  // Wake up after 1ms from now.
+  OCR1A = 2000;
+
+#ifdef LIN_ADVANCE
+  nextMainISR = 0;
+  if(nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
+      nextAdvanceISR = 0;
+#endif
+}
+
+
 // Block until all buffered steps are executed
 void st_synchronize()
 {
@@ -1443,13 +1347,15 @@ void quickStop()
   DISABLE_STEPPER_DRIVER_INTERRUPT();
   while (blocks_queued()) plan_discard_current_block(); 
   current_block = NULL;
+#ifdef LIN_ADVANCE
+  nextAdvanceISR = ADV_NEVER;
+  current_adv_steps = 0;
+#endif
   st_reset_timer();
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
 #ifdef BABYSTEPPING
-
-
 void babystep(const uint8_t axis,const bool direction)
 {
   //MUST ONLY BE CALLED BY A ISR, it depends on that no other ISR interrupts this
@@ -1685,3 +1591,13 @@ void microstep_readings()
       #endif
 }
 #endif //TMC2130
+
+
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+void st_reset_fsensor()
+{
+    CRITICAL_SECTION_START;
+    fsensor_counter = 0;
+    CRITICAL_SECTION_END;
+}
+#endif //FILAMENT_SENSOR

+ 5 - 16
Firmware/stepper.h

@@ -37,12 +37,6 @@ void st_init();
 
 void isr();
 
-#ifdef LIN_ADVANCE
-  void advance_isr();
-  void advance_isr_scheduler();
-  void clear_current_adv_vars(); //Used to reset the built up pretension and remaining esteps on filament change.
-#endif
-
 // Block until all buffered steps are executed
 void st_synchronize();
 
@@ -62,15 +56,7 @@ float st_get_position_mm(uint8_t axis);
 
 // Call this function just before re-enabling the stepper driver interrupt and the global interrupts
 // to avoid a stepper timer overflow.
-FORCE_INLINE void st_reset_timer()
-{
-  // Clear a possible pending interrupt on OCR1A overflow.
-  TIFR1 |= 1 << OCF1A;
-  // Reset the counter.
-  TCNT1 = 0;
-  // Wake up after 1ms from now.
-  OCR1A = 2000;
-}
+void st_reset_timer();
 
 void checkHitEndstops(); //call from somewhere to create an serial error message with the locations the endstops where hit, in case they were triggered
 bool endstops_hit_on_purpose(); //avoid creation of the message, i.e. after homing and before a routine call of checkHitEndstops();
@@ -106,7 +92,10 @@ void microstep_readings();
 #ifdef BABYSTEPPING
   void babystep(const uint8_t axis,const bool direction); // perform a short step with a single stepper motor, outside of any convention
 #endif
-     
 
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
+// reset the internal filament sensor state
+void st_reset_fsensor();
+#endif
 
 #endif

+ 4 - 4
Firmware/system_timer.h

@@ -11,8 +11,8 @@
 #define _millis millis2
 #define _micros micros2
 #define _delay delay2
-#define _tone tone2
-#define _noTone noTone2
+#define _tone tone
+#define _noTone noTone
 
 #define timer02_set_pwm0(pwm0)
 
@@ -20,8 +20,8 @@
 #define _millis millis
 #define _micros micros
 #define _delay delay
-#define _tone(x, y) /*tone*/
-#define _noTone(x) /*noTone*/
+#define _tone tone
+#define _noTone noTone
 #define timer02_set_pwm0(pwm0)
 #endif //SYSTEM_TIMER_2
 

+ 6 - 3
Firmware/temperature.cpp

@@ -1403,6 +1403,7 @@ void disable_heater()
     target_temperature_bed=0;
     soft_pwm_bed=0;
 	timer02_set_pwm0(soft_pwm_bed << 1);
+	bedPWMDisabled = 0;
     #if defined(HEATER_BED_PIN) && HEATER_BED_PIN > -1
       //WRITE(HEATER_BED_PIN,LOW);
     #endif
@@ -2002,6 +2003,8 @@ void check_max_temp()
 //! number of repeating the same state with consecutive step() calls
 //! used to slow down text switching
 struct alert_automaton_mintemp {
+	const char *m2;
+	alert_automaton_mintemp(const char *m2):m2(m2){}
 private:
 	enum { ALERT_AUTOMATON_SPEED_DIV = 5 };
 	enum class States : uint8_t { Init = 0, TempAboveMintemp, ShowPleaseRestart, ShowMintemp };
@@ -2021,7 +2024,6 @@ public:
 	//! @param current_temp current hotend/bed temperature (for computing simple hysteresis)
 	//! @param mintemp minimal temperature including hysteresis to check current_temp against
 	void step(float current_temp, float mintemp){
-		static const char m2[] PROGMEM = "MINTEMP fixed";
 		static const char m1[] PROGMEM = "Please restart";
 		switch(state){
 		case States::Init: // initial state - check hysteresis
@@ -2049,8 +2051,9 @@ public:
 		}
 	}
 };
-
-static alert_automaton_mintemp alert_automaton_hotend, alert_automaton_bed;
+static const char m2hotend[] PROGMEM = "MINTEMP HOTEND fixed";
+static const char m2bed[] PROGMEM = "MINTEMP BED fixed";
+static alert_automaton_mintemp alert_automaton_hotend(m2hotend), alert_automaton_bed(m2bed);
 
 void check_min_temp_heater0()
 {

+ 2 - 0
Firmware/temperature.h

@@ -84,6 +84,8 @@ extern int current_voltage_raw_IR;
   extern unsigned char soft_pwm_bed;
 #endif
 
+extern bool bedPWMDisabled;
+
 #ifdef PIDTEMP
   extern int pid_cycle, pid_number_of_cycles;
   extern float Kc,_Kp,_Ki,_Kd;

+ 0 - 10
Firmware/timer02.c

@@ -136,14 +136,4 @@ void delay2(unsigned long ms)
 	}
 }
 
-void tone2(__attribute__((unused)) uint8_t _pin, __attribute__((unused)) unsigned int frequency/*, unsigned long duration*/)
-{
-	PIN_SET(BEEPER);
-}
-
-void noTone2(__attribute__((unused)) uint8_t _pin)
-{
-	PIN_CLR(BEEPER);
-}
-
 #endif //SYSTEM_TIMER_2

+ 0 - 7
Firmware/timer02.h

@@ -23,13 +23,6 @@ extern unsigned long micros2(void);
 ///! Reimplemented original delay() using timer2
 extern void delay2(unsigned long ms);
 
-///! Reimplemented original tone() using timer2
-///! Does not perform any PWM tone generation, it just sets the beeper pin to 1
-extern void tone2(uint8_t _pin, unsigned int frequency/*, unsigned long duration*/);
-
-///! Turn off beeping - set beeper pin to 0
-extern void noTone2(uint8_t _pin);
-
 #if defined(__cplusplus)
 }
 #endif //defined(__cplusplus)

+ 156 - 150
Firmware/ultralcd.cpp

@@ -246,7 +246,9 @@ static void fil_unload_menu();
 static void lcd_disable_farm_mode();
 static void lcd_set_fan_check();
 static void lcd_cutter_enabled();
+#ifdef SNMM
 static char snmm_stop_print_menu();
+#endif //SNMM
 #ifdef SDCARD_SORT_ALPHA
  static void lcd_sort_type_set();
 #endif
@@ -948,7 +950,6 @@ static void lcd_status_screen()
 	}
 
 	if (current_click
-		&& (lcd_commands_type != LcdCommands::StopPrint) //click is aborted unless stop print finishes
 		&& ( menu_block_entering_on_serious_errors == SERIOUS_ERR_NONE ) // or a serious error blocks entering the menu
 	)
 	{
@@ -1359,98 +1360,6 @@ void lcd_commands()
 
 #endif // not SNMM
 
-	if (lcd_commands_type == LcdCommands::StopPrint)   /// stop print
-	{
-		
-
-		if (lcd_commands_step == 0) 
-		{ 
-			lcd_commands_step = 6; 
-		}
-
-		if (lcd_commands_step == 1 && !blocks_queued())
-		{
-			lcd_commands_step = 0;
-			lcd_commands_type = LcdCommands::Idle;
-			lcd_setstatuspgm(_T(WELCOME_MSG));
-			custom_message_type = CustomMsg::Status;
-			isPrintPaused = false;
-		}
-		if (lcd_commands_step == 2 && !blocks_queued())
-		{
-			setTargetBed(0);
-			enquecommand_P(PSTR("M104 S0")); //set hotend temp to 0
-
-			manage_heater();
-			lcd_setstatuspgm(_T(WELCOME_MSG));
-			cancel_heatup = false;
-			lcd_commands_step = 1;
-		}
-		if (lcd_commands_step == 3 && !blocks_queued())
-		{
-      // M84: Disable steppers.
-			enquecommand_P(PSTR("M84"));
-			autotempShutdown();
-			lcd_commands_step = 2;
-		}
-		if (lcd_commands_step == 4 && !blocks_queued())
-		{
-			lcd_setstatuspgm(_T(MSG_PLEASE_WAIT));
-      // G90: Absolute positioning.
-			enquecommand_P(PSTR("G90"));
-      // M83: Set extruder to relative mode.
-			enquecommand_P(PSTR("M83"));
-			#ifdef X_CANCEL_POS 
-			enquecommand_P(PSTR("G1 X"  STRINGIFY(X_CANCEL_POS) " Y" STRINGIFY(Y_CANCEL_POS) " E0 F7000"));
-			#else
-			enquecommand_P(PSTR("G1 X50 Y" STRINGIFY(Y_MAX_POS) " E0 F7000"));
-			#endif
-			lcd_ignore_click(false);
-			if (mmu_enabled)
-				lcd_commands_step = 8;
-			else
-				lcd_commands_step = 3;
-		}
-		if (lcd_commands_step == 5 && !blocks_queued())
-		{
-			lcd_setstatuspgm(_T(MSG_PRINT_ABORTED));
-      // G91: Set to relative positioning.
-			enquecommand_P(PSTR("G91"));
-      // Lift up.
-			enquecommand_P(PSTR("G1 Z15 F1500"));
-			if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) lcd_commands_step = 4;
-			else lcd_commands_step = 3;
-		}
-		if (lcd_commands_step == 6 && !blocks_queued())
-		{
-			lcd_setstatuspgm(_T(MSG_PRINT_ABORTED));
-			cancel_heatup = true;
-			setTargetBed(0);
-			if (mmu_enabled)
-				setAllTargetHotends(0);
-			manage_heater();
-			custom_message_type = CustomMsg::FilamentLoading;
-			lcd_commands_step = 5;
-		}
-		if (lcd_commands_step == 7 && !blocks_queued())
-		{
-			if (mmu_enabled)
-				enquecommand_P(PSTR("M702 C")); //current
-			else
-				switch(snmm_stop_print_menu())
-				{
-				case 0: enquecommand_P(PSTR("M702")); break;//all 
-				case 1: enquecommand_P(PSTR("M702 U")); break; //used
-				case 2: enquecommand_P(PSTR("M702 C")); break; //current
-				default: enquecommand_P(PSTR("M702")); break;
-				}
-			lcd_commands_step = 3;
-		}
-		if (lcd_commands_step == 8 && !blocks_queued()) { //step 8 is here for delay (going to next step after execution of all gcodes from step 4)
-			lcd_commands_step = 7; 
-		}
-	}
-
 	if (lcd_commands_type == LcdCommands::FarmModeConfirm)   /// farm mode confirm
 	{
 
@@ -1554,7 +1463,7 @@ void lcd_return_to_status()
 //! @brief Pause print, disable nozzle heater, move to park position
 void lcd_pause_print()
 {
-    stop_and_save_print_to_ram(0.0,0.0);
+    stop_and_save_print_to_ram(0.0, -default_retraction);
     lcd_return_to_status();
     isPrintPaused = true;
     if (LcdCommands::Idle == lcd_commands_type)
@@ -1776,11 +1685,23 @@ static void lcd_menu_fails_stats_print()
     uint8_t crashX = eeprom_read_byte((uint8_t*)EEPROM_CRASH_COUNT_X);
     uint8_t crashY = eeprom_read_byte((uint8_t*)EEPROM_CRASH_COUNT_Y);
     lcd_home();
+#ifndef PAT9125
     lcd_printf_P(failStatsFmt,
         _i("Last print failures"),  ////c=20 r=1
         _i("Power failures"), power,  ////c=14 r=1
         _i("Filam. runouts"), filam,  ////c=14 r=1
         _i("Crash"), crashX, crashY);  ////c=7 r=1
+#else
+    // On the MK3 include detailed PAT9125 statistics about soft failures
+    lcd_printf_P(PSTR("%S\n"
+                      " %-16.16S%-3d\n"
+                      " %-7.7S H %-3d S %-3d\n"
+                      " %-7.7S X %-3d Y %-3d"),
+                 _i("Last print failures"), ////c=20 r=1
+                 _i("Power failures"), power, ////c=14 r=1
+                 _i("Runouts"), filam, fsensor_softfail, //c=7 r=1
+                 _i("Crash"), crashX, crashY);  ////c=7 r=1
+#endif
     menu_back_if_clicked_fb();
 }
 
@@ -2211,10 +2132,12 @@ void lcd_set_filament_autoload() {
      fsensor_autoload_set(!fsensor_autoload_enabled);
 }
 
+#if defined(FILAMENT_SENSOR) && defined(PAT9125)
 void lcd_set_filament_oq_meass()
 {
      fsensor_oq_meassure_set(!fsensor_oq_meassure_enabled);
 }
+#endif
 
 
 FilamentAction eFilamentAction=FilamentAction::None; // must be initialized as 'non-autoLoad'
@@ -2774,9 +2697,9 @@ static void lcd_LoadFilament()
 //!
 //! @code{.unparsed}
 //! |01234567890123456789|
-//! |Filament used:      | c=18 r=1
-//! |         00.00m     |
-//! |Print time:         | c=18 r=1
+//! |Filament used:      | c=19 r=1
+//! |           0000.00m |
+//! |Print time:         | c=19 r=1
 //! |        00h 00m 00s |
 //! ----------------------
 //! @endcode
@@ -2785,32 +2708,33 @@ static void lcd_LoadFilament()
 //!
 //! @code{.unparsed}
 //! |01234567890123456789|
-//! |Total filament :    | c=18 r=1
-//! |           000.00 m |
-//! |Total print time :  | c=18 r=1
-//! |     00d :00h :00 m |
+//! |Total filament:     | c=19 r=1
+//! |           0000.00m |
+//! |Total print time:   | c=19 r=1
+//! |        00d 00h 00m |
 //! ----------------------
 //! @endcode
 //! @todo Positioning of the messages and values on LCD aren't fixed to their exact place. This causes issues with translations. Translations missing for "d"days, "h"ours, "m"inutes", "s"seconds".
 void lcd_menu_statistics()
 {
+    lcd_timeoutToStatus.stop(); //infinite timeout
 	if (IS_SD_PRINTING)
 	{
 		const float _met = ((float)total_filament_used) / (100000.f);
 		const uint32_t _t = (_millis() - starttime) / 1000ul;
-		const int _h = _t / 3600;
-		const int _m = (_t - (_h * 3600ul)) / 60ul;
-		const int _s = _t - ((_h * 3600ul) + (_m * 60ul));
+		const uint32_t _h = _t / 3600;
+		const uint8_t _m = (_t - (_h * 3600ul)) / 60ul;
+		const uint8_t _s = _t - ((_h * 3600ul) + (_m * 60ul));
 
-		lcd_clear();
+        lcd_home();
 		lcd_printf_P(_N(
 			"%S:\n"
-			"%17.2fm  \n"
+			"%18.2fm \n"
 			"%S:\n"
-			"%2dh %02dm %02ds"
+			"%10ldh %02hhdm %02hhds"
 		    ),
-            _i("Filament used"), _met,  ////c=18 r=1
-            _i("Print time"), _h, _m, _s);  ////c=18 r=1
+            _i("Filament used"), _met,  ////c=19 r=1
+            _i("Print time"), _h, _m, _s);  ////c=19 r=1
 		menu_back_if_clicked_fb();
 	}
 	else
@@ -2820,29 +2744,20 @@ void lcd_menu_statistics()
 		uint8_t _hours, _minutes;
 		uint32_t _days;
 		float _filament_m = (float)_filament/100;
-//		int _filament_km = (_filament >= 100000) ? _filament / 100000 : 0;
-//		if (_filament_km > 0)  _filament_m = _filament - (_filament_km * 100000);
 		_days = _time / 1440;
 		_hours = (_time - (_days * 1440)) / 60;
 		_minutes = _time - ((_days * 1440) + (_hours * 60));
 
-		lcd_clear();
+		lcd_home();
 		lcd_printf_P(_N(
 			"%S:\n"
-			"%17.2fm  \n"
+			"%18.2fm \n"
 			"%S:\n"
-			"%7ldd :%2hhdh :%02hhdm"
-		), _i("Total filament"), _filament_m, _i("Total print time"), _days, _hours, _minutes);
-		KEEPALIVE_STATE(PAUSED_FOR_USER);
-		while (!lcd_clicked())
-		{
-			manage_heater();
-			manage_inactivity(true);
-			_delay(100);
-		}
-		KEEPALIVE_STATE(NOT_BUSY);
-		lcd_quick_feedback();
-		menu_back();
+			"%10ldd %02hhdh %02hhdm"
+            ),
+            _i("Total filament"), _filament_m,  ////c=19 r=1
+            _i("Total print time"), _days, _hours, _minutes);  ////c=19 r=1
+        menu_back_if_clicked_fb();
 	}
 }
 
@@ -5936,7 +5851,7 @@ void bowden_menu() {
 	}
 }
 
-//#ifdef SNMM
+#ifdef SNMM
 
 static char snmm_stop_print_menu() { //menu for choosing which filaments will be unloaded in stop print
 	lcd_clear();
@@ -5987,6 +5902,8 @@ static char snmm_stop_print_menu() { //menu for choosing which filaments will be
 	
 }
 
+#endif //SNMM
+
 //! @brief Select one of numbered items
 //!
 //! Create list of items with header. Header can not be selected.
@@ -6649,14 +6566,15 @@ static bool fan_error_selftest()
 void lcd_resume_print()
 {
     lcd_return_to_status();
-    lcd_reset_alert_level();
-    lcd_setstatuspgm(_T(MSG_RESUMING_PRINT));
     lcd_reset_alert_level(); //for fan speed error
-
     if (fan_error_selftest()) return; //abort if error persists
 
+    lcd_setstatuspgm(_T(MSG_FINISHING_MOVEMENTS));
+    st_synchronize();
+
+    lcd_setstatuspgm(_T(MSG_RESUMING_PRINT));
     isPrintPaused = false;
-    restore_print_from_ram_and_continue(0.0);
+    restore_print_from_ram_and_continue(default_retraction);
     pause_time += (_millis() - start_pause_print); //accumulate time when print is paused for correct statistics calculation
     refresh_cmd_timeout();
     SERIAL_PROTOCOLLNRPGM(MSG_OCTOPRINT_RESUMED); //resume octoprint
@@ -6795,10 +6713,15 @@ static void lcd_main_menu()
   }
 
 
+  if(isPrintPaused && saved_printing_type == PRINTING_TYPE_USB)
+  {
 #ifdef FANCHECK
-  if((fan_check_error == EFCE_FIXED) && (saved_printing_type == PRINTING_TYPE_USB))
-    MENU_ITEM_SUBMENU_P(_i("Resume print"), lcd_resume_print);////MSG_RESUME_PRINT
+      if((fan_check_error == EFCE_FIXED) || (fan_check_error == EFCE_OK))
+          MENU_ITEM_SUBMENU_P(_i("Resume print"), lcd_resume_print);////MSG_RESUME_PRINT
+#else
+      MENU_ITEM_SUBMENU_P(_i("Resume print"), lcd_resume_print);////MSG_RESUME_PRINT
 #endif
+  }
 
 #ifdef SDSUPPORT
   if (card.cardOK || lcd_commands_type == LcdCommands::Layer1Cal)
@@ -6967,6 +6890,60 @@ static void lcd_colorprint_change() {
 	lcd_draw_update = 3;
 }
 
+
+#ifdef LA_LIVE_K
+// @wavexx: looks like there's no generic float editing function in menu.cpp so we
+//          redefine our custom handling functions to mimick other tunables
+const char menu_fmt_float13off[] PROGMEM = "%c%-13.13S%6.6S";
+
+static void lcd_advance_draw_K(char chr, float val)
+{
+    if (val <= 0)
+        lcd_printf_P(menu_fmt_float13off, chr, MSG_ADVANCE_K, _T(MSG_OFF));
+    else
+        lcd_printf_P(menu_fmt_float13, chr, MSG_ADVANCE_K, val);
+}
+
+static void lcd_advance_edit_K(void)
+{
+    if (lcd_draw_update)
+    {
+        if (lcd_encoder < 0) lcd_encoder = 0;
+        if (lcd_encoder > 999) lcd_encoder = 999;
+        lcd_set_cursor(0, 1);
+        lcd_advance_draw_K(' ', 0.01 * lcd_encoder);
+    }
+    if (LCD_CLICKED)
+    {
+        extruder_advance_K = 0.01 * lcd_encoder;
+        menu_back_no_reset();
+    }
+}
+
+static uint8_t lcd_advance_K()
+{
+    if (menu_item == menu_line)
+    {
+        if (lcd_draw_update)
+        {
+            lcd_set_cursor(0, menu_row);
+            lcd_advance_draw_K((lcd_encoder == menu_item)?'>':' ', extruder_advance_K);
+        }
+        if (menu_clicked && (lcd_encoder == menu_item))
+        {
+            menu_submenu_no_reset(lcd_advance_edit_K);
+            lcd_encoder = 100. * extruder_advance_K;
+            return menu_item_ret();
+        }
+    }
+    menu_item++;
+    return 0;
+}
+
+#define MENU_ITEM_EDIT_advance_K() do { if (lcd_advance_K()) return; } while (0)
+#endif
+
+
 static void lcd_tune_menu()
 {
 	typedef struct
@@ -7005,8 +6982,11 @@ static void lcd_tune_menu()
 
 	MENU_ITEM_EDIT_int3_P(_T(MSG_FAN_SPEED), &fanSpeed, 0, 255);//5
 	MENU_ITEM_EDIT_int3_P(_i("Flow"), &extrudemultiply, 10, 999);//6////MSG_FLOW
+#ifdef LA_LIVE_K
+	MENU_ITEM_EDIT_advance_K();//7
+#endif
 #ifdef FILAMENTCHANGEENABLE
-	MENU_ITEM_FUNCTION_P(_T(MSG_FILAMENTCHANGE), lcd_colorprint_change);//7
+	MENU_ITEM_FUNCTION_P(_T(MSG_FILAMENTCHANGE), lcd_colorprint_change);//8
 #endif
 
 #ifdef FILAMENT_SENSOR
@@ -7205,7 +7185,6 @@ void lcd_print_stop()
 
     // Clear any saved printing state
     cancel_saved_printing();
-	cancel_heatup = true;
 
     // Abort the planner/queue/sd
     planner_abort_hard();
@@ -7216,19 +7195,48 @@ void lcd_print_stop()
 
     CRITICAL_SECTION_END;
 
+#ifdef MESH_BED_LEVELING
+    mbl.active = false; //also prevents undoing the mbl compensation a second time in the second planner_abort_hard()
+#endif
+
 	lcd_setstatuspgm(_T(MSG_PRINT_ABORTED));
 	stoptime = _millis();
 	unsigned long t = (stoptime - starttime - pause_time) / 1000; //time in s
 	pause_time = 0;
 	save_statistics(total_filament_used, t);
-	lcd_return_to_status();
-	lcd_ignore_click(true);
-	lcd_commands_step = 0;
-	lcd_commands_type = LcdCommands::StopPrint;
-	// Turn off the print fan
-	SET_OUTPUT(FAN_PIN);
-	WRITE(FAN_PIN, 0);
-	fanSpeed = 0;
+
+    lcd_commands_step = 0;
+    lcd_commands_type = LcdCommands::Idle;
+
+    lcd_cooldown(); //turns off heaters and fan; goes to status screen.
+    cancel_heatup = true; //unroll temperature wait loop stack.
+
+    current_position[Z_AXIS] += 10; //lift Z.
+    plan_buffer_line_curposXYZE(manual_feedrate[Z_AXIS] / 60, active_extruder);
+
+    if (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) //if axis are homed, move to parked position.
+    {
+        current_position[X_AXIS] = X_CANCEL_POS;
+        current_position[Y_AXIS] = Y_CANCEL_POS;
+        plan_buffer_line_curposXYZE(manual_feedrate[0] / 60, active_extruder);
+    }
+    st_synchronize();
+
+    if (mmu_enabled) extr_unload(); //M702 C
+
+    finishAndDisableSteppers(); //M84
+
+    lcd_setstatuspgm(_T(WELCOME_MSG));
+    custom_message_type = CustomMsg::Status;
+
+    planner_abort_hard(); //needs to be done since plan_buffer_line resets waiting_inside_plan_buffer_line_print_aborted to false. Also copies current to destination.
+    
+    axis_relative_modes[X_AXIS] = false;
+    axis_relative_modes[Y_AXIS] = false;
+    axis_relative_modes[Z_AXIS] = false;
+    axis_relative_modes[E_AXIS] = true;
+    
+    isPrintPaused = false; //clear isPrintPaused flag to allow starting next print after pause->stop scenario.
 }
 
 void lcd_sdcard_stop()
@@ -7420,26 +7428,24 @@ bool lcd_selftest()
 	int _progress = 0;
 	bool _result = true;
 	bool _swapped_fan = false;
+#if IR_SENSOR_ANALOG
+     bool bAction;
+     bAction=lcd_show_fullscreen_message_yes_no_and_wait_P(_i("Is the filament unloaded?"),false,true);
+     if(!bAction)
+          return(false);
+#endif //IR_SENSOR_ANALOG
 	lcd_wait_for_cool_down();
 	lcd_clear();
 	lcd_set_cursor(0, 0); lcd_puts_P(_i("Self test start  "));////MSG_SELFTEST_START c=20
 	#ifdef TMC2130
 	  FORCE_HIGH_POWER_START;
 	#endif // TMC2130
-#if !IR_SENSOR_ANALOG
      _delay(2000);
-#endif //!IR_SENSOR_ANALOG
     
     FORCE_BL_ON_START;
     
 	_delay(2000);
 	KEEPALIVE_STATE(IN_HANDLER);
-#if IR_SENSOR_ANALOG
-     bool bAction;
-     bAction=lcd_show_fullscreen_message_yes_no_and_wait_P(_i("Is filament unloaded?"),false,true);
-     if(!bAction)
-          return(false);
-#endif //IR_SENSOR_ANALOG
 
 	_progress = lcd_selftest_screen(TestScreen::ExtruderFan, _progress, 3, true, 2000);
 #if (defined(FANCHECK) && defined(TACH_0))

+ 1 - 1
Firmware/variants/1_75mm_MK25-RAMBo10a-E3Dv6full.h

@@ -504,7 +504,7 @@
 #define MMU_REQUIRED_FW_BUILDNR 132
 #define MMU_FORCE_STEALTH_MODE
 #define MMU_DEBUG //print communication between MMU2 and printer on serial
-//#define MMU_HAS_CUTTER
+#define MMU_HAS_CUTTER
 
 #define MMU_IDLER_SENSOR_ATTEMPTS_NR 21 //max. number of attempts to load filament if first load failed; value for max bowden length and case when loading fails right at the beginning
 

+ 1 - 1
Firmware/variants/1_75mm_MK25-RAMBo13a-E3Dv6full.h

@@ -505,7 +505,7 @@
 #define MMU_REQUIRED_FW_BUILDNR 132
 #define MMU_FORCE_STEALTH_MODE
 #define MMU_DEBUG //print communication between MMU2 and printer on serial
-//#define MMU_HAS_CUTTER
+#define MMU_HAS_CUTTER
 
 #define MMU_IDLER_SENSOR_ATTEMPTS_NR 21 //max. number of attempts to load filament if first load failed; value for max bowden length and case when loading fails right at the beginning
 

+ 1 - 1
Firmware/variants/1_75mm_MK25S-RAMBo10a-E3Dv6full.h

@@ -504,7 +504,7 @@
 #define MMU_REQUIRED_FW_BUILDNR 132
 #define MMU_FORCE_STEALTH_MODE
 #define MMU_DEBUG //print communication between MMU2 and printer on serial
-//#define MMU_HAS_CUTTER
+#define MMU_HAS_CUTTER
 
 // This is experimental feature requested by our test department.
 // There is no known use for ordinary user. If enabled by this macro

+ 1 - 1
Firmware/variants/1_75mm_MK25S-RAMBo13a-E3Dv6full.h

@@ -505,7 +505,7 @@
 #define MMU_REQUIRED_FW_BUILDNR 132
 #define MMU_FORCE_STEALTH_MODE
 #define MMU_DEBUG //print communication between MMU2 and printer on serial
-//#define MMU_HAS_CUTTER
+#define MMU_HAS_CUTTER
 
 // This is experimental feature requested by our test department.
 // There is no known use for ordinary user. If enabled by this macro

+ 9 - 5
Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h

@@ -151,8 +151,8 @@
 // this value is litlebit higher that real limit, because ambient termistor is on the board and is temperated from it,
 // temperature inside the case is around 31C for ambient temperature 25C, when the printer is powered on long time and idle
 // the real limit is 15C (same as MINTEMP limit), this is because 15C is end of scale for both used thermistors (bed, heater)
-#define MINTEMP_MINAMBIENT      25
-#define MINTEMP_MINAMBIENT_RAW  978
+#define MINTEMP_MINAMBIENT      10
+#define MINTEMP_MINAMBIENT_RAW  1002
 
 #define DEBUG_DCODE3
 
@@ -282,14 +282,14 @@
  *------------------------------------*/
 
 // Mintemps
-#define HEATER_0_MINTEMP 15
+#define HEATER_0_MINTEMP 10
 #define HEATER_1_MINTEMP 5
 #define HEATER_2_MINTEMP 5
 #define HEATER_MINTEMP_DELAY 15000                // [ms] ! if changed, check maximal allowed value @ ShortTimer
 #if HEATER_MINTEMP_DELAY>USHRT_MAX
 #error "Check maximal allowed value @ ShortTimer (see HEATER_MINTEMP_DELAY definition)"
 #endif
-#define BED_MINTEMP 15
+#define BED_MINTEMP 10
 #define BED_MINTEMP_DELAY 50000                   // [ms] ! if changed, check maximal allowed value @ ShortTimer
 #if BED_MINTEMP_DELAY>USHRT_MAX
 #error "Check maximal allowed value @ ShortTimer (see BED_MINTEMP_DELAY definition)"
@@ -618,6 +618,10 @@
 // The following example, 12 * (4 * 16 / 400) = 12 * 0.16mm = 1.92mm.
 //#define UVLO_Z_AXIS_SHIFT 1.92
 #define UVLO_Z_AXIS_SHIFT 0.64
+// When powered off during PP recovery, the Z axis position can still be re-adjusted. In this case
+// we just need to shift to the nearest fullstep, but we need a move which is at least
+// "dropsegments" steps long. All the above rules still need to apply.
+#define UVLO_TINY_Z_AXIS_SHIFT 0.16
 // If power panic occured, and the current temperature is higher then target temperature before interrupt minus this offset, print will be recovered automatically. 
 #define AUTOMATIC_UVLO_BED_TEMP_OFFSET 5 
 
@@ -630,7 +634,7 @@
 #define MMU_REQUIRED_FW_BUILDNR 83
 #define MMU_HWRESET
 #define MMU_DEBUG //print communication between MMU2 and printer on serial
-//#define MMU_HAS_CUTTER
+#define MMU_HAS_CUTTER
 #define MMU_IDLER_SENSOR_ATTEMPTS_NR 21 //max. number of attempts to load filament if first load failed; value for max bowden length and case when loading fails right at the beginning
 
 #endif //__CONFIGURATION_PRUSA_H

+ 10 - 6
Firmware/variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h

@@ -153,8 +153,8 @@
 // this value is litlebit higher that real limit, because ambient termistor is on the board and is temperated from it,
 // temperature inside the case is around 31C for ambient temperature 25C, when the printer is powered on long time and idle
 // the real limit is 15C (same as MINTEMP limit), this is because 15C is end of scale for both used thermistors (bed, heater)
-#define MINTEMP_MINAMBIENT      25
-#define MINTEMP_MINAMBIENT_RAW  978
+#define MINTEMP_MINAMBIENT      10
+#define MINTEMP_MINAMBIENT_RAW  1002
 
 #define DEBUG_DCODE3
 
@@ -284,14 +284,14 @@
  *------------------------------------*/
 
 // Mintemps
-#define HEATER_0_MINTEMP 15
+#define HEATER_0_MINTEMP 10
 #define HEATER_1_MINTEMP 5
 #define HEATER_2_MINTEMP 5
 #define HEATER_MINTEMP_DELAY 15000                // [ms] ! if changed, check maximal allowed value @ ShortTimer
 #if HEATER_MINTEMP_DELAY>USHRT_MAX
 #error "Check maximal allowed value @ ShortTimer (see HEATER_MINTEMP_DELAY definition)"
 #endif
-#define BED_MINTEMP 15
+#define BED_MINTEMP 10
 #define BED_MINTEMP_DELAY 50000                   // [ms] ! if changed, check maximal allowed value @ ShortTimer
 #if BED_MINTEMP_DELAY>USHRT_MAX
 #error "Check maximal allowed value @ ShortTimer (see BED_MINTEMP_DELAY definition)"
@@ -620,7 +620,11 @@
 // The following example, 12 * (4 * 16 / 400) = 12 * 0.16mm = 1.92mm.
 //#define UVLO_Z_AXIS_SHIFT 1.92
 #define UVLO_Z_AXIS_SHIFT 0.64
-// If power panic occured, and the current temperature is higher then target temperature before interrupt minus this offset, print will be recovered automatically. 
+// When powered off during PP recovery, the Z axis position can still be re-adjusted. In this case
+// we just need to shift to the nearest fullstep, but we need a move which is at least
+// "dropsegments" steps long. All the above rules still need to apply.
+#define UVLO_TINY_Z_AXIS_SHIFT 0.16
+// If power panic occured, and the current temperature is higher then target temperature before interrupt minus this offset, print will be recovered automatically.
 #define AUTOMATIC_UVLO_BED_TEMP_OFFSET 5 
 
 #define HEATBED_V2
@@ -632,7 +636,7 @@
 #define MMU_REQUIRED_FW_BUILDNR 83
 #define MMU_HWRESET
 #define MMU_DEBUG //print communication between MMU2 and printer on serial
-//#define MMU_HAS_CUTTER
+#define MMU_HAS_CUTTER
 
 // This is experimental feature requested by our test department.
 // There is no known use for ordinary user. If enabled by this macro

+ 19 - 12
PF-build.sh

@@ -56,7 +56,7 @@
 #   Some may argue that this is only used by a script, BUT as soon someone accidentally or on purpose starts Arduino IDE
 #   it will use the default Arduino IDE folders and so can corrupt the build environment.
 #
-# Version: 1.0.6-Build_10
+# Version: 1.0.6-Build_13
 # Change log:
 # 12 Jan 2019, 3d-gussner, Fixed "compiler.c.elf.flags=-w -Os -Wl,-u,vfprintf -lprintf_flt -lm -Wl,--gc-sections" in 'platform.txt'
 # 16 Jan 2019, 3d-gussner, Build_2, Added development check to modify 'Configuration.h' to prevent unwanted LCD messages that Firmware is unknown
@@ -114,7 +114,10 @@
 # 26 Jul 2019, 3d-gussner, Change JSON repository to prusa3d after PR https://github.com/prusa3d/Arduino_Boards/pull/1 was merged
 # 23 Sep 2019, 3d-gussner, Prepare PF-build.sh for comming Prusa3d/Arduino_Boards version 1.0.2 Pull Request
 # 17 Oct 2019, 3d-gussner, Changed folder and check file names to have seperated build enviroments depening on Arduino IDE version and
-#                          board-versions. 
+#                          board-versions.
+# 15 Dec 2019, 3d-gussner, Prepare for switch to Prusa3d/PF-build-env repository
+# 15 Dec 2019, 3d-gussner, Fix Audrino user preferences for the chosen board.
+# 17 Dec 2019, 3d-gussner, Fix "timer0_fract = 0" warning by using Arduino_boards v1.0.3
 #### Start check if OSTYPE is supported
 OS_FOUND=$( command -v uname)
 
@@ -207,19 +210,22 @@ if ! type python > /dev/null; then
 	fi
 fi
 
-#### End prepare bash environment
+#### End prepare bash / Linux environment
 
 
 #### Set build environment 
 ARDUINO_ENV="1.8.5"
 BUILD_ENV="1.0.6"
-BOARD="rambo"
-BOARD_PACKAGE_NAME="PrusaResearchRambo"
-BOARD_VERSION="1.0.1"
+BOARD="prusa_einsy_rambo"
+BOARD_PACKAGE_NAME="PrusaResearch"
+BOARD_VERSION="1.0.3"
+#BOARD_URL="https://raw.githubusercontent.com/3d-gussner/Arduino_Boards/Prusa_Merge_v1.0.3/IDE_Board_Manager/package_prusa3d_index.json"
 BOARD_URL="https://raw.githubusercontent.com/prusa3d/Arduino_Boards/master/IDE_Board_Manager/package_prusa3d_index.json"
-BOARD_FILENAME="prusa3drambo"
-BOARD_FILE_URL="https://raw.githubusercontent.com/prusa3d/Arduino_Boards/master/IDE_Board_Manager/prusa3drambo-1.0.1.tar.bz2"
-PF_BUILD_FILE_URL="https://github.com/3d-gussner/PF-build-env/releases/download/$BUILD_ENV/PF-build-env-$BUILD_ENV.zip"
+BOARD_FILENAME="prusa3dboards"
+#BOARD_FILE_URL="https://raw.githubusercontent.com/3d-gussner/Arduino_Boards/Prusa_Merge_v1.0.3/IDE_Board_Manager/prusa3dboards-1.0.3.tar.bz2"
+BOARD_FILE_URL="https://raw.githubusercontent.com/prusa3d/Arduino_Boards/master/IDE_Board_Manager/prusa3dboards-1.0.3.tar.bz2"
+#PF_BUILD_FILE_URL="https://github.com/3d-gussner/PF-build-env-1/releases/download/$BUILD_ENV-WinLin/PF-build-env-WinLin-$BUILD_ENV.zip"
+PF_BUILD_FILE_URL="https://github.com/prusa3d/PF-build-env/releases/download/$BUILD_ENV-WinLin/PF-build-env-WinLin-$BUILD_ENV.zip"
 LIB="PrusaLibrary"
 SCRIPT_PATH="$( cd "$(dirname "$0")" ; pwd -P )"
 
@@ -233,6 +239,7 @@ echo "Ardunio IDE :" $ARDUINO_ENV
 echo "Build env   :" $BUILD_ENV
 echo "Board       :" $BOARD
 echo "Package name:" $BOARD_PACKAGE_NAME
+echo "Board v.    :" $BOARD_VERSION
 echo "Specific Lib:" $LIB
 echo ""
 
@@ -321,7 +328,7 @@ if [ ! -e ../PF-build-env-$BUILD_ENV/Preferences-$ARDUINO_ENV-$BOARD_VERSION-$TA
 	echo "update.check"
 	sed -i 's/update.check = true/update.check = false/g' ../PF-build-env-$BUILD_ENV/$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor/lib/preferences.txt
 	echo "board"
-	sed -i 's/board = uno/board = $BOARD/g' ../PF-build-env-$BUILD_ENV/$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor/lib/preferences.txt
+	sed -i "s/board = uno/board = $BOARD/g" ../PF-build-env-$BUILD_ENV/$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor/lib/preferences.txt
 	echo "editor.linenumbers"
 	sed -i 's/editor.linenumbers = false/editor.linenumbers = true/g' ../PF-build-env-$BUILD_ENV/$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor/lib/preferences.txt
 	echo "boardsmanager.additional.urls"
@@ -364,7 +371,7 @@ if [[ ! -d "../PF-build-env-$BUILD_ENV/$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$P
 fi	
 
 # Download and extract Prusa Firmware specific library files
-if [ ! -f "PF-build-env-$BUILD_ENV.zip" ]; then
+if [ ! -f "PF-build-env-WinLin-$BUILD_ENV.zip" ]; then
 	echo "$(tput setaf 6)Downloading Prusa Firmware build environment...$(tput setaf 2)"
 	sleep 2
 	wget $PF_BUILD_FILE_URL || exit 11
@@ -373,7 +380,7 @@ fi
 if [ ! -e "../PF-build-env-$BUILD_ENV/PF-build-env-$BUILD_ENV-$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor.txt" ]; then
 	echo "$(tput setaf 6)Unzipping Prusa Firmware build environment...$(tput setaf 2)"
 	sleep 2
-	unzip -o PF-build-env-$BUILD_ENV.zip -d ../PF-build-env-$BUILD_ENV/$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor || exit 12
+	unzip -o PF-build-env-WinLin-$BUILD_ENV.zip -d ../PF-build-env-$BUILD_ENV/$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor || exit 12
 	echo "# PF-build-env-$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor-$BUILD_ENV" >> ../PF-build-env-$BUILD_ENV/PF-build-env-$BUILD_ENV-$ARDUINO_ENV-$BOARD_VERSION-$TARGET_OS-$Processor.txt
 	echo "$(tput sgr0)"
 fi

+ 1 - 1
README.md

@@ -46,7 +46,7 @@ _Note: Multi language build is not supported._
 
 * Open Arduino and navigate to File -> Preferences -> Settings
 * To the text field `"Additional Boards Manager URLSs"` add `https://raw.githubusercontent.com/prusa3d/Arduino_Boards/master/IDE_Board_Manager/package_prusa3d_index.json`
-* Open Board manager (`Tools->Board->Board manager`), and install `Prusa Research AVR MK3 RAMBo EINSy board`
+* Open Board manager (`Tools->Board->Board manager`), and install `Prusa Research AVR Boards by Prusa Research`
 
 **c.** Modify compiler flags in `platform.txt` file
      

+ 3 - 2
build.sh

@@ -1,5 +1,5 @@
 #!/bin/bash 
-BUILD_ENV="1.0.6"
+BUILD_ENV="1.0.6.1"
 SCRIPT_PATH="$( cd "$(dirname "$0")" ; pwd -P )"
 
 if [ ! -d "build-env" ]; then
@@ -8,7 +8,8 @@ fi
 cd build-env || exit 2
 
 if [ ! -f "PF-build-env-Linux64-$BUILD_ENV.zip" ]; then
-    wget https://github.com/mkbel/PF-build-env/releases/download/$BUILD_ENV/PF-build-env-Linux64-$BUILD_ENV.zip || exit 3
+    #wget https://github.com/3d-gussner/PF-build-env-1/releases/download/$BUILD_ENV-Linux64/PF-build-env-Linux64-$BUILD_ENV.zip || exit 3
+	wget https://github.com/prusa3d/PF-build-env/releases/download/$BUILD_ENV-Linux64/PF-build-env-Linux64-$BUILD_ENV.zip || exit 3
 fi
 
 if [ ! -d "../../PF-build-env-$BUILD_ENV" ]; then