🩹 Ensure safe change of XYZ smoothing (MarlinFirmware#28247)

Author: dbuezas

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;
   }
 }
 

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

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;

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;
   }
 }

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;

Marlin/src/module/ft_motion/stepping.h

@@ -35,25 +35,24 @@ 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
 //       number == (number_fp >> FTM_Q)
 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;