Merge pull request #3566 from leptun/fix_multi_segment_pause_resume

Fix multi segment pause-resume

Firmware/Marlin.h

@@ -236,8 +236,6 @@ void FlushSerialRequestResend();
 void ClearToSend();
 void update_currents();
 
-void get_coordinates();
-void prepare_move();
 void kill(const char *full_screen_message = NULL, unsigned char id = 0);
 void finishAndDisableSteppers();
 
@@ -252,7 +250,6 @@ bool IsStopped();                           // Returns true if the print has bee
 //put an ASCII command at the begin of the current buffer, read from flash
 #define enquecommand_front_P(cmd) enquecommand_front(cmd, true)
 
-void prepare_arc_move(bool isclockwise);
 void clamp_to_software_endstops(float target[3]);
 void refresh_cmd_timeout(void);
 
@@ -289,6 +286,11 @@ extern uint8_t newFanSpeed;
 extern int8_t lcd_change_fil_state;
 extern float default_retraction;
 
+void get_coordinates();
+void prepare_move(uint16_t start_segment_idx = 0);
+void prepare_arc_move(bool isclockwise, uint16_t start_segment_idx = 0);
+uint16_t restore_interrupted_gcode();
+
 #ifdef TMC2130
 void homeaxis(uint8_t axis, uint8_t cnt = 1, uint8_t* pstep = 0);
 #else

Firmware/Marlin_main.cpp

@@ -304,10 +304,11 @@ uint8_t saved_filament_type;
 // Define some coordinates outside the clamp limits (making them invalid past the parsing stage) so
 // that they can be used later for various logical checks
 #define X_COORD_INVALID (X_MIN_POS-1)
-#define Y_COORD_INVALID (Y_MIN_POS-1)
 
-#define SAVED_TARGET_UNSET X_COORD_INVALID
-float saved_target[NUM_AXIS] = {SAVED_TARGET_UNSET, 0, 0, 0};
+#define SAVED_START_POSITION_UNSET X_COORD_INVALID
+float saved_start_position[NUM_AXIS] = {SAVED_START_POSITION_UNSET, 0, 0, 0};
+
+uint16_t saved_segment_idx = 0;
 
 // save/restore printing in case that mmu was not responding 
 bool mmu_print_saved = false;
@@ -445,7 +446,6 @@ AutoReportFeatures autoReportFeatures;
 //=============================Routines======================================
 //===========================================================================
 
-static void get_arc_coordinates();
 static bool setTargetedHotend(int code, uint8_t &extruder);
 static void print_time_remaining_init();
 static void wait_for_heater(long codenum, uint8_t extruder);
@@ -4648,18 +4648,8 @@ eeprom_update_word((uint16_t*)EEPROM_NOZZLE_DIAMETER_uM,0xFFFF);
     case 0: // G0 -> G1
     case 1: // G1
         {
-            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;
-            }
+        uint16_t start_segment_idx = restore_interrupted_gcode();
+        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);
@@ -4680,7 +4670,7 @@ eeprom_update_word((uint16_t*)EEPROM_NOZZLE_DIAMETER_uM,0xFFFF);
         }
 #endif //FWRETRACT
 
-        prepare_move();
+        prepare_move(start_segment_idx);
         //ClearToSend();
       }
       break;
@@ -4705,21 +4695,24 @@ eeprom_update_word((uint16_t*)EEPROM_NOZZLE_DIAMETER_uM,0xFFFF);
 	  - `F` - The feedrate per minute of the move between the starting point and ending point (if supplied)
 	
     */
-    case 2: 
-      {
-        get_arc_coordinates();
-        prepare_arc_move(true);
-      }
-      break;
- 
-    // -------------------------------
-    case 3: 
-      {
-        get_arc_coordinates();
-        prepare_arc_move(false);
-      }
-      break;
+    case 2:
+    case 3:
+    {
+        uint16_t start_segment_idx = restore_interrupted_gcode();
+#ifdef SF_ARC_FIX
+        bool relative_mode_backup = relative_mode;
+        relative_mode = true;
+#endif
+        get_coordinates(); // For X Y Z E F
+#ifdef SF_ARC_FIX
+        relative_mode=relative_mode_backup;
+#endif
 
+        offset[0] = code_seen('I') ? code_value() : 0.f;
+        offset[1] = code_seen('J') ? code_value() : 0.f;
+        
+        prepare_arc_move((gcode_in_progress == 2), start_segment_idx);
+    } break;
 
     /*!
 	### G4 - Dwell <a href="https://reprap.org/wiki/G-code#G4:_Dwell">G4: Dwell</a>
@@ -9426,8 +9419,7 @@ void update_currents() {
 }
 #endif //MOTHERBOARD == BOARD_RAMBO_MINI_1_0 || MOTHERBOARD == BOARD_RAMBO_MINI_1_3
 
-void get_coordinates()
-{
+void get_coordinates() {
   bool seen[4]={false,false,false,false};
   for(int8_t i=0; i < NUM_AXIS; i++) {
     if(code_seen(axis_codes[i]))
@@ -9464,31 +9456,6 @@ void get_coordinates()
   }
 }
 
-void get_arc_coordinates()
-{
-#ifdef SF_ARC_FIX
-   bool relative_mode_backup = relative_mode;
-   relative_mode = true;
-#endif
-   get_coordinates();
-#ifdef SF_ARC_FIX
-   relative_mode=relative_mode_backup;
-#endif
-
-   if(code_seen('I')) {
-     offset[0] = code_value();
-   }
-   else {
-     offset[0] = 0.0;
-   }
-   if(code_seen('J')) {
-     offset[1] = code_value();
-   }
-   else {
-     offset[1] = 0.0;
-   }
-}
-
 void clamp_to_software_endstops(float target[3])
 {
 #ifdef DEBUG_DISABLE_SWLIMITS
@@ -9510,59 +9477,70 @@ void clamp_to_software_endstops(float target[3])
     }
 }
 
+uint16_t restore_interrupted_gcode() {
+    // When recovering from a previous print move, restore the originally
+    // calculated start position on the first USB/SD command. This accounts
+    // properly for relative moves
+    if (
+        (saved_start_position[0] != SAVED_START_POSITION_UNSET) && (
+            (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_SDCARD) ||
+            (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_USB_WITH_LINENR)
+        )
+    ) {
+        memcpy(current_position, saved_start_position, sizeof(current_position));
+        saved_start_position[0] = SAVED_START_POSITION_UNSET;
+        return saved_segment_idx;
+    }
+    else
+        return 1; //begin with the first segment
+}
+
 #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, uint16_t start_segment_idx = 0) {
         float dx = x - current_position[X_AXIS];
         float dy = y - current_position[Y_AXIS];
-        int n_segments = 0;
+        uint16_t n_segments = 0;
 
         if (mbl.active) {
             float len = fabs(dx) + fabs(dy);
             if (len > 0)
                 // Split to 3cm segments or shorter.
-                n_segments = int(ceil(len / 30.f));
+                n_segments = uint16_t(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;
+        if (n_segments > 1 && start_segment_idx) {
 
             float dz = z - current_position[Z_AXIS];
             float de = e - current_position[E_AXIS];
 
-            for (int i = 1; i < n_segments; ++ i) {
+            for (uint16_t i = start_segment_idx; i < n_segments; ++ i) {
                 float t = float(i) / float(n_segments);
                 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, gcode_target);
+                                 feed_rate, extruder, current_position, i);
                 if (planner_aborted)
                     return;
             }
         }
         // The rest of the path.
-        plan_buffer_line(x, y, z, e, feed_rate, extruder);
+        plan_buffer_line(x, y, z, e, feed_rate, extruder, current_position);
     }
 #endif  // MESH_BED_LEVELING
     
-void prepare_move()
+void prepare_move(uint16_t start_segment_idx)
 {
   clamp_to_software_endstops(destination);
   previous_millis_cmd.start();
 
   // Do not use feedmultiply for E or Z only moves
-  if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
+  if((current_position[X_AXIS] == destination[X_AXIS]) && (current_position[Y_AXIS] == destination[Y_AXIS])) {
       plan_buffer_line_destinationXYZE(feedrate/60);
   }
   else {
 #ifdef MESH_BED_LEVELING
-    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply*(1./(60.f*100.f)), active_extruder);
+    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply*(1./(60.f*100.f)), active_extruder, start_segment_idx);
 #else
      plan_buffer_line_destinationXYZE(feedrate*feedmultiply*(1./(60.f*100.f)));
 #endif
@@ -9571,10 +9549,10 @@ void prepare_move()
   set_current_to_destination();
 }
 
-void prepare_arc_move(bool isclockwise) {
+void prepare_arc_move(bool isclockwise, uint16_t start_segment_idx) {
     float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
     // Trace the arc
-    mc_arc(current_position, destination, offset, feedrate * feedmultiply / 60 / 100.0, r, isclockwise, active_extruder);
+    mc_arc(current_position, destination, offset, feedrate * feedmultiply / 60 / 100.0, r, isclockwise, active_extruder, start_segment_idx);
     // As far as the parser is concerned, the position is now == target. In reality the
     // motion control system might still be processing the action and the real tool position
     // in any intermediate location.
@@ -10934,13 +10912,15 @@ void uvlo_()
     uint16_t feedrate_bckp;
     if (current_block && !pos_invalid)
     {
-        memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
+        memcpy(saved_start_position, current_block->gcode_start_position, sizeof(saved_start_position));
         feedrate_bckp = current_block->gcode_feedrate;
+        saved_segment_idx = current_block->segment_idx;
     }
     else
     {
-        saved_target[0] = SAVED_TARGET_UNSET;
+        saved_start_position[0] = SAVED_START_POSITION_UNSET;
         feedrate_bckp = feedrate;
+        saved_segment_idx = 0;
     }
 
     // From this point on and up to the print recovery, Z should not move during X/Y travels and
@@ -11039,10 +11019,12 @@ void uvlo_()
 	eeprom_update_float((float*)(EEPROM_UVLO_TRAVEL_ACCELL), cs.travel_acceleration);
 
     // 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]);
+    eeprom_update_float((float*)(EEPROM_UVLO_SAVED_START_POSITION+0*4), saved_start_position[X_AXIS]);
+    eeprom_update_float((float*)(EEPROM_UVLO_SAVED_START_POSITION+1*4), saved_start_position[Y_AXIS]);
+    eeprom_update_float((float*)(EEPROM_UVLO_SAVED_START_POSITION+2*4), saved_start_position[Z_AXIS]);
+    eeprom_update_float((float*)(EEPROM_UVLO_SAVED_START_POSITION+3*4), saved_start_position[E_AXIS]);
+    
+    eeprom_update_word((uint16_t*)EEPROM_UVLO_SAVED_SEGMENT_IDX, saved_segment_idx);
 
 #ifdef LIN_ADVANCE
 	eeprom_update_float((float*)(EEPROM_UVLO_LA_K), extruder_advance_K);
@@ -11312,10 +11294,12 @@ bool recover_machine_state_after_power_panic()
   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));
+  saved_start_position[X_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_SAVED_START_POSITION+0*4));
+  saved_start_position[Y_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_SAVED_START_POSITION+1*4));
+  saved_start_position[Z_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_SAVED_START_POSITION+2*4));
+  saved_start_position[E_AXIS] = eeprom_read_float((float*)(EEPROM_UVLO_SAVED_START_POSITION+3*4));
+  
+  saved_segment_idx = eeprom_read_word((uint16_t*)EEPROM_UVLO_SAVED_SEGMENT_IDX);
 
 #ifdef LIN_ADVANCE
   extruder_advance_K = eeprom_read_float((float*)EEPROM_UVLO_LA_K);
@@ -11557,13 +11541,16 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
   bool pos_invalid = XY_NO_RESTORE_FLAG;
   if (current_block && !pos_invalid)
   {
-      memcpy(saved_target, current_block->gcode_target, sizeof(saved_target));
+      memcpy(saved_start_position, current_block->gcode_start_position, sizeof(saved_start_position));
       saved_feedrate2 = current_block->gcode_feedrate;
+      saved_segment_idx = current_block->segment_idx;
+      // printf_P(PSTR("stop_and_save_print_to_ram: %f, %f, %f, %f, %u\n"), saved_start_position[0], saved_start_position[1], saved_start_position[2], saved_start_position[3], saved_segment_idx);
   }
   else
   {
-      saved_target[0] = SAVED_TARGET_UNSET;
+      saved_start_position[0] = SAVED_START_POSITION_UNSET;
       saved_feedrate2 = feedrate;
+      saved_segment_idx = 0;
   }
 
 	planner_abort_hard(); //abort printing
@@ -11710,7 +11697,7 @@ void restore_print_from_ram_and_continue(float e_move)
 void cancel_saved_printing()
 {
     eeprom_update_byte((uint8_t*)EEPROM_UVLO, 0);
-    saved_target[0] = SAVED_TARGET_UNSET;
+    saved_start_position[0] = SAVED_START_POSITION_UNSET;
     saved_printing_type = PRINTING_TYPE_NONE;
     saved_printing = false;
 }

Firmware/eeprom.h

@@ -142,9 +142,8 @@ static_assert(sizeof(Sheets) == EEPROM_SHEETS_SIZEOF, "Sizeof(Sheets) is not EEP
 | 0x0F75h 3957		| uint16	| EEPROM_UVLO_MESH_BED_LEVELING			| ???			| ff ffh 65535			| Power Panic Mesh Bed Leveling						| ???			| D3 Ax0f75 C18 
 | 0x0F73h 3955		| uint16	| EEPROM_UVLO_Z_MICROSTEPS				| ???			| ff ffh 65535			| Power Panic Z microsteps							| ???			| D3 Ax0f73 C2 
 | 0x0F72h 3954		| uint8		| EEPROM_UVLO_E_ABS						| ???			| ffh 255				| Power Panic ??? position							| ???			| D3 Ax0f72 C1
-| 0x0F6Eh 3950		| foat		| EEPROM_UVLO_CURRENT_POSITION_E		| ???			| ff ff ff ffh			| Power Panic E position							| ???			| D3 Ax0f6e C4
-| 0x0F6Dh 3949		| ???		| _EEPROM_FREE_NR2_						| ???			| ffh 255				| _Free EEPROM space_								| _free space_	| D3 Ax0f6d C1
-| 0x0F6Ch 3948		| ???		| _EEPROM_FREE_NR3_						| ???			| ffh 255				| _Free EEPROM space_								| _free space_	| D3 Ax0f6c C1
+| 0x0F6Eh 3950		| float		| EEPROM_UVLO_CURRENT_POSITION_E		| ???			| ff ff ff ffh			| Power Panic E position							| ???			| D3 Ax0f6e C4
+| 0x0F6Ch 3948		| uint16_t	| EEPROM_UVLO_SAVED_SEGMENT_IDX			| all			| ff ffh 65535			| Power Panic index of multi-segment move			| ???			| D3 Ax0f6c C2
 | 0x0F6Bh 3947		| ???		| _EEPROM_FREE_NR4_						| ???			| ffh 255				| _Free EEPROM space_								| _free space_	| D3 Ax0f6b C1
 | 0x0F6Ah 3946		| ???		| _EEPROM_FREE_NR5_						| ???			| ffh 255				| _Free EEPROM space_								| _free space_	| D3 Ax0f6a C1
 | 0x0F69h 3945		| uint8		| EEPROM_CRASH_DET						| ffh 255		| ffh 255				| Crash detection: __enabled__						| LCD menu		| D3 Ax0f69 C1
@@ -298,11 +297,11 @@ static_assert(sizeof(Sheets) == EEPROM_SHEETS_SIZEOF, "Sizeof(Sheets) is not EEP
 | ^					| ^			| ^										| 01h 1			| ^						| Filament Sensor type IR 0.4 or newer				| ^				| ^
 | 0x0D47 3399		| uint8		| EEPROM_FSENSOR_ACTION_NA				| 00h 0			| ffh 255				| Filament Sensor action: __Continue__				| LCD menu		| D3 Ax0d47 C1
 | ^					| ^			| ^										| 01h 1			| ^						| Filament Sensor action: __Pause__					| ^				| ^
-| 0x0D37 3383		| float		| EEPROM_UVLO_SAVED_TARGET				| ???			| ff ff ff ffh			| Power panic saved target all-axis					| ???			| D3 Ax0d37 C16
-| ^					| ^			| ^										| ???			| ^						| Power panic saved target e-axis					| ^				| D3 Ax0d43 C4
-| ^					| ^			| ^										| ???			| ^						| Power panic saved target z-axis					| ^				| D3 Ax0d3f C4
-| ^					| ^			| ^										| ???			| ^						| Power panic saved target y-axis					| ^				| D3 Ax0d3b C4
-| ^					| ^			| ^										| ???			| ^						| Power panic saved target x-axis					| ^				| D3 Ax0d37 C4
+| 0x0D37 3383		| float		| EEPROM_UVLO_SAVED_START_POSITION		| ???			| ff ff ff ffh			| Power panic saved start position all-axis			| ???			| D3 Ax0d37 C16
+| ^					| ^			| ^										| ???			| ^						| Power panic saved start position e-axis			| ^				| D3 Ax0d43 C4
+| ^					| ^			| ^										| ???			| ^						| Power panic saved start position z-axis			| ^				| D3 Ax0d3f C4
+| ^					| ^			| ^										| ???			| ^						| Power panic saved start position y-axis			| ^				| D3 Ax0d3b C4
+| ^					| ^			| ^										| ???			| ^						| Power panic saved start position x-axis			| ^				| D3 Ax0d37 C4
 | 0x0D35 3381		| uint16	| EEPROM_UVLO_FEEDMULTIPLY				| ???			| ff ffh 65355			| Power panic saved feed multiplier					| ???			| D3 Ax0d35 C2
 | 0x0D34 3380		| uint8		| EEPROM_BACKLIGHT_LEVEL_HIGH			| 00h - ffh 	| 82h 130				| LCD backlight bright:	__128__	Dim value to 255	| LCD menu		| D3 Ax0d34 C1
 | 0x0D33 3379		| uint8		| EEPROM_BACKLIGHT_LEVEL_LOW			| 00h - ffh		| 32h 50				| LCD backlight dim:	__50__ 	0 to Bright value	| LCD menu		| D3 Ax0d33 C1
@@ -398,15 +397,14 @@ static_assert(sizeof(Sheets) == EEPROM_SHEETS_SIZEOF, "Sizeof(Sheets) is not EEP
 #define EEPROM_UVLO_FAN_SPEED			(EEPROM_UVLO_FEEDRATE - 1) 
 #define EEPROM_FAN_CHECK_ENABLED		(EEPROM_UVLO_FAN_SPEED - 1)
 #define EEPROM_UVLO_MESH_BED_LEVELING     (EEPROM_FAN_CHECK_ENABLED - 9*2)
-
 #define EEPROM_UVLO_Z_MICROSTEPS     (EEPROM_UVLO_MESH_BED_LEVELING - 2) // uint16_t (could be removed)
 #define EEPROM_UVLO_E_ABS            (EEPROM_UVLO_Z_MICROSTEPS - 1)
 #define EEPROM_UVLO_CURRENT_POSITION_E	(EEPROM_UVLO_E_ABS - 4)                 //float for current position in E
+#define EEPROM_UVLO_SAVED_SEGMENT_IDX   (EEPROM_UVLO_CURRENT_POSITION_E - 2) //uint16_t
 
-#define EEPROM_FREE_NR2         (EEPROM_UVLO_CURRENT_POSITION_E - 1)			// FREE EEPROM SPACE
-#define EEPROM_FREE_NR3         (EEPROM_FREE_NR2 - 1)							// FREE EEPROM SPACE
-#define EEPROM_FREE_NR4         (EEPROM_FREE_NR3 - 1)							// FREE EEPROM SPACE
+#define EEPROM_FREE_NR4         (EEPROM_UVLO_SAVED_SEGMENT_IDX - 1)							// FREE EEPROM SPACE
 #define EEPROM_FREE_NR5         (EEPROM_FREE_NR4 - 1)							// FREE EEPROM SPACE
+
 // Crash detection mode EEPROM setting 
 #define EEPROM_CRASH_DET         (EEPROM_FREE_NR5 - 1)       				    // uint8 (orig EEPROM_UVLO_MESH_BED_LEVELING-12) 
 // Crash detection counter Y (last print)
@@ -526,8 +524,8 @@ 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
-#define EEPROM_UVLO_FEEDMULTIPLY (EEPROM_UVLO_SAVED_TARGET - 2) // uint16_t for feedmultiply
+#define EEPROM_UVLO_SAVED_START_POSITION (EEPROM_FSENSOR_ACTION_NA - 4*4) // 4 x float for saved start position for all axes
+#define EEPROM_UVLO_FEEDMULTIPLY (EEPROM_UVLO_SAVED_START_POSITION - 2) // uint16_t for feedmultiply
 
 #define EEPROM_BACKLIGHT_LEVEL_HIGH (EEPROM_UVLO_FEEDMULTIPLY-1) // uint8
 #define EEPROM_BACKLIGHT_LEVEL_LOW (EEPROM_BACKLIGHT_LEVEL_HIGH-1) // uint8

Firmware/motion_control.cpp

@@ -26,13 +26,16 @@
 
 // The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
 // segment is configured in settings.mm_per_arc_segment.  
-void mc_arc(float* position, float* target, float* offset, float feed_rate, float radius, bool isclockwise, uint8_t extruder)
+void mc_arc(const float* position, float* target, const float* offset, float feed_rate, float radius, bool isclockwise, uint8_t extruder, uint16_t start_segment_idx)
 {
+    float start_position[4];
+    memcpy(start_position, position, sizeof(start_position));
+    
     float r_axis_x = -offset[X_AXIS];  // Radius vector from center to current location
     float r_axis_y = -offset[Y_AXIS];
-    float center_axis_x = position[X_AXIS] - r_axis_x;
-    float center_axis_y = position[Y_AXIS] - r_axis_y;
-    float travel_z = target[Z_AXIS] - position[Z_AXIS];
+    float center_axis_x = start_position[X_AXIS] - r_axis_x;
+    float center_axis_y = start_position[Y_AXIS] - r_axis_y;
+    float travel_z = target[Z_AXIS] - start_position[Z_AXIS];
     float rt_x = target[X_AXIS] - center_axis_x;
     float rt_y = target[Y_AXIS] - center_axis_y;
     // 20200419 - Add a variable that will be used to hold the arc segment length
@@ -40,7 +43,7 @@ void mc_arc(float* position, float* target, float* offset, float feed_rate, floa
     // 20210109 - Add a variable to hold the n_arc_correction value
     unsigned char n_arc_correction = cs.n_arc_correction;
 
-    // CCW angle between position and target from circle center. Only one atan2() trig computation required.
+    // CCW angle between start_position and target from circle center. Only one atan2() trig computation required.
     float angular_travel_total = atan2(r_axis_x * rt_y - r_axis_y * rt_x, r_axis_x * rt_x + r_axis_y * rt_y);
     if (angular_travel_total < 0) { angular_travel_total += 2 * M_PI; }
 
@@ -76,7 +79,7 @@ void mc_arc(float* position, float* target, float* offset, float feed_rate, floa
 
     //20141002:full circle for G03 did not work, e.g. G03 X80 Y80 I20 J0 F2000 is giving an Angle of zero so head is not moving
     //to compensate when start pos = target pos && angle is zero -> angle = 2Pi
-    if (position[X_AXIS] == target[X_AXIS] && position[Y_AXIS] == target[Y_AXIS] && angular_travel_total == 0)
+    if (start_position[X_AXIS] == target[X_AXIS] && start_position[Y_AXIS] == target[Y_AXIS] && angular_travel_total == 0)
     {
         angular_travel_total += 2 * M_PI;
     }
@@ -113,13 +116,13 @@ void mc_arc(float* position, float* target, float* offset, float feed_rate, floa
     */
 
     // If there is only one segment, no need to do a bunch of work since this is a straight line!
-    if (segments > 1)
+    if (segments > 1 && start_segment_idx)
     {
         // Calculate theta per segments, and linear (z) travel per segment, e travel per segment
         // as well as the small angle approximation for sin and cos.
         const float theta_per_segment = angular_travel_total / segments,
             linear_per_segment = travel_z / (segments),
-            segment_extruder_travel = (target[E_AXIS] - position[E_AXIS]) / (segments),
+            segment_extruder_travel = (target[E_AXIS] - start_position[E_AXIS]) / (segments),
             sq_theta_per_segment = theta_per_segment * theta_per_segment,
             sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
             cos_T = 1 - 0.5f * sq_theta_per_segment;
@@ -142,14 +145,15 @@ void mc_arc(float* position, float* target, float* offset, float feed_rate, floa
             }
 
             // Update Position
-            position[X_AXIS] = center_axis_x + r_axis_x;
-            position[Y_AXIS] = center_axis_y + r_axis_y;
-            position[Z_AXIS] += linear_per_segment;
-            position[E_AXIS] += segment_extruder_travel;
+            start_position[X_AXIS] = center_axis_x + r_axis_x;
+            start_position[Y_AXIS] = center_axis_y + r_axis_y;
+            start_position[Z_AXIS] += linear_per_segment;
+            start_position[E_AXIS] += segment_extruder_travel;
             // Clamp to the calculated position.
-            clamp_to_software_endstops(position);
+            clamp_to_software_endstops(start_position);
             // Insert the segment into the buffer
-            plan_buffer_line(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS], feed_rate, extruder, position);
+            if (i >= start_segment_idx)
+                plan_buffer_line(start_position[X_AXIS], start_position[Y_AXIS], start_position[Z_AXIS], start_position[E_AXIS], feed_rate, extruder, position, i);
             // Handle the situation where the planner is aborted hard.
             if (planner_aborted)
                 return;
@@ -158,5 +162,5 @@ void mc_arc(float* position, float* target, float* offset, float feed_rate, floa
     // Clamp to the target position.
     clamp_to_software_endstops(target);
     // Ensure last segment arrives at target location.
-    plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, extruder, target);
+    plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, extruder, position, 0);
 }

Firmware/motion_control.h

@@ -26,7 +26,6 @@
 // offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
 // the direction of helical travel, radius == circle radius, isclockwise boolean. Used
 // for vector transformation direction.
-void mc_arc(float *position, float *target, float *offset, float feed_rate, float radius,
-  bool isclockwise, uint8_t extruder);
-  
+void mc_arc(const float *position, float *target, const float *offset, float feed_rate, float radius, bool isclockwise, uint8_t extruder, uint16_t start_segment_idx);
+
 #endif

Firmware/planner.cpp

@@ -703,8 +703,12 @@ 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, const float* gcode_target)
+void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, uint8_t extruder, const float* gcode_start_position, uint16_t segment_idx)
 {
+  // CRITICAL_SECTION_START; //prevent stack overflow in ISR
+  // printf_P(PSTR("plan_buffer_line(%f, %f, %f, %f, %f, %u, [%f,%f,%f,%f], %u)\n"), x, y, z, e, feed_rate, extruder, gcode_start_position[0], gcode_start_position[1], gcode_start_position[2], gcode_start_position[3], segment_idx);
+  // CRITICAL_SECTION_END;
+
   // Calculate the buffer head after we push this byte
   uint8_t next_buffer_head = next_block_index(block_buffer_head);
 
@@ -735,16 +739,14 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate
   // 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));
+  // Save original start position of the move
+  if (gcode_start_position)
+      memcpy(block->gcode_start_position, gcode_start_position, sizeof(block_t::gcode_start_position));
   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;
-  }
+      memcpy(block->gcode_start_position, current_position, sizeof(block_t::gcode_start_position));
+  
+  // Save the index of this segment (when a single G0/1/2/3 command plans multiple segments)
+  block->segment_idx = segment_idx;
 
   // Save the global feedrate at scheduling time
   block->gcode_feedrate = feedrate;

Firmware/planner.h

@@ -122,7 +122,8 @@ typedef struct {
 #endif
 
   // Save/recovery state data
-  float gcode_target[NUM_AXIS];     // Target (abs mm) of the original Gcode instruction
+  float gcode_start_position[NUM_AXIS]; // Start (abs mm) of the original Gcode instruction
+  uint16_t segment_idx;             // The index of the for loop that generates segments
   uint16_t gcode_feedrate;          // Default and/or move feedrate
   uint16_t sdlen;                   // Length of the Gcode instruction
 } block_t;
@@ -159,7 +160,7 @@ void plan_buffer_line_destinationXYZE(float feed_rate);
 
 void plan_set_position_curposXYZE();
 
-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(float x, float y, float z, const float &e, float feed_rate, uint8_t extruder, const float* gcode_start_position = NULL, uint16_t segment_idx = 0);
 //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