diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 918bae1a0d..bb89395425 100755 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -6988,7 +6988,7 @@ inline void gcode_M503() { */ inline void gcode_M905() { stepper.synchronize(); - stepper.advance_M905(code_seen('K') ? code_value_float() : -1.0); + planner.advance_M905(code_seen('K') ? code_value_float() : -1.0); } #endif diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 073179c7b3..5c64c609a1 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -131,6 +131,11 @@ float Planner::previous_speed[NUM_AXIS], long Planner::axis_segment_time[2][3] = { {MAX_FREQ_TIME + 1, 0, 0}, {MAX_FREQ_TIME + 1, 0, 0} }; #endif +#if ENABLED(LIN_ADVANCE) + float Planner::extruder_advance_k = LIN_ADVANCE_K; + float Planner::position_float[NUM_AXIS] = { 0 }; +#endif + /** * Class and Instance Methods */ @@ -140,6 +145,9 @@ Planner::Planner() { init(); } void Planner::init() { block_buffer_head = block_buffer_tail = 0; ZERO(position); + #if ENABLED(LIN_ADVANCE) + ZERO(position_float); + #endif ZERO(previous_speed); previous_nominal_speed = 0.0; #if ABL_PLANAR @@ -604,6 +612,14 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const lround(c * axis_steps_per_mm[Z_AXIS]), lround(e * axis_steps_per_mm[E_AXIS]) }; + + #if ENABLED(LIN_ADVANCE) + float target_float[XYZE] = {a, b, c, e}; + float de_float = target_float[E_AXIS] - position_float[E_AXIS]; + float mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS])); + + memcpy(position_float, target_float, sizeof(position_float)); + #endif long da = target[X_AXIS] - position[X_AXIS], db = target[Y_AXIS] - position[Y_AXIS], @@ -1232,12 +1248,12 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const // This leads to an enormous number of advance steps due to a huge e_acceleration. // The math is correct, but you don't want a retract move done with advance! // So this situation is filtered out here. - if (!esteps || (!block->steps[X_AXIS] && !block->steps[Y_AXIS]) || stepper.get_advance_k() == 0 || (uint32_t)esteps == block->step_event_count) { + if (!esteps || (!block->steps[X_AXIS] && !block->steps[Y_AXIS]) || extruder_advance_k == 0.0 || (uint32_t)esteps == block->step_event_count) { block->use_advance_lead = false; } else { block->use_advance_lead = true; - block->e_speed_multiplier8 = (esteps << 8) / block->step_event_count; + block->abs_adv_steps_multiplier8 = lround(extruder_advance_k * (de_float / mm_D_float) * block->nominal_speed / (float)block->nominal_rate * axis_steps_per_mm[Z_AXIS] * 256.0); } #elif ENABLED(ADVANCE) @@ -1354,3 +1370,14 @@ void Planner::refresh_positioning() { } #endif + +#if ENABLED(LIN_ADVANCE) + + void Planner::advance_M905(const float &k) { + if (k >= 0.0) extruder_advance_k = k; + SERIAL_ECHO_START; + SERIAL_ECHOPAIR("Advance factor: ", extruder_advance_k); + SERIAL_EOL; + } + +#endif \ No newline at end of file diff --git a/Marlin/planner.h b/Marlin/planner.h index 0d97107c0b..ab691a6eb5 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -95,7 +95,7 @@ typedef struct { // Advance extrusion #if ENABLED(LIN_ADVANCE) bool use_advance_lead; - int16_t e_speed_multiplier8; // Factorised by 2^8 to avoid float + uint32_t abs_adv_steps_multiplier8; // Factorised by 2^8 to avoid float #elif ENABLED(ADVANCE) int32_t advance_rate; volatile int32_t initial_advance; @@ -196,6 +196,11 @@ class Planner { // Segment times (in µs). Used for speed calculations static long axis_segment_time[2][3]; #endif + + #if ENABLED(LIN_ADVANCE) + static float position_float[NUM_AXIS]; + static float extruder_advance_k; + #endif public: @@ -245,6 +250,10 @@ class Planner { #define ARG_Z const float &lz #endif + + #if ENABLED(LIN_ADVANCE) + void advance_M905(const float &k); + #endif /** * Planner::_buffer_line diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 5b910a3ce8..cb7eb3c31d 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -96,8 +96,7 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even #if ENABLED(LIN_ADVANCE) volatile int Stepper::e_steps[E_STEPPERS]; - int Stepper::extruder_advance_k = LIN_ADVANCE_K, - Stepper::final_estep_rate, + int Stepper::final_estep_rate, Stepper::current_estep_rate[E_STEPPERS], Stepper::current_adv_steps[E_STEPPERS]; #else @@ -534,7 +533,7 @@ void Stepper::isr() { #if ENABLED(LIN_ADVANCE) if (current_block->use_advance_lead) { - int delta_adv_steps = (((long)extruder_advance_k * current_estep_rate[TOOL_E_INDEX]) >> 9) - current_adv_steps[TOOL_E_INDEX]; + int delta_adv_steps = current_estep_rate[TOOL_E_INDEX] - current_adv_steps[TOOL_E_INDEX]; current_adv_steps[TOOL_E_INDEX] += delta_adv_steps; #if ENABLED(MIXING_EXTRUDER) // Mixing extruders apply advance lead proportionally @@ -572,9 +571,9 @@ void Stepper::isr() { if (current_block->use_advance_lead) { #if ENABLED(MIXING_EXTRUDER) MIXING_STEPPERS_LOOP(j) - current_estep_rate[j] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8; + current_estep_rate[j] = ((uint32_t)acc_step_rate * current_block->abs_adv_steps_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 17; #else - current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8) >> 8; + current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->abs_adv_steps_multiplier8) >> 17; #endif } @@ -624,9 +623,9 @@ void Stepper::isr() { if (current_block->use_advance_lead) { #if ENABLED(MIXING_EXTRUDER) MIXING_STEPPERS_LOOP(j) - current_estep_rate[j] = ((uint32_t)step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8; + current_estep_rate[j] = ((uint32_t)step_rate * current_block->abs_adv_steps_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 17; #else - current_estep_rate[TOOL_E_INDEX] = ((uint32_t)step_rate * current_block->e_speed_multiplier8) >> 8; + current_estep_rate[TOOL_E_INDEX] = ((uint32_t)step_rate * current_block->abs_adv_steps_multiplier8) >> 17; #endif } @@ -1350,14 +1349,3 @@ void Stepper::report_positions() { } #endif // HAS_MICROSTEPS - -#if ENABLED(LIN_ADVANCE) - - void Stepper::advance_M905(const float &k) { - if (k >= 0) extruder_advance_k = k; - SERIAL_ECHO_START; - SERIAL_ECHOPAIR("Advance factor: ", extruder_advance_k); - SERIAL_EOL; - } - -#endif // LIN_ADVANCE diff --git a/Marlin/stepper.h b/Marlin/stepper.h index a722a50b70..10c5c64eee 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -109,7 +109,6 @@ class Stepper { static volatile unsigned char eISR_Rate; #if ENABLED(LIN_ADVANCE) static volatile int e_steps[E_STEPPERS]; - static int extruder_advance_k; static int final_estep_rate; static int current_estep_rate[E_STEPPERS]; // Actual extruder speed [steps/s] static int current_adv_steps[E_STEPPERS]; // The amount of current added esteps due to advance. @@ -277,11 +276,6 @@ class Stepper { return endstops_trigsteps[axis] * planner.steps_to_mm[axis]; } - #if ENABLED(LIN_ADVANCE) - void advance_M905(const float &k); - FORCE_INLINE int get_advance_k() { return extruder_advance_k; } - #endif - private: static FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) { @@ -367,8 +361,8 @@ class Stepper { #if ENABLED(LIN_ADVANCE) if (current_block->use_advance_lead) { - current_estep_rate[current_block->active_extruder] = ((unsigned long)acc_step_rate * current_block->e_speed_multiplier8) >> 8; - final_estep_rate = (current_block->nominal_rate * current_block->e_speed_multiplier8) >> 8; + current_estep_rate[current_block->active_extruder] = ((unsigned long)acc_step_rate * current_block->abs_adv_steps_multiplier8) >> 17; + final_estep_rate = (current_block->nominal_rate * current_block->abs_adv_steps_multiplier8) >> 17; } #endif