diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 72a24312..1cea6c59 100755 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -3356,6 +3356,49 @@ static void gcode_PRUSA_BadRAMBoFanTest(){ #endif } + +// G92 - Set current position to coordinates given +static void gcode_G92() +{ + bool codes[NUM_AXIS]; + float values[NUM_AXIS]; + + // Check which axes need to be set + for(uint8_t i = 0; i < NUM_AXIS; ++i) + { + codes[i] = code_seen(axis_codes[i]); + if(codes[i]) + values[i] = code_value(); + } + + if((codes[E_AXIS] && values[E_AXIS] == 0) && + (!codes[X_AXIS] && !codes[Y_AXIS] && !codes[Z_AXIS])) + { + // As a special optimization, when _just_ clearing the E position + // we schedule a flag asynchronously along with the next block to + // reset the starting E position instead of stopping the planner + current_position[E_AXIS] = 0; + plan_reset_next_e(); + } + else + { + // In any other case we're forced to synchronize + st_synchronize(); + for(uint8_t i = 0; i < 3; ++i) + { + if(codes[i]) + current_position[i] = values[i] + cs.add_homing[i]; + } + if(codes[E_AXIS]) + current_position[E_AXIS] = values[E_AXIS]; + + // Set all at once + plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], + current_position[Z_AXIS], current_position[E_AXIS]); + } +} + + #ifdef BACKLASH_X extern uint8_t st_backlash_x; #endif //BACKLASH_X @@ -5217,22 +5260,10 @@ if(eSoundMode!=e_SOUND_MODE_SILENT) //! ### G92 - Set position // ----------------------------- - case 92: - if(!code_seen(axis_codes[E_AXIS])) - st_synchronize(); - for(int8_t i=0; i < NUM_AXIS; i++) { - if(code_seen(axis_codes[i])) { - if(i == E_AXIS) { - current_position[i] = code_value(); - plan_set_e_position(current_position[E_AXIS]); - } - else { - current_position[i] = code_value()+cs.add_homing[i]; - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); - } - } - } - break; + case 92: { + gcode_G92(); + } + break; //! ### G98 - Activate farm mode diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index f5270d4a..8c26cef9 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -130,6 +130,10 @@ float extruder_advance_K = LIN_ADVANCE_K; float position_float[NUM_AXIS]; #endif +// Request the next block to start at zero E count +static bool plan_reset_next_e_queue; +static bool plan_reset_next_e_sched; + // Returns the index of the next block in the ring buffer // NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication. static inline int8_t next_block_index(int8_t block_index) { @@ -441,6 +445,8 @@ void plan_init() { previous_speed[2] = 0.0; previous_speed[3] = 0.0; previous_nominal_speed = 0.0; + plan_reset_next_e_queue = false; + plan_reset_next_e_sched = false; } @@ -658,6 +664,9 @@ void planner_abort_hard() previous_speed[2] = 0.0; previous_speed[3] = 0.0; + plan_reset_next_e_queue = false; + plan_reset_next_e_sched = false; + // Relay to planner wait routine, that the current line shall be canceled. waiting_inside_plan_buffer_line_print_aborted = true; } @@ -721,6 +730,20 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate // Save the global feedrate at scheduling time block->gcode_feedrate = feedrate; + // Reset the starting E position when requested + if (plan_reset_next_e_queue) + { + position[E_AXIS] = 0; +#ifdef LIN_ADVANCE + position_float[E_AXIS] = 0; +#endif + + // the block might still be discarded later, but we need to ensure the lower-level + // count_position is also reset correctly for consistent results! + plan_reset_next_e_queue = false; + plan_reset_next_e_sched = true; + } + #ifdef ENABLE_AUTO_BED_LEVELING apply_rotation_xyz(plan_bed_level_matrix, x, y, z); #endif // ENABLE_AUTO_BED_LEVELING @@ -1165,6 +1188,13 @@ Having the real displacement of the head, we can calculate the total movement le // Reset the block flag. block->flag = 0; + if (plan_reset_next_e_sched) + { + // finally propagate a pending reset + block->flag |= BLOCK_FLAG_E_RESET; + plan_reset_next_e_sched = false; + } + // Initial limit on the segment entry velocity. float vmax_junction; @@ -1367,6 +1397,11 @@ void plan_set_e_position(const float &e) st_set_e_position(position[E_AXIS]); } +void plan_reset_next_e() +{ + plan_reset_next_e_queue = true; +} + #ifdef PREVENT_DANGEROUS_EXTRUDE void set_extrude_min_temp(float temp) { diff --git a/Firmware/planner.h b/Firmware/planner.h index 8cb3218c..23509acf 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -44,6 +44,8 @@ enum BlockFlag { // than 32767, therefore the DDA algorithm may run with 16bit resolution only. // In addition, the stepper routine will not do any end stop checking for higher performance. BLOCK_FLAG_DDA_LOWRES = 8, + // Block starts with Zeroed E counter + BLOCK_FLAG_E_RESET = 16, }; union dda_isteps_t @@ -168,6 +170,9 @@ void plan_set_position(float x, float y, float z, const float &e); void plan_set_z_position(const float &z); void plan_set_e_position(const float &e); +// Reset the E position to zero at the start of the next segment +void plan_reset_next_e(); + extern bool e_active(); void check_axes_activity(); diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index ff17c845..74625a63 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -362,6 +362,10 @@ FORCE_INLINE void stepper_next_block() LA_phase = -1; #endif + if (current_block->flag & BLOCK_FLAG_E_RESET) { + count_position[E_AXIS] = 0; + } + if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) { counter_x.lo = -(current_block->step_event_count.lo >> 1); counter_y.lo = counter_x.lo;