🐛 Fix FT Motion runout in progress (MarlinFirmware#26020)

Author: narno2202

Marlin/Configuration_adv.h

@@ -1127,7 +1127,7 @@
    * Advanced configuration
    */
   #define FTM_BATCH_SIZE            100                 // Batch size for trajectory generation;
-                                                        // half the window size for Ulendo FBS.
+  #define FTM_WINDOW_SIZE           200                 // Window size for trajectory generation.
   #define FTM_FS                   1000                 // (Hz) Frequency for trajectory generation. (1 / FTM_TS)
   #define FTM_TS                      0.001f            // (s) Time step for trajectory generation. (1 / FTM_FS)
   #define FTM_STEPPER_FS          20000                 // (Hz) Frequency for stepper I/O update.

Marlin/src/module/ft_motion.cpp

@@ -75,6 +75,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.
 
 // Trapezoid data variables.
 xyze_pos_t   FxdTiCtrl::startPosn,                    // (mm) Start position of block
@@ -123,6 +124,8 @@ hal_timer_t FxdTiCtrl::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the nex
   float FxdTiCtrl::e_advanced_z1 = 0.0f;        // (ms) Unit delay of advanced extruder position.
 #endif
 
+constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
+
 //-----------------------------------------------------------------//
 // Function definitions.
 //-----------------------------------------------------------------//
@@ -143,8 +146,8 @@ void FxdTiCtrl::runoutBlock() {
   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 > FTM_BATCH_SIZE)
-      for (uint32_t i = makeVector_batchIdx; i < 2 * (FTM_BATCH_SIZE); i++) {
+    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],
@@ -159,8 +162,9 @@ void FxdTiCtrl::runoutBlock() {
         );
       }
 
-    makeVector_batchIdx = FTM_BATCH_SIZE;
+    makeVector_batchIdx = last_batchIdx;
     batchRdy = true;
+    runout = true;
   }
   runoutEna = false;
 }
@@ -184,14 +188,35 @@ void FxdTiCtrl::loop() {
   }
 
   // Planner processing and block conversion.
-  if (!blockProcRdy) stepper.fxdTiCtrl_BlockQueueUpdate();
+  if (!blockProcRdy && !runout) stepper.fxdTiCtrl_BlockQueueUpdate();
 
   if (blockProcRdy) {
     if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot.
     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) {
 
@@ -371,10 +396,12 @@ void FxdTiCtrl::reset() {
   stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;
 
   traj.reset(); // Reset trajectory history
+  trajMod.reset(); // Reset modified trajectory history
 
   blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
   batchRdy = batchRdyForInterp = false;
   runoutEna = false;
+  runout = false;
 
   endPosn_prevBlock.reset();
 
@@ -611,8 +638,8 @@ void FxdTiCtrl::makeVector() {
   #endif
 
   // Filled up the queue with regular and shaped steps
-  if (++makeVector_batchIdx == 2 * (FTM_BATCH_SIZE)) {
-    makeVector_batchIdx = FTM_BATCH_SIZE;
+  if (++makeVector_batchIdx == (FTM_WINDOW_SIZE)) {
+    makeVector_batchIdx = last_batchIdx;
     batchRdy = true;
   }
 

Marlin/src/module/ft_motion.h

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

Marlin/src/module/ft_types.h

@@ -48,7 +48,7 @@ enum stepDirState_t : uint8_t {
   stepDirState_NEG     = 2U
 };
 
-typedef struct XYZEarray<float, 2 * (FTM_BATCH_SIZE)> xyze_trajectory_t;
+typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
 typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
 
 typedef struct XYZEval<stepDirState_t> xyze_stepDir_t;