FT_MOTION : fix Ulendo's FBS when FTM_WINDOW_SIZE / FTM_BATCH_SIZE ratio is not 2

Marlin/src/inc/Conditionals_adv.h

@@ -1309,6 +1309,12 @@
  #define HAS_ZV_SHAPING 1
 #endif
 
+// FT Motion unified window and batch size
+#if ALL(FT_MOTION, FTM_UNIFIED_BWS)
+ #define FTM_WINDOW_SIZE FTM_BW_SIZE
+ #define FTM_BATCH_SIZE FTM_BW_SIZE
+#endif
+
 // Toolchange Event G-code
 #if !HAS_MULTI_EXTRUDER || !(defined(EVENT_GCODE_TOOLCHANGE_T0) || defined(EVENT_GCODE_TOOLCHANGE_T1) || defined(EVENT_GCODE_TOOLCHANGE_T2) || defined(EVENT_GCODE_TOOLCHANGE_T3) || defined(EVENT_GCODE_TOOLCHANGE_T4) || defined(EVENT_GCODE_TOOLCHANGE_T5) || defined(EVENT_GCODE_TOOLCHANGE_T6) || defined(EVENT_GCODE_TOOLCHANGE_T7))
  #undef TC_GCODE_USE_GLOBAL_X

Marlin/src/inc/SanityCheck.h

@@ -4123,8 +4123,12 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive."
 /**
  * Fixed-Time Motion limitations
  */
-#if ALL(FT_MOTION, MIXING_EXTRUDER)
- #error "FT_MOTION does not currently support MIXING_EXTRUDER."
+#if ENABLED(FT_MOTION)
+ #if ENABLED(MIXING_EXTRUDER)
+ #error "FT_MOTION does not currently support MIXING_EXTRUDER."
+ #elif DISABLED(FTM_UNIFIED_BWS)
+ #error "FT_MOTION requires FTM_UNIFIED_BWS to be enabled because FBS is not yet implemented."
+ #endif
 #endif
 
 // Multi-Stepping Limit

Marlin/src/module/ft_motion.cpp

@@ -55,8 +55,8 @@ FTMotion ftMotion;
 ft_config_t FTMotion::cfg;
 bool FTMotion::busy; // = false
 ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepper commands buffer.
-uint32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer.
-  FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
+int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer.
+ FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
 
 bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use.
 
@@ -123,6 +123,8 @@ uint32_t FTMotion::interpIdx = 0, // Index of current data point b
  float FTMotion::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.
 //-----------------------------------------------------------------
@@ -145,8 +147,16 @@ void FTMotion::runoutBlock() {
  ratio.reset();
 
  max_intervals = cfg.modeHasShaper() ? shaper_intervals : 0;
- if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, min_max_intervals - (FTM_BATCH_SIZE))) max_intervals = min_max_intervals;
- max_intervals += TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_WINDOW_SIZE) - makeVector_batchIdx;
+ if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BATCH_SIZE, min_max_intervals - (FTM_BATCH_SIZE)))
+ max_intervals = min_max_intervals;
+
+ max_intervals += (
+ #if ENABLED(FTM_UNIFIED_BWS)
+ FTM_WINDOW_SIZE - makeVector_batchIdx
+ #else
+ FTM_WINDOW_SIZE - ((last_batchIdx < (FTM_BATCH_SIZE)) ? 0 : makeVector_batchIdx)
+ #endif
+ );
  blockProcRdy = blockDataIsRunout = true;
  runoutEna = blockProcDn = false;
 }
@@ -198,7 +208,7 @@ void FTMotion::loop() {
  );
 
  // Shift the time series back in the window
- #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], (FTM_WINDOW_SIZE - FTM_BATCH_SIZE) * sizeof(traj.A[0]))
+ #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], last_batchIdx * sizeof(traj.A[0]))
  LOGICAL_AXIS_CODE(
  TSHIFT(e),
  TSHIFT(x), TSHIFT(y), TSHIFT(z),
@@ -219,7 +229,7 @@ void FTMotion::loop() {
  && (interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP))
  ) {
  convertToSteps(interpIdx);
- if (++interpIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE)) {
+ if (++interpIdx == FTM_BATCH_SIZE) {
  batchRdyForInterp = false;
  interpIdx = 0;
  }
@@ -449,7 +459,7 @@ void FTMotion::reset() {
  endPosn_prevBlock.reset();
 
  makeVector_idx = makeVector_idx_z1 = 0;
- makeVector_batchIdx = 0;
+ makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MAX(last_batchIdx, FTM_BATCH_SIZE));
 
  steps.reset();
  interpIdx = interpIdx_z1 = 0;
@@ -464,10 +474,11 @@ void FTMotion::reset() {
 }
 
 // Private functions.
+
 // Auxiliary function to get number of step commands in the buffer.
-uint32_t FTMotion::stepperCmdBuffItems() {
- const uint32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx;
- return stepperCmdBuff_produceIdx < stepperCmdBuff_consumeIdx ? (FTM_STEPPERCMD_BUFF_SIZE) + udiff : udiff;
+int32_t FTMotion::stepperCmdBuffItems() {
+ const int32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx;
+ return (udiff < 0) ? udiff + (FTM_STEPPERCMD_BUFF_SIZE) : udiff;
 }
 
 // Initializes storage variables before startup.
@@ -677,8 +688,8 @@ void FTMotion::makeVector() {
  #endif
 
  // Filled up the queue with regular and shaped steps
- if (++makeVector_batchIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE))) {
- makeVector_batchIdx = 0;
+ if (++makeVector_batchIdx == FTM_WINDOW_SIZE) {
+ makeVector_batchIdx = last_batchIdx;
  batchRdy = true;
  }
 

Marlin/src/module/ft_motion.h

@@ -102,12 +102,11 @@ class FTMotion {
  }
 
  static ft_command_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of stepper commands.
- static uint32_t stepperCmdBuff_produceIdx, // Index of next stepper command write to the buffer.
-  stepperCmdBuff_consumeIdx; // Index of next stepper command read from the buffer.
+ static int32_t stepperCmdBuff_produceIdx,  // Index of next stepper command write to the buffer.
+ stepperCmdBuff_consumeIdx;  // Index of next stepper command read from the buffer.
 
  static bool sts_stepperBusy; // The stepper buffer has items and is in use.
 
-
  // Public methods
  static void init();
  static void startBlockProc(); // Set controller states to begin processing a block.
@@ -153,10 +152,10 @@ class FTMotion {
  static uint32_t N1, N2, N3;
  static uint32_t max_intervals;
 
- static constexpr uint32_t _ftm_size = TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE),
-  _ftm_wind = TERN(FTM_UNIFIED_BWS, 2, CEIL((FTM_WINDOW_SIZE) / _ftm_size)),
- shaper_intervals = _ftm_size * CEIL((FTM_ZMAX) / _ftm_size),
- min_max_intervals = _ftm_size * _ftm_wind;
+ #define _DIVCEIL(A,B) (((A) + (B) - 1) / (B))
+ static constexpr uint32_t _ftm_wind = TERN(FTM_UNIFIED_BWS, 2, _DIVCEIL(FTM_WINDOW_SIZE, FTM_BATCH_SIZE)),
+ shaper_intervals = (FTM_BATCH_SIZE) * _DIVCEIL(FTM_ZMAX, FTM_BATCH_SIZE),
+ min_max_intervals = (FTM_BATCH_SIZE) * _ftm_wind;
 
  // Make vector variables.
  static uint32_t makeVector_idx,
@@ -203,7 +202,7 @@ class FTMotion {
  #endif
 
  // Private methods
- static uint32_t stepperCmdBuffItems();
+ static int32_t stepperCmdBuffItems();
  static void loadBlockData(block_t *const current_block);
  static void makeVector();
  static void convertToSteps(const uint32_t idx);

Marlin/src/module/ft_types.h

@@ -44,13 +44,8 @@ enum dynFreqMode_t : uint8_t {
 
 #define IS_EI_MODE(N) WITHIN(N, ftMotionMode_EI, ftMotionMode_3HEI)
 
-#if ENABLED(FTM_UNIFIED_BWS)
- typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectory_t;
- typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryMod_t;
-#else
- typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
- typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
-#endif
+typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
+typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
 
 enum {
  LIST_N(DOUBLE(LOGICAL_AXES),

Marlin/src/module/stepper.cpp

@@ -3448,7 +3448,8 @@ void Stepper::report_positions() {
  // Use one byte to restore one stepper command in the format:
  // |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction|
  const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];
- if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE)) ftMotion.stepperCmdBuff_consumeIdx = 0U;
+ if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE))
+ ftMotion.stepperCmdBuff_consumeIdx = 0;
 
  if (abort_current_block) return;