From 450830a8f6d33f430d9fcaea16ff49dc77b7072f Mon Sep 17 00:00:00 2001 From: queeup Date: Fri, 4 Oct 2024 02:41:50 +0300 Subject: [PATCH] backport TMC2208 linear advance fix https://github.com/MarlinFirmware/Marlin/issues/16076 https://github.com/MarlinFirmware/Marlin/pull/25442 https://github.com/MarlinFirmware/Marlin/pull/25445 https://github.com/MarlinFirmware/Marlin/pull/24533 https://github.com/MarlinFirmware/Marlin/pull/25541 https://github.com/MarlinFirmware/Marlin/pull/25557 https://github.com/MarlinFirmware/Marlin/pull/25696 https://github.com/MarlinFirmware/Marlin/pull/26881 --- Marlin/src/HAL/ESP32/timers.h | 3 +- Marlin/src/module/planner.cpp | 80 ++++---- Marlin/src/module/planner.h | 11 +- Marlin/src/module/stepper.cpp | 369 ++++++++++++++++++---------------- Marlin/src/module/stepper.h | 97 +++------ 5 files changed, 262 insertions(+), 298 deletions(-) diff --git a/Marlin/src/HAL/ESP32/timers.h b/Marlin/src/HAL/ESP32/timers.h index a47697113d..f7fbf3547e 100644 --- a/Marlin/src/HAL/ESP32/timers.h +++ b/Marlin/src/HAL/ESP32/timers.h @@ -53,12 +53,11 @@ typedef uint64_t hal_timer_t; #if ENABLED(I2S_STEPPER_STREAM) #define STEPPER_TIMER_PRESCALE 1 #define STEPPER_TIMER_RATE 250000 // 250khz, 4µs pulses of i2s word clock - #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs // wrong would be 0.25 #else #define STEPPER_TIMER_PRESCALE 40 #define STEPPER_TIMER_RATE ((HAL_TIMER_RATE) / (STEPPER_TIMER_PRESCALE)) // frequency of stepper timer, 2MHz - #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs #endif +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs #define STEP_TIMER_MIN_INTERVAL 8 // minimum time in µs between stepper interrupts diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index b6b7f2a538..26bfe5907e 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -713,8 +713,6 @@ void Planner::init() { #endif #endif -#define MINIMAL_STEP_RATE 120 - /** * Get the current block for processing * and mark the block as busy. @@ -781,10 +779,11 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second) // Limit minimal step rate (Otherwise the timer will overflow.) - NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE)); - NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE)); + NOLESS(initial_rate, stepper.minimal_step_rate); + NOLESS(final_rate, stepper.minimal_step_rate); + NOLESS(block->nominal_rate, stepper.minimal_step_rate); - #if ENABLED(S_CURVE_ACCELERATION) + #if EITHER(S_CURVE_ACCELERATION, LIN_ADVANCE) uint32_t cruise_rate = initial_rate; #endif @@ -805,7 +804,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); plateau_steps = 0; - #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 @@ -837,6 +836,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 + /** * Laser trapezoid calculations * @@ -1183,13 +1190,6 @@ void Planner::recalculate_trapezoids() { const float current_nominal_speed = SQRT(block->nominal_speed_sqr), nomr = 1.0f / current_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 = current_nominal_speed * comp; - block->final_adv_steps = next_entry_speed * comp; - } - #endif } // Reset current only to ensure next trapezoid is computed - The @@ -1222,13 +1222,6 @@ void Planner::recalculate_trapezoids() { const float next_nominal_speed = SQRT(next->nominal_speed_sqr), nomr = 1.0f / next_nominal_speed; calculate_trapezoid_for_block(next, next_entry_speed * nomr, float(MINIMUM_PLANNER_SPEED) * nomr); - #if ENABLED(LIN_ADVANCE) - if (next->use_advance_lead) { - const float comp = next->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS]; - next->max_adv_steps = next_nominal_speed * comp; - next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp; - } - #endif } // Reset next only to ensure its trapezoid is computed - The stepper is free to use @@ -2282,9 +2275,11 @@ bool Planner::_populate_block(block_t * const block, bool split_move, // 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 (!block->steps.a && !block->steps.b && !block->steps.c) { // 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{ \ @@ -2317,27 +2312,23 @@ bool Planner::_populate_block(block_t * const block, bool split_move, * * 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; - - if (block->use_advance_lead) { - block->e_D_ratio = (target_float.e - position_float.e) / - #if IS_KINEMATIC - block->millimeters - #else + use_advance_lead = esteps && extruder_advance_K[extruder] && de > 0; + + 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); @@ -2365,13 +2356,20 @@ bool Planner::_populate_block(block_t * const block, bool split_move, block->acceleration_rate = (uint32_t)(accel * (sq(4096.0f) / (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 < SQRT(block->nominal_speed_sqr) * 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 5ca31471f2..30f616bea8 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -210,11 +210,10 @@ typedef struct block_t { // 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 @@ -977,7 +976,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 1b01ac4588..132dfbecba 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -214,17 +214,12 @@ uint32_t Stepper::advance_divisor = 0, #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 + hal_timer_t Stepper::nextAdvanceISR = LA_ADV_NEVER, + Stepper::la_interval = LA_ADV_NEVER; + int32_t Stepper::la_delta_error = 0, + Stepper::la_dividend = 0, + Stepper::la_advance_steps = 0; +#endif #if HAS_SHAPING shaping_time_t ShapingQueue::now = 0; @@ -249,14 +244,14 @@ uint32_t Stepper::advance_divisor = 0, #endif #if ENABLED(INTEGRATED_BABYSTEPPING) - uint32_t Stepper::nextBabystepISR = BABYSTEP_NEVER; + hal_timer_t Stepper::nextBabystepISR = BABYSTEP_NEVER; #endif #if ENABLED(DIRECT_STEPPING) page_step_state_t Stepper::page_step_state; #endif -int32_t Stepper::ticks_nominal = -1; +hal_timer_t Stepper::ticks_nominal = 0; #if DISABLED(S_CURVE_ACCELERATION) uint32_t Stepper::acc_step_rate; // needed for deceleration start point #endif @@ -508,29 +503,27 @@ void Stepper::set_directions() { SET_STEP_DIR(Z); // C #endif - #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; - } - #else - 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 #if HAS_L64XX if (L64XX_OK_to_power_up) { // OK to send the direction commands (which powers up the L64XX steppers) @@ -1383,7 +1376,7 @@ HAL_STEP_TIMER_ISR() { void Stepper::isr() { - static uint32_t nextMainISR = 0; // Interval until the next main Stepper Pulse phase (0 = Now) + static hal_timer_t nextMainISR = 0; // Interval until the next main Stepper Pulse phase (0 = Now) #ifndef __AVR__ // Disable interrupts, to avoid ISR preemption while we reprogram the period @@ -1413,7 +1406,12 @@ void Stepper::isr() { TERN_(HAS_SHAPING, shaping_isr()); #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_interval) // Start/accelerate LA steps if necessary + nextAdvanceISR = la_interval; #endif #if ENABLED(INTEGRATED_BABYSTEPPING) @@ -1434,7 +1432,8 @@ void Stepper::isr() { #endif // Get the interval to the next ISR call - const uint32_t interval = _MIN( + const hal_timer_t interval = _MIN( + hal_timer_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time nextMainISR // Time until the next Pulse / Block phase OPTARG(INPUT_SHAPING_X, ShapingQueue::peek_x()) // Time until next input shaping echo for X OPTARG(INPUT_SHAPING_Y, ShapingQueue::peek_y()) // Time until next input shaping echo for Y @@ -1532,7 +1531,7 @@ void Stepper::isr() { // sure that the time has not arrived yet - Warrantied by the scheduler // Set the next ISR to fire at the proper time - HAL_timer_set_compare(STEP_TIMER_NUM, hal_timer_t(next_isr_ticks)); + HAL_timer_set_compare(STEP_TIMER_NUM, next_isr_ticks); // Don't forget to finally reenable interrupts ENABLE_ISRS(); @@ -1789,20 +1788,16 @@ void Stepper::pulse_phase_isr() { PULSE_PREP(Z); #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; + la_advance_steps--; + } + #endif #endif #if HAS_SHAPING @@ -1842,12 +1837,10 @@ void Stepper::pulse_phase_isr() { PULSE_START(Z); #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 #if ENABLED(I2S_STEPPER_STREAM) @@ -1871,15 +1864,10 @@ void Stepper::pulse_phase_isr() { PULSE_STOP(Z); #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 @@ -1945,10 +1933,74 @@ void Stepper::pulse_phase_isr() { // 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. -uint32_t Stepper::block_phase_isr() { + +// Calculate timer interval, with all limits applied. +hal_timer_t Stepper::calc_timer_interval(uint32_t step_rate) { + #ifdef CPU_32_BIT + // A fast processor can just do integer division + return step_rate > minimal_step_rate ? uint32_t(STEPPER_TIMER_RATE) / step_rate : HAL_TIMER_TYPE_MAX; + #else + // AVR is able to keep up at 30khz Stepping ISR rate. + if (step_rate >= 0x0800) { // higher step rate + // AVR is able to keep up at around 65kHz Stepping ISR rate at most. + // So values for step_rate > 65535 might as well be truncated. + // Handle it as quickly as possible. i.e., assume highest byte is zero + // because non-zero would represent a step rate far beyond AVR capabilities. + if (uint8_t(step_rate >> 16)) + return uint32_t(STEPPER_TIMER_RATE) / 0x10000; + + const uintptr_t table_address = uintptr_t(&speed_lookuptable_fast[uint8_t(step_rate >> 8)]); + const uint16_t base = uint16_t(pgm_read_word(table_address)); + const uint8_t gain = uint8_t(pgm_read_byte(table_address + 2)); + return base - MultiU8X8toH8(uint8_t(step_rate & 0x00FF), gain); + } + else if (step_rate > minimal_step_rate) { // lower step rates + step_rate -= minimal_step_rate; // Correct for minimal speed + const uintptr_t table_address = uintptr_t(&speed_lookuptable_slow[uint8_t(step_rate >> 3)]); + return uint16_t(pgm_read_word(table_address)) + - ((uint16_t(pgm_read_word(table_address + 2)) * uint8_t(step_rate & 0x0007)) >> 3); + } + + return uint16_t(pgm_read_word(uintptr_t(speed_lookuptable_slow))); + #endif +} + +// Get the timer interval and the number of loops to perform per tick +hal_timer_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); +} + +hal_timer_t Stepper::block_phase_isr() { // If no queued movements, just wait 1ms for the next block - uint32_t interval = (STEPPER_TIMER_RATE) / 1000UL; + hal_timer_t interval = (STEPPER_TIMER_RATE) / 1000UL; // If there is a current block if (current_block) { @@ -1993,15 +2045,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 // Update laser - Accelerating @@ -2067,18 +2118,43 @@ uint32_t Stepper::block_phase_isr() { // step_rate is in steps/second // 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 + la_interval = LA_ADV_NEVER; } - else if (LA_steps) nextAdvanceISR = 0; #endif // LIN_ADVANCE // Update laser - Decelerating @@ -2112,16 +2188,15 @@ uint32_t Stepper::block_phase_isr() { } // Must be in cruise phase otherwise else { - - #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) { + 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 +2366,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; @@ -2335,16 +2410,13 @@ 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(HAS_L64XX) // Always set direction for L64xx (Also enables the chips) @@ -2405,7 +2477,7 @@ uint32_t Stepper::block_phase_isr() { #endif // Mark the time_nominal as not calculated yet - ticks_nominal = -1; + ticks_nominal = 0; #if ENABLED(S_CURVE_ACCELERATION) // Initialize the Bézier speed curve @@ -2418,7 +2490,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 } #if ENABLED(LASER_POWER_INLINE_CONTINUOUS) else { // No new block found; so apply inline laser parameters @@ -2444,71 +2524,16 @@ 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) @@ -2519,12 +2544,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 @@ -2534,15 +2555,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 @@ -2550,7 +2563,7 @@ uint32_t Stepper::block_phase_isr() { #if ENABLED(INTEGRATED_BABYSTEPPING) // Timer interrupt for baby-stepping - uint32_t Stepper::babystepping_isr() { + hal_timer_t Stepper::babystepping_isr() { babystep.task(); return babystep.has_steps() ? BABYSTEP_TICKS : BABYSTEP_NEVER; } diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index deac36ca49..66276e5a04 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -383,6 +383,16 @@ class Stepper { public: + // The minimal step rate ensures calculations stay within limits + // and avoid the most unreasonably slow step rates. + static constexpr uint32_t minimal_step_rate = ( + #ifdef CPU_32_BIT + _MAX((STEPPER_TIMER_RATE) / HAL_TIMER_TYPE_MAX, 1U) // 32-bit shouldn't go below 1 + #else + (F_CPU) / 500000U // AVR shouldn't go below 32 (16MHz) or 40 (20MHz) + #endif + ); + #if EITHER(HAS_EXTRA_ENDSTOPS, Z_STEPPER_AUTO_ALIGN) static bool separate_multi_axis; #endif @@ -484,23 +494,24 @@ class Stepper { #endif #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 constexpr hal_timer_t LA_ADV_NEVER = HAL_TIMER_TYPE_MAX; + static hal_timer_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) - static constexpr uint32_t BABYSTEP_NEVER = 0xFFFFFFFF; - static uint32_t nextBabystepISR; + static constexpr hal_timer_t BABYSTEP_NEVER = HAL_TIMER_TYPE_MAX; + static hal_timer_t nextBabystepISR; #endif #if ENABLED(DIRECT_STEPPING) static page_step_state_t page_step_state; #endif - static int32_t ticks_nominal; + static hal_timer_t ticks_nominal; #if DISABLED(S_CURVE_ACCELERATION) static uint32_t acc_step_rate; // needed for deceleration start point #endif @@ -558,7 +569,7 @@ class Stepper { static void pulse_phase_isr(); // The stepper block processing ISR phase - static uint32_t block_phase_isr(); + static hal_timer_t block_phase_isr(); #if HAS_SHAPING static void shaping_isr(); @@ -566,13 +577,12 @@ 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) // The Babystepping ISR phase - static uint32_t babystepping_isr(); + static hal_timer_t babystepping_isr(); FORCE_INLINE static void initiateBabystepping() { if (nextBabystepISR == BABYSTEP_NEVER) { nextBabystepISR = 0; @@ -619,6 +629,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 @@ -709,65 +720,9 @@ class Stepper { static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e); FORCE_INLINE static void _set_position(const abce_long_t &spos) { _set_position(spos.a, spos.b, spos.c, spos.e); } - 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 hal_timer_t calc_timer_interval(uint32_t step_rate); + static hal_timer_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);