jessikitty/Marlin
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🐛 FT Motion fix and refine (#28220)

Browse Source 
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
David Buezas
2025-12-17 00:25:37 +01:00
committed by GitHub
parent ec752068a5
commit 3e6a0ab713
12 changed files with 173 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Marlin/src/core/types.h
+3 -3
View File
@@ -557,7 +557,7 @@ struct XYval {
#endif
// Length reduced to one dimension
FI constexpr T magnitude() const { return (T)sqrtf(x*x + y*y); }
FI constexpr T magnitude() const { return (T)SQRT(x*x + y*y); }
// Pointer to the data as a simple array
explicit FI operator T* () { return pos; }
// If any element is true then it's true
@@ -734,7 +734,7 @@ struct XYZval {
#endif
// Length reduced to one dimension
FI constexpr T magnitude() const { return (T)TERN(HAS_X_AXIS, sqrtf(NUM_AXIS_GANG(x*x, + y*y, + z*z, + i*i, + j*j, + k*k, + u*u, + v*v, + w*w)), 0); }
FI constexpr T magnitude() const { return (T)TERN(HAS_X_AXIS, SQRT(NUM_AXIS_GANG(x*x, + y*y, + z*z, + i*i, + j*j, + k*k, + u*u, + v*v, + w*w)), 0); }
// Pointer to the data as a simple array
explicit FI operator T* () { return pos; }
// If any element is true then it's true
@@ -905,7 +905,7 @@ struct XYZEval {
#endif
// Length reduced to one dimension
FI constexpr T magnitude() const { return (T)sqrtf(LOGICAL_AXIS_GANG(+ e*e, + x*x, + y*y, + z*z, + i*i, + j*j, + k*k, + u*u, + v*v, + w*w)); }
FI constexpr T magnitude() const { return (T)SQRT(LOGICAL_AXIS_GANG(+ e*e, + x*x, + y*y, + z*z, + i*i, + j*j, + k*k, + u*u, + v*v, + w*w)); }
// Pointer to the data as a simple array
explicit FI operator T* () { return pos; }
// If any element is true then it's true
Marlin/src/gcode/feature/ft_motion/M493.cpp
+9 -9
View File
@@ -33,14 +33,14 @@ void say_shaper_type(const AxisEnum a, bool &sep, const char axis_name) {
SERIAL_CHAR(axis_name, '=');
switch (ftMotion.cfg.shaper[a]) {
default: break;
case ftMotionShaper_ZV: SERIAL_ECHOPGM("ZV"); break;
case ftMotionShaper_ZVD: SERIAL_ECHOPGM("ZVD"); break;
case ftMotionShaper_ZVDD: SERIAL_ECHOPGM("ZVDD"); break;
case ftMotionShaper_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break;
case ftMotionShaper_EI: SERIAL_ECHOPGM("EI"); break;
case ftMotionShaper_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break;
case ftMotionShaper_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break;
case ftMotionShaper_MZV: SERIAL_ECHOPGM("MZV"); break;
TERN_(FTM_SHAPER_ZV, case ftMotionShaper_ZV: SERIAL_ECHOPGM("ZV"); break);
TERN_(FTM_SHAPER_ZVD, case ftMotionShaper_ZVD: SERIAL_ECHOPGM("ZVD"); break);
TERN_(FTM_SHAPER_ZVDD, case ftMotionShaper_ZVDD: SERIAL_ECHOPGM("ZVDD"); break);
TERN_(FTM_SHAPER_ZVDDD, case ftMotionShaper_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break);
TERN_(FTM_SHAPER_EI, case ftMotionShaper_EI: SERIAL_ECHOPGM("EI"); break);
TERN_(FTM_SHAPER_2HEI, case ftMotionShaper_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break);
TERN_(FTM_SHAPER_3HEI, case ftMotionShaper_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break);
TERN_(FTM_SHAPER_MZV, case ftMotionShaper_MZV: SERIAL_ECHOPGM("MZV"); break);
}
sep = true;
}
@@ -296,7 +296,7 @@ void GcodeSuite::M493() {
const float zetaVal = seenI ? parser.value_float() : 0.0f;
const bool goodZeta = seenI && c.goodZeta(zetaVal);
if (seenI && !goodZeta)
SERIAL_ECHOLN(F("?Invalid "), F("(I) Zeta value. (0.01-1.0)")); // Zeta out of range
SERIAL_ECHOLN(F("?Invalid "), F("(I) Zeta value. (0.01-" STRINGIFY(FTM_MAX_DAMPENING) ")")); // Zeta out of range
#if HAS_FTM_EI_SHAPING
// Vibration Tolerance parameter
Marlin/src/gcode/feature/ft_motion/M494.cpp
+4 -2
View File
@@ -48,11 +48,13 @@ void say_ftm_settings() {
void GcodeSuite::M494_report(const bool forReplay/*=true*/) {
TERN_(MARLIN_SMALL_BUILD, return);
const ft_config_t &c = ftMotion.cfg;
report_heading_etc(forReplay, F("FT Motion"));
SERIAL_ECHOPGM(" M494 T", (uint8_t)ftMotion.getTrajectoryType());
#if ANY(FTM_POLYS, FTM_SMOOTHING)
const ft_config_t &c = ftMotion.cfg;
#endif
#if ENABLED(FTM_SMOOTHING)
SERIAL_ECHOPGM(CARTES_PAIRED_LIST(
" X", c.smoothingTime.X,
Marlin/src/inc/Conditionals-5-post.h
+1 -12
View File
@@ -3695,19 +3695,8 @@
// Fixed-Time Motion
#if ENABLED(FT_MOTION)
#define FTM_TS (1.0f / FTM_FS) // (s) Time step for trajectory generation. (Reciprocal of FTM_FS)
#define FTM_RATIO (FTM_FS / FTM_MIN_SHAPE_FREQ) // Factor for use in FTM_ZMAX. DON'T CHANGE.
#define FTM_SMOOTH_MAX_I uint32_t(TERN0(FTM_SMOOTHING, CEIL(FTM_FS * FTM_MAX_SMOOTHING_TIME))) // Max delays for smoothing
// Maximum delays for shaping functions (even numbers only!)
#define FTM_ZMAX (TERN(HAS_FTM_EI_SHAPING, 2, 1) * FTM_RATIO + FTM_SMOOTH_MAX_I)
#define FTM_TS (1.0f / FTM_FS) // (s) Time step for trajectory generation. (Reciprocal of FTM_FS)
#define FTM_SMOOTHING_ORDER 5 // 3 to 5 is closest to Gaussian
// Calculate as:
// ZV : FTM_RATIO / 2
// ZVD, MZV : FTM_RATIO
// 2HEI : FTM_RATIO * 3 / 2
// 3HEI : FTM_RATIO * 2
#ifndef FTM_BUFFER_SIZE
#define FTM_BUFFER_SIZE 128
#endif
Marlin/src/lcd/menu/menu_motion.cpp
+9 -12
View File
@@ -372,14 +372,14 @@ void menu_move() {
BACK_ITEM_N(axis, MSG_FTM_CONFIGURE_AXIS_N);
if (shaper != ftMotionShaper_NONE) ACTION_ITEM_N(axis, MSG_LCD_OFF, []{ ftm_menu_set_shaper(ftMotionShaper_NONE) ; });
if (shaper != ftMotionShaper_ZV) ACTION_ITEM_N(axis, MSG_FTM_ZV, []{ ftm_menu_set_shaper(ftMotionShaper_ZV) ; });
if (shaper != ftMotionShaper_ZVD) ACTION_ITEM_N(axis, MSG_FTM_ZVD, []{ ftm_menu_set_shaper(ftMotionShaper_ZVD) ; });
if (shaper != ftMotionShaper_ZVDD) ACTION_ITEM_N(axis, MSG_FTM_ZVDD, []{ ftm_menu_set_shaper(ftMotionShaper_ZVDD) ; });
if (shaper != ftMotionShaper_ZVDDD) ACTION_ITEM_N(axis, MSG_FTM_ZVDDD,[]{ ftm_menu_set_shaper(ftMotionShaper_ZVDDD) ; });
if (shaper != ftMotionShaper_EI) ACTION_ITEM_N(axis, MSG_FTM_EI, []{ ftm_menu_set_shaper(ftMotionShaper_EI) ; });
if (shaper != ftMotionShaper_2HEI) ACTION_ITEM_N(axis, MSG_FTM_2HEI, []{ ftm_menu_set_shaper(ftMotionShaper_2HEI) ; });
if (shaper != ftMotionShaper_3HEI) ACTION_ITEM_N(axis, MSG_FTM_3HEI, []{ ftm_menu_set_shaper(ftMotionShaper_3HEI) ; });
if (shaper != ftMotionShaper_MZV) ACTION_ITEM_N(axis, MSG_FTM_MZV, []{ ftm_menu_set_shaper(ftMotionShaper_MZV) ; });
TERN_(FTM_SHAPER_ZV, if (shaper != ftMotionShaper_ZV) ACTION_ITEM_N(axis, MSG_FTM_ZV, []{ ftm_menu_set_shaper(ftMotionShaper_ZV) ; }));
TERN_(FTM_SHAPER_ZVD, if (shaper != ftMotionShaper_ZVD) ACTION_ITEM_N(axis, MSG_FTM_ZVD, []{ ftm_menu_set_shaper(ftMotionShaper_ZVD) ; }));
TERN_(FTM_SHAPER_ZVDD, if (shaper != ftMotionShaper_ZVDD) ACTION_ITEM_N(axis, MSG_FTM_ZVDD, []{ ftm_menu_set_shaper(ftMotionShaper_ZVDD) ; }));
TERN_(FTM_SHAPER_ZVDDD, if (shaper != ftMotionShaper_ZVDDD) ACTION_ITEM_N(axis, MSG_FTM_ZVDDD,[]{ ftm_menu_set_shaper(ftMotionShaper_ZVDDD) ; }));
TERN_(FTM_SHAPER_EI, if (shaper != ftMotionShaper_EI) ACTION_ITEM_N(axis, MSG_FTM_EI, []{ ftm_menu_set_shaper(ftMotionShaper_EI) ; }));
TERN_(FTM_SHAPER_2HEI, if (shaper != ftMotionShaper_2HEI) ACTION_ITEM_N(axis, MSG_FTM_2HEI, []{ ftm_menu_set_shaper(ftMotionShaper_2HEI) ; }));
TERN_(FTM_SHAPER_3HEI, if (shaper != ftMotionShaper_3HEI) ACTION_ITEM_N(axis, MSG_FTM_3HEI, []{ ftm_menu_set_shaper(ftMotionShaper_3HEI) ; }));
TERN_(FTM_SHAPER_MZV, if (shaper != ftMotionShaper_MZV) ACTION_ITEM_N(axis, MSG_FTM_MZV, []{ ftm_menu_set_shaper(ftMotionShaper_MZV) ; }));
END_MENU();
}
@@ -529,9 +529,6 @@ void menu_move() {
void menu_tune_ft_motion() {
// Define stuff ahead of the menu loop
ft_config_t &c = ftMotion.cfg;
#ifdef __AVR__
// Copy Flash strings to RAM for C-string substitution
@@ -565,7 +562,7 @@ void menu_move() {
#if ENABLED(FTM_POLYS)
SUBMENU_S(_traj_name(), MSG_FTM_TRAJECTORY, menu_ftm_trajectory_generator);
if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6)
EDIT_ITEM(float42_52, MSG_FTM_POLY6_OVERSHOOT, &c.poly6_acceleration_overshoot, 1.25f, 1.875f);
EDIT_ITEM(float42_52, MSG_FTM_POLY6_OVERSHOOT, &ftMotion.cfg.poly6_acceleration_overshoot, 1.25f, 1.875f);
#endif
SHAPED_MAP(_FTM_AXIS_SUBMENU);
Marlin/src/module/ft_motion.cpp
+62 -54
View File
@@ -409,38 +409,41 @@ bool FTMotion::plan_next_block() {
}
}
/**
* Ensure extruder position stays within floating point precision bounds.
* Float32 numbers have 23 bits of precision, so the minimum increment ("resolution") around a value x is:
* resolution = 2^(floor(log2(|x|)) - 23)
* By resetting at ±1'000mm (1 meter), we get a minimum resolution of ~ 0.00006mm, enough for smoothing to work well.
*/
void FTMotion::ensure_float_precision() {
constexpr float FTM_POSITION_WRAP_THRESHOLD = 1'000.0f; // (mm) Reset when position exceeds this to prevent floating point precision loss
#if HAS_EXTRUDERS
if (fabs(endPos_prevBlock.E) >= FTM_POSITION_WRAP_THRESHOLD) {
const float offset = -endPos_prevBlock.E;
endPos_prevBlock.E += offset;
#if HAS_EXTRUDERS
// Offset extruder shaping buffer
#if ALL(HAS_FTM_SHAPING, FTM_SHAPER_E)
for (uint32_t i = 0; i < FTM_ZMAX; ++i) shaping.E.d_zi[i] += offset;
#endif
/**
* Ensure extruder position stays within floating point precision bounds.
* Float32 numbers have 23 bits of precision, so the minimum increment ("resolution") around a value x is:
* resolution = 2^(floor(log2(|x|)) - 23)
* By resetting at ±1'000mm (1 meter), we get a minimum resolution of ~ 0.00006mm, enough for smoothing to work well.
*/
void FTMotion::ensure_float_precision() {
constexpr float FTM_POSITION_WRAP_THRESHOLD = 1000; // (mm) Reset when position exceeds this to prevent floating point precision loss
if (ABS(endPos_prevBlock.E) < FTM_POSITION_WRAP_THRESHOLD) return;
// Offset extruder smoothing buffer
#if ENABLED(FTM_SMOOTHING)
for (uint8_t i = 0; i < FTM_SMOOTHING_ORDER; ++i) smoothing.E.smoothing_pass[i] += offset;
#endif
const float offset = -endPos_prevBlock.E;
// Offset linear advance previous position
prev_traj_e += offset;
endPos_prevBlock.E = 0;
// Offset stepper current position
const int64_t delta_steps_q48_16 = offset * planner.settings.axis_steps_per_mm[block_extruder_axis] * (1ULL << 16);
stepping.curr_steps_q48_16.E += delta_steps_q48_16;
};
#endif
}
// Offset extruder shaping buffer
#if ALL(HAS_FTM_SHAPING, FTM_SHAPER_E)
for (uint32_t i = 0; i < ftm_zmax; ++i) shaping.E.d_zi[i] += offset;
#endif
// Offset extruder smoothing buffer
#if ENABLED(FTM_SMOOTHING)
for (uint8_t i = 0; i < FTM_SMOOTHING_ORDER; ++i) smoothing.E.smoothing_pass[i] += offset;
#endif
// Offset linear advance previous position
prev_traj_e += offset;
// Offset stepper current position
const int64_t delta_steps_q48_16 = offset * planner.settings.axis_steps_per_mm[block_extruder_axis] * (1ULL << 16);
stepping.curr_steps_q48_16.E += delta_steps_q48_16;
}
#endif // HAS_EXTRUDERS
xyze_float_t FTMotion::calc_traj_point(const float dist) {
xyze_float_t traj_coords;
@@ -504,17 +507,20 @@ xyze_float_t FTMotion::calc_traj_point(const float dist) {
#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; \
// Approximate Gaussian smoothing via chained EMAs
auto _smoothen = [&](const AxisEnum axis, axis_smoothing_t &smoo) {
if (smoo.alpha <= 0.0f) return;
float smooth_val = traj_coords[axis];
for (uint8_t _i = 0; _i < FTM_SMOOTHING_ORDER; ++_i) {
smoo.smoothing_pass[_i] += (smooth_val - smoo.smoothing_pass[_i]) * smoo.alpha;
smooth_val = smoo.smoothing_pass[_i];
}
traj_coords[axis] = smooth_val;
};
#define _SMOOTHEN(A) _smoothen(_AXIS(A), smoothing.A);
CARTES_MAP(_SMOOTHEN);
max_total_delay += smoothing.largest_delay_samples;
#endif // FTM_SMOOTHING
@@ -525,27 +531,29 @@ xyze_float_t FTMotion::calc_traj_point(const float dist) {
max_total_delay += shaping.largest_delay_samples;
// 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);
auto _shape = [&](const AxisEnum axis, axis_shaping_t &shap OPTARG(FTM_SMOOTHING, const axis_smoothing_t &smoo)) {
const uint32_t group_delay = ftMotion.cfg.axis_sync_enabled
? max_total_delay - TERN0(FTM_SMOOTHING, smoo.delay_samples)
: -shap.Ni[0];
//
// α = 1 − exp(−(dt / (τ / order)))
//
shap.d_zi[shaping.zi_idx] = traj_coords[axis];
traj_coords[axis] = 0;
for (uint32_t i = 0; i <= shap.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 + shap.Ni[i];
int32_t udiff = shaping.zi_idx - echo_delay;
if (udiff < 0) udiff += ftm_zmax;
traj_coords[axis] += shap.Ai[i] * shap.d_zi[udiff];
}
};
#define _SHAPE(A) _shape(_AXIS(A), shaping.A OPTARG(FTM_SMOOTHING, smoothing.A));
SHAPED_MAP(_SHAPE);
if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0;
if (++shaping.zi_idx == ftm_zmax) shaping.zi_idx = 0;
#endif // HAS_FTM_SHAPING
Marlin/src/module/ft_motion.h
+2 -2
View File
@@ -109,7 +109,7 @@ typedef struct FTConfig {
#if HAS_FTM_SHAPING
constexpr bool goodZeta(const float z) { return WITHIN(z, 0.01f, 1.0f); }
constexpr bool goodZeta(const float z) { return WITHIN(z, 0.01f, ftm_max_dampening); }
constexpr bool goodVtol(const float v) { return WITHIN(v, 0.00f, 1.0f); }
#if HAS_DYNAMIC_FREQ
@@ -314,7 +314,7 @@ class FTMotion {
static void fill_stepper_plan_buffer();
static xyze_float_t calc_traj_point(const float dist);
static bool plan_next_block();
static void ensure_float_precision();
static void ensure_float_precision() IF_DISABLED(HAS_EXTRUDERS, {});
}; // class FTMotion
Marlin/src/module/ft_motion/shaping.cpp
+34 -53
View File
@@ -137,77 +137,58 @@ void AxisShaping::set_axis_shaping_A(
break;
}
}
} // set_axis_shaping_A
// 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) );
const float df = SQRT(1.f - sq(zeta));
float base = 0.0f;
switch (shaper) {
#if ENABLED(FTM_SHAPER_ZV)
case ftMotionShaper_ZV:
Ni[1] = LROUND((0.5f / f / df) * (FTM_FS));
break;
#endif
#if ENABLED(FTM_SHAPER_ZVD)
case ftMotionShaper_ZVD:
#endif
#if ANY(FTM_SHAPER_ZVD, FTM_SHAPER_EI)
case ftMotionShaper_EI:
Ni[1] = LROUND((0.5f / f / df) * (FTM_FS));
Ni[2] = Ni[1] + Ni[1];
break;
#endif
#if ENABLED(FTM_SHAPER_ZVDD)
case ftMotionShaper_ZVDD:
#endif
#if ANY(FTM_SHAPER_ZVDD, FTM_SHAPER_2HEI)
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;
#endif
#if ENABLED(FTM_SHAPER_ZVDDD)
case ftMotionShaper_ZVDDD:
#endif
#if ANY(FTM_SHAPER_ZVDDD, FTM_SHAPER_3HEI)
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];
#if ANY(FTM_SHAPER_ZV, FTM_SHAPER_ZVD, FTM_SHAPER_EI, FTM_SHAPER_ZVDD, FTM_SHAPER_2HEI, FTM_SHAPER_ZVDDD, FTM_SHAPER_3HEI)
TERN_(FTM_SHAPER_ZV, case ftMotionShaper_ZV: )
TERN_(FTM_SHAPER_ZVD, case ftMotionShaper_ZVD: )
TERN_(FTM_SHAPER_EI, case ftMotionShaper_EI: )
TERN_(FTM_SHAPER_ZVDD, case ftMotionShaper_ZVDD: )
TERN_(FTM_SHAPER_2HEI, case ftMotionShaper_2HEI: )
TERN_(FTM_SHAPER_ZVDDD, case ftMotionShaper_ZVDDD: )
TERN_(FTM_SHAPER_3HEI, case ftMotionShaper_3HEI: )
base = 0.5f;
break;
#endif
#if ENABLED(FTM_SHAPER_MZV)
case ftMotionShaper_MZV:
Ni[1] = LROUND((0.375f / f / df) * (FTM_FS));
Ni[2] = Ni[1] + Ni[1];
break;
case ftMotionShaper_MZV: base = 0.375f; break;
#endif
default:
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
default:
// No echoes. max_i already set to 0 by set_axis_shaping_A
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];
#if HAS_FTM_SHAPING
Ni[0] = LROUND(centroid);
// Compute echo indices
Ni[1] = LROUND((base / f / df) * FTM_FS);
for (uint8_t i = 2; i <= max_i; ++i) Ni[i] = Ni[i - 1] + Ni[1];
// 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];
}
// 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
} // set_axis_shaping_N
#endif // FT_MOTION
Marlin/src/module/ft_motion/shaping.h
+30 -5
View File
@@ -91,14 +91,39 @@ typedef FTShapedAxes<float> ft_shaped_float_t;
typedef FTShapedAxes<ftMotionShaper_t> ft_shaped_shaper_t;
typedef FTShapedAxes<dynFreqMode_t> ft_shaped_dfm_t;
#define FTM_MAX_DAMPENING 0.25
constexpr float ftm_max_dampening = float(FTM_MAX_DAMPENING),
ftm_min_df = SQRT(1.0f - sq(ftm_max_dampening));
constexpr uint32_t CALC_N1(const float v) { return LROUND((v / FTM_MIN_SHAPE_FREQ / ftm_min_df) * (FTM_FS)); }
// Maximum delays for shaping functions
constexpr float ftm_shaping_max_i = _MAX(0.0f
OPTARG(FTM_SHAPER_ZV, 1 * CALC_N1(0.5f)) OPTARG(FTM_SHAPER_EI, 2 * CALC_N1(0.5f) )
OPTARG(FTM_SHAPER_ZVD, 2 * CALC_N1(0.5f)) OPTARG(FTM_SHAPER_2HEI, 3 * CALC_N1(0.5f) )
OPTARG(FTM_SHAPER_ZVDD, 3 * CALC_N1(0.5f)) OPTARG(FTM_SHAPER_3HEI, 4 * CALC_N1(0.5f) )
OPTARG(FTM_SHAPER_ZVDDD, 4 * CALC_N1(0.5f)) OPTARG(FTM_SHAPER_MZV, 2 * CALC_N1(0.375f))
);
// Max delays for smoothing
constexpr uint32_t ftm_smooth_max_i = uint32_t(TERN0(FTM_SMOOTHING, CEIL(FTM_FS * FTM_MAX_SMOOTHING_TIME)));
constexpr size_t ftm_zmax = ftm_shaping_max_i + ftm_smooth_max_i;
constexpr uint8_t ftm_shaping_ni_size = _MAX(1
OPTARG(FTM_SHAPER_ZV, 2) OPTARG(FTM_SHAPER_EI, 3)
OPTARG(FTM_SHAPER_ZVD, 3) OPTARG(FTM_SHAPER_2HEI, 4)
OPTARG(FTM_SHAPER_ZVDD, 4) OPTARG(FTM_SHAPER_3HEI, 5)
OPTARG(FTM_SHAPER_ZVDDD, 5) OPTARG(FTM_SHAPER_MZV, 3)
);
// 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
bool ena = false; // Enabled indication
float d_zi[ftm_zmax] = { 0.0f }; // Data point delay vector
float Ai[ftm_shaping_ni_size]; // Shaping gain vector
int32_t Ni[ftm_shaping_ni_size] = { 0 }; // 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);
Marlin/src/module/ft_motion/smoothing.h
+1 -1
View File
@@ -34,7 +34,7 @@ typedef struct FTSmoothedAxes {
} ft_smoothed_float_t;
// Smoothing data for each axis
// The smoothing algorithm used is an approximation of moving window averaging with gaussian weights, based
// 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
Marlin/src/module/planner.cpp
+17 -10
View File
@@ -783,22 +783,29 @@ block_t* Planner::get_future_block(const uint8_t offset) {
}
/**
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors. If entry_factor is 0 don't change the initial_rate.
* Assumes that the implied initial_rate and final_rate are no less than
* sqrt(block->acceleration_steps_per_s2 / 2). This is ensured through
* minimum_planner_speed_sqr / min_entry_speed_sqr - but there's one
* exception in recalculate_trapezoids().
* Calculate trapezoid (or or update FTM) motion parameters for a block.
*
* `entry_speed` is an optional override in mm/s.
* A value of `0` is a sentinel meaning “do not override the block’s
* existing entry speed / initial_rate.”
*
* This is relied upon by recalculate_trapezoids(), which intentionally
* passes `0` to preserve previously propagated speeds.
*
* Assumes implied entry and exit speeds are >= sqrt(acceleration / 2),
* enforced via minimum_planner_speed_sqr / min_entry_speed_sqr, with one
* controlled exception in recalculate_trapezoids().
*
* ############ VERY IMPORTANT ############
* NOTE: The PRECONDITION to call this function is that the block is
* NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR
* is not and will not use the block while we modify it.
* PRECONDITIONs to run this function:
* - The block is NOT BUSY.
* - The block is marked RECALCULATE.
* That WARRANTIES the Stepper ISR is not and will not use the block while we modify it.
*/
void Planner::calculate_trapezoid_for_block(block_t * const block, const float entry_speed, const float exit_speed) {
#if ENABLED(FT_MOTION)
block->entry_speed = entry_speed;
if (entry_speed) block->entry_speed = entry_speed;
block->exit_speed = exit_speed;
#endif
buildroot/tests/I3DBEEZ9_V1
+1
View File
@@ -20,6 +20,7 @@ opt_set MOTHERBOARD BOARD_I3DBEEZ9_V1 SERIAL_PORT -1 \
X_DRIVER_TYPE TMC2209 Y_DRIVER_TYPE TMC2130
opt_enable FT_MOTION FTM_SMOOTHING FTM_HOME_AND_PROBE FT_MOTION_MENU FTM_RESONANCE_TEST EMERGENCY_PARSER \
BLTOUCH EEPROM_SETTINGS AUTO_BED_LEVELING_3POINT Z_SAFE_HOMING PINS_DEBUGGING
opt_disable FTM_SHAPER_ZVDDD FTM_SHAPER_MZV
exec_test $1 $2 "I3DBEE Z9 Board | 3 Extruders | Auto-Fan | Mixed TMC | FT Motion | BLTOUCH" "$3"
restore_configs