Fix FT Motion layer shifting

Marlin/src/gcode/feature/ft_motion/M493.cpp

@@ -258,7 +258,7 @@ void GcodeSuite::M493() {
         // TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct.
         if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) {
           fxdTiCtrl.cfg.baseFreq[X_AXIS] = val;
-          flag.update_n = flag.reset_ft = flag.report_h = true;
+          flag.update_n = flag.report_h = true;
         }
         else // Frequency out of range.
           SERIAL_ECHOLNPGM("Invalid [", AS_CHAR('A'), "] frequency value.");
@@ -289,7 +289,7 @@ void GcodeSuite::M493() {
         const float val = parser.value_float();
         if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) {
           fxdTiCtrl.cfg.baseFreq[Y_AXIS] = val;
-          flag.update_n = flag.reset_ft = flag.report_h = true;
+          flag.update_n = flag.report_h = true;
         }
         else // Frequency out of range.
           SERIAL_ECHOLNPGM("Invalid frequency [", AS_CHAR('B'), "] value.");

Marlin/src/module/ft_motion.cpp

@@ -26,6 +26,7 @@
 
 #include "ft_motion.h"
 #include "stepper.h" // Access stepper block queue function and abort status.
+                     // Access planner singleton for settings, busy flag, etc..
 
 FxdTiCtrl fxdTiCtrl;
 
@@ -75,7 +76,7 @@ bool FxdTiCtrl::batchRdy = false;                 // Indicates a batch of the fi
 bool FxdTiCtrl::batchRdyForInterp = false;        // Indicates the batch is done being post processed,
                                                   //  if applicable, and is ready to be converted to step commands.
 bool FxdTiCtrl::runoutEna = false;                // True if runout of the block hasn't been done and is allowed.
-bool FxdTiCtrl::runout = false;                   // Indicates if runout is in progress.
+bool FxdTiCtrl::blockDataIsRunout = false;        // Indicates the last loaded block variables are for a runout.
 
 // Trapezoid data variables.
 xyze_pos_t   FxdTiCtrl::startPosn,                    // (mm) Start position of block
@@ -142,31 +143,22 @@ void FxdTiCtrl::startBlockProc(block_t * const current_block) {
 
 // Moves any free data points to the stepper buffer even if a full batch isn't ready.
 void FxdTiCtrl::runoutBlock() {
+  if (!runoutEna) return;
 
-  if (runoutEna && !batchRdy) {   // If the window is full already (block intervals was a multiple of
-                                  // the batch size), or runout is not enabled, no runout is needed.
-    // Fill out the trajectory window with the last position calculated.
-    if (makeVector_batchIdx > last_batchIdx)
-      for (uint32_t i = makeVector_batchIdx; i < (FTM_WINDOW_SIZE); i++) {
-        LOGICAL_AXIS_CODE(
-          traj.e[i] = traj.e[makeVector_batchIdx - 1],
-          traj.x[i] = traj.x[makeVector_batchIdx - 1],
-          traj.y[i] = traj.y[makeVector_batchIdx - 1],
-          traj.z[i] = traj.z[makeVector_batchIdx - 1],
-          traj.i[i] = traj.i[makeVector_batchIdx - 1],
-          traj.j[i] = traj.j[makeVector_batchIdx - 1],
-          traj.k[i] = traj.k[makeVector_batchIdx - 1],
-          traj.u[i] = traj.u[makeVector_batchIdx - 1],
-          traj.v[i] = traj.v[makeVector_batchIdx - 1],
-          traj.w[i] = traj.w[makeVector_batchIdx - 1]
-        );
-      }
+  startPosn = endPosn_prevBlock;
+  ratio.reset();
 
-    makeVector_batchIdx = last_batchIdx;
-    batchRdy = true;
-    runout = true;
-  }
-  runoutEna = false;
+  accel_P = decel_P = 0.0f;
+
+  static constexpr uint32_t shaper_intervals = (FTM_BATCH_SIZE) * ceil((FTM_ZMAX) / (FTM_BATCH_SIZE)),
+                            min_max_intervals = (FTM_BATCH_SIZE) * ceil((FTM_WINDOW_SIZE) / (FTM_BATCH_SIZE));
+
+  max_intervals = cfg.modeHasShaper() ? shaper_intervals : 0;
+  if (max_intervals <= min_max_intervals - (FTM_BATCH_SIZE)) max_intervals = min_max_intervals;
+  max_intervals += (FTM_WINDOW_SIZE) - makeVector_batchIdx;
+
+  blockProcRdy = blockDataIsRunout = true;
+  runoutEna = blockProcDn = false;
 }
 
 // Controller main, to be invoked from non-isr task.
@@ -188,35 +180,17 @@ void FxdTiCtrl::loop() {
   }
 
   // Planner processing and block conversion.
-  if (!blockProcRdy && !runout) stepper.fxdTiCtrl_BlockQueueUpdate();
+  if (!blockProcRdy) stepper.fxdTiCtrl_BlockQueueUpdate();
 
   if (blockProcRdy) {
-    if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot.
+    if (!blockProcRdy_z1) { // One-shot.
+      if (!blockDataIsRunout) loadBlockData(current_block_cpy);
+      else blockDataIsRunout = false;
+    }
     while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP)))
       makeVector();
   }
 
-  if (runout && !batchRdy) { // The lower half of the window has been runout.
-    // Runout the upper half of the window: the upper half has been shifted into the lower
-    // half. Fill out the upper half so another batch can be processed.
-    for (uint32_t i = last_batchIdx; i < (FTM_WINDOW_SIZE) - 1; i++) {
-      LOGICAL_AXIS_CODE(
-        traj.e[i] = traj.e[(FTM_WINDOW_SIZE) - 1],
-        traj.x[i] = traj.x[(FTM_WINDOW_SIZE) - 1],
-        traj.y[i] = traj.y[(FTM_WINDOW_SIZE) - 1],
-        traj.z[i] = traj.z[(FTM_WINDOW_SIZE) - 1],
-        traj.i[i] = traj.i[(FTM_WINDOW_SIZE) - 1],
-        traj.j[i] = traj.j[(FTM_WINDOW_SIZE) - 1],
-        traj.k[i] = traj.k[(FTM_WINDOW_SIZE) - 1],
-        traj.u[i] = traj.u[(FTM_WINDOW_SIZE) - 1],
-        traj.v[i] = traj.v[(FTM_WINDOW_SIZE) - 1],
-        traj.w[i] = traj.w[(FTM_WINDOW_SIZE) - 1]
-      );
-    }
-    batchRdy = true;
-    runout = false;
-  }
-
   // FBS / post processing.
   if (batchRdy && !batchRdyForInterp) {
 
@@ -401,7 +375,6 @@ void FxdTiCtrl::reset() {
   blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
   batchRdy = batchRdyForInterp = false;
   runoutEna = false;
-  runout = false;
 
   endPosn_prevBlock.reset();
 
@@ -530,7 +503,7 @@ void FxdTiCtrl::loadBlockData(block_t * const current_block) {
   s_2e = s_1e + F_P * T2_P;
 
   // One less than (Accel + Coasting + Decel) datapoints
-  max_intervals = N1 + N2 + N3 - 1U;
+  max_intervals = N1 + N2 + N3;
 
   endPosn_prevBlock += moveDist;
 }
@@ -643,13 +616,11 @@ void FxdTiCtrl::makeVector() {
     batchRdy = true;
   }
 
-  if (makeVector_idx == max_intervals) {
+  if (++makeVector_idx == max_intervals) {
     blockProcDn = true;
     blockProcRdy = false;
     makeVector_idx = 0;
   }
-  else
-    makeVector_idx++;
 }
 
 // Interpolates single data point to stepper commands.

Marlin/src/module/ft_motion.h

@@ -133,7 +133,7 @@ class FxdTiCtrl {
     static bool blockProcRdy, blockProcRdy_z1, blockProcDn;
     static bool batchRdy, batchRdyForInterp;
     static bool runoutEna;
-    static bool runout;
+    static bool blockDataIsRunout;
 
     // Trapezoid data variables.
     static xyze_pos_t   startPosn,          // (mm) Start position of block