From fbee2a2ff7ae02d7c87ff991b615afe6ad59641f Mon Sep 17 00:00:00 2001 From: David Buezas Date: Mon, 9 Jun 2025 20:26:57 +0200 Subject: [PATCH] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20SMOOTH=5FLIN=5FADVANCE=20+?= =?UTF-8?q?=20S=5FCURVE=5FACCELERATION=20(#27827)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/inc/SanityCheck.h | 2 -- Marlin/src/module/planner.h | 2 +- Marlin/src/module/stepper.cpp | 59 +++++++++++++++++++++++++---------- Marlin/src/module/stepper.h | 7 +++++ 4 files changed, 50 insertions(+), 20 deletions(-) diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 98498f4355..f3f4a97dff 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -866,8 +866,6 @@ static_assert(COUNT(arm) == LOGICAL_AXES, "AXIS_RELATIVE_MODES must contain " _L #if ENABLED(SMOOTH_LIN_ADVANCE) #ifndef CPU_32_BIT #error "SMOOTH_LIN_ADVANCE requires a 32-bit CPU." - #elif ENABLED(S_CURVE_ACCELERATION) - #error "SMOOTH_LIN_ADVANCE is not compatible with S_CURVE_ACCELERATION." #elif ENABLED(INPUT_SHAPING_E_SYNC) && NONE(INPUT_SHAPING_X, INPUT_SHAPING_Y) #error "INPUT_SHAPING_E_SYNC requires INPUT_SHAPING_X or INPUT_SHAPING_Y." #endif diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index bda5720919..eb8f2ed17f 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -45,7 +45,7 @@ #if ENABLED(SMOOTH_LIN_ADVANCE) #define SMOOTH_LIN_ADV_EXP_ORDER 5 // Closest to Gaussian smoothing between 3 and 7 - #define SMOOTH_LIN_ADV_INTERVAL (STEPPER_TIMER_RATE / SMOOTH_LIN_ADV_HZ) // Hz + #define SMOOTH_LIN_ADV_INTERVAL (STEPPER_TIMER_RATE / (SMOOTH_LIN_ADV_HZ)) // Hz #endif #include "motion.h" diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index fd3d3bd800..945600e4fb 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1401,7 +1401,7 @@ void Stepper::apply_directions() { return (r2 | (uint16_t(r3) << 8)) | (uint32_t(r4) << 16); } - #else + #else // !__AVR__ // For all the other 32bit CPUs FORCE_INLINE void Stepper::_calc_bezier_curve_coeffs(const int32_t v0, const int32_t v1, const uint32_t av) { @@ -1478,7 +1478,9 @@ void Stepper::apply_directions() { #endif } - #endif + + #endif // !__AVR__ + #endif // S_CURVE_ACCELERATION /** @@ -2697,7 +2699,7 @@ hal_timer_t Stepper::block_phase_isr() { oversampling_factor = 0; // Decide if axis smoothing is possible - if (stepper.adaptive_step_smoothing_enabled) { + if (adaptive_step_smoothing_enabled) { 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 @@ -2902,7 +2904,7 @@ hal_timer_t Stepper::block_phase_isr() { #if ENABLED(INPUT_SHAPING_E_SYNC) - constexpr uint16_t IS_COMPENSATION_BUFFER_SIZE = uint16_t(float(SMOOTH_LIN_ADV_HZ) / float(SHAPING_MIN_FREQ) / 2.0f + 0.5f); + constexpr uint16_t IS_COMPENSATION_BUFFER_SIZE = uint16_t(float(SMOOTH_LIN_ADV_HZ) / (2.0f * (SHAPING_MIN_FREQ)) + 0.5f); typedef struct { xy_long_t buffer[IS_COMPENSATION_BUFFER_SIZE]; @@ -2924,21 +2926,39 @@ hal_timer_t Stepper::block_phase_isr() { DelayBuffer delayBuffer; - xy_long_t smooth_lin_adv_lookback(const shaping_time_t stepper_ticks) { - constexpr uint32_t ADV_TICKS_PER_STEPPER_TICKS_Q30 = (uint64_t(SMOOTH_LIN_ADV_HZ) * _BV32(30)) / STEPPER_TIMER_RATE; - const uint16_t delay_steps = MULT_Q(30, stepper_ticks, ADV_TICKS_PER_STEPPER_TICKS_Q30); + xy_long_t Stepper::smooth_lin_adv_lookback(const shaping_time_t stepper_ticks) { + constexpr uint32_t adv_ticks_per_stepper_ticks_Q30 = (uint64_t(SMOOTH_LIN_ADV_HZ) * _BV32(30)) / (STEPPER_TIMER_RATE); + const uint16_t delay_steps = MULT_Q(30, stepper_ticks, adv_ticks_per_stepper_ticks_Q30); return delayBuffer.past_item(delay_steps); } #endif // INPUT_SHAPING_E_SYNC - int32_t smooth_lin_adv_lookahead(uint32_t stepper_ticks) { + #if ENABLED(S_CURVE_ACCELERATION) + int32_t Stepper::calc_bezier_curve(const int32_t v0, const int32_t v1, const uint32_t av, const uint32_t curr_step) { + int32_t A = bezier_A, B = bezier_B, C = bezier_C; + uint32_t F = bezier_F, AV = bezier_AV; + + _calc_bezier_curve_coeffs(v0, v1, av); + uint32_t rate = _eval_bezier_curve(curr_step); + + bezier_A = A; bezier_B = B; bezier_C = C; bezier_F = F; bezier_AV = AV; + return rate; + } + #endif + + int32_t Stepper::smooth_lin_adv_lookahead(uint32_t stepper_ticks) { for (uint8_t i = 0; block_t *block = planner.get_future_block(i); i++) { if (block->is_sync()) continue; if (stepper_ticks <= block->acceleration_time) { if (!block->use_advance_lead) return 0; - uint32_t rate = STEP_MULTIPLY(stepper_ticks, block->acceleration_rate) + block->initial_rate; - NOMORE(rate, block->nominal_rate); + uint32_t rate; + #if ENABLED(S_CURVE_ACCELERATION) + rate = calc_bezier_curve(block->initial_rate, block->cruise_rate, block->acceleration_time_inverse, stepper_ticks); + #else + rate = STEP_MULTIPLY(stepper_ticks, block->acceleration_rate) + block->initial_rate; + NOMORE(rate, block->nominal_rate); + #endif return MULT_Q(30, rate, block->e_step_ratio_q30); } stepper_ticks -= block->acceleration_time; @@ -2951,13 +2971,18 @@ hal_timer_t Stepper::block_phase_isr() { if (stepper_ticks <= block->deceleration_time) { if (!block->use_advance_lead) return 0; - uint32_t rate = STEP_MULTIPLY(stepper_ticks, block->acceleration_rate); - if (rate < block->cruise_rate) { - rate = block->cruise_rate - rate; - NOLESS(rate, block->final_rate); - } - else - rate = block->final_rate; + uint32_t rate; + #if ENABLED(S_CURVE_ACCELERATION) + rate = calc_bezier_curve(block->cruise_rate, block->final_rate, block->deceleration_time_inverse, stepper_ticks); + #else + rate = STEP_MULTIPLY(stepper_ticks, block->acceleration_rate); + if (rate < block->cruise_rate) { + rate = block->cruise_rate - rate; + NOLESS(rate, block->final_rate); + } + else + rate = block->final_rate; + #endif return MULT_Q(30, rate, block->e_step_ratio_q30); } stepper_ticks -= block->deceleration_time; diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 3083ad2973..330ff9f6b6 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -555,8 +555,15 @@ class Stepper { // The Linear advance ISR phase static void advance_isr(); #if ENABLED(SMOOTH_LIN_ADVANCE) + #if ENABLED(INPUT_SHAPING_E_SYNC) + static xy_long_t smooth_lin_adv_lookback(const shaping_time_t stepper_ticks); + #endif + static int32_t smooth_lin_adv_lookahead(uint32_t stepper_ticks); static void set_la_interval(int32_t step_rate); static hal_timer_t smooth_lin_adv_isr(); + #if ENABLED(S_CURVE_ACCELERATION) + static int32_t calc_bezier_curve(const int32_t v0, const int32_t v1, const uint32_t av, const uint32_t curr_step); + #endif #endif #endif