3D-printing/Marlin
1
0
Fork 0
mirror of https://github.com/MarlinFirmware/Marlin.git synced 2025-12-28 02:10:32 -07:00
Code Issues Projects Releases Packages Wiki Activity Actions 4

⚡️ FT Motion trajectories, smoothing, optimization (#28115)

Browse source 
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
Co-authored-by: narno2202 <130909513+narno2202@users.noreply.github.com>
This commit is contained in:
David Buezas 2025-11-02 05:50:48 +01:00 • committed by GitHub
parent 5a0923ed28
commit 1ead60ec46
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
17 changed files with 859 additions and 679 deletions
Download patch file Download diff file Expand all files Collapse all files

30
Marlin/Configuration_adv.h
View file

@ -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
/**

54
Marlin/src/core/types.h
View file

@ -238,6 +238,24 @@ struct Flags<N, false> {
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<N>& operator|=(Flags<N> &p) const { b |= p.b; return *this; }
FI Flags<N>& operator&=(Flags<N> &p) const { b &= p.b; return *this; }
FI Flags<N>& operator^=(Flags<N> &p) const { b ^= p.b; return *this; }
FI Flags<N>& operator|=(const flagbits_t &p) { b |= flagbits_t(p); return *this; }
FI Flags<N>& operator&=(const flagbits_t &p) { b &= flagbits_t(p); return *this; }
FI Flags<N>& operator^=(const flagbits_t &p) { b ^= flagbits_t(p); return *this; }
FI Flags<N> operator|(Flags<N> &p) const { return Flags<N>(b | p.b); }
FI Flags<N> operator&(Flags<N> &p) const { return Flags<N>(b & p.b); }
FI Flags<N> operator^(Flags<N> &p) const { return Flags<N>(b ^ p.b); }
FI Flags<N> operator~() const { return Flags<N>(~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<T> &rs) const { return x < rs.x && y < rs.y; }
FI bool operator<=(const XYval<T> &rs) const { return x <= rs.x && y <= rs.y; }
FI bool operator> (const XYval<T> &rs) const { return x > rs.x && y > rs.y; }
FI bool operator>=(const XYval<T> &rs) const { return x >= rs.x && y >= rs.y; }
FI bool operator< (const XYZval<T> &rs) const { return true XY_GANG(&& x < rs.x, && y < rs.y); }
FI bool operator<=(const XYZval<T> &rs) const { return true XY_GANG(&& x <= rs.x, && y <= rs.y); }
FI bool operator> (const XYZval<T> &rs) const { return true XY_GANG(&& x > rs.x, && y > rs.y); }
FI bool operator>=(const XYZval<T> &rs) const { return true XY_GANG(&& x >= rs.x, && y >= rs.y); }
FI bool operator< (const XYZEval<T> &rs) const { return true XY_GANG(&& x < rs.x, && y < rs.y); }
FI bool operator<=(const XYZEval<T> &rs) const { return true XY_GANG(&& x <= rs.x, && y <= rs.y); }
FI bool operator> (const XYZEval<T> &rs) const { return true XY_GANG(&& x > rs.x, && y > rs.y); }
FI bool operator>=(const XYZEval<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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<T> &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 <string.h> // 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); }

4
Marlin/src/inc/Conditionals-4-adv.h
View file

@ -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

4
Marlin/src/inc/Conditionals-5-post.h
View file

@ -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

3
Marlin/src/inc/SanityCheck.h
View file

@ -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.");

776
Marlin/src/module/ft_motion.cpp
View file

@ -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<millis_t> 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<int64_t> 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<int64_t> 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;
}
}

128
Marlin/src/module/ft_motion.h
View file

@ -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<millis_t> 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<int64_t> curr_steps_q32_32;
}; // class FTMotion
extern FTMotion ftMotion; // Use ftMotion.thing, not FTMotion::thing.

170
Marlin/src/module/ft_motion/shaping.cpp Normal file
View file

@ -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 <https://www.gnu.org/licenses/>.
*
*/
#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

52
Marlin/src/module/ft_types.h → Marlin/src/module/ft_motion/shaping.h
View file

@ -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<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
typedef struct XYZEarray<float, FTM_BATCH_SIZE> 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<float> ft_shaped_float_t;
typedef FTShapedAxes<ftMotionShaper_t> ft_shaped_shaper_t;
typedef FTShapedAxes<dynFreqMode_t> 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;

41
Marlin/src/module/ft_motion/smoothing.cpp Normal file
View file

@ -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 <https://www.gnu.org/licenses/>.
*
*/
#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

47
Marlin/src/module/ft_motion/smoothing.h Normal file
View file

@ -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 <https://www.gnu.org/licenses/>.
*
*/
#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;

112
Marlin/src/module/ft_motion/stepping.cpp Normal file
View file

@ -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 <https://www.gnu.org/licenses/>.
*
*/
#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

53
Marlin/src/module/ft_motion/stepping.h Normal file
View file

@ -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 <https://www.gnu.org/licenses/>.
*
*/
#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;

2
Marlin/src/module/ft_motion/trajectory_generator.h
View file

@ -21,8 +21,6 @@
*/
#pragma once
#include "../../inc/MarlinConfig.h"
#include <stdint.h>
/**

57
Marlin/src/module/stepper.cpp
View file

@ -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

2
Marlin/src/module/stepper.h
View file

@ -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.

3
ini/features.ini
View file

@ -311,7 +311,8 @@ HAS_DUPLICATION_MODE = build_src_filter=+<src/gcode/control/M6
SPI_FLASH_BACKUP = build_src_filter=+<src/gcode/control/M993_M994.cpp>
PLATFORM_M997_SUPPORT = build_src_filter=+<src/gcode/control/M997.cpp>
HAS_TOOLCHANGE = build_src_filter=+<src/gcode/control/T.cpp>
FT_MOTION = build_src_filter=+<src/module/ft_motion.cpp> +<src/module/ft_motion> +<src/gcode/feature/ft_motion>
FT_MOTION = build_src_filter=+<src/module/ft_motion.cpp> +<src/module/ft_motion> -<src/module/ft_motion/smoothing.cpp> +<src/gcode/feature/ft_motion>
FTM_SMOOTHING = build_src_filter=+<src/module/ft_motion/smoothing.cpp>
HAS_LIN_ADVANCE_K = build_src_filter=+<src/gcode/feature/advance>
PHOTO_GCODE = build_src_filter=+<src/gcode/feature/camera>
CONTROLLER_FAN_EDITABLE = build_src_filter=+<src/gcode/feature/controllerfan>