diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 6184e5df93..830db40333 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1191,7 +1191,7 @@ #define FTM_TRAJECTORY_TYPE TRAPEZOIDAL // Block acceleration profile (TRAPEZOIDAL, POLY5, POLY6) // TRAPEZOIDAL: Continuous Velocity. Max acceleration is respected. - // POLY5: Like POLY6 with 1.5x but cpu cheaper. + // POLY5: Like POLY6 with 1.5x but uses less CPU. // POLY6: Continuous Acceleration (aka S_CURVE). // POLY trajectories not only reduce resonances without rounding corners, but also // reduce extruder strain due to linear advance. @@ -1201,30 +1201,12 @@ /** * Advanced configuration */ - #define FTM_UNIFIED_BWS // DON'T DISABLE unless you use Ulendo FBS (not implemented) - #if ENABLED(FTM_UNIFIED_BWS) - #define FTM_BW_SIZE 100 // Unified Window and Batch size with a ratio of 2 - #else - #define FTM_WINDOW_SIZE 200 // Custom Window size for trajectory generation needed by Ulendo FBS - #define FTM_BATCH_SIZE 100 // Custom Batch size for trajectory generation needed by Ulendo FBS - #endif + #define FTM_BUFFER_SIZE 128 // Window size for trajectory generation, must be a power of 2 (e.g 64, 128, 256, ...) + // The total buffered time in seconds is (FTM_BUFFER_SIZE/FTM_FS) + #define FTM_FS 1000 // (Hz) Frequency for trajectory generation. + #define FTM_STEPPER_FS 2'000'000 // (Hz) Time resolution of stepper I/O update. Shouldn't affect CPU much (slower board testing needed) + #define FTM_MIN_SHAPE_FREQ 20 // (Hz) Minimum shaping frequency, lower consumes more RAM - #define FTM_FS 1000 // (Hz) Frequency for trajectory generation - - #if DISABLED(COREXY) - #define FTM_STEPPER_FS 20000 // (Hz) Frequency for stepper I/O update - - // Use this to adjust the time required to consume the command buffer. - // Try increasing this value if stepper motion is choppy. - #define FTM_STEPPERCMD_BUFF_SIZE 3000 // Size of the stepper command buffers - - #else - // CoreXY motion needs a larger buffer size. These values are based on our testing. - #define FTM_STEPPER_FS 30000 - #define FTM_STEPPERCMD_BUFF_SIZE 6000 - #endif - - #define FTM_MIN_SHAPE_FREQ 10 // (Hz) Minimum shaping frequency, lower consumes more RAM #endif // FT_MOTION /** diff --git a/Marlin/src/core/types.h b/Marlin/src/core/types.h index ab3820273a..23d83c4b9e 100644 --- a/Marlin/src/core/types.h +++ b/Marlin/src/core/types.h @@ -238,6 +238,24 @@ struct Flags { FI bool operator[](const int n) const { return test(n); } FI int size() const { return sizeof(b); } FI operator bool() const { return b != 0; } + + FI Flags& operator|=(Flags &p) const { b |= p.b; return *this; } + FI Flags& operator&=(Flags &p) const { b &= p.b; return *this; } + FI Flags& operator^=(Flags &p) const { b ^= p.b; return *this; } + + FI Flags& operator|=(const flagbits_t &p) { b |= flagbits_t(p); return *this; } + FI Flags& operator&=(const flagbits_t &p) { b &= flagbits_t(p); return *this; } + FI Flags& operator^=(const flagbits_t &p) { b ^= flagbits_t(p); return *this; } + + FI Flags operator|(Flags &p) const { return Flags(b | p.b); } + FI Flags operator&(Flags &p) const { return Flags(b & p.b); } + FI Flags operator^(Flags &p) const { return Flags(b ^ p.b); } + FI Flags operator~() const { return Flags(~b); } + + FI flagbits_t operator|(const flagbits_t &p) const { return b | flagbits_t(p); } + FI flagbits_t operator&(const flagbits_t &p) const { return b & flagbits_t(p); } + FI flagbits_t operator^(const flagbits_t &p) const { return b ^ flagbits_t(p); } + }; // Flag bits for more than 64 states @@ -635,6 +653,21 @@ struct XYval { FI bool operator==(const T &p) const { return x == p && y == p; } FI bool operator!=(const T &p) const { return !operator==(p); } + FI bool operator< (const XYval &rs) const { return x < rs.x && y < rs.y; } + FI bool operator<=(const XYval &rs) const { return x <= rs.x && y <= rs.y; } + FI bool operator> (const XYval &rs) const { return x > rs.x && y > rs.y; } + FI bool operator>=(const XYval &rs) const { return x >= rs.x && y >= rs.y; } + + FI bool operator< (const XYZval &rs) const { return true XY_GANG(&& x < rs.x, && y < rs.y); } + FI bool operator<=(const XYZval &rs) const { return true XY_GANG(&& x <= rs.x, && y <= rs.y); } + FI bool operator> (const XYZval &rs) const { return true XY_GANG(&& x > rs.x, && y > rs.y); } + FI bool operator>=(const XYZval &rs) const { return true XY_GANG(&& x >= rs.x, && y >= rs.y); } + + FI bool operator< (const XYZEval &rs) const { return true XY_GANG(&& x < rs.x, && y < rs.y); } + FI bool operator<=(const XYZEval &rs) const { return true XY_GANG(&& x <= rs.x, && y <= rs.y); } + FI bool operator> (const XYZEval &rs) const { return true XY_GANG(&& x > rs.x, && y > rs.y); } + FI bool operator>=(const XYZEval &rs) const { return true XY_GANG(&& x >= rs.x, && y >= rs.y); } + }; // @@ -794,6 +827,16 @@ struct XYZval { FI bool operator==(const T &p) const { return ENABLED(HAS_X_AXIS) NUM_AXIS_GANG(&& x == p, && y == p, && z == p, && i == p, && j == p, && k == p, && u == p, && v == p, && w == p); } FI bool operator!=(const T &p) const { return !operator==(p); } + FI bool operator< (const XYZval &rs) const { return true NUM_AXIS_GANG(&& x < rs.x, && y < rs.y, && z < rs.z, && i < rs.i, && j < rs.j, && k < rs.k, && u < rs.u, && v < rs.v, && w < rs.w); } + FI bool operator<=(const XYZval &rs) const { return true NUM_AXIS_GANG(&& x <= rs.x, && y <= rs.y, && z <= rs.z, && i <= rs.i, && j <= rs.j, && k <= rs.k, && u <= rs.u, && v <= rs.v, && w <= rs.w); } + FI bool operator> (const XYZval &rs) const { return true NUM_AXIS_GANG(&& x > rs.x, && y > rs.y, && z > rs.z, && i > rs.i, && j > rs.j, && k > rs.k, && u > rs.u, && v > rs.v, && w > rs.w); } + FI bool operator>=(const XYZval &rs) const { return true NUM_AXIS_GANG(&& x >= rs.x, && y >= rs.y, && z >= rs.z, && i >= rs.i, && j >= rs.j, && k >= rs.k, && u >= rs.u, && v >= rs.v, && w >= rs.w); } + + FI bool operator< (const XYZEval &rs) const { return true NUM_AXIS_GANG(&& x < rs.x, && y < rs.y, && z < rs.z, && i < rs.i, && j < rs.j, && k < rs.k, && u < rs.u, && v < rs.v, && w < rs.w); } + FI bool operator<=(const XYZEval &rs) const { return true NUM_AXIS_GANG(&& x <= rs.x, && y <= rs.y, && z <= rs.z, && i <= rs.i, && j <= rs.j, && k <= rs.k, && u <= rs.u, && v <= rs.v, && w <= rs.w); } + FI bool operator> (const XYZEval &rs) const { return true NUM_AXIS_GANG(&& x > rs.x, && y > rs.y, && z > rs.z, && i > rs.i, && j > rs.j, && k > rs.k, && u > rs.u, && v > rs.v, && w > rs.w); } + FI bool operator>=(const XYZEval &rs) const { return true NUM_AXIS_GANG(&& x >= rs.x, && y >= rs.y, && z >= rs.z, && i >= rs.i, && j >= rs.j, && k >= rs.k, && u >= rs.u, && v >= rs.v, && w >= rs.w); } + }; // @@ -957,6 +1000,16 @@ struct XYZEval { FI bool operator==(const T &p) const { return ENABLED(HAS_X_AXIS) LOGICAL_AXIS_GANG(&& e == p, && x == p, && y == p, && z == p, && i == p, && j == p, && k == p, && u == p, && v == p, && w == p); } FI bool operator!=(const T &p) const { return !operator==(p); } + FI bool operator< (const XYZEval &rs) const { return true LOGICAL_AXIS_GANG(&& e < rs.e, && x < rs.x, && y < rs.y, && z < rs.z, && i < rs.i, && j < rs.j, && k < rs.k, && u < rs.u, && v < rs.v, && w < rs.w); } + FI bool operator<=(const XYZEval &rs) const { return true LOGICAL_AXIS_GANG(&& e <= rs.e, && x <= rs.x, && y <= rs.y, && z <= rs.z, && i <= rs.i, && j <= rs.j, && k <= rs.k, && u <= rs.u, && v <= rs.v, && w <= rs.w); } + FI bool operator> (const XYZEval &rs) const { return true LOGICAL_AXIS_GANG(&& e > rs.e, && x > rs.x, && y > rs.y, && z > rs.z, && i > rs.i, && j > rs.j, && k > rs.k, && u > rs.u, && v > rs.v, && w > rs.w); } + FI bool operator>=(const XYZEval &rs) const { return true LOGICAL_AXIS_GANG(&& e >= rs.e, && x >= rs.x, && y >= rs.y, && z >= rs.z, && i >= rs.i, && j >= rs.j, && k >= rs.k, && u >= rs.u, && v >= rs.v, && w >= rs.w); } + + FI bool operator< (const XYZval &rs) const { return true NUM_AXIS_GANG(&& x < rs.x, && y < rs.y, && z < rs.z, && i < rs.i, && j < rs.j, && k < rs.k, && u < rs.u, && v < rs.v, && w < rs.w); } + FI bool operator<=(const XYZval &rs) const { return true NUM_AXIS_GANG(&& x <= rs.x, && y <= rs.y, && z <= rs.z, && i <= rs.i, && j <= rs.j, && k <= rs.k, && u <= rs.u, && v <= rs.v, && w <= rs.w); } + FI bool operator> (const XYZval &rs) const { return true NUM_AXIS_GANG(&& x > rs.x, && y > rs.y, && z > rs.z, && i > rs.i, && j > rs.j, && k > rs.k, && u > rs.u, && v > rs.v, && w > rs.w); } + FI bool operator>=(const XYZval &rs) const { return true NUM_AXIS_GANG(&& x >= rs.x, && y >= rs.y, && z >= rs.z, && i >= rs.i, && j >= rs.j, && k >= rs.k, && u >= rs.u, && v >= rs.v, && w >= rs.w); } + }; #include // for memset @@ -1263,6 +1316,7 @@ public: FI AxisBits operator|(const AxisBits &p) const { return AxisBits(bits | p.bits); } FI AxisBits operator&(const AxisBits &p) const { return AxisBits(bits & p.bits); } FI AxisBits operator^(const AxisBits &p) const { return AxisBits(bits ^ p.bits); } + FI AxisBits operator~() const { return AxisBits(~bits); } FI operator bool() const { return !!bits; } FI operator uint16_t() const { return uint16_t(bits & 0xFFFF); } diff --git a/Marlin/src/inc/Conditionals-4-adv.h b/Marlin/src/inc/Conditionals-4-adv.h index 8bec3c1dd3..df2327254d 100644 --- a/Marlin/src/inc/Conditionals-4-adv.h +++ b/Marlin/src/inc/Conditionals-4-adv.h @@ -1537,10 +1537,6 @@ #if !HAS_EXTRUDERS #undef FTM_SHAPER_E #endif - #if ENABLED(FTM_UNIFIED_BWS) - #define FTM_WINDOW_SIZE FTM_BW_SIZE - #define FTM_BATCH_SIZE FTM_BW_SIZE - #endif #endif // Multi-Stepping Limit diff --git a/Marlin/src/inc/Conditionals-5-post.h b/Marlin/src/inc/Conditionals-5-post.h index 6e5fa8de6c..4bed697726 100644 --- a/Marlin/src/inc/Conditionals-5-post.h +++ b/Marlin/src/inc/Conditionals-5-post.h @@ -3680,4 +3680,8 @@ // 2HEI : FTM_RATIO * 3 / 2 // 3HEI : FTM_RATIO * 2 #define FTM_SMOOTHING_ORDER 5 // 3 to 5 is closest to gaussian + #ifndef FTM_BUFFER_SIZE + #define FTM_BUFFER_SIZE 128 + #endif + #define FTM_BUFFER_MASK (FTM_BUFFER_SIZE - 1u) #endif diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index aadb3aea9d..cd77bd24ba 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4491,10 +4491,9 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." * Fixed-Time Motion limitations */ #if ENABLED(FT_MOTION) + static_assert(FTM_BUFFER_SIZE >= 4 && (FTM_BUFFER_SIZE & FTM_BUFFER_MASK) == 0, "FTM_BUFFER_SIZE must be a power of two (128, 256, 512, ...)."); #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 #if !HAS_X_AXIS static_assert(FTM_DEFAULT_SHAPER_X != ftMotionShaper_NONE, "Without any linear axes FTM_DEFAULT_SHAPER_X must be ftMotionShaper_NONE."); diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index 239badc23b..69b2116bba 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -52,30 +52,12 @@ FTMotion ftMotion; ft_config_t FTMotion::cfg; bool FTMotion::busy; // = false -ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepper commands 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::stepperCmdBuffHasData = false; // The stepper buffer has items and is in use. XYZEval FTMotion::axis_move_end_ti = { 0 }; AxisBits FTMotion::axis_move_dir; // Private variables. -// NOTE: These are sized for Ulendo FBS use. -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.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. - // Block data variables. xyze_pos_t FTMotion::startPos, // (mm) Start position of block FTMotion::endPos_prevBlock = { 0.0f }; // (mm) End position of previous block @@ -89,15 +71,12 @@ Poly6TrajectoryGenerator FTMotion::poly6Generator; TrajectoryGenerator* FTMotion::currentGenerator = &FTMotion::trapezoidalGenerator; TrajectoryType FTMotion::trajectoryType = TrajectoryType::FTM_TRAJECTORY_TYPE; -// Make vector variables. -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. +// Compact plan buffer +stepper_plan_t FTMotion::stepper_plan_buff[FTM_BUFFER_SIZE]; +XYZEval FTMotion::curr_steps_q32_32 = {0}; -// 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. +uint32_t FTMotion::stepper_plan_tail = 0, // The index to consume from + FTMotion::stepper_plan_head = 0; // The index to produce into #if ENABLED(DISTINCT_E_FACTORS) uint8_t FTMotion::block_extruder_axis; // Cached E Axis from last-fetched block @@ -108,7 +87,7 @@ uint32_t FTMotion::interpIdx = 0; // Index of current data point b // Shaping variables. #if HAS_FTM_SHAPING - FTMotion::shaping_t FTMotion::shaping = { + shaping_t FTMotion::shaping = { zi_idx: 0 #if HAS_X_AXIS , X:{ false, { 0.0f }, { 0.0f }, { 0 }, 0 } // ena, d_zi[], Ai[], Ni[], max_i @@ -126,7 +105,7 @@ uint32_t FTMotion::interpIdx = 0; // Index of current data point b #endif #if ENABLED(FTM_SMOOTHING) - FTMotion::smoothing_t FTMotion::smoothing = { + smoothing_t FTMotion::smoothing = { #if HAS_X_AXIS X:{ { 0.0f }, 0.0f, 0 }, // smoothing_pass[], alpha, delay_samples #endif @@ -147,7 +126,8 @@ uint32_t FTMotion::interpIdx = 0; // Index of current data point b float FTMotion::prev_traj_e = 0.0f; // (ms) Unit delay of raw extruder position. #endif -constexpr uint32_t BATCH_SIDX_IN_WINDOW = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE); // Batch start index in window. +// Stepping variables. +stepping_t FTMotion::stepping; //----------------------------------------------------------------- // Function definitions. @@ -170,247 +150,18 @@ void FTMotion::loop() { if (stepper.abort_current_block) { discard_planner_block_protected(); reset(); + currentGenerator->planRunout(0.0f); // Reset generator state stepper.abort_current_block = false; // Abort finished. } - while (!blockProcRdy && (stepper.current_block = planner.get_current_block())) { - if (stepper.current_block->is_sync()) { // Sync block? - if (stepper.current_block->is_sync_pos()) // Position sync? Set the position. - stepper._set_position(stepper.current_block->position); - discard_planner_block_protected(); - continue; - } - loadBlockData(stepper.current_block); - - #if ENABLED(POWER_LOSS_RECOVERY) - recovery.info.sdpos = stepper.current_block->sdpos; - recovery.info.current_position = stepper.current_block->start_position; - #endif - - blockProcRdy = true; - - // Some kinematics track axis motion in HX, HY, HZ - #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX) - stepper.last_direction_bits.hx = stepper.current_block->direction_bits.hx; - #endif - #if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX) - stepper.last_direction_bits.hy = stepper.current_block->direction_bits.hy; - #endif - #if ANY(CORE_IS_XZ, CORE_IS_YZ) - stepper.last_direction_bits.hz = stepper.current_block->direction_bits.hz; - #endif - } - - if (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(); - generateTrajectoryPointsFromBlock(); // Additional call to guarantee batchRdy is set this loop. - } - } - } - - // FBS / post processing. - if (batchRdy && !batchRdyForInterp) { - - // Call Ulendo FBS here. - - #if ENABLED(FTM_UNIFIED_BWS) - trajMod = traj; // Move the window to traj - #else - // Copy the uncompensated vectors. - #define TCOPY(A) memcpy(trajMod.A, traj.A, sizeof(trajMod.A)); - LOGICAL_AXIS_MAP_LC(TCOPY); - - // Shift the time series back in the window - #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], BATCH_SIDX_IN_WINDOW * sizeof(traj.A[0])); - LOGICAL_AXIS_MAP_LC(TSHIFT); - #endif - - // ... data is ready in trajMod. - batchRdyForInterp = true; - - 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)) - ) { - generateStepsFromTrajectory(interpIdx); - if (++interpIdx == FTM_BATCH_SIZE) { - batchRdyForInterp = false; - interpIdx = 0; - } - } + fill_stepper_plan_buffer(); // Set busy status for use by planner.busy() - busy = (stepperCmdBuffHasData || blockProcRdy || batchRdy || batchRdyForInterp); - + busy = stepping.bresenham_iterations_pending > 0 || !stepper_plan_is_empty(); } #if HAS_FTM_SHAPING - // Refresh the gains used by shaping functions. - void FTMotion::AxisShaping::set_axis_shaping_A(const ftMotionShaper_t shaper, const float zeta, const float vtol) { - - const float K = exp(-zeta * M_PI / sqrt(1.f - sq(zeta))), - K2 = sq(K), - K3 = K2 * K, - K4 = K3 * K; - - switch (shaper) { - - case ftMotionShaper_ZV: - max_i = 1U; - Ai[0] = 1.0f / (1.0f + K); - Ai[1] = Ai[0] * K; - break; - - case ftMotionShaper_ZVD: - max_i = 2U; - Ai[0] = 1.0f / (1.0f + 2.0f * K + K2); - Ai[1] = Ai[0] * 2.0f * K; - Ai[2] = Ai[0] * K2; - break; - - case ftMotionShaper_ZVDD: - max_i = 3U; - Ai[0] = 1.0f / (1.0f + 3.0f * K + 3.0f * K2 + K3); - Ai[1] = Ai[0] * 3.0f * K; - Ai[2] = Ai[0] * 3.0f * K2; - Ai[3] = Ai[0] * K3; - break; - - case ftMotionShaper_ZVDDD: - max_i = 4U; - Ai[0] = 1.0f / (1.0f + 4.0f * K + 6.0f * K2 + 4.0f * K3 + K4); - Ai[1] = Ai[0] * 4.0f * K; - Ai[2] = Ai[0] * 6.0f * K2; - Ai[3] = Ai[0] * 4.0f * K3; - Ai[4] = Ai[0] * K4; - break; - - case ftMotionShaper_EI: { - max_i = 2U; - Ai[0] = 0.25f * (1.0f + vtol); - Ai[1] = 0.50f * (1.0f - vtol) * K; - Ai[2] = Ai[0] * K2; - - const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2]); - for (uint32_t i = 0; i < 3U; i++) Ai[i] *= adj; - } break; - - case ftMotionShaper_2HEI: { - max_i = 3U; - const float vtolx2 = sq(vtol); - const float X = POW(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f); - Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X); - Ai[1] = (0.5f - Ai[0]) * K; - Ai[2] = Ai[1] * K; - Ai[3] = Ai[0] * K3; - - const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3]); - for (uint32_t i = 0; i < 4U; i++) Ai[i] *= adj; - } break; - - case ftMotionShaper_3HEI: { - max_i = 4U; - Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) ); - Ai[1] = 0.25f * ( 1.0f - vtol ) * K; - Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * Ai[0] ) * K2; - Ai[3] = Ai[1] * K2; - Ai[4] = Ai[0] * K4; - - const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3] + Ai[4]); - for (uint32_t i = 0; i < 5U; i++) Ai[i] *= adj; - } break; - - case ftMotionShaper_MZV: { - max_i = 2U; - const float Bx = 1.4142135623730950488016887242097f * K; - Ai[0] = 1.0f / (1.0f + Bx + K2); - Ai[1] = Ai[0] * Bx; - Ai[2] = Ai[0] * K2; - } - break; - - case ftMotionShaper_NONE: - max_i = 0; - Ai[0] = 1.0f; // No echoes so the whole impulse is applied in the first tap - break; - } - - } - - // Refresh the indices used by shaping functions. - // Ai[] must be precomputed (if zeta or vtol change, call set_axis_shaping_A first) - void FTMotion::AxisShaping::set_axis_shaping_N(const ftMotionShaper_t shaper, const float f, const float zeta) { - // Note that protections are omitted for DBZ and for index exceeding array length. - const float df = sqrt ( 1.f - sq(zeta) ); - switch (shaper) { - case ftMotionShaper_ZV: - Ni[1] = LROUND((0.5f / f / df) * (FTM_FS)); - break; - case ftMotionShaper_ZVD: - case ftMotionShaper_EI: - Ni[1] = LROUND((0.5f / f / df) * (FTM_FS)); - Ni[2] = Ni[1] + Ni[1]; - break; - case ftMotionShaper_ZVDD: - case ftMotionShaper_2HEI: - Ni[1] = LROUND((0.5f / f / df) * (FTM_FS)); - Ni[2] = Ni[1] + Ni[1]; - Ni[3] = Ni[2] + Ni[1]; - break; - case ftMotionShaper_ZVDDD: - case ftMotionShaper_3HEI: - Ni[1] = LROUND((0.5f / f / df) * (FTM_FS)); - Ni[2] = Ni[1] + Ni[1]; - Ni[3] = Ni[2] + Ni[1]; - Ni[4] = Ni[3] + Ni[1]; - break; - case ftMotionShaper_MZV: - Ni[1] = LROUND((0.375f / f / df) * (FTM_FS)); - Ni[2] = Ni[1] + Ni[1]; - break; - case ftMotionShaper_NONE: - // No echoes. - // max_i is set to 0 by set_axis_shaping_A, so delay centroid (Ni[0]) will also correctly be 0 - break; - } - - // Group delay in samples (i.e., Axis delay caused by shaping): sum(Ai * Ni[i]). - // Skipping i=0 since the uncompensated delay of the first impulse is always zero, so Ai[0] * Ni[0] == 0 - float centroid = 0.0f; - for (uint8_t i = 1; i <= max_i; ++i) centroid -= Ai[i] * Ni[i]; - - Ni[0] = LROUND(centroid); - - // The resulting echo index can be negative, this is ok because it will be offset - // by the max delay of all axes before it is used. - for (uint8_t i = 1; i <= max_i; ++i) Ni[i] += Ni[0]; - } - - #if ENABLED(FTM_SMOOTHING) - // Set smoothing time and recalculate alpha and delay. - void FTMotion::AxisSmoothing::set_smoothing_time(const float s_time) { - if (s_time > 0.001f) { - alpha = 1.0f - expf(-(FTM_TS) * (FTM_SMOOTHING_ORDER) / s_time ); - delay_samples = s_time * FTM_FS; - } - else { - alpha = 0.0f; - delay_samples = 0; - } - } - #endif - void FTMotion::update_shaping_params() { #define UPDATE_SHAPER(A) \ shaping.A.ena = ftMotion.cfg.shaper.A != ftMotionShaper_NONE; \ @@ -418,6 +169,7 @@ void FTMotion::loop() { shaping.A.set_axis_shaping_N(cfg.shaper.A, cfg.baseFreq.A, cfg.zeta.A); SHAPED_MAP(UPDATE_SHAPER); + shaping.refresh_largest_delay_samples(); } #endif // HAS_FTM_SHAPING @@ -427,6 +179,7 @@ void FTMotion::loop() { void FTMotion::update_smoothing_params() { #define _SMOOTH_PARAM(A) smoothing.A.set_smoothing_time(cfg.smoothingTime.A); CARTES_MAP(_SMOOTH_PARAM); + smoothing.refresh_largest_delay_samples(); } void FTMotion::set_smoothing_time(uint8_t axis, const float s_time) { @@ -443,21 +196,11 @@ void FTMotion::loop() { // Reset all trajectory processing variables. void FTMotion::reset() { const bool did_suspend = stepper.suspend(); - - stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0; - - traj.reset(); - - blockProcRdy = batchRdy = batchRdyForInterp = false; - endPos_prevBlock.reset(); tau = 0; - 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; + stepper_plan_tail = stepper_plan_head = 0; + stepping.reset(); + curr_steps_q32_32.reset(); #if HAS_FTM_SHAPING #define _RESET_ZI(A) ZERO(shaping.A.d_zi); @@ -488,6 +231,23 @@ void FTMotion::discard_planner_block_protected() { } } +uint32_t FTMotion::calc_runout_samples() { + xyze_long_t delay = {0}; + #if ENABLED(FTM_SMOOTHING) + #define _ADD(A) delay.A += smoothing.A.delay_samples; + LOGICAL_AXIS_MAP(_ADD) + #undef _ADD + #endif + + #if HAS_FTM_SHAPING + // Ni[max_i] is the delay of the last pulse, but it is relative to Ni[0] (the negative delay centroid) + #define _ADD(A) if(shaping.A.ena) delay.A += shaping.A.Ni[shaping.A.max_i] - shaping.A.Ni[0]; + SHAPED_MAP(_ADD) + #undef _ADD + #endif + return delay.large(); +} + /** * Set up a pseudo block to allow motion to settle and buffers to empty. * Called when the planner has one block left. The buffers will be filled @@ -495,33 +255,10 @@ void FTMotion::discard_planner_block_protected() { * the last position of the previous block and all ratios to zero such that no * axes' positions are incremented. */ -void FTMotion::runoutBlock() { - +void FTMotion::plan_runout_block() { startPos = endPos_prevBlock; - - 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(); - - const int32_t n_diff = n_to_settle_shaper - n_to_fill_batch, - n_to_fill_batch_after_settling = n_diff > 0 ? (FTM_BATCH_SIZE) - (n_diff % (FTM_BATCH_SIZE)) : -n_diff; - - ratio.reset(); - uint32_t max_intervals = PROP_BATCHES * (FTM_BATCH_SIZE) + n_to_settle_shaper + n_to_fill_batch_after_settling; - const float reminder_from_last_block = -tau; - const float total_duration = max_intervals * FTM_TS + reminder_from_last_block; - - // Plan a zero-motion trajectory for runout - currentGenerator->planRunout(total_duration); - - blockProcRdy = true; // since ratio is 0, the trajectory positions won't advance in any axis -} - -// Auxiliary function to get number of step commands in the buffer. -int32_t FTMotion::stepperCmdBuffItems() { - const int32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx; - return (udiff < 0) ? udiff + (FTM_STEPPERCMD_BUFF_SIZE) : udiff; + currentGenerator->planRunout(calc_runout_samples() * FTM_TS); + ratio.reset(); // setting ratio to zero means no motion on any axis } // Initializes storage variables before startup. @@ -541,259 +278,266 @@ void FTMotion::setTrajectoryType(const TrajectoryType type) { case TrajectoryType::POLY5: currentGenerator = &poly5Generator; break; case TrajectoryType::POLY6: currentGenerator = &poly6Generator; break; } - currentGenerator->reset(); // Reset the selected generator } // Load / convert block data from planner to fixed-time control variables. // Called from FTMotion::loop() at the fetch of the next planner block. -void FTMotion::loadBlockData(block_t * const current_block) { - // Cache the extruder index for this block - TERN_(DISTINCT_E_FACTORS, block_extruder_axis = E_AXIS_N(current_block->extruder)); +// Return whether a plan is available. +bool FTMotion::plan_next_block() { + while (true) { - const float totalLength = current_block->millimeters, - oneOverLength = 1.0f / totalLength; + const bool had_block = !!stepper.current_block; + discard_planner_block_protected(); // Always clears stepper.current_block... + block_t * const current_block = planner.get_current_block(); // ...so get the current block from the queue - startPos = endPos_prevBlock; - const xyze_pos_t& moveDist = current_block->dist_mm; - ratio = moveDist * oneOverLength; + // The planner had a block and there was not another one? + const bool planner_finished = had_block && !current_block; + if (planner_finished) { + plan_runout_block(); + return true; + } - const float mmps = totalLength / current_block->step_event_count; // (mm/step) Distance for each step - const float initial_speed = mmps * current_block->initial_rate; // (mm/s) Start feedrate - const float final_speed = mmps * current_block->final_rate; // (mm/s) End feedrate + // There was never a block? Run out the plan and bail. + if (!current_block) { + currentGenerator->planRunout(0); + return false; + } - // Plan the trajectory using the trajectory generator - currentGenerator->plan(initial_speed, final_speed, current_block->acceleration, current_block->nominal_speed, totalLength); + // Fetching this block for Stepper and for this loop + stepper.current_block = current_block; - // Accel + Coasting + Decel + datapoints - const float reminder_from_last_block = - tau; + // Handle sync blocks and skip others + if (current_block->is_sync()) { + if (current_block->is_sync_pos()) stepper._set_position(current_block->position); + continue; + } - endPos_prevBlock += moveDist; - - TERN_(FTM_HAS_LIN_ADVANCE, use_advance_lead = current_block->use_advance_lead); - - // Watch endstops until the move ends - const float total_duration = currentGenerator->getTotalDuration(); - uint32_t max_intervals = ceil((total_duration + reminder_from_last_block) * FTM_FS); - const millis_t move_end_ti = millis() + SEC_TO_MS((FTM_TS) * float(max_intervals + num_samples_shaper_settle() + ((PROP_BATCHES) + 1) * (FTM_BATCH_SIZE)) + (float(FTM_STEPPERCMD_BUFF_SIZE) / float(FTM_STEPPER_FS))); - - #define _SET_MOVE_END(A) do{ \ - if (moveDist.A) { \ - axis_move_end_ti.A = move_end_ti; \ - axis_move_dir.A = moveDist.A > 0; \ - } \ - }while(0); - - LOGICAL_AXIS_MAP(_SET_MOVE_END); -} - -// Generate data points of the trajectory. -// Called from FTMotion::loop() at the fetch of a new planner block, after loadBlockData. -void FTMotion::generateTrajectoryPointsFromBlock() { - const float total_duration = currentGenerator->getTotalDuration(); - if (tau + FTM_TS > total_duration) { - // TODO: refactor code so this thing is not twice. - // the reason of it being in the beginning, is that a block can be so short that it has - // zero trajectories. - // the next iteration will fall beyond this block - blockProcRdy = false; - traj_idx_get = 0; - tau -= total_duration; - return; - } - do { - tau += FTM_TS; // (s) Time since start of block - // If the end of the last block doesn't exactly land on a trajectory index, - // tau can start negative, but it always holds that `tau > -FTM_TS` - - // Get distance from trajectory generator - const float dist = currentGenerator->getDistanceAtTime(tau); - - #define _SET_TRAJ(q) traj.q[traj_idx_set] = startPos.q + ratio.q * dist; - LOGICAL_AXIS_MAP_LC(_SET_TRAJ); - - #if FTM_HAS_LIN_ADVANCE - const float advK = planner.get_advance_k(); - if (advK) { - float traj_e = traj.e[traj_idx_set]; - if (use_advance_lead) { - // Don't apply LA to retract/unretract blocks - float e_rate = (traj_e - prev_traj_e) * (FTM_FS); - traj.e[traj_idx_set] += e_rate * advK; - } - prev_traj_e = traj_e; - } + #if ENABLED(POWER_LOSS_RECOVERY) + recovery.info.sdpos = current_block->sdpos; + recovery.info.current_position = current_block->start_position; #endif - // Update shaping parameters if needed. + // Some kinematics track axis motion in HX, HY, HZ + #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX) + stepper.last_direction_bits.hx = current_block->direction_bits.hx; + #endif + #if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX) + stepper.last_direction_bits.hy = current_block->direction_bits.hy; + #endif + #if ANY(CORE_IS_XZ, CORE_IS_YZ) + stepper.last_direction_bits.hz = current_block->direction_bits.hz; + #endif - switch (cfg.dynFreqMode) { + // Cache the extruder index for this block + TERN_(DISTINCT_E_FACTORS, block_extruder_axis = E_AXIS_N(current_block->extruder)); - #if HAS_DYNAMIC_FREQ_MM - case dynFreqMode_Z_BASED: { - static float oldz = 0.0f; - const float z = traj.z[traj_idx_set]; - if (z != oldz) { // Only update if Z changed. - oldz = z; - #if HAS_X_AXIS - const float xf = cfg.baseFreq.x + cfg.dynFreqK.x * z; - shaping.X.set_axis_shaping_N(cfg.shaper.x, _MAX(xf, FTM_MIN_SHAPE_FREQ), cfg.zeta.x); - #endif - #if HAS_Y_AXIS - const float yf = cfg.baseFreq.y + cfg.dynFreqK.y * z; - shaping.Y.set_axis_shaping_N(cfg.shaper.y, _MAX(yf, FTM_MIN_SHAPE_FREQ), cfg.zeta.y); - #endif - } - } break; - #endif + const float totalLength = current_block->millimeters; - #if HAS_DYNAMIC_FREQ_G - case dynFreqMode_MASS_BASED: - // Update constantly. The optimization done for Z value makes - // less sense for E, as E is expected to constantly change. + startPos = endPos_prevBlock; + const xyze_pos_t& moveDist = current_block->dist_mm; + ratio = moveDist / totalLength; + + const float mmps = totalLength / current_block->step_event_count, // (mm/step) Distance for each step + initial_speed = mmps * current_block->initial_rate, // (mm/s) Start feedrate + final_speed = mmps * current_block->final_rate; // (mm/s) End feedrate + + // Plan the trajectory using the trajectory generator + currentGenerator->plan(initial_speed, final_speed, current_block->acceleration, current_block->nominal_speed, totalLength); + + endPos_prevBlock += moveDist; + + TERN_(FTM_HAS_LIN_ADVANCE, use_advance_lead = current_block->use_advance_lead); + + // Watch endstops until the move ends + const millis_t move_end_ti = millis() + \ + stepper_plan_count() * FTM_TS + // Time to empty stepper command buffer + SEC_TO_MS(currentGenerator->getTotalDuration()) + // Time to finish this block + SEC_TO_MS((FTM_TS) * calc_runout_samples()); // Time for a rounout block + + #define _SET_MOVE_END(A) do{ \ + if (moveDist.A) { \ + axis_move_end_ti.A = move_end_ti; \ + axis_move_dir.A = moveDist.A > 0; \ + } \ + }while(0); + + LOGICAL_AXIS_MAP(_SET_MOVE_END); + + return true; + } +} + +xyze_float_t FTMotion::calc_traj_point(const float dist) { + xyze_float_t traj_coords; + #define _SET_TRAJ(q) traj_coords.q = startPos.q + ratio.q * dist; + LOGICAL_AXIS_MAP_LC(_SET_TRAJ); + + #if FTM_HAS_LIN_ADVANCE + const float advK = planner.get_advance_k(); + if (advK) { + const float traj_e = traj_coords.e; + if (use_advance_lead) { + // Don't apply LA to retract/unretract blocks + const float e_rate = (traj_e - prev_traj_e) * (FTM_FS); + traj_coords.e += e_rate * advK; + } + prev_traj_e = traj_e; + } + #endif + + // Update shaping parameters if needed. + switch (cfg.dynFreqMode) { + #if HAS_DYNAMIC_FREQ_MM + case dynFreqMode_Z_BASED: { + static float oldz = 0.0f; + const float z = traj_coords.z; + if (z != oldz) { // Only update if Z changed. + oldz = z; #if HAS_X_AXIS - shaping.X.set_axis_shaping_N(cfg.shaper.x, cfg.baseFreq.x + cfg.dynFreqK.x * traj.e[traj_idx_set], cfg.zeta.x); + const float xf = cfg.baseFreq.x + cfg.dynFreqK.x * z; + shaping.X.set_axis_shaping_N(cfg.shaper.x, _MAX(xf, FTM_MIN_SHAPE_FREQ), 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[traj_idx_set], cfg.zeta.y); + const float yf = cfg.baseFreq.y + cfg.dynFreqK.y * z; + shaping.Y.set_axis_shaping_N(cfg.shaper.y, _MAX(yf, FTM_MIN_SHAPE_FREQ), cfg.zeta.y); #endif - break; - #endif - - default: break; - } - uint32_t max_total_delay = 0; - - #if ENABLED(FTM_SMOOTHING) - #define _SMOOTHEN(A) /* Approximate gaussian smoothing via chained EMAs */ \ - if (smoothing.A.alpha > 0.0f) { \ - float smooth_val = traj.A[traj_idx_set]; \ - for (uint8_t _i = 0; _i < FTM_SMOOTHING_ORDER; ++_i) { \ - smoothing.A.smoothing_pass[_i] += (smooth_val - smoothing.A.smoothing_pass[_i]) * smoothing.A.alpha; \ - smooth_val = smoothing.A.smoothing_pass[_i]; \ - } \ - traj.A[traj_idx_set] = smooth_val; \ + shaping.refresh_largest_delay_samples(); } + } break; + #endif - CARTES_MAP(_SMOOTHEN); - max_total_delay += _MAX(CARTES_LIST( - smoothing.X.delay_samples, smoothing.Y.delay_samples, - smoothing.Z.delay_samples, smoothing.E.delay_samples - )); + #if HAS_DYNAMIC_FREQ_G + case dynFreqMode_MASS_BASED: + // 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_coords.e, cfg.zeta.x); + #endif + #if HAS_Y_AXIS + shaping.Y.set_axis_shaping_N(cfg.shaper.y, cfg.baseFreq.y + cfg.dynFreqK.y * traj_coords.e, cfg.zeta.y); + #endif + shaping.refresh_largest_delay_samples(); + break; + #endif - #endif // FTM_SMOOTHING + default: break; + } - #if HAS_FTM_SHAPING + #if ANY(FTM_SMOOTHING, HAS_FTM_SHAPING) + uint32_t max_total_delay = 0; + #endif - if (ftMotion.cfg.axis_sync_enabled) { - max_total_delay -= _MIN(SHAPED_LIST( - shaping.X.Ni[0], shaping.Y.Ni[0], - shaping.Z.Ni[0], shaping.E.Ni[0] - )); + #if ENABLED(FTM_SMOOTHING) + + #define _SMOOTHEN(A) /* Approximate gaussian smoothing via chained EMAs */ \ + if (smoothing.A.alpha > 0.0f) { \ + float smooth_val = traj_coords.A; \ + for (uint8_t _i = 0; _i < FTM_SMOOTHING_ORDER; ++_i) { \ + smoothing.A.smoothing_pass[_i] += (smooth_val - smoothing.A.smoothing_pass[_i]) * smoothing.A.alpha; \ + smooth_val = smoothing.A.smoothing_pass[_i]; \ + } \ + traj_coords.A = smooth_val; \ } - // Apply shaping if active on each axis - #define _SHAPE(A) \ - do { \ - const uint32_t group_delay = ftMotion.cfg.axis_sync_enabled \ - ? max_total_delay - TERN0(FTM_SMOOTHING, smoothing.A.delay_samples) \ - : -shaping.A.Ni[0]; \ - /* α=1−exp(−(dt / (τ / order))) */ \ - shaping.A.d_zi[shaping.zi_idx] = traj.A[traj_idx_set]; \ - traj.A[traj_idx_set] = 0; \ - for (uint32_t i = 0; i <= shaping.A.max_i; i++) { \ - /* echo_delay is always positive since Ni[i] = echo_relative_delay - group_delay + max_total_delay */ \ - /* where echo_relative_delay > 0 and group_delay ≤ max_total_delay */ \ - const uint32_t echo_delay = group_delay + shaping.A.Ni[i]; \ - int32_t udiff = shaping.zi_idx - echo_delay; \ - if (udiff < 0) udiff += FTM_ZMAX; \ - traj.A[traj_idx_set] += shaping.A.Ai[i] * shaping.A.d_zi[udiff]; \ - } \ - } while (0); + CARTES_MAP(_SMOOTHEN); + max_total_delay += smoothing.largest_delay_samples; - SHAPED_MAP(_SHAPE); + #endif // FTM_SMOOTHING - if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0; + #if HAS_FTM_SHAPING - #endif // HAS_FTM_SHAPING + if (ftMotion.cfg.axis_sync_enabled) + max_total_delay += shaping.largest_delay_samples; - // Filled up the queue with regular and shaped steps - if (++traj_idx_set == FTM_WINDOW_SIZE) { - traj_idx_set = BATCH_SIDX_IN_WINDOW; - batchRdy = true; - } - traj_idx_get++; - if (tau + FTM_TS > total_duration) { - // the next iteration will fall beyond this block - blockProcRdy = false; - traj_idx_get = 0; - tau -= total_duration; - } - } while (blockProcRdy && !batchRdy); -} // generateTrajectoryPointsFromBlock + // Apply shaping if active on each axis + #define _SHAPE(A) \ + do { \ + const uint32_t group_delay = ftMotion.cfg.axis_sync_enabled \ + ? max_total_delay - TERN0(FTM_SMOOTHING, smoothing.A.delay_samples) \ + : -shaping.A.Ni[0]; \ + /* α=1−exp(−(dt / (τ / order))) */ \ + shaping.A.d_zi[shaping.zi_idx] = traj_coords.A; \ + traj_coords.A = 0; \ + for (uint32_t i = 0; i <= shaping.A.max_i; i++) { \ + /* echo_delay is always positive since Ni[i] = echo_relative_delay - group_delay + max_total_delay */ \ + /* where echo_relative_delay > 0 and group_delay ≤ max_total_delay */ \ + const uint32_t echo_delay = group_delay + shaping.A.Ni[i]; \ + int32_t udiff = shaping.zi_idx - echo_delay; \ + if (udiff < 0) udiff += FTM_ZMAX; \ + traj_coords.A += shaping.A.Ai[i] * shaping.A.d_zi[udiff]; \ + } \ + } while (0); + + SHAPED_MAP(_SHAPE); + + if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0; + + #endif // HAS_FTM_SHAPING + + return traj_coords; +} + +stepper_plan_t FTMotion::calc_stepper_plan(xyze_float_t traj_coords) { + // 1) Convert trajectory to step delta + #define _TOSTEPS_q32(A, B) int64_t(traj_coords.A * planner.settings.axis_steps_per_mm[B] * (1ull << 32)) + XYZEval next_steps_q32_32 = LOGICAL_AXIS_ARRAY( + _TOSTEPS_q32(e, block_extruder_axis), + _TOSTEPS_q32(x, X_AXIS), _TOSTEPS_q32(y, Y_AXIS), _TOSTEPS_q32(z, Z_AXIS), + _TOSTEPS_q32(i, I_AXIS), _TOSTEPS_q32(j, J_AXIS), _TOSTEPS_q32(k, K_AXIS), + _TOSTEPS_q32(u, U_AXIS), _TOSTEPS_q32(v, V_AXIS), _TOSTEPS_q32(w, W_AXIS) + ); + #undef _TOSTEPS_q32 + + constexpr uint32_t ITERATIONS_PER_TRAJ_INV_uq0_32 = (1ull << 32) / ITERATIONS_PER_TRAJ; + stepper_plan_t stepper_plan; + + #define _RUN_AXIS(A) do{ \ + int64_t delta_q32_32 = (next_steps_q32_32.A - curr_steps_q32_32.A); \ + /* 2) Set stepper plan direction bits */ \ + int16_t sign = (delta_q32_32 > 0) - (delta_q32_32 < 0); \ + stepper_plan.dir_bits.A = delta_q32_32 > 0; \ + /* 3) Set per-iteration advance dividend Q0.32 */ \ + uint64_t delta_uq32_32 = ABS(delta_q32_32); \ + /* dividend = delta_q32_32 / ITERATIONS_PER_TRAJ, but avoiding division and an intermediate int128 */ \ + /* Note the integer part would overflow if there is eq or more than 1 steps per isr */ \ + uint32_t integer_part = (delta_uq32_32 >> 32) * ITERATIONS_PER_TRAJ_INV_uq0_32; \ + uint32_t fractional_part = ((delta_uq32_32 & UINT32_MAX) * ITERATIONS_PER_TRAJ_INV_uq0_32) >> 32; \ + stepper_plan.advance_dividend_q0_32.A = integer_part + fractional_part; \ + /* 4) Advance curr_steps by the exact integer steps that Bresenham will emit */ \ + /* It's like doing current_steps = next_steps, but considering any fractional error */ \ + /* from the dividend. This way there can be no drift. */ \ + curr_steps_q32_32.A += (int64_t)stepper_plan.advance_dividend_q0_32.A * sign * ITERATIONS_PER_TRAJ; \ + } while(0); + LOGICAL_AXIS_MAP(_RUN_AXIS); + #undef _RUN_AXIS + + return stepper_plan; +} /** - * @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. + * Generate stepper data of the trajectory. + * Called from FTMotion::loop() */ -void FTMotion::generateStepsFromTrajectory(const uint32_t idx) { - constexpr float INV_FTM_STEPS_PER_UNIT_TIME = 1.0f / (FTM_STEPS_PER_UNIT_TIME); +void FTMotion::fill_stepper_plan_buffer() { + while (!stepper_plan_is_full()) { + float total_duration = currentGenerator->getTotalDuration(); // if the current plan is empty, it will have zero duration. + while (tau + FTM_TS > total_duration) { + // Previous block plan consumed, try to get the next one. + tau -= total_duration; // The exact end of the last block may be in-between trajectory points, so the next one may start anywhere of (-FTM_TS, 0]. + const bool plan_available = plan_next_block(); + if (!plan_available) return; + total_duration = currentGenerator->getTotalDuration(); + } + tau += FTM_TS; // (s) Time since start of block - // 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 ) + // Get distance from trajectory generator + xyze_float_t traj_coords = calc_traj_point(currentGenerator->getDistanceAtTime(tau)); - 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) - ); + stepper_plan_t plan = calc_stepper_plan(traj_coords); - // Fixed-point denominator for step accumulation - constexpr int32_t denom_q10 = (FTM_STEPS_PER_UNIT_TIME) << 10; + // Store in buffer + enqueue_stepper_plan(plan); - // 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); - - 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]; - - // Init the command to no STEP (Reverse DIR) - cmd = 0; - - // Accumulate the "error" for all axes according the fixed-point distance - step_error_q10 += delta_q10; - - // 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; } } diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index a24f3271cf..55798adc6c 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -25,12 +25,19 @@ #include "planner.h" // Access block type from planner. #include "stepper.h" // For stepper motion and direction -#include "ft_types.h" #include "ft_motion/trajectory_generator.h" #include "ft_motion/trapezoidal_trajectory_generator.h" #include "ft_motion/poly5_trajectory_generator.h" #include "ft_motion/poly6_trajectory_generator.h" +#if HAS_FTM_SHAPING + #include "ft_motion/shaping.h" +#endif +#if ENABLED(FTM_SMOOTHING) + #include "ft_motion/smoothing.h" +#endif +#include "ft_motion/stepping.h" + #define FTM_VERSION 2 // Change version when hosts need to know #if HAS_X_AXIS && (HAS_Z_AXIS || HAS_EXTRUDERS) @@ -60,10 +67,6 @@ typedef struct FTConfig { ft_shaped_float_t vtol = // Vibration Level SHAPED_ARRAY(FTM_SHAPING_V_TOL_X, FTM_SHAPING_V_TOL_Y, FTM_SHAPING_V_TOL_Z, FTM_SHAPING_V_TOL_E); - #if ENABLED(FTM_SMOOTHING) - ft_smoothed_float_t smoothingTime; // Smoothing time. [s] - #endif - #if HAS_DYNAMIC_FREQ dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration. ft_shaped_float_t dynFreqK = { 0.0f }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g] @@ -73,6 +76,10 @@ typedef struct FTConfig { #endif // HAS_FTM_SHAPING + #if ENABLED(FTM_SMOOTHING) + ft_smoothed_float_t smoothingTime; // Smoothing time. [s] + #endif + TrajectoryType trajectory_type = TrajectoryType::FTM_TRAJECTORY_TYPE; // Trajectory generator type float poly6_acceleration_overshoot; // Overshoot factor for Poly6 (1.25 to 2.0) } ft_config_t; @@ -128,12 +135,6 @@ class FTMotion { reset(); } - static ft_command_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of stepper commands. - 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 stepperCmdBuffHasData; // The stepper buffer has items and is in use. - static XYZEval axis_move_end_ti; static AxisBits axis_move_dir; @@ -155,6 +156,7 @@ class FTMotion { static void reset(); // Reset all states of the fixed time conversion to defaults. + // Safely toggle the active state of FT Motion static bool toggle() { stepper.ftMotion_syncPosition(); FLIP(cfg.active); @@ -173,14 +175,33 @@ class FTMotion { return cfg.active ? axis_move_dir[axis] : stepper.last_direction_bits[axis]; } + + static stepping_t stepping; + FORCE_INLINE static bool stepper_plan_is_empty() { + return stepper_plan_head == stepper_plan_tail; + } + FORCE_INLINE static bool stepper_plan_is_full() { + return ((stepper_plan_head + 1) & FTM_BUFFER_MASK) == stepper_plan_tail; + } + FORCE_INLINE static uint32_t stepper_plan_count() { + return (stepper_plan_head - stepper_plan_tail) & FTM_BUFFER_MASK; + } + // Enqueue a plan + FORCE_INLINE static void enqueue_stepper_plan(const stepper_plan_t& d) { + stepper_plan_buff[stepper_plan_head] = d; + stepper_plan_head = (stepper_plan_head + 1u) & FTM_BUFFER_MASK; + } + // Dequeue a plan. + // Zero-copy consume; caller must use it before next dequeue if they keep a ref. + // Done like this to avoid double copy. + // e.g do: stepper_plan_t data = dequeue_stepper_plan(); this is ok + FORCE_INLINE static stepper_plan_t& dequeue_stepper_plan() { + const uint32_t i = stepper_plan_tail; + stepper_plan_tail = (i + 1u) & FTM_BUFFER_MASK; + return stepper_plan_buff[i]; + } + private: - - static xyze_trajectory_t traj; - static xyze_trajectoryMod_t trajMod; - - static bool blockProcRdy; - static bool batchRdy, batchRdyForInterp; - // Block data variables. static xyze_pos_t startPos, // (mm) Start position of block endPos_prevBlock; // (mm) End position of previous block @@ -194,19 +215,6 @@ class FTMotion { static TrajectoryGenerator* currentGenerator; static TrajectoryType trajectoryType; - // 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; - - // 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 #elif HAS_EXTRUDERS @@ -215,42 +223,12 @@ class FTMotion { #endif #if HAS_FTM_SHAPING - // Shaping data - typedef struct AxisShaping { - bool ena = false; // Enabled indication - float d_zi[FTM_ZMAX] = { 0.0f }; // Data point delay vector - float Ai[5]; // Shaping gain vector - int32_t Ni[5]; // Shaping time index vector - uint32_t max_i; // Vector length for the selected shaper - - void set_axis_shaping_N(const ftMotionShaper_t shaper, const float f, const float zeta); // Sets the gains used by shaping functions. - void set_axis_shaping_A(const ftMotionShaper_t shaper, const float zeta, const float vtol); // Sets the indices used by shaping functions. - - } axis_shaping_t; - - typedef struct Shaping { - uint32_t zi_idx; // Index of storage in the data point delay vectors. - axis_shaping_t SHAPED_AXIS_NAMES; - } shaping_t; - static shaping_t shaping; // Shaping data - - #endif // HAS_FTM_SHAPING + #endif #if ENABLED(FTM_SMOOTHING) - // Smoothing data for each axis - typedef struct AxisSmoothing { - float smoothing_pass[FTM_SMOOTHING_ORDER] = { 0.0f }; // Last value of each of the exponential smoothing passes - float alpha = 0.0f; // Pre-calculated alpha for smoothing. - uint32_t delay_samples = 0; // Pre-calculated delay in samples for smoothing. - void set_smoothing_time(const float s_time); // Set smoothing time, recalculate alpha and delay. - } axis_smoothing_t; - // Smoothing data for XYZE axes - typedef struct Smoothing { - axis_smoothing_t CARTES_AXIS_NAMES; - } smoothing_t; - static smoothing_t smoothing; // Smoothing data + static smoothing_t smoothing; #endif // Linear advance variables. @@ -258,20 +236,18 @@ class FTMotion { static float prev_traj_e; #endif - // Private methods + // Buffers static void discard_planner_block_protected(); - static void runoutBlock(); - static int32_t stepperCmdBuffItems(); - static void loadBlockData(block_t *const current_block); - static void generateTrajectoryPointsFromBlock(); - static void generateStepsFromTrajectory(const uint32_t idx); - - FORCE_INLINE static int32_t num_samples_shaper_settle() { - #define _OR_ENA(A) || shaping.A.ena - return false SHAPED_MAP(_OR_ENA) ? FTM_ZMAX : 0; - #undef _OR_ENA - } - + static uint32_t calc_runout_samples(); + static void plan_runout_block(); + static void fill_stepper_plan_buffer(); + static xyze_float_t calc_traj_point(const float dist); + static stepper_plan_t calc_stepper_plan(xyze_float_t delta); + static bool plan_next_block(); + // stepper_plan buffer variables. + static stepper_plan_t stepper_plan_buff[FTM_BUFFER_SIZE]; + static uint32_t stepper_plan_tail, stepper_plan_head; + static XYZEval curr_steps_q32_32; }; // class FTMotion extern FTMotion ftMotion; // Use ftMotion.thing, not FTMotion::thing. diff --git a/Marlin/src/module/ft_motion/shaping.cpp b/Marlin/src/module/ft_motion/shaping.cpp new file mode 100644 index 0000000000..a077ce99b8 --- /dev/null +++ b/Marlin/src/module/ft_motion/shaping.cpp @@ -0,0 +1,170 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(FT_MOTION) + +#include "shaping.h" + +// Refresh the gains used by shaping functions. +void AxisShaping::set_axis_shaping_A(const ftMotionShaper_t shaper, const float zeta, const float vtol) { + + const float K = exp(-zeta * M_PI / sqrt(1.f - sq(zeta))), + K2 = sq(K), + K3 = K2 * K, + K4 = K3 * K; + + switch (shaper) { + + case ftMotionShaper_ZV: + max_i = 1U; + Ai[0] = 1.0f / (1.0f + K); + Ai[1] = Ai[0] * K; + break; + + case ftMotionShaper_ZVD: + max_i = 2U; + Ai[0] = 1.0f / (1.0f + 2.0f * K + K2); + Ai[1] = Ai[0] * 2.0f * K; + Ai[2] = Ai[0] * K2; + break; + + case ftMotionShaper_ZVDD: + max_i = 3U; + Ai[0] = 1.0f / (1.0f + 3.0f * K + 3.0f * K2 + K3); + Ai[1] = Ai[0] * 3.0f * K; + Ai[2] = Ai[0] * 3.0f * K2; + Ai[3] = Ai[0] * K3; + break; + + case ftMotionShaper_ZVDDD: + max_i = 4U; + Ai[0] = 1.0f / (1.0f + 4.0f * K + 6.0f * K2 + 4.0f * K3 + K4); + Ai[1] = Ai[0] * 4.0f * K; + Ai[2] = Ai[0] * 6.0f * K2; + Ai[3] = Ai[0] * 4.0f * K3; + Ai[4] = Ai[0] * K4; + break; + + case ftMotionShaper_EI: { + max_i = 2U; + Ai[0] = 0.25f * (1.0f + vtol); + Ai[1] = 0.50f * (1.0f - vtol) * K; + Ai[2] = Ai[0] * K2; + + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2]); + for (uint32_t i = 0; i < 3U; i++) Ai[i] *= adj; + } break; + + case ftMotionShaper_2HEI: { + max_i = 3U; + const float vtolx2 = sq(vtol); + const float X = POW(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f); + Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X); + Ai[1] = (0.5f - Ai[0]) * K; + Ai[2] = Ai[1] * K; + Ai[3] = Ai[0] * K3; + + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3]); + for (uint32_t i = 0; i < 4U; i++) Ai[i] *= adj; + } break; + + case ftMotionShaper_3HEI: { + max_i = 4U; + Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) ); + Ai[1] = 0.25f * ( 1.0f - vtol ) * K; + Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * Ai[0] ) * K2; + Ai[3] = Ai[1] * K2; + Ai[4] = Ai[0] * K4; + + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3] + Ai[4]); + for (uint32_t i = 0; i < 5U; i++) Ai[i] *= adj; + } break; + + case ftMotionShaper_MZV: { + max_i = 2U; + const float Bx = 1.4142135623730950488016887242097f * K; + Ai[0] = 1.0f / (1.0f + Bx + K2); + Ai[1] = Ai[0] * Bx; + Ai[2] = Ai[0] * K2; + } + break; + + case ftMotionShaper_NONE: + max_i = 0; + Ai[0] = 1.0f; // No echoes so the whole impulse is applied in the first tap + break; + } + +} + +// Refresh the indices used by shaping functions. +// Ai[] must be precomputed (if zeta or vtol change, call set_axis_shaping_A first) +void AxisShaping::set_axis_shaping_N(const ftMotionShaper_t shaper, const float f, const float zeta) { + // Note that protections are omitted for DBZ and for index exceeding array length. + const float df = sqrt ( 1.f - sq(zeta) ); + switch (shaper) { + case ftMotionShaper_ZV: + Ni[1] = LROUND((0.5f / f / df) * (FTM_FS)); + break; + case ftMotionShaper_ZVD: + case ftMotionShaper_EI: + Ni[1] = LROUND((0.5f / f / df) * (FTM_FS)); + Ni[2] = Ni[1] + Ni[1]; + break; + case ftMotionShaper_ZVDD: + case ftMotionShaper_2HEI: + Ni[1] = LROUND((0.5f / f / df) * (FTM_FS)); + Ni[2] = Ni[1] + Ni[1]; + Ni[3] = Ni[2] + Ni[1]; + break; + case ftMotionShaper_ZVDDD: + case ftMotionShaper_3HEI: + Ni[1] = LROUND((0.5f / f / df) * (FTM_FS)); + Ni[2] = Ni[1] + Ni[1]; + Ni[3] = Ni[2] + Ni[1]; + Ni[4] = Ni[3] + Ni[1]; + break; + case ftMotionShaper_MZV: + Ni[1] = LROUND((0.375f / f / df) * (FTM_FS)); + Ni[2] = Ni[1] + Ni[1]; + break; + case ftMotionShaper_NONE: + // No echoes. + // max_i is set to 0 by set_axis_shaping_A, so delay centroid (Ni[0]) will also correctly be 0 + break; + } + + // Group delay in samples (i.e., Axis delay caused by shaping): sum(Ai * Ni[i]). + // Skipping i=0 since the uncompensated delay of the first impulse is always zero, so Ai[0] * Ni[0] == 0 + float centroid = 0.0f; + for (uint8_t i = 1; i <= max_i; ++i) centroid -= Ai[i] * Ni[i]; + + Ni[0] = LROUND(centroid); + + // The resulting echo index can be negative, this is ok because it will be offset + // by the max delay of all axes before it is used. + for (uint8_t i = 1; i <= max_i; ++i) Ni[i] += Ni[0]; +} + +#endif // FT_MOTION diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_motion/shaping.h similarity index 67% rename from Marlin/src/module/ft_types.h rename to Marlin/src/module/ft_motion/shaping.h index 86b37997e9..4a62acddcc 100644 --- a/Marlin/src/module/ft_types.h +++ b/Marlin/src/module/ft_motion/shaping.h @@ -1,6 +1,6 @@ /** * Marlin 3D Printer Firmware - * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * * Based on Sprinter and grbl. * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm @@ -21,7 +21,7 @@ */ #pragma once -#include "../core/types.h" +#include "../../inc/MarlinConfig.h" enum ftMotionShaper_t : uint8_t { ftMotionShaper_NONE = 0, // No compensator @@ -44,22 +44,6 @@ enum dynFreqMode_t : uint8_t { #define AXIS_IS_SHAPING(A) TERN0(FTM_SHAPER_##A, (ftMotion.cfg.shaper.A != ftMotionShaper_NONE)) #define AXIS_IS_EISHAPING(A) TERN0(FTM_SHAPER_##A, WITHIN(ftMotion.cfg.shaper.A, ftMotionShaper_EI, ftMotionShaper_3HEI)) -typedef struct XYZEarray xyze_trajectory_t; -typedef struct XYZEarray xyze_trajectoryMod_t; - -// TODO: Convert ft_command_t to a struct with bitfields instead of using a primitive type -enum { - LOGICAL_AXIS_PAIRED_LIST( - FT_BIT_DIR_E, FT_BIT_STEP_E, - FT_BIT_DIR_X, FT_BIT_STEP_X, FT_BIT_DIR_Y, FT_BIT_STEP_Y, FT_BIT_DIR_Z, FT_BIT_STEP_Z, - FT_BIT_DIR_I, FT_BIT_STEP_I, FT_BIT_DIR_J, FT_BIT_STEP_J, FT_BIT_DIR_K, FT_BIT_STEP_K, - FT_BIT_DIR_U, FT_BIT_STEP_U, FT_BIT_DIR_V, FT_BIT_STEP_V, FT_BIT_DIR_W, FT_BIT_STEP_W - ), - FT_BIT_COUNT -}; - -typedef bits_t(FT_BIT_COUNT) ft_command_t; - // Emitters for code that only cares about shaped XYZE #if HAS_FTM_SHAPING #define NUM_AXES_SHAPED COUNT_ENABLED(HAS_X_AXIS, HAS_Y_AXIS, FTM_SHAPER_Z, FTM_SHAPER_E) @@ -104,8 +88,30 @@ typedef FTShapedAxes ft_shaped_float_t; typedef FTShapedAxes ft_shaped_shaper_t; typedef FTShapedAxes ft_shaped_dfm_t; -#if ENABLED(FTM_SMOOTHING) - typedef struct FTSmoothedAxes { - float CARTES_AXIS_NAMES; - } ft_smoothed_float_t; -#endif + +// Shaping data +typedef struct AxisShaping { + bool ena = false; // Enabled indication + float d_zi[FTM_ZMAX] = { 0.0f }; // Data point delay vector + float Ai[5]; // Shaping gain vector + int32_t Ni[5]; // Shaping time index vector + uint32_t max_i; // Vector length for the selected shaper + + // Set the gains used by shaping functions + void set_axis_shaping_N(const ftMotionShaper_t shaper, const float f, const float zeta); + + // Set the indices (per pulse delays) used by shaping functions + void set_axis_shaping_A(const ftMotionShaper_t shaper, const float zeta, const float vtol); + +} axis_shaping_t; + +typedef struct Shaping { + uint32_t zi_idx; // Index of storage in the data point delay vectors. + axis_shaping_t SHAPED_AXIS_NAMES; + uint32_t largest_delay_samples; + // Shaping an axis makes it lag with respect to the others by certain amount, the "centroid delay" + // Ni[0] stores how far in the past the first step would need to happen to avoid desynchronisation (it is therefore negative). + // Of course things can't be done in the past, so when shaping is applied, the all axes are delayed by largest_delay_samples + // minus their own centroid delay. This makes them all be equally delayed and therefore in synch. + void refresh_largest_delay_samples() { largest_delay_samples = -_MIN(SHAPED_LIST(X.Ni[0], Y.Ni[0], Z.Ni[0], E.Ni[0])); } +} shaping_t; diff --git a/Marlin/src/module/ft_motion/smoothing.cpp b/Marlin/src/module/ft_motion/smoothing.cpp new file mode 100644 index 0000000000..f0016391b3 --- /dev/null +++ b/Marlin/src/module/ft_motion/smoothing.cpp @@ -0,0 +1,41 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(FTM_SMOOTHING) + +#include "smoothing.h" + +// Set smoothing time and recalculate alpha and delay. +void AxisSmoothing::set_smoothing_time(const float s_time) { + if (s_time > 0.001f) { + alpha = 1.0f - expf(-(FTM_TS) * (FTM_SMOOTHING_ORDER) / s_time ); + delay_samples = s_time * FTM_FS; + } + else { + alpha = 0.0f; + delay_samples = 0; + } +} + +#endif // FTM_SMOOTHING diff --git a/Marlin/src/module/ft_motion/smoothing.h b/Marlin/src/module/ft_motion/smoothing.h new file mode 100644 index 0000000000..ae8c088b30 --- /dev/null +++ b/Marlin/src/module/ft_motion/smoothing.h @@ -0,0 +1,47 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../../inc/MarlinConfig.h" + +typedef struct FTSmoothedAxes { + float CARTES_AXIS_NAMES; +} ft_smoothed_float_t; + +// Smoothing data for each axis +// The smoothing algorithm used is an approximation of moving window averaging with gaussian weights, based +// on chained exponential smoothers. +typedef struct AxisSmoothing { + float smoothing_pass[FTM_SMOOTHING_ORDER] = { 0.0f }; // Last value of each of the exponential smoothing passes + float alpha = 0.0f; // Pre-calculated alpha for smoothing. + uint32_t delay_samples = 0; // Pre-calculated delay in samples for smoothing. + void set_smoothing_time(const float s_time); // Set smoothing time, recalculate alpha and delay. +} axis_smoothing_t; + +typedef struct Smoothing { + axis_smoothing_t CARTES_AXIS_NAMES; + int32_t largest_delay_samples; + // Smoothing causes a phase delay equal to smoothing_time. This delay is componensated for during axis synchronisation, which + // is done by delaying all axes to match the laggiest one (i.e largest_delay_samples). + void refresh_largest_delay_samples() { largest_delay_samples = _MAX(CARTES_LIST(X.delay_samples, Y.delay_samples, Z.delay_samples, E.delay_samples)); } + // Note: the delay equals smoothing_time iff the input signal frequency is lower than 1/smoothing_time, luckily for us, this holds in this case +} smoothing_t; diff --git a/Marlin/src/module/ft_motion/stepping.cpp b/Marlin/src/module/ft_motion/stepping.cpp new file mode 100644 index 0000000000..96ef4466d8 --- /dev/null +++ b/Marlin/src/module/ft_motion/stepping.cpp @@ -0,0 +1,112 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(FT_MOTION) + +#include "stepping.h" +#include "../ft_motion.h" + +void Stepping::reset() { + stepper_plan.reset(); + delta_error_q32.set(LOGICAL_AXIS_ARRAY_1(_BV32(31))); // start as 0.5 in q32 so steps are rounded + step_bits = 0; + bresenham_iterations_pending = 0; +} + +uint32_t Stepping::advance_until_step() { + xyze_ulong_t space_q32 = -delta_error_q32 + UINT32_MAX; // How much accumulation until a step in any axis is ALMOST due + // scalar in the right hand because types.h is missing scalar on left cases + + xyze_ulong_t advance_q32 = stepper_plan.advance_dividend_q0_32; + uint32_t iterations = bresenham_iterations_pending; + // Per-axis lower-bound approx of floor(space_q32/adv), min across axes (lower bound because this fast division underestimates result by up to 1) + // #define RUN_AXIS(A) if(advance_q32.A > 0) NOMORE(iterations, space_q32.A/advance_q32.A); + #define RUN_AXIS(A) if(advance_q32.A > 0) NOMORE(iterations, uint32_t((uint64_t(space_q32.A) * advance_dividend_reciprocal.A) >> 32)); + LOGICAL_AXIS_MAP(RUN_AXIS); + #undef RUN_AXIS + + #define RUN_AXIS(A) delta_error_q32.A += advance_q32.A * iterations; + LOGICAL_AXIS_MAP(RUN_AXIS); + #undef RUN_AXIS + + bresenham_iterations_pending -= iterations; + step_bits = 0; + // iterations may be underestimated by 1 by the cheap division, therefore we may have to do 2 iterations here + while (bresenham_iterations_pending && !(bool)step_bits) { + iterations++; + bresenham_iterations_pending--; + #define RUN_AXIS(A) do{ \ + delta_error_q32.A += stepper_plan.advance_dividend_q0_32.A; \ + step_bits.A = delta_error_q32.A < stepper_plan.advance_dividend_q0_32.A; \ + }while(0); + LOGICAL_AXIS_MAP(RUN_AXIS); + #undef RUN_AXIS + } + + return iterations * INTERVAL_PER_ITERATION; +} + +uint32_t Stepping::plan() { + uint32_t intervals = 0; + if (bresenham_iterations_pending > 0) { + intervals = advance_until_step(); + if (bool(step_bits)) return intervals; // steps to make => return the wait time so it gets done in due time + // Else all bresenham iterations were advanced without steps => this is just the frame end, so plan the next one directly and accumulate the wait + } + + if (ftMotion.stepper_plan_is_empty()) { + bresenham_iterations_pending = 0; + step_bits = 0; + return INTERVAL_PER_TRAJ_POINT; + } + + AxisBits old_dir_bits = stepper_plan.dir_bits; + stepper_plan = ftMotion.dequeue_stepper_plan(); + const AxisBits dir_flip_mask = old_dir_bits ^ stepper_plan.dir_bits; // axes that must toggle now + if (dir_flip_mask) { + #define _HANDLE_DIR_CHANGES(A) if (dir_flip_mask.A) delta_error_q32.A *= -1; + LOGICAL_AXIS_MAP(_HANDLE_DIR_CHANGES); + #undef _HANDLE_DIR_CHANGES + } + + if (!(bool)stepper_plan.advance_dividend_q0_32) { + // don't waste time in zero motion traj points + bresenham_iterations_pending = 0; + step_bits = 0; + return INTERVAL_PER_TRAJ_POINT; + } + + // This vector division is unavoidable, but it saves a division per step during bresenham + // The reciprocal is actually 2^32/dividend, but that requires dividing a uint64_t, which quite expensive + // Since even the real reciprocal may underestimate the quotient by 1 anyway already, this optimisation doesn't + // make things worse. This underestimation is compensated for in advance_until_step. + #define _DIVIDEND_RECIP(A) advance_dividend_reciprocal.A = UINT32_MAX / stepper_plan.advance_dividend_q0_32.A; + LOGICAL_AXIS_MAP(_DIVIDEND_RECIP); + #undef _DIVIDEND_RECIP + + bresenham_iterations_pending = ITERATIONS_PER_TRAJ; + return intervals + advance_until_step(); +} + +#endif // FT_MOTION diff --git a/Marlin/src/module/ft_motion/stepping.h b/Marlin/src/module/ft_motion/stepping.h new file mode 100644 index 0000000000..6d0f3d31a5 --- /dev/null +++ b/Marlin/src/module/ft_motion/stepping.h @@ -0,0 +1,53 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../../inc/MarlinConfig.h" + +typedef struct stepper_plan { + AxisBits dir_bits; + xyze_ulong_t advance_dividend_q0_32{0}; + void reset() { advance_dividend_q0_32.reset(); } +} stepper_plan_t; + + +// Stepping plan handles steps for a while frame (trajectory point delta) +typedef struct Stepping { + stepper_plan_t stepper_plan; + xyze_ulong_t advance_dividend_reciprocal{0}; // Note this 32 bit reciprocal underestimates quotients by at most one. + xyze_ulong_t delta_error_q32{ LOGICAL_AXIS_LIST_1(_BV32(31)) }; + AxisBits step_bits; + uint32_t bresenham_iterations_pending; + + void reset(); + // Updates error and bresenham_iterations_pending, sets step_bits and returns interval until the next step (or end of frame). + uint32_t advance_until_step(); + /** + * If bresenham_iterations_pending, advance to next actual step. + * Else, consume stepper data point + * Then return interval until that next step + */ + uint32_t plan(); + #define INTERVAL_PER_ITERATION (STEPPER_TIMER_RATE / FTM_STEPPER_FS) + #define INTERVAL_PER_TRAJ_POINT (STEPPER_TIMER_RATE / FTM_FS) + #define ITERATIONS_PER_TRAJ (FTM_STEPPER_FS * FTM_TS) +} stepping_t; diff --git a/Marlin/src/module/ft_motion/trajectory_generator.h b/Marlin/src/module/ft_motion/trajectory_generator.h index 991ea6075c..934394940a 100644 --- a/Marlin/src/module/ft_motion/trajectory_generator.h +++ b/Marlin/src/module/ft_motion/trajectory_generator.h @@ -21,8 +21,6 @@ */ #pragma once -#include "../../inc/MarlinConfig.h" - #include /** diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 3025cb83bd..8a44a40b8b 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1512,7 +1512,7 @@ void Stepper::isr() { uint8_t max_loops = 10; #if ENABLED(FT_MOTION) - static uint32_t ftMotion_nextAuxISR = 0U; // Storage for the next ISR of the auxiliary tasks. + static uint32_t ftMotion_nextStepperISR = 0U; // Storage for the next ISR for stepping. const bool using_ftMotion = ftMotion.cfg.active; #else constexpr bool using_ftMotion = false; @@ -1527,19 +1527,22 @@ void Stepper::isr() { #if ENABLED(FT_MOTION) if (using_ftMotion) { - ftMotion_stepper(); // Run FTM Stepping + if (!ftMotion_nextStepperISR) ftMotion_stepper(); + TERN_(BABYSTEPPING, if (!nextBabystepISR) nextBabystepISR = babystepping_isr()); - // Define 2.5 msec task for auxiliary functions. - if (!ftMotion_nextAuxISR) { - TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr()); - ftMotion_nextAuxISR = (STEPPER_TIMER_RATE) / 400; - } + // ^== Time critical. NOTHING besides pulse generation should be above here!!! // Enable ISRs to reduce latency for higher priority ISRs hal.isr_on(); - interval = FTM_MIN_TICKS; - ftMotion_nextAuxISR -= interval; + if (!ftMotion_nextStepperISR) ftMotion_nextStepperISR = ftMotion.stepping.plan(); + + interval = HAL_TIMER_TYPE_MAX; // Time until the next step + NOMORE(interval, ftMotion_nextStepperISR); + TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); + + TERN_(BABYSTEPPING, nextBabystepISR -= interval); + ftMotion_nextStepperISR -= interval; } #endif @@ -1583,7 +1586,8 @@ void Stepper::isr() { #endif // Get the interval to the next ISR call - interval = _MIN(nextMainISR, uint32_t(HAL_TIMER_TYPE_MAX)); // Time until the next Pulse / Block phase + interval = uint32_t(STEPPER_TIMER_RATE * 0.030); // Max wait of 30ms regardless of stepper timer frequency + NOMORE(interval, nextMainISR); // Time until the next Pulse / Block phase TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y TERN_(INPUT_SHAPING_Z, NOMORE(interval, ShapingQueue::peek_z())); // Time until next input shaping echo for Z @@ -3546,30 +3550,23 @@ void Stepper::report_positions() { * - Apply STEP/DIR along with any delays required. A command may be empty, with no STEP/DIR. */ void Stepper::ftMotion_stepper() { + AxisBits &step_bits = ftMotion.stepping.step_bits; // Aliases for prettier code + AxisBits &dir_bits = ftMotion.stepping.stepper_plan.dir_bits; - // Check if the buffer is empty. - ftMotion.stepperCmdBuffHasData = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx); - if (!ftMotion.stepperCmdBuffHasData) return; - - // "Pop" one command from current motion buffer - const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx]; - if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE)) - ftMotion.stepperCmdBuff_consumeIdx = 0; + if (step_bits.bits == 0) return; USING_TIMED_PULSE(); - // Get FT Motion command flags for axis STEP / DIR - #define _FTM_STEP(AXIS) TEST(command, FT_BIT_STEP_##AXIS) - #define _FTM_DIR(AXIS) TEST(command, FT_BIT_DIR_##AXIS) - /** * Update direction bits for steppers that were stepped by this command. * HX, HY, HZ direction bits were set for Core kinematics * when the block was fetched and are not overwritten here. */ - #define _FTM_SET_DIR(AXIS) if (_FTM_STEP(AXIS)) last_direction_bits.bset(_AXIS(AXIS), _FTM_DIR(AXIS)); - LOGICAL_AXIS_MAP(_FTM_SET_DIR); + // Replace last_direction_bits with current dir bits for all stepped axes + last_direction_bits = (last_direction_bits & ~step_bits) | (dir_bits & step_bits); + //#define _FTM_SET_DIR(A) if (step_bits.A) last_direction_bits.A = dir_bits.A; + //LOGICAL_AXIS_MAP(_FTM_SET_DIR); if (last_set_direction != last_direction_bits) { // Apply directions (generally applying to the entire linear move) @@ -3583,7 +3580,7 @@ void Stepper::report_positions() { } // Start step pulses. Edge stepping will toggle the STEP pin. - #define _FTM_STEP_START(A) A##_APPLY_STEP(_FTM_STEP(A), false); + #define _FTM_STEP_START(A) A##_APPLY_STEP(step_bits.A, false); LOGICAL_AXIS_MAP(_FTM_STEP_START); // Apply steps via I2S @@ -3593,7 +3590,7 @@ void Stepper::report_positions() { START_TIMED_PULSE(); // Update step counts - #define _FTM_STEP_COUNT(A) if (_FTM_STEP(A)) count_position.A += last_direction_bits.A ? 1 : -1; + #define _FTM_STEP_COUNT(A) if (step_bits.A) count_position.A += count_direction.A; LOGICAL_AXIS_MAP(_FTM_STEP_COUNT); // Provide EDGE flags for E stepper(s) @@ -3609,10 +3606,10 @@ void Stepper::report_positions() { // Only wait for axes without edge stepping const bool any_wait = false LOGICAL_AXIS_GANG( - || (!e_axis_has_dedge && _FTM_STEP(E)), - || (!AXIS_HAS_DEDGE(X) && _FTM_STEP(X)), || (!AXIS_HAS_DEDGE(Y) && _FTM_STEP(Y)), || (!AXIS_HAS_DEDGE(Z) && _FTM_STEP(Z)), - || (!AXIS_HAS_DEDGE(I) && _FTM_STEP(I)), || (!AXIS_HAS_DEDGE(J) && _FTM_STEP(J)), || (!AXIS_HAS_DEDGE(K) && _FTM_STEP(K)), - || (!AXIS_HAS_DEDGE(U) && _FTM_STEP(U)), || (!AXIS_HAS_DEDGE(V) && _FTM_STEP(V)), || (!AXIS_HAS_DEDGE(W) && _FTM_STEP(W)) + || (!e_axis_has_dedge && step_bits.E), + || (!AXIS_HAS_DEDGE(X) && step_bits.X), || (!AXIS_HAS_DEDGE(Y) && step_bits.Y), || (!AXIS_HAS_DEDGE(Z) && step_bits.Z), + || (!AXIS_HAS_DEDGE(I) && step_bits.I), || (!AXIS_HAS_DEDGE(J) && step_bits.J), || (!AXIS_HAS_DEDGE(K) && step_bits.K), + || (!AXIS_HAS_DEDGE(U) && step_bits.U), || (!AXIS_HAS_DEDGE(V) && step_bits.V), || (!AXIS_HAS_DEDGE(W) && step_bits.W) ); // Allow pulses to be registered by stepper drivers diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 50617800ba..5d5c000577 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -52,7 +52,7 @@ #endif #if ENABLED(FT_MOTION) - #include "ft_types.h" + class FTMotion; #endif // TODO: Review and ensure proper handling for special E axes with commands like M17/M18, stepper timeout, etc. diff --git a/ini/features.ini b/ini/features.ini index d1b02bb4d4..f727f00f16 100644 --- a/ini/features.ini +++ b/ini/features.ini @@ -311,7 +311,8 @@ HAS_DUPLICATION_MODE = build_src_filter=+ PLATFORM_M997_SUPPORT = build_src_filter=+ HAS_TOOLCHANGE = build_src_filter=+ -FT_MOTION = build_src_filter=+ + + +FT_MOTION = build_src_filter=+ + - + +FTM_SMOOTHING = build_src_filter=+ HAS_LIN_ADVANCE_K = build_src_filter=+ PHOTO_GCODE = build_src_filter=+ CONTROLLER_FAN_EDITABLE = build_src_filter=+