diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index eee111a5412..be54354d5c0 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -1125,7 +1125,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.
diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp
index d3c4de89625..6fe8e936e0e 100644
--- a/Marlin/src/module/ft_motion.cpp
+++ b/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,7 +188,7 @@ 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.
@@ -192,6 +196,27 @@ void FxdTiCtrl::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;
}
diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h
index f4be977786f..2186ecb710a 100644
--- a/Marlin/src/module/ft_motion.h
+++ b/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
diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h
index c4465bd37bb..a7228d32d6f 100644
--- a/Marlin/src/module/ft_types.h
+++ b/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;