From fd319928d2889efe57c76913b3d5d28ff49a70d1 Mon Sep 17 00:00:00 2001 From: tombrazier <68918209+tombrazier@users.noreply.github.com> Date: Sun, 31 Jul 2022 03:39:48 +0100 Subject: [PATCH] Fix, improve Linear Advance (#24533) --- Marlin/src/module/planner.cpp | 217 ++++++++++----------- Marlin/src/module/planner.h | 11 +- Marlin/src/module/stepper.cpp | 351 +++++++++++++++++----------------- Marlin/src/module/stepper.h | 75 +------- 4 files changed, 304 insertions(+), 350 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 4bc81c1051..1c9601632d 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -788,7 +788,7 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE)); NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE)); - #if ENABLED(S_CURVE_ACCELERATION) + #if EITHER(S_CURVE_ACCELERATION, LIN_ADVANCE) // If we have some plateau time, the cruise rate will be the nominal rate uint32_t cruise_rate = block->nominal_rate; #endif @@ -820,7 +820,7 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t accelerate_steps = _MIN(uint32_t(_MAX(accelerate_steps_float, 0)), block->step_event_count); decelerate_steps = block->step_event_count - accelerate_steps; - #if ENABLED(S_CURVE_ACCELERATION) + #if EITHER(S_CURVE_ACCELERATION, LIN_ADVANCE) // We won't reach the cruising rate. Let's calculate the speed we will reach cruise_rate = final_speed(initial_rate, accel, accelerate_steps); #endif @@ -849,6 +849,14 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t #endif block->final_rate = final_rate; + #if ENABLED(LIN_ADVANCE) + if (block->la_advance_rate) { + const float comp = extruder_advance_K[block->extruder] * block->steps.e / block->step_event_count; + block->max_adv_steps = cruise_rate * comp; + block->final_adv_steps = final_rate * comp; + } + #endif + #if ENABLED(LASER_POWER_TRAP) /** * Laser Trapezoid Calculations @@ -899,75 +907,76 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t #endif // LASER_POWER_TRAP } -/* PLANNER SPEED DEFINITION - +--------+ <- current->nominal_speed - / \ - current->entry_speed -> + \ - | + <- next->entry_speed (aka exit speed) - +-------------+ - time --> - - Recalculates the motion plan according to the following basic guidelines: - - 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds - (i.e. current->entry_speed) such that: - a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of - neighboring blocks. - b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) - with a maximum allowable deceleration over the block travel distance. - c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). - 2. Go over every block in chronological (forward) order and dial down junction speed values if - a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable - acceleration over the block travel distance. - - When these stages are complete, the planner will have maximized the velocity profiles throughout the all - of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In - other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements - are possible. If a new block is added to the buffer, the plan is recomputed according to the said - guidelines for a new optimal plan. - - To increase computational efficiency of these guidelines, a set of planner block pointers have been - created to indicate stop-compute points for when the planner guidelines cannot logically make any further - changes or improvements to the plan when in normal operation and new blocks are streamed and added to the - planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are - bracketed by junction velocities at their maximums (or by the first planner block as well), no new block - added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute - them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute - point) are all accelerating, they are all optimal and can not be altered by a new block added to the - planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum - junction velocity is reached. However, if the operational conditions of the plan changes from infrequently - used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is - recomputed as stated in the general guidelines. - - Planner buffer index mapping: - - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. - - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether - the buffer is full or empty. As described for standard ring buffers, this block is always empty. - - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal - streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the - planner buffer that don't change with the addition of a new block, as describe above. In addition, - this block can never be less than block_buffer_tail and will always be pushed forward and maintain - this requirement when encountered by the Planner::release_current_block() routine during a cycle. - - NOTE: Since the planner only computes on what's in the planner buffer, some motions with many short - segments (e.g., complex curves) may seem to move slowly. This is because there simply isn't - enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and - then decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this - happens and becomes an annoyance, there are a few simple solutions: - - - Maximize the machine acceleration. The planner will be able to compute higher velocity profiles - within the same combined distance. - - - Maximize line motion(s) distance per block to a desired tolerance. The more combined distance the - planner has to use, the faster it can go. - - - Maximize the planner buffer size. This also will increase the combined distance for the planner to - compute over. It also increases the number of computations the planner has to perform to compute an - optimal plan, so select carefully. - - - Use G2/G3 arcs instead of many short segments. Arcs inform the planner of a safe exit speed at the - end of the last segment, which alleviates this problem. -*/ +/** + * PLANNER SPEED DEFINITION + * +--------+ <- current->nominal_speed + * / \ + * current->entry_speed -> + \ + * | + <- next->entry_speed (aka exit speed) + * +-------------+ + * time --> + * + * Recalculates the motion plan according to the following basic guidelines: + * + * 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds + * (i.e. current->entry_speed) such that: + * a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of + * neighboring blocks. + * b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) + * with a maximum allowable deceleration over the block travel distance. + * c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). + * 2. Go over every block in chronological (forward) order and dial down junction speed values if + * a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable + * acceleration over the block travel distance. + * + * When these stages are complete, the planner will have maximized the velocity profiles throughout the all + * of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In + * other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements + * are possible. If a new block is added to the buffer, the plan is recomputed according to the said + * guidelines for a new optimal plan. + * + * To increase computational efficiency of these guidelines, a set of planner block pointers have been + * created to indicate stop-compute points for when the planner guidelines cannot logically make any further + * changes or improvements to the plan when in normal operation and new blocks are streamed and added to the + * planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are + * bracketed by junction velocities at their maximums (or by the first planner block as well), no new block + * added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute + * them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute + * point) are all accelerating, they are all optimal and can not be altered by a new block added to the + * planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum + * junction velocity is reached. However, if the operational conditions of the plan changes from infrequently + * used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is + * recomputed as stated in the general guidelines. + * + * Planner buffer index mapping: + * - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. + * - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether + * the buffer is full or empty. As described for standard ring buffers, this block is always empty. + * - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal + * streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the + * planner buffer that don't change with the addition of a new block, as describe above. In addition, + * this block can never be less than block_buffer_tail and will always be pushed forward and maintain + * this requirement when encountered by the Planner::release_current_block() routine during a cycle. + * + * NOTE: Since the planner only computes on what's in the planner buffer, some motions with many short + * segments (e.g., complex curves) may seem to move slowly. This is because there simply isn't + * enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and + * then decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this + * happens and becomes an annoyance, there are a few simple solutions: + * + * - Maximize the machine acceleration. The planner will be able to compute higher velocity profiles + * within the same combined distance. + * + * - Maximize line motion(s) distance per block to a desired tolerance. The more combined distance the + * planner has to use, the faster it can go. + * + * - Maximize the planner buffer size. This also will increase the combined distance for the planner to + * compute over. It also increases the number of computations the planner has to perform to compute an + * optimal plan, so select carefully. + * + * - Use G2/G3 arcs instead of many short segments. Arcs inform the planner of a safe exit speed at the + * end of the last segment, which alleviates this problem. + */ // The kernel called by recalculate() when scanning the plan from last to first entry. void Planner::reverse_pass_kernel(block_t * const current, const block_t * const next @@ -1211,13 +1220,6 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t // NOTE: Entry and exit factors always > 0 by all previous logic operations. const float nomr = 1.0f / block->nominal_speed; calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr); - #if ENABLED(LIN_ADVANCE) - if (block->use_advance_lead) { - const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS]; - block->max_adv_steps = block->nominal_speed * comp; - block->final_adv_steps = next_entry_speed * comp; - } - #endif } // Reset current only to ensure next trapezoid is computed - The @@ -1251,13 +1253,6 @@ void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t const float nomr = 1.0f / block->nominal_speed; calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr); - #if ENABLED(LIN_ADVANCE) - if (block->use_advance_lead) { - const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS]; - block->max_adv_steps = block->nominal_speed * comp; - block->final_adv_steps = next_entry_speed * comp; - } - #endif } // Reset block to ensure its trapezoid is computed - The stepper is free to use @@ -2502,13 +2497,15 @@ bool Planner::_populate_block( // Compute and limit the acceleration rate for the trapezoid generator. const float steps_per_mm = block->step_event_count * inverse_millimeters; uint32_t accel; + #if ENABLED(LIN_ADVANCE) + bool use_advance_lead = false; + #endif if (NUM_AXIS_GANG( !block->steps.a, && !block->steps.b, && !block->steps.c, && !block->steps.i, && !block->steps.j, && !block->steps.k, && !block->steps.u, && !block->steps.v, && !block->steps.w) ) { // Is this a retract / recover move? accel = CEIL(settings.retract_acceleration * steps_per_mm); // Convert to: acceleration steps/sec^2 - TERN_(LIN_ADVANCE, block->use_advance_lead = false); // No linear advance for simple retract/recover } else { #define LIMIT_ACCEL_LONG(AXIS,INDX) do{ \ @@ -2535,33 +2532,29 @@ bool Planner::_populate_block( /** * Use LIN_ADVANCE for blocks if all these are true: * - * esteps : This is a print move, because we checked for A, B, C steps before. + * esteps : This is a print move, because we checked for A, B, C steps before. * - * extruder_advance_K[active_extruder] : There is an advance factor set for this extruder. + * extruder_advance_K[extruder] : There is an advance factor set for this extruder. * - * de > 0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves) + * de > 0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves) */ - block->use_advance_lead = esteps - && extruder_advance_K[active_extruder] - && de > 0; + use_advance_lead = esteps && extruder_advance_K[extruder] && de > 0; - if (block->use_advance_lead) { - block->e_D_ratio = (target_float.e - position_float.e) / - #if IS_KINEMATIC - block->millimeters - #else + if (use_advance_lead) { + float e_D_ratio = (target_float.e - position_float.e) / + TERN(IS_KINEMATIC, block->millimeters, SQRT(sq(target_float.x - position_float.x) + sq(target_float.y - position_float.y) + sq(target_float.z - position_float.z)) - #endif - ; + ); // Check for unusual high e_D ratio to detect if a retract move was combined with the last print move due to min. steps per segment. Never execute this with advance! // This assumes no one will use a retract length of 0mm < retr_length < ~0.2mm and no one will print 100mm wide lines using 3mm filament or 35mm wide lines using 1.75mm filament. - if (block->e_D_ratio > 3.0f) - block->use_advance_lead = false; + if (e_D_ratio > 3.0f) + use_advance_lead = false; else { - const uint32_t max_accel_steps_per_s2 = MAX_E_JERK(extruder) / (extruder_advance_K[active_extruder] * block->e_D_ratio) * steps_per_mm; + // Scale E acceleration so that it will be possible to jump to the advance speed. + const uint32_t max_accel_steps_per_s2 = MAX_E_JERK(extruder) / (extruder_advance_K[extruder] * e_D_ratio) * steps_per_mm; if (TERN0(LA_DEBUG, accel > max_accel_steps_per_s2)) SERIAL_ECHOLNPGM("Acceleration limited."); NOMORE(accel, max_accel_steps_per_s2); @@ -2593,13 +2586,21 @@ bool Planner::_populate_block( block->acceleration_rate = (uint32_t)(accel * (float(1UL << 24) / (STEPPER_TIMER_RATE))); #endif #if ENABLED(LIN_ADVANCE) - if (block->use_advance_lead) { - block->advance_speed = (STEPPER_TIMER_RATE) / (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * settings.axis_steps_per_mm[E_AXIS_N(extruder)]); + block->la_advance_rate = 0; + block->la_scaling = 0; + + if (use_advance_lead) { + // the Bresenham algorithm will convert this step rate into extruder steps + block->la_advance_rate = extruder_advance_K[extruder] * block->acceleration_steps_per_s2; + + // reduce LA ISR frequency by calling it only often enough to ensure that there will + // never be more than four extruder steps per call + for (uint32_t dividend = block->steps.e << 1; dividend <= (block->step_event_count >> 2); dividend <<= 1) + block->la_scaling++; + #if ENABLED(LA_DEBUG) - if (extruder_advance_K[active_extruder] * block->e_D_ratio * block->acceleration * 2 < block->nominal_speed * block->e_D_ratio) - SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed."); - if (block->advance_speed < 200) - SERIAL_ECHOLNPGM("eISR running at > 10kHz."); + if (block->la_advance_rate >> block->la_scaling > 10000) + SERIAL_ECHOLNPGM("eISR running at > 10kHz: ", block->la_advance_rate); #endif } #endif diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 6eb5272071..09afee7db1 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -239,11 +239,10 @@ typedef struct PlannerBlock { // Advance extrusion #if ENABLED(LIN_ADVANCE) - bool use_advance_lead; - uint16_t advance_speed, // STEP timer value for extruder speed offset ISR - max_adv_steps, // max. advance steps to get cruising speed pressure (not always nominal_speed!) - final_adv_steps; // advance steps due to exit speed - float e_D_ratio; + uint32_t la_advance_rate; // The rate at which steps are added whilst accelerating + uint8_t la_scaling; // Scale ISR frequency down and step frequency up by 2 ^ la_scaling + uint16_t max_adv_steps, // Max advance steps to get cruising speed pressure + final_adv_steps; // Advance steps for exit speed pressure #endif uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec @@ -1018,7 +1017,7 @@ class Planner { return target_velocity_sqr - 2 * accel * distance; } - #if ENABLED(S_CURVE_ACCELERATION) + #if EITHER(S_CURVE_ACCELERATION, LIN_ADVANCE) /** * Calculate the speed reached given initial speed, acceleration and distance */ diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index b7fd918561..cac2161a47 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -217,18 +217,12 @@ uint32_t Stepper::advance_divisor = 0, #endif #if ENABLED(LIN_ADVANCE) - uint32_t Stepper::nextAdvanceISR = LA_ADV_NEVER, - Stepper::LA_isr_rate = LA_ADV_NEVER; - uint16_t Stepper::LA_current_adv_steps = 0, - Stepper::LA_final_adv_steps, - Stepper::LA_max_adv_steps; - - int8_t Stepper::LA_steps = 0; - - bool Stepper::LA_use_advance_lead; - -#endif // LIN_ADVANCE + Stepper::la_interval = LA_ADV_NEVER; + int32_t Stepper::la_delta_error = 0, + Stepper::la_dividend = 0, + Stepper::la_advance_steps = 0; +#endif #if ENABLED(INTEGRATED_BABYSTEPPING) uint32_t Stepper::nextBabystepISR = BABYSTEP_NEVER; @@ -588,29 +582,27 @@ void Stepper::set_directions() { TERN_(HAS_V_DIR, SET_STEP_DIR(V)); TERN_(HAS_W_DIR, SET_STEP_DIR(W)); - #if DISABLED(LIN_ADVANCE) - #if ENABLED(MIXING_EXTRUDER) - // Because this is valid for the whole block we don't know - // what E steppers will step. Likely all. Set all. - if (motor_direction(E_AXIS)) { - MIXER_STEPPER_LOOP(j) REV_E_DIR(j); - count_direction.e = -1; - } - else { - MIXER_STEPPER_LOOP(j) NORM_E_DIR(j); - count_direction.e = 1; - } - #elif HAS_EXTRUDERS - if (motor_direction(E_AXIS)) { - REV_E_DIR(stepper_extruder); - count_direction.e = -1; - } - else { - NORM_E_DIR(stepper_extruder); - count_direction.e = 1; - } - #endif - #endif // !LIN_ADVANCE + #if ENABLED(MIXING_EXTRUDER) + // Because this is valid for the whole block we don't know + // what E steppers will step. Likely all. Set all. + if (motor_direction(E_AXIS)) { + MIXER_STEPPER_LOOP(j) REV_E_DIR(j); + count_direction.e = -1; + } + else { + MIXER_STEPPER_LOOP(j) NORM_E_DIR(j); + count_direction.e = 1; + } + #elif HAS_EXTRUDERS + if (motor_direction(E_AXIS)) { + REV_E_DIR(stepper_extruder); + count_direction.e = -1; + } + else { + NORM_E_DIR(stepper_extruder); + count_direction.e = 1; + } + #endif DIR_WAIT_AFTER(); } @@ -1467,14 +1459,19 @@ void Stepper::isr() { // Enable ISRs to reduce USART processing latency hal.isr_on(); - if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses + if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses #if ENABLED(LIN_ADVANCE) - if (!nextAdvanceISR) nextAdvanceISR = advance_isr(); // 0 = Do Linear Advance E Stepper pulses + if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses + advance_isr(); + nextAdvanceISR = la_interval; + } + else if (nextAdvanceISR == LA_ADV_NEVER) // Start LA steps if necessary + nextAdvanceISR = la_interval; #endif #if ENABLED(INTEGRATED_BABYSTEPPING) - const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses + const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses if (is_babystep) nextBabystepISR = babystepping_isr(); #endif @@ -1796,20 +1793,18 @@ void Stepper::pulse_phase_isr() { PULSE_PREP(W); #endif - #if EITHER(LIN_ADVANCE, MIXING_EXTRUDER) - delta_error.e += advance_dividend.e; - if (delta_error.e >= 0) { - #if ENABLED(LIN_ADVANCE) - delta_error.e -= advance_divisor; - // Don't step E here - But remember the number of steps to perform - motor_direction(E_AXIS) ? --LA_steps : ++LA_steps; - #else - count_position.e += count_direction.e; - step_needed.e = true; - #endif - } - #elif HAS_E0_STEP + #if EITHER(HAS_E0_STEP, MIXING_EXTRUDER) PULSE_PREP(E); + + #if ENABLED(LIN_ADVANCE) + if (step_needed.e && current_block->la_advance_rate) { + // don't actually step here, but do subtract movements steps + // from the linear advance step count + step_needed.e = false; + count_position.e -= count_direction.e; + la_advance_steps--; + } + #endif #endif } @@ -1849,12 +1844,10 @@ void Stepper::pulse_phase_isr() { PULSE_START(W); #endif - #if DISABLED(LIN_ADVANCE) - #if ENABLED(MIXING_EXTRUDER) - if (step_needed.e) E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN); - #elif HAS_E0_STEP - PULSE_START(E); - #endif + #if ENABLED(MIXING_EXTRUDER) + if (step_needed.e) E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN); + #elif HAS_E0_STEP + PULSE_START(E); #endif TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); @@ -1894,15 +1887,10 @@ void Stepper::pulse_phase_isr() { PULSE_STOP(W); #endif - #if DISABLED(LIN_ADVANCE) - #if ENABLED(MIXING_EXTRUDER) - if (delta_error.e >= 0) { - delta_error.e -= advance_divisor; - E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN); - } - #elif HAS_E0_STEP - PULSE_STOP(E); - #endif + #if ENABLED(MIXING_EXTRUDER) + if (step_needed.e) E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN); + #elif HAS_E0_STEP + PULSE_STOP(E); #endif #if ISR_MULTI_STEPS @@ -1912,6 +1900,69 @@ void Stepper::pulse_phase_isr() { } while (--events_to_do); } +// Calculate timer interval, with all limits applied. +uint32_t Stepper::calc_timer_interval(uint32_t step_rate) { + #ifdef CPU_32_BIT + // In case of high-performance processor, it is able to calculate in real-time + return uint32_t(STEPPER_TIMER_RATE) / step_rate; + #else + // AVR is able to keep up at 30khz Stepping ISR rate. + constexpr uint32_t min_step_rate = (F_CPU) / 500000U; + if (step_rate <= min_step_rate) { + step_rate = 0; + uintptr_t table_address = (uintptr_t)&speed_lookuptable_slow[0][0]; + return uint16_t(pgm_read_word(table_address)); + } + else { + step_rate -= min_step_rate; // Correct for minimal speed + if (step_rate >= 0x0800) { // higher step rate + const uint8_t rate_mod_256 = (step_rate & 0x00FF); + const uintptr_t table_address = uintptr_t(&speed_lookuptable_fast[uint8_t(step_rate >> 8)][0]), + gain = uint16_t(pgm_read_word(table_address + 2)); + return uint16_t(pgm_read_word(table_address)) - MultiU16X8toH16(rate_mod_256, gain); + } + else { // lower step rates + uintptr_t table_address = uintptr_t(&speed_lookuptable_slow[0][0]); + table_address += (step_rate >> 1) & 0xFFFC; + return uint16_t(pgm_read_word(table_address)) + - ((uint16_t(pgm_read_word(table_address + 2)) * uint8_t(step_rate & 0x0007)) >> 3); + } + } + #endif +} + +// Get the timer interval and the number of loops to perform per tick +uint32_t Stepper::calc_timer_interval(uint32_t step_rate, uint8_t &loops) { + uint8_t multistep = 1; + #if DISABLED(DISABLE_MULTI_STEPPING) + + // The stepping frequency limits for each multistepping rate + static const uint32_t limit[] PROGMEM = { + ( MAX_STEP_ISR_FREQUENCY_1X ), + ( MAX_STEP_ISR_FREQUENCY_2X >> 1), + ( MAX_STEP_ISR_FREQUENCY_4X >> 2), + ( MAX_STEP_ISR_FREQUENCY_8X >> 3), + ( MAX_STEP_ISR_FREQUENCY_16X >> 4), + ( MAX_STEP_ISR_FREQUENCY_32X >> 5), + ( MAX_STEP_ISR_FREQUENCY_64X >> 6), + (MAX_STEP_ISR_FREQUENCY_128X >> 7) + }; + + // Select the proper multistepping + uint8_t idx = 0; + while (idx < 7 && step_rate > (uint32_t)pgm_read_dword(&limit[idx])) { + step_rate >>= 1; + multistep <<= 1; + ++idx; + }; + #else + NOMORE(step_rate, uint32_t(MAX_STEP_ISR_FREQUENCY_1X)); + #endif + loops = multistep; + + return calc_timer_interval(step_rate); +} + // This is the last half of the stepper interrupt: This one processes and // properly schedules blocks from the planner. This is executed after creating // the step pulses, so it is not time critical, as pulses are already done. @@ -1964,15 +2015,14 @@ uint32_t Stepper::block_phase_isr() { // acc_step_rate is in steps/second // step_rate to timer interval and steps per stepper isr - interval = calc_timer_interval(acc_step_rate, &steps_per_isr); + interval = calc_timer_interval(acc_step_rate << oversampling_factor, steps_per_isr); acceleration_time += interval; #if ENABLED(LIN_ADVANCE) - if (LA_use_advance_lead) { - // Fire ISR if final adv_rate is reached - if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0; + if (current_block->la_advance_rate) { + const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; + la_interval = calc_timer_interval(acc_step_rate + la_step_rate) << current_block->la_scaling; } - else if (LA_steps) nextAdvanceISR = 0; #endif /** @@ -2035,18 +2085,41 @@ uint32_t Stepper::block_phase_isr() { #endif // step_rate to timer interval and steps per stepper isr - interval = calc_timer_interval(step_rate, &steps_per_isr); + interval = calc_timer_interval(step_rate << oversampling_factor, steps_per_isr); deceleration_time += interval; #if ENABLED(LIN_ADVANCE) - if (LA_use_advance_lead) { - // Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached - if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) { - initiateLA(); - LA_isr_rate = current_block->advance_speed; + if (current_block->la_advance_rate) { + const uint32_t la_step_rate = la_advance_steps > current_block->final_adv_steps ? current_block->la_advance_rate : 0; + if (la_step_rate != step_rate) { + bool reverse_e = la_step_rate > step_rate; + la_interval = calc_timer_interval(reverse_e ? la_step_rate - step_rate : step_rate - la_step_rate) << current_block->la_scaling; + + if (reverse_e != motor_direction(E_AXIS)) { + TBI(last_direction_bits, E_AXIS); + count_direction.e = -count_direction.e; + + DIR_WAIT_BEFORE(); + + if (reverse_e) { + #if ENABLED(MIXING_EXTRUDER) + MIXER_STEPPER_LOOP(j) REV_E_DIR(j); + #else + REV_E_DIR(stepper_extruder); + #endif + } + else { + #if ENABLED(MIXING_EXTRUDER) + MIXER_STEPPER_LOOP(j) NORM_E_DIR(j); + #else + NORM_E_DIR(stepper_extruder); + #endif + } + + DIR_WAIT_AFTER(); + } } } - else if (LA_steps) nextAdvanceISR = 0; #endif // LIN_ADVANCE /* @@ -2069,15 +2142,15 @@ uint32_t Stepper::block_phase_isr() { } else { // Must be in cruise phase otherwise - #if ENABLED(LIN_ADVANCE) - // If there are any esteps, fire the next advance_isr "now" - if (LA_steps && LA_isr_rate != current_block->advance_speed) initiateLA(); - #endif - // Calculate the ticks_nominal for this nominal speed, if not done yet if (ticks_nominal < 0) { // step_rate to timer interval and loops for the nominal speed - ticks_nominal = calc_timer_interval(current_block->nominal_rate, &steps_per_isr); + ticks_nominal = calc_timer_interval(current_block->nominal_rate << oversampling_factor, steps_per_isr); + + #if ENABLED(LIN_ADVANCE) + if (current_block->la_advance_rate) + la_interval = calc_timer_interval(current_block->nominal_rate) << current_block->la_scaling; + #endif } // The timer interval is just the nominal value for the nominal speed @@ -2291,7 +2364,7 @@ uint32_t Stepper::block_phase_isr() { step_event_count = current_block->step_event_count << oversampling; // Initialize Bresenham delta errors to 1/2 - delta_error = -int32_t(step_event_count); + delta_error = TERN_(LIN_ADVANCE, la_delta_error =) -int32_t(step_event_count); // Calculate Bresenham dividends and divisors advance_dividend = current_block->steps << 1; @@ -2312,16 +2385,12 @@ uint32_t Stepper::block_phase_isr() { #if ENABLED(LIN_ADVANCE) #if DISABLED(MIXING_EXTRUDER) && E_STEPPERS > 1 // If the now active extruder wasn't in use during the last move, its pressure is most likely gone. - if (stepper_extruder != last_moved_extruder) LA_current_adv_steps = 0; + if (stepper_extruder != last_moved_extruder) la_advance_steps = 0; #endif - - if ((LA_use_advance_lead = current_block->use_advance_lead)) { - LA_final_adv_steps = current_block->final_adv_steps; - LA_max_adv_steps = current_block->max_adv_steps; - initiateLA(); // Start the ISR - LA_isr_rate = current_block->advance_speed; + if (current_block->la_advance_rate) { + // apply LA scaling and discount the effect of frequency scaling + la_dividend = (advance_dividend.e << current_block->la_scaling) << oversampling; } - else LA_isr_rate = LA_ADV_NEVER; #endif if ( ENABLED(DUAL_X_CARRIAGE) // TODO: Find out why this fixes "jittery" small circles @@ -2375,7 +2444,15 @@ uint32_t Stepper::block_phase_isr() { #endif // Calculate the initial timer interval - interval = calc_timer_interval(current_block->initial_rate, &steps_per_isr); + interval = calc_timer_interval(current_block->initial_rate << oversampling_factor, steps_per_isr); + acceleration_time += interval; + + #if ENABLED(LIN_ADVANCE) + if (current_block->la_advance_rate) { + const uint32_t la_step_rate = la_advance_steps < current_block->max_adv_steps ? current_block->la_advance_rate : 0; + la_interval = calc_timer_interval(current_block->initial_rate + la_step_rate) << current_block->la_scaling; + } + #endif } } @@ -2386,71 +2463,15 @@ uint32_t Stepper::block_phase_isr() { #if ENABLED(LIN_ADVANCE) // Timer interrupt for E. LA_steps is set in the main routine - uint32_t Stepper::advance_isr() { - uint32_t interval; - - if (LA_use_advance_lead) { - if (step_events_completed > decelerate_after && LA_current_adv_steps > LA_final_adv_steps) { - LA_steps--; - LA_current_adv_steps--; - interval = LA_isr_rate; - } - else if (step_events_completed < decelerate_after && LA_current_adv_steps < LA_max_adv_steps) { - LA_steps++; - LA_current_adv_steps++; - interval = LA_isr_rate; - } - else - interval = LA_isr_rate = LA_ADV_NEVER; - } - else - interval = LA_ADV_NEVER; - - if (!LA_steps) return interval; // Leave pins alone if there are no steps! - - DIR_WAIT_BEFORE(); - - #if ENABLED(MIXING_EXTRUDER) - // We don't know which steppers will be stepped because LA loop follows, - // with potentially multiple steps. Set all. - if (LA_steps > 0) { - MIXER_STEPPER_LOOP(j) NORM_E_DIR(j); - count_direction.e = 1; - } - else if (LA_steps < 0) { - MIXER_STEPPER_LOOP(j) REV_E_DIR(j); - count_direction.e = -1; - } - #else - if (LA_steps > 0) { - NORM_E_DIR(stepper_extruder); - count_direction.e = 1; - } - else if (LA_steps < 0) { - REV_E_DIR(stepper_extruder); - count_direction.e = -1; - } - #endif - - DIR_WAIT_AFTER(); - - //const hal_timer_t added_step_ticks = hal_timer_t(ADDED_STEP_TICKS); - - // Step E stepper if we have steps - #if ISR_MULTI_STEPS - bool firstStep = true; - USING_TIMED_PULSE(); - #endif - - while (LA_steps) { - #if ISR_MULTI_STEPS - if (firstStep) - firstStep = false; - else - AWAIT_LOW_PULSE(); - #endif - + void Stepper::advance_isr() { + // Apply Bresenham algorithm so that linear advance can piggy back on + // the acceleration and speed values calculated in block_phase_isr(). + // This helps keep LA in sync with, for example, S_CURVE_ACCELERATION. + la_delta_error += la_dividend; + if (la_delta_error >= 0) { count_position.e += count_direction.e; + la_advance_steps += count_direction.e; + la_delta_error -= advance_divisor; // Set the STEP pulse ON #if ENABLED(MIXING_EXTRUDER) @@ -2461,12 +2482,8 @@ uint32_t Stepper::block_phase_isr() { // Enforce a minimum duration for STEP pulse ON #if ISR_PULSE_CONTROL + USING_TIMED_PULSE(); START_HIGH_PULSE(); - #endif - - LA_steps < 0 ? ++LA_steps : --LA_steps; - - #if ISR_PULSE_CONTROL AWAIT_HIGH_PULSE(); #endif @@ -2476,15 +2493,7 @@ uint32_t Stepper::block_phase_isr() { #else E_STEP_WRITE(stepper_extruder, INVERT_E_STEP_PIN); #endif - - // For minimum pulse time wait before looping - // Just wait for the requested pulse duration - #if ISR_PULSE_CONTROL - if (LA_steps) START_LOW_PULSE(); - #endif - } // LA_steps - - return interval; + } } #endif // LIN_ADVANCE diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index d8fb5af229..ccf342b573 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -417,10 +417,11 @@ class Stepper { #if ENABLED(LIN_ADVANCE) static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF; - static uint32_t nextAdvanceISR, LA_isr_rate; - static uint16_t LA_current_adv_steps, LA_final_adv_steps, LA_max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early". - static int8_t LA_steps; - static bool LA_use_advance_lead; + static uint32_t nextAdvanceISR, + la_interval; // Interval between ISR calls for LA + static int32_t la_delta_error, // Analogue of delta_error.e for E steps in LA ISR + la_dividend, // Analogue of advance_dividend.e for E steps in LA ISR + la_advance_steps; // Count of steps added to increase nozzle pressure #endif #if ENABLED(INTEGRATED_BABYSTEPPING) @@ -475,8 +476,7 @@ class Stepper { #if ENABLED(LIN_ADVANCE) // The Linear advance ISR phase - static uint32_t advance_isr(); - FORCE_INLINE static void initiateLA() { nextAdvanceISR = 0; } + static void advance_isr(); #endif #if ENABLED(INTEGRATED_BABYSTEPPING) @@ -512,6 +512,7 @@ class Stepper { current_block = nullptr; axis_did_move = 0; planner.release_current_block(); + TERN_(LIN_ADVANCE, la_interval = nextAdvanceISR = LA_ADV_NEVER); } // Quickly stop all steppers @@ -631,65 +632,9 @@ class Stepper { // Set the current position in steps static void _set_position(const abce_long_t &spos); - FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t *loops) { - uint32_t timer; - - // Scale the frequency, as requested by the caller - step_rate <<= oversampling_factor; - - uint8_t multistep = 1; - #if DISABLED(DISABLE_MULTI_STEPPING) - - // The stepping frequency limits for each multistepping rate - static const uint32_t limit[] PROGMEM = { - ( MAX_STEP_ISR_FREQUENCY_1X ), - ( MAX_STEP_ISR_FREQUENCY_2X >> 1), - ( MAX_STEP_ISR_FREQUENCY_4X >> 2), - ( MAX_STEP_ISR_FREQUENCY_8X >> 3), - ( MAX_STEP_ISR_FREQUENCY_16X >> 4), - ( MAX_STEP_ISR_FREQUENCY_32X >> 5), - ( MAX_STEP_ISR_FREQUENCY_64X >> 6), - (MAX_STEP_ISR_FREQUENCY_128X >> 7) - }; - - // Select the proper multistepping - uint8_t idx = 0; - while (idx < 7 && step_rate > (uint32_t)pgm_read_dword(&limit[idx])) { - step_rate >>= 1; - multistep <<= 1; - ++idx; - }; - #else - NOMORE(step_rate, uint32_t(MAX_STEP_ISR_FREQUENCY_1X)); - #endif - *loops = multistep; - - #ifdef CPU_32_BIT - // In case of high-performance processor, it is able to calculate in real-time - timer = uint32_t(STEPPER_TIMER_RATE) / step_rate; - #else - constexpr uint32_t min_step_rate = (F_CPU) / 500000U; - NOLESS(step_rate, min_step_rate); - step_rate -= min_step_rate; // Correct for minimal speed - if (step_rate >= (8 * 256)) { // higher step rate - const uint8_t tmp_step_rate = (step_rate & 0x00FF); - const uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0], - gain = (uint16_t)pgm_read_word(table_address + 2); - timer = MultiU16X8toH16(tmp_step_rate, gain); - timer = (uint16_t)pgm_read_word(table_address) - timer; - } - else { // lower step rates - uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0]; - table_address += ((step_rate) >> 1) & 0xFFFC; - timer = (uint16_t)pgm_read_word(table_address) - - (((uint16_t)pgm_read_word(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3); - } - // (there is no need to limit the timer value here. All limits have been - // applied above, and AVR is able to keep up at 30khz Stepping ISR rate) - #endif - - return timer; - } + // Calculate timing interval for the given step rate + static uint32_t calc_timer_interval(uint32_t step_rate); + static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t &loops); #if ENABLED(S_CURVE_ACCELERATION) static void _calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t av);