From 05bd1ba57fc9d8448a1398caa8e19dcb777e1959 Mon Sep 17 00:00:00 2001 From: Alex Voinea Date: Mon, 15 Aug 2022 15:10:17 +0300 Subject: [PATCH 1/4] Multi-segment pause resume initial --- Firmware/Marlin.h | 7 +- Firmware/Marlin_main.cpp | 171 +++++++++++++++++------------------- Firmware/eeprom.h | 14 +-- Firmware/motion_control.cpp | 34 +++---- Firmware/motion_control.h | 5 +- Firmware/planner.cpp | 22 ++--- Firmware/planner.h | 5 +- 7 files changed, 128 insertions(+), 130 deletions(-) diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h index 6c4ff484f..3ef67139d 100755 --- a/Firmware/Marlin.h +++ b/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,10 @@ 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); + #ifdef TMC2130 void homeaxis(uint8_t axis, uint8_t cnt = 1, uint8_t* pstep = 0); #else diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index d26affdaa..facb4511b 100644 --- a/Firmware/Marlin_main.cpp +++ b/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,21 @@ 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; - } + // 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 + uint16_t start_segment_idx = 1; + 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; + start_segment_idx = saved_segment_idx; + } + 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 +4683,7 @@ eeprom_update_word((uint16_t*)EEPROM_NOZZLE_DIAMETER_uM,0xFFFF); } #endif //FWRETRACT - prepare_move(); + prepare_move(start_segment_idx); //ClearToSend(); } break; @@ -4705,21 +4708,37 @@ 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: + { + // 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 + uint16_t start_segment_idx = 1; + 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; + start_segment_idx = saved_segment_idx; + } +#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 + 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 G4: Dwell @@ -9426,8 +9445,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 +9482,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 @@ -9511,58 +9504,51 @@ 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, 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 +9557,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 +10920,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 +11027,10 @@ 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]); #ifdef LIN_ADVANCE eeprom_update_float((float*)(EEPROM_UVLO_LA_K), extruder_advance_K); @@ -11312,10 +11300,10 @@ 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)); #ifdef LIN_ADVANCE extruder_advance_K = eeprom_read_float((float*)EEPROM_UVLO_LA_K); @@ -11557,13 +11545,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 +11701,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; } diff --git a/Firmware/eeprom.h b/Firmware/eeprom.h index d3ef0a28f..6af0bb4a8 100644 --- a/Firmware/eeprom.h +++ b/Firmware/eeprom.h @@ -298,11 +298,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 @@ -526,8 +526,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 diff --git a/Firmware/motion_control.cpp b/Firmware/motion_control.cpp index 5b062d3f1..78e9f4976 100644 --- a/Firmware/motion_control.cpp +++ b/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); } diff --git a/Firmware/motion_control.h b/Firmware/motion_control.h index 4cc16ffaa..bc6780290 100644 --- a/Firmware/motion_control.h +++ b/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 diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index c3f5262cc..a51dff872 100644 --- a/Firmware/planner.cpp +++ b/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; diff --git a/Firmware/planner.h b/Firmware/planner.h index 8534288ea..ed46cc82c 100644 --- a/Firmware/planner.h +++ b/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 From b27c8b50e06794b7f1b31e3db42a02883cff3604 Mon Sep 17 00:00:00 2001 From: Alex Voinea Date: Mon, 15 Aug 2022 15:25:28 +0300 Subject: [PATCH 2/4] Fix power panic handling --- Firmware/Marlin_main.cpp | 4 ++++ Firmware/eeprom.h | 12 +++++------- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index facb4511b..0dcdd79b5 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -11031,6 +11031,8 @@ void uvlo_() 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); @@ -11304,6 +11306,8 @@ bool recover_machine_state_after_power_panic() 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); diff --git a/Firmware/eeprom.h b/Firmware/eeprom.h index 6af0bb4a8..2d9e2fec8 100644 --- a/Firmware/eeprom.h +++ b/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 @@ -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) From 671519caf346d2882f75fb36988f986de3bc35b9 Mon Sep 17 00:00:00 2001 From: Alex Voinea Date: Mon, 15 Aug 2022 22:12:31 +0300 Subject: [PATCH 3/4] Optimize by hand since the lto is still stooopit saves ~100B --- Firmware/Marlin.h | 1 + Firmware/Marlin_main.cpp | 50 +++++++++++++++++----------------------- 2 files changed, 22 insertions(+), 29 deletions(-) diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h index 3ef67139d..84e07361c 100755 --- a/Firmware/Marlin.h +++ b/Firmware/Marlin.h @@ -289,6 +289,7 @@ 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); diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 0dcdd79b5..679e1fe71 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -4648,20 +4648,7 @@ eeprom_update_word((uint16_t*)EEPROM_NOZZLE_DIAMETER_uM,0xFFFF); case 0: // G0 -> G1 case 1: // G1 { - // 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 - uint16_t start_segment_idx = 1; - 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; - start_segment_idx = saved_segment_idx; - } + 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 @@ -4711,25 +4698,12 @@ eeprom_update_word((uint16_t*)EEPROM_NOZZLE_DIAMETER_uM,0xFFFF); case 2: case 3: { - // 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 - uint16_t start_segment_idx = 1; - 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; - start_segment_idx = saved_segment_idx; - } + uint16_t start_segment_idx = restore_interrupted_gcode(); #ifdef SF_ARC_FIX bool relative_mode_backup = relative_mode; relative_mode = true; #endif - get_coordinates(); + get_coordinates(); // For X Y Z E F #ifdef SF_ARC_FIX relative_mode=relative_mode_backup; #endif @@ -9503,6 +9477,24 @@ 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 0; +} + #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, uint16_t start_segment_idx = 0) { float dx = x - current_position[X_AXIS]; From 78534f3b481629df5cda10f028ce93876fa8c46e Mon Sep 17 00:00:00 2001 From: Alex Voinea Date: Sun, 21 Aug 2022 10:17:52 +0300 Subject: [PATCH 4/4] Start at the first segment, not the last segment --- Firmware/Marlin_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 679e1fe71..076576af2 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -9492,7 +9492,7 @@ uint16_t restore_interrupted_gcode() { return saved_segment_idx; } else - return 0; + return 1; //begin with the first segment } #ifdef MESH_BED_LEVELING