jessikitty/Marlin
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🩹 Ensure safe change of XYZ smoothing (#28247)

Browse Source
This commit is contained in:
David Buezas
2025-12-30 09:40:40 +01:00
committed by GitHub
parent c45354a38c
commit d9ed6f0e5f
6 changed files with 44 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Marlin/src/module/ft_motion.cpp
+11 -8
View File
@@ -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;
}
}
Marlin/src/module/ft_motion.h
+14 -2
View File
@@ -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
Marlin/src/module/ft_motion/shaping.h
+5
View File
@@ -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;
Marlin/src/module/ft_motion/smoothing.cpp
+3 -3
View File
@@ -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;
}
}
Marlin/src/module/ft_motion/smoothing.h
+5
View File
@@ -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;
Marlin/src/module/ft_motion/stepping.h
+6 -7
View File
@@ -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;