From 34fe801cee54afedb46af9319fff0c875bc5a24c Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Tue, 25 Oct 2022 22:38:11 +0100 Subject: [PATCH 01/33] Frequency = 0 now disables input shaping --- .../src/gcode/feature/input_shaping/M593.cpp | 5 +- Marlin/src/module/planner.cpp | 17 ++- Marlin/src/module/planner.h | 6 +- Marlin/src/module/stepper.cpp | 107 +++++++++++------- Marlin/src/module/stepper.h | 20 +++- 5 files changed, 101 insertions(+), 54 deletions(-) diff --git a/Marlin/src/gcode/feature/input_shaping/M593.cpp b/Marlin/src/gcode/feature/input_shaping/M593.cpp index e1e99ca51b7a..6eac366ec48d 100644 --- a/Marlin/src/gcode/feature/input_shaping/M593.cpp +++ b/Marlin/src/gcode/feature/input_shaping/M593.cpp @@ -72,12 +72,13 @@ void GcodeSuite::M593() { if (parser.seen('F')) { const float freq = parser.value_float(); - if (freq > 0) { + constexpr float max_freq = float(uint32_t(STEPPER_TIMER_RATE) / 2) / shaping_time_t(-2); + if (freq == 0.0f || freq > max_freq) { if (for_X) stepper.set_shaping_frequency(X_AXIS, freq); if (for_Y) stepper.set_shaping_frequency(Y_AXIS, freq); } else - SERIAL_ECHO_MSG("?Frequency (F) must be greater than 0"); + SERIAL_ECHOLNPGM("?Frequency (F) must be greater than ", max_freq, " or 0 to disable"); } } diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index dce91606647f..e6d990fab140 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1724,6 +1724,13 @@ float Planner::triggered_position_mm(const AxisEnum axis) { return result * mm_per_step[axis]; } +bool Planner::busy() { + return (has_blocks_queued() || cleaning_buffer_counter + || TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING()) + || TERN0(INPUT_SHAPING, stepper.input_shaping_busy()) + ); +} + void Planner::finish_and_disable() { while (has_blocks_queued() || cleaning_buffer_counter) idle(); stepper.disable_all_steppers(); @@ -2484,10 +2491,12 @@ bool Planner::_populate_block( #endif // XY_FREQUENCY_LIMIT #if ENABLED(INPUT_SHAPING) - const float top_freq = _MIN(float(0x7FFFFFFFL) - OPTARG(HAS_SHAPING_X, stepper.get_shaping_frequency(X_AXIS)) - OPTARG(HAS_SHAPING_Y, stepper.get_shaping_frequency(Y_AXIS))), - max_factor = (top_freq * float(shaping_dividends - 3) * 2.0f) / block->nominal_rate; + float bottom_freq = float(0x7FFFFFFFL); + if (TERN0(HAS_SHAPING_X, stepper.get_shaping_frequency(X_AXIS))) + NOMORE(bottom_freq, stepper.get_shaping_frequency(X_AXIS)); + if (TERN0(HAS_SHAPING_Y, stepper.get_shaping_frequency(Y_AXIS))) + NOMORE(bottom_freq, stepper.get_shaping_frequency(Y_AXIS)); + const float max_factor = (bottom_freq * (float(shaping_dividends - 3) * 2.0f)) / block->nominal_rate; NOMORE(speed_factor, max_factor); #endif diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index cfd7b455165f..c269e88fc9e9 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -930,11 +930,7 @@ class Planner { static float triggered_position_mm(const AxisEnum axis); // Blocks are queued, or we're running out moves, or the closed loop controller is waiting - static bool busy() { - return (has_blocks_queued() || cleaning_buffer_counter - || TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING()) - ); - } + static bool busy(); // Block until all buffered steps are executed / cleaned static void synchronize(); diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 6cc40ccecee9..cd9ddb4d37c0 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1639,8 +1639,8 @@ void Stepper::pulse_phase_isr() { #if ENABLED(INPUT_SHAPING) shaping_dividend_queue.purge(); shaping_queue.purge(); - TERN_(HAS_SHAPING_X, delta_error.x = 0); - TERN_(HAS_SHAPING_Y, delta_error.y = 0); + if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) delta_error.x = 0; + if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) delta_error.y = 0; #endif } } @@ -1823,18 +1823,16 @@ void Stepper::pulse_phase_isr() { // Determine if pulses are needed #if HAS_X_STEP - #if HAS_SHAPING_X + if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) PULSE_PREP_SHAPING(X, advance_dividend.x); - #else + else PULSE_PREP(X); - #endif #endif #if HAS_Y_STEP - #if HAS_SHAPING_Y + if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) PULSE_PREP_SHAPING(Y, advance_dividend.y); - #else + else PULSE_PREP(Y); - #endif #endif #if HAS_Z_STEP PULSE_PREP(Z); @@ -2491,44 +2489,56 @@ uint32_t Stepper::block_phase_isr() { advance_dividend = (current_block->steps << 1).asLong(); advance_divisor = step_event_count << 1; + #if HAS_SHAPING_X + int32_t echo_x; + #endif + #if HAS_SHAPING_Y + int32_t echo_y; + #endif + // for input shaped axes, advance_divisor is replaced with 0x40000000 // and steps are repeated twice so dividends have to be scaled and halved // and the dividend is directional, i.e. signed - TERN_(HAS_SHAPING_X, advance_dividend.x = (uint64_t(current_block->steps.x) << 29) / step_event_count); - TERN_(HAS_SHAPING_X, if (TEST(current_block->direction_bits, X_AXIS)) advance_dividend.x *= -1); - TERN_(HAS_SHAPING_X, if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS))); - TERN_(HAS_SHAPING_Y, advance_dividend.y = (uint64_t(current_block->steps.y) << 29) / step_event_count); - TERN_(HAS_SHAPING_Y, if (TEST(current_block->direction_bits, Y_AXIS)) advance_dividend.y *= -1); - TERN_(HAS_SHAPING_Y, if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS))); - - // The scaling operation above introduces rounding errors which must now be removed. - // For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes. - // For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000 - // so (e.g. for x) delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. - // For a divisor which is a power of 2, modulo is the same as as a bitmask, i.e. - // (advance_dividend.x * step_event_count) & 0x1FFFFFFF. - // This segment's final change in delta_error should actually be zero so we need to increase delta_error by - // 0 - ((advance_dividend.x * step_event_count) & 0x1FFFFFFF) - // And this needs to be adjusted to the range -0x10000000 to 0x10000000. - // Adding and subtracting 0x10000000 inside the outside the modulo achieves this. - TERN_(HAS_SHAPING_X, delta_error.x = old_delta_error_x + 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL)); - TERN_(HAS_SHAPING_Y, delta_error.y = old_delta_error_y + 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL)); - - // when there is damping, the signal and its echo have different amplitudes - #if ENABLED(HAS_SHAPING_X) - const int32_t echo_x = shaping_x.factor * (advance_dividend.x >> 7); - #endif - #if ENABLED(HAS_SHAPING_Y) - const int32_t echo_y = shaping_y.factor * (advance_dividend.y >> 7); - #endif + if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) { + TERN_(HAS_SHAPING_X, advance_dividend.x = (uint64_t(current_block->steps.x) << 29) / step_event_count); + TERN_(HAS_SHAPING_X, if (TEST(current_block->direction_bits, X_AXIS)) advance_dividend.x *= -1); + TERN_(HAS_SHAPING_X, if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS))); + + // The scaling operation above introduces rounding errors which must now be removed. + // For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes. + // For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000 + // so (e.g. for x) delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. + // For a divisor which is a power of 2, modulo is the same as as a bitmask, i.e. + // (advance_dividend.x * step_event_count) & 0x1FFFFFFF. + // This segment's final change in delta_error should actually be zero so we need to increase delta_error by + // 0 - ((advance_dividend.x * step_event_count) & 0x1FFFFFFF) + // And this needs to be adjusted to the range -0x10000000 to 0x10000000. + // Adding and subtracting 0x10000000 inside the outside the modulo achieves this. + TERN_(HAS_SHAPING_X, delta_error.x = old_delta_error_x + 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL)); + + // when there is damping, the signal and its echo have different amplitudes + TERN_(HAS_SHAPING_X, echo_x = shaping_x.factor * (advance_dividend.x >> 7)); + + // apply the adjustment to the primary signal + TERN_(HAS_SHAPING_X, advance_dividend.x -= echo_x); + } + + // Y follows the same logic as X (but the comments aren't repeated) + if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) { + TERN_(HAS_SHAPING_Y, advance_dividend.y = (uint64_t(current_block->steps.y) << 29) / step_event_count); + TERN_(HAS_SHAPING_Y, if (TEST(current_block->direction_bits, Y_AXIS)) advance_dividend.y *= -1); + TERN_(HAS_SHAPING_Y, if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS))); + + TERN_(HAS_SHAPING_Y, delta_error.y = old_delta_error_y + 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL)); + + TERN_(HAS_SHAPING_Y, echo_y = shaping_y.factor * (advance_dividend.y >> 7)); + + TERN_(HAS_SHAPING_Y, advance_dividend.y -= echo_y); + } // plan the change of values for advance_dividend for the input shaping echoes TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y))); - // apply the adjustment to the primary signal - TERN_(HAS_SHAPING_X, advance_dividend.x -= echo_x); - TERN_(HAS_SHAPING_Y, advance_dividend.y -= echo_y); - // No step events completed so far step_events_completed = 0; @@ -3008,8 +3018,23 @@ void Stepper::init() { } void Stepper::set_shaping_frequency(const AxisEnum axis, const float freq) { - TERN_(HAS_SHAPING_X, if (axis == X_AXIS) { DelayTimeManager::set_delay(axis, float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq); shaping_x.frequency = freq; }) - TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) { DelayTimeManager::set_delay(axis, float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq); shaping_y.frequency = freq; }) + // enabling or disabling shaping whilst moving can result in lost steps + Planner::synchronize(); + + const bool was_on = hal.isr_state(); + hal.isr_off(); + + const shaping_time_t delay = freq ? float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq : shaping_time_t(-1); + if (TERN0(HAS_SHAPING_X, axis == X_AXIS)) { + DelayTimeManager::set_delay(X_AXIS, delay); + TERN_(HAS_SHAPING_X, shaping_x.frequency = freq); + } + if (TERN0(HAS_SHAPING_Y, axis == Y_AXIS)) { + DelayTimeManager::set_delay(Y_AXIS, delay); + TERN_(HAS_SHAPING_Y, shaping_y.frequency = freq); + } + + if (was_on) hal.isr_on(); } float Stepper::get_shaping_frequency(const AxisEnum axis) { diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 5b634c52e476..b54f4ab9df83 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -338,10 +338,10 @@ constexpr ena_mask_t enable_overlap[] = { private: static shaping_time_t now; #ifdef HAS_SHAPING_X - static shaping_time_t delay_x; + static shaping_time_t delay_x; // = shaping_time_t(-1) to disable queueing #endif #ifdef HAS_SHAPING_Y - static shaping_time_t delay_y; + static shaping_time_t delay_y; // = shaping_time_t(-1) to disable queueing #endif public: static void decrement_delays(const shaping_time_t interval) { now += interval; } @@ -361,6 +361,8 @@ constexpr ena_mask_t enable_overlap[] = { void enqueue() { times[tail] = now; if (++tail == SIZE) tail = 0; + TERN_(HAS_SHAPING_X, if (delay_x == shaping_time_t(-1)) head_x = tail); + TERN_(HAS_SHAPING_Y, if (delay_y == shaping_time_t(-1)) head_y = tail); } #ifdef HAS_SHAPING_X shaping_time_t peek_x() { @@ -620,6 +622,20 @@ class Stepper { // Check if the given block is busy or not - Must not be called from ISR contexts static bool is_block_busy(const block_t * const block); + #if ENABLED(INPUT_SHAPING) + // Check whether the stepper is processing any input shaping echoes + static bool input_shaping_busy() { + const bool was_on = hal.isr_state(); + hal.isr_off(); + + const bool result = !shaping_queue.empty_x() || !shaping_queue.empty_y(); + + if (was_on) hal.isr_on(); + + return result; + } + #endif + // Get the position of a stepper, in steps static int32_t position(const AxisEnum axis); From 518a43ee7e6cee84b68f0c87cc37f585c453b554 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Fri, 28 Oct 2022 12:41:10 +0100 Subject: [PATCH 02/33] Adjusted ADAPTIVE_STEP_SMOOTHING calculation to take input shaping into account --- Marlin/src/module/stepper.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index b54f4ab9df83..7512caaa65d3 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -202,8 +202,11 @@ #error "Expected at least one of MINIMUM_STEPPER_PULSE or MAXIMUM_STEPPER_RATE to be defined" #endif +// The main loop can be called up to three times as often with input shaping +#define INPUT_SHAPING_FACTOR (1 + TERN0(HAS_SHAPING_X, 1) + TERN0(HAS_SHAPING_Y, 1)) + // But the user could be enforcing a minimum time, so the loop time is -#define ISR_LOOP_CYCLES (ISR_LOOP_BASE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES)) +#define ISR_LOOP_CYCLES ((ISR_LOOP_BASE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES)) * INPUT_SHAPING_FACTOR) // If linear advance is enabled, then it is handled separately #if ENABLED(LIN_ADVANCE) From aaacf79eb0de4cbe71df66d58716004aa00072ed Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sat, 29 Oct 2022 17:22:23 +0100 Subject: [PATCH 03/33] Fixed synchronisation error between echo steps and changes in dividends --- Marlin/src/module/stepper.cpp | 17 ++++++++++------- Marlin/src/module/stepper.h | 2 +- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index cd9ddb4d37c0..b9769bc74e7d 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1971,13 +1971,6 @@ void Stepper::pulse_phase_isr() { const bool shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()), shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); - #if HAS_SHAPING_X - if (!shaping_dividend_queue.peek_x()) shaping_x.dividend = shaping_dividend_queue.dequeue_x(); - #endif - #if HAS_SHAPING_Y - if (!shaping_dividend_queue.peek_y()) shaping_y.dividend = shaping_dividend_queue.dequeue_y(); - #endif - #if HAS_SHAPING_X if (shapex) { shaping_queue.dequeue_x(); @@ -2009,6 +2002,16 @@ void Stepper::pulse_phase_isr() { if (shapey) PULSE_STOP(Y); #endif } + + // if a new dividend appears in the same "moment" that a step takes place, it is because + // the step is the last step of a segment and the new dividend is for the new segment + // so only update the dividend after the step + #if HAS_SHAPING_X + if (!shaping_dividend_queue.peek_x()) shaping_x.dividend = shaping_dividend_queue.dequeue_x(); + #endif + #if HAS_SHAPING_Y + if (!shaping_dividend_queue.peek_y()) shaping_y.dividend = shaping_dividend_queue.dequeue_y(); + #endif } #endif // INPUT_SHAPING diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 7512caaa65d3..92e122bee792 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -423,7 +423,7 @@ constexpr ena_mask_t enable_overlap[] = { float frequency; float zeta; uint8_t factor; - int32_t dividend; + int32_t dividend = 0UL; }; #endif // INPUT_SHAPING From 58797c8610b759344c6557b51264954fd342a573 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sat, 29 Oct 2022 17:37:47 +0100 Subject: [PATCH 04/33] Laser seems to work okay with input shaping, so allow it --- Marlin/src/inc/SanityCheck.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 744e68a5bb0d..5915dada4703 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4277,8 +4277,6 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive."); #if ENABLED(INPUT_SHAPING) #if ENABLED(DIRECT_STEPPING) #error "INPUT_SHAPING cannot currently be used with DIRECT_STEPPING." - #elif ENABLED(LASER_FEATURE) - #error "INPUT_SHAPING cannot currently be used with LASER_FEATURE." #endif #endif From 2023140d84075d2043e75674b394dbfabd3d47a2 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sat, 29 Oct 2022 17:24:48 +0100 Subject: [PATCH 05/33] Ensure the double pulse generated on direction change is slow enough not to confuse the stepper driver and remove a very edge case for direction flipping twice in succession --- Marlin/src/module/stepper.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index b9769bc74e7d..397e695189e2 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1684,11 +1684,17 @@ void Stepper::pulse_phase_isr() { #define PULSE_PREP_SHAPING(AXIS, DIVIDEND) do{ \ delta_error[_AXIS(AXIS)] += (DIVIDEND); \ - if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x30000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x30000000L)) { \ + if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] < -0x30000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] > 0x30000000L)) { \ TBI(last_direction_bits, _AXIS(AXIS)); \ DIR_WAIT_BEFORE(); \ SET_STEP_DIR(AXIS); \ DIR_WAIT_AFTER(); \ + _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), 0); \ + { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_HIGH_PULSE(); } \ + _APPLY_STEP(AXIS, _INVERT_STEP_PIN(AXIS), 0); \ + count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ + delta_error[_AXIS(AXIS)] += MAXDIR(AXIS) ? -0x20000000L : 0x20000000L; \ + { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } \ } \ step_needed[_AXIS(AXIS)] = (MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x10000000L) || \ (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x10000000L); \ From 60966e2b8ffca733fba1452a43995931b49f1a71 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sat, 29 Oct 2022 17:31:19 +0100 Subject: [PATCH 06/33] Spread the tiny rounding errors caused by scaling over subsequent segments --- Marlin/src/module/stepper.cpp | 48 ++++++++++++++++++++--------------- Marlin/src/module/stepper.h | 1 + 2 files changed, 28 insertions(+), 21 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 397e695189e2..cfd760683f5f 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -2493,6 +2493,8 @@ uint32_t Stepper::block_phase_isr() { const int32_t old_delta_error_y = delta_error.y; #endif delta_error = TERN_(LIN_ADVANCE, la_delta_error =) -int32_t(step_event_count); + TERN_(HAS_SHAPING_X, delta_error.x = old_delta_error_x); + TERN_(HAS_SHAPING_Y, delta_error.y = old_delta_error_y); // Calculate Bresenham dividends and divisors advance_dividend = (current_block->steps << 1).asLong(); @@ -2505,25 +2507,31 @@ uint32_t Stepper::block_phase_isr() { int32_t echo_y; #endif - // for input shaped axes, advance_divisor is replaced with 0x40000000 - // and steps are repeated twice so dividends have to be scaled and halved - // and the dividend is directional, i.e. signed if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) { - TERN_(HAS_SHAPING_X, advance_dividend.x = (uint64_t(current_block->steps.x) << 29) / step_event_count); - TERN_(HAS_SHAPING_X, if (TEST(current_block->direction_bits, X_AXIS)) advance_dividend.x *= -1); - TERN_(HAS_SHAPING_X, if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS))); - - // The scaling operation above introduces rounding errors which must now be removed. + // For input shaped axes, advance_divisor is replaced with 0x20000000 + // and the dividend is directional, i.e. signed. + const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); + TERN_(HAS_SHAPING_X, advance_dividend.x = ((steps << 29) + shaping_x.remainder) / step_event_count); + TERN_(HAS_SHAPING_X, LIMIT(advance_dividend.x, -0x20000000, 0x20000000)); + + // The scaling operation above introduces small rounding errors which are recorded in shaping_x.remainder + // and carried over to the next segment. // For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes. // For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000 - // so (e.g. for x) delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. - // For a divisor which is a power of 2, modulo is the same as as a bitmask, i.e. + // so delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. + // For a divisor which is a power of 2, modulo is the same as as a bitmask, so that is // (advance_dividend.x * step_event_count) & 0x1FFFFFFF. - // This segment's final change in delta_error should actually be zero so we need to increase delta_error by - // 0 - ((advance_dividend.x * step_event_count) & 0x1FFFFFFF) - // And this needs to be adjusted to the range -0x10000000 to 0x10000000. - // Adding and subtracting 0x10000000 inside the outside the modulo achieves this. - TERN_(HAS_SHAPING_X, delta_error.x = old_delta_error_x + 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL)); + // Since the bitmask removed the sign, this needs to be adjusted to the range -0x10000000 to 0x10000000. + // Adding and subtracting 0x10000000 inside the outside the modulo achieves this, i.e. + // ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL) - 0x10000000L + // This segment's final change in delta_error should actually be shaping_x.remainder so the new remainder is + // shaping_x.remainder - (((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL) - 0x10000000L) + TERN_(HAS_SHAPING_X, shaping_x.remainder += 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL)); + + // If there are any remaining echos unprocessed, then direction change must + // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step + // to be missed, which will need recovering and this can be done through shaping_x.remainder. + TERN_(HAS_SHAPING_X, if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS))); // when there is damping, the signal and its echo have different amplitudes TERN_(HAS_SHAPING_X, echo_x = shaping_x.factor * (advance_dividend.x >> 7)); @@ -2534,14 +2542,12 @@ uint32_t Stepper::block_phase_isr() { // Y follows the same logic as X (but the comments aren't repeated) if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) { - TERN_(HAS_SHAPING_Y, advance_dividend.y = (uint64_t(current_block->steps.y) << 29) / step_event_count); - TERN_(HAS_SHAPING_Y, if (TEST(current_block->direction_bits, Y_AXIS)) advance_dividend.y *= -1); + const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); + TERN_(HAS_SHAPING_Y, advance_dividend.y = ((steps << 29) + shaping_y.remainder) / step_event_count); + TERN_(HAS_SHAPING_Y, LIMIT(advance_dividend.y, -0x20000000, 0x20000000)); + TERN_(HAS_SHAPING_Y, shaping_y.remainder += 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL)); TERN_(HAS_SHAPING_Y, if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS))); - - TERN_(HAS_SHAPING_Y, delta_error.y = old_delta_error_y + 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL)); - TERN_(HAS_SHAPING_Y, echo_y = shaping_y.factor * (advance_dividend.y >> 7)); - TERN_(HAS_SHAPING_Y, advance_dividend.y -= echo_y); } diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 92e122bee792..5292046f597d 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -424,6 +424,7 @@ constexpr ena_mask_t enable_overlap[] = { float zeta; uint8_t factor; int32_t dividend = 0UL; + int32_t remainder = 0UL; }; #endif // INPUT_SHAPING From c87be2e941090ec3bf987076ec355467f455f646 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 31 Oct 2022 13:28:37 +0000 Subject: [PATCH 07/33] Remember steps that have not yet happened because of echoes when processing a sync block --- Marlin/src/module/stepper.cpp | 21 ++++++++++++++++++++- Marlin/src/module/stepper.h | 1 + 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index cfd760683f5f..db287eca9a70 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1641,6 +1641,8 @@ void Stepper::pulse_phase_isr() { shaping_queue.purge(); if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) delta_error.x = 0; if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) delta_error.y = 0; + TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); + TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); #endif } } @@ -2508,9 +2510,11 @@ uint32_t Stepper::block_phase_isr() { #endif if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) { + const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); + TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos += steps); + // For input shaped axes, advance_divisor is replaced with 0x20000000 // and the dividend is directional, i.e. signed. - const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); TERN_(HAS_SHAPING_X, advance_dividend.x = ((steps << 29) + shaping_x.remainder) / step_event_count); TERN_(HAS_SHAPING_X, LIMIT(advance_dividend.x, -0x20000000, 0x20000000)); @@ -2543,6 +2547,7 @@ uint32_t Stepper::block_phase_isr() { // Y follows the same logic as X (but the comments aren't repeated) if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) { const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); + TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos += steps); TERN_(HAS_SHAPING_Y, advance_dividend.y = ((steps << 29) + shaping_y.remainder) / step_event_count); TERN_(HAS_SHAPING_Y, LIMIT(advance_dividend.y, -0x20000000, 0x20000000)); TERN_(HAS_SHAPING_Y, shaping_y.remainder += 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL)); @@ -3069,6 +3074,13 @@ void Stepper::init() { * derive the current XYZE position later on. */ void Stepper::_set_position(const abce_long_t &spos) { + #if HAS_SHAPING_X + int32_t x_shaping_delta = count_position.x - shaping_x.last_block_end_pos; + #endif + #if HAS_SHAPING_Y + int32_t y_shaping_delta = count_position.y - shaping_y.last_block_end_pos; + #endif + #if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX) #if CORE_IS_XY // corexy positioning @@ -3098,6 +3110,11 @@ void Stepper::_set_position(const abce_long_t &spos) { // default non-h-bot planning count_position = spos; #endif + + TERN_(HAS_SHAPING_X, count_position.x += x_shaping_delta); + TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = spos.x); + TERN_(HAS_SHAPING_Y, count_position.y += y_shaping_delta); + TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = spos.y); } /** @@ -3137,6 +3154,8 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) { #endif count_position[a] = v; + TERN_(HAS_SHAPING_X, if (a == X_AXIS) shaping_x.last_block_end_pos = v); + TERN_(HAS_SHAPING_Y, if (a == Y_AXIS) shaping_y.last_block_end_pos = v); #ifdef __AVR__ // Reenable Stepper ISR diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 5292046f597d..ceaf0bd61e83 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -425,6 +425,7 @@ constexpr ena_mask_t enable_overlap[] = { uint8_t factor; int32_t dividend = 0UL; int32_t remainder = 0UL; + int32_t last_block_end_pos = 0; }; #endif // INPUT_SHAPING From c5865dac679ff948f2dad7751a16f0f816a570f9 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Thu, 3 Nov 2022 13:34:26 +0000 Subject: [PATCH 08/33] Fixed some errors around IS being disabled for specific axes --- Marlin/src/module/stepper.cpp | 27 ++++++++++++++++++++------- Marlin/src/module/stepper.h | 4 ++-- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index db287eca9a70..f41e08cfb25d 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -2503,14 +2503,15 @@ uint32_t Stepper::block_phase_isr() { advance_divisor = step_event_count << 1; #if HAS_SHAPING_X - int32_t echo_x; + int32_t echo_x = 0; #endif #if HAS_SHAPING_Y - int32_t echo_y; + int32_t echo_y = 0; #endif if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) { const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); + UNUSED(steps); TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos += steps); // For input shaped axes, advance_divisor is replaced with 0x20000000 @@ -2547,6 +2548,7 @@ uint32_t Stepper::block_phase_isr() { // Y follows the same logic as X (but the comments aren't repeated) if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) { const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); + UNUSED(steps); TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos += steps); TERN_(HAS_SHAPING_Y, advance_dividend.y = ((steps << 29) + shaping_y.remainder) / step_event_count); TERN_(HAS_SHAPING_Y, LIMIT(advance_dividend.y, -0x20000000, 0x20000000)); @@ -2557,7 +2559,8 @@ uint32_t Stepper::block_phase_isr() { } // plan the change of values for advance_dividend for the input shaping echoes - TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y))); + if (TERN0(HAS_SHAPING_X, shaping_x.frequency) || TERN0(HAS_SHAPING_Y, shaping_y.frequency)) + TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y))); // No step events completed so far step_events_completed = 0; @@ -3048,10 +3051,16 @@ void Stepper::init() { if (TERN0(HAS_SHAPING_X, axis == X_AXIS)) { DelayTimeManager::set_delay(X_AXIS, delay); TERN_(HAS_SHAPING_X, shaping_x.frequency = freq); + delta_error = 0L; + TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); + TERN_(HAS_SHAPING_X, shaping_x.dividend = shaping_x.remainder = 0UL); } if (TERN0(HAS_SHAPING_Y, axis == Y_AXIS)) { DelayTimeManager::set_delay(Y_AXIS, delay); TERN_(HAS_SHAPING_Y, shaping_y.frequency = freq); + delta_error = 0L; + TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); + TERN_(HAS_SHAPING_Y, shaping_y.dividend = shaping_y.remainder = 0UL); } if (was_on) hal.isr_on(); @@ -3111,10 +3120,14 @@ void Stepper::_set_position(const abce_long_t &spos) { count_position = spos; #endif - TERN_(HAS_SHAPING_X, count_position.x += x_shaping_delta); - TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = spos.x); - TERN_(HAS_SHAPING_Y, count_position.y += y_shaping_delta); - TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = spos.y); + if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) { + TERN_(HAS_SHAPING_X, count_position.x += x_shaping_delta); + TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = spos.x); + } + if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) { + TERN_(HAS_SHAPING_Y, count_position.y += y_shaping_delta); + TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = spos.y); + } } /** diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index ceaf0bd61e83..01c22cb5d0c3 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -385,7 +385,7 @@ constexpr ena_mask_t enable_overlap[] = { bool empty_y() { return head_y == tail; } uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; } #endif - void purge() { auto temp = TERN_(HAS_SHAPING_X, head_x) = TERN_(HAS_SHAPING_Y, head_y) = tail; UNUSED(temp);} + void purge() { auto temp = TERN_(HAS_SHAPING_X, head_x =) TERN_(HAS_SHAPING_Y, head_y =) tail; UNUSED(temp);} }; class ParamDelayQueue : public DelayQueue { @@ -633,7 +633,7 @@ class Stepper { const bool was_on = hal.isr_state(); hal.isr_off(); - const bool result = !shaping_queue.empty_x() || !shaping_queue.empty_y(); + const bool result = TERN0(HAS_SHAPING_X, !shaping_queue.empty_x()) || TERN0(HAS_SHAPING_Y, !shaping_queue.empty_y()); if (was_on) hal.isr_on(); From c9458cd2fadad08d9fb2ea0999d1377a568ebb25 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Thu, 3 Nov 2022 16:34:15 +0000 Subject: [PATCH 09/33] Handle multi-stepping in input shaping --- Marlin/src/module/stepper.cpp | 72 ++++++++++++++++++++--------------- 1 file changed, 41 insertions(+), 31 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index f41e08cfb25d..5b244705465d 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1976,44 +1976,54 @@ void Stepper::pulse_phase_isr() { void Stepper::shaping_isr() { xyze_bool_t step_needed{0}; - const bool shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()), - shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); + bool shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()), + shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); - #if HAS_SHAPING_X - if (shapex) { - shaping_queue.dequeue_x(); - PULSE_PREP_SHAPING(X, shaping_x.dividend); - PULSE_START(X); - } - #endif - - #if HAS_SHAPING_Y - if (shapey) { - shaping_queue.dequeue_y(); - PULSE_PREP_SHAPING(Y, shaping_y.dividend); - PULSE_START(Y); - } - #endif - - TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); - - if (shapex || shapey) { - #if ISR_MULTI_STEPS - USING_TIMED_PULSE(); - START_TIMED_PULSE(); - AWAIT_HIGH_PULSE(); - #endif + while (true) { #if HAS_SHAPING_X - if (shapex) PULSE_STOP(X); + if (shapex) { + shaping_queue.dequeue_x(); + PULSE_PREP_SHAPING(X, shaping_x.dividend); + PULSE_START(X); + } #endif + #if HAS_SHAPING_Y - if (shapey) PULSE_STOP(Y); + if (shapey) { + shaping_queue.dequeue_y(); + PULSE_PREP_SHAPING(Y, shaping_y.dividend); + PULSE_START(Y); + } #endif + + TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); + + USING_TIMED_PULSE(); + if (shapex || shapey) { + #if ISR_MULTI_STEPS + START_TIMED_PULSE(); + AWAIT_HIGH_PULSE(); + #endif + #if HAS_SHAPING_X + if (shapex) PULSE_STOP(X); + #endif + #if HAS_SHAPING_Y + if (shapey) PULSE_STOP(Y); + #endif + } + + shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()); + shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); + + if (!shapex && !shapey) break; + + START_TIMED_PULSE(); + AWAIT_LOW_PULSE(); } - // if a new dividend appears in the same "moment" that a step takes place, it is because - // the step is the last step of a segment and the new dividend is for the new segment - // so only update the dividend after the step + // if a new dividend appears in the same "moment" that steps takes place, it is because + // the steps are the last steps of a segment and the new dividend is for the new segment + // so only update the dividend after the steps #if HAS_SHAPING_X if (!shaping_dividend_queue.peek_x()) shaping_x.dividend = shaping_dividend_queue.dequeue_x(); #endif From 1c67a616660d0ffcb4168cea4158d5a1f277e27b Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sat, 5 Nov 2022 14:58:41 +0000 Subject: [PATCH 10/33] Allow disabling input shaping from the menu and make it clear in config how to disable per axis. --- Marlin/Configuration_adv.h | 4 ++-- Marlin/src/lcd/language/language_en.h | 8 +++---- Marlin/src/lcd/menu/menu_advanced.cpp | 30 +++++++++++++++------------ 3 files changed, 23 insertions(+), 19 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 9a54b10b82b1..c69d520fe71c 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1079,8 +1079,8 @@ */ //#define INPUT_SHAPING #if ENABLED(INPUT_SHAPING) - #define SHAPING_FREQ_X 40 // (Hz) The dominant resonant frequency of the X axis. - #define SHAPING_FREQ_Y 40 // (Hz) The dominant resonant frequency of the Y axis. + #define SHAPING_FREQ_X 40 // (Hz) The dominant resonant frequency of the X axis. Comment to disable shaping for X. + #define SHAPING_FREQ_Y 40 // (Hz) The dominant resonant frequency of the Y axis. Comment to disable shaping for Y. #define SHAPING_ZETA_X 0.3f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). #define SHAPING_ZETA_Y 0.3f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. diff --git a/Marlin/src/lcd/language/language_en.h b/Marlin/src/lcd/language/language_en.h index c888207ed123..f84c9d3e9d42 100644 --- a/Marlin/src/lcd/language/language_en.h +++ b/Marlin/src/lcd/language/language_en.h @@ -400,10 +400,10 @@ namespace Language_en { LSTR MSG_A_RETRACT = _UxGT("Retract Accel"); LSTR MSG_A_TRAVEL = _UxGT("Travel Accel"); LSTR MSG_INPUT_SHAPING = _UxGT("Input Shaping"); - LSTR MSG_SHAPING_X_FREQ = STR_X _UxGT(" frequency"); - LSTR MSG_SHAPING_Y_FREQ = STR_Y _UxGT(" frequency"); - LSTR MSG_SHAPING_X_ZETA = STR_X _UxGT(" damping"); - LSTR MSG_SHAPING_Y_ZETA = STR_Y _UxGT(" damping"); + LSTR MSG_SHAPING_ENABLE = _UxGT("Enable @ shaping"); + LSTR MSG_SHAPING_DISABLE = _UxGT("Disable @ shaping"); + LSTR MSG_SHAPING_FREQ = _UxGT("@ frequency"); + LSTR MSG_SHAPING_ZETA = _UxGT("@ damping"); LSTR MSG_XY_FREQUENCY_LIMIT = _UxGT("XY Freq Limit"); LSTR MSG_XY_FREQUENCY_FEEDRATE = _UxGT("Min FR Factor"); LSTR MSG_STEPS_PER_MM = _UxGT("Steps/mm"); diff --git a/Marlin/src/lcd/menu/menu_advanced.cpp b/Marlin/src/lcd/menu/menu_advanced.cpp index 9d6d79efd741..68daffab1a59 100644 --- a/Marlin/src/lcd/menu/menu_advanced.cpp +++ b/Marlin/src/lcd/menu/menu_advanced.cpp @@ -545,24 +545,28 @@ void menu_backlash(); START_MENU(); BACK_ITEM(MSG_ADVANCED_SETTINGS); - // M593 F Frequency + // M593 F Frequency and D Damping ratio #if HAS_SHAPING_X editable.decimal = stepper.get_shaping_frequency(X_AXIS); - EDIT_ITEM_FAST(float61, MSG_SHAPING_X_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(X_AXIS, editable.decimal); }); + if (editable.decimal) { + ACTION_ITEM_N(X_AXIS, MSG_SHAPING_DISABLE, []{ stepper.set_shaping_frequency(X_AXIS, 0.0f); }); + EDIT_ITEM_FAST_N(float61, X_AXIS, MSG_SHAPING_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(X_AXIS, editable.decimal); }); + editable.decimal = stepper.get_shaping_damping_ratio(X_AXIS); + EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_SHAPING_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(X_AXIS, editable.decimal); }); + } + else + ACTION_ITEM_N(X_AXIS, MSG_SHAPING_ENABLE, []{ stepper.set_shaping_frequency(X_AXIS, SHAPING_FREQ_X); }); #endif #if HAS_SHAPING_Y editable.decimal = stepper.get_shaping_frequency(Y_AXIS); - EDIT_ITEM_FAST(float61, MSG_SHAPING_Y_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(Y_AXIS, editable.decimal); }); - #endif - - // M593 D Damping ratio - #if HAS_SHAPING_X - editable.decimal = stepper.get_shaping_damping_ratio(X_AXIS); - EDIT_ITEM_FAST(float42_52, MSG_SHAPING_X_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(X_AXIS, editable.decimal); }); - #endif - #if HAS_SHAPING_Y - editable.decimal = stepper.get_shaping_damping_ratio(Y_AXIS); - EDIT_ITEM_FAST(float42_52, MSG_SHAPING_Y_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(Y_AXIS, editable.decimal); }); + if (editable.decimal) { + ACTION_ITEM_N(Y_AXIS, MSG_SHAPING_DISABLE, []{ stepper.set_shaping_frequency(Y_AXIS, 0.0f); }); + EDIT_ITEM_FAST_N(float61, Y_AXIS, MSG_SHAPING_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(Y_AXIS, editable.decimal); }); + editable.decimal = stepper.get_shaping_damping_ratio(Y_AXIS); + EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_SHAPING_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(Y_AXIS, editable.decimal); }); + } + else + ACTION_ITEM_N(Y_AXIS, MSG_SHAPING_ENABLE, []{ stepper.set_shaping_frequency(Y_AXIS, SHAPING_FREQ_Y); }); #endif END_MENU(); From 8e6aae5fc1854281c7d23169a6953cf73b8e47f2 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Wed, 9 Nov 2022 21:40:40 +0000 Subject: [PATCH 11/33] Cache the values for peek_x and peek_y which saves a lot of cycles in shaping calcs --- Marlin/src/module/stepper.cpp | 2 ++ Marlin/src/module/stepper.h | 37 +++++++++++++++++++++++------------ 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 5b244705465d..a6915e597a98 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1541,6 +1541,8 @@ void Stepper::isr() { nextMainISR -= interval; TERN_(INPUT_SHAPING, DelayTimeManager::decrement_delays(interval)); + TERN_(INPUT_SHAPING, shaping_queue.decrement_peeks(interval)); + TERN_(INPUT_SHAPING, shaping_dividend_queue.decrement_peeks(interval)); #if ENABLED(LIN_ADVANCE) if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval; diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 01c22cb5d0c3..47c94e772e7e 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -358,34 +358,45 @@ constexpr ena_mask_t enable_overlap[] = { class DelayQueue : public DelayTimeManager { protected: shaping_time_t times[SIZE]; + shaping_time_t peek_x_val = shaping_time_t(-1), + peek_y_val = shaping_time_t(-1); uint16_t tail = 0 OPTARG(HAS_SHAPING_X, head_x = 0) OPTARG(HAS_SHAPING_Y, head_y = 0); public: + void decrement_peeks(const shaping_time_t interval) { + if (peek_x_val != shaping_time_t(-1)) peek_x_val -= interval; + if (peek_y_val != shaping_time_t(-1)) peek_y_val -= interval; + } void enqueue() { + TERN_(HAS_SHAPING_X, if (head_x == tail) peek_x_val = delay_x); + TERN_(HAS_SHAPING_Y, if (head_y == tail) peek_y_val = delay_y); times[tail] = now; if (++tail == SIZE) tail = 0; TERN_(HAS_SHAPING_X, if (delay_x == shaping_time_t(-1)) head_x = tail); TERN_(HAS_SHAPING_Y, if (delay_y == shaping_time_t(-1)) head_y = tail); } #ifdef HAS_SHAPING_X - shaping_time_t peek_x() { - if (head_x != tail) return times[head_x] + delay_x - now; - else return shaping_time_t(-1); + inline shaping_time_t peek_x() { return peek_x_val; } + inline void dequeue_x() { + if (++head_x == SIZE) head_x = 0; + peek_x_val = head_x == tail ? shaping_time_t(-1) : times[head_x] + delay_x - now; } - void dequeue_x() { if (++head_x == SIZE) head_x = 0; } - bool empty_x() { return head_x == tail; } - uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; } + inline bool empty_x() { return head_x == tail; } + inline uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; } #endif #ifdef HAS_SHAPING_Y - shaping_time_t peek_y() { - if (head_y != tail) return times[head_y] + delay_y - now; - else return shaping_time_t(-1); + inline shaping_time_t peek_y() { return peek_y_val; } + inline void dequeue_y() { + if (++head_y == SIZE) head_y = 0; + peek_y_val = head_y == tail ? shaping_time_t(-1) : times[head_y] + delay_y - now; } - void dequeue_y() { if (++head_y == SIZE) head_y = 0; } - bool empty_y() { return head_y == tail; } - uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; } + inline bool empty_y() { return head_y == tail; } + inline uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; } #endif - void purge() { auto temp = TERN_(HAS_SHAPING_X, head_x =) TERN_(HAS_SHAPING_Y, head_y =) tail; UNUSED(temp);} + void purge() { + const auto temp1 = TERN_(HAS_SHAPING_X, head_x =) TERN_(HAS_SHAPING_Y, head_y =) tail; UNUSED(temp1); + const auto temp2 = TERN_(HAS_SHAPING_X, peek_x_val =) TERN_(HAS_SHAPING_Y, peek_y_val =) shaping_time_t(-1); UNUSED(temp2); + } }; class ParamDelayQueue : public DelayQueue { From b76a8df21fa753289e1b27d5b18ec9bf7c910ec1 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Wed, 9 Nov 2022 21:44:29 +0000 Subject: [PATCH 12/33] Testing a bool for shaping being enabled is a lot cheaper than testing a float --- Marlin/src/module/stepper.cpp | 20 +++++++++++--------- Marlin/src/module/stepper.h | 1 + 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index a6915e597a98..27959330f604 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1641,8 +1641,8 @@ void Stepper::pulse_phase_isr() { #if ENABLED(INPUT_SHAPING) shaping_dividend_queue.purge(); shaping_queue.purge(); - if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) delta_error.x = 0; - if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) delta_error.y = 0; + if (TERN0(HAS_SHAPING_X, shaping_x.enabled)) delta_error.x = 0; + if (TERN0(HAS_SHAPING_Y, shaping_y.enabled)) delta_error.y = 0; TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); #endif @@ -1833,13 +1833,13 @@ void Stepper::pulse_phase_isr() { // Determine if pulses are needed #if HAS_X_STEP - if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) + if (TERN0(HAS_SHAPING_X, shaping_x.enabled)) PULSE_PREP_SHAPING(X, advance_dividend.x); else PULSE_PREP(X); #endif #if HAS_Y_STEP - if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) + if (TERN0(HAS_SHAPING_Y, shaping_y.enabled)) PULSE_PREP_SHAPING(Y, advance_dividend.y); else PULSE_PREP(Y); @@ -2521,7 +2521,7 @@ uint32_t Stepper::block_phase_isr() { int32_t echo_y = 0; #endif - if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) { + if (TERN0(HAS_SHAPING_X, shaping_x.enabled)) { const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); UNUSED(steps); TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos += steps); @@ -2558,7 +2558,7 @@ uint32_t Stepper::block_phase_isr() { } // Y follows the same logic as X (but the comments aren't repeated) - if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) { + if (TERN0(HAS_SHAPING_Y, shaping_y.enabled)) { const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); UNUSED(steps); TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos += steps); @@ -2571,7 +2571,7 @@ uint32_t Stepper::block_phase_isr() { } // plan the change of values for advance_dividend for the input shaping echoes - if (TERN0(HAS_SHAPING_X, shaping_x.frequency) || TERN0(HAS_SHAPING_Y, shaping_y.frequency)) + if (TERN0(HAS_SHAPING_X, shaping_x.enabled) || TERN0(HAS_SHAPING_Y, shaping_y.enabled)) TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y))); // No step events completed so far @@ -3063,6 +3063,7 @@ void Stepper::init() { if (TERN0(HAS_SHAPING_X, axis == X_AXIS)) { DelayTimeManager::set_delay(X_AXIS, delay); TERN_(HAS_SHAPING_X, shaping_x.frequency = freq); + TERN_(HAS_SHAPING_X, shaping_x.enabled = !!freq); delta_error = 0L; TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); TERN_(HAS_SHAPING_X, shaping_x.dividend = shaping_x.remainder = 0UL); @@ -3070,6 +3071,7 @@ void Stepper::init() { if (TERN0(HAS_SHAPING_Y, axis == Y_AXIS)) { DelayTimeManager::set_delay(Y_AXIS, delay); TERN_(HAS_SHAPING_Y, shaping_y.frequency = freq); + TERN_(HAS_SHAPING_Y, shaping_y.enabled = !!freq); delta_error = 0L; TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); TERN_(HAS_SHAPING_Y, shaping_y.dividend = shaping_y.remainder = 0UL); @@ -3132,11 +3134,11 @@ void Stepper::_set_position(const abce_long_t &spos) { count_position = spos; #endif - if (TERN0(HAS_SHAPING_X, shaping_x.frequency)) { + if (TERN0(HAS_SHAPING_X, shaping_x.enabled)) { TERN_(HAS_SHAPING_X, count_position.x += x_shaping_delta); TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = spos.x); } - if (TERN0(HAS_SHAPING_Y, shaping_y.frequency)) { + if (TERN0(HAS_SHAPING_Y, shaping_y.enabled)) { TERN_(HAS_SHAPING_Y, count_position.y += y_shaping_delta); TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = spos.y); } diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 47c94e772e7e..7095ef17205c 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -433,6 +433,7 @@ constexpr ena_mask_t enable_overlap[] = { struct ShapeParams { float frequency; float zeta; + bool enabled; uint8_t factor; int32_t dividend = 0UL; int32_t remainder = 0UL; From 3d094f1918cd79c0f28f1e3eac64fcc671b3a110 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Wed, 9 Nov 2022 21:45:57 +0000 Subject: [PATCH 13/33] shaping_isr() gets called a lot, so optimised it --- Marlin/src/module/stepper.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 27959330f604..988fe549fe0f 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1976,12 +1976,12 @@ void Stepper::pulse_phase_isr() { #if ENABLED(INPUT_SHAPING) void Stepper::shaping_isr() { - xyze_bool_t step_needed{0}; + xy_bool_t step_needed{0}; bool shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()), shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); - while (true) { + if (shapex || shapey) while (true) { #if HAS_SHAPING_X if (shapex) { shaping_queue.dequeue_x(); @@ -2001,16 +2001,16 @@ void Stepper::pulse_phase_isr() { TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); USING_TIMED_PULSE(); - if (shapex || shapey) { + if (step_needed.x || step_needed.y) { #if ISR_MULTI_STEPS START_TIMED_PULSE(); AWAIT_HIGH_PULSE(); #endif #if HAS_SHAPING_X - if (shapex) PULSE_STOP(X); + PULSE_STOP(X); #endif #if HAS_SHAPING_Y - if (shapey) PULSE_STOP(Y); + PULSE_STOP(Y); #endif } From 5d10dc527d1bf5d2fa0c363578f930fb6d98c38d Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Wed, 9 Nov 2022 21:47:37 +0000 Subject: [PATCH 14/33] Updated ASS calculations --- Marlin/src/module/stepper.h | 36 +++++++++++++++++++++++------------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 7095ef17205c..ad06cf6306f1 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -75,8 +75,8 @@ */ #define TIMER_READ_ADD_AND_STORE_CYCLES 34UL - // The base ISR takes 792 cycles - #define ISR_BASE_CYCLES 792UL + // The base ISR + #define ISR_BASE_CYCLES 1000UL // Linear advance base time is 64 cycles #if ENABLED(LIN_ADVANCE) @@ -92,14 +92,18 @@ #define ISR_S_CURVE_CYCLES 0UL #endif + // Input shaping base time + #if ENABLED(INPUT_SHAPING) + #define ISR_SHAPING_BASE_CYCLES 250UL + #else + #define ISR_SHAPING_BASE_CYCLES 0UL + #endif + // Stepper Loop base cycles #define ISR_LOOP_BASE_CYCLES 4UL - // To start the step pulse, in the worst case takes - #define ISR_START_STEPPER_CYCLES 13UL - // And each stepper (start + stop pulse) takes in worst case - #define ISR_STEPPER_CYCLES 16UL + #define ISR_STEPPER_CYCLES 100UL #else // Cycles to perform actions in START_TIMED_PULSE @@ -122,12 +126,16 @@ #define ISR_S_CURVE_CYCLES 0UL #endif + // Input shaping base time + #if ENABLED(INPUT_SHAPING) + #define ISR_SHAPING_BASE_CYCLES 250UL + #else + #define ISR_SHAPING_BASE_CYCLES 0UL + #endif + // Stepper Loop base cycles #define ISR_LOOP_BASE_CYCLES 32UL - // To start the step pulse, in the worst case takes - #define ISR_START_STEPPER_CYCLES 57UL - // And each stepper (start + stop pulse) takes in worst case #define ISR_STEPPER_CYCLES 88UL @@ -202,11 +210,13 @@ #error "Expected at least one of MINIMUM_STEPPER_PULSE or MAXIMUM_STEPPER_RATE to be defined" #endif -// The main loop can be called up to three times as often with input shaping -#define INPUT_SHAPING_FACTOR (1 + TERN0(HAS_SHAPING_X, 1) + TERN0(HAS_SHAPING_Y, 1)) +#define HIGH_PULSE_CYCLES uint32_t(uint64_t(F_CPU) * (_MIN_PULSE_HIGH_NS) / 1000000000UL) // But the user could be enforcing a minimum time, so the loop time is -#define ISR_LOOP_CYCLES ((ISR_LOOP_BASE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES)) * INPUT_SHAPING_FACTOR) +#define ISR_LOOP_CYCLES (ISR_LOOP_BASE_CYCLES + HIGH_PULSE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES)) + +// Model input shaping as an extra loop call +#define ISR_SHAPING_LOOP_CYCLES (TERN0(INPUT_SHAPING, ISR_LOOP_BASE_CYCLES + HIGH_PULSE_CYCLES) + TERN0(HAS_SHAPING_X, ISR_X_STEPPER_CYCLES) + TERN0(HAS_SHAPING_Y, ISR_Y_STEPPER_CYCLES)) // If linear advance is enabled, then it is handled separately #if ENABLED(LIN_ADVANCE) @@ -231,7 +241,7 @@ #endif // Now estimate the total ISR execution time in cycles given a step per ISR multiplier -#define ISR_EXECUTION_CYCLES(R) (((ISR_BASE_CYCLES + ISR_S_CURVE_CYCLES + (ISR_LOOP_CYCLES) * (R) + ISR_LA_BASE_CYCLES + ISR_LA_LOOP_CYCLES)) / (R)) +#define ISR_EXECUTION_CYCLES(R) (((ISR_BASE_CYCLES + ISR_S_CURVE_CYCLES + ISR_SHAPING_BASE_CYCLES + (ISR_LOOP_CYCLES + ISR_SHAPING_LOOP_CYCLES) * (R) + ISR_LA_BASE_CYCLES + ISR_LA_LOOP_CYCLES)) / (R)) // The maximum allowable stepping frequency when doing x128-x1 stepping (in Hz) #define MAX_STEP_ISR_FREQUENCY_128X ((F_CPU) / ISR_EXECUTION_CYCLES(128)) From 312b4c2de124e63ff4660c50505162482ccfdbf4 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 11 Nov 2022 17:22:40 -0600 Subject: [PATCH 15/33] Fix DelayQueue --- Marlin/src/module/stepper.h | 32 ++++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index ad06cf6306f1..a3f289d8cf05 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -368,14 +368,18 @@ constexpr ena_mask_t enable_overlap[] = { class DelayQueue : public DelayTimeManager { protected: shaping_time_t times[SIZE]; - shaping_time_t peek_x_val = shaping_time_t(-1), - peek_y_val = shaping_time_t(-1); + #if HAS_SHAPING_X + shaping_time_t peek_x_val = shaping_time_t(-1); + #endif + #if HAS_SHAPING_Y + shaping_time_t peek_y_val = shaping_time_t(-1); + #endif uint16_t tail = 0 OPTARG(HAS_SHAPING_X, head_x = 0) OPTARG(HAS_SHAPING_Y, head_y = 0); public: void decrement_peeks(const shaping_time_t interval) { - if (peek_x_val != shaping_time_t(-1)) peek_x_val -= interval; - if (peek_y_val != shaping_time_t(-1)) peek_y_val -= interval; + TERN_(HAS_SHAPING_X, if (peek_x_val != shaping_time_t(-1)) peek_x_val -= interval); + TERN_(HAS_SHAPING_Y, if (peek_y_val != shaping_time_t(-1)) peek_y_val -= interval); } void enqueue() { TERN_(HAS_SHAPING_X, if (head_x == tail) peek_x_val = delay_x); @@ -385,23 +389,23 @@ constexpr ena_mask_t enable_overlap[] = { TERN_(HAS_SHAPING_X, if (delay_x == shaping_time_t(-1)) head_x = tail); TERN_(HAS_SHAPING_Y, if (delay_y == shaping_time_t(-1)) head_y = tail); } - #ifdef HAS_SHAPING_X - inline shaping_time_t peek_x() { return peek_x_val; } - inline void dequeue_x() { + #if HAS_SHAPING_X + shaping_time_t peek_x() { return peek_x_val; } + void dequeue_x() { if (++head_x == SIZE) head_x = 0; peek_x_val = head_x == tail ? shaping_time_t(-1) : times[head_x] + delay_x - now; } - inline bool empty_x() { return head_x == tail; } - inline uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; } + bool empty_x() { return head_x == tail; } + uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; } #endif - #ifdef HAS_SHAPING_Y - inline shaping_time_t peek_y() { return peek_y_val; } - inline void dequeue_y() { + #if HAS_SHAPING_Y + shaping_time_t peek_y() { return peek_y_val; } + void dequeue_y() { if (++head_y == SIZE) head_y = 0; peek_y_val = head_y == tail ? shaping_time_t(-1) : times[head_y] + delay_y - now; } - inline bool empty_y() { return head_y == tail; } - inline uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; } + bool empty_y() { return head_y == tail; } + uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; } #endif void purge() { const auto temp1 = TERN_(HAS_SHAPING_X, head_x =) TERN_(HAS_SHAPING_Y, head_y =) tail; UNUSED(temp1); From d8f5e26d4d885f79661c2c9353bfd96a84daf28d Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 11 Nov 2022 17:22:51 -0600 Subject: [PATCH 16/33] misc. cleanup --- Marlin/src/module/planner.cpp | 8 +-- Marlin/src/module/stepper.cpp | 108 +++++++++++++++++----------------- Marlin/src/module/stepper.h | 17 ++++-- 3 files changed, 70 insertions(+), 63 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index e6d990fab140..ba8662e2fa98 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -2491,11 +2491,9 @@ bool Planner::_populate_block( #endif // XY_FREQUENCY_LIMIT #if ENABLED(INPUT_SHAPING) - float bottom_freq = float(0x7FFFFFFFL); - if (TERN0(HAS_SHAPING_X, stepper.get_shaping_frequency(X_AXIS))) - NOMORE(bottom_freq, stepper.get_shaping_frequency(X_AXIS)); - if (TERN0(HAS_SHAPING_Y, stepper.get_shaping_frequency(Y_AXIS))) - NOMORE(bottom_freq, stepper.get_shaping_frequency(Y_AXIS)); + float bottom_freq = float(0x7FFFFFFFL), t; + TERN_(HAS_SHAPING_X, if ((t = stepper.get_shaping_frequency(X_AXIS))) NOMORE(bottom_freq, t)); + TERN_(HAS_SHAPING_Y, if ((t = stepper.get_shaping_frequency(Y_AXIS))) NOMORE(bottom_freq, t)); const float max_factor = (bottom_freq * (float(shaping_dividends - 3) * 2.0f)) / block->nominal_rate; NOMORE(speed_factor, max_factor); #endif diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 988fe549fe0f..4e38cafa35e6 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1540,9 +1540,11 @@ void Stepper::isr() { nextMainISR -= interval; - TERN_(INPUT_SHAPING, DelayTimeManager::decrement_delays(interval)); - TERN_(INPUT_SHAPING, shaping_queue.decrement_peeks(interval)); - TERN_(INPUT_SHAPING, shaping_dividend_queue.decrement_peeks(interval)); + #if ENABLED(INPUT_SHAPING) + DelayTimeManager::decrement_delays(interval); + shaping_queue.decrement_peeks(interval); + shaping_dividend_queue.decrement_peeks(interval); + #endif #if ENABLED(LIN_ADVANCE) if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval; @@ -2001,7 +2003,7 @@ void Stepper::pulse_phase_isr() { TERN_(I2S_STEPPER_STREAM, i2s_push_sample()); USING_TIMED_PULSE(); - if (step_needed.x || step_needed.y) { + if (bool(step_needed)) { #if ISR_MULTI_STEPS START_TIMED_PULSE(); AWAIT_HIGH_PULSE(); @@ -2507,6 +2509,7 @@ uint32_t Stepper::block_phase_isr() { const int32_t old_delta_error_y = delta_error.y; #endif delta_error = TERN_(LIN_ADVANCE, la_delta_error =) -int32_t(step_event_count); + // Compiler should optimize these out, by not assigning them in the first place TERN_(HAS_SHAPING_X, delta_error.x = old_delta_error_x); TERN_(HAS_SHAPING_Y, delta_error.y = old_delta_error_y); @@ -2516,60 +2519,59 @@ uint32_t Stepper::block_phase_isr() { #if HAS_SHAPING_X int32_t echo_x = 0; + if (shaping_x.enabled) { + const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); + UNUSED(steps); + shaping_x.last_block_end_pos += steps; + + // For input shaped axes, advance_divisor is replaced with 0x20000000 + // and the dividend is directional, i.e. signed. + advance_dividend.x = ((steps << 29) + shaping_x.remainder) / step_event_count; + LIMIT(advance_dividend.x, -0x20000000, 0x20000000); + + // The scaling operation above introduces small rounding errors which are recorded in shaping_x.remainder + // and carried over to the next segment. + // For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes. + // For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000 + // so delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. + // For a divisor which is a power of 2, modulo is the same as as a bitmask, so that is + // (advance_dividend.x * step_event_count) & 0x1FFFFFFF. + // Since the bitmask removed the sign, this needs to be adjusted to the range -0x10000000 to 0x10000000. + // Adding and subtracting 0x10000000 inside the outside the modulo achieves this, i.e. + // ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL) - 0x10000000L + // This segment's final change in delta_error should actually be shaping_x.remainder so the new remainder is + // shaping_x.remainder - (((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL) - 0x10000000L) + shaping_x.remainder += 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL); + + // If there are any remaining echos unprocessed, then direction change must + // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step + // to be missed, which will need recovering and this can be done through shaping_x.remainder. + if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS)); + + // when there is damping, the signal and its echo have different amplitudes + echo_x = shaping_x.factor * (advance_dividend.x >> 7); + + // apply the adjustment to the primary signal + advance_dividend.x -= echo_x; + } #endif + + // Y follows the same logic as X (but the comments aren't repeated) #if HAS_SHAPING_Y int32_t echo_y = 0; + if (shaping_y.enabled) { + const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); + UNUSED(steps); + shaping_y.last_block_end_pos += steps; + advance_dividend.y = ((steps << 29) + shaping_y.remainder) / step_event_count; + LIMIT(advance_dividend.y, -0x20000000, 0x20000000); + shaping_y.remainder += 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL); + if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS)); + echo_y = shaping_y.factor * (advance_dividend.y >> 7); + advance_dividend.y -= echo_y; + } #endif - if (TERN0(HAS_SHAPING_X, shaping_x.enabled)) { - const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); - UNUSED(steps); - TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos += steps); - - // For input shaped axes, advance_divisor is replaced with 0x20000000 - // and the dividend is directional, i.e. signed. - TERN_(HAS_SHAPING_X, advance_dividend.x = ((steps << 29) + shaping_x.remainder) / step_event_count); - TERN_(HAS_SHAPING_X, LIMIT(advance_dividend.x, -0x20000000, 0x20000000)); - - // The scaling operation above introduces small rounding errors which are recorded in shaping_x.remainder - // and carried over to the next segment. - // For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes. - // For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000 - // so delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. - // For a divisor which is a power of 2, modulo is the same as as a bitmask, so that is - // (advance_dividend.x * step_event_count) & 0x1FFFFFFF. - // Since the bitmask removed the sign, this needs to be adjusted to the range -0x10000000 to 0x10000000. - // Adding and subtracting 0x10000000 inside the outside the modulo achieves this, i.e. - // ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL) - 0x10000000L - // This segment's final change in delta_error should actually be shaping_x.remainder so the new remainder is - // shaping_x.remainder - (((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL) - 0x10000000L) - TERN_(HAS_SHAPING_X, shaping_x.remainder += 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL)); - - // If there are any remaining echos unprocessed, then direction change must - // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step - // to be missed, which will need recovering and this can be done through shaping_x.remainder. - TERN_(HAS_SHAPING_X, if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS))); - - // when there is damping, the signal and its echo have different amplitudes - TERN_(HAS_SHAPING_X, echo_x = shaping_x.factor * (advance_dividend.x >> 7)); - - // apply the adjustment to the primary signal - TERN_(HAS_SHAPING_X, advance_dividend.x -= echo_x); - } - - // Y follows the same logic as X (but the comments aren't repeated) - if (TERN0(HAS_SHAPING_Y, shaping_y.enabled)) { - const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); - UNUSED(steps); - TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos += steps); - TERN_(HAS_SHAPING_Y, advance_dividend.y = ((steps << 29) + shaping_y.remainder) / step_event_count); - TERN_(HAS_SHAPING_Y, LIMIT(advance_dividend.y, -0x20000000, 0x20000000)); - TERN_(HAS_SHAPING_Y, shaping_y.remainder += 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL)); - TERN_(HAS_SHAPING_Y, if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS))); - TERN_(HAS_SHAPING_Y, echo_y = shaping_y.factor * (advance_dividend.y >> 7)); - TERN_(HAS_SHAPING_Y, advance_dividend.y -= echo_y); - } - // plan the change of values for advance_dividend for the input shaping echoes if (TERN0(HAS_SHAPING_X, shaping_x.enabled) || TERN0(HAS_SHAPING_Y, shaping_y.enabled)) TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y))); diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index a3f289d8cf05..91379be3876b 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -370,11 +370,13 @@ constexpr ena_mask_t enable_overlap[] = { shaping_time_t times[SIZE]; #if HAS_SHAPING_X shaping_time_t peek_x_val = shaping_time_t(-1); + uint16_t head_x = 0; #endif #if HAS_SHAPING_Y shaping_time_t peek_y_val = shaping_time_t(-1); + uint16_t head_y = 0; #endif - uint16_t tail = 0 OPTARG(HAS_SHAPING_X, head_x = 0) OPTARG(HAS_SHAPING_Y, head_y = 0); + uint16_t tail = 0; public: void decrement_peeks(const shaping_time_t interval) { @@ -396,7 +398,7 @@ constexpr ena_mask_t enable_overlap[] = { peek_x_val = head_x == tail ? shaping_time_t(-1) : times[head_x] + delay_x - now; } bool empty_x() { return head_x == tail; } - uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; } + uint16_t free_count_x() { return (head_x > tail ? 0 : SIZE) + (head_x - tail - 1); } #endif #if HAS_SHAPING_Y shaping_time_t peek_y() { return peek_y_val; } @@ -405,11 +407,16 @@ constexpr ena_mask_t enable_overlap[] = { peek_y_val = head_y == tail ? shaping_time_t(-1) : times[head_y] + delay_y - now; } bool empty_y() { return head_y == tail; } - uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; } + uint16_t free_count_y() { return (head_y > tail ? 0 : SIZE) + (head_y - tail - 1); } #endif void purge() { - const auto temp1 = TERN_(HAS_SHAPING_X, head_x =) TERN_(HAS_SHAPING_Y, head_y =) tail; UNUSED(temp1); - const auto temp2 = TERN_(HAS_SHAPING_X, peek_x_val =) TERN_(HAS_SHAPING_Y, peek_y_val =) shaping_time_t(-1); UNUSED(temp2); + const auto st = shaping_time_t(-1); + #if HAS_SHAPING_X + head_x = tail; peek_x_val = st; + #endif + #if HAS_SHAPING_Y + head_y = tail; peek_y_val = st; + #endif } }; From e570e1c0c902169a373e2368a6541bde72403c8f Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 11 Nov 2022 18:05:42 -0600 Subject: [PATCH 17/33] Modify configuration --- Marlin/Configuration_adv.h | 19 ++- .../src/gcode/feature/input_shaping/M593.cpp | 16 +- Marlin/src/gcode/gcode.cpp | 2 +- Marlin/src/gcode/gcode.h | 4 +- Marlin/src/inc/Conditionals_adv.h | 18 +-- Marlin/src/inc/SanityCheck.h | 12 +- Marlin/src/lcd/menu/menu_advanced.cpp | 4 +- Marlin/src/module/planner.cpp | 8 +- Marlin/src/module/settings.cpp | 22 +-- Marlin/src/module/stepper.cpp | 138 +++++++++--------- Marlin/src/module/stepper.h | 72 ++++----- buildroot/tests/mega2560 | 4 +- ini/features.ini | 2 +- 13 files changed, 160 insertions(+), 161 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index c69d520fe71c..2daf6123784e 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1077,13 +1077,18 @@ * X<1> Set the given parameters only for the X axis. * Y<1> Set the given parameters only for the Y axis. */ -//#define INPUT_SHAPING -#if ENABLED(INPUT_SHAPING) - #define SHAPING_FREQ_X 40 // (Hz) The dominant resonant frequency of the X axis. Comment to disable shaping for X. - #define SHAPING_FREQ_Y 40 // (Hz) The dominant resonant frequency of the Y axis. Comment to disable shaping for Y. - #define SHAPING_ZETA_X 0.3f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). - #define SHAPING_ZETA_Y 0.3f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). - //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. +//#define INPUT_SHAPING_X +//#define INPUT_SHAPING_Y +#if EITHER(INPUT_SHAPING_X, INPUT_SHAPING_Y) + #if ENABLED(INPUT_SHAPING_X) + #define SHAPING_FREQ_X 40 // (Hz) The default dominant resonant frequency on the X axis. + #define SHAPING_ZETA_X 0.3f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). + #endif + #if ENABLED(INPUT_SHAPING_Y) + #define SHAPING_FREQ_Y 40 // (Hz) The default dominant resonant frequency on the Y axis. + #define SHAPING_ZETA_Y 0.3f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). + #endif + //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. #endif #define AXIS_RELATIVE_MODES { false, false, false, false } diff --git a/Marlin/src/gcode/feature/input_shaping/M593.cpp b/Marlin/src/gcode/feature/input_shaping/M593.cpp index 6eac366ec48d..040710f3e580 100644 --- a/Marlin/src/gcode/feature/input_shaping/M593.cpp +++ b/Marlin/src/gcode/feature/input_shaping/M593.cpp @@ -22,21 +22,21 @@ #include "../../../inc/MarlinConfig.h" -#if ENABLED(INPUT_SHAPING) +#if HAS_SHAPING #include "../../gcode.h" #include "../../../module/stepper.h" void GcodeSuite::M593_report(const bool forReplay/*=true*/) { report_heading_etc(forReplay, F("Input Shaping")); - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) SERIAL_ECHOLNPGM(" M593 X" " F", stepper.get_shaping_frequency(X_AXIS), " D", stepper.get_shaping_damping_ratio(X_AXIS) ); #endif - #if HAS_SHAPING_Y - TERN_(HAS_SHAPING_X, report_echo_start(forReplay)); + #if ENABLED(INPUT_SHAPING_Y) + TERN_(INPUT_SHAPING_X, report_echo_start(forReplay)); SERIAL_ECHOLNPGM(" M593 Y" " F", stepper.get_shaping_frequency(Y_AXIS), " D", stepper.get_shaping_damping_ratio(Y_AXIS) @@ -55,10 +55,10 @@ void GcodeSuite::M593_report(const bool forReplay/*=true*/) { void GcodeSuite::M593() { if (!parser.seen_any()) return M593_report(); - const bool seen_X = TERN0(HAS_SHAPING_X, parser.seen_test('X')), - seen_Y = TERN0(HAS_SHAPING_Y, parser.seen_test('Y')), - for_X = seen_X || TERN0(HAS_SHAPING_X, (!seen_X && !seen_Y)), - for_Y = seen_Y || TERN0(HAS_SHAPING_Y, (!seen_X && !seen_Y)); + const bool seen_X = TERN0(INPUT_SHAPING_X, parser.seen_test('X')), + seen_Y = TERN0(INPUT_SHAPING_Y, parser.seen_test('Y')), + for_X = seen_X || TERN0(INPUT_SHAPING_X, (!seen_X && !seen_Y)), + for_Y = seen_Y || TERN0(INPUT_SHAPING_Y, (!seen_X && !seen_Y)); if (parser.seen('D')) { const float zeta = parser.value_float(); diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp index ff066ed67837..bb859d802641 100644 --- a/Marlin/src/gcode/gcode.cpp +++ b/Marlin/src/gcode/gcode.cpp @@ -933,7 +933,7 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) { case 575: M575(); break; // M575: Set serial baudrate #endif - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING case 593: M593(); break; // M593: Set Input Shaping parameters #endif diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h index 0ce8ab39025e..5d56e53dd5e0 100644 --- a/Marlin/src/gcode/gcode.h +++ b/Marlin/src/gcode/gcode.h @@ -259,7 +259,7 @@ * M554 - Get or set IP gateway. (Requires enabled Ethernet port) * M569 - Enable stealthChop on an axis. (Requires at least one _DRIVER_TYPE to be TMC2130/2160/2208/2209/5130/5160) * M575 - Change the serial baud rate. (Requires BAUD_RATE_GCODE) - * M593 - Get or set input shaping parameters. (Requires INPUT_SHAPING) + * M593 - Get or set input shaping parameters. (Requires INPUT_SHAPING_[XY]) * M600 - Pause for filament change: "M600 X Y Z E L". (Requires ADVANCED_PAUSE_FEATURE) * M603 - Configure filament change: "M603 T U L". (Requires ADVANCED_PAUSE_FEATURE) * M605 - Set Dual X-Carriage movement mode: "M605 S [X] [R]". (Requires DUAL_X_CARRIAGE) @@ -1081,7 +1081,7 @@ class GcodeSuite { static void M575(); #endif - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING static void M593(); static void M593_report(const bool forReplay=true); #endif diff --git a/Marlin/src/inc/Conditionals_adv.h b/Marlin/src/inc/Conditionals_adv.h index dd69e61c9275..94e805403f90 100644 --- a/Marlin/src/inc/Conditionals_adv.h +++ b/Marlin/src/inc/Conditionals_adv.h @@ -1119,15 +1119,11 @@ #endif // Input shaping -#if ENABLED(INPUT_SHAPING) - #if !HAS_Y_AXIS - #undef SHAPING_FREQ_Y - #undef SHAPING_BUFFER_Y - #endif - #ifdef SHAPING_FREQ_X - #define HAS_SHAPING_X 1 - #endif - #ifdef SHAPING_FREQ_Y - #define HAS_SHAPING_Y 1 - #endif +#if !HAS_Y_AXIS + #undef INPUT_SHAPING_Y + #undef SHAPING_FREQ_Y + #undef SHAPING_BUFFER_Y +#endif +#if EITHER(INPUT_SHAPING_X, INPUT_SHAPING_Y) + #define HAS_SHAPING 1 #endif diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 5915dada4703..3ff799269719 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4258,14 +4258,14 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive."); #endif // Check requirements for Input Shaping -#if ENABLED(INPUT_SHAPING) && defined(__AVR__) - #if HAS_SHAPING_X +#if HAS_SHAPING && defined(__AVR__) + #if ENABLED(INPUT_SHAPING_X) #if F_CPU > 16000000 static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (20) for AVR 20MHz."); #else static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (16) for AVR 16MHz."); #endif - #elif HAS_SHAPING_Y + #elif INPUT_SHAPING_Y #if F_CPU > 16000000 static_assert((SHAPING_FREQ_Y) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Y is below the minimum (20) for AVR 20MHz."); #else @@ -4274,10 +4274,8 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive."); #endif #endif -#if ENABLED(INPUT_SHAPING) - #if ENABLED(DIRECT_STEPPING) - #error "INPUT_SHAPING cannot currently be used with DIRECT_STEPPING." - #endif +#if BOTH(HAS_SHAPING, DIRECT_STEPPING) + #error "INPUT_SHAPING_[XY] cannot currently be used with DIRECT_STEPPING." #endif // Misc. Cleanup diff --git a/Marlin/src/lcd/menu/menu_advanced.cpp b/Marlin/src/lcd/menu/menu_advanced.cpp index 68daffab1a59..875e74e8bb15 100644 --- a/Marlin/src/lcd/menu/menu_advanced.cpp +++ b/Marlin/src/lcd/menu/menu_advanced.cpp @@ -546,7 +546,7 @@ void menu_backlash(); BACK_ITEM(MSG_ADVANCED_SETTINGS); // M593 F Frequency and D Damping ratio - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) editable.decimal = stepper.get_shaping_frequency(X_AXIS); if (editable.decimal) { ACTION_ITEM_N(X_AXIS, MSG_SHAPING_DISABLE, []{ stepper.set_shaping_frequency(X_AXIS, 0.0f); }); @@ -557,7 +557,7 @@ void menu_backlash(); else ACTION_ITEM_N(X_AXIS, MSG_SHAPING_ENABLE, []{ stepper.set_shaping_frequency(X_AXIS, SHAPING_FREQ_X); }); #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) editable.decimal = stepper.get_shaping_frequency(Y_AXIS); if (editable.decimal) { ACTION_ITEM_N(Y_AXIS, MSG_SHAPING_DISABLE, []{ stepper.set_shaping_frequency(Y_AXIS, 0.0f); }); diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index ba8662e2fa98..948fd39ad67d 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1727,7 +1727,7 @@ float Planner::triggered_position_mm(const AxisEnum axis) { bool Planner::busy() { return (has_blocks_queued() || cleaning_buffer_counter || TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING()) - || TERN0(INPUT_SHAPING, stepper.input_shaping_busy()) + || TERN0(HAS_SHAPING, stepper.input_shaping_busy()) ); } @@ -2490,10 +2490,10 @@ bool Planner::_populate_block( #endif // XY_FREQUENCY_LIMIT - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING float bottom_freq = float(0x7FFFFFFFL), t; - TERN_(HAS_SHAPING_X, if ((t = stepper.get_shaping_frequency(X_AXIS))) NOMORE(bottom_freq, t)); - TERN_(HAS_SHAPING_Y, if ((t = stepper.get_shaping_frequency(Y_AXIS))) NOMORE(bottom_freq, t)); + TERN_(INPUT_SHAPING_X, if ((t = stepper.get_shaping_frequency(X_AXIS))) NOMORE(bottom_freq, t)); + TERN_(INPUT_SHAPING_Y, if ((t = stepper.get_shaping_frequency(Y_AXIS))) NOMORE(bottom_freq, t)); const float max_factor = (bottom_freq * (float(shaping_dividends - 3) * 2.0f)) / block->nominal_rate; NOMORE(speed_factor, max_factor); #endif diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp index f76b5afe74e2..d172506f89bb 100644 --- a/Marlin/src/module/settings.cpp +++ b/Marlin/src/module/settings.cpp @@ -580,11 +580,11 @@ typedef struct SettingsDataStruct { // // Input Shaping // - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) float shaping_x_frequency, // M593 X F shaping_x_zeta; // M593 X D #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) float shaping_y_frequency, // M593 Y F shaping_y_zeta; // M593 Y D #endif @@ -1617,12 +1617,12 @@ void MarlinSettings::postprocess() { // // Input Shaping /// - #if ENABLED(INPUT_SHAPING) - #if HAS_SHAPING_X + #if HAS_SHAPING + #if ENABLED(INPUT_SHAPING_X) EEPROM_WRITE(stepper.get_shaping_frequency(X_AXIS)); EEPROM_WRITE(stepper.get_shaping_damping_ratio(X_AXIS)); #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) EEPROM_WRITE(stepper.get_shaping_frequency(Y_AXIS)); EEPROM_WRITE(stepper.get_shaping_damping_ratio(Y_AXIS)); #endif @@ -2602,7 +2602,7 @@ void MarlinSettings::postprocess() { // // Input Shaping // - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) { float _data[2]; EEPROM_READ(_data); @@ -2611,7 +2611,7 @@ void MarlinSettings::postprocess() { } #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) { float _data[2]; EEPROM_READ(_data); @@ -3393,12 +3393,12 @@ void MarlinSettings::reset() { // // Input Shaping // - #if ENABLED(INPUT_SHAPING) - #if HAS_SHAPING_X + #if HAS_SHAPING + #if ENABLED(INPUT_SHAPING_X) stepper.set_shaping_frequency(X_AXIS, SHAPING_FREQ_X); stepper.set_shaping_damping_ratio(X_AXIS, SHAPING_ZETA_X); #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) stepper.set_shaping_frequency(Y_AXIS, SHAPING_FREQ_Y); stepper.set_shaping_damping_ratio(Y_AXIS, SHAPING_ZETA_Y); #endif @@ -3654,7 +3654,7 @@ void MarlinSettings::reset() { // // Input Shaping // - TERN_(INPUT_SHAPING, gcode.M593_report(forReplay)); + TERN_(HAS_SHAPING, gcode.M593_report(forReplay)); // // Linear Advance diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 4e38cafa35e6..153c907b2f7b 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -232,15 +232,15 @@ uint32_t Stepper::advance_divisor = 0, Stepper::la_advance_steps = 0; #endif -#if ENABLED(INPUT_SHAPING) +#if HAS_SHAPING shaping_time_t DelayTimeManager::now = 0; ParamDelayQueue Stepper::shaping_dividend_queue; DelayQueue Stepper::shaping_queue; - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) shaping_time_t DelayTimeManager::delay_x; ShapeParams Stepper::shaping_x; #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) shaping_time_t DelayTimeManager::delay_y; ShapeParams Stepper::shaping_y; #endif @@ -1479,19 +1479,19 @@ void Stepper::isr() { // Enable ISRs to reduce USART processing latency hal.isr_on(); - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING // Speed limiting should ensure the buffers never get full. But if somehow they do, stutter rather than overflow. if (!nextMainISR) { - TERN_(HAS_SHAPING_X, if (shaping_dividend_queue.free_count_x() == 0) nextMainISR = shaping_dividend_queue.peek_x() + 1); - TERN_(HAS_SHAPING_Y, if (shaping_dividend_queue.free_count_y() == 0) NOLESS(nextMainISR, shaping_dividend_queue.peek_y() + 1)); - TERN_(HAS_SHAPING_X, if (shaping_queue.free_count_x() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_x() + 1)); - TERN_(HAS_SHAPING_Y, if (shaping_queue.free_count_y() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_y() + 1)); + TERN_(INPUT_SHAPING_X, if (shaping_dividend_queue.free_count_x() == 0) nextMainISR = shaping_dividend_queue.peek_x() + 1); + TERN_(INPUT_SHAPING_Y, if (shaping_dividend_queue.free_count_y() == 0) NOLESS(nextMainISR, shaping_dividend_queue.peek_y() + 1)); + TERN_(INPUT_SHAPING_X, if (shaping_queue.free_count_x() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_x() + 1)); + TERN_(INPUT_SHAPING_Y, if (shaping_queue.free_count_y() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_y() + 1)); } #endif if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses - TERN_(INPUT_SHAPING, shaping_isr()); // Do Shaper stepping, if needed + TERN_(HAS_SHAPING, shaping_isr()); // Do Shaper stepping, if needed #if ENABLED(LIN_ADVANCE) if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses @@ -1523,10 +1523,10 @@ void Stepper::isr() { const uint32_t interval = _MIN( uint32_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time nextMainISR // Time until the next Pulse / Block phase - OPTARG(HAS_SHAPING_X, shaping_dividend_queue.peek_x()) // Time until next input shaping dividend change for X - OPTARG(HAS_SHAPING_Y, shaping_dividend_queue.peek_y()) // Time until next input shaping dividend change for Y - OPTARG(HAS_SHAPING_X, shaping_queue.peek_x()) // Time until next input shaping echo for X - OPTARG(HAS_SHAPING_Y, shaping_queue.peek_y()) // Time until next input shaping echo for Y + OPTARG(INPUT_SHAPING_X, shaping_dividend_queue.peek_x()) // Time until next input shaping dividend change for X + OPTARG(INPUT_SHAPING_Y, shaping_dividend_queue.peek_y()) // Time until next input shaping dividend change for Y + OPTARG(INPUT_SHAPING_X, shaping_queue.peek_x()) // Time until next input shaping echo for X + OPTARG(INPUT_SHAPING_Y, shaping_queue.peek_y()) // Time until next input shaping echo for Y OPTARG(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance? OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping? ); @@ -1540,7 +1540,7 @@ void Stepper::isr() { nextMainISR -= interval; - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING DelayTimeManager::decrement_delays(interval); shaping_queue.decrement_peeks(interval); shaping_dividend_queue.decrement_peeks(interval); @@ -1640,13 +1640,13 @@ void Stepper::pulse_phase_isr() { abort_current_block = false; if (current_block) { discard_current_block(); - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING shaping_dividend_queue.purge(); shaping_queue.purge(); - if (TERN0(HAS_SHAPING_X, shaping_x.enabled)) delta_error.x = 0; - if (TERN0(HAS_SHAPING_Y, shaping_y.enabled)) delta_error.y = 0; - TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); - TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); + if (TERN0(INPUT_SHAPING_X, shaping_x.enabled)) delta_error.x = 0; + if (TERN0(INPUT_SHAPING_Y, shaping_y.enabled)) delta_error.y = 0; + TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); + TERN_(INPUT_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); #endif } } @@ -1831,17 +1831,17 @@ void Stepper::pulse_phase_isr() { #endif // DIRECT_STEPPING if (!is_page) { - TERN_(INPUT_SHAPING, shaping_queue.enqueue()); + TERN_(HAS_SHAPING, shaping_queue.enqueue()); // Determine if pulses are needed #if HAS_X_STEP - if (TERN0(HAS_SHAPING_X, shaping_x.enabled)) + if (TERN0(INPUT_SHAPING_X, shaping_x.enabled)) PULSE_PREP_SHAPING(X, advance_dividend.x); else PULSE_PREP(X); #endif #if HAS_Y_STEP - if (TERN0(HAS_SHAPING_Y, shaping_y.enabled)) + if (TERN0(INPUT_SHAPING_Y, shaping_y.enabled)) PULSE_PREP_SHAPING(Y, advance_dividend.y); else PULSE_PREP(Y); @@ -1975,16 +1975,16 @@ void Stepper::pulse_phase_isr() { } while (--events_to_do); } -#if ENABLED(INPUT_SHAPING) +#if HAS_SHAPING void Stepper::shaping_isr() { xy_bool_t step_needed{0}; - bool shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()), - shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); + bool shapex = TERN0(INPUT_SHAPING_X, !shaping_queue.peek_x()), + shapey = TERN0(INPUT_SHAPING_Y, !shaping_queue.peek_y()); if (shapex || shapey) while (true) { - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) if (shapex) { shaping_queue.dequeue_x(); PULSE_PREP_SHAPING(X, shaping_x.dividend); @@ -1992,7 +1992,7 @@ void Stepper::pulse_phase_isr() { } #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) if (shapey) { shaping_queue.dequeue_y(); PULSE_PREP_SHAPING(Y, shaping_y.dividend); @@ -2008,16 +2008,16 @@ void Stepper::pulse_phase_isr() { START_TIMED_PULSE(); AWAIT_HIGH_PULSE(); #endif - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) PULSE_STOP(X); #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) PULSE_STOP(Y); #endif } - shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()); - shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y()); + shapex = TERN0(INPUT_SHAPING_X, !shaping_queue.peek_x()); + shapey = TERN0(INPUT_SHAPING_Y, !shaping_queue.peek_y()); if (!shapex && !shapey) break; @@ -2028,15 +2028,15 @@ void Stepper::pulse_phase_isr() { // if a new dividend appears in the same "moment" that steps takes place, it is because // the steps are the last steps of a segment and the new dividend is for the new segment // so only update the dividend after the steps - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) if (!shaping_dividend_queue.peek_x()) shaping_x.dividend = shaping_dividend_queue.dequeue_x(); #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) if (!shaping_dividend_queue.peek_y()) shaping_y.dividend = shaping_dividend_queue.dequeue_y(); #endif } -#endif // INPUT_SHAPING +#endif // HAS_SHAPING // Calculate timer interval, with all limits applied. uint32_t Stepper::calc_timer_interval(uint32_t step_rate) { @@ -2502,22 +2502,22 @@ uint32_t Stepper::block_phase_isr() { step_event_count = current_block->step_event_count << oversampling; // Initialize Bresenham delta errors to 1/2 - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) const int32_t old_delta_error_x = delta_error.x; #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) const int32_t old_delta_error_y = delta_error.y; #endif delta_error = TERN_(LIN_ADVANCE, la_delta_error =) -int32_t(step_event_count); // Compiler should optimize these out, by not assigning them in the first place - TERN_(HAS_SHAPING_X, delta_error.x = old_delta_error_x); - TERN_(HAS_SHAPING_Y, delta_error.y = old_delta_error_y); + TERN_(INPUT_SHAPING_X, delta_error.x = old_delta_error_x); + TERN_(INPUT_SHAPING_Y, delta_error.y = old_delta_error_y); // Calculate Bresenham dividends and divisors advance_dividend = (current_block->steps << 1).asLong(); advance_divisor = step_event_count << 1; - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) int32_t echo_x = 0; if (shaping_x.enabled) { const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); @@ -2557,7 +2557,7 @@ uint32_t Stepper::block_phase_isr() { #endif // Y follows the same logic as X (but the comments aren't repeated) - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) int32_t echo_y = 0; if (shaping_y.enabled) { const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); @@ -2573,8 +2573,8 @@ uint32_t Stepper::block_phase_isr() { #endif // plan the change of values for advance_dividend for the input shaping echoes - if (TERN0(HAS_SHAPING_X, shaping_x.enabled) || TERN0(HAS_SHAPING_Y, shaping_y.enabled)) - TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y))); + if (TERN0(INPUT_SHAPING_X, shaping_x.enabled) || TERN0(INPUT_SHAPING_Y, shaping_y.enabled)) + TERN_(HAS_SHAPING, shaping_dividend_queue.enqueue(TERN0(INPUT_SHAPING_X, echo_x), TERN0(INPUT_SHAPING_Y, echo_y))); // No step events completed so far step_events_completed = 0; @@ -3021,7 +3021,7 @@ void Stepper::init() { #endif } -#if ENABLED(INPUT_SHAPING) +#if HAS_SHAPING /** * Calculate a fixed point factor to apply to the signal and its echo * when shaping an axis. @@ -3043,14 +3043,14 @@ void Stepper::init() { const bool was_on = hal.isr_state(); hal.isr_off(); - TERN_(HAS_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor = floor(shaping_factor); shaping_x.zeta = zeta; }) - TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor = floor(shaping_factor); shaping_y.zeta = zeta; }) + TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor = floor(shaping_factor); shaping_x.zeta = zeta; }) + TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor = floor(shaping_factor); shaping_y.zeta = zeta; }) if (was_on) hal.isr_on(); } float Stepper::get_shaping_damping_ratio(const AxisEnum axis) { - TERN_(HAS_SHAPING_X, if (axis == X_AXIS) return shaping_x.zeta); - TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.zeta); + TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) return shaping_x.zeta); + TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.zeta); return -1; } @@ -3062,29 +3062,29 @@ void Stepper::init() { hal.isr_off(); const shaping_time_t delay = freq ? float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq : shaping_time_t(-1); - if (TERN0(HAS_SHAPING_X, axis == X_AXIS)) { + if (TERN0(INPUT_SHAPING_X, axis == X_AXIS)) { DelayTimeManager::set_delay(X_AXIS, delay); - TERN_(HAS_SHAPING_X, shaping_x.frequency = freq); - TERN_(HAS_SHAPING_X, shaping_x.enabled = !!freq); + TERN_(INPUT_SHAPING_X, shaping_x.frequency = freq); + TERN_(INPUT_SHAPING_X, shaping_x.enabled = !!freq); delta_error = 0L; - TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); - TERN_(HAS_SHAPING_X, shaping_x.dividend = shaping_x.remainder = 0UL); + TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); + TERN_(INPUT_SHAPING_X, shaping_x.dividend = shaping_x.remainder = 0UL); } - if (TERN0(HAS_SHAPING_Y, axis == Y_AXIS)) { + if (TERN0(INPUT_SHAPING_Y, axis == Y_AXIS)) { DelayTimeManager::set_delay(Y_AXIS, delay); - TERN_(HAS_SHAPING_Y, shaping_y.frequency = freq); - TERN_(HAS_SHAPING_Y, shaping_y.enabled = !!freq); + TERN_(INPUT_SHAPING_Y, shaping_y.frequency = freq); + TERN_(INPUT_SHAPING_Y, shaping_y.enabled = !!freq); delta_error = 0L; - TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); - TERN_(HAS_SHAPING_Y, shaping_y.dividend = shaping_y.remainder = 0UL); + TERN_(INPUT_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); + TERN_(INPUT_SHAPING_Y, shaping_y.dividend = shaping_y.remainder = 0UL); } if (was_on) hal.isr_on(); } float Stepper::get_shaping_frequency(const AxisEnum axis) { - TERN_(HAS_SHAPING_X, if (axis == X_AXIS) return shaping_x.frequency); - TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency); + TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) return shaping_x.frequency); + TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency); return -1; } #endif @@ -3099,10 +3099,10 @@ void Stepper::init() { * derive the current XYZE position later on. */ void Stepper::_set_position(const abce_long_t &spos) { - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) int32_t x_shaping_delta = count_position.x - shaping_x.last_block_end_pos; #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) int32_t y_shaping_delta = count_position.y - shaping_y.last_block_end_pos; #endif @@ -3136,13 +3136,13 @@ void Stepper::_set_position(const abce_long_t &spos) { count_position = spos; #endif - if (TERN0(HAS_SHAPING_X, shaping_x.enabled)) { - TERN_(HAS_SHAPING_X, count_position.x += x_shaping_delta); - TERN_(HAS_SHAPING_X, shaping_x.last_block_end_pos = spos.x); + if (TERN0(INPUT_SHAPING_X, shaping_x.enabled)) { + TERN_(INPUT_SHAPING_X, count_position.x += x_shaping_delta); + TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = spos.x); } - if (TERN0(HAS_SHAPING_Y, shaping_y.enabled)) { - TERN_(HAS_SHAPING_Y, count_position.y += y_shaping_delta); - TERN_(HAS_SHAPING_Y, shaping_y.last_block_end_pos = spos.y); + if (TERN0(INPUT_SHAPING_Y, shaping_y.enabled)) { + TERN_(INPUT_SHAPING_Y, count_position.y += y_shaping_delta); + TERN_(INPUT_SHAPING_Y, shaping_y.last_block_end_pos = spos.y); } } @@ -3183,8 +3183,8 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) { #endif count_position[a] = v; - TERN_(HAS_SHAPING_X, if (a == X_AXIS) shaping_x.last_block_end_pos = v); - TERN_(HAS_SHAPING_Y, if (a == Y_AXIS) shaping_y.last_block_end_pos = v); + TERN_(INPUT_SHAPING_X, if (a == X_AXIS) shaping_x.last_block_end_pos = v); + TERN_(INPUT_SHAPING_Y, if (a == Y_AXIS) shaping_y.last_block_end_pos = v); #ifdef __AVR__ // Reenable Stepper ISR diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 91379be3876b..27ec28e0087c 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -93,7 +93,7 @@ #endif // Input shaping base time - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING #define ISR_SHAPING_BASE_CYCLES 250UL #else #define ISR_SHAPING_BASE_CYCLES 0UL @@ -127,7 +127,7 @@ #endif // Input shaping base time - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING #define ISR_SHAPING_BASE_CYCLES 250UL #else #define ISR_SHAPING_BASE_CYCLES 0UL @@ -216,7 +216,7 @@ #define ISR_LOOP_CYCLES (ISR_LOOP_BASE_CYCLES + HIGH_PULSE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES)) // Model input shaping as an extra loop call -#define ISR_SHAPING_LOOP_CYCLES (TERN0(INPUT_SHAPING, ISR_LOOP_BASE_CYCLES + HIGH_PULSE_CYCLES) + TERN0(HAS_SHAPING_X, ISR_X_STEPPER_CYCLES) + TERN0(HAS_SHAPING_Y, ISR_Y_STEPPER_CYCLES)) +#define ISR_SHAPING_LOOP_CYCLES (TERN0(HAS_SHAPING, ISR_LOOP_BASE_CYCLES + HIGH_PULSE_CYCLES) + TERN0(INPUT_SHAPING_X, ISR_X_STEPPER_CYCLES) + TERN0(INPUT_SHAPING_Y, ISR_Y_STEPPER_CYCLES)) // If linear advance is enabled, then it is handled separately #if ENABLED(LIN_ADVANCE) @@ -325,7 +325,7 @@ constexpr ena_mask_t enable_overlap[] = { //static_assert(!any_enable_overlap(), "There is some overlap."); -#if ENABLED(INPUT_SHAPING) +#if HAS_SHAPING typedef IF::type shaping_time_t; @@ -344,23 +344,23 @@ constexpr ena_mask_t enable_overlap[] = { max_feedrate.v * steps_per_unit.v, max_feedrate.w * steps_per_unit.w )); - constexpr uint16_t shaping_dividends = max_steprate / _MIN(0x7FFFFFFFL OPTARG(HAS_SHAPING_X, SHAPING_FREQ_X) OPTARG(HAS_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; - constexpr uint16_t shaping_segments = max_steprate / (MIN_STEPS_PER_SEGMENT) / _MIN(0x7FFFFFFFL OPTARG(HAS_SHAPING_X, SHAPING_FREQ_X) OPTARG(HAS_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; + constexpr uint16_t shaping_dividends = max_steprate / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; + constexpr uint16_t shaping_segments = max_steprate / (MIN_STEPS_PER_SEGMENT) / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; class DelayTimeManager { private: static shaping_time_t now; - #ifdef HAS_SHAPING_X + #ifdef INPUT_SHAPING_X static shaping_time_t delay_x; // = shaping_time_t(-1) to disable queueing #endif - #ifdef HAS_SHAPING_Y + #ifdef INPUT_SHAPING_Y static shaping_time_t delay_y; // = shaping_time_t(-1) to disable queueing #endif public: static void decrement_delays(const shaping_time_t interval) { now += interval; } static void set_delay(const AxisEnum axis, const shaping_time_t delay) { - TERN_(HAS_SHAPING_X, if (axis == X_AXIS) delay_x = delay); - TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay); + TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) delay_x = delay); + TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay); } }; @@ -368,11 +368,11 @@ constexpr ena_mask_t enable_overlap[] = { class DelayQueue : public DelayTimeManager { protected: shaping_time_t times[SIZE]; - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) shaping_time_t peek_x_val = shaping_time_t(-1); uint16_t head_x = 0; #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) shaping_time_t peek_y_val = shaping_time_t(-1); uint16_t head_y = 0; #endif @@ -380,18 +380,18 @@ constexpr ena_mask_t enable_overlap[] = { public: void decrement_peeks(const shaping_time_t interval) { - TERN_(HAS_SHAPING_X, if (peek_x_val != shaping_time_t(-1)) peek_x_val -= interval); - TERN_(HAS_SHAPING_Y, if (peek_y_val != shaping_time_t(-1)) peek_y_val -= interval); + TERN_(INPUT_SHAPING_X, if (peek_x_val != shaping_time_t(-1)) peek_x_val -= interval); + TERN_(INPUT_SHAPING_Y, if (peek_y_val != shaping_time_t(-1)) peek_y_val -= interval); } void enqueue() { - TERN_(HAS_SHAPING_X, if (head_x == tail) peek_x_val = delay_x); - TERN_(HAS_SHAPING_Y, if (head_y == tail) peek_y_val = delay_y); + TERN_(INPUT_SHAPING_X, if (head_x == tail) peek_x_val = delay_x); + TERN_(INPUT_SHAPING_Y, if (head_y == tail) peek_y_val = delay_y); times[tail] = now; if (++tail == SIZE) tail = 0; - TERN_(HAS_SHAPING_X, if (delay_x == shaping_time_t(-1)) head_x = tail); - TERN_(HAS_SHAPING_Y, if (delay_y == shaping_time_t(-1)) head_y = tail); + TERN_(INPUT_SHAPING_X, if (delay_x == shaping_time_t(-1)) head_x = tail); + TERN_(INPUT_SHAPING_Y, if (delay_y == shaping_time_t(-1)) head_y = tail); } - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) shaping_time_t peek_x() { return peek_x_val; } void dequeue_x() { if (++head_x == SIZE) head_x = 0; @@ -400,7 +400,7 @@ constexpr ena_mask_t enable_overlap[] = { bool empty_x() { return head_x == tail; } uint16_t free_count_x() { return (head_x > tail ? 0 : SIZE) + (head_x - tail - 1); } #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) shaping_time_t peek_y() { return peek_y_val; } void dequeue_y() { if (++head_y == SIZE) head_y = 0; @@ -411,10 +411,10 @@ constexpr ena_mask_t enable_overlap[] = { #endif void purge() { const auto st = shaping_time_t(-1); - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) head_x = tail; peek_x_val = st; #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) head_y = tail; peek_y_val = st; #endif } @@ -422,27 +422,27 @@ constexpr ena_mask_t enable_overlap[] = { class ParamDelayQueue : public DelayQueue { private: - #ifdef HAS_SHAPING_X + #ifdef INPUT_SHAPING_X int32_t params_x[shaping_segments]; #endif - #ifdef HAS_SHAPING_Y + #ifdef INPUT_SHAPING_Y int32_t params_y[shaping_segments]; #endif public: void enqueue(const int32_t param_x, const int32_t param_y) { - TERN(HAS_SHAPING_X, params_x[DelayQueue::tail] = param_x, UNUSED(param_x)); - TERN(HAS_SHAPING_Y, params_y[DelayQueue::tail] = param_y, UNUSED(param_y)); + TERN(INPUT_SHAPING_X, params_x[DelayQueue::tail] = param_x, UNUSED(param_x)); + TERN(INPUT_SHAPING_Y, params_y[DelayQueue::tail] = param_y, UNUSED(param_y)); DelayQueue::enqueue(); } - #ifdef HAS_SHAPING_X + #ifdef INPUT_SHAPING_X const int32_t dequeue_x() { const int32_t result = params_x[DelayQueue::head_x]; DelayQueue::dequeue_x(); return result; } #endif - #ifdef HAS_SHAPING_Y + #ifdef INPUT_SHAPING_Y const int32_t dequeue_y() { const int32_t result = params_y[DelayQueue::head_y]; DelayQueue::dequeue_y(); @@ -461,7 +461,7 @@ constexpr ena_mask_t enable_overlap[] = { int32_t last_block_end_pos = 0; }; -#endif // INPUT_SHAPING +#endif // HAS_SHAPING // // Stepper class definition @@ -567,13 +567,13 @@ class Stepper { static bool bezier_2nd_half; // If Bézier curve has been initialized or not #endif - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING static ParamDelayQueue shaping_dividend_queue; static DelayQueue shaping_queue; - #if HAS_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) static ShapeParams shaping_x; #endif - #if HAS_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) static ShapeParams shaping_y; #endif #endif @@ -637,7 +637,7 @@ class Stepper { // The stepper block processing ISR phase static uint32_t block_phase_isr(); - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING static void shaping_isr(); #endif @@ -660,13 +660,13 @@ class Stepper { // Check if the given block is busy or not - Must not be called from ISR contexts static bool is_block_busy(const block_t * const block); - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING // Check whether the stepper is processing any input shaping echoes static bool input_shaping_busy() { const bool was_on = hal.isr_state(); hal.isr_off(); - const bool result = TERN0(HAS_SHAPING_X, !shaping_queue.empty_x()) || TERN0(HAS_SHAPING_Y, !shaping_queue.empty_y()); + const bool result = TERN0(INPUT_SHAPING_X, !shaping_queue.empty_x()) || TERN0(INPUT_SHAPING_Y, !shaping_queue.empty_y()); if (was_on) hal.isr_on(); @@ -808,7 +808,7 @@ class Stepper { set_directions(); } - #if ENABLED(INPUT_SHAPING) + #if HAS_SHAPING static void set_shaping_damping_ratio(const AxisEnum axis, const float zeta); static float get_shaping_damping_ratio(const AxisEnum axis); static void set_shaping_frequency(const AxisEnum axis, const float freq); diff --git a/buildroot/tests/mega2560 b/buildroot/tests/mega2560 index 333be2f0faae..383b50ffc71b 100755 --- a/buildroot/tests/mega2560 +++ b/buildroot/tests/mega2560 @@ -80,9 +80,9 @@ opt_set MOTHERBOARD BOARD_AZTEEG_X3_PRO MIXING_STEPPERS 5 LCD_LANGUAGE ru \ FIL_RUNOUT2_PIN 16 FIL_RUNOUT3_PIN 17 FIL_RUNOUT4_PIN 4 FIL_RUNOUT5_PIN 5 opt_enable MIXING_EXTRUDER GRADIENT_MIX GRADIENT_VTOOL CR10_STOCKDISPLAY \ USE_CONTROLLER_FAN CONTROLLER_FAN_EDITABLE CONTROLLER_FAN_IGNORE_Z \ - FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE INPUT_SHAPING + FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE INPUT_SHAPING_X INPUT_SHAPING_Y opt_disable DISABLE_INACTIVE_EXTRUDER -exec_test $1 $2 "Azteeg X3 | Mixing Extruder (x5) | Gradient Mix | Greek" "$3" +exec_test $1 $2 "Azteeg X3 | Mixing Extruder (x5) | Gradient Mix | Input Shaping | Greek" "$3" # # Test SPEAKER with BOARD_BQ_ZUM_MEGA_3D and BQ_LCD_SMART_CONTROLLER diff --git a/ini/features.ini b/ini/features.ini index 7c8fd2fd8f33..e376e2757e6e 100644 --- a/ini/features.ini +++ b/ini/features.ini @@ -187,7 +187,7 @@ HAS_DUPLICATION_MODE = src_filter=+ PHOTO_GCODE = src_filter=+ CONTROLLER_FAN_EDITABLE = src_filter=+ -INPUT_SHAPING = src_filter=+ +HAS_SHAPING = src_filter=+ GCODE_MACROS = src_filter=+ GRADIENT_MIX = src_filter=+ HAS_SAVED_POSITIONS = src_filter=+ + From 9ea21dd1b7378145a90f24bc7927d781bf3d00ee Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sat, 12 Nov 2022 20:55:54 +0000 Subject: [PATCH 18/33] Minor fixes --- Marlin/src/inc/SanityCheck.h | 2 +- Marlin/src/module/stepper.cpp | 2 -- Marlin/src/module/stepper.h | 14 +++++++------- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 3ff799269719..2eb22b814730 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4265,7 +4265,7 @@ static_assert(_PLUS_TEST(4), "HOMING_FEEDRATE_MM_M values must be positive."); #else static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (16) for AVR 16MHz."); #endif - #elif INPUT_SHAPING_Y + #elif ENABLED(INPUT_SHAPING_Y) #if F_CPU > 16000000 static_assert((SHAPING_FREQ_Y) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Y is below the minimum (20) for AVR 20MHz."); #else diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 153c907b2f7b..6753a20a3564 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -2521,7 +2521,6 @@ uint32_t Stepper::block_phase_isr() { int32_t echo_x = 0; if (shaping_x.enabled) { const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); - UNUSED(steps); shaping_x.last_block_end_pos += steps; // For input shaped axes, advance_divisor is replaced with 0x20000000 @@ -2561,7 +2560,6 @@ uint32_t Stepper::block_phase_isr() { int32_t echo_y = 0; if (shaping_y.enabled) { const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); - UNUSED(steps); shaping_y.last_block_end_pos += steps; advance_dividend.y = ((steps << 29) + shaping_y.remainder) / step_event_count; LIMIT(advance_dividend.y, -0x20000000, 0x20000000); diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 27ec28e0087c..ae856409bb49 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -350,10 +350,10 @@ constexpr ena_mask_t enable_overlap[] = { class DelayTimeManager { private: static shaping_time_t now; - #ifdef INPUT_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) static shaping_time_t delay_x; // = shaping_time_t(-1) to disable queueing #endif - #ifdef INPUT_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) static shaping_time_t delay_y; // = shaping_time_t(-1) to disable queueing #endif public: @@ -364,7 +364,7 @@ constexpr ena_mask_t enable_overlap[] = { } }; - template + template class DelayQueue : public DelayTimeManager { protected: shaping_time_t times[SIZE]; @@ -422,10 +422,10 @@ constexpr ena_mask_t enable_overlap[] = { class ParamDelayQueue : public DelayQueue { private: - #ifdef INPUT_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) int32_t params_x[shaping_segments]; #endif - #ifdef INPUT_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) int32_t params_y[shaping_segments]; #endif @@ -435,14 +435,14 @@ constexpr ena_mask_t enable_overlap[] = { TERN(INPUT_SHAPING_Y, params_y[DelayQueue::tail] = param_y, UNUSED(param_y)); DelayQueue::enqueue(); } - #ifdef INPUT_SHAPING_X + #if ENABLED(INPUT_SHAPING_X) const int32_t dequeue_x() { const int32_t result = params_x[DelayQueue::head_x]; DelayQueue::dequeue_x(); return result; } #endif - #ifdef INPUT_SHAPING_Y + #if ENABLED(INPUT_SHAPING_Y) const int32_t dequeue_y() { const int32_t result = params_y[DelayQueue::head_y]; DelayQueue::dequeue_y(); From 9d6f98e5c4b536270527d1373ee55dc5aca7261a Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 14 Nov 2022 10:24:24 +0000 Subject: [PATCH 19/33] Cleaned up oversampling_factor vs oversampling --- Marlin/src/module/stepper.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 6753a20a3564..c5f769985cb7 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -2485,21 +2485,18 @@ uint32_t Stepper::block_phase_isr() { acceleration_time = deceleration_time = 0; #if ENABLED(ADAPTIVE_STEP_SMOOTHING) - uint8_t oversampling = 0; // Assume no axis smoothing (via oversampling) + oversampling_factor = 0; // Assume no axis smoothing (via oversampling) // Decide if axis smoothing is possible uint32_t max_rate = current_block->nominal_rate; // Get the step event rate while (max_rate < MIN_STEP_ISR_FREQUENCY) { // As long as more ISRs are possible... max_rate <<= 1; // Try to double the rate if (max_rate < MIN_STEP_ISR_FREQUENCY) // Don't exceed the estimated ISR limit - ++oversampling; // Increase the oversampling (used for left-shift) + ++oversampling_factor; // Increase the oversampling (used for left-shift) } - oversampling_factor = oversampling; // For all timer interval calculations - #else - constexpr uint8_t oversampling = 0; #endif // Based on the oversampling factor, do the calculations - step_event_count = current_block->step_event_count << oversampling; + step_event_count = current_block->step_event_count << oversampling_factor; // Initialize Bresenham delta errors to 1/2 #if ENABLED(INPUT_SHAPING_X) @@ -2578,8 +2575,8 @@ uint32_t Stepper::block_phase_isr() { step_events_completed = 0; // Compute the acceleration and deceleration points - accelerate_until = current_block->accelerate_until << oversampling; - decelerate_after = current_block->decelerate_after << oversampling; + accelerate_until = current_block->accelerate_until << oversampling_factor; + decelerate_after = current_block->decelerate_after << oversampling_factor; TERN_(MIXING_EXTRUDER, mixer.stepper_setup(current_block->b_color)); @@ -2593,7 +2590,7 @@ uint32_t Stepper::block_phase_isr() { #endif 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; + la_dividend = (advance_dividend.e << current_block->la_scaling) << oversampling_factor; } #endif From c2a9c5fbc1450a86342078f7527b8e8b3ef9405d Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 14 Nov 2022 10:25:02 +0000 Subject: [PATCH 20/33] Refined step frequency calculations for multi-stepping --- Marlin/src/module/stepper.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index ae856409bb49..46333d5217bc 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -210,13 +210,12 @@ #error "Expected at least one of MINIMUM_STEPPER_PULSE or MAXIMUM_STEPPER_RATE to be defined" #endif -#define HIGH_PULSE_CYCLES uint32_t(uint64_t(F_CPU) * (_MIN_PULSE_HIGH_NS) / 1000000000UL) - -// But the user could be enforcing a minimum time, so the loop time is -#define ISR_LOOP_CYCLES (ISR_LOOP_BASE_CYCLES + HIGH_PULSE_CYCLES + _MAX(MIN_STEPPER_PULSE_CYCLES, MIN_ISR_LOOP_CYCLES)) +// The loop takes the base time plus the time for all the bresenham logic for R pulses plus the time +// between pulses for (R-1) pulses. But the user could be enforcing a minimum time so the loop time is: +#define ISR_LOOP_CYCLES(R) ((ISR_LOOP_BASE_CYCLES + MIN_ISR_LOOP_CYCLES + MIN_STEPPER_PULSE_CYCLES) * (R - 1) + _MAX(MIN_ISR_LOOP_CYCLES, MIN_STEPPER_PULSE_CYCLES)) // Model input shaping as an extra loop call -#define ISR_SHAPING_LOOP_CYCLES (TERN0(HAS_SHAPING, ISR_LOOP_BASE_CYCLES + HIGH_PULSE_CYCLES) + TERN0(INPUT_SHAPING_X, ISR_X_STEPPER_CYCLES) + TERN0(INPUT_SHAPING_Y, ISR_Y_STEPPER_CYCLES)) +#define ISR_SHAPING_LOOP_CYCLES(R) ((TERN0(HAS_SHAPING, ISR_LOOP_BASE_CYCLES) + TERN0(INPUT_SHAPING_X, ISR_X_STEPPER_CYCLES) + TERN0(INPUT_SHAPING_Y, ISR_Y_STEPPER_CYCLES)) * (R) + (MIN_ISR_LOOP_CYCLES) * (R - 1)) // If linear advance is enabled, then it is handled separately #if ENABLED(LIN_ADVANCE) @@ -241,7 +240,7 @@ #endif // Now estimate the total ISR execution time in cycles given a step per ISR multiplier -#define ISR_EXECUTION_CYCLES(R) (((ISR_BASE_CYCLES + ISR_S_CURVE_CYCLES + ISR_SHAPING_BASE_CYCLES + (ISR_LOOP_CYCLES + ISR_SHAPING_LOOP_CYCLES) * (R) + ISR_LA_BASE_CYCLES + ISR_LA_LOOP_CYCLES)) / (R)) +#define ISR_EXECUTION_CYCLES(R) (((ISR_BASE_CYCLES + ISR_S_CURVE_CYCLES + ISR_SHAPING_BASE_CYCLES + ISR_LOOP_CYCLES(R) + ISR_SHAPING_LOOP_CYCLES(R) + ISR_LA_BASE_CYCLES + ISR_LA_LOOP_CYCLES)) / (R)) // The maximum allowable stepping frequency when doing x128-x1 stepping (in Hz) #define MAX_STEP_ISR_FREQUENCY_128X ((F_CPU) / ISR_EXECUTION_CYCLES(128)) From 24b589b8d2d9000b4bfc608b17df92fc44a4d037 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 14 Nov 2022 15:20:16 +0000 Subject: [PATCH 21/33] Add a sufficient delay when changing direction to prevent confusing the stepper driver --- Marlin/Configuration_adv.h | 2 ++ Marlin/src/module/stepper.cpp | 25 ++++++++++++++++++------- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 2daf6123784e..5ba26c5cbe2a 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1083,10 +1083,12 @@ #if ENABLED(INPUT_SHAPING_X) #define SHAPING_FREQ_X 40 // (Hz) The default dominant resonant frequency on the X axis. #define SHAPING_ZETA_X 0.3f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). + //#define SHAPING_MAX_STEPRATE_JUMP_X 10000 // (Hz) Set with TMC2208 or TMC2225 to prevent triggering an over-current condition. #endif #if ENABLED(INPUT_SHAPING_Y) #define SHAPING_FREQ_Y 40 // (Hz) The default dominant resonant frequency on the Y axis. #define SHAPING_ZETA_Y 0.3f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). + //#define SHAPING_MAX_STEPRATE_JUMP_Y 10000 // (Hz) Set with TMC2208 or TMC2225 to prevent triggering an over-current condition. #endif //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. #endif diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index c5f769985cb7..96b2717a7c96 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1688,19 +1688,30 @@ void Stepper::pulse_phase_isr() { } \ }while(0) + // Direction changes can happen with almost no time gap from a previous STEP pulse. Wait for + // the time given by MAXIMUM_STEPPER_RATE. Or, to work around the TMC2208 / TMC2225 over-current + // shutdown bug, wait for the time given by SHAPING_MAX_STEPRATE_JUMP_[XY]. + #ifdef SHAPING_MAX_STEPRATE_JUMP_X + #define AWAIT_DIRCHANGE_X while (((STEPPER_TIMER_RATE) / (SHAPING_MAX_STEPRATE_JUMP_X)) > HAL_timer_get_count(MF_TIMER_PULSE) - start_pulse_count) {} + #else + #define AWAIT_DIRCHANGE_X AWAIT_LOW_PULSE() + #endif + #ifdef SHAPING_MAX_STEPRATE_JUMP_Y + #define AWAIT_DIRCHANGE_Y while (((STEPPER_TIMER_RATE) / (SHAPING_MAX_STEPRATE_JUMP_Y)) > HAL_timer_get_count(MF_TIMER_PULSE) - start_pulse_count) {} + #else + #define AWAIT_DIRCHANGE_Y AWAIT_LOW_PULSE() + #endif + #define _AWAIT_DIRCHANGE(AXIS) AWAIT_DIRCHANGE_##AXIS + #define AWAIT_DIRCHANGE(AXIS) _AWAIT_DIRCHANGE(AXIS) + #define PULSE_PREP_SHAPING(AXIS, DIVIDEND) do{ \ delta_error[_AXIS(AXIS)] += (DIVIDEND); \ - if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] < -0x30000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] > 0x30000000L)) { \ + if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] < -0x10000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] > 0x10000000L)) { \ + { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_DIRCHANGE(AXIS); } \ TBI(last_direction_bits, _AXIS(AXIS)); \ DIR_WAIT_BEFORE(); \ SET_STEP_DIR(AXIS); \ DIR_WAIT_AFTER(); \ - _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), 0); \ - { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_HIGH_PULSE(); } \ - _APPLY_STEP(AXIS, _INVERT_STEP_PIN(AXIS), 0); \ - count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ - delta_error[_AXIS(AXIS)] += MAXDIR(AXIS) ? -0x20000000L : 0x20000000L; \ - { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } \ } \ step_needed[_AXIS(AXIS)] = (MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x10000000L) || \ (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x10000000L); \ From a1546cb3f084d55eb868161ba3f46e980439252e Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Wed, 16 Nov 2022 15:19:11 +0000 Subject: [PATCH 22/33] Take extra step rate from ADAPTIVE_STEP_SMOOTHING into account when setting buffer sizes --- Marlin/Configuration_adv.h | 7 ++++--- Marlin/src/module/stepper.h | 4 +++- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 5ba26c5cbe2a..3cf5fcc09952 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1065,9 +1065,10 @@ * This option uses a lot of SRAM for the step buffer, which is proportional * to the largest step rate possible for any axis. If the build fails due to * low SRAM the buffer size may be reduced by setting smaller values for - * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE. Runtime editing - * of max feedrate (M203) or resonant frequency (M593) may result feedrate - * being capped to prevent buffer overruns. + * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE and disabling + * ADAPTIVE_STEP_SMOOTHING. Runtime editing of max feedrate (M203) or + * resonant frequency (M593) may result feedrate being capped to prevent + * buffer overruns. * * Tune with M593 D F: * diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 46333d5217bc..6881aeb155d9 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -342,7 +342,9 @@ constexpr ena_mask_t enable_overlap[] = { max_feedrate.u * steps_per_unit.u, max_feedrate.v * steps_per_unit.v, max_feedrate.w * steps_per_unit.w - )); + ) + OPTARG(ADAPTIVE_STEP_SMOOTHING, MIN_STEP_ISR_FREQUENCY) + ); constexpr uint16_t shaping_dividends = max_steprate / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; constexpr uint16_t shaping_segments = max_steprate / (MIN_STEPS_PER_SEGMENT) / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; From 24a1b4d8835e6efd9460ad414d63c1925bc03cfc Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sat, 19 Nov 2022 10:58:19 +0000 Subject: [PATCH 23/33] Remove remaining small hysteresis on direction change --- Marlin/src/module/stepper.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 96b2717a7c96..8442546fcefd 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1706,15 +1706,14 @@ void Stepper::pulse_phase_isr() { #define PULSE_PREP_SHAPING(AXIS, DIVIDEND) do{ \ delta_error[_AXIS(AXIS)] += (DIVIDEND); \ - if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] < -0x10000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] > 0x10000000L)) { \ + if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x10000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x10000000L)) { \ { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_DIRCHANGE(AXIS); } \ TBI(last_direction_bits, _AXIS(AXIS)); \ DIR_WAIT_BEFORE(); \ SET_STEP_DIR(AXIS); \ DIR_WAIT_AFTER(); \ } \ - step_needed[_AXIS(AXIS)] = (MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x10000000L) || \ - (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x10000000L); \ + step_needed[_AXIS(AXIS)] = delta_error[_AXIS(AXIS)] >= 0x10000000L || delta_error[_AXIS(AXIS)] <= -0x10000000L; \ if (step_needed[_AXIS(AXIS)]) { \ count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ delta_error[_AXIS(AXIS)] += MAXDIR(AXIS) ? -0x20000000L : 0x20000000L; \ From 35904b34e3e5ab899c956a3b9a849535a0c4c750 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sat, 19 Nov 2022 15:22:04 +0000 Subject: [PATCH 24/33] Instead of changing existing bresenham logic, do IS with a secondary bresenham --- Marlin/Configuration_adv.h | 11 +- Marlin/src/module/planner.cpp | 9 +- Marlin/src/module/stepper.cpp | 187 +++++++++++++--------------------- Marlin/src/module/stepper.h | 132 ++++++++++++------------ 4 files changed, 148 insertions(+), 191 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 3cf5fcc09952..61ba1180db10 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1062,11 +1062,12 @@ * * Zero Vibration (ZV) Input Shaping for X and/or Y movements. * - * This option uses a lot of SRAM for the step buffer, which is proportional - * to the largest step rate possible for any axis. If the build fails due to - * low SRAM the buffer size may be reduced by setting smaller values for - * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE and disabling - * ADAPTIVE_STEP_SMOOTHING. Runtime editing of max feedrate (M203) or + * This option uses a lot of SRAM for the step buffer, which is related + * to the largest step rate possible for the shaped axes. If the build fails + * due to low SRAM the buffer size may be reduced by setting smaller values for + * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE. Disabling + * ADAPTIVE_STEP_SMOOTHING and reducing the step rate for non-shaped axes may + * also reduce the buffer sizes. Runtime editing of max feedrate (M203) or * resonant frequency (M593) may result feedrate being capped to prevent * buffer overruns. * diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 948fd39ad67d..af8d5e37375c 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -2494,7 +2494,14 @@ bool Planner::_populate_block( float bottom_freq = float(0x7FFFFFFFL), t; TERN_(INPUT_SHAPING_X, if ((t = stepper.get_shaping_frequency(X_AXIS))) NOMORE(bottom_freq, t)); TERN_(INPUT_SHAPING_Y, if ((t = stepper.get_shaping_frequency(Y_AXIS))) NOMORE(bottom_freq, t)); - const float max_factor = (bottom_freq * (float(shaping_dividends - 3) * 2.0f)) / block->nominal_rate; + const float max_echo_rate = bottom_freq * (float(shaping_echoes - 3) * 2.0f); + const float rate_sum = (TERN0(HAS_SHAPING_X, block->steps.x) + TERN0(HAS_SHAPING_Y, block->steps.y)) * inverse_secs; + #if ENABLED(ADAPTIVE_STEP_SMOOTHING) + const float limiting_rate = rate_sum; + #else + const float limiting_rate = _MIN(float(block->nominal_rate), rate_sum); + #endif + const float max_factor = limiting_rate ? max_echo_rate / limiting_rate : 1.0f; NOMORE(speed_factor, max_factor); #endif diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 8442546fcefd..1f5aedf322f3 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -233,9 +233,8 @@ uint32_t Stepper::advance_divisor = 0, #endif #if HAS_SHAPING - shaping_time_t DelayTimeManager::now = 0; - ParamDelayQueue Stepper::shaping_dividend_queue; - DelayQueue Stepper::shaping_queue; + shaping_time_t DelayTimeManager::now = 0; + DelayQueue Stepper::shaping_queue; #if ENABLED(INPUT_SHAPING_X) shaping_time_t DelayTimeManager::delay_x; ShapeParams Stepper::shaping_x; @@ -1482,8 +1481,6 @@ void Stepper::isr() { #if HAS_SHAPING // Speed limiting should ensure the buffers never get full. But if somehow they do, stutter rather than overflow. if (!nextMainISR) { - TERN_(INPUT_SHAPING_X, if (shaping_dividend_queue.free_count_x() == 0) nextMainISR = shaping_dividend_queue.peek_x() + 1); - TERN_(INPUT_SHAPING_Y, if (shaping_dividend_queue.free_count_y() == 0) NOLESS(nextMainISR, shaping_dividend_queue.peek_y() + 1)); TERN_(INPUT_SHAPING_X, if (shaping_queue.free_count_x() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_x() + 1)); TERN_(INPUT_SHAPING_Y, if (shaping_queue.free_count_y() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_y() + 1)); } @@ -1523,10 +1520,8 @@ void Stepper::isr() { const uint32_t interval = _MIN( uint32_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time nextMainISR // Time until the next Pulse / Block phase - OPTARG(INPUT_SHAPING_X, shaping_dividend_queue.peek_x()) // Time until next input shaping dividend change for X - OPTARG(INPUT_SHAPING_Y, shaping_dividend_queue.peek_y()) // Time until next input shaping dividend change for Y - OPTARG(INPUT_SHAPING_X, shaping_queue.peek_x()) // Time until next input shaping echo for X - OPTARG(INPUT_SHAPING_Y, shaping_queue.peek_y()) // Time until next input shaping echo for Y + OPTARG(INPUT_SHAPING_X, shaping_queue.peek_x()) // Time until next input shaping echo for X + OPTARG(INPUT_SHAPING_Y, shaping_queue.peek_y()) // Time until next input shaping echo for Y OPTARG(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance? OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping? ); @@ -1543,7 +1538,6 @@ void Stepper::isr() { #if HAS_SHAPING DelayTimeManager::decrement_delays(interval); shaping_queue.decrement_peeks(interval); - shaping_dividend_queue.decrement_peeks(interval); #endif #if ENABLED(LIN_ADVANCE) @@ -1641,10 +1635,9 @@ void Stepper::pulse_phase_isr() { if (current_block) { discard_current_block(); #if HAS_SHAPING - shaping_dividend_queue.purge(); shaping_queue.purge(); - if (TERN0(INPUT_SHAPING_X, shaping_x.enabled)) delta_error.x = 0; - if (TERN0(INPUT_SHAPING_Y, shaping_y.enabled)) delta_error.y = 0; + TERN_(INPUT_SHAPING_X, shaping_x.delta_error = 0); + TERN_(INPUT_SHAPING_Y, shaping_y.delta_error = 0); TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); TERN_(INPUT_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); #endif @@ -1682,10 +1675,8 @@ void Stepper::pulse_phase_isr() { #define PULSE_PREP(AXIS) do{ \ delta_error[_AXIS(AXIS)] += advance_dividend[_AXIS(AXIS)]; \ step_needed[_AXIS(AXIS)] = (delta_error[_AXIS(AXIS)] >= 0); \ - if (step_needed[_AXIS(AXIS)]) { \ - count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ + if (step_needed[_AXIS(AXIS)]) \ delta_error[_AXIS(AXIS)] -= advance_divisor; \ - } \ }while(0) // Direction changes can happen with almost no time gap from a previous STEP pulse. Wait for @@ -1704,25 +1695,26 @@ void Stepper::pulse_phase_isr() { #define _AWAIT_DIRCHANGE(AXIS) AWAIT_DIRCHANGE_##AXIS #define AWAIT_DIRCHANGE(AXIS) _AWAIT_DIRCHANGE(AXIS) - #define PULSE_PREP_SHAPING(AXIS, DIVIDEND) do{ \ - delta_error[_AXIS(AXIS)] += (DIVIDEND); \ - if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x10000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x10000000L)) { \ - { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_DIRCHANGE(AXIS); } \ - TBI(last_direction_bits, _AXIS(AXIS)); \ - DIR_WAIT_BEFORE(); \ - SET_STEP_DIR(AXIS); \ - DIR_WAIT_AFTER(); \ - } \ - step_needed[_AXIS(AXIS)] = delta_error[_AXIS(AXIS)] >= 0x10000000L || delta_error[_AXIS(AXIS)] <= -0x10000000L; \ + #define PULSE_PREP_SHAPING(AXIS, DELTA_ERROR, DIVIDEND) do{ \ if (step_needed[_AXIS(AXIS)]) { \ - count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ - delta_error[_AXIS(AXIS)] += MAXDIR(AXIS) ? -0x20000000L : 0x20000000L; \ + DELTA_ERROR += (DIVIDEND); \ + if ((MAXDIR(AXIS) && DELTA_ERROR <= -64) || (MINDIR(AXIS) && DELTA_ERROR >= 64)) { \ + { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_DIRCHANGE(AXIS); } \ + TBI(last_direction_bits, _AXIS(AXIS)); \ + DIR_WAIT_BEFORE(); \ + SET_STEP_DIR(AXIS); \ + DIR_WAIT_AFTER(); \ + } \ + step_needed[_AXIS(AXIS)] = DELTA_ERROR >= 64 || DELTA_ERROR <= -64; \ + if (step_needed[_AXIS(AXIS)]) \ + DELTA_ERROR += MAXDIR(AXIS) ? -128 : 128; \ } \ }while(0) // Start an active pulse if needed #define PULSE_START(AXIS) do{ \ if (step_needed[_AXIS(AXIS)]) { \ + count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \ _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), 0); \ } \ }while(0) @@ -1841,20 +1833,12 @@ void Stepper::pulse_phase_isr() { #endif // DIRECT_STEPPING if (!is_page) { - TERN_(HAS_SHAPING, shaping_queue.enqueue()); - // Determine if pulses are needed #if HAS_X_STEP - if (TERN0(INPUT_SHAPING_X, shaping_x.enabled)) - PULSE_PREP_SHAPING(X, advance_dividend.x); - else - PULSE_PREP(X); + PULSE_PREP(X); #endif #if HAS_Y_STEP - if (TERN0(INPUT_SHAPING_Y, shaping_y.enabled)) - PULSE_PREP_SHAPING(Y, advance_dividend.y); - else - PULSE_PREP(Y); + PULSE_PREP(Y); #endif #if HAS_Z_STEP PULSE_PREP(Z); @@ -1891,6 +1875,24 @@ void Stepper::pulse_phase_isr() { } #endif #endif + + #if HAS_SHAPING + // record an echo if a step is needed in the primary bresenham + const bool x_step = TERN0(INPUT_SHAPING_X, shaping_x.enabled && step_needed[X_AXIS]), + y_step = TERN0(INPUT_SHAPING_Y, shaping_y.enabled && step_needed[Y_AXIS]); + if (x_step || y_step) + shaping_queue.enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward)); + + // do the first part of the secondary bresenham + #if ENABLED(INPUT_SHAPING_X) + if (shaping_x.enabled) + PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.factor1 * (shaping_x.forward ? 1 : -1)); + #endif + #if ENABLED(INPUT_SHAPING_Y) + if (shaping_y.enabled) + PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.factor1 * (shaping_y.forward ? 1 : -1)); + #endif + #endif } #if ISR_MULTI_STEPS @@ -1930,7 +1932,10 @@ void Stepper::pulse_phase_isr() { #endif #if ENABLED(MIXING_EXTRUDER) - if (step_needed.e) E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN); + if (step_needed.e) { + count_position[E_AXIS] += count_direction[E_AXIS]; + E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN); + } #elif HAS_E0_STEP PULSE_START(E); #endif @@ -1990,22 +1995,22 @@ void Stepper::pulse_phase_isr() { void Stepper::shaping_isr() { xy_bool_t step_needed{0}; - bool shapex = TERN0(INPUT_SHAPING_X, !shaping_queue.peek_x()), - shapey = TERN0(INPUT_SHAPING_Y, !shaping_queue.peek_y()); + step_needed[X_AXIS] = TERN0(INPUT_SHAPING_X, !shaping_queue.peek_x()); + step_needed[Y_AXIS] = TERN0(INPUT_SHAPING_Y, !shaping_queue.peek_y()); - if (shapex || shapey) while (true) { + if (bool(step_needed)) while (true) { #if ENABLED(INPUT_SHAPING_X) - if (shapex) { - shaping_queue.dequeue_x(); - PULSE_PREP_SHAPING(X, shaping_x.dividend); + if (step_needed[X_AXIS]) { + bool forward = shaping_queue.dequeue_x(); + PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.factor2 * (forward ? 1 : -1)); PULSE_START(X); } #endif #if ENABLED(INPUT_SHAPING_Y) - if (shapey) { - shaping_queue.dequeue_y(); - PULSE_PREP_SHAPING(Y, shaping_y.dividend); + if (step_needed[Y_AXIS]) { + bool forward = shaping_queue.dequeue_y(); + PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.factor2 * (forward ? 1 : -1)); PULSE_START(Y); } #endif @@ -2026,24 +2031,14 @@ void Stepper::pulse_phase_isr() { #endif } - shapex = TERN0(INPUT_SHAPING_X, !shaping_queue.peek_x()); - shapey = TERN0(INPUT_SHAPING_Y, !shaping_queue.peek_y()); + step_needed[X_AXIS] = TERN0(INPUT_SHAPING_X, !shaping_queue.peek_x()); + step_needed[Y_AXIS] = TERN0(INPUT_SHAPING_Y, !shaping_queue.peek_y()); - if (!shapex && !shapey) break; + if (!bool(step_needed)) break; START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } - - // if a new dividend appears in the same "moment" that steps takes place, it is because - // the steps are the last steps of a segment and the new dividend is for the new segment - // so only update the dividend after the steps - #if ENABLED(INPUT_SHAPING_X) - if (!shaping_dividend_queue.peek_x()) shaping_x.dividend = shaping_dividend_queue.dequeue_x(); - #endif - #if ENABLED(INPUT_SHAPING_Y) - if (!shaping_dividend_queue.peek_y()) shaping_y.dividend = shaping_dividend_queue.dequeue_y(); - #endif } #endif // HAS_SHAPING @@ -2509,78 +2504,35 @@ uint32_t Stepper::block_phase_isr() { step_event_count = current_block->step_event_count << oversampling_factor; // Initialize Bresenham delta errors to 1/2 - #if ENABLED(INPUT_SHAPING_X) - const int32_t old_delta_error_x = delta_error.x; - #endif - #if ENABLED(INPUT_SHAPING_Y) - const int32_t old_delta_error_y = delta_error.y; - #endif delta_error = TERN_(LIN_ADVANCE, la_delta_error =) -int32_t(step_event_count); - // Compiler should optimize these out, by not assigning them in the first place - TERN_(INPUT_SHAPING_X, delta_error.x = old_delta_error_x); - TERN_(INPUT_SHAPING_Y, delta_error.y = old_delta_error_y); // Calculate Bresenham dividends and divisors advance_dividend = (current_block->steps << 1).asLong(); advance_divisor = step_event_count << 1; #if ENABLED(INPUT_SHAPING_X) - int32_t echo_x = 0; if (shaping_x.enabled) { const int64_t steps = TEST(current_block->direction_bits, X_AXIS) ? -int64_t(current_block->steps.x) : int64_t(current_block->steps.x); shaping_x.last_block_end_pos += steps; - // For input shaped axes, advance_divisor is replaced with 0x20000000 - // and the dividend is directional, i.e. signed. - advance_dividend.x = ((steps << 29) + shaping_x.remainder) / step_event_count; - LIMIT(advance_dividend.x, -0x20000000, 0x20000000); - - // The scaling operation above introduces small rounding errors which are recorded in shaping_x.remainder - // and carried over to the next segment. - // For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes. - // For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000 - // so delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000. - // For a divisor which is a power of 2, modulo is the same as as a bitmask, so that is - // (advance_dividend.x * step_event_count) & 0x1FFFFFFF. - // Since the bitmask removed the sign, this needs to be adjusted to the range -0x10000000 to 0x10000000. - // Adding and subtracting 0x10000000 inside the outside the modulo achieves this, i.e. - // ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL) - 0x10000000L - // This segment's final change in delta_error should actually be shaping_x.remainder so the new remainder is - // shaping_x.remainder - (((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL) - 0x10000000L) - shaping_x.remainder += 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL); - // If there are any remaining echos unprocessed, then direction change must // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step // to be missed, which will need recovering and this can be done through shaping_x.remainder. + shaping_x.forward = !TEST(current_block->direction_bits, X_AXIS); if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS)); - - // when there is damping, the signal and its echo have different amplitudes - echo_x = shaping_x.factor * (advance_dividend.x >> 7); - - // apply the adjustment to the primary signal - advance_dividend.x -= echo_x; } #endif // Y follows the same logic as X (but the comments aren't repeated) #if ENABLED(INPUT_SHAPING_Y) - int32_t echo_y = 0; if (shaping_y.enabled) { const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); shaping_y.last_block_end_pos += steps; - advance_dividend.y = ((steps << 29) + shaping_y.remainder) / step_event_count; - LIMIT(advance_dividend.y, -0x20000000, 0x20000000); - shaping_y.remainder += 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL); + shaping_y.forward = !TEST(current_block->direction_bits, Y_AXIS); if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS)); - echo_y = shaping_y.factor * (advance_dividend.y >> 7); - advance_dividend.y -= echo_y; } #endif - // plan the change of values for advance_dividend for the input shaping echoes - if (TERN0(INPUT_SHAPING_X, shaping_x.enabled) || TERN0(INPUT_SHAPING_Y, shaping_y.enabled)) - TERN_(HAS_SHAPING, shaping_dividend_queue.enqueue(TERN0(INPUT_SHAPING_X, echo_x), TERN0(INPUT_SHAPING_Y, echo_y))); - // No step events completed so far step_events_completed = 0; @@ -3035,21 +2987,22 @@ void Stepper::init() { // from the damping ratio, get a factor that can be applied to advance_dividend for fixed point maths // for ZV, we use amplitudes 1/(1+K) and K/(1+K) where K = exp(-zeta * M_PI / sqrt(1.0f - zeta * zeta)) // which can be converted to 1:7 fixed point with an excellent fit with a 3rd order polynomial - float shaping_factor; - if (zeta <= 0.0f) shaping_factor = 64.0f; - else if (zeta >= 1.0f) shaping_factor = 0.0f; + float factor2; + if (zeta <= 0.0f) factor2 = 64.0f; + else if (zeta >= 1.0f) factor2 = 0.0f; else { - shaping_factor = 64.44056192 + -99.02008832 * zeta; + factor2 = 64.44056192 + -99.02008832 * zeta; const float zeta2 = zeta * zeta; - shaping_factor += -7.58095488 * zeta2; + factor2 += -7.58095488 * zeta2; const float zeta3 = zeta2 * zeta; - shaping_factor += 43.073216 * zeta3; + factor2 += 43.073216 * zeta3; + factor2 = floor(factor2); } const bool was_on = hal.isr_state(); hal.isr_off(); - TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor = floor(shaping_factor); shaping_x.zeta = zeta; }) - TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor = floor(shaping_factor); shaping_y.zeta = zeta; }) + TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor2 = factor2; shaping_x.factor1 = 128 - factor2; shaping_x.zeta = zeta; }) + TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor2 = factor2; shaping_y.factor1 = 128 - factor2; shaping_y.zeta = zeta; }) if (was_on) hal.isr_on(); } @@ -3071,17 +3024,15 @@ void Stepper::init() { DelayTimeManager::set_delay(X_AXIS, delay); TERN_(INPUT_SHAPING_X, shaping_x.frequency = freq); TERN_(INPUT_SHAPING_X, shaping_x.enabled = !!freq); - delta_error = 0L; + TERN_(INPUT_SHAPING_X, shaping_x.delta_error = 0); TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); - TERN_(INPUT_SHAPING_X, shaping_x.dividend = shaping_x.remainder = 0UL); } if (TERN0(INPUT_SHAPING_Y, axis == Y_AXIS)) { DelayTimeManager::set_delay(Y_AXIS, delay); TERN_(INPUT_SHAPING_Y, shaping_y.frequency = freq); TERN_(INPUT_SHAPING_Y, shaping_y.enabled = !!freq); - delta_error = 0L; + TERN_(INPUT_SHAPING_Y, shaping_y.delta_error = 0); TERN_(INPUT_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); - TERN_(INPUT_SHAPING_Y, shaping_y.dividend = shaping_y.remainder = 0UL); } if (was_on) hal.isr_on(); diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 6881aeb155d9..4544c659ebe0 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -331,25 +331,35 @@ constexpr ena_mask_t enable_overlap[] = { // These constexpr are used to calculate the shaping queue buffer sizes constexpr xyze_float_t max_feedrate = DEFAULT_MAX_FEEDRATE; constexpr xyze_float_t steps_per_unit = DEFAULT_AXIS_STEPS_PER_UNIT; - constexpr float max_steprate = _MAX(LOGICAL_AXIS_LIST( - max_feedrate.e * steps_per_unit.e, - max_feedrate.x * steps_per_unit.x, - max_feedrate.y * steps_per_unit.y, - max_feedrate.z * steps_per_unit.z, - max_feedrate.i * steps_per_unit.i, - max_feedrate.j * steps_per_unit.j, - max_feedrate.k * steps_per_unit.k, - max_feedrate.u * steps_per_unit.u, - max_feedrate.v * steps_per_unit.v, - max_feedrate.w * steps_per_unit.w - ) - OPTARG(ADAPTIVE_STEP_SMOOTHING, MIN_STEP_ISR_FREQUENCY) - ); - constexpr uint16_t shaping_dividends = max_steprate / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; - constexpr uint16_t shaping_segments = max_steprate / (MIN_STEPS_PER_SEGMENT) / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; + // MIN_STEP_ISR_FREQUENCY is known at compile time on AVRs and any reduction in SRAM is welcome + #ifdef __AVR__ + constexpr float max_isr_rate = _MAX( + LOGICAL_AXIS_LIST( + max_feedrate.e * steps_per_unit.e, + max_feedrate.x * steps_per_unit.x, + max_feedrate.y * steps_per_unit.y, + max_feedrate.z * steps_per_unit.z, + max_feedrate.i * steps_per_unit.i, + max_feedrate.j * steps_per_unit.j, + max_feedrate.k * steps_per_unit.k, + max_feedrate.u * steps_per_unit.u, + max_feedrate.v * steps_per_unit.v, + max_feedrate.w * steps_per_unit.w + ) + OPTARG(ADAPTIVE_STEP_SMOOTHING, MIN_STEP_ISR_FREQUENCY) + ); + constexpr float max_step_rate = _MIN(max_isr_rate, + TERN0(INPUT_SHAPING_X, max_feedrate.x * steps_per_unit.x) + + TERN0(INPUT_SHAPING_Y, max_feedrate.y * steps_per_unit.y) + ); + #else + constexpr float max_step_rate = TERN0(INPUT_SHAPING_X, max_feedrate.x * steps_per_unit.x) + + TERN0(INPUT_SHAPING_Y, max_feedrate.y * steps_per_unit.y); + #endif + constexpr uint16_t shaping_echoes = max_step_rate / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; class DelayTimeManager { - private: + protected: static shaping_time_t now; #if ENABLED(INPUT_SHAPING_X) static shaping_time_t delay_x; // = shaping_time_t(-1) to disable queueing @@ -365,10 +375,19 @@ constexpr ena_mask_t enable_overlap[] = { } }; - template class DelayQueue : public DelayTimeManager { protected: - shaping_time_t times[SIZE]; + shaping_time_t times[shaping_echoes]; + enum echo_t { ECHO_NONE = 0, ECHO_FWD = 1, ECHO_BWD = 2 }; + struct { + #if ENABLED(INPUT_SHAPING_X) + echo_t x:2; + #endif + #if ENABLED(INPUT_SHAPING_Y) + echo_t y:2; + #endif + } echo_axes[shaping_echoes]; + #if ENABLED(INPUT_SHAPING_X) shaping_time_t peek_x_val = shaping_time_t(-1); uint16_t head_x = 0; @@ -384,31 +403,41 @@ constexpr ena_mask_t enable_overlap[] = { TERN_(INPUT_SHAPING_X, if (peek_x_val != shaping_time_t(-1)) peek_x_val -= interval); TERN_(INPUT_SHAPING_Y, if (peek_y_val != shaping_time_t(-1)) peek_y_val -= interval); } - void enqueue() { - TERN_(INPUT_SHAPING_X, if (head_x == tail) peek_x_val = delay_x); - TERN_(INPUT_SHAPING_Y, if (head_y == tail) peek_y_val = delay_y); + void enqueue(const bool x_step, const bool x_forward, const bool y_step, const bool y_forward) { + TERN_(INPUT_SHAPING_X, if (head_x == tail && x_step) peek_x_val = delay_x); + TERN_(INPUT_SHAPING_Y, if (head_y == tail && y_step) peek_y_val = delay_y); times[tail] = now; - if (++tail == SIZE) tail = 0; - TERN_(INPUT_SHAPING_X, if (delay_x == shaping_time_t(-1)) head_x = tail); - TERN_(INPUT_SHAPING_Y, if (delay_y == shaping_time_t(-1)) head_y = tail); + TERN_(INPUT_SHAPING_X, echo_axes[tail].x = x_step ? (x_forward ? ECHO_FWD : ECHO_BWD) : ECHO_NONE); + TERN_(INPUT_SHAPING_Y, echo_axes[tail].y = y_step ? (y_forward ? ECHO_FWD : ECHO_BWD) : ECHO_NONE); + if (++tail == shaping_echoes) tail = 0; + TERN_(INPUT_SHAPING_X, if (echo_axes[head_x].x == ECHO_NONE) dequeue_x()); + TERN_(INPUT_SHAPING_Y, if (echo_axes[head_y].y == ECHO_NONE) dequeue_y()); } #if ENABLED(INPUT_SHAPING_X) shaping_time_t peek_x() { return peek_x_val; } - void dequeue_x() { - if (++head_x == SIZE) head_x = 0; + bool dequeue_x() { + bool forward = echo_axes[head_x].x == ECHO_FWD; + do + if (++head_x == shaping_echoes) head_x = 0; + while (head_x != tail && echo_axes[head_x].x == ECHO_NONE); peek_x_val = head_x == tail ? shaping_time_t(-1) : times[head_x] + delay_x - now; + return forward; } bool empty_x() { return head_x == tail; } - uint16_t free_count_x() { return (head_x > tail ? 0 : SIZE) + (head_x - tail - 1); } + uint16_t free_count_x() { return (head_x > tail ? 0 : shaping_echoes) + (head_x - tail - 1); } #endif #if ENABLED(INPUT_SHAPING_Y) shaping_time_t peek_y() { return peek_y_val; } - void dequeue_y() { - if (++head_y == SIZE) head_y = 0; + bool dequeue_y() { + bool forward = echo_axes[head_y].y == ECHO_FWD; + do + if (++head_y == shaping_echoes) head_y = 0; + while (head_y != tail && echo_axes[head_y].y == ECHO_NONE); peek_y_val = head_y == tail ? shaping_time_t(-1) : times[head_y] + delay_y - now; + return forward; } bool empty_y() { return head_y == tail; } - uint16_t free_count_y() { return (head_y > tail ? 0 : SIZE) + (head_y - tail - 1); } + uint16_t free_count_y() { return (head_y > tail ? 0 : shaping_echoes) + (head_y - tail - 1); } #endif void purge() { const auto st = shaping_time_t(-1); @@ -421,44 +450,14 @@ constexpr ena_mask_t enable_overlap[] = { } }; - class ParamDelayQueue : public DelayQueue { - private: - #if ENABLED(INPUT_SHAPING_X) - int32_t params_x[shaping_segments]; - #endif - #if ENABLED(INPUT_SHAPING_Y) - int32_t params_y[shaping_segments]; - #endif - - public: - void enqueue(const int32_t param_x, const int32_t param_y) { - TERN(INPUT_SHAPING_X, params_x[DelayQueue::tail] = param_x, UNUSED(param_x)); - TERN(INPUT_SHAPING_Y, params_y[DelayQueue::tail] = param_y, UNUSED(param_y)); - DelayQueue::enqueue(); - } - #if ENABLED(INPUT_SHAPING_X) - const int32_t dequeue_x() { - const int32_t result = params_x[DelayQueue::head_x]; - DelayQueue::dequeue_x(); - return result; - } - #endif - #if ENABLED(INPUT_SHAPING_Y) - const int32_t dequeue_y() { - const int32_t result = params_y[DelayQueue::head_y]; - DelayQueue::dequeue_y(); - return result; - } - #endif - }; - struct ShapeParams { float frequency; float zeta; bool enabled; - uint8_t factor; - int32_t dividend = 0UL; - int32_t remainder = 0UL; + int16_t delta_error = 0; // delta_error for seconday bresenham mod 128 + uint8_t factor1; + uint8_t factor2; + bool forward; int32_t last_block_end_pos = 0; }; @@ -569,8 +568,7 @@ class Stepper { #endif #if HAS_SHAPING - static ParamDelayQueue shaping_dividend_queue; - static DelayQueue shaping_queue; + static DelayQueue shaping_queue; #if ENABLED(INPUT_SHAPING_X) static ShapeParams shaping_x; #endif From 62eb0c530aabcc302d0bf833714683d9de8b2fff Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sun, 20 Nov 2022 16:12:14 +0000 Subject: [PATCH 25/33] Cleanup --- Marlin/src/module/stepper.cpp | 54 ++++++++++++---------- Marlin/src/module/stepper.h | 85 ++++++++++++++++------------------- 2 files changed, 68 insertions(+), 71 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 1f5aedf322f3..df119486b248 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -233,15 +233,22 @@ uint32_t Stepper::advance_divisor = 0, #endif #if HAS_SHAPING - shaping_time_t DelayTimeManager::now = 0; - DelayQueue Stepper::shaping_queue; + shaping_time_t ShapingQueue::now = 0; + shaping_time_t ShapingQueue::times[shaping_echoes]; + shaping_echo_axis_t ShapingQueue::echo_axes[shaping_echoes]; + uint16_t ShapingQueue::tail = 0; + #if ENABLED(INPUT_SHAPING_X) - shaping_time_t DelayTimeManager::delay_x; - ShapeParams Stepper::shaping_x; + shaping_time_t ShapingQueue::delay_x; + shaping_time_t ShapingQueue::peek_x_val = shaping_time_t(-1); + uint16_t ShapingQueue::head_x = 0; + ShapeParams Stepper::shaping_x; #endif #if ENABLED(INPUT_SHAPING_Y) - shaping_time_t DelayTimeManager::delay_y; - ShapeParams Stepper::shaping_y; + shaping_time_t ShapingQueue::delay_y; + shaping_time_t ShapingQueue::peek_y_val = shaping_time_t(-1); + uint16_t ShapingQueue::head_y = 0; + ShapeParams Stepper::shaping_y; #endif #endif @@ -1481,8 +1488,8 @@ void Stepper::isr() { #if HAS_SHAPING // Speed limiting should ensure the buffers never get full. But if somehow they do, stutter rather than overflow. if (!nextMainISR) { - TERN_(INPUT_SHAPING_X, if (shaping_queue.free_count_x() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_x() + 1)); - TERN_(INPUT_SHAPING_Y, if (shaping_queue.free_count_y() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_y() + 1)); + TERN_(INPUT_SHAPING_X, if (ShapingQueue::free_count_x() < steps_per_isr) NOLESS(nextMainISR, ShapingQueue::peek_x() + 1)); + TERN_(INPUT_SHAPING_Y, if (ShapingQueue::free_count_y() < steps_per_isr) NOLESS(nextMainISR, ShapingQueue::peek_y() + 1)); } #endif @@ -1520,8 +1527,8 @@ void Stepper::isr() { const uint32_t interval = _MIN( uint32_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time nextMainISR // Time until the next Pulse / Block phase - OPTARG(INPUT_SHAPING_X, shaping_queue.peek_x()) // Time until next input shaping echo for X - OPTARG(INPUT_SHAPING_Y, shaping_queue.peek_y()) // Time until next input shaping echo for Y + 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 OPTARG(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance? OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping? ); @@ -1536,8 +1543,7 @@ void Stepper::isr() { nextMainISR -= interval; #if HAS_SHAPING - DelayTimeManager::decrement_delays(interval); - shaping_queue.decrement_peeks(interval); + ShapingQueue::decrement_delays(interval); #endif #if ENABLED(LIN_ADVANCE) @@ -1635,7 +1641,7 @@ void Stepper::pulse_phase_isr() { if (current_block) { discard_current_block(); #if HAS_SHAPING - shaping_queue.purge(); + ShapingQueue::purge(); TERN_(INPUT_SHAPING_X, shaping_x.delta_error = 0); TERN_(INPUT_SHAPING_Y, shaping_y.delta_error = 0); TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); @@ -1881,7 +1887,7 @@ void Stepper::pulse_phase_isr() { const bool x_step = TERN0(INPUT_SHAPING_X, shaping_x.enabled && step_needed[X_AXIS]), y_step = TERN0(INPUT_SHAPING_Y, shaping_y.enabled && step_needed[Y_AXIS]); if (x_step || y_step) - shaping_queue.enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward)); + ShapingQueue::enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward)); // do the first part of the secondary bresenham #if ENABLED(INPUT_SHAPING_X) @@ -1995,13 +2001,13 @@ void Stepper::pulse_phase_isr() { void Stepper::shaping_isr() { xy_bool_t step_needed{0}; - step_needed[X_AXIS] = TERN0(INPUT_SHAPING_X, !shaping_queue.peek_x()); - step_needed[Y_AXIS] = TERN0(INPUT_SHAPING_Y, !shaping_queue.peek_y()); + step_needed[X_AXIS] = TERN0(INPUT_SHAPING_X, !ShapingQueue::peek_x()); + step_needed[Y_AXIS] = TERN0(INPUT_SHAPING_Y, !ShapingQueue::peek_y()); if (bool(step_needed)) while (true) { #if ENABLED(INPUT_SHAPING_X) if (step_needed[X_AXIS]) { - bool forward = shaping_queue.dequeue_x(); + bool forward = ShapingQueue::dequeue_x(); PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.factor2 * (forward ? 1 : -1)); PULSE_START(X); } @@ -2009,7 +2015,7 @@ void Stepper::pulse_phase_isr() { #if ENABLED(INPUT_SHAPING_Y) if (step_needed[Y_AXIS]) { - bool forward = shaping_queue.dequeue_y(); + bool forward = ShapingQueue::dequeue_y(); PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.factor2 * (forward ? 1 : -1)); PULSE_START(Y); } @@ -2031,8 +2037,8 @@ void Stepper::pulse_phase_isr() { #endif } - step_needed[X_AXIS] = TERN0(INPUT_SHAPING_X, !shaping_queue.peek_x()); - step_needed[Y_AXIS] = TERN0(INPUT_SHAPING_Y, !shaping_queue.peek_y()); + step_needed[X_AXIS] = TERN0(INPUT_SHAPING_X, !ShapingQueue::peek_x()); + step_needed[Y_AXIS] = TERN0(INPUT_SHAPING_Y, !ShapingQueue::peek_y()); if (!bool(step_needed)) break; @@ -2519,7 +2525,7 @@ uint32_t Stepper::block_phase_isr() { // be delayed and processed in PULSE_PREP_SHAPING. This will cause half a step // to be missed, which will need recovering and this can be done through shaping_x.remainder. shaping_x.forward = !TEST(current_block->direction_bits, X_AXIS); - if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS)); + if (!ShapingQueue::empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS)); } #endif @@ -2529,7 +2535,7 @@ uint32_t Stepper::block_phase_isr() { const int64_t steps = TEST(current_block->direction_bits, Y_AXIS) ? -int64_t(current_block->steps.y) : int64_t(current_block->steps.y); shaping_y.last_block_end_pos += steps; shaping_y.forward = !TEST(current_block->direction_bits, Y_AXIS); - if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS)); + if (!ShapingQueue::empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS)); } #endif @@ -3021,14 +3027,14 @@ void Stepper::init() { const shaping_time_t delay = freq ? float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq : shaping_time_t(-1); if (TERN0(INPUT_SHAPING_X, axis == X_AXIS)) { - DelayTimeManager::set_delay(X_AXIS, delay); + ShapingQueue::set_delay(X_AXIS, delay); TERN_(INPUT_SHAPING_X, shaping_x.frequency = freq); TERN_(INPUT_SHAPING_X, shaping_x.enabled = !!freq); TERN_(INPUT_SHAPING_X, shaping_x.delta_error = 0); TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); } if (TERN0(INPUT_SHAPING_Y, axis == Y_AXIS)) { - DelayTimeManager::set_delay(Y_AXIS, delay); + ShapingQueue::set_delay(Y_AXIS, delay); TERN_(INPUT_SHAPING_Y, shaping_y.frequency = freq); TERN_(INPUT_SHAPING_Y, shaping_y.enabled = !!freq); TERN_(INPUT_SHAPING_Y, shaping_y.delta_error = 0); diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 4544c659ebe0..552f1f9243d0 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -326,8 +326,6 @@ constexpr ena_mask_t enable_overlap[] = { #if HAS_SHAPING - typedef IF::type shaping_time_t; - // These constexpr are used to calculate the shaping queue buffer sizes constexpr xyze_float_t max_feedrate = DEFAULT_MAX_FEEDRATE; constexpr xyze_float_t steps_per_unit = DEFAULT_AXIS_STEPS_PER_UNIT; @@ -358,52 +356,46 @@ constexpr ena_mask_t enable_overlap[] = { #endif constexpr uint16_t shaping_echoes = max_step_rate / _MIN(0x7FFFFFFFL OPTARG(INPUT_SHAPING_X, SHAPING_FREQ_X) OPTARG(INPUT_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3; - class DelayTimeManager { - protected: - static shaping_time_t now; - #if ENABLED(INPUT_SHAPING_X) - static shaping_time_t delay_x; // = shaping_time_t(-1) to disable queueing - #endif - #if ENABLED(INPUT_SHAPING_Y) - static shaping_time_t delay_y; // = shaping_time_t(-1) to disable queueing - #endif - public: - static void decrement_delays(const shaping_time_t interval) { now += interval; } - static void set_delay(const AxisEnum axis, const shaping_time_t delay) { - TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) delay_x = delay); - TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay); - } + typedef IF::type shaping_time_t; + enum shaping_echo_t { ECHO_NONE = 0, ECHO_FWD = 1, ECHO_BWD = 2 }; + struct shaping_echo_axis_t { + #if ENABLED(INPUT_SHAPING_X) + shaping_echo_t x:2; + #endif + #if ENABLED(INPUT_SHAPING_Y) + shaping_echo_t y:2; + #endif }; - class DelayQueue : public DelayTimeManager { - protected: - shaping_time_t times[shaping_echoes]; - enum echo_t { ECHO_NONE = 0, ECHO_FWD = 1, ECHO_BWD = 2 }; - struct { - #if ENABLED(INPUT_SHAPING_X) - echo_t x:2; - #endif - #if ENABLED(INPUT_SHAPING_Y) - echo_t y:2; - #endif - } echo_axes[shaping_echoes]; + class ShapingQueue { + private: + static shaping_time_t now; + static shaping_time_t times[shaping_echoes]; + static shaping_echo_axis_t echo_axes[shaping_echoes]; + static uint16_t tail; #if ENABLED(INPUT_SHAPING_X) - shaping_time_t peek_x_val = shaping_time_t(-1); - uint16_t head_x = 0; + static shaping_time_t delay_x; // = shaping_time_t(-1) to disable queueing + static shaping_time_t peek_x_val; + static uint16_t head_x; #endif #if ENABLED(INPUT_SHAPING_Y) - shaping_time_t peek_y_val = shaping_time_t(-1); - uint16_t head_y = 0; + static shaping_time_t delay_y; // = shaping_time_t(-1) to disable queueing + static shaping_time_t peek_y_val; + static uint16_t head_y; #endif - uint16_t tail = 0; public: - void decrement_peeks(const shaping_time_t interval) { + static void decrement_delays(const shaping_time_t interval) { + now += interval; TERN_(INPUT_SHAPING_X, if (peek_x_val != shaping_time_t(-1)) peek_x_val -= interval); TERN_(INPUT_SHAPING_Y, if (peek_y_val != shaping_time_t(-1)) peek_y_val -= interval); } - void enqueue(const bool x_step, const bool x_forward, const bool y_step, const bool y_forward) { + static void set_delay(const AxisEnum axis, const shaping_time_t delay) { + TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) delay_x = delay); + TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay); + } + static void enqueue(const bool x_step, const bool x_forward, const bool y_step, const bool y_forward) { TERN_(INPUT_SHAPING_X, if (head_x == tail && x_step) peek_x_val = delay_x); TERN_(INPUT_SHAPING_Y, if (head_y == tail && y_step) peek_y_val = delay_y); times[tail] = now; @@ -414,8 +406,8 @@ constexpr ena_mask_t enable_overlap[] = { TERN_(INPUT_SHAPING_Y, if (echo_axes[head_y].y == ECHO_NONE) dequeue_y()); } #if ENABLED(INPUT_SHAPING_X) - shaping_time_t peek_x() { return peek_x_val; } - bool dequeue_x() { + static shaping_time_t peek_x() { return peek_x_val; } + static bool dequeue_x() { bool forward = echo_axes[head_x].x == ECHO_FWD; do if (++head_x == shaping_echoes) head_x = 0; @@ -423,12 +415,12 @@ constexpr ena_mask_t enable_overlap[] = { peek_x_val = head_x == tail ? shaping_time_t(-1) : times[head_x] + delay_x - now; return forward; } - bool empty_x() { return head_x == tail; } - uint16_t free_count_x() { return (head_x > tail ? 0 : shaping_echoes) + (head_x - tail - 1); } + static bool empty_x() { return head_x == tail; } + static uint16_t free_count_x() { return (head_x > tail ? 0 : shaping_echoes) + (head_x - tail - 1); } #endif #if ENABLED(INPUT_SHAPING_Y) - shaping_time_t peek_y() { return peek_y_val; } - bool dequeue_y() { + static shaping_time_t peek_y() { return peek_y_val; } + static bool dequeue_y() { bool forward = echo_axes[head_y].y == ECHO_FWD; do if (++head_y == shaping_echoes) head_y = 0; @@ -436,10 +428,10 @@ constexpr ena_mask_t enable_overlap[] = { peek_y_val = head_y == tail ? shaping_time_t(-1) : times[head_y] + delay_y - now; return forward; } - bool empty_y() { return head_y == tail; } - uint16_t free_count_y() { return (head_y > tail ? 0 : shaping_echoes) + (head_y - tail - 1); } + static bool empty_y() { return head_y == tail; } + static uint16_t free_count_y() { return (head_y > tail ? 0 : shaping_echoes) + (head_y - tail - 1); } #endif - void purge() { + static void purge() { const auto st = shaping_time_t(-1); #if ENABLED(INPUT_SHAPING_X) head_x = tail; peek_x_val = st; @@ -568,7 +560,6 @@ class Stepper { #endif #if HAS_SHAPING - static DelayQueue shaping_queue; #if ENABLED(INPUT_SHAPING_X) static ShapeParams shaping_x; #endif @@ -665,7 +656,7 @@ class Stepper { const bool was_on = hal.isr_state(); hal.isr_off(); - const bool result = TERN0(INPUT_SHAPING_X, !shaping_queue.empty_x()) || TERN0(INPUT_SHAPING_Y, !shaping_queue.empty_y()); + const bool result = TERN0(INPUT_SHAPING_X, !ShapingQueue::empty_x()) || TERN0(INPUT_SHAPING_Y, !ShapingQueue::empty_y()); if (was_on) hal.isr_on(); From badaab2489693ea9b94b54f03d66c956b121fce9 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sun, 20 Nov 2022 18:53:33 +0000 Subject: [PATCH 26/33] Alternative method of preventing buffer overflows: reduce shaping delay at high speed --- Marlin/Configuration_adv.h | 4 +-- Marlin/src/module/planner.cpp | 15 ---------- Marlin/src/module/stepper.cpp | 55 +++++++++++++++++++++-------------- 3 files changed, 35 insertions(+), 39 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 61ba1180db10..8e98b4b90d7a 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1068,8 +1068,8 @@ * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE. Disabling * ADAPTIVE_STEP_SMOOTHING and reducing the step rate for non-shaped axes may * also reduce the buffer sizes. Runtime editing of max feedrate (M203) or - * resonant frequency (M593) may result feedrate being capped to prevent - * buffer overruns. + * resonant frequency (M593) may result in input shaping losing effectiveness + * during high speed movements to prevent buffer overruns. * * Tune with M593 D F: * diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index af8d5e37375c..86321601817d 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -2490,21 +2490,6 @@ bool Planner::_populate_block( #endif // XY_FREQUENCY_LIMIT - #if HAS_SHAPING - float bottom_freq = float(0x7FFFFFFFL), t; - TERN_(INPUT_SHAPING_X, if ((t = stepper.get_shaping_frequency(X_AXIS))) NOMORE(bottom_freq, t)); - TERN_(INPUT_SHAPING_Y, if ((t = stepper.get_shaping_frequency(Y_AXIS))) NOMORE(bottom_freq, t)); - const float max_echo_rate = bottom_freq * (float(shaping_echoes - 3) * 2.0f); - const float rate_sum = (TERN0(HAS_SHAPING_X, block->steps.x) + TERN0(HAS_SHAPING_Y, block->steps.y)) * inverse_secs; - #if ENABLED(ADAPTIVE_STEP_SMOOTHING) - const float limiting_rate = rate_sum; - #else - const float limiting_rate = _MIN(float(block->nominal_rate), rate_sum); - #endif - const float max_factor = limiting_rate ? max_echo_rate / limiting_rate : 1.0f; - NOMORE(speed_factor, max_factor); - #endif - // Correct the speed if (speed_factor < 1.0f) { current_speed *= speed_factor; diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index df119486b248..b23f5ffd1c41 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1485,14 +1485,6 @@ void Stepper::isr() { // Enable ISRs to reduce USART processing latency hal.isr_on(); - #if HAS_SHAPING - // Speed limiting should ensure the buffers never get full. But if somehow they do, stutter rather than overflow. - if (!nextMainISR) { - TERN_(INPUT_SHAPING_X, if (ShapingQueue::free_count_x() < steps_per_isr) NOLESS(nextMainISR, ShapingQueue::peek_x() + 1)); - TERN_(INPUT_SHAPING_Y, if (ShapingQueue::free_count_y() < steps_per_isr) NOLESS(nextMainISR, ShapingQueue::peek_y() + 1)); - } - #endif - if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses TERN_(HAS_SHAPING, shaping_isr()); // Do Shaper stepping, if needed @@ -1999,24 +1991,46 @@ void Stepper::pulse_phase_isr() { #if HAS_SHAPING void Stepper::shaping_isr() { - xy_bool_t step_needed{0}; + // Work out the total movement to clear the echoes that are ready to process. + // If the buffers are too full and risk overflowing, also apply echoes early. + #if ENABLED(INPUT_SHAPING_X) + int16_t x_offset = 0; + uint16_t x_free_count = ShapingQueue::free_count_x(); + while (!ShapingQueue::peek_x() || x_free_count < steps_per_isr) { + bool forward = ShapingQueue::dequeue_x(); + x_free_count++; + x_offset += shaping_x.factor2 * (forward ? 1 : -1); + } + #endif + #if ENABLED(INPUT_SHAPING_Y) + int16_t y_offset = 0; + uint16_t y_free_count = ShapingQueue::free_count_y(); + while (!ShapingQueue::peek_y() || y_free_count < steps_per_isr) { + bool forward = ShapingQueue::dequeue_y(); + y_free_count++; + y_offset += shaping_y.factor2 * (forward ? 1 : -1); + } + #endif - step_needed[X_AXIS] = TERN0(INPUT_SHAPING_X, !ShapingQueue::peek_x()); - step_needed[Y_AXIS] = TERN0(INPUT_SHAPING_Y, !ShapingQueue::peek_y()); + if (x_offset || y_offset) while (true) { + xy_bool_t step_needed{0}; - if (bool(step_needed)) while (true) { #if ENABLED(INPUT_SHAPING_X) - if (step_needed[X_AXIS]) { - bool forward = ShapingQueue::dequeue_x(); - PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.factor2 * (forward ? 1 : -1)); + if (x_offset) { + step_needed[X_AXIS] = true; + int16_t dividend = int16_t(shaping_x.factor2) * (x_offset > 0 ? 1 : -1); + x_offset -= dividend; + PULSE_PREP_SHAPING(X, shaping_x.delta_error, dividend); PULSE_START(X); } #endif #if ENABLED(INPUT_SHAPING_Y) - if (step_needed[Y_AXIS]) { - bool forward = ShapingQueue::dequeue_y(); - PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.factor2 * (forward ? 1 : -1)); + if (y_offset) { + step_needed[Y_AXIS] = true; + int16_t dividend = int16_t(shaping_y.factor2) * (y_offset > 0 ? 1 : -1); + y_offset -= dividend; + PULSE_PREP_SHAPING(Y, shaping_y.delta_error, dividend); PULSE_START(Y); } #endif @@ -2037,10 +2051,7 @@ void Stepper::pulse_phase_isr() { #endif } - step_needed[X_AXIS] = TERN0(INPUT_SHAPING_X, !ShapingQueue::peek_x()); - step_needed[Y_AXIS] = TERN0(INPUT_SHAPING_Y, !ShapingQueue::peek_y()); - - if (!bool(step_needed)) break; + if (!x_offset && !x_offset) break; START_TIMED_PULSE(); AWAIT_LOW_PULSE(); From cca1b8e0d893000f5243b451ddb54b05d1194783 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sun, 20 Nov 2022 20:57:30 +0000 Subject: [PATCH 27/33] Optimised free_count_[xy]() functions and updated cycle calculations --- Marlin/src/module/stepper.cpp | 2 ++ Marlin/src/module/stepper.h | 32 +++++++++++++++++++------------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index b23f5ffd1c41..25e82928f1ee 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -242,12 +242,14 @@ uint32_t Stepper::advance_divisor = 0, shaping_time_t ShapingQueue::delay_x; shaping_time_t ShapingQueue::peek_x_val = shaping_time_t(-1); uint16_t ShapingQueue::head_x = 0; + uint16_t ShapingQueue::_free_count_x = shaping_echoes - 1; ShapeParams Stepper::shaping_x; #endif #if ENABLED(INPUT_SHAPING_Y) shaping_time_t ShapingQueue::delay_y; shaping_time_t ShapingQueue::peek_y_val = shaping_time_t(-1); uint16_t ShapingQueue::head_y = 0; + uint16_t ShapingQueue::_free_count_y = shaping_echoes - 1; ShapeParams Stepper::shaping_y; #endif #endif diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 552f1f9243d0..60783882694b 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -76,7 +76,7 @@ #define TIMER_READ_ADD_AND_STORE_CYCLES 34UL // The base ISR - #define ISR_BASE_CYCLES 1000UL + #define ISR_BASE_CYCLES 770UL // Linear advance base time is 64 cycles #if ENABLED(LIN_ADVANCE) @@ -94,7 +94,7 @@ // Input shaping base time #if HAS_SHAPING - #define ISR_SHAPING_BASE_CYCLES 250UL + #define ISR_SHAPING_BASE_CYCLES 220UL #else #define ISR_SHAPING_BASE_CYCLES 0UL #endif @@ -109,8 +109,8 @@ // Cycles to perform actions in START_TIMED_PULSE #define TIMER_READ_ADD_AND_STORE_CYCLES 13UL - // The base ISR takes 752 cycles - #define ISR_BASE_CYCLES 752UL + // The base ISR + #define ISR_BASE_CYCLES 1000UL // Linear advance base time is 32 cycles #if ENABLED(LIN_ADVANCE) @@ -128,7 +128,7 @@ // Input shaping base time #if HAS_SHAPING - #define ISR_SHAPING_BASE_CYCLES 250UL + #define ISR_SHAPING_BASE_CYCLES 400UL #else #define ISR_SHAPING_BASE_CYCLES 0UL #endif @@ -378,11 +378,13 @@ constexpr ena_mask_t enable_overlap[] = { static shaping_time_t delay_x; // = shaping_time_t(-1) to disable queueing static shaping_time_t peek_x_val; static uint16_t head_x; + static uint16_t _free_count_x; #endif #if ENABLED(INPUT_SHAPING_Y) static shaping_time_t delay_y; // = shaping_time_t(-1) to disable queueing static shaping_time_t peek_y_val; static uint16_t head_y; + static uint16_t _free_count_y; #endif public: @@ -402,6 +404,8 @@ constexpr ena_mask_t enable_overlap[] = { TERN_(INPUT_SHAPING_X, echo_axes[tail].x = x_step ? (x_forward ? ECHO_FWD : ECHO_BWD) : ECHO_NONE); TERN_(INPUT_SHAPING_Y, echo_axes[tail].y = y_step ? (y_forward ? ECHO_FWD : ECHO_BWD) : ECHO_NONE); if (++tail == shaping_echoes) tail = 0; + TERN_(INPUT_SHAPING_X, _free_count_x--); + TERN_(INPUT_SHAPING_Y, _free_count_y--); TERN_(INPUT_SHAPING_X, if (echo_axes[head_x].x == ECHO_NONE) dequeue_x()); TERN_(INPUT_SHAPING_Y, if (echo_axes[head_y].y == ECHO_NONE) dequeue_y()); } @@ -409,35 +413,37 @@ constexpr ena_mask_t enable_overlap[] = { static shaping_time_t peek_x() { return peek_x_val; } static bool dequeue_x() { bool forward = echo_axes[head_x].x == ECHO_FWD; - do + do { + _free_count_x++; if (++head_x == shaping_echoes) head_x = 0; - while (head_x != tail && echo_axes[head_x].x == ECHO_NONE); + } while (head_x != tail && echo_axes[head_x].x == ECHO_NONE); peek_x_val = head_x == tail ? shaping_time_t(-1) : times[head_x] + delay_x - now; return forward; } static bool empty_x() { return head_x == tail; } - static uint16_t free_count_x() { return (head_x > tail ? 0 : shaping_echoes) + (head_x - tail - 1); } + static uint16_t free_count_x() { return _free_count_x; } #endif #if ENABLED(INPUT_SHAPING_Y) static shaping_time_t peek_y() { return peek_y_val; } static bool dequeue_y() { bool forward = echo_axes[head_y].y == ECHO_FWD; - do + do { + _free_count_y++; if (++head_y == shaping_echoes) head_y = 0; - while (head_y != tail && echo_axes[head_y].y == ECHO_NONE); + } while (head_y != tail && echo_axes[head_y].y == ECHO_NONE); peek_y_val = head_y == tail ? shaping_time_t(-1) : times[head_y] + delay_y - now; return forward; } static bool empty_y() { return head_y == tail; } - static uint16_t free_count_y() { return (head_y > tail ? 0 : shaping_echoes) + (head_y - tail - 1); } + static uint16_t free_count_y() { return _free_count_y; } #endif static void purge() { const auto st = shaping_time_t(-1); #if ENABLED(INPUT_SHAPING_X) - head_x = tail; peek_x_val = st; + head_x = tail; _free_count_x = shaping_echoes - 1; peek_x_val = st; #endif #if ENABLED(INPUT_SHAPING_Y) - head_y = tail; peek_y_val = st; + head_y = tail; _free_count_y = shaping_echoes - 1; peek_y_val = st; #endif } }; From d27390993ba0f3230b600e20bc8d0b7f80adb8e3 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Sun, 20 Nov 2022 21:21:14 +0000 Subject: [PATCH 28/33] More realistic default zetas --- Marlin/Configuration_adv.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 8e98b4b90d7a..4e72dafc593d 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1084,12 +1084,12 @@ #if EITHER(INPUT_SHAPING_X, INPUT_SHAPING_Y) #if ENABLED(INPUT_SHAPING_X) #define SHAPING_FREQ_X 40 // (Hz) The default dominant resonant frequency on the X axis. - #define SHAPING_ZETA_X 0.3f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). + #define SHAPING_ZETA_X 0.15f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). //#define SHAPING_MAX_STEPRATE_JUMP_X 10000 // (Hz) Set with TMC2208 or TMC2225 to prevent triggering an over-current condition. #endif #if ENABLED(INPUT_SHAPING_Y) #define SHAPING_FREQ_Y 40 // (Hz) The default dominant resonant frequency on the Y axis. - #define SHAPING_ZETA_Y 0.3f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). + #define SHAPING_ZETA_Y 0.15f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). //#define SHAPING_MAX_STEPRATE_JUMP_Y 10000 // (Hz) Set with TMC2208 or TMC2225 to prevent triggering an over-current condition. #endif //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. From 8fe8a4fe4a62869474a537a02931e6a3624f235b Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 21 Nov 2022 11:45:43 +0000 Subject: [PATCH 29/33] shaping_isr() must now happen before pulse_phase_isr() to prevent buffer overflows --- Marlin/src/module/stepper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 25e82928f1ee..0373a1e2079e 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1487,10 +1487,10 @@ void Stepper::isr() { // Enable ISRs to reduce USART processing latency hal.isr_on(); - if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses - TERN_(HAS_SHAPING, shaping_isr()); // Do Shaper stepping, if needed + if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses + #if ENABLED(LIN_ADVANCE) if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses advance_isr(); From 52664a0029f13905bf51ecf06f991365ab4d3508 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 21 Nov 2022 09:11:19 +0000 Subject: [PATCH 30/33] Simpler, cleaner alternative buffer overflow logic --- Marlin/src/module/stepper.cpp | 48 +++++++++++------------------------ 1 file changed, 15 insertions(+), 33 deletions(-) diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 0373a1e2079e..03983320246b 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1993,46 +1993,25 @@ void Stepper::pulse_phase_isr() { #if HAS_SHAPING void Stepper::shaping_isr() { - // Work out the total movement to clear the echoes that are ready to process. - // If the buffers are too full and risk overflowing, also apply echoes early. - #if ENABLED(INPUT_SHAPING_X) - int16_t x_offset = 0; - uint16_t x_free_count = ShapingQueue::free_count_x(); - while (!ShapingQueue::peek_x() || x_free_count < steps_per_isr) { - bool forward = ShapingQueue::dequeue_x(); - x_free_count++; - x_offset += shaping_x.factor2 * (forward ? 1 : -1); - } - #endif - #if ENABLED(INPUT_SHAPING_Y) - int16_t y_offset = 0; - uint16_t y_free_count = ShapingQueue::free_count_y(); - while (!ShapingQueue::peek_y() || y_free_count < steps_per_isr) { - bool forward = ShapingQueue::dequeue_y(); - y_free_count++; - y_offset += shaping_y.factor2 * (forward ? 1 : -1); - } - #endif + xy_bool_t step_needed{0}; - if (x_offset || y_offset) while (true) { - xy_bool_t step_needed{0}; + // Clear the echoes that are ready to process. If the buffers are too full and risk overflo, also apply echoes early. + TERN_(INPUT_SHAPING_X, step_needed[X_AXIS] = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr); + TERN_(INPUT_SHAPING_Y, step_needed[Y_AXIS] = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr); + if (bool(step_needed)) while (true) { #if ENABLED(INPUT_SHAPING_X) - if (x_offset) { - step_needed[X_AXIS] = true; - int16_t dividend = int16_t(shaping_x.factor2) * (x_offset > 0 ? 1 : -1); - x_offset -= dividend; - PULSE_PREP_SHAPING(X, shaping_x.delta_error, dividend); + if (step_needed[X_AXIS]) { + bool forward = ShapingQueue::dequeue_x(); + PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.factor2 * (forward ? 1 : -1)); PULSE_START(X); } #endif #if ENABLED(INPUT_SHAPING_Y) - if (y_offset) { - step_needed[Y_AXIS] = true; - int16_t dividend = int16_t(shaping_y.factor2) * (y_offset > 0 ? 1 : -1); - y_offset -= dividend; - PULSE_PREP_SHAPING(Y, shaping_y.delta_error, dividend); + if (step_needed[Y_AXIS]) { + bool forward = ShapingQueue::dequeue_y(); + PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.factor2 * (forward ? 1 : -1)); PULSE_START(Y); } #endif @@ -2053,7 +2032,10 @@ void Stepper::pulse_phase_isr() { #endif } - if (!x_offset && !x_offset) break; + TERN_(INPUT_SHAPING_X, step_needed[X_AXIS] = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr); + TERN_(INPUT_SHAPING_Y, step_needed[Y_AXIS] = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr); + + if (!bool(step_needed)) break; START_TIMED_PULSE(); AWAIT_LOW_PULSE(); From 35725a79218be9a4ebbc34579173b54e250a7ed3 Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Mon, 21 Nov 2022 13:01:43 +0000 Subject: [PATCH 31/33] Updated timings --- Marlin/src/module/stepper.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 60783882694b..f29bb346d1e3 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -94,7 +94,7 @@ // Input shaping base time #if HAS_SHAPING - #define ISR_SHAPING_BASE_CYCLES 220UL + #define ISR_SHAPING_BASE_CYCLES 180UL #else #define ISR_SHAPING_BASE_CYCLES 0UL #endif @@ -128,7 +128,7 @@ // Input shaping base time #if HAS_SHAPING - #define ISR_SHAPING_BASE_CYCLES 400UL + #define ISR_SHAPING_BASE_CYCLES 290UL #else #define ISR_SHAPING_BASE_CYCLES 0UL #endif From 583f249118cf71cd8ee095b19fcdee80751f707c Mon Sep 17 00:00:00 2001 From: Tom Brazier Date: Wed, 23 Nov 2022 08:57:58 +0000 Subject: [PATCH 32/33] Prevent TMC2208/TMC2225 bug using hysteresis --- Marlin/Configuration_adv.h | 2 -- Marlin/src/module/stepper.cpp | 30 ++++++++++++++++-------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 4e72dafc593d..1fc200cecd06 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1085,12 +1085,10 @@ #if ENABLED(INPUT_SHAPING_X) #define SHAPING_FREQ_X 40 // (Hz) The default dominant resonant frequency on the X axis. #define SHAPING_ZETA_X 0.15f // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping). - //#define SHAPING_MAX_STEPRATE_JUMP_X 10000 // (Hz) Set with TMC2208 or TMC2225 to prevent triggering an over-current condition. #endif #if ENABLED(INPUT_SHAPING_Y) #define SHAPING_FREQ_Y 40 // (Hz) The default dominant resonant frequency on the Y axis. #define SHAPING_ZETA_Y 0.15f // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping). - //#define SHAPING_MAX_STEPRATE_JUMP_Y 10000 // (Hz) Set with TMC2208 or TMC2225 to prevent triggering an over-current condition. #endif //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. #endif diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 03983320246b..7af8f6ba410a 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1679,33 +1679,35 @@ void Stepper::pulse_phase_isr() { delta_error[_AXIS(AXIS)] -= advance_divisor; \ }while(0) - // Direction changes can happen with almost no time gap from a previous STEP pulse. Wait for - // the time given by MAXIMUM_STEPPER_RATE. Or, to work around the TMC2208 / TMC2225 over-current - // shutdown bug, wait for the time given by SHAPING_MAX_STEPRATE_JUMP_[XY]. - #ifdef SHAPING_MAX_STEPRATE_JUMP_X - #define AWAIT_DIRCHANGE_X while (((STEPPER_TIMER_RATE) / (SHAPING_MAX_STEPRATE_JUMP_X)) > HAL_timer_get_count(MF_TIMER_PULSE) - start_pulse_count) {} + // With input shaping, direction changes can happen with almost only + // AWAIT_LOW_PULSE() and DIR_WAIT_BEFORE() between steps. To work around + // the TMC2208 / TMC2225 shutdown bug (#16076), add a half step hysteresis + // in each direction. This results in the position being off by half an + // average half step during travel but correct at the end of each segment. + #if AXIS_DRIVER_TYPE_X(TMC2208) || AXIS_DRIVER_TYPE_X(TMC2208_STANDALONE) + #define HYSTERESIS_X 64 #else - #define AWAIT_DIRCHANGE_X AWAIT_LOW_PULSE() + #define HYSTERESIS_X 0 #endif - #ifdef SHAPING_MAX_STEPRATE_JUMP_Y - #define AWAIT_DIRCHANGE_Y while (((STEPPER_TIMER_RATE) / (SHAPING_MAX_STEPRATE_JUMP_Y)) > HAL_timer_get_count(MF_TIMER_PULSE) - start_pulse_count) {} + #if AXIS_DRIVER_TYPE_Y(TMC2208) || AXIS_DRIVER_TYPE_Y(TMC2208_STANDALONE) + #define HYSTERESIS_Y 64 #else - #define AWAIT_DIRCHANGE_Y AWAIT_LOW_PULSE() + #define HYSTERESIS_Y 0 #endif - #define _AWAIT_DIRCHANGE(AXIS) AWAIT_DIRCHANGE_##AXIS - #define AWAIT_DIRCHANGE(AXIS) _AWAIT_DIRCHANGE(AXIS) + #define _HYSTERESIS(AXIS) HYSTERESIS_##AXIS + #define HYSTERESIS(AXIS) _HYSTERESIS(AXIS) #define PULSE_PREP_SHAPING(AXIS, DELTA_ERROR, DIVIDEND) do{ \ if (step_needed[_AXIS(AXIS)]) { \ DELTA_ERROR += (DIVIDEND); \ - if ((MAXDIR(AXIS) && DELTA_ERROR <= -64) || (MINDIR(AXIS) && DELTA_ERROR >= 64)) { \ - { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_DIRCHANGE(AXIS); } \ + if ((MAXDIR(AXIS) && DELTA_ERROR <= -(64 + HYSTERESIS(AXIS))) || (MINDIR(AXIS) && DELTA_ERROR >= (64 + HYSTERESIS(AXIS)))) { \ + { USING_TIMED_PULSE(); START_TIMED_PULSE(); AWAIT_LOW_PULSE(); } \ TBI(last_direction_bits, _AXIS(AXIS)); \ DIR_WAIT_BEFORE(); \ SET_STEP_DIR(AXIS); \ DIR_WAIT_AFTER(); \ } \ - step_needed[_AXIS(AXIS)] = DELTA_ERROR >= 64 || DELTA_ERROR <= -64; \ + step_needed[_AXIS(AXIS)] = DELTA_ERROR <= -(64 + HYSTERESIS(AXIS)) || DELTA_ERROR >= (64 + HYSTERESIS(AXIS)); \ if (step_needed[_AXIS(AXIS)]) \ DELTA_ERROR += MAXDIR(AXIS) ? -128 : 128; \ } \ From f2c06afa72850c3dfaa5a905d75a734613b1483d Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 27 Nov 2022 00:46:34 -0600 Subject: [PATCH 33/33] cleanup --- Marlin/Configuration_adv.h | 6 +-- Marlin/src/module/stepper.cpp | 95 ++++++++++++++++++----------------- 2 files changed, 53 insertions(+), 48 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 6e7ff2fc3f6b..c168240e243d 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1062,9 +1062,9 @@ * * Zero Vibration (ZV) Input Shaping for X and/or Y movements. * - * This option uses a lot of SRAM for the step buffer, which is related - * to the largest step rate possible for the shaped axes. If the build fails - * due to low SRAM the buffer size may be reduced by setting smaller values for + * This option uses a lot of SRAM for the step buffer, which is related to the + * largest step rate possible for the shaped axes. If the build fails due to + * low SRAM the buffer size may be reduced by setting smaller values for * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE. Disabling * ADAPTIVE_STEP_SMOOTHING and reducing the step rate for non-shaped axes may * also reduce the buffer sizes. Runtime editing of max feedrate (M203) or diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 7af8f6ba410a..74e761dc6478 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1535,18 +1535,9 @@ void Stepper::isr() { // nextMainISR -= interval; - - #if HAS_SHAPING - ShapingQueue::decrement_delays(interval); - #endif - - #if ENABLED(LIN_ADVANCE) - if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval; - #endif - - #if ENABLED(INTEGRATED_BABYSTEPPING) - if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval; - #endif + TERN_(HAS_SHAPING, ShapingQueue::decrement_delays(interval)); + TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval); + TERN_(INTEGRATED_BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval); /** * This needs to avoid a race-condition caused by interleaving @@ -1636,10 +1627,14 @@ void Stepper::pulse_phase_isr() { discard_current_block(); #if HAS_SHAPING ShapingQueue::purge(); - TERN_(INPUT_SHAPING_X, shaping_x.delta_error = 0); - TERN_(INPUT_SHAPING_Y, shaping_y.delta_error = 0); - TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); - TERN_(INPUT_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); + #if ENABLED(INPUT_SHAPING_X) + shaping_x.delta_error = 0; + shaping_x.last_block_end_pos = count_position.x; + #endif + #if ENABLED(INPUT_SHAPING_Y) + shaping_y.delta_error = 0; + shaping_y.last_block_end_pos = count_position.y; + #endif #endif } } @@ -2004,7 +1999,7 @@ void Stepper::pulse_phase_isr() { if (bool(step_needed)) while (true) { #if ENABLED(INPUT_SHAPING_X) if (step_needed[X_AXIS]) { - bool forward = ShapingQueue::dequeue_x(); + const bool forward = ShapingQueue::dequeue_x(); PULSE_PREP_SHAPING(X, shaping_x.delta_error, shaping_x.factor2 * (forward ? 1 : -1)); PULSE_START(X); } @@ -2012,7 +2007,7 @@ void Stepper::pulse_phase_isr() { #if ENABLED(INPUT_SHAPING_Y) if (step_needed[Y_AXIS]) { - bool forward = ShapingQueue::dequeue_y(); + const bool forward = ShapingQueue::dequeue_y(); PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.factor2 * (forward ? 1 : -1)); PULSE_START(Y); } @@ -2034,8 +2029,8 @@ void Stepper::pulse_phase_isr() { #endif } - TERN_(INPUT_SHAPING_X, step_needed[X_AXIS] = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr); - TERN_(INPUT_SHAPING_Y, step_needed[Y_AXIS] = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr); + TERN_(INPUT_SHAPING_X, step_needed[X_AXIS] = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr); + TERN_(INPUT_SHAPING_Y, step_needed[Y_AXIS] = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr); if (!bool(step_needed)) break; @@ -2982,6 +2977,7 @@ void Stepper::init() { } #if HAS_SHAPING + /** * Calculate a fixed point factor to apply to the signal and its echo * when shaping an axis. @@ -3023,20 +3019,24 @@ void Stepper::init() { hal.isr_off(); const shaping_time_t delay = freq ? float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq : shaping_time_t(-1); - if (TERN0(INPUT_SHAPING_X, axis == X_AXIS)) { - ShapingQueue::set_delay(X_AXIS, delay); - TERN_(INPUT_SHAPING_X, shaping_x.frequency = freq); - TERN_(INPUT_SHAPING_X, shaping_x.enabled = !!freq); - TERN_(INPUT_SHAPING_X, shaping_x.delta_error = 0); - TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = count_position.x); - } - if (TERN0(INPUT_SHAPING_Y, axis == Y_AXIS)) { - ShapingQueue::set_delay(Y_AXIS, delay); - TERN_(INPUT_SHAPING_Y, shaping_y.frequency = freq); - TERN_(INPUT_SHAPING_Y, shaping_y.enabled = !!freq); - TERN_(INPUT_SHAPING_Y, shaping_y.delta_error = 0); - TERN_(INPUT_SHAPING_Y, shaping_y.last_block_end_pos = count_position.y); - } + #if ENABLED(INPUT_SHAPING_X) + if (axis == X_AXIS) { + ShapingQueue::set_delay(X_AXIS, delay); + shaping_x.frequency = freq; + shaping_x.enabled = !!freq; + shaping_x.delta_error = 0; + shaping_x.last_block_end_pos = count_position.x; + } + #endif + #if ENABLED(INPUT_SHAPING_Y) + if (axis == Y_AXIS) { + ShapingQueue::set_delay(Y_AXIS, delay); + shaping_y.frequency = freq; + shaping_y.enabled = !!freq; + shaping_y.delta_error = 0; + shaping_y.last_block_end_pos = count_position.y; + } + #endif if (was_on) hal.isr_on(); } @@ -3046,7 +3046,8 @@ void Stepper::init() { TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency); return -1; } -#endif + +#endif // HAS_SHAPING /** * Set the stepper positions directly in steps @@ -3059,10 +3060,10 @@ void Stepper::init() { */ void Stepper::_set_position(const abce_long_t &spos) { #if ENABLED(INPUT_SHAPING_X) - int32_t x_shaping_delta = count_position.x - shaping_x.last_block_end_pos; + const int32_t x_shaping_delta = count_position.x - shaping_x.last_block_end_pos; #endif #if ENABLED(INPUT_SHAPING_Y) - int32_t y_shaping_delta = count_position.y - shaping_y.last_block_end_pos; + const int32_t y_shaping_delta = count_position.y - shaping_y.last_block_end_pos; #endif #if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX) @@ -3095,14 +3096,18 @@ void Stepper::_set_position(const abce_long_t &spos) { count_position = spos; #endif - if (TERN0(INPUT_SHAPING_X, shaping_x.enabled)) { - TERN_(INPUT_SHAPING_X, count_position.x += x_shaping_delta); - TERN_(INPUT_SHAPING_X, shaping_x.last_block_end_pos = spos.x); - } - if (TERN0(INPUT_SHAPING_Y, shaping_y.enabled)) { - TERN_(INPUT_SHAPING_Y, count_position.y += y_shaping_delta); - TERN_(INPUT_SHAPING_Y, shaping_y.last_block_end_pos = spos.y); - } + #if ENABLED(INPUT_SHAPING_X) + if (shaping_x.enabled) { + count_position.x += x_shaping_delta; + shaping_x.last_block_end_pos = spos.x; + } + #endif + #if ENABLED(INPUT_SHAPING_Y) + if (shaping_y.enabled) { + count_position.y += y_shaping_delta; + shaping_y.last_block_end_pos = spos.y; + } + #endif } /**