Merge remote-tracking branch 'upstream/MK3' into flashair_display_ip

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
 	}
 }

Firmware/Configuration_adv.h

@@ -268,43 +268,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:

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">D3: Read/Write RAM</a>
+    This command can be used without any additional parameters. It will read the entire RAM.
+      
+          D3 [ 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.
+      
+          D3 [ 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");

Firmware/Marlin.h

@@ -308,6 +308,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);
@@ -455,6 +456,7 @@ extern void print_mesh_bed_leveling_table();
 
 extern void stop_and_save_print_to_ram(float z_move, float e_move);
 extern void restore_print_from_ram_and_continue(float e_move);
+extern void cancel_saved_printing();
 
 
 //estimated time to end of the print

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"
@@ -342,10 +345,6 @@ static float next_feedrate;
 // Original feedrate saved during homing moves
 static float saved_feedrate;
 
-// Determines Absolute or Relative Coordinates.
-// Also there is bool axis_relative_modes[] per axis flag.
-static bool relative_mode = false;  
-
 const int sensitive_pins[] = SENSITIVE_PINS; // Sensitive pin list for M42
 
 //static float tt = 0;
@@ -361,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
@@ -388,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
 //! @}
@@ -2066,35 +2061,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() {
@@ -2125,9 +2121,15 @@ void raise_z_above(float target, bool plan)
     // Z needs raising
     current_position[Z_AXIS] = target;
 
-    if (axis_known_position[Z_AXIS])
+#if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
+    bool z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
+#else
+    bool z_min_endstop = false;
+#endif
+
+    if (axis_known_position[Z_AXIS] || z_min_endstop)
     {
-        // current position is known, it's safe to raise Z
+        // current position is known or very low, it's safe to raise Z
         if(plan) plan_buffer_line_curposXYZE(max_feedrate[Z_AXIS], active_extruder);
         return;
     }
@@ -3354,6 +3356,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
@@ -3365,8 +3410,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 
@@ -3385,7 +3429,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)
@@ -3502,7 +3546,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()
@@ -3546,7 +3594,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)
@@ -3558,7 +3624,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"))
 	  {
@@ -3568,12 +3634,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();
@@ -3581,7 +3647,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)
 		{
@@ -3594,7 +3660,7 @@ void process_commands()
 			}
 		}
     
-    //! ### TMC_SET_STEP_
+    // ### TMC_SET_STEP_
     //  ------------------
 		else if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_STEP_"), 9) == 0)
 		{
@@ -3608,7 +3674,7 @@ void process_commands()
 			}
 		}
 
-    //! ### TMC_SET_CHOP_
+    // ### TMC_SET_CHOP_
     //  -------------------
 		else if (strncmp_P(CMDBUFFER_CURRENT_STRING + 4, PSTR("SET_CHOP_"), 9) == 0)
 		{
@@ -3663,13 +3729,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
@@ -3690,7 +3757,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`
-    *
     */
 
 
@@ -3705,7 +3771,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) {
@@ -3874,8 +3940,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) {
@@ -4085,8 +4167,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();
@@ -4094,8 +4193,6 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       }
       break;
  
-
-    //! ### G3  - CCW ARC
     // -------------------------------
     case 3: 
       if(Stopped == false) {
@@ -4105,8 +4202,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
@@ -4124,8 +4232,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
@@ -4136,8 +4246,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]);
@@ -4148,8 +4260,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;
@@ -4181,8 +4305,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
@@ -4327,8 +4455,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();
@@ -4352,15 +4482,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;
@@ -4369,8 +4503,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();
@@ -4387,8 +4524,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++)
@@ -4396,9 +4535,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)
@@ -4655,11 +4811,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
 	*  |
@@ -4675,6 +4848,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
@@ -4692,13 +4870,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;
 		} 
 		
@@ -4728,23 +4901,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;
@@ -5084,12 +5248,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: ");
@@ -5112,12 +5274,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();
@@ -5128,9 +5290,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;
@@ -5153,50 +5316,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
 
@@ -5206,40 +5369,56 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 
 #endif  // ENABLE_MESH_BED_LEVELING
             
-    //! ### G90 - Switch off relative mode
-    // -------------------------------
-    case 90:
-      relative_mode = false;
-      break;
 
-    //! ### G91 - Switch on relative mode
-    // -------------------------------
-    case 91:
-      relative_mode = true;
-      break;
+    /*!
+	### 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;
 
-    //! ### 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;
+    /*!
+	### 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 <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();
@@ -5250,8 +5429,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();
@@ -5265,9 +5445,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'))
   {
@@ -5287,8 +5473,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
     {
@@ -5337,8 +5524,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();
@@ -5351,28 +5539,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)
@@ -5380,24 +5576,35 @@ 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();
+#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();
@@ -5408,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){
@@ -5426,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();
@@ -5449,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) {
@@ -5481,14 +5697,22 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
         if(code_seen('S'))
           if(strchr_pointer<namestartpos) //only if "S" is occuring _before_ the filename
             card.setIndex(code_value_long());
+#ifndef LA_NOCOMPAT
+        la10c_reset();
+#endif
         card.startFileprint();
         if(!call_procedure)
           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){
@@ -5501,8 +5725,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();
@@ -5519,8 +5744,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'))
       {
@@ -5550,8 +5784,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.
@@ -5566,8 +5801,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;
@@ -5584,9 +5826,11 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
     }
 	break;
 
-    //! ### M46 - Show the assigned IP address (Prusa specific)
-    // --------------------------------------------------------------------
-    case 46:
+    /*!
+	### 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:
     {
         // M46: Prusa3D: Show the assigned IP address.
         if (card.ToshibaFlashAir_isEnabled()) {
@@ -5611,8 +5855,9 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
         break;
     }
 
-    //! ### 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);
@@ -5660,22 +5905,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
@@ -5914,12 +6162,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();
@@ -5933,8 +6187,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;
@@ -5948,20 +6209,44 @@ 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
-    // ----------------------------------------- 
+    /*!
+	### 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;
@@ -6057,16 +6342,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;
@@ -6116,12 +6407,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
     {
@@ -6176,8 +6474,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);
@@ -6187,8 +6492,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;
@@ -6196,8 +6502,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);
@@ -6215,8 +6523,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();
@@ -6239,25 +6549,41 @@ 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[3] = false;
+      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[3] = true;
+      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>]  
-    //! 
-    case 18: //compatibility
+    /*!
+	### 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')){
         stepper_inactive_time = code_value() * 1000;
@@ -6293,8 +6619,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;
@@ -6302,13 +6635,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;
@@ -6317,9 +6653,19 @@ 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++)
       {
@@ -6342,15 +6688,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();
@@ -6363,16 +6725,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());
@@ -6394,16 +6774,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
@@ -6415,20 +6792,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("");
@@ -6487,12 +6868,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;
@@ -6508,8 +6899,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).
       {
 
@@ -6534,10 +6933,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;
 		  }
@@ -6549,8 +6948,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++)
 		{
@@ -6584,8 +6985,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++)
 		{
@@ -6610,15 +7013,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')) {
@@ -6637,23 +7053,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: 
     {
@@ -6669,8 +7092,17 @@ Sigma_Exit:
     }
     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++)
       {
@@ -6679,8 +7111,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'))
@@ -6697,8 +7138,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'))
@@ -6711,8 +7160,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'))
@@ -6754,8 +7211,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;
@@ -6783,8 +7249,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
@@ -6801,8 +7276,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'))
@@ -6825,8 +7308,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')){
@@ -6881,8 +7373,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;
@@ -6922,8 +7423,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;
@@ -6944,8 +7454,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();
@@ -6975,8 +7497,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();
@@ -6996,8 +7529,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
@@ -7031,8 +7569,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;
@@ -7042,8 +7588,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;
@@ -7058,17 +7614,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
@@ -7084,40 +7653,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();
@@ -7127,8 +7707,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;
@@ -7136,6 +7724,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:
     {
@@ -7174,8 +7773,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();
@@ -7251,10 +7863,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:
 	{
@@ -7267,30 +7884,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;
@@ -7335,17 +7956,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();
@@ -7418,38 +8042,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;
@@ -7511,19 +8137,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]))
 			{
@@ -7553,8 +8204,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
@@ -7568,16 +8228,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());
@@ -7587,8 +8261,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());
@@ -7598,17 +8283,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();
@@ -7616,9 +8306,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();
@@ -7626,8 +8318,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();
@@ -7639,8 +8342,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());
@@ -7650,8 +8364,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());
@@ -7663,9 +8388,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
@@ -7710,11 +8450,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
@@ -7734,8 +8484,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'))
@@ -7744,16 +8496,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
@@ -7776,14 +8529,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);
     }
@@ -7792,19 +8550,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;
@@ -7893,15 +8648,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();
@@ -7988,95 +8736,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:
 	{
@@ -8102,15 +8952,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:
 	{
@@ -8135,8 +8990,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) {
@@ -8151,42 +9007,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;
@@ -8194,8 +9069,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
@@ -8215,7 +9102,9 @@ Sigma_Exit:
   ClearToSend();
 }
 
-
+/*!
+#### End of D-Codes
+*/
 
   /** @defgroup GCodes G-Code List 
   */
@@ -8283,7 +9172,7 @@ void get_coordinates()
   for(int8_t i=0; i < NUM_AXIS; i++) {
     if(code_seen(axis_codes[i]))
     {
-      bool relative = axis_relative_modes[i] || relative_mode;
+      bool relative = axis_relative_modes[i];
       destination[i] = (float)code_value();
       if (i == E_AXIS) {
         float emult = extruder_multiplier[active_extruder];
@@ -8952,6 +9841,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
@@ -9575,10 +10465,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;
@@ -9738,12 +10624,17 @@ 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);
 
@@ -9997,6 +10888,10 @@ 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
 }
 
 void restore_print_from_eeprom() {
@@ -10228,8 +11123,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
@@ -10246,23 +11139,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();
@@ -10316,12 +11215,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
@@ -10355,6 +11252,14 @@ void restore_print_from_ram_and_continue(float e_move)
     waiting_inside_plan_buffer_line_print_aborted = true; //unroll the stack
 }
 
+// Cancel the state related to a currently saved print
+void cancel_saved_printing()
+{
+    saved_target[0] = SAVED_TARGET_UNSET;
+    saved_printing_type = PRINTING_TYPE_NONE;
+    saved_printing = false;
+}
+
 void print_world_coordinates()
 {
 	printf_P(_N("world coordinates: (%.3f, %.3f, %.3f)\n"), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);

Firmware/eeprom.h

@@ -212,9 +212,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 !!!!!
 // !!!!!

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"
 
@@ -261,7 +258,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;
@@ -534,23 +531,11 @@ void fsensor_setup_interrupt(void)
 
 #endif //PAT9125
 
-void fsensor_st_block_begin(block_t* bl)
-{
-	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);}
-	}
-}
-
-void fsensor_st_block_chunk(block_t* bl, int cnt)
+void fsensor_st_block_chunk(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))
+	fsensor_st_cnt += cnt;
+	if (abs(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);}

Firmware/fsensor.h

@@ -60,12 +60,15 @@ 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)
 //! @}
 
 

Firmware/la10compat.cpp

@@ -0,0 +1,48 @@
+#include "la10compat.h"
+#include "Marlin.h"
+
+
+static LA10C_MODE la10c_mode = LA10C_UNKNOWN;
+
+
+void la10c_mode_change(LA10C_MODE mode)
+{
+    if(mode == la10c_mode) return;
+
+    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;
+}
+
+
+// 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);
+}

Firmware/la10compat.h

@@ -0,0 +1,39 @@
+// 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).
+//
+// 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/reset the interpretation mode for la10c_value()
+void la10c_mode_change(LA10C_MODE mode);
+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);

Firmware/menu.cpp

@@ -102,7 +102,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)
 	{
@@ -136,7 +136,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)
 	{

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_if_clicked(void);
@@ -83,6 +84,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 uint8_t menu_item_ret(void);
 
@@ -131,7 +133,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);

Firmware/messages.c

@@ -168,3 +168,6 @@ 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
+#ifdef LA_LIVE_K
+const char MSG_ADVANCE_K[] PROGMEM_N1 = "Advance K:"; ////c=13
+#endif

Firmware/messages.h

@@ -169,6 +169,7 @@ 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[];
 
 #if defined(__cplusplus)
 }

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;
             }

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,40 @@ 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;
+      block->advance_rate = (F_CPU / 8.0) / advance_speed;
+      if (block->advance_rate > 20000) {
+          block->advance_rate = (block->advance_rate >> 2)&0x3fff;
+          block->advance_step_loops = 4;
+      }
+      else if (block->advance_rate > 10000) {
+          block->advance_rate = (block->advance_rate >> 1)&0x7fff;
+          block->advance_step_loops = 2;
+      }
+      else
+          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 +1188,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 +1283,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 +1296,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).
@@ -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)
 {

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();

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

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

Firmware/stepper.cpp

@@ -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,24 @@ FORCE_INLINE void stepper_next_block()
 #endif /* LIN_ADVANCE */
       count_direction[E_AXIS] = 1;
     }
+#ifdef FILAMENT_SENSOR
+	fsensor_counter = 0;
+	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 +648,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 +710,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 +723,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 +782,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 +806,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 +825,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 +849,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 +896,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);
+#ifdef FILAMENT_SENSOR
+            fsensor_counter += (rev? -1: 1);
+#endif
+        }
+        while(--max_ticks);
+
+#ifdef FILAMENT_SENSOR
+        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 +1221,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,6 +1348,10 @@ 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();
 }

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();

Firmware/ultralcd.cpp

@@ -250,7 +250,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
@@ -586,7 +588,7 @@ void lcdui_print_Z_coord(void)
     if (custom_message_type == CustomMsg::MeshBedLeveling)
         lcd_puts_P(_N("Z   --- "));
     else
-		lcd_printf_P(_N("Z%6.2f "), current_position[Z_AXIS]);
+		lcd_printf_P(_N("Z%6.2f%c"), current_position[Z_AXIS], axis_known_position[Z_AXIS]?' ':'?');
 }
 
 #ifdef PLANNER_DIAGNOSTICS
@@ -622,21 +624,24 @@ void lcdui_print_feedrate(void)
 // Print percent done in form "USB---%", " SD---%", "   ---%" (7 chars total)
 void lcdui_print_percent_done(void)
 {
-	char sheet[8];
 	const char* src = is_usb_printing?_N("USB"):(IS_SD_PRINTING?_N(" SD"):_N("   "));
 	char per[4];
 	bool num = IS_SD_PRINTING || (PRINTER_ACTIVE && (print_percent_done_normal != PRINT_PERCENT_DONE_INIT));
 	if (!num || heating_status) // either not printing or heating
 	{
-		eeprom_read_block(sheet, EEPROM_Sheets_base->s[eeprom_read_byte(&(EEPROM_Sheets_base->active_sheet))].name, 7);
-		sheet[7] = '\0';
-		lcd_printf_P(PSTR("%s"),sheet);
-	}
-	else
-	{
-		sprintf_P(per, num?_N("%3hhd"):_N("---"), calc_percent_done());
-		lcd_printf_P(_N("%3S%3s%%"), src, per);
+		const int8_t sheetNR = eeprom_read_byte(&(EEPROM_Sheets_base->active_sheet));
+		const int8_t nextSheet = eeprom_next_initialized_sheet(sheetNR);
+		if ((nextSheet >= 0) && (sheetNR != nextSheet))
+		{
+			char sheet[8];
+			eeprom_read_block(sheet, EEPROM_Sheets_base->s[sheetNR].name, 7);
+			sheet[7] = '\0';
+			lcd_printf_P(PSTR("%-7s"),sheet);
+			return; //do not also print the percentage
+		}
 	}
+	sprintf_P(per, num?_N("%3hhd"):_N("---"), calc_percent_done());
+	lcd_printf_P(_N("%3S%3s%%"), src, per);
 }
 
 // Print extruder status (5 chars total)
@@ -1056,7 +1061,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
 	)
 	{
@@ -1467,98 +1471,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
 	{
 
@@ -1662,7 +1574,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)
@@ -6058,7 +5970,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();
@@ -6109,6 +6021,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.
@@ -6771,14 +6685,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
@@ -6917,10 +6832,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)
@@ -7089,6 +7009,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
@@ -7127,8 +7101,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
@@ -7319,42 +7296,66 @@ static void lcd_sd_updir()
 
 void lcd_print_stop()
 {
-//-//
-     if(!card.sdprinting)
-          {
-          SERIAL_ECHOLNRPGM(MSG_OCTOPRINT_CANCEL);   // for Octoprint
-          }
-	saved_printing = false;
-    saved_printing_type = PRINTING_TYPE_NONE;
-	cancel_heatup = true;
-#ifdef MESH_BED_LEVELING
-	mbl.active = false;
-#endif
-	// Stop the stoppers, update the position from the stoppers.
-	if (mesh_bed_leveling_flag == false && homing_flag == false)
-	{
-		planner_abort_hard();
-		// Because the planner_abort_hard() initialized current_position[Z] from the stepper,
-		// Z baystep is no more applied. Reset it.
-		babystep_reset();
-	}
-	// Clean the input command queue.
+    if (!card.sdprinting) {
+        SERIAL_ECHOLNRPGM(MSG_OCTOPRINT_CANCEL);   // for Octoprint
+    }
+
+    CRITICAL_SECTION_START;
+
+    // Clear any saved printing state
+    cancel_saved_printing();
+
+    // Abort the planner/queue/sd
+    planner_abort_hard();
 	cmdqueue_reset();
-	lcd_setstatuspgm(_T(MSG_PRINT_ABORTED));
 	card.sdprinting = false;
 	card.closefile();
+    st_reset_timer();
+
+    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()

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
 

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
 

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

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

Firmware/variants/1_75mm_MK3-EINSy10a-E3Dv6full.h

@@ -630,7 +630,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

Firmware/variants/1_75mm_MK3S-EINSy10a-E3Dv6full.h

@@ -632,7 +632,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