From 909d1dae817177f86f21d7045aef4aae924af996 Mon Sep 17 00:00:00 2001
From: narno2202 <130909513+narno2202@users.noreply.github.com>
Date: Wed, 20 Dec 2023 02:56:47 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=E2=9A=A1=EF=B8=8F=20FT=5FMOTION=20?=
 =?UTF-8?q?improvements=20(#26074)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-Authored-By: Scott Lahteine <thinkyhead@users.noreply.github.com>
---
 Marlin/Configuration_adv.h                  |  84 ++-
 Marlin/src/gcode/bedlevel/G35.cpp           |   4 +-
 Marlin/src/gcode/bedlevel/abl/G29.cpp       |   1 +
 Marlin/src/gcode/bedlevel/mbl/G29.cpp       |   1 +
 Marlin/src/gcode/calibrate/G34_M422.cpp     |   1 +
 Marlin/src/gcode/feature/ft_motion/M493.cpp | 107 ++-
 Marlin/src/gcode/probe/G38.cpp              |   1 +
 Marlin/src/inc/Conditionals_adv.h           |   2 +-
 Marlin/src/lcd/language/language_en.h       |   6 +-
 Marlin/src/lcd/menu/menu_motion.cpp         |  23 +-
 Marlin/src/module/ft_motion.cpp             | 701 ++++++++++----------
 Marlin/src/module/ft_motion.h               |  69 +-
 Marlin/src/module/ft_types.h                |  29 +-
 Marlin/src/module/motion.cpp                |  19 -
 Marlin/src/module/stepper.cpp               | 244 +++----
 Marlin/src/module/stepper.h                 |   7 +-
 16 files changed, 654 insertions(+), 645 deletions(-)

diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 8b4fa1ab7ca18..72aee115884d1 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -1121,42 +1121,62 @@
 #if ENABLED(FT_MOTION)
   #define FTM_DEFAULT_MODE        ftMotionMode_DISABLED // Default mode of fixed time control. (Enums in ft_types.h)
   #define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (Enums in ft_types.h)
-  #define FTM_SHAPING_DEFAULT_X_FREQ 37.0f              // (Hz) Default peak frequency used by input shapers.
-  #define FTM_SHAPING_DEFAULT_Y_FREQ 37.0f              // (Hz) Default peak frequency used by input shapers.
-  #define FTM_LINEAR_ADV_DEFAULT_ENA false              // Default linear advance enable (true) or disable (false).
-  #define FTM_LINEAR_ADV_DEFAULT_K    0.0f              // Default linear advance gain.
-  #define FTM_SHAPING_ZETA            0.1f              // Zeta used by input shapers.
-  #define FTM_SHAPING_V_TOL           0.05f             // Vibration tolerance used by EI input shapers.
+  #define FTM_SHAPING_DEFAULT_X_FREQ   37.0f      // (Hz) Default peak frequency used by input shapers
+  #define FTM_SHAPING_DEFAULT_Y_FREQ   37.0f      // (Hz) Default peak frequency used by input shapers
+  #define FTM_LINEAR_ADV_DEFAULT_ENA   false      // Default linear advance enable (true) or disable (false)
+  #define FTM_LINEAR_ADV_DEFAULT_K      0.0f      // Default linear advance gain
+  #define FTM_SHAPING_ZETA_X            0.1f      // Zeta used by input shapers for X axis
+  #define FTM_SHAPING_ZETA_Y            0.1f      // Zeta used by input shapers for Y axis
+
+  #define FTM_SHAPING_V_TOL_X           0.05f     // Vibration tolerance used by EI input shapers for X axis
+  #define FTM_SHAPING_V_TOL_Y           0.05f     // Vibration tolerance used by EI input shapers for Y axis
+
+  //#define FT_MOTION_MENU                        // Provide a MarlinUI menu to set M493 parameters
 
   /**
    * Advanced configuration
    */
-  #define FTM_BATCH_SIZE            100                 // Batch size for trajectory generation;
-  #define FTM_WINDOW_SIZE           200                 // Window size for trajectory generation.
-  #define FTM_FS                   1000                 // (Hz) Frequency for trajectory generation. (1 / FTM_TS)
-  #define FTM_TS                      0.001f            // (s) Time step for trajectory generation. (1 / FTM_FS)
-  #define FTM_STEPPER_FS          20000                 // (Hz) Frequency for stepper I/O update.
-  #define FTM_MIN_TICKS ((STEPPER_TIMER_RATE) / (FTM_STEPPER_FS)) // Minimum stepper ticks between steps.
-  #define FTM_MIN_SHAPE_FREQ         10                 // Minimum shaping frequency.
-  #define FTM_ZMAX                  100                 // Maximum delays for shaping functions (even numbers only!).
-                                                        // Calculate as:
-                                                        //    1/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZV.
-                                                        //    (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZVD, MZV.
-                                                        //    3/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 2HEI.
-                                                        //    2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 3HEI.
-  #define FTM_STEPS_PER_UNIT_TIME    20                 // Interpolated stepper commands per unit time.
-                                                        // Calculate as (FTM_STEPPER_FS / FTM_FS).
-  #define FTM_CTS_COMPARE_VAL        10                 // Comparison value used in interpolation algorithm.
-                                                        // Calculate as (FTM_STEPS_PER_UNIT_TIME / 2).
-  // These values may be configured to adjust duration of loop().
-  #define FTM_STEPS_PER_LOOP         60                 // Number of stepper commands to generate each loop().
-  #define FTM_POINTS_PER_LOOP       100                 // Number of trajectory points to generate each loop().
-
-  // This value may be configured to adjust duration to consume the command buffer.
-  // Try increasing this value if stepper motion is not smooth.
-  #define FTM_STEPPERCMD_BUFF_SIZE 1000                 // Size of the stepper command buffers.
-
-  //#define FT_MOTION_MENU                              // Provide a MarlinUI menu to set M493 parameters.
+  #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_FS                     1000         // (Hz) Frequency for trajectory generation. (Reciprocal of FTM_TS)
+  #define FTM_TS                        0.001f    // (s) Time step for trajectory generation. (Reciprocal of FTM_FS)
+
+  // These values may be configured to adjust the duration of loop().
+  #define FTM_STEPS_PER_LOOP           60         // Number of stepper commands to generate each loop()
+  #define FTM_POINTS_PER_LOOP         100         // Number of trajectory points to generate each loop()
+
+  #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
+                                                  // (FTM_STEPS_PER_LOOP * FTM_POINTS_PER_LOOP) is a good start
+                                                  // If you run out of memory, fall back to 3000 and increase progressively
+  #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_STEPS_PER_UNIT_TIME (FTM_STEPPER_FS / FTM_FS)       // Interpolated stepper commands per unit time
+  #define FTM_CTS_COMPARE_VAL (FTM_STEPS_PER_UNIT_TIME / 2)       // Comparison value used in interpolation algorithm
+  #define FTM_MIN_TICKS ((STEPPER_TIMER_RATE) / (FTM_STEPPER_FS)) // Minimum stepper ticks between steps
+
+  #define FTM_MIN_SHAPE_FREQ           10         // Minimum shaping frequency
+  #define FTM_RATIO (FTM_FS / FTM_MIN_SHAPE_FREQ) // Factor for use in FTM_ZMAX. DON'T CHANGE.
+  #define FTM_ZMAX (FTM_RATIO * 2)                // Maximum delays for shaping functions (even numbers only!)
+                                                  // Calculate as:
+                                                  //   ZV       : FTM_RATIO / 2
+                                                  //   ZVD, MZV : FTM_RATIO
+                                                  //   2HEI     : FTM_RATIO * 3 / 2
+                                                  //   3HEI     : FTM_RATIO * 2
 #endif
 
 /**
diff --git a/Marlin/src/gcode/bedlevel/G35.cpp b/Marlin/src/gcode/bedlevel/G35.cpp
index 1b2f115cdfb74..224dd2a0aa2d7 100644
--- a/Marlin/src/gcode/bedlevel/G35.cpp
+++ b/Marlin/src/gcode/bedlevel/G35.cpp
@@ -57,9 +57,9 @@
  *               41 - Counter-Clockwise M4
  *               50 - Clockwise M5
  *               51 - Counter-Clockwise M5
- *
- */
+ **/
 void GcodeSuite::G35() {
+
   DEBUG_SECTION(log_G35, "G35", DEBUGGING(LEVELING));
 
   if (DEBUGGING(LEVELING)) log_machine_info();
diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp
index d6d648c12560e..acd920c7290d1 100644
--- a/Marlin/src/gcode/bedlevel/abl/G29.cpp
+++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp
@@ -225,6 +225,7 @@ class G29_State {
  *     There's no extra effect if you have a fixed Z probe.
  */
 G29_TYPE GcodeSuite::G29() {
+
   DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING));
 
   // Leveling state is persistent when done manually with multiple G29 commands
diff --git a/Marlin/src/gcode/bedlevel/mbl/G29.cpp b/Marlin/src/gcode/bedlevel/mbl/G29.cpp
index c5e02b91f9bd5..b3f03d03ba435 100644
--- a/Marlin/src/gcode/bedlevel/mbl/G29.cpp
+++ b/Marlin/src/gcode/bedlevel/mbl/G29.cpp
@@ -64,6 +64,7 @@ inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM("
  *  S5              Reset and disable mesh
  */
 void GcodeSuite::G29() {
+
   DEBUG_SECTION(log_G29, "G29", true);
 
   // G29 Q is also available if debugging
diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp
index f76df4316cd66..af2c6a5f65056 100644
--- a/Marlin/src/gcode/calibrate/G34_M422.cpp
+++ b/Marlin/src/gcode/calibrate/G34_M422.cpp
@@ -79,6 +79,7 @@
  *   R                 Flag to recalculate points based on current probe offsets
  */
 void GcodeSuite::G34() {
+
   DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING));
   if (DEBUGGING(LEVELING)) log_machine_info();
 
diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp
index f6e6a1261d468..fd95a3b7baf81 100644
--- a/Marlin/src/gcode/feature/ft_motion/M493.cpp
+++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp
@@ -26,6 +26,7 @@
 
 #include "../../gcode.h"
 #include "../../../module/ft_motion.h"
+#include "../../../module/stepper.h"
 
 void say_shaping() {
   // FT Enabled
@@ -39,6 +40,8 @@ void say_shaping() {
         default: break;
         case ftMotionMode_ZV:   SERIAL_ECHOPGM("ZV");        break;
         case ftMotionMode_ZVD:  SERIAL_ECHOPGM("ZVD");       break;
+        case ftMotionMode_ZVDD: SERIAL_ECHOPGM("ZVDD");      break;
+        case ftMotionMode_ZVDDD: SERIAL_ECHOPGM("ZVDDD");    break;
         case ftMotionMode_EI:   SERIAL_ECHOPGM("EI");        break;
         case ftMotionMode_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break;
         case ftMotionMode_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break;
@@ -155,20 +158,19 @@ void GcodeSuite::M493() {
 
   if (!parser.seen_any())
     flag.report_h = true;
-  else
-    planner.synchronize();
 
   // Parse 'S' mode parameter.
   if (parser.seenval('S')) {
-    const ftMotionMode_t oldmm = ftMotion.cfg.mode,
-                         newmm = (ftMotionMode_t)parser.value_byte();
+    const ftMotionMode_t newmm = (ftMotionMode_t)parser.value_byte();
 
-    if (newmm != oldmm) {
+    if (newmm != ftMotion.cfg.mode) {
       switch (newmm) {
         default: SERIAL_ECHOLNPGM("?Invalid control mode [S] value."); return;
         #if HAS_X_AXIS
           case ftMotionMode_ZV:
           case ftMotionMode_ZVD:
+          case ftMotionMode_ZVDD:
+          case ftMotionMode_ZVDDD:
           case ftMotionMode_EI:
           case ftMotionMode_2HEI:
           case ftMotionMode_3HEI:
@@ -177,11 +179,10 @@ void GcodeSuite::M493() {
           //case ftMotionMode_DISCTF:
             flag.update_n = flag.update_a = true;
         #endif
-        case ftMotionMode_DISABLED:
+        case ftMotionMode_DISABLED: flag.reset_ft = true;
         case ftMotionMode_ENABLED:
           ftMotion.cfg.mode = newmm;
           flag.report_h = true;
-          if (oldmm == ftMotionMode_DISABLED) flag.reset_ft = true;
           break;
       }
     }
@@ -193,6 +194,7 @@ void GcodeSuite::M493() {
     if (parser.seen('P')) {
       const bool val = parser.value_bool();
       ftMotion.cfg.linearAdvEna = val;
+      flag.report_h = true;
       SERIAL_ECHO_TERNARY(val, "Linear Advance ", "en", "dis", "abled.\n");
     }
 
@@ -216,22 +218,16 @@ void GcodeSuite::M493() {
       if (ftMotion.cfg.modeHasShaper()) {
         const dynFreqMode_t val = dynFreqMode_t(parser.value_byte());
         switch (val) {
-          case dynFreqMode_DISABLED:
-            ftMotion.cfg.dynFreqMode = val;
-            flag.report_h = true;
-            break;
           #if HAS_DYNAMIC_FREQ_MM
             case dynFreqMode_Z_BASED:
-              ftMotion.cfg.dynFreqMode = val;
-              flag.report_h = true;
-              break;
           #endif
           #if HAS_DYNAMIC_FREQ_G
             case dynFreqMode_MASS_BASED:
-              ftMotion.cfg.dynFreqMode = val;
-              flag.report_h = true;
-              break;
           #endif
+          case dynFreqMode_DISABLED:
+            ftMotion.cfg.dynFreqMode = val;
+            flag.report_h = true;
+            break;
           default:
             SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value.");
             break;
@@ -279,6 +275,36 @@ void GcodeSuite::M493() {
       }
     #endif
 
+    // Parse zeta parameter (X axis).
+    if (parser.seenval('I')) {
+      const float val = parser.value_float();
+      if (ftMotion.cfg.modeHasShaper()) {
+        if (WITHIN(val, 0.01f, 1.0f)) {
+          ftMotion.cfg.zeta[0] = val;
+          flag.update_n = flag.update_a = true;
+        }
+        else
+          SERIAL_ECHOLNPGM("Invalid X zeta [", AS_CHAR('I'), "] value."); // Zeta out of range.
+      }
+      else
+        SERIAL_ECHOLNPGM("Wrong mode for zeta parameter.");
+    }
+
+    // Parse vtol parameter (X axis).
+    if (parser.seenval('Q')) {
+      const float val = parser.value_float();
+      if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) {
+        if (WITHIN(val, 0.00f, 1.0f)) {
+          ftMotion.cfg.vtol[0] = val;
+          flag.update_a = true;
+        }
+        else
+          SERIAL_ECHOLNPGM("Invalid X vtol [", AS_CHAR('Q'), "] value."); // VTol out of range.
+      }
+      else
+        SERIAL_ECHOLNPGM("Wrong mode for vtol parameter.");
+    }
+
   #endif // HAS_X_AXIS
 
   #if HAS_Y_AXIS
@@ -310,15 +336,50 @@ void GcodeSuite::M493() {
       }
     #endif
 
+    // Parse zeta parameter (Y axis).
+    if (parser.seenval('J')) {
+      const float val = parser.value_float();
+      if (ftMotion.cfg.modeHasShaper()) {
+        if (WITHIN(val, 0.01f, 1.0f)) {
+          ftMotion.cfg.zeta[1] = val;
+          flag.update_n = flag.update_a = true;
+        }
+        else
+          SERIAL_ECHOLNPGM("Invalid Y zeta [", AS_CHAR('J'), "] value."); // Zeta Out of range
+      }
+      else
+        SERIAL_ECHOLNPGM("Wrong mode for zeta parameter.");
+    }
+
+    // Parse vtol parameter (Y axis).
+    if (parser.seenval('R')) {
+      const float val = parser.value_float();
+      if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) {
+        if (WITHIN(val, 0.00f, 1.0f)) {
+          ftMotion.cfg.vtol[1] = val;
+          flag.update_a = true;
+        }
+        else
+          SERIAL_ECHOLNPGM("Invalid Y vtol [", AS_CHAR('R'), "] value."); // VTol out of range.
+      }
+      else
+        SERIAL_ECHOLNPGM("Wrong mode for vtol parameter.");
+    }
+
   #endif // HAS_Y_AXIS
 
-  #if HAS_X_AXIS
-    if (flag.update_n) ftMotion.refreshShapingN();
-    if (flag.update_a) ftMotion.updateShapingA();
-  #endif
-  if (flag.reset_ft) ftMotion.reset();
-  if (flag.report_h) say_shaping();
+  planner.synchronize();
 
+  if (flag.update_n) ftMotion.refreshShapingN();
+
+  if (flag.update_a) ftMotion.updateShapingA();
+
+  if (flag.reset_ft) {
+    stepper.ftMotion_syncPosition();
+    ftMotion.reset();
+  }
+
+  if (flag.report_h) say_shaping();
 }
 
 #endif // FT_MOTION
diff --git a/Marlin/src/gcode/probe/G38.cpp b/Marlin/src/gcode/probe/G38.cpp
index 5b138fdaf6dd4..1f22cc84ed5c7 100644
--- a/Marlin/src/gcode/probe/G38.cpp
+++ b/Marlin/src/gcode/probe/G38.cpp
@@ -105,6 +105,7 @@ inline bool G38_run_probe() {
  *  G38.5 - Probe away from workpiece, stop on contact break
  */
 void GcodeSuite::G38(const int8_t subcode) {
+
   // Get X Y Z E F
   get_destination_from_command();
 
diff --git a/Marlin/src/inc/Conditionals_adv.h b/Marlin/src/inc/Conditionals_adv.h
index f1caf52c7374b..89a57f274d54a 100644
--- a/Marlin/src/inc/Conditionals_adv.h
+++ b/Marlin/src/inc/Conditionals_adv.h
@@ -1144,7 +1144,7 @@
   #elif HAS_DRIVER(A4988)
     #define MINIMUM_STEPPER_POST_DIR_DELAY 200
   #elif HAS_TRINAMIC_CONFIG || HAS_TRINAMIC_STANDALONE
-    #define MINIMUM_STEPPER_POST_DIR_DELAY 60
+    #define MINIMUM_STEPPER_POST_DIR_DELAY 70
   #else
     #define MINIMUM_STEPPER_POST_DIR_DELAY 0   // Expect at least 10µS since one Stepper ISR must transpire
   #endif
diff --git a/Marlin/src/lcd/language/language_en.h b/Marlin/src/lcd/language/language_en.h
index 8c9dd08d07da1..7945b26d5b724 100644
--- a/Marlin/src/lcd/language/language_en.h
+++ b/Marlin/src/lcd/language/language_en.h
@@ -802,6 +802,8 @@ namespace LanguageNarrow_en {
   LSTR MSG_FTM_MODE                       = _UxGT("Motion Mode:");
   LSTR MSG_FTM_ZV                         = _UxGT("ZV");
   LSTR MSG_FTM_ZVD                        = _UxGT("ZVD");
+  LSTR MSG_FTM_ZVDD                       = _UxGT("ZVDD");
+  LSTR MSG_FTM_ZVDDD                      = _UxGT("ZVDDD");
   LSTR MSG_FTM_EI                         = _UxGT("EI");
   LSTR MSG_FTM_2HEI                       = _UxGT("2HEI");
   LSTR MSG_FTM_3HEI                       = _UxGT("3HEI");
@@ -813,8 +815,8 @@ namespace LanguageNarrow_en {
   LSTR MSG_FTM_MASS_BASED                 = _UxGT("Mass-based");
   LSTR MSG_FTM_BASE_FREQ_N                = _UxGT("@ Base Freq.");
   LSTR MSG_FTM_DFREQ_K_N                  = _UxGT("@ Dyn. Freq.");
-  LSTR MSG_FTM_ZETA                       = _UxGT("Damping");
-  LSTR MSG_FTM_VTOL                       = _UxGT("Vib. Level");
+  LSTR MSG_FTM_ZETA_N                     = _UxGT("@ Damping");
+  LSTR MSG_FTM_VTOL_N                     = _UxGT("@ Vib. Level");
 
   LSTR MSG_LEVEL_X_AXIS                   = _UxGT("Level X Axis");
   LSTR MSG_AUTO_CALIBRATE                 = _UxGT("Auto Calibrate");
diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp
index 35adf1211f048..239d03d9777a6 100644
--- a/Marlin/src/lcd/menu/menu_motion.cpp
+++ b/Marlin/src/lcd/menu/menu_motion.cpp
@@ -342,6 +342,8 @@ void menu_move() {
     #if HAS_X_AXIS
       if (mode != ftMotionMode_ZV)     ACTION_ITEM(MSG_FTM_ZV,   []{ ftm_menu_setShaping(ftMotionMode_ZV); });
       if (mode != ftMotionMode_ZVD)    ACTION_ITEM(MSG_FTM_ZVD,  []{ ftm_menu_setShaping(ftMotionMode_ZVD); });
+      if (mode != ftMotionMode_ZVDD)   ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_setShaping(ftMotionMode_ZVDD); });
+      if (mode != ftMotionMode_ZVDDD)  ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_setShaping(ftMotionMode_ZVDDD); });
       if (mode != ftMotionMode_EI)     ACTION_ITEM(MSG_FTM_EI,   []{ ftm_menu_setShaping(ftMotionMode_EI); });
       if (mode != ftMotionMode_2HEI)   ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_setShaping(ftMotionMode_2HEI); });
       if (mode != ftMotionMode_3HEI)   ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_setShaping(ftMotionMode_3HEI); });
@@ -384,6 +386,8 @@ void menu_move() {
       case ftMotionMode_ENABLED:  ftmode = GET_TEXT_F(MSG_LCD_ON);   break;
       case ftMotionMode_ZV:       ftmode = GET_TEXT_F(MSG_FTM_ZV);   break;
       case ftMotionMode_ZVD:      ftmode = GET_TEXT_F(MSG_FTM_ZVD);  break;
+      case ftMotionMode_ZVDD:     ftmode = GET_TEXT_F(MSG_FTM_ZVDD); break;
+      case ftMotionMode_ZVDDD:    ftmode = GET_TEXT_F(MSG_FTM_ZVDDD);break;
       case ftMotionMode_EI:       ftmode = GET_TEXT_F(MSG_FTM_EI);   break;
       case ftMotionMode_2HEI:     ftmode = GET_TEXT_F(MSG_FTM_2HEI); break;
       case ftMotionMode_3HEI:     ftmode = GET_TEXT_F(MSG_FTM_3HEI); break;
@@ -416,10 +420,21 @@ void menu_move() {
         EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[Y_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.refreshShapingN);
       #endif
 
-      EDIT_ITEM_FAST(float42_52, MSG_FTM_ZETA, &c.zeta, 0.0f, 1.0f, ftMotion.refreshShapingN);
+      #if HAS_X_AXIS
+        EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta[0], 0.0f, 1.0f, ftMotion.refreshShapingN);
+      #endif
+      #if HAS_Y_AXIS
+        EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta[1], 0.0f, 1.0f, ftMotion.refreshShapingN);
+      #endif
 
-      if (WITHIN(c.mode, ftMotionMode_EI, ftMotionMode_3HEI))
-        EDIT_ITEM_FAST(float42_52, MSG_FTM_VTOL, &c.vtol, 0.0f, 1.0f, ftMotion.refreshShapingN);
+      if (IS_EI_MODE(c.mode)) {
+        #if HAS_X_AXIS
+          EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol[0], 0.0f, 1.0f, ftMotion.refreshShapingN);
+        #endif
+        #if HAS_Y_AXIS
+          EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol[1], 0.0f, 1.0f, ftMotion.refreshShapingN);
+        #endif
+      }
 
       #if HAS_DYNAMIC_FREQ
         SUBMENU(MSG_FTM_DYN_MODE, menu_ftm_dyn_mode);
@@ -437,7 +452,7 @@ void menu_move() {
 
     #if HAS_EXTRUDERS
       EDIT_ITEM(bool, MSG_LINEAR_ADVANCE, &c.linearAdvEna);
-      EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10);
+      if (c.linearAdvEna) EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10);
     #endif
 
     END_MENU();
diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp
index ad37e8a9b3daf..a2049db3b63f7 100644
--- a/Marlin/src/module/ft_motion.cpp
+++ b/Marlin/src/module/ft_motion.cpp
@@ -32,6 +32,8 @@ FTMotion ftMotion;
 #if !HAS_X_AXIS
   static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZV, "ftMotionMode_ZV requires at least one linear axis.");
   static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVD, "ftMotionMode_ZVD requires at least one linear axis.");
+  static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVDD, "ftMotionMode_ZVD requires at least one linear axis.");
+  static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVDDD, "ftMotionMode_ZVD requires at least one linear axis.");
   static_assert(FTM_DEFAULT_MODE == ftMotionMode_EI, "ftMotionMode_EI requires at least one linear axis.");
   static_assert(FTM_DEFAULT_MODE == ftMotionMode_2HEI, "ftMotionMode_2HEI requires at least one linear axis.");
   static_assert(FTM_DEFAULT_MODE == ftMotionMode_3HEI, "ftMotionMode_3HEI requires at least one linear axis.");
@@ -52,9 +54,7 @@ FTMotion ftMotion;
 
 ft_config_t FTMotion::cfg;
 bool FTMotion::busy; // = false
-ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U};               // Buffer of stepper commands.
-hal_timer_t FTMotion::stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of the stepper command timing.
-uint8_t FTMotion::stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE] = {0U};            // Buffer of whether DIR needs to be updated.
+ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepper commands buffer.
 uint32_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.
 
@@ -62,10 +62,10 @@ bool FTMotion::sts_stepperBusy = false;         // The stepper buffer has items
 
 // 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 modified fixed-time-based trajectory.
+  xyze_trajectory_t FTMotion::traj;               // = {0.0f} Storage for fixed-time-based trajectory.
+  xyze_trajectoryMod_t FTMotion::trajMod;         // = {0.0f} Storage for modified fixed-time-based trajectory.
+  xyze_trajectoryWin_t FTMotion::trajWin;         // = {0.0f} Storage for fixed time trajectory window.
 
-block_t* FTMotion::current_block_cpy = nullptr; // Pointer to current block being processed.
 bool FTMotion::blockProcRdy = false,            // Indicates a block is ready to be processed.
      FTMotion::blockProcRdy_z1 = false,         // Storage for the previous indicator.
      FTMotion::blockProcDn = false;             // Indicates current block is done being processed.
@@ -76,7 +76,7 @@ bool FTMotion::batchRdy = false;                // Indicates a batch of the fixe
 bool FTMotion::batchRdyForInterp = false;       // Indicates the batch is done being post processed,
                                                 //  if applicable, and is ready to be converted to step commands.
 bool FTMotion::runoutEna = false;               // True if runout of the block hasn't been done and is allowed.
-bool FTMotion::runout = false;                  // Indicates if runout is in progress.
+bool FTMotion::blockDataIsRunout = false;       // Indicates the last loaded block variables are for a runout.
 
 // Trapezoid data variables.
 xyze_pos_t   FTMotion::startPosn,                     // (mm) Start position of block
@@ -98,15 +98,13 @@ uint32_t FTMotion::max_intervals;               // Total number of data points t
 // Make vector variables.
 uint32_t FTMotion::makeVector_idx = 0,                    // Index of fixed time trajectory generation of the overall block.
          FTMotion::makeVector_idx_z1 = 0,                 // Storage for the previously calculated index above.
-         FTMotion::makeVector_batchIdx = FTM_BATCH_SIZE;  // Index of fixed time trajectory generation within the batch.
+         FTMotion::makeVector_batchIdx = 0;               // Index of fixed time trajectory generation within the batch.
 
 // Interpolation variables.
 xyze_long_t FTMotion::steps = { 0 };                  // Step count accumulator.
-xyze_stepDir_t FTMotion::dirState = LOGICAL_AXIS_ARRAY_1(stepDirState_NOT_SET); // Memory of the currently set step direction of the axis.
 
 uint32_t FTMotion::interpIdx = 0,                     // Index of current data point being interpolated.
          FTMotion::interpIdx_z1 = 0;                  // Storage for the previously calculated index above.
-hal_timer_t FTMotion::nextStepTicks = FTM_MIN_TICKS;  // Accumulator for the next step time (in ticks).
 
 // Shaping variables.
 #if HAS_X_AXIS
@@ -125,8 +123,6 @@ hal_timer_t FTMotion::nextStepTicks = FTM_MIN_TICKS;  // Accumulator for the nex
   float FTMotion::e_advanced_z1 = 0.0f;   // (ms) Unit delay of advanced extruder position.
 #endif
 
-constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
-
 //-----------------------------------------------------------------//
 // Function definitions.
 //-----------------------------------------------------------------//
@@ -134,8 +130,7 @@ constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
 // Public functions.
 
 // Sets controller states to begin processing a block.
-void FTMotion::startBlockProc(block_t * const current_block) {
-  current_block_cpy = current_block;
+void FTMotion::startBlockProc() {
   blockProcRdy = true;
   blockProcDn = false;
   runoutEna = true;
@@ -144,30 +139,16 @@ void FTMotion::startBlockProc(block_t * const current_block) {
 // Move any free data points to the stepper buffer even if a full batch isn't ready.
 void FTMotion::runoutBlock() {
 
-  if (runoutEna && !batchRdy) {   // If the window is full already (block intervals was a multiple of
-                                  // the batch size), or runout is not enabled, no runout is needed.
-    // Fill out the trajectory window with the last position calculated.
-    if (makeVector_batchIdx > last_batchIdx)
-      for (uint32_t i = makeVector_batchIdx; i < (FTM_WINDOW_SIZE); i++) {
-        LOGICAL_AXIS_CODE(
-          traj.e[i] = traj.e[makeVector_batchIdx - 1],
-          traj.x[i] = traj.x[makeVector_batchIdx - 1],
-          traj.y[i] = traj.y[makeVector_batchIdx - 1],
-          traj.z[i] = traj.z[makeVector_batchIdx - 1],
-          traj.i[i] = traj.i[makeVector_batchIdx - 1],
-          traj.j[i] = traj.j[makeVector_batchIdx - 1],
-          traj.k[i] = traj.k[makeVector_batchIdx - 1],
-          traj.u[i] = traj.u[makeVector_batchIdx - 1],
-          traj.v[i] = traj.v[makeVector_batchIdx - 1],
-          traj.w[i] = traj.w[makeVector_batchIdx - 1]
-        );
-      }
+  if (!runoutEna) return;
 
-    makeVector_batchIdx = last_batchIdx;
-    batchRdy = true;
-    runout = true;
-  }
-  runoutEna = false;
+  startPosn = endPosn_prevBlock;
+  ratio.reset();
+
+  max_intervals = cfg.modeHasShaper() ? shaper_intervals : 0;
+  if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, min_max_intervals - (FTM_BATCH_SIZE))) max_intervals = min_max_intervals;
+  max_intervals += TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_WINDOW_SIZE) - makeVector_batchIdx;
+  blockProcRdy = blockDataIsRunout = true;
+  runoutEna = blockProcDn = false;
 }
 
 // Controller main, to be invoked from non-isr task.
@@ -184,81 +165,63 @@ void FTMotion::loop() {
     if (sts_stepperBusy) return;          // Wait until motion buffers are emptied
     reset();
     blockProcDn = true;                   // Set queueing to look for next block.
-    runoutEna = false;                    // Disabling running out this block, since we want to halt the motion.
     stepper.abort_current_block = false;  // Abort finished.
   }
 
   // Planner processing and block conversion.
-  if (!blockProcRdy && !runout) stepper.ftMotion_BlockQueueUpdate();
+  if (!blockProcRdy) stepper.ftMotion_blockQueueUpdate();
 
   if (blockProcRdy) {
-    if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot.
+    if (!blockProcRdy_z1) { // One-shot.
+      if (!blockDataIsRunout) loadBlockData(stepper.current_block);
+      else blockDataIsRunout = false;
+    }
     while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP)))
       makeVector();
   }
 
-  if (runout && !batchRdy) { // The lower half of the window has been runout.
-    // Runout the upper half of the window: the upper half has been shifted into the lower
-    // half. Fill out the upper half so another batch can be processed.
-    for (uint32_t i = last_batchIdx; i < (FTM_WINDOW_SIZE) - 1; i++) {
-      LOGICAL_AXIS_CODE(
-        traj.e[i] = traj.e[(FTM_WINDOW_SIZE) - 1],
-        traj.x[i] = traj.x[(FTM_WINDOW_SIZE) - 1],
-        traj.y[i] = traj.y[(FTM_WINDOW_SIZE) - 1],
-        traj.z[i] = traj.z[(FTM_WINDOW_SIZE) - 1],
-        traj.i[i] = traj.i[(FTM_WINDOW_SIZE) - 1],
-        traj.j[i] = traj.j[(FTM_WINDOW_SIZE) - 1],
-        traj.k[i] = traj.k[(FTM_WINDOW_SIZE) - 1],
-        traj.u[i] = traj.u[(FTM_WINDOW_SIZE) - 1],
-        traj.v[i] = traj.v[(FTM_WINDOW_SIZE) - 1],
-        traj.w[i] = traj.w[(FTM_WINDOW_SIZE) - 1]
-      );
-    }
-    batchRdy = true;
-    runout = false;
-  }
-
   // FBS / post processing.
   if (batchRdy && !batchRdyForInterp) {
 
     // Call Ulendo FBS here.
 
-    // Copy the uncompensated vectors. (XY done, other axes uncompensated)
-    LOGICAL_AXIS_CODE(
-      memcpy(trajMod.e, &traj.e[FTM_BATCH_SIZE], sizeof(trajMod.e)),
-      memcpy(trajMod.x, &traj.x[FTM_BATCH_SIZE], sizeof(trajMod.x)),
-      memcpy(trajMod.y, &traj.y[FTM_BATCH_SIZE], sizeof(trajMod.y)),
-      memcpy(trajMod.z, &traj.z[FTM_BATCH_SIZE], sizeof(trajMod.z)),
-      memcpy(trajMod.i, &traj.i[FTM_BATCH_SIZE], sizeof(trajMod.i)),
-      memcpy(trajMod.j, &traj.j[FTM_BATCH_SIZE], sizeof(trajMod.j)),
-      memcpy(trajMod.k, &traj.k[FTM_BATCH_SIZE], sizeof(trajMod.k)),
-      memcpy(trajMod.u, &traj.u[FTM_BATCH_SIZE], sizeof(trajMod.u)),
-      memcpy(trajMod.v, &traj.v[FTM_BATCH_SIZE], sizeof(trajMod.v)),
-      memcpy(trajMod.w, &traj.w[FTM_BATCH_SIZE], sizeof(trajMod.w))
-    );
-
-    // Shift the time series back in the window for (shaped) X and Y
-    TERN_(HAS_X_AXIS, memcpy(traj.x, &traj.x[FTM_BATCH_SIZE], sizeof(traj.x) / 2));
-    TERN_(HAS_Y_AXIS, memcpy(traj.y, &traj.y[FTM_BATCH_SIZE], sizeof(traj.y) / 2));
+    #if ENABLED(FTM_UNIFIED_BWS)
+      trajMod = traj; // Copy the uncompensated vectors.
+      traj = trajWin; // Move the window to traj
+    #else
+      // Copy the uncompensated vectors.
+      #define TCOPY(A) memcpy(trajMod.A, traj.A, sizeof(trajMod.A))
+      LOGICAL_AXIS_CODE(
+        TCOPY(e),
+        TCOPY(x), TCOPY(y), TCOPY(z),
+        TCOPY(i), TCOPY(j), TCOPY(k),
+        TCOPY(u), TCOPY(v), TCOPY(w)
+      );
 
-    // Z...W and E Disabled! Uncompensated so the lower half is not used.
-    //TERN_(HAS_Z_AXIS, memcpy(&traj.z[0], &traj.z[FTM_BATCH_SIZE], sizeof(traj.z) / 2));
+      // Shift the time series back in the window
+      #define TSHIFT(A) memcpy(traj.A, trajWin.A, sizeof(trajWin.A))
+      LOGICAL_AXIS_CODE(
+        TSHIFT(e),
+        TSHIFT(x), TSHIFT(y), TSHIFT(z),
+        TSHIFT(i), TSHIFT(j), TSHIFT(k),
+        TSHIFT(u), TSHIFT(v), TSHIFT(w)
+      );
+    #endif
 
     // ... data is ready in trajMod.
     batchRdyForInterp = true;
 
-    batchRdy = false; // Clear so that makeVector() may resume generating points.
-
-  } // if (batchRdy && !batchRdyForInterp)
+    batchRdy = false; // Clear so makeVector() can resume generating points.
+  }
 
   // Interpolation.
   while ( batchRdyForInterp
-          && ( stepperCmdBuffItems() < ((FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME)) )
-          && ( (interpIdx - interpIdx_z1) < (FTM_STEPS_PER_LOOP) )
+          && ( stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME) )
+          && ( interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP) )
   ) {
     convertToSteps(interpIdx);
 
-    if (++interpIdx == FTM_BATCH_SIZE) {
+    if (++interpIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE)) {
       batchRdyForInterp = false;
       interpIdx = 0;
     }
@@ -277,76 +240,153 @@ void FTMotion::loop() {
   // Refresh the gains used by shaping functions.
   // To be called on init or mode or zeta change.
 
-  void FTMotion::Shaping::updateShapingA(const_float_t zeta/*=cfg.zeta*/, const_float_t vtol/*=cfg.vtol*/) {
+  void FTMotion::Shaping::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) {
 
-    const float K = exp(-zeta * M_PI / sqrt(1.0f - sq(zeta))),
-                K2 = sq(K);
+    const float Kx = exp(-zeta[0] * M_PI / sqrt(1.0f - sq(zeta[0]))),
+                Ky = exp(-zeta[1] * M_PI / sqrt(1.0f - sq(zeta[1]))),
+                Kx2 = sq(Kx),
+                Ky2 = sq(Ky);
 
     switch (cfg.mode) {
 
       case ftMotionMode_ZV:
         max_i = 1U;
-        x.Ai[0] = 1.0f / (1.0f + K);
-        x.Ai[1] = x.Ai[0] * K;
+        x.Ai[0] = 1.0f / (1.0f + Kx);
+        x.Ai[1] = x.Ai[0] * Kx;
+
+        y.Ai[0] = 1.0f / (1.0f + Ky);
+        y.Ai[1] = y.Ai[0] * Ky;
         break;
 
       case ftMotionMode_ZVD:
         max_i = 2U;
-        x.Ai[0] = 1.0f / ( 1.0f + 2.0f * K + K2 );
-        x.Ai[1] = x.Ai[0] * 2.0f * K;
-        x.Ai[2] = x.Ai[0] * K2;
+        x.Ai[0] = 1.0f / (1.0f + 2.0f * Kx + Kx2);
+        x.Ai[1] = x.Ai[0] * 2.0f * Kx;
+        x.Ai[2] = x.Ai[0] * Kx2;
+
+        y.Ai[0] = 1.0f / (1.0f + 2.0f * Ky + Ky2);
+        y.Ai[1] = y.Ai[0] * 2.0f * Ky;
+        y.Ai[2] = y.Ai[0] * Ky2;
+        break;
+
+      case ftMotionMode_ZVDD:
+        max_i = 3U;
+        x.Ai[0] = 1.0f / (1.0f + 3.0f * Kx + 3.0f * Kx2 + cu(Kx));
+        x.Ai[1] = x.Ai[0] * 3.0f * Kx;
+        x.Ai[2] = x.Ai[0] * 3.0f * Kx2;
+        x.Ai[3] = x.Ai[0] * cu(Kx);
+
+        y.Ai[0] = 1.0f / (1.0f + 3.0f * Ky + 3.0f * Ky2 + cu(Ky));
+        y.Ai[1] = y.Ai[0] * 3.0f * Ky;
+        y.Ai[2] = y.Ai[0] * 3.0f * Ky2;
+        y.Ai[3] = y.Ai[0] * cu(Ky);
+        break;
+
+      case ftMotionMode_ZVDDD:
+        max_i = 4U;
+        x.Ai[0] = 1.0f / (1.0f + 4.0f * Kx + 6.0f * Kx2 + 4.0f * cu(Kx) + sq(Kx2));
+        x.Ai[1] = x.Ai[0] * 4.0f * Kx;
+        x.Ai[2] = x.Ai[0] * 6.0f * Kx2;
+        x.Ai[3] = x.Ai[0] * 4.0f * cu(Kx);
+        x.Ai[4] = x.Ai[0] * sq(Kx2);
+
+        y.Ai[0] = 1.0f / (1.0f + 4.0f * Ky + 6.0f * Ky2 + 4.0f * cu(Ky) + sq(Ky2));
+        y.Ai[1] = y.Ai[0] * 4.0f * Ky;
+        y.Ai[2] = y.Ai[0] * 6.0f * Ky2;
+        y.Ai[3] = y.Ai[0] * 4.0f * cu(Ky);
+        y.Ai[4] = y.Ai[0] * sq(Ky2);
         break;
 
       case ftMotionMode_EI: {
         max_i = 2U;
-        x.Ai[0] = 0.25f * (1.0f + vtol);
-        x.Ai[1] = 0.50f * (1.0f - vtol) * K;
-        x.Ai[2] = x.Ai[0] * K2;
-        const float A_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2]);
-        for (uint32_t i = 0U; i < 3U; i++) { x.Ai[i] *= A_adj; }
-      } break;
+        x.Ai[0] = 0.25f * (1.0f + vtol[0]);
+        x.Ai[1] = 0.50f * (1.0f - vtol[0]) * Kx;
+        x.Ai[2] = x.Ai[0] * Kx2;
+
+        y.Ai[0] = 0.25f * (1.0f + vtol[1]);
+        y.Ai[1] = 0.50f * (1.0f - vtol[1]) * Ky;
+        y.Ai[2] = y.Ai[0] * Ky2;
+
+        const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2]);
+        const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2]);
+        for (uint32_t i = 0U; i < 3U; i++) {
+          x.Ai[i] *= X_adj;
+          y.Ai[i] *= Y_adj;
+        }
+      }
+      break;
 
       case ftMotionMode_2HEI: {
         max_i = 3U;
-        const float vtol2 = sq(vtol);
-        const float X = pow(vtol2 * (sqrt(1.0f - vtol2) + 1.0f), 1.0f / 3.0f);
-        x.Ai[0] = ( 3.0f * sq(X) + 2.0f * X + 3.0f * vtol2 ) / (16.0f * X);
-        x.Ai[1] = ( 0.5f - x.Ai[0] ) * K;
-        x.Ai[2] = x.Ai[1] * K;
-        x.Ai[3] = x.Ai[0] * cu(K);
-        const float A_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3]);
-        for (uint32_t i = 0U; i < 4U; i++) { x.Ai[i] *= A_adj; }
-      } break;
+        const float vtolx2 = sq(vtol[0]);
+        const float X = pow(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f);
+        x.Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X);
+        x.Ai[1] = (0.5f - x.Ai[0]) * Kx;
+        x.Ai[2] = x.Ai[1] * Kx;
+        x.Ai[3] = x.Ai[0] * cu(Kx);
+       
+        const float vtoly2 = sq(vtol[1]);
+        const float Y = pow(vtoly2 * (sqrt(1.0f - vtoly2) + 1.0f), 1.0f / 3.0f);
+        y.Ai[0] = (3.0f * sq(Y) + 2.0f * Y + 3.0f * vtoly2) / (16.0f * Y);
+        y.Ai[1] = (0.5f - y.Ai[0]) * Ky;
+        y.Ai[2] = y.Ai[1] * Ky;
+        y.Ai[3] = y.Ai[0] * cu(Ky);
+
+        const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3]);
+        const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2] + y.Ai[3]);
+        for (uint32_t i = 0U; i < 4U; i++) {
+          x.Ai[i] *= X_adj;
+          y.Ai[i] *= Y_adj;
+        }
+      }
+      break;
 
       case ftMotionMode_3HEI: {
         max_i = 4U;
-        x.Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) );
-        x.Ai[1] = 0.25f * ( 1.0f - vtol ) * K;
-        x.Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * x.Ai[0] ) * K2;
-        x.Ai[3] = x.Ai[1] * K2;
-        x.Ai[4] = x.Ai[0] * sq(K2);
-        const float A_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3] + x.Ai[4]);
-        for (uint32_t i = 0U; i < 5U; i++) { x.Ai[i] *= A_adj; }
-      } break;
+        x.Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol[0] + 2.0f * sqrt( 2.0f * ( vtol[0] + 1.0f ) * vtol[0] ) );
+        x.Ai[1] = 0.25f * ( 1.0f - vtol[0] ) * Kx;
+        x.Ai[2] = ( 0.5f * ( 1.0f + vtol[0] ) - 2.0f * x.Ai[0] ) * Kx2;
+        x.Ai[3] = x.Ai[1] * Kx2;
+        x.Ai[4] = x.Ai[0] * sq(Kx2);
+
+        y.Ai[0] = 0.0625f * (1.0f + 3.0f * vtol[1] + 2.0f * sqrt(2.0f * (vtol[1] + 1.0f) * vtol[1]));
+        y.Ai[1] = 0.25f * (1.0f - vtol[1]) * Ky;
+        y.Ai[2] = (0.5f * (1.0f + vtol[1]) - 2.0f * y.Ai[0]) * Ky2;
+        y.Ai[3] = y.Ai[1] * Ky2;
+        y.Ai[4] = y.Ai[0] * sq(Ky2);
+
+        const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3] + x.Ai[4]);
+        const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2] + y.Ai[3] + y.Ai[4]);
+        for (uint32_t i = 0U; i < 5U; i++) {
+          x.Ai[i] *= X_adj;
+          y.Ai[i] *= Y_adj;
+        }
+      }    
+      break;
 
       case ftMotionMode_MZV: {
         max_i = 2U;
-        const float B = 1.4142135623730950488016887242097f * K;
-        x.Ai[0] = 1.0f / (1.0f + B + K2);
-        x.Ai[1] = x.Ai[0] * B;
-        x.Ai[2] = x.Ai[0] * K2;
-      } break;
+        const float Bx = 1.4142135623730950488016887242097f * Kx;
+        x.Ai[0] = 1.0f / (1.0f + Bx + Kx2);
+        x.Ai[1] = x.Ai[0] * Bx;
+        x.Ai[2] = x.Ai[0] * Kx2;
+
+        const float By = 1.4142135623730950488016887242097f * Ky;
+        y.Ai[0] = 1.0f / (1.0f + By + Ky2);
+        y.Ai[1] = y.Ai[0] * By;
+        y.Ai[2] = y.Ai[0] * Ky2;
+      } 
+      break;
 
       default:
-        for (uint32_t i = 0U; i < 5U; i++) x.Ai[i] = 0.0f;
+        ZERO(x.Ai);
+        ZERO(y.Ai);
         max_i = 0;
     }
-    #if HAS_Y_AXIS
-      memcpy(y.Ai, x.Ai, sizeof(x.Ai)); // For now, zeta and vtol are shared across x and y.
-    #endif
+    
   }
 
-  void FTMotion::updateShapingA(const_float_t zeta/*=cfg.zeta*/, const_float_t vtol/*=cfg.vtol*/) {
+  void FTMotion::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) {
     shaping.updateShapingA(zeta, vtol);
   }
 
@@ -364,11 +404,13 @@ void FTMotion::loop() {
         Ni[1] = round((0.5f / f / df) * (FTM_FS));
         Ni[2] = Ni[1] + Ni[1];
         break;
+      case ftMotionMode_ZVDD:
       case ftMotionMode_2HEI:
         Ni[1] = round((0.5f / f / df) * (FTM_FS));
         Ni[2] = Ni[1] + Ni[1];
         Ni[3] = Ni[2] + Ni[1];
         break;
+      case ftMotionMode_ZVDDD:
       case ftMotionMode_3HEI:
         Ni[1] = round((0.5f / f / df) * (FTM_FS));
         Ni[2] = Ni[1] + Ni[1];
@@ -383,10 +425,14 @@ void FTMotion::loop() {
     }
   }
 
-  void FTMotion::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), const_float_t zeta/*=cfg.zeta*/) {
-    const float df = sqrt(1.0f - sq(zeta));
-    shaping.x.updateShapingN(xf, df);
-    TERN_(HAS_Y_AXIS, shaping.y.updateShapingN(yf, df));
+  void FTMotion::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), float zeta[]/*=cfg.zeta*/) {
+    const float xdf = sqrt(1.0f - sq(zeta[0]));
+    shaping.x.updateShapingN(xf, xdf);
+    
+    #if HAS_Y_AXIS
+      const float ydf = sqrt(1.0f - sq(zeta[1]));
+      shaping.y.updateShapingN(yf, ydf);
+    #endif
   }
 
 #endif // HAS_X_AXIS
@@ -396,27 +442,24 @@ void FTMotion::reset() {
 
   stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;
 
-  traj.reset();     // Reset trajectory history
-  trajMod.reset();  // Reset modified trajectory history
+  traj.reset();
+  trajWin.reset();
 
   blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
   batchRdy = batchRdyForInterp = false;
   runoutEna = false;
-  runout = false;
 
   endPosn_prevBlock.reset();
 
   makeVector_idx = makeVector_idx_z1 = 0;
-  makeVector_batchIdx = FTM_BATCH_SIZE;
+  makeVector_batchIdx = 0;
 
   steps.reset();
   interpIdx = interpIdx_z1 = 0;
-  dirState = LOGICAL_AXIS_ARRAY_1(stepDirState_NOT_SET);
-  nextStepTicks = FTM_MIN_TICKS;
 
   #if HAS_X_AXIS
-    for (uint32_t i = 0U; i < (FTM_ZMAX); i++)
-      shaping.x.d_zi[i] = TERN_(HAS_Y_AXIS, shaping.y.d_zi[i] =) 0.0f;
+    ZERO(shaping.x.d_zi);
+    TERN_(HAS_Y_AXIS, ZERO(shaping.y.d_zi));
     shaping.zi_idx = 0;
   #endif
 
@@ -445,40 +488,26 @@ void FTMotion::loadBlockData(block_t * const current_block) {
   const float totalLength = current_block->millimeters,
               oneOverLength = 1.0f / totalLength;
 
-  const AxisBits direction = current_block->direction_bits;
-
   startPosn = endPosn_prevBlock;
   xyze_pos_t moveDist = LOGICAL_AXIS_ARRAY(
-    current_block->steps.e / planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)],
-    current_block->steps.x / planner.settings.axis_steps_per_mm[X_AXIS],
-    current_block->steps.y / planner.settings.axis_steps_per_mm[Y_AXIS],
-    current_block->steps.z / planner.settings.axis_steps_per_mm[Z_AXIS],
-    current_block->steps.i / planner.settings.axis_steps_per_mm[I_AXIS],
-    current_block->steps.j / planner.settings.axis_steps_per_mm[J_AXIS],
-    current_block->steps.k / planner.settings.axis_steps_per_mm[K_AXIS],
-    current_block->steps.u / planner.settings.axis_steps_per_mm[U_AXIS],
-    current_block->steps.v / planner.settings.axis_steps_per_mm[V_AXIS],
-    current_block->steps.w / planner.settings.axis_steps_per_mm[W_AXIS]
-  );
-
-  LOGICAL_AXIS_CODE(
-    if (!direction.e) moveDist.e *= -1.0f,
-    if (!direction.x) moveDist.x *= -1.0f,
-    if (!direction.y) moveDist.y *= -1.0f,
-    if (!direction.z) moveDist.z *= -1.0f,
-    if (!direction.i) moveDist.i *= -1.0f,
-    if (!direction.j) moveDist.j *= -1.0f,
-    if (!direction.k) moveDist.k *= -1.0f,
-    if (!direction.u) moveDist.u *= -1.0f,
-    if (!direction.v) moveDist.v *= -1.0f,
-    if (!direction.w) moveDist.w *= -1.0f
+    current_block->steps.e * planner.mm_per_step[E_AXIS_N(current_block->extruder)] * (current_block->direction_bits.e ? 1 : -1),
+    current_block->steps.x * planner.mm_per_step[X_AXIS] * (current_block->direction_bits.x ? 1 : -1),
+    current_block->steps.y * planner.mm_per_step[Y_AXIS] * (current_block->direction_bits.y ? 1 : -1),
+    current_block->steps.z * planner.mm_per_step[Z_AXIS] * (current_block->direction_bits.z ? 1 : -1),
+    current_block->steps.i * planner.mm_per_step[I_AXIS] * (current_block->direction_bits.i ? 1 : -1),
+    current_block->steps.j * planner.mm_per_step[J_AXIS] * (current_block->direction_bits.j ? 1 : -1),
+    current_block->steps.k * planner.mm_per_step[K_AXIS] * (current_block->direction_bits.k ? 1 : -1),
+    current_block->steps.u * planner.mm_per_step[U_AXIS] * (current_block->direction_bits.u ? 1 : -1),
+    current_block->steps.v * planner.mm_per_step[V_AXIS] * (current_block->direction_bits.v ? 1 : -1),
+    current_block->steps.w * planner.mm_per_step[W_AXIS] * (current_block->direction_bits.w ? 1 : -1)
   );
 
   ratio = moveDist * oneOverLength;
 
+  /* Keep for comprehension
   const float spm = totalLength / current_block->step_event_count;  // (steps/mm) Distance for each step
-              f_s = spm * current_block->initial_rate;  // (steps/s) Start feedrate
-  const float f_e = spm * current_block->final_rate;    // (steps/s) End feedrate
+              f_s = spm * current_block->initial_rate,  // (steps/s) Start feedrate
+              f_e = spm * current_block->final_rate;    // (steps/s) End feedrate
 
   const float a = current_block->acceleration,          // (mm/s^2) Same magnitude for acceleration or deceleration
               oneby2a = 1.0f / (2.0f * a),              // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2
@@ -499,25 +528,46 @@ void FTMotion::loadBlockData(block_t * const current_block) {
 
   const float T1 = (F_n - f_s) / a,                     // (s) Accel Time = difference in feedrate over acceleration
               T3 = (F_n - f_e) / a;                     // (s) Decel Time = difference in feedrate over acceleration
+  */
+
+  const float spm = totalLength / current_block->step_event_count,
+              f_s = spm * current_block->initial_rate,
+              f_e = spm * current_block->final_rate;
+
+  const float accel = current_block->acceleration,
+              oneOverAccel = 1.0f / accel;
+
+  float F_n = current_block->nominal_speed;
+  const float ldiff = totalLength + 0.5f * oneOverAccel * (sq(f_s) + sq(f_e));
+
+  float T2 = ldiff / F_n - oneOverAccel * F_n;
+  if (T2 < 0.0f) {
+    T2 = 0.0f;
+    F_n = SQRT(ldiff * accel);
+  }
 
-  N1 = ceil(T1 * (FTM_FS));                       // Accel datapoints based on Hz frequency
-  N2 = ceil(T2 * (FTM_FS));                       // Coast
-  N3 = ceil(T3 * (FTM_FS));                       // Decel
-
-  const float T1_P = N1 * (FTM_TS),               // (s) Accel datapoints x timestep resolution
-              T2_P = N2 * (FTM_TS),               // (s) Coast
-              T3_P = N3 * (FTM_TS);               // (s) Decel
-
-  // Calculate the reachable feedrate at the end of the accel phase
-  // totalLength is the total distance to travel in mm
-  // f_s is the starting feedrate in mm/s
-  // f_e is the ending feedrate in mm/s
-  // T1_P is the time spent accelerating in seconds
-  // T2_P is the time spent coasting in seconds
-  // T3_P is the time spent decelerating in seconds
-  // f_s * T1_P is the distance traveled during the accel phase
-  // f_e * T3_P is the distance traveled during the decel phase
-  //
+  const float T1 = (F_n - f_s) * oneOverAccel,
+              T3 = (F_n - f_e) * oneOverAccel;
+
+  N1 = ceil(T1 * (FTM_FS));         // Accel datapoints based on Hz frequency
+  N2 = ceil(T2 * (FTM_FS));         // Coast
+  N3 = ceil(T3 * (FTM_FS));         // Decel
+
+  const float T1_P = N1 * (FTM_TS), // (s) Accel datapoints x timestep resolution
+              T2_P = N2 * (FTM_TS), // (s) Coast
+              T3_P = N3 * (FTM_TS); // (s) Decel
+
+  /**
+   * Calculate the reachable feedrate at the end of the accel phase.
+   *  totalLength is the total distance to travel in mm
+   *  f_s        : (mm/s) Starting feedrate
+   *  f_e        : (mm/s) Ending feedrate
+   *  T1_P       : (sec) Time spent accelerating
+   *  T2_P       : (sec) Time spent coasting
+   *  T3_P       : (sec) Time spent decelerating
+   *  f_s * T1_P : (mm) Distance traveled during the accel phase
+   *  f_e * T3_P : (mm) Distance traveled during the decel phase
+   */
   F_P = (2.0f * totalLength - f_s * T1_P - f_e * T3_P) / (T1_P + 2.0f * T2_P + T3_P); // (mm/s) Feedrate at the end of the accel phase
 
   // Calculate the acceleration and deceleration rates
@@ -531,77 +581,69 @@ void FTMotion::loadBlockData(block_t * const current_block) {
   // Calculate the distance traveled during the decel phase
   s_2e = s_1e + F_P * T2_P;
 
-  // One less than (Accel + Coasting + Decel) datapoints
-  max_intervals = N1 + N2 + N3 - 1U;
+  // Accel + Coasting + Decel datapoints
+  max_intervals = N1 + N2 + N3;
 
   endPosn_prevBlock += moveDist;
 }
 
 // Generate data points of the trajectory.
 void FTMotion::makeVector() {
-  float accel_k = 0.0f;                                   // (mm/s^2) Acceleration K factor
-  float tau = (makeVector_idx + 1) * (FTM_TS);            // (s) Time since start of block
-  float dist = 0.0f;                                      // (mm) Distance traveled
+  float accel_k = 0.0f;                                 // (mm/s^2) Acceleration K factor
+  float tau = (makeVector_idx + 1) * (FTM_TS);          // (s) Time since start of block
+  float dist = 0.0f;                                    // (mm) Distance traveled
 
   if (makeVector_idx < N1) {
     // Acceleration phase
-    dist = (f_s * tau) + (0.5f * accel_P * sq(tau));      // (mm) Distance traveled for acceleration phase
-    accel_k = accel_P;                                    // (mm/s^2) Acceleration K factor from Accel phase
+    dist = (f_s * tau) + (0.5f * accel_P * sq(tau));    // (mm) Distance traveled for acceleration phase since start of block
+    accel_k = accel_P;                                  // (mm/s^2) Acceleration K factor from Accel phase
   }
-  else if (makeVector_idx >= N1 && makeVector_idx < (N1 + N2)) {
+  else if (makeVector_idx < (N1 + N2)) {
     // Coasting phase
-    dist = s_1e + F_P * (tau - N1 * (FTM_TS));            // (mm) Distance traveled for coasting phase
+    dist = s_1e + F_P * (tau - N1 * (FTM_TS));          // (mm) Distance traveled for coasting phase since start of block
     //accel_k = 0.0f;
   }
   else {
     // Deceleration phase
-    const float tau_ = tau - (N1 + N2) * (FTM_TS);        // (s) Time since start of decel phase
-    dist = s_2e + F_P * tau_ + 0.5f * decel_P * sq(tau_); // (mm) Distance traveled for deceleration phase
-    accel_k = decel_P;                                    // (mm/s^2) Acceleration K factor from Decel phase
+    tau -= (N1 + N2) * (FTM_TS);                        // (s) Time since start of decel phase
+    dist = s_2e + F_P * tau + 0.5f * decel_P * sq(tau); // (mm) Distance traveled for deceleration phase since start of block
+    accel_k = decel_P;                                  // (mm/s^2) Acceleration K factor from Decel phase
   }
 
-  NUM_AXIS_CODE(
-    traj.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist,
-    traj.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist,
-    traj.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist,
-    traj.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist,
-    traj.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist,
-    traj.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist,
-    traj.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist,
-    traj.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist,
-    traj.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist
+  LOGICAL_AXIS_CODE(
+    trajWin.e[makeVector_batchIdx] = startPosn.e + ratio.e * dist,
+    trajWin.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist,
+    trajWin.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist,
+    trajWin.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist,
+    trajWin.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist,
+    trajWin.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist,
+    trajWin.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist,
+    trajWin.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist,
+    trajWin.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist,
+    trajWin.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist
   );
 
   #if HAS_EXTRUDERS
-    const float new_raw_z1 = startPosn.e + ratio.e * dist;
     if (cfg.linearAdvEna) {
-      float dedt_adj = (new_raw_z1 - e_raw_z1) * (FTM_FS);
+      float dedt_adj = (trajWin.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS);
       if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK;
 
+      e_raw_z1 = trajWin.e[makeVector_batchIdx];
       e_advanced_z1 += dedt_adj * (FTM_TS);
-      traj.e[makeVector_batchIdx] = e_advanced_z1;
-
-      e_raw_z1 = new_raw_z1;
-    }
-    else {
-      traj.e[makeVector_batchIdx] = new_raw_z1;
-      // Alternatively: ed[makeVector_batchIdx] = startPosn.e + (ratio.e * dist) / (N1 + N2 + N3);
+      trajWin.e[makeVector_batchIdx] = e_advanced_z1;
     }
   #endif
 
   // Update shaping parameters if needed.
-  #if HAS_DYNAMIC_FREQ_MM
-    static float zd_z1 = 0.0f;
-  #endif
+
   switch (cfg.dynFreqMode) {
 
     #if HAS_DYNAMIC_FREQ_MM
       case dynFreqMode_Z_BASED:
-        if (traj.z[makeVector_batchIdx] != zd_z1) { // Only update if Z changed.
-          const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx],
-                      yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx];
-          updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ), _MAX(yf, FTM_MIN_SHAPE_FREQ));
-          zd_z1 = traj.z[makeVector_batchIdx];
+        if (trajWin.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed.
+                 const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * trajWin.z[makeVector_batchIdx]
+          OPTARG(HAS_Y_AXIS, yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * trajWin.z[makeVector_batchIdx]);
+          updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ) OPTARG(HAS_Y_AXIS, _MAX(yf, FTM_MIN_SHAPE_FREQ)));
         }
         break;
     #endif
@@ -610,8 +652,8 @@ void FTMotion::makeVector() {
       case dynFreqMode_MASS_BASED:
         // Update constantly. The optimization done for Z value makes
         // less sense for E, as E is expected to constantly change.
-        updateShapingN(      cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx]
-          OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx]) );
+        updateShapingN(      cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * trajWin.e[makeVector_batchIdx]
+          OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * trajWin.e[makeVector_batchIdx]) );
         break;
     #endif
 
@@ -621,18 +663,18 @@ void FTMotion::makeVector() {
   // Apply shaping if in mode.
   #if HAS_X_AXIS
     if (cfg.modeHasShaper()) {
-      shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx];
-      traj.x[makeVector_batchIdx] *= shaping.x.Ai[0];
+      shaping.x.d_zi[shaping.zi_idx] = trajWin.x[makeVector_batchIdx];
+      trajWin.x[makeVector_batchIdx] *= shaping.x.Ai[0];
       #if HAS_Y_AXIS
-        shaping.y.d_zi[shaping.zi_idx] = traj.y[makeVector_batchIdx];
-        traj.y[makeVector_batchIdx] *= shaping.y.Ai[0];
+        shaping.y.d_zi[shaping.zi_idx] = trajWin.y[makeVector_batchIdx];
+        trajWin.y[makeVector_batchIdx] *= shaping.y.Ai[0];
       #endif
       for (uint32_t i = 1U; i <= shaping.max_i; i++) {
         const uint32_t udiffx = shaping.zi_idx - shaping.x.Ni[i];
-        traj.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
+        trajWin.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
         #if HAS_Y_AXIS
           const uint32_t udiffy = shaping.zi_idx - shaping.y.Ni[i];
-          traj.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
+          trajWin.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
         #endif
       }
       if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0;
@@ -640,18 +682,38 @@ void FTMotion::makeVector() {
   #endif
 
   // Filled up the queue with regular and shaped steps
-  if (++makeVector_batchIdx == (FTM_WINDOW_SIZE)) {
-    makeVector_batchIdx = last_batchIdx;
+  if (++makeVector_batchIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE))) {
+    makeVector_batchIdx = 0;
     batchRdy = true;
   }
 
-  if (makeVector_idx == max_intervals) {
+  if (++makeVector_idx == max_intervals) {
     blockProcDn = true;
     blockProcRdy = false;
     makeVector_idx = 0;
   }
-  else
-    makeVector_idx++;
+}
+
+/**
+ * Convert to steps
+ * - Commands are written in a bitmask with step and dir as single bits.
+ * - Tests for delta are moved outside the loop.
+ * - Two functions are used for command computation with an array of function pointers.
+ */
+static void (*command_set[NUM_AXES TERN0(HAS_EXTRUDERS, +1)])(int32_t&, int32_t&, ft_command_t&, int32_t, int32_t);
+
+static void command_set_pos(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) {
+  if (e < FTM_CTS_COMPARE_VAL) return;
+  s++;
+  b |= bd | bs;
+  e -= FTM_STEPS_PER_UNIT_TIME;
+}
+
+static void command_set_neg(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) {
+  if (e > -(FTM_CTS_COMPARE_VAL)) return;
+  s--;
+  b |= bs;
+  e += FTM_STEPS_PER_UNIT_TIME;
 }
 
 // Interpolates single data point to stepper commands.
@@ -660,154 +722,61 @@ void FTMotion::convertToSteps(const uint32_t idx) {
 
   //#define STEPS_ROUNDING
   #if ENABLED(STEPS_ROUNDING)
-    const xyze_float_t steps_tar = LOGICAL_AXIS_ARRAY(
-      trajMod.e[idx] * planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)] + (trajMod.e[idx] < 0.0f ? -0.5f : 0.5f), // May be eliminated if guaranteed positive.
-      trajMod.x[idx] * planner.settings.axis_steps_per_mm[X_AXIS] + (trajMod.x[idx] < 0.0f ? -0.5f : 0.5f),
-      trajMod.y[idx] * planner.settings.axis_steps_per_mm[Y_AXIS] + (trajMod.y[idx] < 0.0f ? -0.5f : 0.5f),
-      trajMod.z[idx] * planner.settings.axis_steps_per_mm[Z_AXIS] + (trajMod.z[idx] < 0.0f ? -0.5f : 0.5f),
-      trajMod.i[idx] * planner.settings.axis_steps_per_mm[I_AXIS] + (trajMod.i[idx] < 0.0f ? -0.5f : 0.5f),
-      trajMod.j[idx] * planner.settings.axis_steps_per_mm[J_AXIS] + (trajMod.j[idx] < 0.0f ? -0.5f : 0.5f),
-      trajMod.k[idx] * planner.settings.axis_steps_per_mm[K_AXIS] + (trajMod.k[idx] < 0.0f ? -0.5f : 0.5f),
-      trajMod.u[idx] * planner.settings.axis_steps_per_mm[U_AXIS] + (trajMod.u[idx] < 0.0f ? -0.5f : 0.5f),
-      trajMod.v[idx] * planner.settings.axis_steps_per_mm[V_AXIS] + (trajMod.v[idx] < 0.0f ? -0.5f : 0.5f),
-      trajMod.w[idx] * planner.settings.axis_steps_per_mm[W_AXIS] + (trajMod.w[idx] < 0.0f ? -0.5f : 0.5f),
+    #define TOSTEPS(A,B) int32_t(trajMod.A[idx] * planner.settings.axis_steps_per_mm[B] + (trajMod.A[idx] < 0.0f ? -0.5f : 0.5f))
+    const xyze_long_t steps_tar = LOGICAL_AXIS_ARRAY(
+      TOSTEPS(e, E_AXIS_N(current_block->extruder)), // May be eliminated if guaranteed positive.
+      TOSTEPS(x, X_AXIS), TOSTEPS(y, Y_AXIS), TOSTEPS(z, Z_AXIS),
+      TOSTEPS(i, I_AXIS), TOSTEPS(j, J_AXIS), TOSTEPS(k, K_AXIS),
+      TOSTEPS(u, U_AXIS), TOSTEPS(v, V_AXIS), TOSTEPS(w, W_AXIS)
     );
-    xyze_long_t delta = xyze_long_t(steps_tar) - steps;
-    //const xyze_long_t delta = LOGICAL_AXIS_ARRAY(
-    //  int32_t(steps_tar.e) - steps.e,
-    //  int32_t(steps_tar.x) - steps.x,
-    //  int32_t(steps_tar.y) - steps.y,
-    //  int32_t(steps_tar.z) - steps.z,
-    //  int32_t(steps_tar.i) - steps.i,
-    //  int32_t(steps_tar.j) - steps.j,
-    //  int32_t(steps_tar.k) - steps.k,
-    //  int32_t(steps_tar.u) - steps.u,
-    //  int32_t(steps_tar.v) - steps.v,
-    //  int32_t(steps_tar.w) - steps.w
-    //);
+    xyze_long_t delta = steps_tar - steps;
   #else
+    #define TOSTEPS(A,B) int32_t(trajMod.A[idx] * planner.settings.axis_steps_per_mm[B]) - steps.A
     xyze_long_t delta = LOGICAL_AXIS_ARRAY(
-      int32_t(trajMod.e[idx] * planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)]) - steps.e,
-      int32_t(trajMod.x[idx] * planner.settings.axis_steps_per_mm[X_AXIS]) - steps.x,
-      int32_t(trajMod.y[idx] * planner.settings.axis_steps_per_mm[Y_AXIS]) - steps.y,
-      int32_t(trajMod.z[idx] * planner.settings.axis_steps_per_mm[Z_AXIS]) - steps.z,
-      int32_t(trajMod.i[idx] * planner.settings.axis_steps_per_mm[I_AXIS]) - steps.i,
-      int32_t(trajMod.j[idx] * planner.settings.axis_steps_per_mm[J_AXIS]) - steps.j,
-      int32_t(trajMod.k[idx] * planner.settings.axis_steps_per_mm[K_AXIS]) - steps.k,
-      int32_t(trajMod.u[idx] * planner.settings.axis_steps_per_mm[U_AXIS]) - steps.u,
-      int32_t(trajMod.v[idx] * planner.settings.axis_steps_per_mm[V_AXIS]) - steps.v,
-      int32_t(trajMod.w[idx] * planner.settings.axis_steps_per_mm[W_AXIS]) - steps.w
+      TOSTEPS(e, E_AXIS_N(current_block->extruder)),
+      TOSTEPS(x, X_AXIS), TOSTEPS(y, Y_AXIS), TOSTEPS(z, Z_AXIS),
+      TOSTEPS(i, I_AXIS), TOSTEPS(j, J_AXIS), TOSTEPS(k, K_AXIS),
+      TOSTEPS(u, U_AXIS), TOSTEPS(v, V_AXIS), TOSTEPS(w, W_AXIS)
     );
   #endif
 
-  bool any_dirChange = (false
-    LOGICAL_AXIS_GANG(
-      || (delta.e > 0 && dirState.e != stepDirState_POS) || (delta.e < 0 && dirState.e != stepDirState_NEG),
-      || (delta.x > 0 && dirState.x != stepDirState_POS) || (delta.x < 0 && dirState.x != stepDirState_NEG),
-      || (delta.y > 0 && dirState.y != stepDirState_POS) || (delta.y < 0 && dirState.y != stepDirState_NEG),
-      || (delta.z > 0 && dirState.z != stepDirState_POS) || (delta.z < 0 && dirState.z != stepDirState_NEG),
-      || (delta.i > 0 && dirState.i != stepDirState_POS) || (delta.i < 0 && dirState.i != stepDirState_NEG),
-      || (delta.j > 0 && dirState.j != stepDirState_POS) || (delta.j < 0 && dirState.j != stepDirState_NEG),
-      || (delta.k > 0 && dirState.k != stepDirState_POS) || (delta.k < 0 && dirState.k != stepDirState_NEG),
-      || (delta.u > 0 && dirState.u != stepDirState_POS) || (delta.u < 0 && dirState.u != stepDirState_NEG),
-      || (delta.v > 0 && dirState.v != stepDirState_POS) || (delta.v < 0 && dirState.v != stepDirState_NEG),
-      || (delta.w > 0 && dirState.w != stepDirState_POS) || (delta.w < 0 && dirState.w != stepDirState_NEG)
-    )
+  LOGICAL_AXIS_CODE(
+    command_set[E_AXIS_N(current_block->extruder)] = delta.e >= 0 ?  command_set_pos : command_set_neg,
+    command_set[X_AXIS] = delta.x >= 0 ?  command_set_pos : command_set_neg,
+    command_set[Y_AXIS] = delta.y >= 0 ?  command_set_pos : command_set_neg,
+    command_set[Z_AXIS] = delta.z >= 0 ?  command_set_pos : command_set_neg,
+    command_set[I_AXIS] = delta.i >= 0 ?  command_set_pos : command_set_neg,
+    command_set[J_AXIS] = delta.j >= 0 ?  command_set_pos : command_set_neg,
+    command_set[K_AXIS] = delta.k >= 0 ?  command_set_pos : command_set_neg,
+    command_set[U_AXIS] = delta.u >= 0 ?  command_set_pos : command_set_neg,
+    command_set[V_AXIS] = delta.v >= 0 ?  command_set_pos : command_set_neg,
+    command_set[W_AXIS] = delta.w >= 0 ?  command_set_pos : command_set_neg
   );
 
   for (uint32_t i = 0U; i < (FTM_STEPS_PER_UNIT_TIME); i++) {
 
-    // TODO: (?) Since the *delta variables will not change,
-    // the comparison may be done once before iterating at
-    // expense of storage and lines of code.
-
-    bool anyStep = false;
-
-    // Commands are written in a bitmask with step and dir as single bits
-    auto COMMAND_SET = [&](auto &d, auto &e, auto &s, auto &b, auto bd, auto bs) {
-      if (d >= 0) {
-        if (e + d < (FTM_CTS_COMPARE_VAL)) {
-          e += d;
-        }
-        else {
-          s++;
-          b |= bd | bs;
-          e += d - (FTM_STEPS_PER_UNIT_TIME);
-          anyStep = true;
-        }
-      }
-      else {
-        if ((e + d) > -(FTM_CTS_COMPARE_VAL)) {
-          e += d;
-        }
-        else {
-          s--;
-          b |= bs;
-          e += d + (FTM_STEPS_PER_UNIT_TIME);
-          anyStep = true;
-        }
-      }
-    };
-
     // Init all step/dir bits to 0 (defaulting to reverse/negative motion)
     stepperCmdBuff[stepperCmdBuff_produceIdx] = 0;
 
+    err_P += delta;
+
     // Set up step/dir bits for all axes
     LOGICAL_AXIS_CODE(
-      COMMAND_SET(delta.e, err_P.e, steps.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E), _BV(FT_BIT_STEP_E)),
-      COMMAND_SET(delta.x, err_P.x, steps.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X), _BV(FT_BIT_STEP_X)),
-      COMMAND_SET(delta.y, err_P.y, steps.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y), _BV(FT_BIT_STEP_Y)),
-      COMMAND_SET(delta.z, err_P.z, steps.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z), _BV(FT_BIT_STEP_Z)),
-      COMMAND_SET(delta.i, err_P.i, steps.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I), _BV(FT_BIT_STEP_I)),
-      COMMAND_SET(delta.j, err_P.j, steps.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J), _BV(FT_BIT_STEP_J)),
-      COMMAND_SET(delta.k, err_P.k, steps.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K), _BV(FT_BIT_STEP_K)),
-      COMMAND_SET(delta.u, err_P.u, steps.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U), _BV(FT_BIT_STEP_U)),
-      COMMAND_SET(delta.v, err_P.v, steps.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V), _BV(FT_BIT_STEP_V)),
-      COMMAND_SET(delta.w, err_P.w, steps.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W), _BV(FT_BIT_STEP_W)),
+      command_set[E_AXIS_N(current_block->extruder)](err_P.e, steps.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E), _BV(FT_BIT_STEP_E)),
+      command_set[X_AXIS](err_P.x, steps.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X), _BV(FT_BIT_STEP_X)),
+      command_set[Y_AXIS](err_P.y, steps.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y), _BV(FT_BIT_STEP_Y)),
+      command_set[Z_AXIS](err_P.z, steps.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z), _BV(FT_BIT_STEP_Z)),
+      command_set[I_AXIS](err_P.i, steps.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I), _BV(FT_BIT_STEP_I)),
+      command_set[J_AXIS](err_P.j, steps.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J), _BV(FT_BIT_STEP_J)),
+      command_set[K_AXIS](err_P.k, steps.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K), _BV(FT_BIT_STEP_K)),
+      command_set[U_AXIS](err_P.u, steps.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U), _BV(FT_BIT_STEP_U)),
+      command_set[V_AXIS](err_P.v, steps.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V), _BV(FT_BIT_STEP_V)),
+      command_set[W_AXIS](err_P.w, steps.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W), _BV(FT_BIT_STEP_W)),
     );
 
-    if (!anyStep) {
-      nextStepTicks += (FTM_MIN_TICKS);
-    }
-    else {
-      stepperCmdBuff_StepRelativeTi[stepperCmdBuff_produceIdx] = nextStepTicks;
-
-      const uint8_t dir_index = stepperCmdBuff_produceIdx >> 3,
-                    dir_bit = stepperCmdBuff_produceIdx & 0x7;
-      if (any_dirChange) {
-
-        SBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
-
-        auto DIR_SET = [&](auto &d, auto &c, auto &b, auto bd) {
-          if (d > 0) { b |= bd; c = stepDirState_POS; } else { c = stepDirState_NEG; }
-        };
-
-        LOGICAL_AXIS_CODE(
-          DIR_SET(delta.e, dirState.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E)),
-          DIR_SET(delta.x, dirState.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X)),
-          DIR_SET(delta.y, dirState.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y)),
-          DIR_SET(delta.z, dirState.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z)),
-          DIR_SET(delta.i, dirState.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I)),
-          DIR_SET(delta.j, dirState.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J)),
-          DIR_SET(delta.k, dirState.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K)),
-          DIR_SET(delta.u, dirState.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U)),
-          DIR_SET(delta.v, dirState.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V)),
-          DIR_SET(delta.w, dirState.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W)),
-        );
-
-        any_dirChange = false;
-      }
-      else { // ...no direction change.
-        CBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
-      }
+    if (++stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE))
+      stepperCmdBuff_produceIdx = 0;
 
-      if (stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE) - 1)
-        stepperCmdBuff_produceIdx = 0;
-      else
-        stepperCmdBuff_produceIdx++;
-
-      nextStepTicks = FTM_MIN_TICKS;
-    }
   } // FTM_STEPS_PER_UNIT_TIME loop
 }
 
diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h
index 3f620b2f54198..cea754088138b 100644
--- a/Marlin/src/module/ft_motion.h
+++ b/Marlin/src/module/ft_motion.h
@@ -26,8 +26,6 @@
 
 #include "ft_types.h"
 
-#define FTM_STEPPERCMD_DIR_SIZE ((FTM_STEPPERCMD_BUFF_SIZE + 7) / 8)
-
 #if HAS_X_AXIS && (HAS_Z_AXIS || HAS_EXTRUDERS)
   #define HAS_DYNAMIC_FREQ 1
   #if HAS_Z_AXIS
@@ -39,28 +37,30 @@
 #endif
 
 typedef struct FTConfig {
-  ftMotionMode_t mode = FTM_DEFAULT_MODE;                   // Mode / active compensation mode configuration.
+  ftMotionMode_t mode = FTM_DEFAULT_MODE;                 // Mode / active compensation mode configuration.
 
   bool modeHasShaper() { return WITHIN(mode, 10U, 19U); }
 
   #if HAS_X_AXIS
-    float baseFreq[1 + ENABLED(HAS_Y_AXIS)] =               // Base frequency. [Hz]
+    float baseFreq[1 + ENABLED(HAS_Y_AXIS)] =             // Base frequency. [Hz]
       { FTM_SHAPING_DEFAULT_X_FREQ OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) };
+    
+    float zeta[1 + ENABLED(HAS_Y_AXIS)] =                 // Damping factor
+        { FTM_SHAPING_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_ZETA_Y) };
+    float vtol[1 + ENABLED(HAS_Y_AXIS)] =                 // Vibration Level
+        { FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) };
   #endif
 
-  float zeta = FTM_SHAPING_ZETA;                            // Damping factor
-  float vtol = FTM_SHAPING_V_TOL;                           // Vibration Level
-
-  #if HAS_DYNAMIC_FREQ
-    dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE;   // Dynamic frequency mode configuration.
-    float dynFreqK[1 + ENABLED(HAS_Y_AXIS)] = { 0.0f };     // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
+#if HAS_DYNAMIC_FREQ
+    dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration.
+    float dynFreqK[1 + ENABLED(HAS_Y_AXIS)] = { 0.0f };   // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
   #else
     static constexpr dynFreqMode_t dynFreqMode = dynFreqMode_DISABLED;
   #endif
 
   #if HAS_EXTRUDERS
-    bool linearAdvEna = FTM_LINEAR_ADV_DEFAULT_ENA;         // Linear advance enable configuration.
-    float linearAdvK = FTM_LINEAR_ADV_DEFAULT_K;            // Linear advance gain.
+    bool linearAdvEna = FTM_LINEAR_ADV_DEFAULT_ENA;       // Linear advance enable configuration.
+    float linearAdvK = FTM_LINEAR_ADV_DEFAULT_K;          // Linear advance gain.
   #endif
 } ft_config_t;
 
@@ -78,8 +78,11 @@ class FTMotion {
       TERN_(HAS_X_AXIS, cfg.baseFreq[X_AXIS] = FTM_SHAPING_DEFAULT_X_FREQ);
       TERN_(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] = FTM_SHAPING_DEFAULT_Y_FREQ);
 
-      cfg.zeta = FTM_SHAPING_ZETA;  // Damping factor
-      cfg.vtol = FTM_SHAPING_V_TOL; // Vibration Level
+      TERN_(HAS_X_AXIS, cfg.zeta[X_AXIS] = FTM_SHAPING_ZETA_X);
+      TERN_(HAS_Y_AXIS, cfg.zeta[Y_AXIS] = FTM_SHAPING_ZETA_Y);
+
+      TERN_(HAS_X_AXIS, cfg.vtol[X_AXIS] = FTM_SHAPING_V_TOL_X);
+      TERN_(HAS_Y_AXIS, cfg.vtol[Y_AXIS] = FTM_SHAPING_V_TOL_Y);
 
       #if HAS_DYNAMIC_FREQ
         cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE;
@@ -99,47 +102,45 @@ class FTMotion {
       reset();
     }
 
-    static ft_command_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE];               // Buffer of stepper commands.
-    static hal_timer_t stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of the stepper command timing.
-    static uint8_t stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE];            // Buffer of whether DIR needs to be updated.
-    static uint32_t stepperCmdBuff_produceIdx,              // Index of next stepper command write to the buffer.
-                    stepperCmdBuff_consumeIdx;              // Index of next stepper command read from the buffer.
+    static ft_command_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of stepper commands.
+    static uint32_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 sts_stepperBusy;                            // The stepper buffer has items and is in use.
+    static bool sts_stepperBusy;                          // The stepper buffer has items and is in use.
 
 
     // Public methods
     static void init();
-    static void startBlockProc(block_t * const current_block); // Set controller states to begin processing a block.
-    static bool getBlockProcDn() { return blockProcDn; }    // Return true if the controller no longer needs the current block.
-    static void runoutBlock();                              // Move any free data points to the stepper buffer even if a full batch isn't ready.
-    static void loop();                                     // Controller main, to be invoked from non-isr task.
+    static void startBlockProc();                         // Set controller states to begin processing a block.
+    static bool getBlockProcDn() { return blockProcDn; }  // Return true if the controller no longer needs the current block.
+    static void runoutBlock();                            // Move any free data points to the stepper buffer even if a full batch isn't ready.
+    static void loop();                                   // Controller main, to be invoked from non-isr task.
 
     #if HAS_X_AXIS
       // Refresh the gains used by shaping functions.
       // To be called on init or mode or zeta change.
-      static void updateShapingA(const_float_t zeta=cfg.zeta, const_float_t vtol=cfg.vtol);
+      static void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol);
 
       // Refresh the indices used by shaping functions.
       // To be called when frequencies change.
-      static void updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), const_float_t zeta=cfg.zeta);
+      static void updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), float zeta[]=cfg.zeta);
 
       static void refreshShapingN() { updateShapingN(cfg.baseFreq[X_AXIS] OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS])); }
 
     #endif
 
-    static void reset();                                    // Resets all states of the fixed time conversion to defaults.
+    static void reset();                                  // Reset all states of the fixed time conversion to defaults.
 
   private:
 
     static xyze_trajectory_t traj;
     static xyze_trajectoryMod_t trajMod;
+    static xyze_trajectoryWin_t trajWin;
 
-    static block_t *current_block_cpy;
     static bool blockProcRdy, blockProcRdy_z1, blockProcDn;
     static bool batchRdy, batchRdyForInterp;
     static bool runoutEna;
-    static bool runout;
+    static bool blockDataIsRunout;
 
     // Trapezoid data variables.
     static xyze_pos_t   startPosn,          // (mm) Start position of block
@@ -154,6 +155,11 @@ class FTMotion {
     static uint32_t N1, N2, N3;
     static uint32_t max_intervals;
 
+    static constexpr uint32_t _ftm_size = TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE),
+                              _ftm_wind = TERN(FTM_UNIFIED_BWS, 2, ceil((FTM_WINDOW_SIZE) / _ftm_size)),
+                              shaper_intervals = _ftm_size * ceil((FTM_ZMAX) / _ftm_size),
+                              min_max_intervals = _ftm_size * _ftm_wind;
+
     // Make vector variables.
     static uint32_t makeVector_idx,
                     makeVector_idx_z1,
@@ -164,9 +170,6 @@ class FTMotion {
                     interpIdx_z1;
 
     static xyze_long_t steps;
-    static xyze_stepDir_t dirState;
-
-    static hal_timer_t nextStepTicks;
 
     // Shaping variables.
     #if HAS_X_AXIS
@@ -188,7 +191,7 @@ class FTMotion {
           axis_shaping_t y;
         #endif
 
-        void updateShapingA(const_float_t zeta=cfg.zeta, const_float_t vtol=cfg.vtol);
+        void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol);
 
       } shaping_t;
 
diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h
index 6b708677d19c4..06680488754fb 100644
--- a/Marlin/src/module/ft_types.h
+++ b/Marlin/src/module/ft_types.h
@@ -29,10 +29,12 @@ typedef enum FXDTICtrlMode : uint8_t {
   //ftMotionMode_ULENDO_FBS = 2U,
   ftMotionMode_ZV         = 10U,
   ftMotionMode_ZVD        = 11U,
-  ftMotionMode_EI         = 12U,
-  ftMotionMode_2HEI       = 13U,
-  ftMotionMode_3HEI       = 14U,
-  ftMotionMode_MZV        = 15U,
+  ftMotionMode_ZVDD       = 12U,
+  ftMotionMode_ZVDDD      = 13U,
+  ftMotionMode_EI         = 14U,
+  ftMotionMode_2HEI       = 15U,
+  ftMotionMode_3HEI       = 16U,
+  ftMotionMode_MZV        = 17U,
   //ftMotionMode_DISCTF   = 20U
 } ftMotionMode_t;
 
@@ -42,16 +44,17 @@ enum dynFreqMode_t : uint8_t {
   dynFreqMode_MASS_BASED = 2U
 };
 
-enum stepDirState_t : uint8_t {
-  stepDirState_NOT_SET = 0U,
-  stepDirState_POS     = 1U,
-  stepDirState_NEG     = 2U
-};
-
-typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
-typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
+#define IS_EI_MODE(N) WITHIN(N, ftMotionMode_EI, ftMotionMode_3HEI)
 
-typedef struct XYZEval<stepDirState_t> xyze_stepDir_t;
+#if ENABLED(FTM_UNIFIED_BWS)
+  typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectory_t;
+  typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryMod_t;
+  typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryWin_t;
+#else
+  typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
+  typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
+  typedef struct XYZEarray<float, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE)> xyze_trajectoryWin_t;
+#endif
 
 enum {
   LIST_N(DOUBLE(LOGICAL_AXES),
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index 74ba41ccf6990..2259eedf41d40 100644
--- a/Marlin/src/module/motion.cpp
+++ b/Marlin/src/module/motion.cpp
@@ -33,10 +33,6 @@
 #include "../lcd/marlinui.h"
 #include "../inc/MarlinConfig.h"
 
-#if ENABLED(FT_MOTION)
-  #include "ft_motion.h"
-#endif
-
 #if IS_SCARA
   #include "../libs/buzzer.h"
   #include "../lcd/marlinui.h"
@@ -2095,21 +2091,6 @@ void prepare_line_to_destination() {
 
   void homeaxis(const AxisEnum axis) {
 
-    #if ENABLED(FT_MOTION)
-      // Disable ft-motion for homing
-      struct OnExit {
-        ftMotionMode_t oldmm;
-        OnExit() {
-          oldmm = ftMotion.cfg.mode;
-          ftMotion.cfg.mode = ftMotionMode_DISABLED;
-        }
-        ~OnExit() {
-          ftMotion.cfg.mode = oldmm;
-          ftMotion.init();
-        }
-      } on_exit;
-    #endif
-
     #if ANY(MORGAN_SCARA, MP_SCARA)
       // Only Z homing (with probe) is permitted
       if (axis != Z_AXIS) { BUZZ(100, 880); return; }
diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
index c68db6bb89b40..862b2728042dc 100644
--- a/Marlin/src/module/stepper.cpp
+++ b/Marlin/src/module/stepper.cpp
@@ -1492,11 +1492,6 @@ void Stepper::isr() {
   uint8_t max_loops = 10;
 
   #if ENABLED(FT_MOTION)
-    static bool ftMotion_stepCmdRdy = false;    // Indicates a step command was loaded from the
-                                                //  buffers and is ready to be output.
-    static bool ftMotion_applyDir = false;      // Indicates the DIR output should be set.
-    static ft_command_t ftMotion_stepCmd = 0U;  // Storage for the step command to be output.
-    static uint32_t ftMotion_nextAuxISR = 0U;   // Storage for the next ISR of the auxilliary tasks.
     const bool using_ftMotion = ftMotion.cfg.mode;
   #else
     constexpr bool using_ftMotion = false;
@@ -1508,57 +1503,19 @@ void Stepper::isr() {
     // Enable ISRs to reduce USART processing latency
     hal.isr_on();
 
-    hal_timer_t interval;
+    hal_timer_t interval = 0;
 
     #if ENABLED(FT_MOTION)
 
       if (using_ftMotion) {
         if (!nextMainISR) {
-          if (abort_current_block) {
-            ftMotion_stepCmdRdy = false; // If a command was ready, cancel it.
-            ftMotion.sts_stepperBusy = false; // Set busy false to allow a reset.
-            nextMainISR = 0.01f * (STEPPER_TIMER_RATE); // Come back in 10 msec.
-          }
-          else { // !(abort_current_block)
-            if (ftMotion_stepCmdRdy) {
-              ftMotion_stepper(ftMotion_applyDir, ftMotion_stepCmd);
-              ftMotion_stepCmdRdy = false;
-            }
-            // Check if there is data in the buffers.
-            if (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx) {
-
-              ftMotion.sts_stepperBusy = true;
-
-              // "Pop" one command from the command buffer.
-              ftMotion_stepCmd = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];
-              const uint8_t dir_index = ftMotion.stepperCmdBuff_consumeIdx >> 3,
-                            dir_bit = ftMotion.stepperCmdBuff_consumeIdx & 0x7;
-              ftMotion_applyDir = TEST(ftMotion.stepperCmdBuff_ApplyDir[dir_index], dir_bit);
-              nextMainISR = ftMotion.stepperCmdBuff_StepRelativeTi[ftMotion.stepperCmdBuff_consumeIdx];
-              ftMotion_stepCmdRdy = true;
-
-              if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE))
-                ftMotion.stepperCmdBuff_consumeIdx = 0;
-
-            }
-            else { // Buffer empty.
-              ftMotion.sts_stepperBusy = false;
-              nextMainISR = 0.01f * (STEPPER_TIMER_RATE); // Come back in 10 msec.
-            }
-          } // !(abort_current_block)
-        } // if (!nextMainISR)
-
-        // Define 2.5 msec task for auxiliary functions.
-        if (!ftMotion_nextAuxISR) {
+          nextMainISR = FTM_MIN_TICKS;
+          ftMotion_stepper();
           endstops.update();
           TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
-          ftMotion_refreshAxisDidMove();
-          ftMotion_nextAuxISR = 0.0025f * (STEPPER_TIMER_RATE);
         }
-
-        interval = _MIN(nextMainISR, ftMotion_nextAuxISR);
+        interval = nextMainISR;
         nextMainISR -= interval;
-        ftMotion_nextAuxISR -= interval;
       }
 
     #endif
@@ -3365,6 +3322,28 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
   #endif
 }
 
+#if ENABLED(FT_MOTION)
+
+  void Stepper::ftMotion_syncPosition() {
+    //planner.synchronize(); planner already synchronized in M493
+
+    #ifdef __AVR__
+      // Protect the access to the position. Only required for AVR, as
+      //  any 32bit CPU offers atomic access to 32bit variables
+      const bool was_enabled = suspend();
+    #endif
+
+    // Update stepper positions from the planner
+    count_position = planner.position;
+
+    #ifdef __AVR__
+      // Reenable Stepper ISR
+      if (was_enabled) wake_up();
+    #endif
+  }
+
+#endif // FT_MOTION
+
 // Signal endstops were triggered - This function can be called from
 // an ISR context  (Temperature, Stepper or limits ISR), so we must
 // be very careful here. If the interrupt being preempted was the
@@ -3459,65 +3438,70 @@ void Stepper::report_positions() {
 #if ENABLED(FT_MOTION)
 
   // Set stepper I/O for fixed time controller.
-  void Stepper::ftMotion_stepper(const bool applyDir, const ft_command_t command) {
+  void Stepper::ftMotion_stepper() {
+
+    // Check if the buffer is empty.
+    ftMotion.sts_stepperBusy = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx);
+    if (!ftMotion.sts_stepperBusy) return;
+
+    // "Pop" one command from current motion buffer
+    // Use one byte to restore one stepper command in the format:
+    // |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction|
+    const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];
+    if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE)) ftMotion.stepperCmdBuff_consumeIdx = 0U;
+
+    if (abort_current_block) return;
 
     USING_TIMED_PULSE();
 
-    const xyze_bool_t axis_step = LOGICAL_AXIS_ARRAY(
+    axis_did_move = LOGICAL_AXIS_ARRAY(
       TEST(command, FT_BIT_STEP_E),
       TEST(command, FT_BIT_STEP_X), TEST(command, FT_BIT_STEP_Y), TEST(command, FT_BIT_STEP_Z),
       TEST(command, FT_BIT_STEP_I), TEST(command, FT_BIT_STEP_J), TEST(command, FT_BIT_STEP_K),
       TEST(command, FT_BIT_STEP_U), TEST(command, FT_BIT_STEP_V), TEST(command, FT_BIT_STEP_W)
     );
 
+    last_direction_bits = LOGICAL_AXIS_ARRAY(
+      axis_did_move.e ? TEST(command, FT_BIT_DIR_E) : last_direction_bits.e,
+      axis_did_move.x ? TEST(command, FT_BIT_DIR_X) : last_direction_bits.x,
+      axis_did_move.y ? TEST(command, FT_BIT_DIR_Y) : last_direction_bits.y,
+      axis_did_move.z ? TEST(command, FT_BIT_DIR_Z) : last_direction_bits.z,
+      axis_did_move.i ? TEST(command, FT_BIT_DIR_I) : last_direction_bits.i,
+      axis_did_move.j ? TEST(command, FT_BIT_DIR_J) : last_direction_bits.j,
+      axis_did_move.k ? TEST(command, FT_BIT_DIR_K) : last_direction_bits.k,
+      axis_did_move.u ? TEST(command, FT_BIT_DIR_U) : last_direction_bits.u,
+      axis_did_move.v ? TEST(command, FT_BIT_DIR_V) : last_direction_bits.v,
+      axis_did_move.w ? TEST(command, FT_BIT_DIR_W) : last_direction_bits.w
+    );
+
     // Apply directions (which will apply to the entire linear move)
-    AxisBits axis_dir = last_direction_bits;
-    if (applyDir) {
-      axis_dir = LOGICAL_AXIS_ARRAY(
-        TEST(command, FT_BIT_DIR_E),
-        TEST(command, FT_BIT_DIR_X), TEST(command, FT_BIT_DIR_Y), TEST(command, FT_BIT_DIR_Z),
-        TEST(command, FT_BIT_DIR_I), TEST(command, FT_BIT_DIR_J), TEST(command, FT_BIT_DIR_K),
-        TEST(command, FT_BIT_DIR_U), TEST(command, FT_BIT_DIR_V), TEST(command, FT_BIT_DIR_W)
-      );
-      LOGICAL_AXIS_CODE(
-        E_APPLY_DIR(axis_dir.e, false),
-        X_APPLY_DIR(axis_dir.x, false), Y_APPLY_DIR(axis_dir.y, false), Z_APPLY_DIR(axis_dir.z, false),
-        I_APPLY_DIR(axis_dir.i, false), J_APPLY_DIR(axis_dir.j, false), K_APPLY_DIR(axis_dir.k, false),
-        U_APPLY_DIR(axis_dir.u, false), V_APPLY_DIR(axis_dir.v, false), W_APPLY_DIR(axis_dir.w, false)
-      );
-      last_direction_bits = axis_dir;
-      DIR_WAIT_AFTER();
-    }
+    LOGICAL_AXIS_CODE(
+      E_APPLY_DIR(last_direction_bits.e, false),
+      X_APPLY_DIR(last_direction_bits.x, false), Y_APPLY_DIR(last_direction_bits.y, false), Z_APPLY_DIR(last_direction_bits.z, false),
+      I_APPLY_DIR(last_direction_bits.i, false), J_APPLY_DIR(last_direction_bits.j, false), K_APPLY_DIR(last_direction_bits.k, false),
+      U_APPLY_DIR(last_direction_bits.u, false), V_APPLY_DIR(last_direction_bits.v, false), W_APPLY_DIR(last_direction_bits.w, false)
+    );
+
+    DIR_WAIT_AFTER();
 
     // Start a step pulse
     LOGICAL_AXIS_CODE(
-      if (axis_step.e) E_APPLY_STEP(STEP_STATE_E, false),
-      if (axis_step.x) X_APPLY_STEP(STEP_STATE_X, false), if (axis_step.y) Y_APPLY_STEP(STEP_STATE_Y, false),
-      if (axis_step.z) Z_APPLY_STEP(STEP_STATE_Z, false), if (axis_step.i) I_APPLY_STEP(STEP_STATE_I, false),
-      if (axis_step.j) J_APPLY_STEP(STEP_STATE_J, false), if (axis_step.k) K_APPLY_STEP(STEP_STATE_K, false),
-      if (axis_step.u) U_APPLY_STEP(STEP_STATE_U, false), if (axis_step.v) V_APPLY_STEP(STEP_STATE_V, false),
-      if (axis_step.w) W_APPLY_STEP(STEP_STATE_W, false)
+      E_APPLY_STEP(axis_did_move.e, false),
+      X_APPLY_STEP(axis_did_move.x, false), Y_APPLY_STEP(axis_did_move.y, false), Z_APPLY_STEP(axis_did_move.z, false),
+      I_APPLY_STEP(axis_did_move.i, false), J_APPLY_STEP(axis_did_move.j, false), K_APPLY_STEP(axis_did_move.k, false),
+      U_APPLY_STEP(axis_did_move.u, false), V_APPLY_STEP(axis_did_move.v, false), W_APPLY_STEP(axis_did_move.w, false)
     );
 
     // Begin waiting for the minimum pulse duration
     START_TIMED_PULSE();
 
-    // Update axis direction adders
-    count_direction = LOGICAL_AXIS_ARRAY(
-      int8_t(axis_dir.e ? 1 : -1),
-      int8_t(axis_dir.x ? 1 : -1), int8_t(axis_dir.y ? 1 : -1), int8_t(axis_dir.z ? 1 : -1),
-      int8_t(axis_dir.i ? 1 : -1), int8_t(axis_dir.j ? 1 : -1), int8_t(axis_dir.k ? 1 : -1),
-      int8_t(axis_dir.u ? 1 : -1), int8_t(axis_dir.v ? 1 : -1), int8_t(axis_dir.w ? 1 : -1)
-    );
-
-    // Update stepper counts - required for various operations
+    // Update step counts
     LOGICAL_AXIS_CODE(
-      if (axis_step.e) count_position.e += count_direction.e,
-      if (axis_step.x) count_position.x += count_direction.x, if (axis_step.y) count_position.y += count_direction.y,
-      if (axis_step.z) count_position.z += count_direction.z, if (axis_step.i) count_position.i += count_direction.i,
-      if (axis_step.j) count_position.j += count_direction.j, if (axis_step.k) count_position.k += count_direction.k,
-      if (axis_step.u) count_position.u += count_direction.u, if (axis_step.v) count_position.v += count_direction.v,
-      if (axis_step.w) count_position.w += count_direction.w
+      if (axis_did_move.e) count_position.e += last_direction_bits.e ? 1 : -1, if (axis_did_move.x) count_position.x += last_direction_bits.x ? 1 : -1,
+      if (axis_did_move.y) count_position.y += last_direction_bits.y ? 1 : -1, if (axis_did_move.z) count_position.z += last_direction_bits.z ? 1 : -1,
+      if (axis_did_move.i) count_position.i += last_direction_bits.i ? 1 : -1, if (axis_did_move.j) count_position.j += last_direction_bits.j ? 1 : -1,
+      if (axis_did_move.k) count_position.k += last_direction_bits.k ? 1 : -1, if (axis_did_move.u) count_position.u += last_direction_bits.u ? 1 : -1,
+      if (axis_did_move.v) count_position.v += last_direction_bits.v ? 1 : -1, if (axis_did_move.w) count_position.w += last_direction_bits.w ? 1 : -1
     );
 
     #if HAS_EXTRUDERS
@@ -3532,10 +3516,10 @@ void Stepper::report_positions() {
 
     // Only wait for axes without edge stepping
     const bool any_wait = false LOGICAL_AXIS_GANG(
-      || (!e_axis_has_dedge && axis_step.e),
-      || (!AXIS_HAS_DEDGE(X) && axis_step.x), || (!AXIS_HAS_DEDGE(Y) && axis_step.y), || (!AXIS_HAS_DEDGE(Z) && axis_step.z),
-      || (!AXIS_HAS_DEDGE(I) && axis_step.i), || (!AXIS_HAS_DEDGE(J) && axis_step.j), || (!AXIS_HAS_DEDGE(K) && axis_step.k),
-      || (!AXIS_HAS_DEDGE(U) && axis_step.u), || (!AXIS_HAS_DEDGE(V) && axis_step.v), || (!AXIS_HAS_DEDGE(W) && axis_step.w)
+      || (!e_axis_has_dedge && axis_did_move.e),
+      || (!AXIS_HAS_DEDGE(X) && axis_did_move.x), || (!AXIS_HAS_DEDGE(Y) && axis_did_move.y), || (!AXIS_HAS_DEDGE(Z) && axis_did_move.z),
+      || (!AXIS_HAS_DEDGE(I) && axis_did_move.i), || (!AXIS_HAS_DEDGE(J) && axis_did_move.j), || (!AXIS_HAS_DEDGE(K) && axis_did_move.k),
+      || (!AXIS_HAS_DEDGE(U) && axis_did_move.u), || (!AXIS_HAS_DEDGE(V) && axis_did_move.v), || (!AXIS_HAS_DEDGE(W) && axis_did_move.w)
     );
 
     // Allow pulses to be registered by stepper drivers
@@ -3543,80 +3527,46 @@ void Stepper::report_positions() {
 
     // Stop pulses. Axes with DEDGE will do nothing, assuming STEP_STATE_* is HIGH
     LOGICAL_AXIS_CODE(
-      if (axis_step.e) E_APPLY_STEP(!STEP_STATE_E, false),
-      if (axis_step.x) X_APPLY_STEP(!STEP_STATE_X, false), if (axis_step.y) Y_APPLY_STEP(!STEP_STATE_Y, false),
-      if (axis_step.z) Z_APPLY_STEP(!STEP_STATE_Z, false), if (axis_step.i) I_APPLY_STEP(!STEP_STATE_I, false),
-      if (axis_step.j) J_APPLY_STEP(!STEP_STATE_J, false), if (axis_step.k) K_APPLY_STEP(!STEP_STATE_K, false),
-      if (axis_step.u) U_APPLY_STEP(!STEP_STATE_U, false), if (axis_step.v) V_APPLY_STEP(!STEP_STATE_V, false),
-      if (axis_step.w) W_APPLY_STEP(!STEP_STATE_W, false)
+      E_APPLY_STEP(!STEP_STATE_E, false),
+      X_APPLY_STEP(!STEP_STATE_X, false), Y_APPLY_STEP(!STEP_STATE_Y, false), Z_APPLY_STEP(!STEP_STATE_Z, false),
+      I_APPLY_STEP(!STEP_STATE_I, false), J_APPLY_STEP(!STEP_STATE_J, false), K_APPLY_STEP(!STEP_STATE_K, false),
+      U_APPLY_STEP(!STEP_STATE_U, false), V_APPLY_STEP(!STEP_STATE_V, false), W_APPLY_STEP(!STEP_STATE_W, false)
     );
 
   } // Stepper::ftMotion_stepper
 
-  void Stepper::ftMotion_BlockQueueUpdate() {
+  void Stepper::ftMotion_blockQueueUpdate() {
 
     if (current_block) {
       // If the current block is not done processing, return right away
       if (!ftMotion.getBlockProcDn()) return;
 
       axis_did_move.reset();
-      current_block = nullptr;
-      discard_current_block();
+      planner.release_current_block();
     }
 
-    if (!current_block) { // No current block
-
-      // Check the buffer for a new block
-      current_block = planner.get_current_block();
-
-      if (current_block) {
-        // Sync block? Sync the stepper counts and return
-        while (current_block->is_sync()) {
-          if (!(current_block->is_fan_sync() || current_block->is_pwr_sync())) _set_position(current_block->position);
-          discard_current_block();
-
-          // Try to get a new block
-          if (!(current_block = planner.get_current_block()))
-            return; // No more queued movements!image.png
-        }
+    // Check the buffer for a new block
+    current_block = planner.get_current_block();
 
-        // this is needed by motor_direction() and subsequently bed leveling (somehow)
-        // update it here, even though it will may be out of sync with step commands
-        last_direction_bits = current_block->direction_bits;
-
-        ftMotion.startBlockProc(current_block);
+    if (current_block) {
+      // Sync block? Sync the stepper counts and return
+      while (current_block->is_sync()) {
+        TERN_(LASER_FEATURE, if (!(current_block->is_fan_sync() || current_block->is_pwr_sync()))) _set_position(current_block->position);
+        
+        planner.release_current_block();
 
-      }
-      else {
-        ftMotion.runoutBlock();
-        return; // No queued blocks
+        // Try to get a new block
+        if (!(current_block = planner.get_current_block()))
+          return; // No queued blocks.
       }
 
-    } // if (!current_block)
-
-  } // Stepper::ftMotion_BlockQueueUpdate()
-
-  // Debounces the axis move indication to account for potential
-  // delay between the block information and the stepper commands
-  void Stepper::ftMotion_refreshAxisDidMove() {
-
-    // Set the debounce time in seconds.
-    #define AXIS_DID_MOVE_DEB 5 // TODO: The debounce time should be calculated if possible,
-                                // or the set conditions should be changed from the block to
-                                // the motion trajectory or motor commands.
-
-    AxisBits didmove;
-    static abce_ulong_t debounce{0};
-    auto debounce_axis = [&](const AxisEnum axis) {
-      if (current_block->steps[axis]) debounce[axis] = (AXIS_DID_MOVE_DEB) * 400; // divide by 0.0025f */
-      if (debounce[axis]) { didmove.bset(axis); debounce[axis]--; }
-    };
-    #define _DEBOUNCE(N) debounce_axis(AxisEnum(N));
+      ftMotion.startBlockProc();
+      return;
+    }
 
-    if (current_block) { REPEAT(LOGICAL_AXES, _DEBOUNCE); }
+    ftMotion.runoutBlock();
 
-    axis_did_move = didmove;
-  }
+  } // Stepper::ftMotion_blockQueueUpdate()
 
 #endif // FT_MOTION
 
diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h
index 2ccc9e7be9f82..99fd2d293609d 100644
--- a/Marlin/src/module/stepper.h
+++ b/Marlin/src/module/stepper.h
@@ -655,7 +655,9 @@ class Stepper {
 
     #if ENABLED(FT_MOTION)
       // Manage the planner
-      static void ftMotion_BlockQueueUpdate();
+      static void ftMotion_blockQueueUpdate();
+      // Set current position in steps when reset flag is set in M493 and planner already synchronized
+      static void ftMotion_syncPosition();
     #endif
 
     #if HAS_ZV_SHAPING
@@ -694,8 +696,7 @@ class Stepper {
     #endif
 
     #if ENABLED(FT_MOTION)
-      static void ftMotion_stepper(const bool applyDir, const ft_command_t command);
-      static void ftMotion_refreshAxisDidMove();
+      static void ftMotion_stepper();
     #endif
 
 };