diff --git a/Marlin/src/core/types.h b/Marlin/src/core/types.h index 7008d04c53..b4d7705ffd 100644 --- a/Marlin/src/core/types.h +++ b/Marlin/src/core/types.h @@ -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 diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index 85e2fc9d3b..f38bbf9ca1 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -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 diff --git a/Marlin/src/gcode/feature/ft_motion/M494.cpp b/Marlin/src/gcode/feature/ft_motion/M494.cpp index 212a69c0a2..17a9227148 100644 --- a/Marlin/src/gcode/feature/ft_motion/M494.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M494.cpp @@ -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, diff --git a/Marlin/src/inc/Conditionals-5-post.h b/Marlin/src/inc/Conditionals-5-post.h index 08c600749f..fb2a6865f2 100644 --- a/Marlin/src/inc/Conditionals-5-post.h +++ b/Marlin/src/inc/Conditionals-5-post.h @@ -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 diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index ddc2f90a7b..4c7dea00f0 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -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); diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index c4c2e5f302..2c58119b97 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -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 diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index bf6a38a88f..ad1417fd44 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -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 diff --git a/Marlin/src/module/ft_motion/shaping.cpp b/Marlin/src/module/ft_motion/shaping.cpp index e568aba791..bfefe77ae7 100644 --- a/Marlin/src/module/ft_motion/shaping.cpp +++ b/Marlin/src/module/ft_motion/shaping.cpp @@ -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 diff --git a/Marlin/src/module/ft_motion/shaping.h b/Marlin/src/module/ft_motion/shaping.h index f207b3d2b7..89e6612fdc 100644 --- a/Marlin/src/module/ft_motion/shaping.h +++ b/Marlin/src/module/ft_motion/shaping.h @@ -91,14 +91,39 @@ typedef FTShapedAxes ft_shaped_float_t; typedef FTShapedAxes ft_shaped_shaper_t; typedef FTShapedAxes 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); diff --git a/Marlin/src/module/ft_motion/smoothing.h b/Marlin/src/module/ft_motion/smoothing.h index 3e32b2b917..7ca12efdba 100644 --- a/Marlin/src/module/ft_motion/smoothing.h +++ b/Marlin/src/module/ft_motion/smoothing.h @@ -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 diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index f39a358457..c1a2e4bc61 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -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 diff --git a/buildroot/tests/I3DBEEZ9_V1 b/buildroot/tests/I3DBEEZ9_V1 index 4cf64ec044..126bfc1df8 100755 --- a/buildroot/tests/I3DBEEZ9_V1 +++ b/buildroot/tests/I3DBEEZ9_V1 @@ -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