diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index f6113c398e..f9b7c86998 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -70,7 +70,8 @@ AxisBits FTMotion::moving_axis_flags, // These axes are moving in the // Block data variables. xyze_pos_t FTMotion::startPos, // (mm) Start position of block - FTMotion::endPos_prevBlock = { 0.0f }; // (mm) End position of previous block + FTMotion::endPos_prevBlock = { 0.0f }, // (mm) End position of previous block + FTMotion::last_target_traj = { 0.0f }; // (mm) Last target position after shaping and smoothing xyze_float_t FTMotion::ratio; // (ratio) Axis move ratio of block float FTMotion::tau = 0.0f; // (s) Time since start of block bool FTMotion::fastForwardUntilMotion = false; // Fast forward time if there is no motion @@ -524,13 +525,14 @@ xyze_float_t FTMotion::calc_traj_point(const float dist) { // Approximate Gaussian smoothing via chained EMAs auto _smoothen = [&](const AxisEnum axis, axis_smoothing_t &smoo) { - if (smoo.alpha <= 0.0f) return; - float smooth_val = traj_coords[axis]; - for (uint8_t _i = 0; _i < FTM_SMOOTHING_ORDER; ++_i) { - smoo.smoothing_pass[_i] += (smooth_val - smoo.smoothing_pass[_i]) * smoo.alpha; - smooth_val = smoo.smoothing_pass[_i]; + if (smoo.alpha != 1.0f) { + float smooth_val = traj_coords[axis]; + for (uint8_t _i = 0; _i < FTM_SMOOTHING_ORDER; ++_i) { + smoo.smoothing_pass[_i] += (smooth_val - smoo.smoothing_pass[_i]) * smoo.alpha; + smooth_val = smoo.smoothing_pass[_i]; + } + traj_coords[axis] = smooth_val; } - traj_coords[axis] = smooth_val; }; #define _SMOOTHEN(A) _smoothen(_AXIS(A), smoothing.A); @@ -604,7 +606,7 @@ void FTMotion::fill_stepper_plan_buffer() { // Get distance from trajectory generator xyze_float_t traj_coords = calc_traj_point(currentGenerator->getDistanceAtTime(tau)); - if (fastForwardUntilMotion && traj_coords == startPos) { + if (fastForwardUntilMotion && traj_coords == last_target_traj) { // Axis synchronization delays all axes. When coming from a reset, there is a ramp up time filling all buffers. // If the slowest axis doesn't move and it isn't smoothened, this time can be skipped. // It eliminates idle time when changing smoothing time or shapers and speeds up homing and bed leveling. @@ -614,6 +616,7 @@ void FTMotion::fill_stepper_plan_buffer() { // Calculate and store stepper plan in buffer stepping_enqueue(traj_coords); } + last_target_traj = traj_coords; } } diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 42dab9951d..1d2849f289 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -326,7 +326,8 @@ class FTMotion { private: // Block data variables. static xyze_pos_t startPos, // (mm) Start position of block - endPos_prevBlock; // (mm) End position of previous block + endPos_prevBlock, // (mm) End position of previous block + last_target_traj; // (mm) Last target position after shaping and smoothing static xyze_float_t ratio; // (ratio) Axis move ratio of block static float tau; // (s) Time since start of block static bool fastForwardUntilMotion; // Fast forward time if there is no motion @@ -375,8 +376,19 @@ class FTMotion { // Synchronize and reset motion prior to parameter changes friend void ft_config_t::prep_for_shaper_change(); static void prep_for_shaper_change() { + // planner.synchronize guarantees that motion reached a standstill with no echoes pending execution (including a runout block) planner.synchronize(); - reset(); + // Due to smoothing, the end position may not have been reached exactly. + // This is normally fine, but if smoothing time changes, and we assume it was reached, + // it may cause discontinuities. + // Therefore, set the next starting position to the exact reached position. + endPos_prevBlock = last_target_traj; + // We now know that we are not moving and there are no pending echoes, + // so set all shaping buffers to current position in case the new smoothing/shaping + // parameters force input shaping to look in a past position for echoes. + shaping.fill(endPos_prevBlock); + TERN_(FTM_SMOOTHING, smoothing.fill(endPos_prevBlock)); + fastForwardUntilMotion = true; } // Buffers diff --git a/Marlin/src/module/ft_motion/shaping.h b/Marlin/src/module/ft_motion/shaping.h index 0140a969ba..88b02fbdac 100644 --- a/Marlin/src/module/ft_motion/shaping.h +++ b/Marlin/src/module/ft_motion/shaping.h @@ -151,4 +151,9 @@ typedef struct Shaping { SHAPED_MAP(_RESET_ZI); zi_idx = 0; } + void fill(const xyze_float_t pos) { + #define _FILL_ZI(A) for (uint32_t i = 0; i < ftm_zmax; i++) A.d_zi[i] = pos.A; + SHAPED_MAP(_FILL_ZI); + #undef _FILL_ZI + } } shaping_t; diff --git a/Marlin/src/module/ft_motion/smoothing.cpp b/Marlin/src/module/ft_motion/smoothing.cpp index f40bd828aa..3708629271 100644 --- a/Marlin/src/module/ft_motion/smoothing.cpp +++ b/Marlin/src/module/ft_motion/smoothing.cpp @@ -28,12 +28,12 @@ // Set smoothing time and recalculate alpha and delay. void AxisSmoothing::set_time(const float s_time) { - if (s_time > 0.001f) { - alpha = 1.0f - expf(-(FTM_TS) * (FTM_SMOOTHING_ORDER) / s_time ); + if (s_time >= 0.0001f) { + alpha = 1.0f - expf(-(FTM_TS) * (FTM_SMOOTHING_ORDER) / s_time); delay_samples = s_time * FTM_FS; } else { - alpha = 0.0f; + alpha = 1.0f; delay_samples = 0; } } diff --git a/Marlin/src/module/ft_motion/smoothing.h b/Marlin/src/module/ft_motion/smoothing.h index e3f9962a09..00be0f8892 100644 --- a/Marlin/src/module/ft_motion/smoothing.h +++ b/Marlin/src/module/ft_motion/smoothing.h @@ -55,4 +55,9 @@ typedef struct Smoothing { LOGICAL_AXIS_MAP(_CLEAR); #undef _CLEAR } + void fill(const xyze_float_t pos) { + #define _FILL_SMO(A) for (uint32_t i = 0; i < FTM_SMOOTHING_ORDER; i++) A.smoothing_pass[i] = pos.A; + LOGICAL_AXIS_MAP(_FILL_SMO); + #undef _FILL_SMO + } } smoothing_t; diff --git a/Marlin/src/module/ft_motion/stepping.h b/Marlin/src/module/ft_motion/stepping.h index b7cbe8391b..636945ac06 100644 --- a/Marlin/src/module/ft_motion/stepping.h +++ b/Marlin/src/module/ft_motion/stepping.h @@ -35,18 +35,11 @@ FORCE_INLINE constexpr uint32_t a_times_b_shift_16(const uint32_t a, const uint3 constexpr int CLZ32(const uint32_t v, const int c=0) { return v ? (TEST32(v, 31)) ? c : CLZ32(v << 1, c + 1) : 32; } -#define FTM_NEVER uint32_t(UINT16_MAX) // Reserved number to indicate "no ticks in this frame" (FRAME_TICKS_FP+1 would work too) constexpr uint32_t FRAME_TICKS = STEPPER_TIMER_RATE / FTM_FS; // Timer ticks per frame constexpr uint32_t FTM_Q_INT = 32u - CLZ32(FRAME_TICKS + 1U); // Bits to represent the integer part of the max value (duration of a frame, +1 one for FTM_NEVER). constexpr uint32_t FTM_Q = 16u - FTM_Q_INT; // uint16 interval fractional bits. // Intervals buffer has fixed point numbers with the point on this position -static_assert(FRAME_TICKS < FTM_NEVER, "(STEPPER_TIMER_RATE / FTM_FS) (" STRINGIFY(STEPPER_TIMER_RATE) " / " STRINGIFY(FTM_FS) ") must be < " STRINGIFY(FTM_NEVER) " to fit 16-bit fixed-point numbers."); - -// Sanity check -static_assert(POW(2, 16 - FTM_Q) > FRAME_TICKS, "FRAME_TICKS in Q format should fit in a uint16"); -static_assert(POW(2, 16 - FTM_Q - 1) <= FRAME_TICKS, "A smaller FTM_Q would still alow a FRAME_TICKS in Q format to fit in a uint16"); - // The _FP and _fp suffixes mean the number is in fixed point format with the point at the FTM_Q position. // See: https://en.wikipedia.org/wiki/Fixed-point_arithmetic // e.g., number_fp = number << FTM_Q @@ -54,6 +47,12 @@ static_assert(POW(2, 16 - FTM_Q - 1) <= FRAME_TICKS, "A smaller FTM_Q would stil constexpr uint32_t ONE_FP = 1UL << FTM_Q; // Number 1 in fixed point format constexpr uint32_t FP_FLOOR_MASK = ~(ONE_FP - 1); // Bit mask to do FLOOR in fixed point constexpr uint32_t FRAME_TICKS_FP = FRAME_TICKS << FTM_Q; // Ticks in a frame in fixed point +constexpr uint32_t FTM_NEVER = FRAME_TICKS_FP + 1; // Reserved number to indicate "no ticks in this frame", also max isr wait on empty stepper buffer + +// Sanity check +static_assert(FRAME_TICKS < FTM_NEVER, "(STEPPER_TIMER_RATE / FTM_FS) (" STRINGIFY(STEPPER_TIMER_RATE) " / " STRINGIFY(FTM_FS) ") must be < " STRINGIFY(FTM_NEVER) " to fit 16-bit fixed-point numbers."); +static_assert(POW(2, 16 - FTM_Q) > FRAME_TICKS, "FRAME_TICKS in Q format should fit in a uint16"); +static_assert(POW(2, 16 - FTM_Q - 1) <= FRAME_TICKS, "A smaller FTM_Q would still alow a FRAME_TICKS in Q format to fit in a uint16"); typedef struct stepper_plan { AxisBits dir_bits;