From e561058051da5986afa883e728697e77db59b4eb Mon Sep 17 00:00:00 2001 From: David Buezas Date: Sat, 20 Dec 2025 00:33:44 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20FT=20Motion=20edit=20with?= =?UTF-8?q?=20UI=20(#28233)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/src/HAL/STM32/timers.h | 2 +- Marlin/src/gcode/feature/ft_motion/M493.cpp | 16 +++- Marlin/src/gcode/feature/ft_motion/M494.cpp | 21 +++-- Marlin/src/lcd/extui/mks_ui/wifi_upload.cpp | 2 +- Marlin/src/lcd/menu/menu_motion.cpp | 99 ++++++++++++++------- Marlin/src/module/ft_motion.cpp | 39 +++++--- Marlin/src/module/ft_motion.h | 84 ++++++++--------- Marlin/src/module/ft_motion/shaping.h | 2 +- Marlin/src/module/ft_motion/smoothing.h | 16 ++-- Marlin/src/module/ft_motion/stepping.h | 12 +-- buildroot/tests/I3DBEEZ9_V1 | 5 +- 11 files changed, 185 insertions(+), 113 deletions(-) diff --git a/Marlin/src/HAL/STM32/timers.h b/Marlin/src/HAL/STM32/timers.h index a6f09fd24e..a6c9204a77 100644 --- a/Marlin/src/HAL/STM32/timers.h +++ b/Marlin/src/HAL/STM32/timers.h @@ -59,7 +59,7 @@ // Timer prescaler calculations #define STEPPER_TIMER_PRESCALE ((HAL_TIMER_RATE) / (STEPPER_TIMER_RATE)) -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (MHz) Stepper Timer ticks per µs +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000UL) // (ticks/μs) Stepper Timer ticks per µs // Pulse Timer (counter) calculations #define PULSE_TIMER_RATE STEPPER_TIMER_RATE // (Hz) Frequency of Pulse Timer diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index 25ab2460f3..272c3d7ab3 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -27,6 +27,7 @@ #include "../../gcode.h" #include "../../../module/ft_motion.h" #include "../../../module/stepper.h" +#include "../../../lcd/marlinui.h" void say_shaper_type(const AxisEnum a, bool &sep, const char axis_name) { if (sep) SERIAL_ECHOPGM(" ; "); @@ -87,7 +88,7 @@ void say_shaping() { #if HAS_X_AXIS SERIAL_CHAR(STEPPER_A_NAME); SERIAL_ECHO_TERNARY(dynamic, " ", "base dynamic", "static", " shaper frequency: "); - SERIAL_ECHO(p_float_t(c.baseFreq.x, 2), F("Hz")); + SERIAL_ECHO(p_float_t(c.baseFreq.x, 2), F(" Hz")); #if HAS_DYNAMIC_FREQ if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(c.dynFreqK.x, 2), F("Hz/"), z_based ? F("mm") : F("g")); #endif @@ -113,6 +114,16 @@ void say_shaping() { #endif SERIAL_EOL(); #endif + + #if ENABLED(FTM_SHAPER_E) + SERIAL_CHAR('E'); + SERIAL_ECHO_TERNARY(dynamic, " ", "base dynamic", "static", " shaper frequency: "); + SERIAL_ECHO(p_float_t(c.baseFreq.e, 2), F(" Hz")); + #if HAS_DYNAMIC_FREQ + if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(c.dynFreqK.e, 2), F("Hz/"), z_based ? F("mm") : F("g")); + #endif + SERIAL_EOL(); + #endif } } @@ -483,7 +494,8 @@ void GcodeSuite::M493() { #endif // FTM_SHAPER_E - if (flag.update) ftMotion.update_shaping_params(); + if (flag.update || flag.report) + ui.refresh(); if (flag.report) say_shaping(); } diff --git a/Marlin/src/gcode/feature/ft_motion/M494.cpp b/Marlin/src/gcode/feature/ft_motion/M494.cpp index 17a9227148..d6161a9574 100644 --- a/Marlin/src/gcode/feature/ft_motion/M494.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M494.cpp @@ -27,6 +27,7 @@ #include "../../../module/ft_motion.h" #include "../../../module/stepper.h" #include "../../../module/planner.h" +#include "../../../lcd/marlinui.h" void say_ftm_settings() { #if ANY(FTM_POLYS, FTM_SMOOTHING) @@ -111,14 +112,17 @@ void GcodeSuite::M494() { #if ENABLED(FTM_SMOOTHING) - #define SMOOTH_SET(A,N) \ - if (parser.seenval(CHARIFY(A))) { \ - if (ftMotion.set_smoothing_time(_AXIS(A), parser.value_float())) \ - report = true; \ - else \ - SERIAL_ECHOLNPGM("?Invalid ", C(N), " smoothing time (", C(CHARIFY(A)), ") value."); \ + auto smooth_set = [](AxisEnum axis, char axis_name) { + if (parser.seenval(IAXIS_CHAR(axis))) { + if (ftMotion.set_smoothing_time(axis, parser.value_float())) + return true; + else + SERIAL_ECHOLNPGM("?Invalid ", C(axis_name), " smoothing time (", C(IAXIS_CHAR(axis)), ") value."); } + return false; + }; + #define SMOOTH_SET(A,N) report |= smooth_set(_AXIS(A), N); CARTES_GANG( SMOOTH_SET(X, STEPPER_A_NAME), SMOOTH_SET(Y, STEPPER_B_NAME), SMOOTH_SET(Z, STEPPER_C_NAME), SMOOTH_SET(E, 'E') @@ -126,7 +130,10 @@ void GcodeSuite::M494() { #endif // FTM_SMOOTHING - if (report) say_ftm_settings(); + if (report) { + ui.refresh(); + say_ftm_settings(); + } } #endif // FT_MOTION diff --git a/Marlin/src/lcd/extui/mks_ui/wifi_upload.cpp b/Marlin/src/lcd/extui/mks_ui/wifi_upload.cpp index 3ce8bb42ec..93a9767fc0 100644 --- a/Marlin/src/lcd/extui/mks_ui/wifi_upload.cpp +++ b/Marlin/src/lcd/extui/mks_ui/wifi_upload.cpp @@ -422,7 +422,7 @@ EspUploadResult doCommand(uint8_t op, const uint8_t *data, size_t dataLen, uint3 return stat; } -// Send a synchronising packet to the serial port in an attempt to induce +// Send a synchronizing packet to the serial port in an attempt to induce // the ESP8266 to auto-baud lock on the baud rate. EspUploadResult sync(uint16_t timeout) { uint8_t buf[36]; diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index 0c21fdd6aa..a9261dc922 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -359,8 +359,7 @@ void menu_move() { #endif void ftm_menu_set_shaper(const ftMotionShaper_t s) { - ftMotion.cfg.shaper[MenuItemBase::itemIndex] = s; - ftMotion.update_shaping_params(); + queue.inject(TS(F("M493"), IAXIS_CHAR(MenuItemBase::itemIndex), 'C', int(s))); ui.go_back(); } @@ -371,7 +370,7 @@ void menu_move() { START_MENU(); 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_NONE) ACTION_ITEM_N(axis, MSG_LCD_OFF, []{ ftm_menu_set_shaper(ftMotionShaper_NONE) ; }); 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) ; })); @@ -391,9 +390,15 @@ void menu_move() { START_MENU(); BACK_ITEM(MSG_FIXED_TIME_MOTION); - if (traj_type != TrajectoryType::TRAPEZOIDAL) ACTION_ITEM(MSG_FTM_TRAPEZOIDAL, []{ ftMotion.updateTrajectoryType(TrajectoryType::TRAPEZOIDAL); ui.go_back(); }); - if (traj_type != TrajectoryType::POLY5) ACTION_ITEM(MSG_FTM_POLY5, []{ ftMotion.updateTrajectoryType(TrajectoryType::POLY5); ui.go_back(); }); - if (traj_type != TrajectoryType::POLY6) ACTION_ITEM(MSG_FTM_POLY6, []{ ftMotion.updateTrajectoryType(TrajectoryType::POLY6); ui.go_back(); }); + if (traj_type != TrajectoryType::TRAPEZOIDAL) ACTION_ITEM(MSG_FTM_TRAPEZOIDAL, []{ + queue.inject(TS(F("M494"), 'T', int(TrajectoryType::TRAPEZOIDAL))); ui.go_back(); + }); + if (traj_type != TrajectoryType::POLY5) ACTION_ITEM(MSG_FTM_POLY5, []{ + queue.inject(TS(F("M494"), 'T', int(TrajectoryType::POLY5))); ui.go_back(); + }); + if (traj_type != TrajectoryType::POLY6) ACTION_ITEM(MSG_FTM_POLY6, []{ + queue.inject(TS(F("M494"), 'T', int(TrajectoryType::POLY6))); ui.go_back(); + }); END_MENU(); } @@ -441,12 +446,18 @@ void menu_move() { START_MENU(); BACK_ITEM_N(MenuItemBase::itemIndex, MSG_FTM_CONFIGURE_AXIS_N); - if (dmode != dynFreqMode_DISABLED) ACTION_ITEM(MSG_LCD_OFF, []{ (void)ftMotion.cfg.setDynFreqMode(dynFreqMode_DISABLED); ui.go_back(); }); + if (dmode != dynFreqMode_DISABLED) ACTION_ITEM(MSG_LCD_OFF, []{ + queue.inject(TS(F("M493D"), int(dynFreqMode_DISABLED))); ui.go_back(); + }); #if HAS_DYNAMIC_FREQ_MM - if (dmode != dynFreqMode_Z_BASED) ACTION_ITEM(MSG_FTM_Z_BASED, []{ (void)ftMotion.cfg.setDynFreqMode(dynFreqMode_Z_BASED); ui.go_back(); }); + if (dmode != dynFreqMode_Z_BASED) ACTION_ITEM(MSG_FTM_Z_BASED, []{ + queue.inject(TS(F("M493D"), int(dynFreqMode_Z_BASED))); ui.go_back(); + }); #endif #if HAS_DYNAMIC_FREQ_G - if (dmode != dynFreqMode_MASS_BASED) ACTION_ITEM(MSG_FTM_MASS_BASED, []{ (void)ftMotion.cfg.setDynFreqMode(dynFreqMode_MASS_BASED); ui.go_back(); }); + if (dmode != dynFreqMode_MASS_BASED) ACTION_ITEM(MSG_FTM_MASS_BASED, []{ + queue.inject(TS(F("M493D"), int(dynFreqMode_MASS_BASED))); ui.go_back(); + }); #endif END_MENU(); @@ -460,33 +471,45 @@ void menu_move() { START_MENU(); BACK_ITEM(MSG_FIXED_TIME_MOTION); - #if HAS_FTM_EI_SHAPING - #define EISHAPER_MENU_ITEM(A) \ - if (AXIS_IS_EISHAPING(A)) \ - EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_VTOL_N, &c.vtol[axis], 0.0f, 1.0f, ftMotion.update_shaping_params); - #else - #define EISHAPER_MENU_ITEM(A) NOOP - #endif - if (false SHAPED_GANG(|| axis == X_AXIS, || axis == Y_AXIS, || axis == Z_AXIS, || axis == E_AXIS)) { + SUBMENU_N_S(axis, get_shaper_name(axis), MSG_FTM_CMPN_MODE, menu_ftm_shaper); - if (AXIS_IS_SHAPING(axis)) { - EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_BASE_FREQ_N, &c.baseFreq[axis], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params); - EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_ZETA_N, &c.zeta[axis], 0.0f, 1.0f, ftMotion.update_shaping_params); - EISHAPER_MENU_ITEM(axis); + if (IS_SHAPING(c.shaper[axis])) { + editable.decimal = c.baseFreq[axis]; + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_BASE_FREQ_N, &editable.decimal, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, []{ + queue.inject(TS(F("M493"), IAXIS_CHAR(MenuItemBase::itemIndex), 'A', p_float_t(editable.decimal, 3))); + }); + editable.decimal = c.zeta[axis]; + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_ZETA_N, &editable.decimal, 0.0f, FTM_MAX_DAMPENING, []{ + queue.inject(TS(F("M493"), IAXIS_CHAR(MenuItemBase::itemIndex), 'I', p_float_t(editable.decimal, 3))); + }); + #if HAS_FTM_EI_SHAPING + if (IS_EISHAPING(c.shaper[axis])) { + editable.decimal = c.vtol[axis]; + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_VTOL_N, &editable.decimal, 0.0f, 1.0f, []{ + queue.inject(TS(F("M493"), IAXIS_CHAR(MenuItemBase::itemIndex), 'Q', p_float_t(editable.decimal, 3))); + }); + } + #endif } } #if ENABLED(FTM_SMOOTHING) editable.decimal = c.smoothingTime[axis]; - EDIT_ITEM_FAST_N(float43, axis, MSG_FTM_SMOOTH_TIME_N, &editable.decimal, 0.0f, FTM_MAX_SMOOTHING_TIME, []{ (void)ftMotion.set_smoothing_time(AxisEnum(MenuItemBase::itemIndex), editable.decimal); }); + EDIT_ITEM_FAST_N(float43, axis, MSG_FTM_SMOOTH_TIME_N, &editable.decimal, 0.0f, FTM_MAX_SMOOTHING_TIME, []{ + queue.inject(TS(F("M494"), IAXIS_CHAR(MenuItemBase::itemIndex), p_float_t(editable.decimal, 4))); + }); #endif #if HAS_DYNAMIC_FREQ if (axis == X_AXIS || axis == Y_AXIS) { SUBMENU_N_S(axis, get_dyn_freq_mode_name(), MSG_FTM_DYN_MODE, menu_ftm_dyn_mode); - if (c.dynFreqMode != dynFreqMode_DISABLED) - EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_DFREQ_K_N, &c.dynFreqK[axis], 0.0f, 20.0f); + if (c.dynFreqMode != dynFreqMode_DISABLED) { + editable.decimal = c.dynFreqK[axis]; + EDIT_ITEM_FAST_N(float42_52, axis, MSG_FTM_DFREQ_K_N, &editable.decimal, 0.0f, 20.0f, []{ + queue.inject(TS(F("M493"), IAXIS_CHAR(MenuItemBase::itemIndex), 'F', p_float_t(editable.decimal, 3))); + }); + } } #endif @@ -502,8 +525,12 @@ void menu_move() { BACK_ITEM(MSG_MOTION); #if HAS_STANDARD_MOTION - bool show_state = c.active; - EDIT_ITEM(bool, MSG_FIXED_TIME_MOTION, &show_state, []{ (void)ftMotion.toggle(); }); + // Because this uses G-code the display of the actual state will be delayed by an unknown period of time. + // To fix this G-codes M493/M494 could refresh the UI when they are done. + editable.state = c.active; + EDIT_ITEM(bool, MSG_FIXED_TIME_MOTION, &editable.state, []{ + queue.inject(TS(F("M493"), 'S', int(editable.state))); + }); #endif // Show only when FT Motion is active (or optionally always show) @@ -511,14 +538,20 @@ void menu_move() { #if ENABLED(FTM_POLYS) SUBMENU_S(ftMotion.getTrajectoryName(), 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); + if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6) { + editable.decimal = c.poly6_acceleration_overshoot; + EDIT_ITEM(float42_52, MSG_FTM_POLY6_OVERSHOOT, &editable.decimal, 1.25f, 1.875f, []{ + queue.inject(TS(F("M494"), 'O', editable.decimal)); + }); + } #endif CARTES_MAP(_FTM_AXIS_SUBMENU); editable.state = c.axis_sync_enabled; - EDIT_ITEM(bool, MSG_FTM_AXIS_SYNC, &editable.state, []{ c.setAxisSync(editable.state); }); + EDIT_ITEM(bool, MSG_FTM_AXIS_SYNC, &editable.state, []{ + queue.inject(TS(F("M493"), IAXIS_CHAR(MenuItemBase::itemIndex), 'T', int(editable.state))); + }); #if ENABLED(FTM_RESONANCE_TEST) SUBMENU(MSG_FTM_RESONANCE_TEST, menu_ftm_resonance_test); @@ -562,8 +595,12 @@ 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, &ftMotion.cfg.poly6_acceleration_overshoot, 1.25f, 1.875f); + if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6) { + editable.decimal = ftMotion.cfg.poly6_acceleration_overshoot; + EDIT_ITEM(float42_52, MSG_FTM_POLY6_OVERSHOOT, &editable.decimal, 1.25f, 1.875f, []{ + queue.inject(TS(F("M494"), 'O', editable.decimal)); + }); + } #endif SHAPED_MAP(_FTM_AXIS_SUBMENU); diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index c8db197655..b7677df99f 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -51,6 +51,9 @@ FTMotion ftMotion; +void ft_config_t::prep_for_shaper_change() { ftMotion.prep_for_shaper_change(); } +void ft_config_t::update_shaping_params() { TERN_(HAS_FTM_SHAPING, ftMotion.update_shaping_params()); } + //----------------------------------------------------------------- // Variables. //----------------------------------------------------------------- @@ -70,6 +73,7 @@ xyze_pos_t FTMotion::startPos, // (mm) Start position of bl FTMotion::endPos_prevBlock = { 0.0f }; // (mm) End position of previous block xyze_float_t FTMotion::ratio; // (ratio) Axis move ratio of block float FTMotion::tau = 0.0f; // (s) Time since start of block +bool FTMotion::fastForwardUntilMotion = false; // Fast forward time if there is no motion // Trajectory generators TrapezoidalTrajectoryGenerator FTMotion::trapezoidalGenerator; @@ -191,12 +195,16 @@ void FTMotion::loop() { #if HAS_FTM_SHAPING void FTMotion::update_shaping_params() { - #define UPDATE_SHAPER(A) \ - shaping.A.ena = IS_SHAPING(ftMotion.cfg.shaper.A); \ - shaping.A.set_axis_shaping_A(cfg.shaper.A, cfg.zeta.A OPTARG(HAS_FTM_EI_SHAPING, cfg.vtol.A)); \ - shaping.A.set_axis_shaping_N(cfg.shaper.A, cfg.baseFreq.A, cfg.zeta.A); + prep_for_shaper_change(); + auto update_shaper = [&](AxisEnum axis, axis_shaping_t &shap) { + shap.ena = IS_SHAPING(cfg.shaper[axis]); + shap.set_axis_shaping_A(cfg.shaper[axis], cfg.zeta[axis] OPTARG(HAS_FTM_EI_SHAPING, cfg.vtol[axis])); + shap.set_axis_shaping_N(cfg.shaper[axis], cfg.baseFreq[axis], cfg.zeta[axis]); + }; + #define UPDATE_SHAPER(A) update_shaper(_AXIS(A), shaping.A); SHAPED_MAP(UPDATE_SHAPER); + shaping.refresh_largest_delay_samples(); } @@ -212,9 +220,9 @@ void FTMotion::loop() { smoothing.refresh_largest_delay_samples(); } - bool FTMotion::set_smoothing_time(const AxisEnum axis, const float s_time) { - if (!WITHIN(s_time, 0.0f, FTM_MAX_SMOOTHING_TIME)) return false; - planner.synchronize(); + bool FTMotion::set_smoothing_time(const AxisEnum axis, float s_time) { + LIMIT(s_time, 0.0f, FTM_MAX_SMOOTHING_TIME); + prep_for_shaper_change(); cfg.smoothingTime[axis] = s_time; update_smoothing_params(); return true; @@ -229,6 +237,7 @@ void FTMotion::reset() { tau = 0; stepping.reset(); shaping.reset(); + fastForwardUntilMotion = true; TERN_(FTM_SMOOTHING, smoothing.reset();); TERN_(HAS_EXTRUDERS, prev_traj_e = 0.0f); // Reset linear advance variables. @@ -315,7 +324,7 @@ void FTMotion::init() { case TrajectoryType::POLY6: break; } - planner.synchronize(); + prep_for_shaper_change(); setTrajectoryType(type); return true; } @@ -590,10 +599,16 @@ void FTMotion::fill_stepper_plan_buffer() { // Get distance from trajectory generator xyze_float_t traj_coords = calc_traj_point(currentGenerator->getDistanceAtTime(tau)); - - // Calculate and store stepper plan in buffer - stepping_enqueue(traj_coords); - + if (fastForwardUntilMotion && traj_coords == startPos) { + // Axis synchronization delays all axes. When coming from a reset, there is a ramp up time filling all buffers. + // If the slowest axis doesn't move and it isn't smoothened, this time can be skipped. + // It eliminates idle time when changing smoothing time or shapers and speeds up homing and bed leveling. + } + else { + fastForwardUntilMotion = false; + // Calculate and store stepper plan in buffer + stepping_enqueue(traj_coords); + } } } diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 88a9a49e4d..42dab9951d 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -98,20 +98,25 @@ typedef struct FTConfig { static constexpr TrajectoryType trajectory_type = TrajectoryType::TRAPEZOIDAL; #endif + static void prep_for_shaper_change(); + static void update_shaping_params(); + #if HAS_STANDARD_MOTION bool setActive(const bool a) { if (a == active) return false; stepper.ftMotion_syncPosition(); - planner.synchronize(); + prep_for_shaper_change(); active = a; + update_shaping_params(); return true; } #endif bool setAxisSync(const bool ena) { if (ena == axis_sync_enabled) return false; - planner.synchronize(); + prep_for_shaper_change(); axis_sync_enabled = ena; + update_shaping_params(); return true; } @@ -119,18 +124,20 @@ typedef struct FTConfig { bool setShaper(const AxisEnum a, const ftMotionShaper_t s) { if (s == shaper[a]) return false; - planner.synchronize(); + prep_for_shaper_change(); shaper[a] = s; + update_shaping_params(); return true; } - constexpr bool goodZeta(const float z) { return WITHIN(z, 0.01f, ftm_max_dampening); } + constexpr bool goodZeta(const float z) { return WITHIN(z, 0.00f, ftm_max_dampening); } - bool setZeta(const AxisEnum a, const float z) { + bool setZeta(const AxisEnum a, float z) { if (z == zeta[a]) return false; - if (!goodZeta(z)) return false; - planner.synchronize(); + LIMIT(z, 0.00f, ftm_max_dampening); + prep_for_shaper_change(); zeta[a] = z; + update_shaping_params(); return true; } @@ -138,11 +145,12 @@ typedef struct FTConfig { constexpr bool goodVtol(const float v) { return WITHIN(v, 0.00f, 1.0f); } - bool setVtol(const AxisEnum a, const float v) { + bool setVtol(const AxisEnum a, float v) { if (v == vtol[a]) return false; - if (!goodVtol(v)) return false; - planner.synchronize(); + LIMIT(v, 0.00f, 1.0f); + prep_for_shaper_change(); vtol[a] = v; + update_shaping_params(); return true; } @@ -157,10 +165,11 @@ typedef struct FTConfig { TERN_(HAS_DYNAMIC_FREQ_MM, case dynFreqMode_Z_BASED:) TERN_(HAS_DYNAMIC_FREQ_G, case dynFreqMode_MASS_BASED:) case dynFreqMode_DISABLED: - planner.synchronize(); + prep_for_shaper_change(); dynFreqMode = dynFreqMode_t(m); break; } + update_shaping_params(); return 1; } @@ -172,8 +181,9 @@ typedef struct FTConfig { bool setDynFreqK(const AxisEnum a, const float k) { if (!modeUsesDynFreq()) return false; if (k == dynFreqK[a]) return false; - planner.synchronize(); + prep_for_shaper_change(); dynFreqK[a] = k; + update_shaping_params(); return true; } @@ -183,11 +193,12 @@ typedef struct FTConfig { constexpr bool goodBaseFreq(const float f) { return WITHIN(f, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2); } - bool setBaseFreq(const AxisEnum a, const float f) { + bool setBaseFreq(const AxisEnum a, float f) { if (f == baseFreq[a]) return false; - if (!goodBaseFreq(a)) return false; - planner.synchronize(); + LIMIT(f, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2); + prep_for_shaper_change(); baseFreq[a] = f; + update_shaping_params(); return true; } @@ -216,6 +227,8 @@ typedef struct FTConfig { #endif // HAS_FTM_SHAPING TERN_(FTM_POLYS, poly6_acceleration_overshoot = FTM_POLY6_ACCELERATION_OVERSHOOT); + + update_shaping_params(); } } ft_config_t; @@ -238,8 +251,6 @@ class FTMotion { static void set_defaults() { cfg.set_defaults(); - TERN_(HAS_FTM_SHAPING, update_shaping_params()); - #if ENABLED(FTM_SMOOTHING) #define _RESET_SMOOTH(A) (void)set_smoothing_time(_AXIS(A), FTM_SMOOTHING_TIME_##A); CARTES_MAP(_RESET_SMOOTH); @@ -262,11 +273,6 @@ class FTMotion { static ResonanceGenerator rtg; // Resonance trajectory generator instance #endif - #if HAS_FTM_SHAPING - // Refresh gains and indices used by shaping functions. - static void update_shaping_params(); - #endif - #if ENABLED(FTM_SMOOTHING) // Refresh alpha and delay samples used by smoothing functions. static void update_smoothing_params(); @@ -286,26 +292,6 @@ class FTMotion { } #endif - // Setters for baseFreq, zeta, vtol - static bool setBaseFreq(const AxisEnum a, const float f) { - if (!cfg.setBaseFreq(a, f)) return false; - update_shaping_params(); - return true; - } - static bool setZeta(const AxisEnum a, const float z) { - if (!cfg.setZeta(a, z)) return false; - update_shaping_params(); - return true; - } - - #if HAS_FTM_EI_SHAPING - static bool setVtol(const AxisEnum a, const float v) { - if (!cfg.setVtol(a, v)) return false; - update_shaping_params(); - return true; - } - #endif - // Trajectory generator selection #if ENABLED(FTM_POLYS) static void setTrajectoryType(const TrajectoryType type); @@ -343,6 +329,7 @@ class FTMotion { endPos_prevBlock; // (mm) End position of previous block static xyze_float_t ratio; // (ratio) Axis move ratio of block static float tau; // (s) Time since start of block + static bool fastForwardUntilMotion; // Fast forward time if there is no motion // Trajectory generators static TrapezoidalTrajectoryGenerator trapezoidalGenerator; @@ -379,6 +366,19 @@ class FTMotion { static float prev_traj_e; #endif + #if HAS_FTM_SHAPING + // Refresh gains and indices used by shaping functions. + friend void ft_config_t::update_shaping_params(); + static void update_shaping_params(); + #endif + + // Synchronize and reset motion prior to parameter changes + friend void ft_config_t::prep_for_shaper_change(); + static void prep_for_shaper_change() { + planner.synchronize(); + reset(); + } + // Buffers static void discard_planner_block_protected(); static uint32_t calc_runout_samples(); diff --git a/Marlin/src/module/ft_motion/shaping.h b/Marlin/src/module/ft_motion/shaping.h index 89e6612fdc..0140a969ba 100644 --- a/Marlin/src/module/ft_motion/shaping.h +++ b/Marlin/src/module/ft_motion/shaping.h @@ -142,7 +142,7 @@ typedef struct Shaping { 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). + // Ni[0] stores how far in the past the first step would need to happen to avoid desynchronization (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])); } diff --git a/Marlin/src/module/ft_motion/smoothing.h b/Marlin/src/module/ft_motion/smoothing.h index 7ca12efdba..e3f9962a09 100644 --- a/Marlin/src/module/ft_motion/smoothing.h +++ b/Marlin/src/module/ft_motion/smoothing.h @@ -34,22 +34,22 @@ 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 -// on chained exponential smoothers. +// For the smoothing algorithm use 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_time(const float s_time); // Set smoothing time, recalculate alpha and delay. + float alpha = 0.0f; // Pre-calculated alpha for smoothing + uint32_t delay_samples = 0; // Pre-calculated delay in samples for smoothing + void set_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). + // Smoothing causes a phase delay equal to smoothing_time. This delay is compensated-for during axis synchronization, + // 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 + // Note: The delay equals smoothing_time only if the input signal frequency is under 1/smoothing_time; which, luckily, holds in this case. void reset() { #define _CLEAR(A) ZERO(A.smoothing_pass); LOGICAL_AXIS_MAP(_CLEAR); diff --git a/Marlin/src/module/ft_motion/stepping.h b/Marlin/src/module/ft_motion/stepping.h index ff72d90be1..b7cbe8391b 100644 --- a/Marlin/src/module/ft_motion/stepping.h +++ b/Marlin/src/module/ft_motion/stepping.h @@ -30,22 +30,22 @@ FORCE_INLINE constexpr uint32_t a_times_b_shift_16(const uint32_t a, const uint3 const uint32_t hi = a >> 16, lo = a & 0x0000FFFF; return (hi * b) + ((lo * b) >> 16); } + +// Count leading zeroes of v when stored in a 32 bit uint, equivalent to `32 - ceil(log2(v))` constexpr int CLZ32(const uint32_t v, const int c=0) { return v ? (TEST32(v, 31)) ? c : CLZ32(v << 1, c + 1) : 32; } #define FTM_NEVER uint32_t(UINT16_MAX) // Reserved number to indicate "no ticks in this frame" (FRAME_TICKS_FP+1 would work too) -constexpr uint32_t FRAME_TICKS = STEPPER_TIMER_RATE / FTM_FS; // Timer ticks per frame (by default, 1kHz) -constexpr uint32_t TICKS_BITS = CLZ32(FRAME_TICKS + 1U); // Bits to represent the max value (duration of a frame, +1 one for FTM_NEVER). -constexpr uint32_t FTM_Q_INT = 32u - TICKS_BITS; // Bits remaining - // "clz" counts leading zeroes. +constexpr uint32_t FRAME_TICKS = STEPPER_TIMER_RATE / FTM_FS; // Timer ticks per frame +constexpr uint32_t FTM_Q_INT = 32u - CLZ32(FRAME_TICKS + 1U); // Bits to represent the integer part of the max value (duration of a frame, +1 one for FTM_NEVER). constexpr uint32_t FTM_Q = 16u - FTM_Q_INT; // uint16 interval fractional bits. // Intervals buffer has fixed point numbers with the point on this position static_assert(FRAME_TICKS < FTM_NEVER, "(STEPPER_TIMER_RATE / FTM_FS) (" STRINGIFY(STEPPER_TIMER_RATE) " / " STRINGIFY(FTM_FS) ") must be < " STRINGIFY(FTM_NEVER) " to fit 16-bit fixed-point numbers."); // Sanity check -static_assert(FRAME_TICKS != 2000 || FTM_Q == 5, "FTM_Q should be 5"); -static_assert(FRAME_TICKS != 25000 || FTM_Q == 1, "FTM_Q should be 1"); +static_assert(POW(2, 16 - FTM_Q) > FRAME_TICKS, "FRAME_TICKS in Q format should fit in a uint16"); +static_assert(POW(2, 16 - FTM_Q - 1) <= FRAME_TICKS, "A smaller FTM_Q would still alow a FRAME_TICKS in Q format to fit in a uint16"); // The _FP and _fp suffixes mean the number is in fixed point format with the point at the FTM_Q position. // See: https://en.wikipedia.org/wiki/Fixed-point_arithmetic diff --git a/buildroot/tests/I3DBEEZ9_V1 b/buildroot/tests/I3DBEEZ9_V1 index 126bfc1df8..af6f529289 100755 --- a/buildroot/tests/I3DBEEZ9_V1 +++ b/buildroot/tests/I3DBEEZ9_V1 @@ -18,8 +18,9 @@ opt_set MOTHERBOARD BOARD_I3DBEEZ9_V1 SERIAL_PORT -1 \ EXTRUDERS 3 TEMP_SENSOR_1 1 TEMP_SENSOR_2 1 \ E0_AUTO_FAN_PIN PC10 E1_AUTO_FAN_PIN PC11 E2_AUTO_FAN_PIN PC12 \ 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_enable FT_MOTION FTM_SMOOTHING FTM_HOME_AND_PROBE FTM_RESONANCE_TEST FT_MOTION_MENU \ + REPRAP_DISCOUNT_SMART_CONTROLLER EMERGENCY_PARSER EEPROM_SETTINGS \ + BLTOUCH 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"