From daa68d560ee9bcadf0d518496811b9ac65f50d9f Mon Sep 17 00:00:00 2001 From: David Buezas Date: Thu, 11 Sep 2025 01:24:32 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20FT=20Motion=20step=20rate?= =?UTF-8?q?=20instability=20(#28043)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/Configuration_adv.h | 1 - Marlin/src/module/ft_motion.cpp | 186 ++++++++++++++++---------------- Marlin/src/module/ft_motion.h | 11 +- 3 files changed, 96 insertions(+), 102 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index a9d26ac920..f3eef3895e 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1192,7 +1192,6 @@ #endif #define FTM_STEPS_PER_UNIT_TIME (FTM_STEPPER_FS / FTM_FS) // Interpolated stepper commands per unit time - #define FTM_CTS_COMPARE_VAL (FTM_STEPS_PER_UNIT_TIME / 2) // Comparison value used in interpolation algorithm #define FTM_MIN_TICKS ((STEPPER_TIMER_RATE) / (FTM_STEPPER_FS)) // Minimum stepper ticks between steps #define FTM_MIN_SHAPE_FREQ 10 // Minimum shaping frequency diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index d5865913ab..648237e19e 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -53,15 +53,14 @@ AxisBits FTMotion::axis_move_dir; xyze_trajectory_t FTMotion::traj; // = {0.0f} Storage for fixed-time-based trajectory. xyze_trajectoryMod_t FTMotion::trajMod; // = {0.0f} Storage for fixed time trajectory window. -bool FTMotion::blockProcRdy = false; // Set when new block data is loaded from stepper module into FTM, ... - // ... and reset when block is completely converted to FTM trajectory. -bool FTMotion::batchRdy = false; // Indicates a batch of the fixed time trajectory... - // ... has been generated, is now available in the upper - - // batch of traj.x[], y, z ... e vectors, and is ready to be - // post processed, if applicable, then interpolated. Reset when the - // data has been shifted out. -bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done being post processed... - // ... if applicable, and is ready to be converted to step commands. +bool FTMotion::blockProcRdy = false; // Set when new block data is loaded from stepper module into FTM, + // and reset when block is completely converted to FTM trajectory. +bool FTMotion::batchRdy = false; // Indicates a batch of the fixed time trajectory has been + // generated, is now available in the upper-batch of traj.A[], and + // is ready to be post-processed (if applicable) and interpolated. + // Reset once the data has been shifted out. +bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done being post processed + // (if applicable) and is ready to be converted to step commands. // Trapezoid data variables. xyze_pos_t FTMotion::startPos, // (mm) Start position of block @@ -81,11 +80,12 @@ uint32_t FTMotion::N1, // Number of data points in the uint32_t FTMotion::max_intervals; // Total number of data points that will be generated from block. // Make vector variables. -uint32_t FTMotion::makeVector_idx = 0, // Index of fixed time trajectory generation of the overall block. - FTMotion::makeVector_batchIdx = 0; // Index of fixed time trajectory generation within the batch. +uint32_t FTMotion::traj_idx_get = 0, // Index of fixed time trajectory generation of the overall block. + FTMotion::traj_idx_set = 0; // Index of fixed time trajectory generation within the batch. // Interpolation variables. xyze_long_t FTMotion::steps = { 0 }; // Step count accumulator. +xyze_long_t FTMotion::step_error_q10 = { 0 }; // Fractional remainder in q10.21 format uint32_t FTMotion::interpIdx = 0; // Index of current data point being interpolated. @@ -170,14 +170,14 @@ void FTMotion::loop() { if (blockProcRdy) { - if (!batchRdy) makeVector(); // may clear blockProcRdy + if (!batchRdy) generateTrajectoryPointsFromBlock(); // may clear blockProcRdy // Check if the block has been completely converted: if (!blockProcRdy) { discard_planner_block_protected(); if (!batchRdy && !planner.has_blocks_queued()) { runoutBlock(); - makeVector(); // Additional call to guarantee batchRdy is set this loop. + generateTrajectoryPointsFromBlock(); // Additional call to guarantee batchRdy is set this loop. } } } @@ -202,13 +202,13 @@ void FTMotion::loop() { // ... data is ready in trajMod. batchRdyForInterp = true; - batchRdy = false; // Clear so makeVector() can resume generating points. + batchRdy = false; // Clear so generateTrajectoryPointsFromBlock() can resume generating points. } // Interpolation (generation of step commands from fixed time trajectory). while (batchRdyForInterp && (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))) { - convertToSteps(interpIdx); + generateStepsFromTrajectory(interpIdx); if (++interpIdx == FTM_BATCH_SIZE) { batchRdyForInterp = false; interpIdx = 0; @@ -384,10 +384,11 @@ void FTMotion::reset() { endPos_prevBlock.reset(); - makeVector_idx = 0; - makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MIN(BATCH_SIDX_IN_WINDOW, FTM_BATCH_SIZE)); + traj_idx_get = 0; + traj_idx_set = TERN(FTM_UNIFIED_BWS, 0, _MIN(BATCH_SIDX_IN_WINDOW, FTM_BATCH_SIZE)); steps.reset(); + step_error_q10.reset(); interpIdx = 0; #if HAS_FTM_SHAPING @@ -429,7 +430,7 @@ void FTMotion::runoutBlock() { startPos = endPos_prevBlock; ratio.reset(); - const int32_t n_to_fill_batch = (FTM_WINDOW_SIZE) - makeVector_batchIdx; + const int32_t n_to_fill_batch = (FTM_WINDOW_SIZE) - traj_idx_set; // This line or function is to be modified for FBS use; do not optimize out. const int32_t n_to_settle_shaper = num_samples_shaper_settle(); @@ -463,9 +464,7 @@ void FTMotion::loadBlockData(block_t * const current_block) { oneOverLength = 1.0f / totalLength; startPos = endPos_prevBlock; - const xyze_pos_t& moveDist = current_block->dist_mm; - ratio = moveDist * oneOverLength; const float spm = totalLength / current_block->step_event_count; // (steps/mm) Distance for each step @@ -563,20 +562,20 @@ void FTMotion::loadBlockData(block_t * const current_block) { } // Generate data points of the trajectory. -void FTMotion::makeVector() { +void FTMotion::generateTrajectoryPointsFromBlock() { do { - float tau = (makeVector_idx + 1) * (FTM_TS); // (s) Time since start of block + float tau = (traj_idx_get + 1) * (FTM_TS); // (s) Time since start of block float dist = 0.0f; // (mm) Distance traveled #if HAS_EXTRUDERS float accel_k = 0.0f; // (mm/s^2) Acceleration K factor #endif - if (makeVector_idx < N1) { + if (traj_idx_get < N1) { // Acceleration phase dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase since start of block TERN_(HAS_EXTRUDERS, accel_k = accel_P); // (mm/s^2) Acceleration K factor from Accel phase } - else if (makeVector_idx < (N1 + N2)) { + else if (traj_idx_get < (N1 + N2)) { // Coasting phase dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase since start of block //TERN_(HAS_EXTRUDERS, accel_k = 0.0f); @@ -588,17 +587,17 @@ void FTMotion::makeVector() { TERN_(HAS_EXTRUDERS, accel_k = decel_P); // (mm/s^2) Acceleration K factor from Decel phase } - #define _SET_TRAJ(q) traj.q[makeVector_batchIdx] = startPos.q + ratio.q * dist; + #define _SET_TRAJ(q) traj.q[traj_idx_set] = startPos.q + ratio.q * dist; LOGICAL_AXIS_MAP_LC(_SET_TRAJ); #if HAS_EXTRUDERS if (cfg.linearAdvEna) { - float dedt_adj = (traj.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS); + float dedt_adj = (traj.e[traj_idx_set] - e_raw_z1) * (FTM_FS); if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK * 0.0001f; - e_raw_z1 = traj.e[makeVector_batchIdx]; + e_raw_z1 = traj.e[traj_idx_set]; e_advanced_z1 += dedt_adj * (FTM_TS); - traj.e[makeVector_batchIdx] = e_advanced_z1; + traj.e[traj_idx_set] = e_advanced_z1; } #endif @@ -609,7 +608,7 @@ void FTMotion::makeVector() { #if HAS_DYNAMIC_FREQ_MM case dynFreqMode_Z_BASED: { static float oldz = 0.0f; - const float z = traj.z[makeVector_batchIdx]; + const float z = traj.z[traj_idx_set]; if (z != oldz) { // Only update if Z changed. oldz = z; #if HAS_X_AXIS @@ -629,10 +628,10 @@ void FTMotion::makeVector() { // Update constantly. The optimization done for Z value makes // less sense for E, as E is expected to constantly change. #if HAS_X_AXIS - shaping.x.set_axis_shaping_N(cfg.shaper.x, cfg.baseFreq.x + cfg.dynFreqK.x * traj.e[makeVector_batchIdx], cfg.zeta.x); + shaping.x.set_axis_shaping_N(cfg.shaper.x, cfg.baseFreq.x + cfg.dynFreqK.x * traj.e[traj_idx_set], cfg.zeta.x); #endif #if HAS_Y_AXIS - shaping.y.set_axis_shaping_N(cfg.shaper.y, cfg.baseFreq.y + cfg.dynFreqK.y * traj.e[makeVector_batchIdx], cfg.zeta.y); + shaping.y.set_axis_shaping_N(cfg.shaper.y, cfg.baseFreq.y + cfg.dynFreqK.y * traj.e[traj_idx_set], cfg.zeta.y); #endif break; #endif @@ -644,22 +643,22 @@ void FTMotion::makeVector() { #if HAS_FTM_SHAPING #if HAS_X_AXIS if (shaping.x.ena) { - shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx]; - traj.x[makeVector_batchIdx] *= shaping.x.Ai[0]; + shaping.x.d_zi[shaping.zi_idx] = traj.x[traj_idx_set]; + traj.x[traj_idx_set] *= shaping.x.Ai[0]; for (uint32_t i = 1U; i <= shaping.x.max_i; i++) { const uint32_t udiffx = shaping.zi_idx - shaping.x.Ni[i]; - traj.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx]; + traj.x[traj_idx_set] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx]; } } #endif #if HAS_Y_AXIS if (shaping.y.ena) { - shaping.y.d_zi[shaping.zi_idx] = traj.y[makeVector_batchIdx]; - traj.y[makeVector_batchIdx] *= shaping.y.Ai[0]; + shaping.y.d_zi[shaping.zi_idx] = traj.y[traj_idx_set]; + traj.y[traj_idx_set] *= shaping.y.Ai[0]; for (uint32_t i = 1U; i <= shaping.y.max_i; i++) { const uint32_t udiffy = shaping.zi_idx - shaping.y.Ni[i]; - traj.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy]; + traj.y[traj_idx_set] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy]; } } #endif @@ -667,86 +666,81 @@ void FTMotion::makeVector() { #endif // HAS_FTM_SHAPING // Filled up the queue with regular and shaped steps - if (++makeVector_batchIdx == FTM_WINDOW_SIZE) { - makeVector_batchIdx = BATCH_SIDX_IN_WINDOW; + if (++traj_idx_set == FTM_WINDOW_SIZE) { + traj_idx_set = BATCH_SIDX_IN_WINDOW; batchRdy = true; } - - if (++makeVector_idx == max_intervals) { + if (++traj_idx_get == max_intervals) { blockProcRdy = false; - makeVector_idx = 0; + traj_idx_get = 0; } } while (blockProcRdy && !batchRdy); -} +} // generateTrajectoryPointsFromBlock /** - * Convert to steps - * - Commands are written in a bitmask with step and dir as single bits. - * - Tests for delta are moved outside the loop. - * - Two functions are used for command computation with an array of function pointers. + * @brief Interpolate a single trajectory data point into stepper commands. + * @param idx The index of the trajectory point to convert. + * + * Calculate the required stepper movements for each axis based on the + * difference between the current and previous trajectory points. + * Add up to one stepper command to the buffer with STEP/DIR bits for all axes. */ -static void (*command_set[LOGICAL_AXES])(int32_t&, int32_t&, ft_command_t&, int32_t, int32_t); +void FTMotion::generateStepsFromTrajectory(const uint32_t idx) { + constexpr float INV_FTM_STEPS_PER_UNIT_TIME = 1.0f / (FTM_STEPS_PER_UNIT_TIME); -static void command_set_pos(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) { - if (e < FTM_CTS_COMPARE_VAL) return; - s++; - b |= bd | bs; - e -= FTM_STEPS_PER_UNIT_TIME; -} + // q10 per-stepper-slot increment toward this sample’s target step count. + // (traj * steps_per_mm - steps) = steps still due at the start of this UNIT_TIME. + // Convert to q10 (×2^10), then subtract the current accumulator error: step_error_q10 / FTM_STEPS_PER_UNIT_TIME. + // Over FTM_STEPS_PER_UNIT_TIME stepper-slots this sums to the exact target (no drift). + // Any fraction of a step that may remain will be accounted for by the next UNIT_TIME + #define TOSTEPS_q10(A, B) int32_t( \ + (trajMod.A[idx] * planner.settings.axis_steps_per_mm[B] - steps.A) * _BV(10) \ + - step_error_q10.A * INV_FTM_STEPS_PER_UNIT_TIME ) -static void command_set_neg(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) { - if (e > -(FTM_CTS_COMPARE_VAL)) return; - s--; - b |= bs; - e += FTM_STEPS_PER_UNIT_TIME; -} + xyze_long_t delta_q10 = LOGICAL_AXIS_ARRAY( + TOSTEPS_q10(e, block_extruder_axis), + TOSTEPS_q10(x, X_AXIS), TOSTEPS_q10(y, Y_AXIS), TOSTEPS_q10(z, Z_AXIS), + TOSTEPS_q10(i, I_AXIS), TOSTEPS_q10(j, J_AXIS), TOSTEPS_q10(k, K_AXIS), + TOSTEPS_q10(u, U_AXIS), TOSTEPS_q10(v, V_AXIS), TOSTEPS_q10(w, W_AXIS) + ); -// Interpolates single data point to stepper commands. -void FTMotion::convertToSteps(const uint32_t idx) { - xyze_long_t err_P = { 0 }; + // Fixed-point denominator for step accumulation + constexpr int32_t denom_q10 = (FTM_STEPS_PER_UNIT_TIME) << 10; - //#define STEPS_ROUNDING - #if ENABLED(STEPS_ROUNDING) - #define TOSTEPS(A,B) int32_t(trajMod.A[idx] * planner.settings.axis_steps_per_mm[B] + (trajMod.A[idx] < 0.0f ? -0.5f : 0.5f)) - const xyze_long_t steps_tar = LOGICAL_AXIS_ARRAY( - TOSTEPS(e, block_extruder_axis), // May be eliminated if guaranteed positive. - TOSTEPS(x, X_AXIS), TOSTEPS(y, Y_AXIS), TOSTEPS(z, Z_AXIS), - TOSTEPS(i, I_AXIS), TOSTEPS(j, J_AXIS), TOSTEPS(k, K_AXIS), - TOSTEPS(u, U_AXIS), TOSTEPS(v, V_AXIS), TOSTEPS(w, W_AXIS) - ); - xyze_long_t delta = steps_tar - steps; - #else - #define TOSTEPS(A,B) int32_t(trajMod.A[idx] * planner.settings.axis_steps_per_mm[B]) - steps.A - xyze_long_t delta = LOGICAL_AXIS_ARRAY( - TOSTEPS(e, block_extruder_axis), - TOSTEPS(x, X_AXIS), TOSTEPS(y, Y_AXIS), TOSTEPS(z, Z_AXIS), - TOSTEPS(i, I_AXIS), TOSTEPS(j, J_AXIS), TOSTEPS(k, K_AXIS), - TOSTEPS(u, U_AXIS), TOSTEPS(v, V_AXIS), TOSTEPS(w, W_AXIS) - ); - #endif + // 1. Subtract one whole step from the accumulated distance + // 2. Accumulate one positive or negative step + // 3. Set the step and direction bits for the stepper command + #define RUN_AXIS(A) \ + do { \ + if (step_error_q10.A >= denom_q10) { \ + step_error_q10.A -= denom_q10; \ + steps.A++; \ + cmd |= _BV(FT_BIT_DIR_##A) | _BV(FT_BIT_STEP_##A); \ + } \ + if (step_error_q10.A <= -denom_q10) { \ + step_error_q10.A += denom_q10; \ + steps.A--; \ + cmd |= _BV(FT_BIT_STEP_##A); /* neg dir implicit */ \ + } \ + } while (0); - #define _COMMAND_SET(AXIS) command_set[_AXIS(AXIS)] = delta[_AXIS(AXIS)] >= 0 ? command_set_pos : command_set_neg; - LOGICAL_AXIS_MAP(_COMMAND_SET); + for (uint32_t i = 0; i < uint32_t(FTM_STEPS_PER_UNIT_TIME); i++) { + // Reference the next stepper command in the circular buffer + ft_command_t& cmd = stepperCmdBuff[stepperCmdBuff_produceIdx]; - for (uint32_t i = 0U; i < (FTM_STEPS_PER_UNIT_TIME); i++) { - - ft_command_t &cmd = stepperCmdBuff[stepperCmdBuff_produceIdx]; - - // Init all step/dir bits to 0 (defaulting to reverse/negative motion) + // Init the command to no STEP (Reverse DIR) cmd = 0; - // Accumulate the errors for all axes - err_P += delta; + // Accumulate the "error" for all axes according the fixed-point distance + step_error_q10 += delta_q10; - // Set up step/dir bits for all axes - #define _COMMAND_RUN(A) command_set[_AXIS(A)](err_P.A, steps.A, cmd, _BV(FT_BIT_DIR_##A), _BV(FT_BIT_STEP_##A)); - LOGICAL_AXIS_MAP(_COMMAND_RUN); + // Where the error has accumulated whole axis steps, add them to the command + LOGICAL_AXIS_MAP(RUN_AXIS); // Next circular buffer index if (++stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE)) stepperCmdBuff_produceIdx = 0; - - } // FTM_STEPS_PER_UNIT_TIME loop + } } #endif // FT_MOTION diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 9d3d5c9833..495ea706c1 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -160,14 +160,15 @@ class FTMotion { // Number of batches needed to propagate the current trajectory to the stepper. static constexpr uint32_t PROP_BATCHES = CEIL((FTM_WINDOW_SIZE) / (FTM_BATCH_SIZE)) - 1; - // Make vector variables. - static uint32_t makeVector_idx, - makeVector_batchIdx; + // generateTrajectoryPointsFromBlock variables. + static uint32_t traj_idx_get, + traj_idx_set; // Interpolation variables. static uint32_t interpIdx; static xyze_long_t steps; + static xyze_long_t step_error_q10; #if ENABLED(DISTINCT_E_FACTORS) static uint8_t block_extruder_axis; // Cached extruder axis index @@ -214,8 +215,8 @@ class FTMotion { static void runoutBlock(); static int32_t stepperCmdBuffItems(); static void loadBlockData(block_t *const current_block); - static void makeVector(); - static void convertToSteps(const uint32_t idx); + static void generateTrajectoryPointsFromBlock(); + static void generateStepsFromTrajectory(const uint32_t idx); FORCE_INLINE static int32_t num_samples_shaper_settle() { return ( shaping.x.ena || shaping.y.ena ) ? FTM_ZMAX : 0; }