From 404802b5e6e3ebd3a21f936691b7885f11966c26 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sun, 13 Oct 2019 18:49:32 +0200
Subject: [PATCH 01/10] Clear "sdprinting" state only when all SD moves are
 complete

Ensure card.printingHasFinished sees all the planned moves before
clearing sdprinting. To do that, we need to ensure all SD commands
exited the command queue.
---
 Firmware/cmdqueue.cpp | 55 +++++++++++++++++++++++++------------------
 1 file changed, 32 insertions(+), 23 deletions(-)

diff --git a/Firmware/cmdqueue.cpp b/Firmware/cmdqueue.cpp
index 74a8b90a5..e17db42a3 100755
--- a/Firmware/cmdqueue.cpp
+++ b/Firmware/cmdqueue.cpp
@@ -582,30 +582,8 @@ void get_command()
        ((serial_char == '#' || serial_char == ':') && comment_mode == false) ||
        serial_count >= (MAX_CMD_SIZE - 1) || n==-1)
     {
-      if(card.eof()){
-        SERIAL_PROTOCOLLNRPGM(_n("Done printing file"));////MSG_FILE_PRINTED
-        stoptime=_millis();
-        char time[30];
-        unsigned long t=(stoptime-starttime-pause_time)/1000;
-        pause_time = 0;
-        int hours, minutes;
-        minutes=(t/60)%60;
-        hours=t/60/60;
-        save_statistics(total_filament_used, t);
-        sprintf_P(time, PSTR("%i hours %i minutes"),hours, minutes);
-        SERIAL_ECHO_START;
-        SERIAL_ECHOLN(time);
-        lcd_setstatus(time);
-        card.printingHasFinished();
-        card.checkautostart(true);
+      if(card.eof()) break;
 
-        if (farm_mode)
-        {
-            prusa_statistics(6);
-            lcd_commands_type = LcdCommands::FarmModeConfirm;
-        }
-
-      }
       if(serial_char=='#')
         stop_buffering=true;
 
@@ -663,6 +641,37 @@ void get_command()
       else if(!comment_mode) cmdbuffer[bufindw+CMDHDRSIZE+serial_count++] = serial_char;
     }
   }
+  if(card.eof())
+  {
+      // file was fully buffered, but commands might still need to be planned!
+      // do *not* clear sdprinting until all SD commands are consumed to ensure
+      // SD state can be resumed from a saved printing state. sdprinting is only
+      // cleared by printingHasFinished after peforming all remaining moves.
+      if(!cmdqueue_calc_sd_length())
+      {
+          SERIAL_PROTOCOLLNRPGM(_n("Done printing file"));////MSG_FILE_PRINTED
+          stoptime=_millis();
+          char time[30];
+          unsigned long t=(stoptime-starttime-pause_time)/1000;
+          pause_time = 0;
+          int hours, minutes;
+          minutes=(t/60)%60;
+          hours=t/60/60;
+          save_statistics(total_filament_used, t);
+          sprintf_P(time, PSTR("%i hours %i minutes"),hours, minutes);
+          SERIAL_ECHO_START;
+          SERIAL_ECHOLN(time);
+          lcd_setstatus(time);
+          card.printingHasFinished();
+          card.checkautostart(true);
+
+          if (farm_mode)
+          {
+              prusa_statistics(6);
+              lcd_commands_type = LcdCommands::FarmModeConfirm;
+          }
+      }
+  }
 
   #endif //SDSUPPORT
 }

From 0f6b9b40512eca03abbc930ae337a87ddf9323d6 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 15 Oct 2019 21:09:28 +0200
Subject: [PATCH 02/10] Unroll the stack in restore_print_from_ram_and_continue

While handling moves in a recursive plan, such a filament check,
ensure restore_print_from_ram_and_continue unwinds the stack by
aborting early from any call that waits on the planner.

This currently only handles G1 moves, but hard-coded behavior that can
trigger recursive behavior (such as filament change) will probably have
to be checked too.
---
 Firmware/Marlin_main.cpp | 6 ++++++
 Firmware/planner.cpp     | 2 +-
 Firmware/planner.h       | 1 +
 3 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 19d799534..46fef27a0 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -8363,6 +8363,8 @@ void clamp_to_software_endstops(float target[3])
                                  current_position[Z_AXIS] + t * dz,
                                  current_position[E_AXIS] + t * de,
                                  feed_rate, extruder);
+                if (waiting_inside_plan_buffer_line_print_aborted)
+                    return;
             }
         }
         // The rest of the path.
@@ -8390,6 +8392,8 @@ void prepare_move()
      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply*(1./(60.f*100.f)), active_extruder);
 #endif
   }
+  if (waiting_inside_plan_buffer_line_print_aborted)
+      return;
 
   for(int8_t i=0; i < NUM_AXIS; i++) {
     current_position[i] = destination[i];
@@ -10264,10 +10268,12 @@ void restore_print_from_ram_and_continue(float e_move)
 	else {
 		//not sd printing nor usb printing
 	}
+
 	SERIAL_PROTOCOLLNRPGM(MSG_OK); //dummy response because of octoprint is waiting for this
 	lcd_setstatuspgm(_T(WELCOME_MSG));
     saved_printing_type = PRINTING_TYPE_NONE;
 	saved_printing = false;
+    waiting_inside_plan_buffer_line_print_aborted = true; //unroll the stack
 }
 
 void print_world_coordinates()
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index 752d07258..63058d2cf 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -666,8 +666,8 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
 
   // If the buffer is full: good! That means we are well ahead of the robot. 
   // Rest here until there is room in the buffer.
+  waiting_inside_plan_buffer_line_print_aborted = false;
   if (block_buffer_tail == next_buffer_head) {
-      waiting_inside_plan_buffer_line_print_aborted = false;
       do {
           manage_heater(); 
           // Vojtech: Don't disable motors inside the planner!
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 7904c45a5..4977265bf 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -238,6 +238,7 @@ FORCE_INLINE bool planner_queue_full() {
 // wait for the steppers to stop,
 // update planner's current position and the current_position of the front end.
 extern void planner_abort_hard();
+extern bool waiting_inside_plan_buffer_line_print_aborted;
 
 #ifdef PREVENT_DANGEROUS_EXTRUDE
 void set_extrude_min_temp(float temp);

From 5fa7c178b5fa2232c9c2f3e1939611e9ad453f1b Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 15 Oct 2019 21:14:28 +0200
Subject: [PATCH 03/10] Do not set current_position in mesh_plan_buffer_line

This is a task which _has_ to be performed by the caller,
such as prepare_move().
---
 Firmware/Marlin_main.cpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 46fef27a0..6686fba0a 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -8369,10 +8369,6 @@ void clamp_to_software_endstops(float target[3])
         }
         // The rest of the path.
         plan_buffer_line(x, y, z, e, feed_rate, extruder);
-        current_position[X_AXIS] = x;
-        current_position[Y_AXIS] = y;
-        current_position[Z_AXIS] = z;
-        current_position[E_AXIS] = e;
     }
 #endif  // MESH_BED_LEVELING
     

From faa9e925fe38acfc3de13aae35e6227c68fedc1e Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 15 Oct 2019 21:20:18 +0200
Subject: [PATCH 04/10] Replace a few loops with set_current_to_destination()

---
 Firmware/Marlin_main.cpp | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 6686fba0a..3d1945efd 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -8391,9 +8391,7 @@ void prepare_move()
   if (waiting_inside_plan_buffer_line_print_aborted)
       return;
 
-  for(int8_t i=0; i < NUM_AXIS; i++) {
-    current_position[i] = destination[i];
-  }
+  set_current_to_destination();
 }
 
 void prepare_arc_move(char isclockwise) {
@@ -9114,10 +9112,8 @@ void bed_check(float x_dimension, float y_dimension, int x_points_num, int y_poi
 		destination[X_AXIS] = ix * (x_dimension / (x_points_num - 1)) + shift_x;
 		destination[Y_AXIS] = iy * (y_dimension / (y_points_num - 1)) + shift_y;
 
-		mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], XY_AXIS_FEEDRATE/6, active_extruder);
-		for(int8_t i=0; i < NUM_AXIS; i++) {
-			current_position[i] = destination[i];
-		}
+        mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], XY_AXIS_FEEDRATE/6, active_extruder);
+        set_current_to_destination();
 		st_synchronize();
 
 	//	printf_P(PSTR("X = %f; Y= %f \n"), current_position[X_AXIS], current_position[Y_AXIS]);

From 4268c2fdae35c97d732fa4318a60c342e5547065 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 15 Oct 2019 21:23:50 +0200
Subject: [PATCH 05/10] Fix recovery from relative/chunked moves

When starting to replay existing USB/SD commands from a recovery state,
an immediate relative move needs to compensate for a previously
interrupted move. This is almost the norm for the E axis.

Instead of saving the relative status of the move (which needs to
account for the world2machine conversion and is not always available on
a chunked move split by MBL) save directly the calculated target
position for the move in the original plan, which is easy to replay.
---
 Firmware/Marlin_main.cpp | 61 ++++++++++++++++++++++++++++++++++------
 Firmware/eeprom.h        |  3 +-
 Firmware/planner.cpp     | 31 +++++++++++++-------
 Firmware/planner.h       |  5 ++--
 4 files changed, 78 insertions(+), 22 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 3d1945efd..4877f7a8b 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -309,6 +309,8 @@ bool no_response = false;
 uint8_t important_status;
 uint8_t saved_filament_type;
 
+#define SAVED_TARGET_UNSET (X_MIN_POS-1)
+float saved_target[NUM_AXIS] = {SAVED_TARGET_UNSET, 0, 0, 0};
 
 // save/restore printing in case that mmu was not responding 
 bool mmu_print_saved = false;
@@ -4043,8 +4045,19 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
 
         #endif
 
+            get_coordinates(); // For X Y Z E F
+
+            // When recovering from a previous print move, restore the originally
+            // calculated target position on the first USB/SD command. This accounts
+            // properly for relative moves
+            if ((saved_target[0] != SAVED_TARGET_UNSET) &&
+                ((CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_SDCARD) ||
+                 (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_USB_WITH_LINENR)))
+            {
+                memcpy(destination, saved_target, sizeof(destination));
+                saved_target[0] = SAVED_TARGET_UNSET;
+            }
 
-        get_coordinates(); // For X Y Z E F
 		if (total_filament_used > ((current_position[E_AXIS] - destination[E_AXIS]) * 100)) { //protection against total_filament_used overflow
 			total_filament_used = total_filament_used + ((destination[E_AXIS] - current_position[E_AXIS]) * 100);
 		}
@@ -8339,30 +8352,37 @@ void clamp_to_software_endstops(float target[3])
 }
 
 #ifdef MESH_BED_LEVELING
-    void mesh_plan_buffer_line(const float &x, const float &y, const float &z, const float &e, const float &feed_rate, const uint8_t extruder) {
+void mesh_plan_buffer_line(const float &x, const float &y, const float &z, const float &e, const float &feed_rate, const uint8_t extruder) {
         float dx = x - current_position[X_AXIS];
         float dy = y - current_position[Y_AXIS];
-        float dz = z - current_position[Z_AXIS];
         int n_segments = 0;
-		
+
         if (mbl.active) {
             float len = abs(dx) + abs(dy);
             if (len > 0)
                 // Split to 3cm segments or shorter.
                 n_segments = int(ceil(len / 30.f));
         }
-        
+
         if (n_segments > 1) {
+            // In a multi-segment move explicitly set the final target in the plan
+            // as the move will be recalculated in it's entirety
+            float gcode_target[NUM_AXIS];
+            gcode_target[X_AXIS] = x;
+            gcode_target[Y_AXIS] = y;
+            gcode_target[Z_AXIS] = z;
+            gcode_target[E_AXIS] = e;
+
+            float dz = z - current_position[Z_AXIS];
             float de = e - current_position[E_AXIS];
+
             for (int i = 1; i < n_segments; ++ i) {
                 float t = float(i) / float(n_segments);
-                if (saved_printing || (mbl.active == false)) return;
-                plan_buffer_line(
-                                 current_position[X_AXIS] + t * dx,
+                plan_buffer_line(current_position[X_AXIS] + t * dx,
                                  current_position[Y_AXIS] + t * dy,
                                  current_position[Z_AXIS] + t * dz,
                                  current_position[E_AXIS] + t * de,
-                                 feed_rate, extruder);
+                                 feed_rate, extruder, gcode_target);
                 if (waiting_inside_plan_buffer_line_print_aborted)
                     return;
             }
@@ -9602,6 +9622,12 @@ void uvlo_()
     // Backup the feedrate in mm/min.
     int feedrate_bckp = blocks_queued() ? (block_buffer[block_buffer_tail].nominal_speed * 60.f) : feedrate;
 
+    // save the original target position of the current move
+    if (blocks_queued())
+        memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
+    else
+        saved_target[0] = SAVED_TARGET_UNSET;
+
     // After this call, the planner queue is emptied and the current_position is set to a current logical coordinate.
     // The logical coordinate will likely differ from the machine coordinate if the skew calibration and mesh bed leveling
     // are in action.
@@ -9679,6 +9705,11 @@ 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]);
 
     // Finaly store the "power outage" flag.
 	if(sd_print) eeprom_update_byte((uint8_t*)EEPROM_UVLO, 1);
@@ -9927,6 +9958,12 @@ void recover_machine_state_after_power_panic(bool bTiny)
 #endif
 #endif
   extrudemultiply = (int)eeprom_read_word((uint16_t*)(EEPROM_EXTRUDEMULTIPLY));
+
+  // 9) Recover the saved target
+  saved_target[X_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_SAVED_TARGET+0*4));
+  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));
 }
 
 void restore_print_from_eeprom() {
@@ -10143,6 +10180,12 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
   saved_feedrate2 = blocks_queued() ? (block_buffer[block_buffer_tail].nominal_speed * 60.f) : feedrate;
 #endif
 
+  // save the original target position of the current move
+  if (blocks_queued())
+      memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
+  else
+      saved_target[0] = SAVED_TARGET_UNSET;
+
 	planner_abort_hard(); //abort printing
 	memcpy(saved_pos, current_position, sizeof(saved_pos));
 	saved_active_extruder = active_extruder; //save active_extruder
diff --git a/Firmware/eeprom.h b/Firmware/eeprom.h
index 0949e4110..82852220f 100644
--- a/Firmware/eeprom.h
+++ b/Firmware/eeprom.h
@@ -204,9 +204,10 @@ static Sheets * const EEPROM_Sheets_base = (Sheets*)(EEPROM_SHEETS_BASE);
 #define EEPROM_FSENSOR_PCB (EEPROM_SHEETS_BASE-1) // uint8
 #define EEPROM_FSENSOR_ACTION_NA (EEPROM_FSENSOR_PCB-1) // uint8
 
+#define EEPROM_UVLO_SAVED_TARGET (EEPROM_FSENSOR_ACTION_NA - 4*4) // 4 x float for saved target for all axes
 
 //This is supposed to point to last item to allow EEPROM overrun check. Please update when adding new items.
-#define EEPROM_LAST_ITEM EEPROM_SHEETS_BASE
+#define EEPROM_LAST_ITEM EEPROM_UVLO_SAVED_TARGET
 // !!!!!
 // !!!!! this is end of EEPROM section ... all updates MUST BE inserted before this mark !!!!!
 // !!!!!
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index 63058d2cf..c1285aed6 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -659,7 +659,7 @@ float junction_deviation = 0.1;
 // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
 // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, uint8_t extruder)
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, uint8_t extruder, const float* gcode_target)
 {
     // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
@@ -687,6 +687,26 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
   planner_update_queue_min_counter();
 #endif /* PLANNER_DIAGNOSTICS */
 
+  // Prepare to set up new block
+  block_t *block = &block_buffer[block_buffer_head];
+
+  // Set sdlen for calculating sd position
+  block->sdlen = 0;
+
+  // Mark block as not busy (Not executed by the stepper interrupt, could be still tinkered with.)
+  block->busy = false;
+
+  // Save original destination of the move
+  if (gcode_target)
+      memcpy(block->gcode_target, gcode_target, sizeof(block_t::gcode_target));
+  else
+  {
+      block->gcode_target[X_AXIS] = x;
+      block->gcode_target[Y_AXIS] = y;
+      block->gcode_target[Z_AXIS] = z;
+      block->gcode_target[E_AXIS] = e;
+  }
+
 #ifdef ENABLE_AUTO_BED_LEVELING
   apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #endif // ENABLE_AUTO_BED_LEVELING
@@ -786,15 +806,6 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
   }
   #endif
 
-  // Prepare to set up new block
-  block_t *block = &block_buffer[block_buffer_head];
-
-  // Set sdlen for calculating sd position
-  block->sdlen = 0;
-
-  // Mark block as not busy (Not executed by the stepper interrupt, could be still tinkered with.)
-  block->busy = false;
-
   // Number of steps for each axis
 #ifndef COREXY
 // default non-h-bot planning
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 4977265bf..d4c6bc2b4 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -116,7 +116,8 @@ typedef struct {
   unsigned long abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float
 #endif
 
-  uint16_t sdlen;
+  float gcode_target[NUM_AXIS];     // Target (abs mm) of the original Gcode instruction
+  uint16_t sdlen;                   // Length of the Gcode instruction
 } block_t;
 
 #ifdef LIN_ADVANCE
@@ -147,7 +148,7 @@ vector_3 plan_get_position();
 /// The performance penalty is negligible, since these planned lines are usually maintenance moves with the extruder.
 void plan_buffer_line_curposXYZE(float feed_rate, uint8_t extruder);
 
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, uint8_t extruder);
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, uint8_t extruder, const float* gcode_target = NULL);
 //void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
 #endif // ENABLE_AUTO_BED_LEVELING
 

From 6ecff003b78e276ae775e47279559b45b8584d75 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Wed, 16 Oct 2019 18:47:18 +0200
Subject: [PATCH 06/10] Fix M600 insertion by the filament sensor

Remove incorrect usage of stop_and_save_print combined with the
fsensor_recovert internal instruction which would result in a
broken sequence of events and/or broken stack.

Re-use the now safe stop/recover functions in the same spot
(fsensor_checkpoint_stream) to effectively cut a hole in the current
gcode stream to insert an M600 instruction, which removes all
recursive behavior without the need of extra state variables.
---
 Firmware/Marlin_main.cpp |  9 +-------
 Firmware/fsensor.cpp     | 46 +++++++++++++++++-----------------------
 Firmware/fsensor.h       |  2 ++
 Firmware/mmu.cpp         |  3 +--
 4 files changed, 24 insertions(+), 36 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 4877f7a8b..5afeb525b 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -3657,7 +3657,7 @@ void process_commands()
     
     Set of internal PRUSA commands
       
-          PRUSA [ Ping | PRN | FAN | fn | thx | uvlo | fsensor_recover | MMURES | RESET | fv | M28 | SN | Fir | Rev | Lang | Lz | Beat | FR ]
+          PRUSA [ Ping | PRN | FAN | fn | thx | uvlo | MMURES | RESET | fv | M28 | SN | Fir | Rev | Lang | Lz | Beat | FR ]
       
       - `Ping` 
       - `PRN` - Prints revision of the printer
@@ -3665,7 +3665,6 @@ void process_commands()
       - `fn` - Prints farm no.
       - `thx` 
       - `uvlo` 
-      - `fsensor_recover` - Filament sensor recover - restore print and continue
       - `MMURES` - Reset MMU
       - `RESET` - (Careful!)
       - `fv`  - ?
@@ -3715,12 +3714,6 @@ void process_commands()
                eeprom_update_byte((uint8_t*)EEPROM_UVLO,0); 
                enquecommand_P(PSTR("M24")); 
 		}	
-#ifdef FILAMENT_SENSOR
-		else if (code_seen("fsensor_recover")) // PRUSA fsensor_recover
-		{
-               fsensor_restore_print_and_continue();
-		}	
-#endif //FILAMENT_SENSOR
 		else if (code_seen("MMURES")) // PRUSA MMURES
 		{
 			mmu_reset();
diff --git a/Firmware/fsensor.cpp b/Firmware/fsensor.cpp
index c01274fb3..1c28d025f 100755
--- a/Firmware/fsensor.cpp
+++ b/Firmware/fsensor.cpp
@@ -57,15 +57,8 @@ bool fsensor_enabled = true;
 bool fsensor_watch_runout = true;
 //! not responding - is set if any communication error occurred during initialization or readout
 bool fsensor_not_responding = false;
-//! printing saved
-bool fsensor_printing_saved = false;
 //! enable/disable quality meassurement
 bool fsensor_oq_meassure_enabled = false;
-//! as explained in the CHECK_FSENSOR macro: this flag is set to true when fsensor posts
-//! the M600 into the command queue, which elliminates the hazard of having posted multiple M600's
-//! before the first one gets read and started processing.
-//! Btw., the IR fsensor could do up to 6 posts before the command queue managed to start processing the first M600 ;)
-static bool fsensor_m600_enqueued = false;
 
 //! number of errors, updated in ISR
 uint8_t fsensor_err_cnt = 0;
@@ -137,12 +130,19 @@ void fsensor_stop_and_save_print(void)
 void fsensor_restore_print_and_continue(void)
 {
     printf_P(PSTR("fsensor_restore_print_and_continue\n"));
-	fsensor_watch_runout = true;
 	fsensor_err_cnt = 0;
-	fsensor_m600_enqueued = false;
     restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
 }
 
+// fsensor_checkpoint_print cuts the current print job at the current position,
+// allowing new instructions to be inserted in the middle
+void fsensor_checkpoint_print(void)
+{
+    printf_P(PSTR("fsensor_checkpoint_print\n"));
+    stop_and_save_print_to_ram(0, 0);
+    restore_print_from_ram_and_continue(0);
+}
+
 void fsensor_init(void)
 {
 #ifdef PAT9125
@@ -565,8 +565,6 @@ void fsensor_enque_M600(){
 	printf_P(PSTR("fsensor_update - M600\n"));
 	eeprom_update_byte((uint8_t*)EEPROM_FERROR_COUNT, eeprom_read_byte((uint8_t*)EEPROM_FERROR_COUNT) + 1);
 	eeprom_update_word((uint16_t*)EEPROM_FERROR_COUNT_TOT, eeprom_read_word((uint16_t*)EEPROM_FERROR_COUNT_TOT) + 1);
-	enquecommand_front_P(PSTR("PRUSA fsensor_recover"));
-	fsensor_m600_enqueued = true;
 	enquecommand_front_P((PSTR("M600")));
 }
 
@@ -578,7 +576,7 @@ void fsensor_enque_M600(){
 void fsensor_update(void)
 {
 #ifdef PAT9125
-		if (fsensor_enabled && fsensor_watch_runout && (fsensor_err_cnt > FSENSOR_ERR_MAX) && ( ! fsensor_m600_enqueued) )
+		if (fsensor_enabled && fsensor_watch_runout && (fsensor_err_cnt > FSENSOR_ERR_MAX))
 		{
 			bool autoload_enabled_tmp = fsensor_autoload_enabled;
 			fsensor_autoload_enabled = false;
@@ -611,22 +609,18 @@ void fsensor_update(void)
 			err |= (fsensor_oq_er_sum > 2);
 			err |= (fsensor_oq_yd_sum < (4 * FSENSOR_OQ_MIN_YD));
 
-			if (!err)
-			{
-				printf_P(PSTR("fsensor_err_cnt = 0\n"));
-				fsensor_restore_print_and_continue();
-			}
-			else
-			{
-				fsensor_enque_M600();
-				fsensor_watch_runout = false;
-			}
+            fsensor_restore_print_and_continue();
 			fsensor_autoload_enabled = autoload_enabled_tmp;
 			fsensor_oq_meassure_enabled = oq_meassure_enabled_tmp;
+
+			if (!err)
+				printf_P(PSTR("fsensor_err_cnt = 0\n"));
+			else
+				fsensor_enque_M600();
 		}
 #else //PAT9125
-		if (CHECK_FSENSOR && fsensor_enabled && ir_sensor_detected && ( ! fsensor_m600_enqueued) )
-          {
+		if (CHECK_FSENSOR && fsensor_enabled && ir_sensor_detected)
+        {
                if(digitalRead(IR_SENSOR_PIN))
                {                                  // IR_SENSOR_PIN ~ H
 #if IR_SENSOR_ANALOG
@@ -670,8 +664,8 @@ void fsensor_update(void)
                               else
                               {
 #endif //IR_SENSOR_ANALOG
-                                   fsensor_stop_and_save_print();
-                                   fsensor_enque_M600();
+                                  fsensor_checkpoint_print();
+                                  fsensor_enque_M600();
 #if IR_SENSOR_ANALOG
                               }
                          }
diff --git a/Firmware/fsensor.h b/Firmware/fsensor.h
index 48ad5fba0..aa4963afa 100755
--- a/Firmware/fsensor.h
+++ b/Firmware/fsensor.h
@@ -21,6 +21,8 @@ extern bool fsensor_oq_meassure_enabled;
 extern void fsensor_stop_and_save_print(void);
 //! restore print - restore position and heatup to original temperature
 extern void fsensor_restore_print_and_continue(void);
+//! split the current gcode stream to insert new instructions
+extern void fsensor_checkpoint_print(void);
 //! @}
 
 //! initialize
diff --git a/Firmware/mmu.cpp b/Firmware/mmu.cpp
index c9ca9cb65..f1515d988 100755
--- a/Firmware/mmu.cpp
+++ b/Firmware/mmu.cpp
@@ -382,8 +382,7 @@ void mmu_loop(void)
 			FDEBUG_PRINTF_P(PSTR("MMU => '%dok'\n"), mmu_finda);
 			//printf_P(PSTR("Eact: %d\n"), int(e_active()));
 			if (!mmu_finda && CHECK_FSENSOR && fsensor_enabled) {
-				fsensor_stop_and_save_print();
-				enquecommand_front_P(PSTR("PRUSA fsensor_recover")); //then recover
+				fsensor_checkpoint_print();
 				ad_markDepleted(mmu_extruder);
 				if (lcd_autoDepleteEnabled() && !ad_allDepleted())
 				{

From 4f0af648fb95903942d0ebf1cad9d1d214048084 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sat, 19 Oct 2019 18:48:02 +0200
Subject: [PATCH 07/10] Save/restore feedmultiply independently of the feedrate

---
 Firmware/Marlin_main.cpp | 34 ++++++++++++++++++----------------
 Firmware/eeprom.h        |  3 ++-
 2 files changed, 20 insertions(+), 17 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 5afeb525b..00e0ae993 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -375,8 +375,8 @@ bool saved_printing = false; //!< Print is paused and saved in RAM
 static uint32_t saved_sdpos = 0; //!< SD card position, or line number in case of USB printing
 uint8_t saved_printing_type = PRINTING_TYPE_SD;
 static float saved_pos[4] = { 0, 0, 0, 0 };
-//! Feedrate hopefully derived from an active block of the planner at the time the print has been canceled, in mm/min.
 static float saved_feedrate2 = 0;
+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;
@@ -9612,9 +9612,6 @@ void uvlo_()
       if (sd_position < 0) sd_position = 0;
     }
 
-    // Backup the feedrate in mm/min.
-    int feedrate_bckp = blocks_queued() ? (block_buffer[block_buffer_tail].nominal_speed * 60.f) : feedrate;
-
     // save the original target position of the current move
     if (blocks_queued())
         memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
@@ -9686,7 +9683,9 @@ void uvlo_()
     eeprom_update_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 4), current_position[Y_AXIS]);
     eeprom_update_float((float*)EEPROM_UVLO_CURRENT_POSITION_Z , current_position[Z_AXIS]);
     // Store the current feed rate, temperatures, fan speed and extruder multipliers (flow rates)
-    EEPROM_save_B(EEPROM_UVLO_FEEDRATE, &feedrate_bckp);
+    int i_feedrate = feedrate;
+    EEPROM_save_B(EEPROM_UVLO_FEEDRATE, &i_feedrate);
+    EEPROM_save_B(EEPROM_UVLO_FEEDMULTIPLY, &feedmultiply);
     eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_HOTEND, target_temperature[active_extruder]);
     eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_BED, target_temperature_bed);
     eeprom_update_byte((uint8_t*)EEPROM_UVLO_FAN_SPEED, fanSpeed);
@@ -9961,6 +9960,7 @@ void recover_machine_state_after_power_panic(bool bTiny)
 
 void restore_print_from_eeprom() {
 	int feedrate_rec;
+	int feedmultiply_rec;
 	uint8_t fan_speed_rec;
 	char cmd[30];
 	char filename[13];
@@ -9969,8 +9969,11 @@ void restore_print_from_eeprom() {
 
 	fan_speed_rec = eeprom_read_byte((uint8_t*)EEPROM_UVLO_FAN_SPEED);
 	EEPROM_read_B(EEPROM_UVLO_FEEDRATE, &feedrate_rec);
+	EEPROM_read_B(EEPROM_UVLO_FEEDMULTIPLY, &feedmultiply_rec);
 	SERIAL_ECHOPGM("Feedrate:");
-	MYSERIAL.println(feedrate_rec);
+	MYSERIAL.print(feedrate_rec);
+	SERIAL_ECHOPGM(", feedmultiply:");
+	MYSERIAL.println(feedmultiply_rec);
 
 	depth = eeprom_read_byte((uint8_t*)EEPROM_DIR_DEPTH);
 	
@@ -10011,9 +10014,11 @@ void restore_print_from_eeprom() {
 	enquecommand(cmd);
   // Unretract.
 	enquecommand_P(PSTR("G1 E"  STRINGIFY(2*default_retraction)" F480"));
-  // Set the feedrate saved at the power panic.
+  // Set the feedrates saved at the power panic.
 	sprintf_P(cmd, PSTR("G1 F%d"), feedrate_rec);
 	enquecommand(cmd);
+	sprintf_P(cmd, PSTR("M220 S%d"), feedmultiply_rec);
+	enquecommand(cmd);
 	if (eeprom_read_byte((uint8_t*)EEPROM_UVLO_E_ABS))
 	{
 	  enquecommand_P(PSTR("M82")); //E axis abslute mode
@@ -10165,14 +10170,6 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
   }
 #endif
 
-#if 0
-  saved_feedrate2 = feedrate; //save feedrate
-#else
-  // Try to deduce the feedrate from the first block of the planner.
-  // Speed is in mm/min.
-  saved_feedrate2 = blocks_queued() ? (block_buffer[block_buffer_tail].nominal_speed * 60.f) : feedrate;
-#endif
-
   // save the original target position of the current move
   if (blocks_queued())
       memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
@@ -10181,6 +10178,8 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
 
 	planner_abort_hard(); //abort printing
 	memcpy(saved_pos, current_position, sizeof(saved_pos));
+    saved_feedrate2 = feedrate; //save feedrate
+    saved_feedmultiply2 = feedmultiply; //save feedmultiply
 	saved_active_extruder = active_extruder; //save active_extruder
 	saved_extruder_temperature = degTargetHotend(active_extruder);
 
@@ -10258,7 +10257,6 @@ void restore_print_from_ram_and_continue(float e_move)
 		wait_for_heater(_millis(), saved_active_extruder);
 		heating_status = 2;
 	}
-	feedrate = saved_feedrate2; //restore feedrate
 	axis_relative_modes[E_AXIS] = saved_extruder_relative_mode;
 	float e = saved_pos[E_AXIS] - e_move;
 	plan_set_e_position(e);
@@ -10281,6 +10279,10 @@ void restore_print_from_ram_and_continue(float e_move)
     fans_check_enabled = true;
   #endif
 
+    // restore original feedrate/feedmultiply _after_ restoring the extruder position
+	feedrate = saved_feedrate2;
+	feedmultiply = saved_feedmultiply2;
+
 	memcpy(current_position, saved_pos, sizeof(saved_pos));
 	memcpy(destination, current_position, sizeof(destination));
 	if (saved_printing_type == PRINTING_TYPE_SD) { //was sd printing
diff --git a/Firmware/eeprom.h b/Firmware/eeprom.h
index 82852220f..3f1e61419 100644
--- a/Firmware/eeprom.h
+++ b/Firmware/eeprom.h
@@ -205,9 +205,10 @@ static Sheets * const EEPROM_Sheets_base = (Sheets*)(EEPROM_SHEETS_BASE);
 #define EEPROM_FSENSOR_ACTION_NA (EEPROM_FSENSOR_PCB-1) // uint8
 
 #define EEPROM_UVLO_SAVED_TARGET (EEPROM_FSENSOR_ACTION_NA - 4*4) // 4 x float for saved target for all axes
+#define EEPROM_UVLO_FEEDMULTIPLY (EEPROM_UVLO_SAVED_TARGET - 2) // uint16_t for feedmultiply
 
 //This is supposed to point to last item to allow EEPROM overrun check. Please update when adding new items.
-#define EEPROM_LAST_ITEM EEPROM_UVLO_SAVED_TARGET
+#define EEPROM_LAST_ITEM EEPROM_UVLO_FEEDMULTIPLY
 // !!!!!
 // !!!!! this is end of EEPROM section ... all updates MUST BE inserted before this mark !!!!!
 // !!!!!

From 17176c1df21574c53c2fefc0b1455f9eb3f5e625 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sat, 19 Oct 2019 21:20:38 +0200
Subject: [PATCH 08/10] Save/restore the default feedrate correctly

Since the global feedrate can be similarly modified for moves ahead of
time, save the original feedrate in the planner as we do for
gcode_target.

This avoids having to undo feedmultiply (and machine limits!) from
"nominal_speed" as previously done.

Thanks @leptun
---
 Firmware/Marlin.h        |  1 +
 Firmware/Marlin_main.cpp | 31 +++++++++++++++++++++++++------
 Firmware/planner.cpp     |  9 ++++++---
 Firmware/planner.h       |  2 ++
 4 files changed, 34 insertions(+), 9 deletions(-)

diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 8a396dac1..d9078ada2 100755
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -296,6 +296,7 @@ void setPwmFrequency(uint8_t pin, int val);
 extern bool fans_check_enabled;
 extern float homing_feedrate[];
 extern bool axis_relative_modes[];
+extern float feedrate;
 extern int feedmultiply;
 extern int extrudemultiply; // Sets extrude multiply factor (in percent) for all extruders
 extern int extruder_multiply[EXTRUDERS]; // sets extrude multiply factor (in percent) for each extruder individually
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 00e0ae993..6e33a3974 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -331,7 +331,15 @@ float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};
 
 // For tracing an arc
 static float offset[3] = {0.0, 0.0, 0.0};
-static float feedrate = 1500.0, next_feedrate, saved_feedrate;
+
+// Current feedrate
+float feedrate = 1500.0;
+
+// Feedrate for the next move
+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.
@@ -9612,11 +9620,18 @@ void uvlo_()
       if (sd_position < 0) sd_position = 0;
     }
 
-    // save the original target position of the current move
+    // save the global state at planning time
+    int feedrate_bckp;
     if (blocks_queued())
+    {
         memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
+        feedrate_bckp = current_block->gcode_feedrate;
+    }
     else
+    {
         saved_target[0] = SAVED_TARGET_UNSET;
+        feedrate_bckp = feedrate;
+    }
 
     // After this call, the planner queue is emptied and the current_position is set to a current logical coordinate.
     // The logical coordinate will likely differ from the machine coordinate if the skew calibration and mesh bed leveling
@@ -9683,8 +9698,7 @@ void uvlo_()
     eeprom_update_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 4), current_position[Y_AXIS]);
     eeprom_update_float((float*)EEPROM_UVLO_CURRENT_POSITION_Z , current_position[Z_AXIS]);
     // Store the current feed rate, temperatures, fan speed and extruder multipliers (flow rates)
-    int i_feedrate = feedrate;
-    EEPROM_save_B(EEPROM_UVLO_FEEDRATE, &i_feedrate);
+    EEPROM_save_B(EEPROM_UVLO_FEEDRATE, &feedrate_bckp);
     EEPROM_save_B(EEPROM_UVLO_FEEDMULTIPLY, &feedmultiply);
     eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_HOTEND, target_temperature[active_extruder]);
     eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_BED, target_temperature_bed);
@@ -10170,15 +10184,20 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
   }
 #endif
 
-  // save the original target position of the current move
+  // save the global state at planning time
   if (blocks_queued())
+  {
       memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
+      saved_feedrate2 = current_block->gcode_feedrate;
+  }
   else
+  {
       saved_target[0] = SAVED_TARGET_UNSET;
+      saved_feedrate2 = feedrate;
+  }
 
 	planner_abort_hard(); //abort printing
 	memcpy(saved_pos, current_position, sizeof(saved_pos));
-    saved_feedrate2 = feedrate; //save feedrate
     saved_feedmultiply2 = feedmultiply; //save feedmultiply
 	saved_active_extruder = active_extruder; //save active_extruder
 	saved_extruder_temperature = degTargetHotend(active_extruder);
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index c1285aed6..0ccf54f83 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -690,12 +690,12 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
   // Prepare to set up new block
   block_t *block = &block_buffer[block_buffer_head];
 
-  // Set sdlen for calculating sd position
-  block->sdlen = 0;
-
   // Mark block as not busy (Not executed by the stepper interrupt, could be still tinkered with.)
   block->busy = false;
 
+  // Set sdlen for calculating sd position
+  block->sdlen = 0;
+
   // Save original destination of the move
   if (gcode_target)
       memcpy(block->gcode_target, gcode_target, sizeof(block_t::gcode_target));
@@ -707,6 +707,9 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
       block->gcode_target[E_AXIS] = e;
   }
 
+  // Save the global feedrate at scheduling time
+  block->gcode_feedrate = feedrate;
+
 #ifdef ENABLE_AUTO_BED_LEVELING
   apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #endif // ENABLE_AUTO_BED_LEVELING
diff --git a/Firmware/planner.h b/Firmware/planner.h
index d4c6bc2b4..27541e5f0 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -116,7 +116,9 @@ typedef struct {
   unsigned long abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float
 #endif
 
+  // Save/recovery state data
   float gcode_target[NUM_AXIS];     // Target (abs mm) of the original Gcode instruction
+  float gcode_feedrate;             // Original feedrate
   uint16_t sdlen;                   // Length of the Gcode instruction
 } block_t;
 

From 18eaf21baf38ec6bff47882af4b1c664f4b9e935 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sat, 19 Oct 2019 21:45:50 +0200
Subject: [PATCH 09/10] Halve memory usage of saved feedrate

Truncate the saved feedrate to an uint16_t.
This is more than sufficient for recovery.
---
 Firmware/Marlin_main.cpp | 8 ++++----
 Firmware/eeprom.h        | 2 +-
 Firmware/planner.h       | 2 +-
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 6e33a3974..a312f3e52 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -383,7 +383,7 @@ bool saved_printing = false; //!< Print is paused and saved in RAM
 static uint32_t saved_sdpos = 0; //!< SD card position, or line number in case of USB printing
 uint8_t saved_printing_type = PRINTING_TYPE_SD;
 static float saved_pos[4] = { 0, 0, 0, 0 };
-static float saved_feedrate2 = 0;
+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
@@ -9621,7 +9621,7 @@ void uvlo_()
     }
 
     // save the global state at planning time
-    int feedrate_bckp;
+    uint16_t feedrate_bckp;
     if (blocks_queued())
     {
         memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
@@ -9698,7 +9698,7 @@ void uvlo_()
     eeprom_update_float((float*)(EEPROM_UVLO_CURRENT_POSITION + 4), current_position[Y_AXIS]);
     eeprom_update_float((float*)EEPROM_UVLO_CURRENT_POSITION_Z , current_position[Z_AXIS]);
     // Store the current feed rate, temperatures, fan speed and extruder multipliers (flow rates)
-    EEPROM_save_B(EEPROM_UVLO_FEEDRATE, &feedrate_bckp);
+	eeprom_update_word((uint16_t*)EEPROM_UVLO_FEEDRATE, feedrate_bckp);
     EEPROM_save_B(EEPROM_UVLO_FEEDMULTIPLY, &feedmultiply);
     eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_HOTEND, target_temperature[active_extruder]);
     eeprom_update_byte((uint8_t*)EEPROM_UVLO_TARGET_BED, target_temperature_bed);
@@ -9982,7 +9982,7 @@ void restore_print_from_eeprom() {
 	char dir_name[9];
 
 	fan_speed_rec = eeprom_read_byte((uint8_t*)EEPROM_UVLO_FAN_SPEED);
-	EEPROM_read_B(EEPROM_UVLO_FEEDRATE, &feedrate_rec);
+    feedrate_rec = eeprom_read_word((uint16_t*)EEPROM_UVLO_FEEDRATE);
 	EEPROM_read_B(EEPROM_UVLO_FEEDMULTIPLY, &feedmultiply_rec);
 	SERIAL_ECHOPGM("Feedrate:");
 	MYSERIAL.print(feedrate_rec);
diff --git a/Firmware/eeprom.h b/Firmware/eeprom.h
index 3f1e61419..12a31d3cc 100644
--- a/Firmware/eeprom.h
+++ b/Firmware/eeprom.h
@@ -74,7 +74,7 @@ static_assert(sizeof(Sheets) == EEPROM_SHEETS_SIZEOF, "Sizeof(Sheets) is not EEP
 #define EEPROM_UVLO_CURRENT_POSITION_Z	(EEPROM_FILE_POSITION - 4) //float for current position in Z
 #define EEPROM_UVLO_TARGET_HOTEND		(EEPROM_UVLO_CURRENT_POSITION_Z - 1)
 #define EEPROM_UVLO_TARGET_BED			(EEPROM_UVLO_TARGET_HOTEND - 1)
-#define EEPROM_UVLO_FEEDRATE			(EEPROM_UVLO_TARGET_BED - 2)
+#define EEPROM_UVLO_FEEDRATE			(EEPROM_UVLO_TARGET_BED - 2) //uint16_t
 #define EEPROM_UVLO_FAN_SPEED			(EEPROM_UVLO_FEEDRATE - 1) 
 #define EEPROM_FAN_CHECK_ENABLED		(EEPROM_UVLO_FAN_SPEED - 1)
 #define EEPROM_UVLO_MESH_BED_LEVELING     (EEPROM_FAN_CHECK_ENABLED - 9*2)
diff --git a/Firmware/planner.h b/Firmware/planner.h
index 27541e5f0..0d0cb41ab 100644
--- a/Firmware/planner.h
+++ b/Firmware/planner.h
@@ -118,7 +118,7 @@ typedef struct {
 
   // Save/recovery state data
   float gcode_target[NUM_AXIS];     // Target (abs mm) of the original Gcode instruction
-  float gcode_feedrate;             // Original feedrate
+  uint16_t gcode_feedrate;          // Default and/or move feedrate
   uint16_t sdlen;                   // Length of the Gcode instruction
 } block_t;
 

From dbe2ed41500863b6948f5a0154700feb76dafdb7 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sun, 20 Oct 2019 17:34:54 +0200
Subject: [PATCH 10/10] Fix pause/resume when using M25/M601

Remove the conflicting and mostly useless card.paused flag (the printing
is either paused, or not) and switch to isPrintPaused only which
accounts for both cases (SD/USB) correctly.

Fix M27/getStatus to show the current real status of the SD print.

Synchronize the queue on M601, as required to precisely pause the print
at the correct instruction.

Alias M25 to M601, which when combined with PR #1899 fixes issue #1614.

Guard against incorrect usage in M601, M602 and M603.
---
 Firmware/Marlin.h              |  2 +-
 Firmware/Marlin_main.cpp       | 36 ++++++++++++---------
 Firmware/cardreader.cpp        | 57 ++++++++++++++--------------------
 Firmware/cardreader.h          |  2 --
 Firmware/ultralcd.cpp          |  2 +-
 Tests/PrusaStatistics_test.cpp |  5 ++-
 6 files changed, 48 insertions(+), 56 deletions(-)

diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index d9078ada2..7dedc8a4b 100755
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -396,7 +396,7 @@ extern uint16_t gcode_in_progress;
 extern LongTimer safetyTimer;
 
 #define PRINT_PERCENT_DONE_INIT   0xff
-#define PRINTER_ACTIVE (IS_SD_PRINTING || is_usb_printing || isPrintPaused || (custom_message_type == CustomMsg::TempCal) || saved_printing || (lcd_commands_type == LcdCommands::Layer1Cal) || card.paused || mmu_print_saved)
+#define PRINTER_ACTIVE (IS_SD_PRINTING || is_usb_printing || isPrintPaused || (custom_message_type == CustomMsg::TempCal) || saved_printing || (lcd_commands_type == LcdCommands::Layer1Cal) || mmu_print_saved)
 
 //! Beware - mcode_in_progress is set as soon as the command gets really processed,
 //! which is not the same as posting the M600 command into the command queue
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index a312f3e52..fe94fab5b 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -5377,21 +5377,19 @@ if(eSoundMode!=e_SOUND_MODE_SILENT)
       card.openFile(strchr_pointer + 4,true);
       break;
 
-    //! ### M24 - Start SD print
+    //! ### M24 - Start/resume SD print
     // ----------------------------------
     case 24:
-	  if (!card.paused) 
-		failstats_reset_print();
-      card.startFileprint();
-      starttime=_millis();
+	  if (isPrintPaused)
+          lcd_resume_print();
+      else
+      {
+          failstats_reset_print();
+          card.startFileprint();
+          starttime=_millis();
+      }
 	  break;
 
-    //! ### M25 - Pause SD print
-    // ----------------------------------
-    case 25:
-      card.pauseSDPrint();
-      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.
@@ -7246,26 +7244,34 @@ Sigma_Exit:
     break;
     #endif //FILAMENTCHANGEENABLE
 
+  //! ### M25 - Pause SD print
   //! ### M601 - Pause print
+  //! ### M125 - Pause print (TODO: not implemented)
   // -------------------------------
+	case 25:
 	case 601:
 	{
-		cmdqueue_pop_front(); //trick because we want skip this command (M601) after restore
-		lcd_pause_print();
+        if (!isPrintPaused)
+        {
+            st_synchronize();
+            cmdqueue_pop_front(); //trick because we want skip this command (M601) after restore
+            lcd_pause_print();
+        }
 	}
 	break;
 
   //! ### M602 - Resume print
   // -------------------------------
 	case 602: {
-		lcd_resume_print();
+	  if (isPrintPaused)
+          lcd_resume_print();
 	}
 	break;
 
   //! ### M603 - Stop print
   // -------------------------------
 	case 603: {
-		lcd_print_stop();
+		Stop();
 	}
 	break;
 
diff --git a/Firmware/cardreader.cpp b/Firmware/cardreader.cpp
index 3c01bf84b..047a2c761 100644
--- a/Firmware/cardreader.cpp
+++ b/Firmware/cardreader.cpp
@@ -25,7 +25,6 @@ CardReader::CardReader()
    sdpos = 0;
    sdprinting = false;
    cardOK = false;
-   paused = false;
    saving = false;
    logging = false;
    autostart_atmillis=0;
@@ -242,24 +241,13 @@ void CardReader::startFileprint()
   if(cardOK)
   {
     sdprinting = true;
-	paused = false;
-     Stopped = false;
+    Stopped = false;
 	#ifdef SDCARD_SORT_ALPHA
 		//flush_presort();
 	#endif
   }
 }
 
-void CardReader::pauseSDPrint()
-{
-  if(sdprinting)
-  {
-    sdprinting = false;
-	paused = true;
-  }
-}
-
-
 void CardReader::openLogFile(const char* name)
 {
   logging = true;
@@ -408,9 +396,7 @@ void CardReader::openFile(const char* name,bool read, bool replace_current/*=tru
     SERIAL_ECHOLN(name);
   }
   sdprinting = false;
-  paused = false;
-  
- 
+
   SdFile myDir;
   const char *fname=name;
   diveSubfolder(fname,myDir);
@@ -492,24 +478,27 @@ uint32_t CardReader::getFileSize()
 
 void CardReader::getStatus()
 {
-  if(sdprinting){
-    SERIAL_PROTOCOL(longFilename);
-    SERIAL_PROTOCOLPGM("\n");
-    SERIAL_PROTOCOLRPGM(_N("SD printing byte "));////MSG_SD_PRINTING_BYTE
-    SERIAL_PROTOCOL(sdpos);
-    SERIAL_PROTOCOLPGM("/");
-    SERIAL_PROTOCOLLN(filesize);
-    uint16_t time = _millis()/60000 - starttime/60000;
-    SERIAL_PROTOCOL(itostr2(time/60));
-    SERIAL_PROTOCOL(':');
-    SERIAL_PROTOCOL(itostr2(time%60));
-    SERIAL_PROTOCOLPGM("\n");
-  }
-  else if (paused) {
-	SERIAL_PROTOCOLLNPGM("SD print paused");
-  }
-  else if (saved_printing) {
-	SERIAL_PROTOCOLLNPGM("Print saved");
+  if(sdprinting)
+  {
+      if (isPrintPaused) {
+          SERIAL_PROTOCOLLNPGM("SD print paused");
+      }
+      else if (saved_printing) {
+          SERIAL_PROTOCOLLNPGM("Print saved");
+      }
+      else {
+          SERIAL_PROTOCOL(longFilename);
+          SERIAL_PROTOCOLPGM("\n");
+          SERIAL_PROTOCOLRPGM(_N("SD printing byte "));////MSG_SD_PRINTING_BYTE
+          SERIAL_PROTOCOL(sdpos);
+          SERIAL_PROTOCOLPGM("/");
+          SERIAL_PROTOCOLLN(filesize);
+          uint16_t time = _millis()/60000 - starttime/60000;
+          SERIAL_PROTOCOL(itostr2(time/60));
+          SERIAL_PROTOCOL(':');
+          SERIAL_PROTOCOL(itostr2(time%60));
+          SERIAL_PROTOCOLPGM("\n");
+      }
   }
   else {
     SERIAL_PROTOCOLLNPGM("Not SD printing");
diff --git a/Firmware/cardreader.h b/Firmware/cardreader.h
index b7df8ff41..9a7d0f697 100644
--- a/Firmware/cardreader.h
+++ b/Firmware/cardreader.h
@@ -25,7 +25,6 @@ public:
   void closefile(bool store_location=false);
   void release();
   void startFileprint();
-  void pauseSDPrint();
   uint32_t getFileSize();
   void getStatus();
   void printingHasFinished();
@@ -75,7 +74,6 @@ public:
   bool logging;
   bool sdprinting ;  
   bool cardOK ;
-  bool paused ;
   char filename[13];
   uint16_t modificationTime, modificationDate;
   uint32_t cluster, position;
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 465fde2b4..cbfe017fd 100755
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -4108,7 +4108,7 @@ void prusa_statistics(int _message, uint8_t _fil_nr) {
 		{   
 			prusa_statistics_case0(15);
 		}
-		else if (isPrintPaused || card.paused) 
+		else if (isPrintPaused)
 		{
 			prusa_statistics_case0(14);
 		}
diff --git a/Tests/PrusaStatistics_test.cpp b/Tests/PrusaStatistics_test.cpp
index 3c15899ab..261a4dc6d 100644
--- a/Tests/PrusaStatistics_test.cpp
+++ b/Tests/PrusaStatistics_test.cpp
@@ -198,7 +198,7 @@ void prusa_statistics(int _message, uint8_t _fil_nr) {
 			SERIAL_ECHOLN("}");
 			status_number = 15;
 		}
-		else if (isPrintPaused || card.paused) 
+		else if (isPrintPaused)
 		{
 			SERIAL_ECHO("{");
 			prusa_stat_printerstatus(14);
@@ -490,7 +490,7 @@ void prusa_statistics(int _message, uint8_t _fil_nr) {
 		{   
 			prusa_statistics_case0(15);
 		}
-		else if (isPrintPaused || card.paused) 
+		else if (isPrintPaused)
 		{
 			prusa_statistics_case0(14);
 		}
@@ -753,7 +753,6 @@ TEST_CASE("Prusa_statistics test", "[prusa_stats]")
 				SERIALS_RESET();
 
 				isPrintPaused = 0;
-				card.paused = 0;
 				IS_SD_PRINTING = 1;
 				old_code::prusa_statistics(test_codes[i],0);
 				new_code::prusa_statistics(test_codes[i],0);