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

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

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 !!!!!
 // !!!!!

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

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