diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 070309d5aa67..18e5d9e64766 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -1086,7 +1086,51 @@
#endif
-// @section motion
+// @section motion control
+
+/**
+ * Fixed-time-based Motion Control -- EXPERIMENTAL
+ * Enable/disable and set parameters with G-code M493.
+ */
+//#define FT_MOTION
+#if ENABLED(FT_MOTION)
+ #define FTM_DEFAULT_MODE ftMotionMode_ENABLED // 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.
+
+ /**
+ * Advanced configuration
+ */
+ #define FTM_BATCH_SIZE 100 // Batch size for trajectory generation;
+ // half the window size for Ulendo FBS.
+ #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.
+#endif
/**
* Input Shaping -- EXPERIMENTAL
@@ -1125,6 +1169,8 @@
//#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters.
#endif
+// @section motion
+
#define AXIS_RELATIVE_MODES { false, false, false, false }
// Add a Duplicate option for well-separated conjoined nozzles
diff --git a/Marlin/src/MarlinCore.cpp b/Marlin/src/MarlinCore.cpp
index 213cbebc2627..19aaedf7c65d 100644
--- a/Marlin/src/MarlinCore.cpp
+++ b/Marlin/src/MarlinCore.cpp
@@ -50,6 +50,9 @@
#include "module/settings.h"
#include "module/stepper.h"
#include "module/temperature.h"
+#if ENABLED(FT_MOTION)
+ #include "module/ft_motion.h"
+#endif
#include "gcode/gcode.h"
#include "gcode/parser.h"
@@ -885,8 +888,12 @@ void idle(bool no_stepper_sleep/*=false*/) {
// Update the LVGL interface
TERN_(HAS_TFT_LVGL_UI, LV_TASK_HANDLER());
+ // Manage Fixed-time Motion Control
+ TERN_(FT_MOTION, fxdTiCtrl.loop());
+
IDLE_DONE:
TERN_(MARLIN_DEV_MODE, idle_depth--);
+
return;
}
diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp
new file mode 100644
index 000000000000..31e16a194d7a
--- /dev/null
+++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp
@@ -0,0 +1,282 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(FT_MOTION)
+
+#include "../../gcode.h"
+#include "../../../module/ft_motion.h"
+
+void say_shaping() {
+ SERIAL_ECHO_TERNARY(fxdTiCtrl.cfg_mode, "Fixed time controller ", "en", "dis", "abled");
+ if (fxdTiCtrl.cfg_mode == ftMotionMode_DISABLED || fxdTiCtrl.cfg_mode == ftMotionMode_ENABLED) {
+ SERIAL_ECHOLNPGM(".");
+ return;
+ }
+ #if HAS_X_AXIS
+ SERIAL_ECHOPGM(" with ");
+ switch (fxdTiCtrl.cfg_mode) {
+ default: break;
+ //case ftMotionMode_ULENDO_FBS: SERIAL_ECHOLNPGM("Ulendo FBS."); return;
+ case ftMotionMode_ZV: SERIAL_ECHOLNPGM("ZV"); break;
+ case ftMotionMode_ZVD: SERIAL_ECHOLNPGM("ZVD"); break;
+ case ftMotionMode_EI: SERIAL_ECHOLNPGM("EI"); break;
+ case ftMotionMode_2HEI: SERIAL_ECHOLNPGM("2 Hump EI"); break;
+ case ftMotionMode_3HEI: SERIAL_ECHOLNPGM("3 Hump EI"); break;
+ case ftMotionMode_MZV: SERIAL_ECHOLNPGM("MZV"); break;
+ //case ftMotionMode_DISCTF: SERIAL_ECHOLNPGM("discrete transfer functions"); break;
+ }
+ SERIAL_ECHOLNPGM(" shaping.");
+ #endif
+}
+
+/**
+ * M493: Set Fixed-time Motion Control parameters
+ *
+ * S Set the motion / shaping mode. Shaping requires an X axis, at the minimum.
+ * 0: NORMAL
+ * 1: FIXED-TIME
+ * 10: ZV
+ * 11: ZVD
+ * 12: EI
+ * 13: 2HEI
+ * 14: 3HEI
+ * 15: MZV
+ *
+ * P Enable (1) or Disable (0) Linear Advance pressure control
+ *
+ * K Set Linear Advance gain
+ *
+ * D Set Dynamic Frequency mode
+ * 0: DISABLED
+ * 1: Z-based (Requires a Z axis)
+ * 2: Mass-based (Requires X and E axes)
+ *
+ * A Set static/base frequency for the X axis
+ * F Set frequency scaling for the X axis
+ *
+ * B Set static/base frequency for the Y axis
+ * H Set frequency scaling for the Y axis
+ */
+void GcodeSuite::M493() {
+ // Parse 'S' mode parameter.
+ if (parser.seenval('S')) {
+ const ftMotionMode_t val = (ftMotionMode_t)parser.value_byte();
+ switch (val) {
+ case ftMotionMode_DISABLED:
+ case ftMotionMode_ENABLED:
+ #if HAS_X_AXIS
+ case ftMotionMode_ZVD:
+ case ftMotionMode_2HEI:
+ case ftMotionMode_3HEI:
+ case ftMotionMode_MZV:
+ //case ftMotionMode_ULENDO_FBS:
+ //case ftMotionMode_DISCTF:
+ fxdTiCtrl.cfg_mode = val;
+ say_shaping();
+ break;
+ #endif
+ default:
+ SERIAL_ECHOLNPGM("?Invalid control mode [M] value.");
+ return;
+ }
+
+ switch (val) {
+ case ftMotionMode_ENABLED: fxdTiCtrl.reset(); break;
+ #if HAS_X_AXIS
+ case ftMotionMode_ZV:
+ case ftMotionMode_ZVD:
+ case ftMotionMode_EI:
+ case ftMotionMode_2HEI:
+ case ftMotionMode_3HEI:
+ case ftMotionMode_MZV:
+ fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
+ fxdTiCtrl.updateShapingA();
+ fxdTiCtrl.reset();
+ break;
+ //case ftMotionMode_ULENDO_FBS:
+ //case ftMotionMode_DISCTF:
+ #endif
+ default: break;
+ }
+ }
+
+ #if HAS_EXTRUDERS
+
+ // Pressure control (linear advance) parameter.
+ if (parser.seen('P')) {
+ const bool val = parser.value_bool();
+ fxdTiCtrl.cfg_linearAdvEna = val;
+ SERIAL_ECHO_TERNARY(val, "Pressure control: Linear Advance ", "en", "dis", "abled.\n");
+ }
+
+ // Pressure control (linear advance) gain parameter.
+ if (parser.seenval('K')) {
+ const float val = parser.value_float();
+ if (val >= 0.0f) {
+ fxdTiCtrl.cfg_linearAdvK = val;
+ SERIAL_ECHOPGM("Pressure control: Linear Advance gain set to: ");
+ SERIAL_ECHO_F(val, 5);
+ SERIAL_ECHOLNPGM(".");
+ }
+ else { // Value out of range.
+ SERIAL_ECHOLNPGM("Pressure control: Linear Advance gain out of range.");
+ }
+ }
+
+ #endif // HAS_EXTRUDERS
+
+ #if HAS_Z_AXIS || HAS_EXTRUDERS
+
+ // Dynamic frequency mode parameter.
+ if (parser.seenval('D')) {
+ if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
+ const dynFreqMode_t val = dynFreqMode_t(parser.value_byte());
+ switch (val) {
+ case dynFreqMode_DISABLED:
+ fxdTiCtrl.cfg_dynFreqMode = val;
+ SERIAL_ECHOLNPGM("Dynamic frequency mode disabled.");
+ break;
+ #if HAS_Z_AXIS
+ case dynFreqMode_Z_BASED:
+ fxdTiCtrl.cfg_dynFreqMode = val;
+ SERIAL_ECHOLNPGM("Z-based Dynamic Frequency Mode.");
+ break;
+ #endif
+ #if HAS_EXTRUDERS
+ case dynFreqMode_MASS_BASED:
+ fxdTiCtrl.cfg_dynFreqMode = val;
+ SERIAL_ECHOLNPGM("Mass-based Dynamic Frequency Mode.");
+ break;
+ #endif
+ default:
+ SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value.");
+ break;
+ }
+ }
+ else {
+ SERIAL_ECHOLNPGM("Incompatible shaper for [D] Dynamic Frequency mode.");
+ }
+ }
+
+ #endif // HAS_Z_AXIS || HAS_EXTRUDERS
+
+ #if HAS_X_AXIS
+
+ // Parse frequency parameter (X axis).
+ if (parser.seenval('A')) {
+ if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
+ const float val = parser.value_float();
+ const bool frequencyInRange = WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2);
+ // TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct.
+ if (frequencyInRange) {
+ fxdTiCtrl.cfg_baseFreq[0] = val;
+ fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
+ fxdTiCtrl.reset();
+ if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (X/A axis) set to:"); }
+ else { SERIAL_ECHOPGM("Compensator static frequency (X/A axis) set to: "); }
+ SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[0], 2 );
+ SERIAL_ECHOLNPGM(".");
+ }
+ else { // Frequency out of range.
+ SERIAL_ECHOLNPGM("Invalid [A] frequency value.");
+ }
+ }
+ else { // Mode doesn't use frequency.
+ SERIAL_ECHOLNPGM("Incompatible mode for [A] frequency.");
+ }
+ }
+
+ #if HAS_Z_AXIS || HAS_EXTRUDERS
+ // Parse frequency scaling parameter (X axis).
+ if (parser.seenval('F')) {
+ const bool modeUsesDynFreq = (
+ TERN0(HAS_Z_AXIS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_Z_BASED)
+ || TERN0(HAS_EXTRUDERS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_MASS_BASED)
+ );
+
+ if (modeUsesDynFreq) {
+ const float val = parser.value_float();
+ fxdTiCtrl.cfg_dynFreqK[0] = val;
+ SERIAL_ECHOPGM("Frequency scaling (X/A axis) set to: ");
+ SERIAL_ECHO_F(fxdTiCtrl.cfg_dynFreqK[0], 8);
+ SERIAL_ECHOLNPGM(".");
+ }
+ else {
+ SERIAL_ECHOLNPGM("Incompatible mode for [F] frequency scaling.");
+ }
+ }
+ #endif // HAS_Z_AXIS || HAS_EXTRUDERS
+
+ #endif // HAS_X_AXIS
+
+ #if HAS_Y_AXIS
+
+ // Parse frequency parameter (Y axis).
+ if (parser.seenval('B')) {
+ if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
+ const float val = parser.value_float();
+ const bool frequencyInRange = WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2);
+ if (frequencyInRange) {
+ fxdTiCtrl.cfg_baseFreq[1] = val;
+ fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
+ fxdTiCtrl.reset();
+ if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (Y/B axis) set to:"); }
+ else { SERIAL_ECHOPGM("Compensator static frequency (Y/B axis) set to: "); }
+ SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[1], 2 );
+ SERIAL_ECHOLNPGM(".");
+ }
+ else { // Frequency out of range.
+ SERIAL_ECHOLNPGM("Invalid frequency [B] value.");
+ }
+ }
+ else { // Mode doesn't use frequency.
+ SERIAL_ECHOLNPGM("Incompatible mode for [B] frequency.");
+ }
+ }
+
+ #if HAS_Z_AXIS || HAS_EXTRUDERS
+ // Parse frequency scaling parameter (Y axis).
+ if (parser.seenval('H')) {
+ const bool modeUsesDynFreq = (
+ TERN0(HAS_Z_AXIS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_Z_BASED)
+ || TERN0(HAS_EXTRUDERS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_MASS_BASED)
+ );
+
+ if (modeUsesDynFreq) {
+ const float val = parser.value_float();
+ fxdTiCtrl.cfg_dynFreqK[1] = val;
+ SERIAL_ECHOPGM("Frequency scaling (Y/B axis) set to: ");
+ SERIAL_ECHO_F(val, 8);
+ SERIAL_ECHOLNPGM(".");
+ }
+ else {
+ SERIAL_ECHOLNPGM("Incompatible mode for [H] frequency scaling.");
+ }
+ }
+ #endif // HAS_Z_AXIS || HAS_EXTRUDERS
+
+ #endif // HAS_Y_AXIS
+}
+
+#endif // FT_MOTION
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
index 1752ae8a1842..0711d39204f2 100644
--- a/Marlin/src/gcode/gcode.cpp
+++ b/Marlin/src/gcode/gcode.cpp
@@ -895,6 +895,10 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
case 486: M486(); break; // M486: Identify and cancel objects
#endif
+ #if ENABLED(FT_MOTION)
+ case 493: M493(); break; // M493: Fixed-Time Motion control
+ #endif
+
case 500: M500(); break; // M500: Store settings in EEPROM
case 501: M501(); break; // M501: Read settings from EEPROM
case 502: M502(); break; // M502: Revert to default settings
@@ -934,7 +938,7 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
#endif
#if HAS_ZV_SHAPING
- case 593: M593(); break; // M593: Set Input Shaping parameters
+ case 593: M593(); break; // M593: Input Shaping control
#endif
#if ENABLED(ADVANCED_PAUSE_FEATURE)
diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
index cc3f221a82ca..8493d7f2911d 100644
--- a/Marlin/src/gcode/gcode.h
+++ b/Marlin/src/gcode/gcode.h
@@ -1038,6 +1038,10 @@ class GcodeSuite {
static void M486();
#endif
+ #if ENABLED(FT_MOTION)
+ static void M493();
+ #endif
+
static void M500();
static void M501();
static void M502();
diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp
new file mode 100644
index 000000000000..dfef961c7968
--- /dev/null
+++ b/Marlin/src/module/ft_motion.cpp
@@ -0,0 +1,924 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(FT_MOTION)
+
+#include "ft_motion.h"
+#include "stepper.h" // Access stepper block queue function and abort status.
+
+FxdTiCtrl fxdTiCtrl;
+
+//-----------------------------------------------------------------//
+// Variables.
+//-----------------------------------------------------------------//
+
+// Public variables.
+ftMotionMode_t FxdTiCtrl::cfg_mode = FTM_DEFAULT_MODE; // Mode / active compensation mode configuration.
+
+#if HAS_EXTRUDERS
+ bool FxdTiCtrl::cfg_linearAdvEna = FTM_LINEAR_ADV_DEFAULT_ENA; // Linear advance enable configuration.
+ float FxdTiCtrl::cfg_linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; // Linear advance gain.
+#endif
+
+dynFreqMode_t FxdTiCtrl::cfg_dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration.
+#if !HAS_Z_AXIS
+ static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis.");
+#endif
+#if !(HAS_X_AXIS && HAS_EXTRUDERS)
+ static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_MASS_BASED, "dynFreqMode_MASS_BASED requires an X axis and an extruder.");
+#endif
+
+#if HAS_X_AXIS
+ float FxdTiCtrl::cfg_baseFreq[] = { FTM_SHAPING_DEFAULT_X_FREQ // Base frequency. [Hz]
+ OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) };
+ float FxdTiCtrl::cfg_dynFreqK[] = { 0.0f OPTARG(HAS_Y_AXIS, 0.0f) }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
+#endif
+
+ft_command_t FxdTiCtrl::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of stepper commands.
+hal_timer_t FxdTiCtrl::stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of the stepper command timing.
+uint8_t FxdTiCtrl::stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE] = {0U}; // Buffer of whether DIR needs to be updated.
+uint32_t FxdTiCtrl::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer.
+ FxdTiCtrl::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
+
+bool FxdTiCtrl::sts_stepperBusy = false; // The stepper buffer has items and is in use.
+
+// Private variables.
+// NOTE: These are sized for Ulendo FBS use.
+#if HAS_X_AXIS
+ float FxdTiCtrl::xd[2 * (FTM_BATCH_SIZE)], // = {0.0f} Storage for fixed-time-based trajectory.
+ FxdTiCtrl::xm[FTM_BATCH_SIZE]; // = {0.0f} Storage for modified fixed-time-based trajectory.
+#endif
+#if HAS_Y_AXIS
+ float FxdTiCtrl::yd[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::ym[FTM_BATCH_SIZE];
+#endif
+#if HAS_Z_AXIS
+ float FxdTiCtrl::zd[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::zm[FTM_BATCH_SIZE];
+#endif
+#if HAS_EXTRUDERS
+ float FxdTiCtrl::ed[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::em[FTM_BATCH_SIZE];
+#endif
+
+block_t* FxdTiCtrl::current_block_cpy = nullptr; // Pointer to current block being processed.
+bool FxdTiCtrl::blockProcRdy = false, // Indicates a block is ready to be processed.
+ FxdTiCtrl::blockProcRdy_z1 = false, // Storage for the previous indicator.
+ FxdTiCtrl::blockProcDn = false; // Indicates current block is done being processed.
+bool FxdTiCtrl::batchRdy = false; // Indicates a batch of the fixed time trajectory
+ // has been generated, is now available in the upper -
+ // half of xd, yd, zd, ed vectors, and is ready to be
+ // post processed, if applicable, then interpolated.
+bool FxdTiCtrl::batchRdyForInterp = false; // Indicates the batch is done being post processed,
+ // if applicable, and is ready to be converted to step commands.
+bool FxdTiCtrl::runoutEna = false; // True if runout of the block hasn't been done and is allowed.
+
+// Trapezoid data variables.
+#if HAS_X_AXIS
+ float FxdTiCtrl::x_startPosn, // (mm) Start position of block
+ FxdTiCtrl::x_endPosn_prevBlock = 0.0f, // (mm) Start position of block
+ FxdTiCtrl::x_Ratio; // (ratio) Axis move ratio of block
+#endif
+#if HAS_Y_AXIS
+ float FxdTiCtrl::y_startPosn,
+ FxdTiCtrl::y_endPosn_prevBlock = 0.0f,
+ FxdTiCtrl::y_Ratio;
+#endif
+#if HAS_Z_AXIS
+ float FxdTiCtrl::z_startPosn,
+ FxdTiCtrl::z_endPosn_prevBlock = 0.0f,
+ FxdTiCtrl::z_Ratio;
+#endif
+#if HAS_EXTRUDERS
+ float FxdTiCtrl::e_startPosn,
+ FxdTiCtrl::e_endPosn_prevBlock = 0.0f,
+ FxdTiCtrl::e_Ratio;
+#endif
+float FxdTiCtrl::accel_P, // Acceleration prime of block. [mm/sec/sec]
+ FxdTiCtrl::decel_P, // Deceleration prime of block. [mm/sec/sec]
+ FxdTiCtrl::F_P, // Feedrate prime of block. [mm/sec]
+ FxdTiCtrl::f_s, // Starting feedrate of block. [mm/sec]
+ FxdTiCtrl::s_1e, // Position after acceleration phase of block.
+ FxdTiCtrl::s_2e; // Position after acceleration and coasting phase of block.
+
+uint32_t FxdTiCtrl::N1, // Number of data points in the acceleration phase.
+ FxdTiCtrl::N2, // Number of data points in the coasting phase.
+ FxdTiCtrl::N3; // Number of data points in the deceleration phase.
+
+uint32_t FxdTiCtrl::max_intervals; // Total number of data points that will be generated from block.
+
+// Make vector variables.
+uint32_t FxdTiCtrl::makeVector_idx = 0, // Index of fixed time trajectory generation of the overall block.
+ FxdTiCtrl::makeVector_idx_z1 = 0, // Storage for the previously calculated index above.
+ FxdTiCtrl::makeVector_batchIdx = FTM_BATCH_SIZE; // Index of fixed time trajectory generation within the batch.
+
+// Interpolation variables.
+#if HAS_X_AXIS
+ int32_t FxdTiCtrl::x_steps = 0; // Step count accumulator.
+ stepDirState_t FxdTiCtrl::x_dirState = stepDirState_NOT_SET; // Memory of the currently set step direction of the axis.
+#endif
+#if HAS_Y_AXIS
+ int32_t FxdTiCtrl::y_steps = 0;
+ stepDirState_t FxdTiCtrl::y_dirState = stepDirState_NOT_SET;
+#endif
+#if HAS_Z_AXIS
+ int32_t FxdTiCtrl::z_steps = 0;
+ stepDirState_t FxdTiCtrl::z_dirState = stepDirState_NOT_SET;
+#endif
+#if HAS_EXTRUDERS
+ int32_t FxdTiCtrl::e_steps = 0;
+ stepDirState_t FxdTiCtrl::e_dirState = stepDirState_NOT_SET;
+#endif
+
+uint32_t FxdTiCtrl::interpIdx = 0, // Index of current data point being interpolated.
+ FxdTiCtrl::interpIdx_z1 = 0; // Storage for the previously calculated index above.
+hal_timer_t FxdTiCtrl::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the next step time (in ticks).
+
+// Shaping variables.
+#if HAS_X_AXIS
+ uint32_t FxdTiCtrl::xy_zi_idx = 0, // Index of storage in the data point delay vectors.
+ FxdTiCtrl::xy_max_i = 0; // Vector length for the selected shaper.
+ float FxdTiCtrl::xd_zi[FTM_ZMAX] = { 0.0f }; // Data point delay vector.
+ float FxdTiCtrl::x_Ai[5]; // Shaping gain vector.
+ uint32_t FxdTiCtrl::x_Ni[5]; // Shaping time index vector.
+#endif
+#if HAS_Y_AXIS
+ float FxdTiCtrl::yd_zi[FTM_ZMAX] = { 0.0f };
+ float FxdTiCtrl::y_Ai[5];
+ uint32_t FxdTiCtrl::y_Ni[5];
+#endif
+
+#if HAS_EXTRUDERS
+ // Linear advance variables.
+ float FxdTiCtrl::e_raw_z1 = 0.0f; // (ms) Unit delay of raw extruder position.
+ float FxdTiCtrl::e_advanced_z1 = 0.0f; // (ms) Unit delay of advanced extruder position.
+#endif
+
+//-----------------------------------------------------------------//
+// Function definitions.
+//-----------------------------------------------------------------//
+
+// Public functions.
+
+// Sets controller states to begin processing a block.
+void FxdTiCtrl::startBlockProc(block_t * const current_block) {
+ current_block_cpy = current_block;
+ blockProcRdy = true;
+ blockProcDn = false;
+ runoutEna = true;
+}
+
+// Moves any free data points to the stepper buffer even if a full batch isn't ready.
+void FxdTiCtrl::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 > FTM_BATCH_SIZE) {
+ for (uint32_t i = makeVector_batchIdx; i < 2 * (FTM_BATCH_SIZE); i++) {
+ xd[i] = xd[makeVector_batchIdx - 1];
+ TERN_(HAS_Y_AXIS, yd[i] = yd[makeVector_batchIdx - 1]);
+ TERN_(HAS_Y_AXIS, zd[i] = zd[makeVector_batchIdx - 1]);
+ TERN_(HAS_EXTRUDERS, ed[i] = ed[makeVector_batchIdx - 1]);
+ }
+ }
+ makeVector_batchIdx = FTM_BATCH_SIZE;
+ batchRdy = true;
+ }
+ runoutEna = false;
+}
+
+// Controller main, to be invoked from non-isr task.
+void FxdTiCtrl::loop() {
+
+ if (!cfg_mode) return;
+
+ static bool initd = false;
+ if (!initd) { init(); initd = true; }
+
+ // Handle block abort with the following sequence:
+ // 1. Zero out commands in stepper ISR.
+ // 2. Drain the motion buffer, stop processing until they are emptied.
+ // 3. Reset all the states / memory.
+ // 4. Signal ready for new block.
+ if (stepper.abort_current_block) {
+ 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) stepper.fxdTiCtrl_BlockQueueUpdate();
+
+ if (blockProcRdy) {
+ if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot.
+ while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP)))
+ makeVector();
+ }
+
+ // FBS / post processing.
+ if (batchRdy && !batchRdyForInterp) {
+
+ // Call Ulendo FBS here.
+
+ memcpy(xm, &xd[FTM_BATCH_SIZE], sizeof(xm));
+ TERN_(HAS_Y_AXIS, memcpy(ym, &yd[FTM_BATCH_SIZE], sizeof(ym)));
+
+ // Done compensating ...
+
+ // Copy the uncompensated vectors.
+ TERN_(HAS_Z_AXIS, memcpy(zm, &zd[FTM_BATCH_SIZE], sizeof(zm)));
+ TERN_(HAS_EXTRUDERS, memcpy(em, &ed[FTM_BATCH_SIZE], sizeof(em)));
+
+ // Shift the time series back in the window.
+ memcpy(xd, &xd[FTM_BATCH_SIZE], sizeof(xd) / 2);
+ TERN_(HAS_Y_AXIS, memcpy(yd, &yd[FTM_BATCH_SIZE], sizeof(yd) / 2));
+ // Disabled by comment as these are uncompensated, the lower half is not used.
+ //TERN_(HAS_Z_AXIS, memcpy(zd, &zd[FTM_BATCH_SIZE], (sizeof(zd) / 2)));
+ //TERN_(HAS_EXTRUDERS, memcpy(ed, &ed[FTM_BATCH_SIZE], (sizeof(ed) / 2)));
+
+ // ... data is ready in xm, ym, zm, em.
+ batchRdyForInterp = true;
+
+ batchRdy = false; // Clear so that makeVector() may resume generating points.
+
+ } // if (batchRdy && !batchRdyForInterp)
+
+ // Interpolation.
+ while ( batchRdyForInterp
+ && ( stepperCmdBuffItems() < ((FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME)) )
+ && ( (interpIdx - interpIdx_z1) < (FTM_STEPS_PER_LOOP) )
+ ) {
+ convertToSteps(interpIdx);
+
+ if (++interpIdx == FTM_BATCH_SIZE) {
+ batchRdyForInterp = false;
+ interpIdx = 0;
+ }
+ }
+
+ // Report busy status to planner.
+ planner.fxdTiCtrl_busy = (sts_stepperBusy || ((!blockProcDn && blockProcRdy) || batchRdy || batchRdyForInterp || runoutEna));
+
+ blockProcRdy_z1 = blockProcRdy;
+ makeVector_idx_z1 = makeVector_idx;
+ interpIdx_z1 = interpIdx;
+}
+
+#if HAS_X_AXIS
+
+ // Refresh the gains used by shaping functions.
+ // To be called on init or mode or zeta change.
+ void FxdTiCtrl::updateShapingA(const_float_t zeta/*=FTM_SHAPING_ZETA*/, const_float_t vtol/*=FTM_SHAPING_V_TOL*/) {
+
+ const float K = exp( -zeta * PI / sqrt(1.0f - sq(zeta)) ),
+ K2 = sq(K);
+
+ switch (cfg_mode) {
+
+ case ftMotionMode_ZV:
+ xy_max_i = 1U;
+ x_Ai[0] = 1.0f / (1.0f + K);
+ x_Ai[1] = x_Ai[0] * K;
+ break;
+
+ case ftMotionMode_ZVD:
+ xy_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;
+ break;
+
+ case ftMotionMode_EI: {
+ xy_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;
+
+ case ftMotionMode_2HEI: {
+ xy_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;
+
+ case ftMotionMode_3HEI: {
+ xy_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;
+
+ case ftMotionMode_MZV: {
+ xy_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;
+
+ default:
+ for (uint32_t i = 0U; i < 5U; i++) x_Ai[i] = 0.0f;
+ xy_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
+ }
+
+ // Refresh the indices used by shaping functions.
+ // To be called when frequencies change.
+ void FxdTiCtrl::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), const_float_t zeta/*=FTM_SHAPING_ZETA*/) {
+
+ // Protections omitted for DBZ and for index exceeding array length.
+
+ const float df = sqrt(1.0f - sq(zeta));
+
+ switch (cfg_mode) {
+ case ftMotionMode_ZV:
+ x_Ni[1] = round((0.5f / xf / df) * (FTM_FS));
+ #if HAS_Y_AXIS
+ y_Ni[1] = round((0.5f / yf / df) * (FTM_FS));
+ #endif
+ break;
+ case ftMotionMode_ZVD:
+ case ftMotionMode_EI:
+ x_Ni[1] = round((0.5f / xf / df) * (FTM_FS));
+ x_Ni[2] = 2 * x_Ni[1];
+ #if HAS_Y_AXIS
+ y_Ni[1] = round((0.5f / yf / df) * (FTM_FS));
+ y_Ni[2] = 2 * y_Ni[1];
+ #endif
+ break;
+ case ftMotionMode_2HEI:
+ x_Ni[1] = round((0.5f / xf / df) * (FTM_FS));
+ x_Ni[2] = 2 * x_Ni[1];
+ x_Ni[3] = 3 * x_Ni[1];
+ #if HAS_Y_AXIS
+ y_Ni[1] = round((0.5f / yf / df) * (FTM_FS));
+ y_Ni[2] = 2 * y_Ni[1];
+ y_Ni[3] = 3 * y_Ni[1];
+ #endif
+ break;
+ case ftMotionMode_3HEI:
+ x_Ni[1] = round((0.5f / xf / df) * (FTM_FS));
+ x_Ni[2] = 2 * x_Ni[1];
+ x_Ni[3] = 3 * x_Ni[1];
+ x_Ni[4] = 4 * x_Ni[1];
+ #if HAS_Y_AXIS
+ y_Ni[1] = round((0.5f / yf / df) * (FTM_FS));
+ y_Ni[2] = 2 * y_Ni[1];
+ y_Ni[3] = 3 * y_Ni[1];
+ y_Ni[4] = 4 * y_Ni[1];
+ #endif
+ break;
+ case ftMotionMode_MZV:
+ x_Ni[1] = round((0.375f / xf / df) * (FTM_FS));
+ x_Ni[2] = 2 * x_Ni[1];
+ #if HAS_Y_AXIS
+ y_Ni[1] = round((0.375f / yf / df) * (FTM_FS));
+ y_Ni[2] = 2 * y_Ni[1];
+ #endif
+ break;
+ default:
+ for (uint32_t i = 0U; i < 5U; i++) { x_Ni[i] = 0; TERN_(HAS_Y_AXIS, y_Ni[i] = 0); }
+ }
+ }
+
+#endif // HAS_X_AXIS
+
+// Reset all trajectory processing variables.
+void FxdTiCtrl::reset() {
+
+ stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;
+
+ for (uint32_t i = 0U; i < (FTM_BATCH_SIZE); i++) { // Reset trajectory history
+ TERN_(HAS_X_AXIS, xd[i] = 0.0f);
+ TERN_(HAS_Y_AXIS, yd[i] = 0.0f);
+ TERN_(HAS_Z_AXIS, zd[i] = 0.0f);
+ TERN_(HAS_EXTRUDERS, ed[i] = 0.0f);
+ }
+
+ blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
+ batchRdy = batchRdyForInterp = false;
+ runoutEna = false;
+
+ TERN_(HAS_X_AXIS, x_endPosn_prevBlock = 0.0f);
+ TERN_(HAS_Y_AXIS, y_endPosn_prevBlock = 0.0f);
+ TERN_(HAS_Z_AXIS, z_endPosn_prevBlock = 0.0f);
+ TERN_(HAS_EXTRUDERS, e_endPosn_prevBlock = 0.0f);
+
+ makeVector_idx = makeVector_idx_z1 = 0;
+ makeVector_batchIdx = FTM_BATCH_SIZE;
+
+ TERN_(HAS_X_AXIS, x_steps = 0);
+ TERN_(HAS_Y_AXIS, y_steps = 0);
+ TERN_(HAS_Z_AXIS, z_steps = 0);
+ TERN_(HAS_EXTRUDERS, e_steps = 0);
+ interpIdx = interpIdx_z1 = 0;
+ TERN_(HAS_X_AXIS, x_dirState = stepDirState_NOT_SET);
+ TERN_(HAS_Y_AXIS, y_dirState = stepDirState_NOT_SET);
+ TERN_(HAS_Z_AXIS, z_dirState = stepDirState_NOT_SET);
+ TERN_(HAS_EXTRUDERS, e_dirState = stepDirState_NOT_SET);
+ nextStepTicks = FTM_MIN_TICKS;
+
+ #if HAS_X_AXIS
+ for (uint32_t i = 0U; i < (FTM_ZMAX); i++) { xd_zi[i] = 0.0f; TERN_(HAS_Y_AXIS, yd_zi[i] = 0.0f); }
+ xy_zi_idx = 0;
+ #endif
+
+ TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f);
+}
+
+// Private functions.
+// Auxiliary function to get number of step commands in the buffer.
+uint32_t FxdTiCtrl::stepperCmdBuffItems() {
+ const uint32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx;
+ return stepperCmdBuff_produceIdx < stepperCmdBuff_consumeIdx ? (FTM_STEPPERCMD_BUFF_SIZE) + udiff : udiff;
+}
+
+// Initializes storage variables before startup.
+void FxdTiCtrl::init() {
+ #if HAS_X_AXIS
+ updateShapingN(cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, cfg_baseFreq[1]));
+ updateShapingA(FTM_SHAPING_ZETA, FTM_SHAPING_V_TOL);
+ #endif
+ reset(); // Precautionary.
+}
+
+// Loads / converts block data from planner to fixed-time control variables.
+void FxdTiCtrl::loadBlockData(block_t * const current_block) {
+
+ const float totalLength = current_block->millimeters,
+ oneOverLength = 1.0f / totalLength;
+
+ const axis_bits_t direction = current_block->direction_bits;
+
+ #if HAS_X_AXIS
+ x_startPosn = x_endPosn_prevBlock;
+ float x_moveDist = current_block->steps.a / planner.settings.axis_steps_per_mm[X_AXIS];
+ if (TEST(direction, X_AXIS)) x_moveDist *= -1.0f;
+ x_Ratio = x_moveDist * oneOverLength;
+ #endif
+
+ #if HAS_Y_AXIS
+ y_startPosn = y_endPosn_prevBlock;
+ float y_moveDist = current_block->steps.b / planner.settings.axis_steps_per_mm[Y_AXIS];
+ if (TEST(direction, Y_AXIS)) y_moveDist *= -1.0f;
+ y_Ratio = y_moveDist * oneOverLength;
+ #endif
+
+ #if HAS_Z_AXIS
+ z_startPosn = z_endPosn_prevBlock;
+ float z_moveDist = current_block->steps.c / planner.settings.axis_steps_per_mm[Z_AXIS];
+ if (TEST(direction, Z_AXIS)) z_moveDist *= -1.0f;
+ z_Ratio = z_moveDist * oneOverLength;
+ #endif
+
+ #if HAS_EXTRUDERS
+ e_startPosn = e_endPosn_prevBlock;
+ float extrusion = current_block->steps.e / planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)];
+ if (TEST(direction, E_AXIS_N(current_block->extruder))) extrusion *= -1.0f;
+ e_Ratio = extrusion * oneOverLength;
+ #endif
+
+ 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
+
+ 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
+ oneby2d = -oneby2a; // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2
+ const float fsSqByTwoA = sq(f_s) * oneby2a, // (mm) Distance to accelerate from start speed to nominal speed
+ feSqByTwoD = sq(f_e) * oneby2d; // (mm) Distance to decelerate from nominal speed to end speed
+
+ float F_n = current_block->nominal_speed; // (mm/s) Speed we hope to achieve, if possible
+ const float fdiff = feSqByTwoD - fsSqByTwoA, // (mm) Coasting distance if nominal speed is reached
+ odiff = oneby2a - oneby2d, // (i.e., oneby2a * 2) (mm/s) Change in speed for one second of acceleration
+ ldiff = totalLength - fdiff; // (mm) Distance to travel if nominal speed is reached
+ float T2 = (1.0f / F_n) * (ldiff - odiff * sq(F_n)); // (s) Coasting duration after nominal speed reached
+ if (T2 < 0.0f) {
+ T2 = 0.0f;
+ F_n = SQRT(ldiff / odiff); // Clip by intersection if nominal speed can't be reached.
+ }
+
+ 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
+
+ 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
+ //
+ 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
+ accel_P = N1 ? ((F_P - f_s) / T1_P) : 0.0f;
+
+ decel_P = (f_e - F_P) / T3_P;
+
+ // Calculate the distance traveled during the accel phase
+ s_1e = f_s * T1_P + 0.5f * accel_P * sq(T1_P);
+
+ // 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;
+
+ TERN_(HAS_X_AXIS, x_endPosn_prevBlock += x_moveDist);
+ TERN_(HAS_Y_AXIS, y_endPosn_prevBlock += y_moveDist);
+ TERN_(HAS_Z_AXIS, z_endPosn_prevBlock += z_moveDist);
+ TERN_(HAS_EXTRUDERS, e_endPosn_prevBlock += extrusion);
+}
+
+// Generate data points of the trajectory.
+void FxdTiCtrl::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
+
+ 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
+ }
+ else if (makeVector_idx >= N1 && makeVector_idx < (N1 + N2)) {
+ // Coasting phase
+ dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase
+ //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
+ }
+
+ TERN_(HAS_X_AXIS, xd[makeVector_batchIdx] = x_startPosn + x_Ratio * dist); // (mm) X position for this datapoint
+ TERN_(HAS_Y_AXIS, yd[makeVector_batchIdx] = y_startPosn + y_Ratio * dist); // (mm) Y
+ TERN_(HAS_Z_AXIS, zd[makeVector_batchIdx] = z_startPosn + z_Ratio * dist); // (mm) Z
+
+ #if HAS_EXTRUDERS
+ const float new_raw_z1 = e_startPosn + e_Ratio * dist;
+ if (cfg_linearAdvEna) {
+ float dedt_adj = (new_raw_z1 - e_raw_z1) * (FTM_FS);
+ if (e_Ratio > 0.0f) dedt_adj += accel_k * cfg_linearAdvK;
+
+ e_advanced_z1 += dedt_adj * (FTM_TS);
+ ed[makeVector_batchIdx] = e_advanced_z1;
+
+ e_raw_z1 = new_raw_z1;
+ }
+ else {
+ ed[makeVector_batchIdx] = new_raw_z1;
+ // Alternatively: coordArray_e[makeVector_batchIdx] = e_startDist + extrusion / (N1 + N2 + N3);
+ }
+ #endif
+
+ // Update shaping parameters if needed.
+ #if HAS_Z_AXIS
+ static float zd_z1 = 0.0f;
+ #endif
+ switch (cfg_dynFreqMode) {
+
+ #if HAS_Z_AXIS
+ case dynFreqMode_Z_BASED:
+ if (zd[makeVector_batchIdx] != zd_z1) { // Only update if Z changed.
+ const float xf = cfg_baseFreq[0] + cfg_dynFreqK[0] * zd[makeVector_batchIdx],
+ yf = cfg_baseFreq[1] + cfg_dynFreqK[1] * zd[makeVector_batchIdx];
+ updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ), _MAX(yf, FTM_MIN_SHAPE_FREQ));
+ zd_z1 = zd[makeVector_batchIdx];
+ }
+ break;
+ #endif
+
+ #if HAS_X_AXIS && HAS_EXTRUDERS
+ 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[0] + cfg_dynFreqK[0] * ed[makeVector_batchIdx]
+ OPTARG(HAS_Y_AXIS, cfg_baseFreq[1] + cfg_dynFreqK[1] * ed[makeVector_batchIdx]) );
+ break;
+ #endif
+
+ default: break;
+ }
+
+ // Apply shaping if in mode.
+ #if HAS_X_AXIS
+ if (WITHIN(cfg_mode, 10U, 19U)) {
+ xd_zi[xy_zi_idx] = xd[makeVector_batchIdx];
+ xd[makeVector_batchIdx] *= x_Ai[0];
+ #if HAS_Y_AXIS
+ yd_zi[xy_zi_idx] = yd[makeVector_batchIdx];
+ yd[makeVector_batchIdx] *= y_Ai[0];
+ #endif
+ for (uint32_t i = 1U; i <= xy_max_i; i++) {
+ const uint32_t udiffx = xy_zi_idx - x_Ni[i];
+ xd[makeVector_batchIdx] += x_Ai[i] * xd_zi[x_Ni[i] > xy_zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
+ #if HAS_Y_AXIS
+ const uint32_t udiffy = xy_zi_idx - y_Ni[i];
+ yd[makeVector_batchIdx] += y_Ai[i] * yd_zi[y_Ni[i] > xy_zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
+ #endif
+ }
+ if (++xy_zi_idx == (FTM_ZMAX)) xy_zi_idx = 0;
+ }
+ #endif
+
+ // Filled up the queue with regular and shaped steps
+ if (++makeVector_batchIdx == 2 * (FTM_BATCH_SIZE)) {
+ makeVector_batchIdx = FTM_BATCH_SIZE;
+ batchRdy = true;
+ }
+
+ if (makeVector_idx == max_intervals) {
+ blockProcDn = true;
+ blockProcRdy = false;
+ makeVector_idx = 0;
+ }
+ else
+ makeVector_idx++;
+}
+
+// Interpolates single data point to stepper commands.
+void FxdTiCtrl::convertToSteps(const uint32_t idx) {
+ #if HAS_X_AXIS
+ int32_t x_err_P = 0;
+ #endif
+ #if HAS_Y_AXIS
+ int32_t y_err_P = 0;
+ #endif
+ #if HAS_Z_AXIS
+ int32_t z_err_P = 0;
+ #endif
+ #if HAS_EXTRUDERS
+ int32_t e_err_P = 0;
+ #endif
+
+ //#define STEPS_ROUNDING
+ #if ENABLED(STEPS_ROUNDING)
+ #if HAS_X_AXIS
+ const float x_steps_tar = xm[idx] * planner.settings.axis_steps_per_mm[X_AXIS] + (xm[idx] < 0.0f ? -0.5f : 0.5f); // May be eliminated if guaranteed positive.
+ const int32_t x_delta = int32_t(x_steps_tar) - x_steps;
+ #endif
+ #if HAS_Y_AXIS
+ const float y_steps_tar = ym[idx] * planner.settings.axis_steps_per_mm[Y_AXIS] + (ym[idx] < 0.0f ? -0.5f : 0.5f);
+ const int32_t y_delta = int32_t(y_steps_tar) - y_steps;
+ #endif
+ #if HAS_Z_AXIS
+ const float z_steps_tar = zm[idx] * planner.settings.axis_steps_per_mm[Z_AXIS] + (zm[idx] < 0.0f ? -0.5f : 0.5f);
+ const int32_t z_delta = int32_t(z_steps_tar) - z_steps;
+ #endif
+ #if HAS_EXTRUDERS
+ const float e_steps_tar = em[idx] * planner.settings.axis_steps_per_mm[E_AXIS] + (em[idx] < 0.0f ? -0.5f : 0.5f);
+ const int32_t e_delta = int32_t(e_steps_tar) - e_steps;
+ #endif
+ #else
+ #if HAS_X_AXIS
+ const int32_t x_delta = int32_t(xm[idx] * planner.settings.axis_steps_per_mm[X_AXIS]) - x_steps;
+ #endif
+ #if HAS_Y_AXIS
+ const int32_t y_delta = int32_t(ym[idx] * planner.settings.axis_steps_per_mm[Y_AXIS]) - y_steps;
+ #endif
+ #if HAS_Z_AXIS
+ const int32_t z_delta = int32_t(zm[idx] * planner.settings.axis_steps_per_mm[Z_AXIS]) - z_steps;
+ #endif
+ #if HAS_EXTRUDERS
+ const int32_t e_delta = int32_t(em[idx] * planner.settings.axis_steps_per_mm[E_AXIS]) - e_steps;
+ #endif
+ #endif
+
+ bool any_dirChange = (false
+ || TERN0(HAS_X_AXIS, (x_delta > 0 && x_dirState != stepDirState_POS) || (x_delta < 0 && x_dirState != stepDirState_NEG))
+ || TERN0(HAS_Y_AXIS, (y_delta > 0 && y_dirState != stepDirState_POS) || (y_delta < 0 && y_dirState != stepDirState_NEG))
+ || TERN0(HAS_Z_AXIS, (z_delta > 0 && z_dirState != stepDirState_POS) || (z_delta < 0 && z_dirState != stepDirState_NEG))
+ || TERN0(HAS_EXTRUDERS, (e_delta > 0 && e_dirState != stepDirState_POS) || (e_delta < 0 && e_dirState != stepDirState_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;
+
+ stepperCmdBuff[stepperCmdBuff_produceIdx] = 0;
+
+ // Commands are written in the format:
+ // |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction|
+ #if HAS_X_AXIS
+ if (x_delta >= 0) {
+ if ((x_err_P + x_delta) < (FTM_CTS_COMPARE_VAL)) {
+ x_err_P += x_delta;
+ }
+ else {
+ x_steps++;
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_X) | _BV(FT_BIT_STEP_X);
+ x_err_P += x_delta - (FTM_STEPS_PER_UNIT_TIME);
+ anyStep = true;
+ }
+ }
+ else {
+ if ((x_err_P + x_delta) > -(FTM_CTS_COMPARE_VAL)) {
+ x_err_P += x_delta;
+ }
+ else {
+ x_steps--;
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_X);
+ x_err_P += x_delta + (FTM_STEPS_PER_UNIT_TIME);
+ anyStep = true;
+ }
+ }
+ #endif // HAS_X_AXIS
+
+ #if HAS_Y_AXIS
+ if (y_delta >= 0) {
+ if ((y_err_P + y_delta) < (FTM_CTS_COMPARE_VAL)) {
+ y_err_P += y_delta;
+ }
+ else {
+ y_steps++;
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Y) | _BV(FT_BIT_STEP_Y);
+ y_err_P += y_delta - (FTM_STEPS_PER_UNIT_TIME);
+ anyStep = true;
+ }
+ }
+ else {
+ if ((y_err_P + y_delta) > -(FTM_CTS_COMPARE_VAL)) {
+ y_err_P += y_delta;
+ }
+ else {
+ y_steps--;
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_Y);
+ y_err_P += y_delta + (FTM_STEPS_PER_UNIT_TIME);
+ anyStep = true;
+ }
+ }
+ #endif // HAS_Y_AXIS
+
+ #if HAS_Z_AXIS
+ if (z_delta >= 0) {
+ if ((z_err_P + z_delta) < (FTM_CTS_COMPARE_VAL)) {
+ z_err_P += z_delta;
+ }
+ else {
+ z_steps++;
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Z) | _BV(FT_BIT_STEP_Z);
+ z_err_P += z_delta - (FTM_STEPS_PER_UNIT_TIME);
+ anyStep = true;
+ }
+ }
+ else {
+ if ((z_err_P + z_delta) > -(FTM_CTS_COMPARE_VAL)) {
+ z_err_P += z_delta;
+ }
+ else {
+ z_steps--;
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_Z);
+ z_err_P += z_delta + (FTM_STEPS_PER_UNIT_TIME);
+ anyStep = true;
+ }
+ }
+ #endif // HAS_Z_AXIS
+
+ #if HAS_EXTRUDERS
+ if (e_delta >= 0) {
+ if ((e_err_P + e_delta) < (FTM_CTS_COMPARE_VAL)) {
+ e_err_P += e_delta;
+ }
+ else {
+ e_steps++;
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_E) | _BV(FT_BIT_STEP_E);
+ e_err_P += e_delta - (FTM_STEPS_PER_UNIT_TIME);
+ anyStep = true;
+ }
+ }
+ else {
+ if ((e_err_P + e_delta) > -(FTM_CTS_COMPARE_VAL)) {
+ e_err_P += e_delta;
+ }
+ else {
+ e_steps--;
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_E);
+ e_err_P += e_delta + (FTM_STEPS_PER_UNIT_TIME);
+ anyStep = true;
+ }
+ }
+ #endif // HAS_EXTRUDERS
+
+ 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);
+ #if HAS_X_AXIS
+ if (x_delta > 0) {
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_X);
+ x_dirState = stepDirState_POS;
+ }
+ else {
+ x_dirState = stepDirState_NEG;
+ }
+ #endif
+
+ #if HAS_Y_AXIS
+ if (y_delta > 0) {
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Y);
+ y_dirState = stepDirState_POS;
+ }
+ else {
+ y_dirState = stepDirState_NEG;
+ }
+ #endif
+
+ #if HAS_Z_AXIS
+ if (z_delta > 0) {
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Z);
+ z_dirState = stepDirState_POS;
+ }
+ else {
+ z_dirState = stepDirState_NEG;
+ }
+ #endif
+
+ #if HAS_EXTRUDERS
+ if (e_delta > 0) {
+ stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_E);
+ e_dirState = stepDirState_POS;
+ }
+ else {
+ e_dirState = stepDirState_NEG;
+ }
+ #endif
+
+ any_dirChange = false;
+ }
+ else { // ...no direction change.
+ CBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
+ }
+
+ if (stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE) - 1) {
+ stepperCmdBuff_produceIdx = 0;
+ }
+ else {
+ stepperCmdBuff_produceIdx++;
+ }
+
+ nextStepTicks = FTM_MIN_TICKS;
+ }
+ } // FTM_STEPS_PER_UNIT_TIME loop
+}
+
+#endif // FT_MOTION
diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h
new file mode 100644
index 000000000000..2794608bf9cd
--- /dev/null
+++ b/Marlin/src/module/ft_motion.h
@@ -0,0 +1,170 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ */
+#pragma once
+
+#include "../inc/MarlinConfigPre.h" // Access the top level configurations.
+#include "../module/planner.h" // Access block type from planner.
+
+#include "ft_types.h"
+
+#define FTM_STEPPERCMD_DIR_SIZE ((FTM_STEPPERCMD_BUFF_SIZE + 7) / 8)
+
+class FxdTiCtrl {
+
+ public:
+
+ // Public variables
+ static ftMotionMode_t cfg_mode; // Mode / active compensation mode configuration.
+ static bool cfg_linearAdvEna; // Linear advance enable configuration.
+ static float cfg_linearAdvK; // Linear advance gain.
+ static dynFreqMode_t cfg_dynFreqMode; // Dynamic frequency mode configuration.
+
+ #if HAS_X_AXIS
+ static float cfg_baseFreq[1 + ENABLED(HAS_Y_AXIS)]; // Base frequency. [Hz]
+ static float cfg_dynFreqK[1 + ENABLED(HAS_Y_AXIS)]; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
+ #endif
+
+ static uint8_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 bool sts_stepperBusy; // The stepper buffer has items and is in use.
+
+
+ // Public methods
+ 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.
+
+
+ #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=FTM_SHAPING_ZETA, const_float_t vtol=FTM_SHAPING_V_TOL);
+
+ // 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=FTM_SHAPING_ZETA);
+ #endif
+
+ static void reset(); // Resets all states of the fixed time conversion to defaults.
+
+ private:
+
+ #if HAS_X_AXIS
+ static float xd[2 * (FTM_BATCH_SIZE)], xm[FTM_BATCH_SIZE];
+ #endif
+ #if HAS_Y_AXIS
+ static float yd[2 * (FTM_BATCH_SIZE)], ym[FTM_BATCH_SIZE];
+ #endif
+ #if HAS_Z_AXIS
+ static float zd[2 * (FTM_BATCH_SIZE)], zm[FTM_BATCH_SIZE];
+ #endif
+ #if HAS_EXTRUDERS
+ static float ed[2 * (FTM_BATCH_SIZE)], em[FTM_BATCH_SIZE];
+ #endif
+
+ static block_t *current_block_cpy;
+ static bool blockProcRdy, blockProcRdy_z1, blockProcDn;
+ static bool batchRdy, batchRdyForInterp;
+ static bool runoutEna;
+
+ // Trapezoid data variables.
+ #if HAS_X_AXIS
+ static float x_startPosn, x_endPosn_prevBlock, x_Ratio;
+ #endif
+ #if HAS_Y_AXIS
+ static float y_startPosn, y_endPosn_prevBlock, y_Ratio;
+ #endif
+ #if HAS_Z_AXIS
+ static float z_startPosn, z_endPosn_prevBlock, z_Ratio;
+ #endif
+ #if HAS_EXTRUDERS
+ static float e_startPosn, e_endPosn_prevBlock, e_Ratio;
+ #endif
+ static float accel_P, decel_P,
+ F_P,
+ f_s,
+ s_1e,
+ s_2e;
+
+ static uint32_t N1, N2, N3;
+ static uint32_t max_intervals;
+
+ // Make vector variables.
+ static uint32_t makeVector_idx,
+ makeVector_idx_z1,
+ makeVector_batchIdx;
+
+ // Interpolation variables.
+ static uint32_t interpIdx,
+ interpIdx_z1;
+ #if HAS_X_AXIS
+ static int32_t x_steps;
+ static stepDirState_t x_dirState;
+ #endif
+ #if HAS_Y_AXIS
+ static int32_t y_steps;
+ static stepDirState_t y_dirState;
+ #endif
+ #if HAS_Z_AXIS
+ static int32_t z_steps;
+ static stepDirState_t z_dirState;
+ #endif
+ #if HAS_EXTRUDERS
+ static int32_t e_steps;
+ static stepDirState_t e_dirState;
+ #endif
+
+ static hal_timer_t nextStepTicks;
+
+ // Shaping variables.
+ #if HAS_X_AXIS
+ static uint32_t xy_zi_idx, xy_max_i;
+ static float xd_zi[FTM_ZMAX];
+ static float x_Ai[5];
+ static uint32_t x_Ni[5];
+ #endif
+ #if HAS_Y_AXIS
+ static float yd_zi[FTM_ZMAX];
+ static float y_Ai[5];
+ static uint32_t y_Ni[5];
+ #endif
+
+ // Linear advance variables.
+ #if HAS_EXTRUDERS
+ static float e_raw_z1, e_advanced_z1;
+ #endif
+
+ // Private methods
+ static uint32_t stepperCmdBuffItems();
+ static void init();
+ static void loadBlockData(block_t * const current_block);
+ static void makeVector();
+ static void convertToSteps(const uint32_t idx);
+
+}; // class fxdTiCtrl
+
+extern FxdTiCtrl fxdTiCtrl;
diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h
new file mode 100644
index 000000000000..613e177a3915
--- /dev/null
+++ b/Marlin/src/module/ft_types.h
@@ -0,0 +1,59 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ *
+ */
+#pragma once
+
+#include "../core/types.h"
+
+typedef enum FXDTICtrlMode : uint8_t {
+ ftMotionMode_DISABLED = 0U,
+ ftMotionMode_ENABLED = 1U,
+ ftMotionMode_ULENDO_FBS = 2U,
+ ftMotionMode_ZV = 10U,
+ ftMotionMode_ZVD = 11U,
+ ftMotionMode_EI = 12U,
+ ftMotionMode_2HEI = 13U,
+ ftMotionMode_3HEI = 14U,
+ ftMotionMode_MZV = 15U,
+ ftMotionMode_DISCTF = 20U
+} ftMotionMode_t;
+
+enum dynFreqMode_t : uint8_t {
+ dynFreqMode_DISABLED = 0U,
+ dynFreqMode_Z_BASED = 1U,
+ dynFreqMode_MASS_BASED = 2U
+};
+
+enum stepDirState_t {
+ stepDirState_NOT_SET = 0U,
+ stepDirState_POS = 1U,
+ stepDirState_NEG = 2U
+};
+
+enum {
+ FT_BIT_DIR_E, FT_BIT_STEP_E,
+ FT_BIT_DIR_Z, FT_BIT_STEP_Z,
+ FT_BIT_DIR_Y, FT_BIT_STEP_Y,
+ FT_BIT_DIR_X, FT_BIT_STEP_X,
+ FT_BIT_COUNT
+};
+
+typedef bits_t(FT_BIT_COUNT) ft_command_t;
diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp
index 552d212e2e69..dadb22fbde9c 100644
--- a/Marlin/src/module/planner.cpp
+++ b/Marlin/src/module/planner.cpp
@@ -69,6 +69,9 @@
#include "stepper.h"
#include "motion.h"
#include "temperature.h"
+#if ENABLED(FT_MOTION)
+ #include "ft_motion.h"
+#endif
#include "../lcd/marlinui.h"
#include "../gcode/parser.h"
@@ -112,7 +115,8 @@
// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
// fewer movements. The delay is measured in milliseconds, and must be less than 250ms
-#define BLOCK_DELAY_FOR_1ST_MOVE 100
+#define BLOCK_DELAY_NONE 0U
+#define BLOCK_DELAY_FOR_1ST_MOVE 100U
Planner planner;
@@ -127,7 +131,7 @@ volatile uint8_t Planner::block_buffer_head, // Index of the next block to be
Planner::block_buffer_planned, // Index of the optimally planned block
Planner::block_buffer_tail; // Index of the busy block, if any
uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks
-uint8_t Planner::delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+uint8_t Planner::delay_before_delivering; // Delay block delivery so initial blocks in an empty queue may merge
planner_settings_t Planner::settings; // Initialized by settings.load()
@@ -225,6 +229,10 @@ float Planner::previous_nominal_speed;
int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0f / (XY_FREQUENCY_LIMIT));
#endif
+#if ENABLED(FT_MOTION)
+ bool Planner::fxdTiCtrl_busy = false;
+#endif
+
#if ENABLED(LIN_ADVANCE)
float Planner::extruder_advance_K[DISTINCT_E]; // Initialized by settings.load()
#endif
@@ -1683,7 +1691,8 @@ void Planner::quick_stop() {
// Restart the block delay for the first movement - As the queue was
// forced to empty, there's no risk the ISR will touch this.
- delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+
+ delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
TERN_(HAS_WIRED_LCD, clear_block_buffer_runtime()); // Clear the accumulated runtime
@@ -1729,6 +1738,7 @@ bool Planner::busy() {
return (has_blocks_queued() || cleaning_buffer_counter
|| TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING())
|| TERN0(HAS_ZV_SHAPING, stepper.input_shaping_busy())
+ || TERN0(FT_MOTION, fxdTiCtrl_busy)
);
}
@@ -1841,7 +1851,7 @@ bool Planner::_buffer_steps(const xyze_long_t &target
// As there are no queued movements, the Stepper ISR will not touch this
// variable, so there is no risk setting this here (but it MUST be done
// before the following line!!)
- delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+ delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
// Move buffer head
@@ -2945,7 +2955,7 @@ void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_PO
// As there are no queued movements, the Stepper ISR will not touch this
// variable, so there is no risk setting this here (but it MUST be done
// before the following line!!)
- delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+ delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
block_buffer_head = next_buffer_head;
@@ -3243,7 +3253,7 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
// As there are no queued movements, the Stepper ISR will not touch this
// variable, so there is no risk setting this here (but it MUST be done
// before the following line!!)
- delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+ delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
// Move buffer head
diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h
index ccf6ba08d3ca..e2d1d6739cda 100644
--- a/Marlin/src/module/planner.h
+++ b/Marlin/src/module/planner.h
@@ -512,6 +512,10 @@ class Planner {
}
#endif
+ #if ENABLED(FT_MOTION)
+ static bool fxdTiCtrl_busy;
+ #endif
+
private:
/**
diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
index 34a5ca28a3fd..1062a778d841 100644
--- a/Marlin/src/module/stepper.cpp
+++ b/Marlin/src/module/stepper.cpp
@@ -91,6 +91,10 @@ Stepper stepper; // Singleton
#include "planner.h"
#include "motion.h"
+#if ENABLED(FT_MOTION)
+ #include "ft_motion.h"
+#endif
+
#include "../lcd/marlinui.h"
#include "../gcode/queue.h"
#include "../sd/cardreader.h"
@@ -1488,63 +1492,133 @@ void Stepper::isr() {
// Limit the amount of iterations
uint8_t max_loops = 10;
+ #if ENABLED(FT_MOTION)
+ static bool fxdTiCtrl_stepCmdRdy = false; // Indicates a step command was loaded from the
+ // buffers and is ready to be output.
+ static bool fxdTiCtrl_applyDir = false; // Indicates the DIR output should be set.
+ static ft_command_t fxdTiCtrl_stepCmd = 0U; // Storage for the step command to be output.
+ static uint32_t fxdTiCtrl_nextAuxISR = 0U; // Storage for the next ISR of the auxilliary tasks.
+ #endif
+
// We need this variable here to be able to use it in the following loop
hal_timer_t min_ticks;
do {
// Enable ISRs to reduce USART processing latency
hal.isr_on();
- TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed
+ hal_timer_t interval;
+
+ #if ENABLED(FT_MOTION)
+
+ // NOTE STEPPER_TIMER_RATE is equal to 2000000, not what VSCode shows
+ const bool using_fxtictrl = fxdTiCtrl.cfg_mode;
+ if (using_fxtictrl) {
+ if (!nextMainISR) {
+ if (abort_current_block) {
+ fxdTiCtrl_stepCmdRdy = false; // If a command was ready, cancel it.
+ fxdTiCtrl.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 (fxdTiCtrl_stepCmdRdy) {
+ fxdTiCtrl_stepper(fxdTiCtrl_applyDir, fxdTiCtrl_stepCmd);
+ fxdTiCtrl_stepCmdRdy = false;
+ }
+ // Check if there is data in the buffers.
+ if (fxdTiCtrl.stepperCmdBuff_produceIdx != fxdTiCtrl.stepperCmdBuff_consumeIdx) {
+
+ fxdTiCtrl.sts_stepperBusy = true;
+
+ // "Pop" one command from the command buffer.
+ fxdTiCtrl_stepCmd = fxdTiCtrl.stepperCmdBuff[fxdTiCtrl.stepperCmdBuff_consumeIdx];
+ const uint8_t dir_index = fxdTiCtrl.stepperCmdBuff_consumeIdx >> 3,
+ dir_bit = fxdTiCtrl.stepperCmdBuff_consumeIdx & 0x7;
+ fxdTiCtrl_applyDir = TEST(fxdTiCtrl.stepperCmdBuff_ApplyDir[dir_index], dir_bit);
+ nextMainISR = fxdTiCtrl.stepperCmdBuff_StepRelativeTi[fxdTiCtrl.stepperCmdBuff_consumeIdx];
+ fxdTiCtrl_stepCmdRdy = true;
- if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses
+ if (++fxdTiCtrl.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE))
+ fxdTiCtrl.stepperCmdBuff_consumeIdx = 0;
- #if ENABLED(LIN_ADVANCE)
- if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses
- advance_isr();
- nextAdvanceISR = la_interval;
+ }
+ else { // Buffer empty.
+ fxdTiCtrl.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 auxilliary functions.
+ if (!fxdTiCtrl_nextAuxISR) {
+ endstops.update();
+ TERN_(INTEGRATED_BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
+ fxdTiCtrl_refreshAxisDidMove();
+ fxdTiCtrl_nextAuxISR = 0.0025f * (STEPPER_TIMER_RATE);
+ }
+
+ interval = _MIN(nextMainISR, fxdTiCtrl_nextAuxISR);
+ nextMainISR -= interval;
+ fxdTiCtrl_nextAuxISR -= interval;
}
- else if (nextAdvanceISR == LA_ADV_NEVER) // Start LA steps if necessary
- nextAdvanceISR = la_interval;
- #endif
- #if ENABLED(INTEGRATED_BABYSTEPPING)
- const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses
- if (is_babystep) nextBabystepISR = babystepping_isr();
+ #else
+
+ constexpr bool using_fxtictrl = false;
+
#endif
- // ^== Time critical. NOTHING besides pulse generation should be above here!!!
+ if (!using_fxtictrl) {
- if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block
+ TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed
- #if ENABLED(INTEGRATED_BABYSTEPPING)
- if (is_babystep) // Avoid ANY stepping too soon after baby-stepping
- NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step
+ if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses
- if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping
- NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping
- #endif
+ #if ENABLED(LIN_ADVANCE)
+ if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses
+ advance_isr();
+ nextAdvanceISR = la_interval;
+ }
+ else if (nextAdvanceISR == LA_ADV_NEVER) // Start LA steps if necessary
+ nextAdvanceISR = la_interval;
+ #endif
- // Get the interval to the next ISR call
- const hal_timer_t interval = _MIN(
- hal_timer_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time
- nextMainISR // Time until the next Pulse / Block phase
- OPTARG(INPUT_SHAPING_X, ShapingQueue::peek_x()) // Time until next input shaping echo for X
- OPTARG(INPUT_SHAPING_Y, ShapingQueue::peek_y()) // Time until next input shaping echo for Y
- OPTARG(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance?
- OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping?
- );
+ #if ENABLED(INTEGRATED_BABYSTEPPING)
+ const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses
+ if (is_babystep) nextBabystepISR = babystepping_isr();
+ #endif
+
+ // ^== Time critical. NOTHING besides pulse generation should be above here!!!
+
+ if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block
+
+ #if ENABLED(INTEGRATED_BABYSTEPPING)
+ if (is_babystep) // Avoid ANY stepping too soon after baby-stepping
+ NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step
+
+ if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping
+ NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping
+ #endif
+
+ // Get the interval to the next ISR call
+ interval = _MIN(nextMainISR, uint32_t(HAL_TIMER_TYPE_MAX)); // Time until the next Pulse / Block phase
+ TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X
+ TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y
+ TERN_(LIN_ADVANCE, NOMORE(interval, nextAdvanceISR)); // Come back early for Linear Advance?
+ TERN_(INTEGRATED_BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping?
+
+ //
+ // Compute remaining time for each ISR phase
+ // NEVER : The phase is idle
+ // Zero : The phase will occur on the next ISR call
+ // Non-zero : The phase will occur on a future ISR call
+ //
- //
- // Compute remaining time for each ISR phase
- // NEVER : The phase is idle
- // Zero : The phase will occur on the next ISR call
- // Non-zero : The phase will occur on a future ISR call
- //
+ nextMainISR -= interval;
+ TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval));
+ TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval);
+ TERN_(INTEGRATED_BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval);
- nextMainISR -= interval;
- TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval));
- TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval);
- TERN_(INTEGRATED_BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval);
+ } // standard motion control
/**
* This needs to avoid a race-condition caused by interleaving
@@ -1978,7 +2052,7 @@ void Stepper::pulse_phase_isr() {
#if ENABLED(MIXING_EXTRUDER)
if (step_needed.e) {
- count_position[E_AXIS] += count_direction[E_AXIS];
+ count_position.e += count_direction.e;
E_STEP_WRITE(mixer.get_next_stepper(), STEP_STATE_E);
}
#elif HAS_E0_STEP
@@ -3381,6 +3455,127 @@ void Stepper::report_positions() {
report_a_position(pos);
}
+#if ENABLED(FT_MOTION)
+
+ // Set stepper I/O for fixed time controller.
+ void Stepper::fxdTiCtrl_stepper(const bool applyDir, const ft_command_t command) {
+
+ USING_TIMED_PULSE();
+
+ #if HAS_Z_AXIS
+ // Z is handled differently to update the stepper
+ // counts (needed by Marlin for bed level probing).
+ const bool z_dir = !TEST(command, FT_BIT_DIR_Z),
+ z_step = TEST(command, FT_BIT_STEP_Z);
+ #endif
+
+ if (applyDir) {
+ X_DIR_WRITE(TEST(command, FT_BIT_DIR_X));
+ TERN_(HAS_Y_AXIS, Y_DIR_WRITE(TEST(command, FT_BIT_DIR_Y)));
+ TERN_(HAS_Z_AXIS, Z_DIR_WRITE(z_dir));
+ TERN_(HAS_EXTRUDERS, E0_DIR_WRITE(TEST(command, FT_BIT_DIR_E)));
+ DIR_WAIT_AFTER();
+ }
+
+ X_STEP_WRITE(TEST(command, FT_BIT_STEP_X));
+ TERN_(HAS_Y_AXIS, Y_STEP_WRITE(TEST(command, FT_BIT_STEP_Y)));
+ TERN_(HAS_Z_AXIS, Z_STEP_WRITE(z_step));
+ TERN_(HAS_EXTRUDERS, E0_STEP_WRITE(TEST(command, FT_BIT_STEP_E)));
+
+ START_TIMED_PULSE();
+
+ #if HAS_Z_AXIS
+ // Update step counts
+ if (z_step) count_position.z += z_dir ? -1 : 1;
+ #endif
+
+ AWAIT_HIGH_PULSE();
+
+ X_STEP_WRITE(0);
+ TERN_(HAS_Y_AXIS, Y_STEP_WRITE(0));
+ TERN_(HAS_Z_AXIS, Z_STEP_WRITE(0));
+ TERN_(HAS_EXTRUDERS, E0_STEP_WRITE(0));
+
+ } // Stepper::fxdTiCtrl_stepper
+
+ void Stepper::fxdTiCtrl_BlockQueueUpdate() {
+
+ if (current_block) {
+ // If the current block is not done processing, return right away
+ if (!fxdTiCtrl.getBlockProcDn()) return;
+
+ axis_did_move = 0;
+ current_block = nullptr;
+ discard_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
+ }
+
+ // 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;
+
+ fxdTiCtrl.startBlockProc(current_block);
+
+ }
+ else {
+ fxdTiCtrl.runoutBlock();
+ return; // No queued blocks
+ }
+
+ } // if (!current_block)
+
+ } // Stepper::fxdTiCtrl_BlockQueueUpdate()
+
+ // Debounces the axis move indication to account for potential
+ // delay between the block information and the stepper commands
+ void Stepper::fxdTiCtrl_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.
+
+ uint8_t axis_bits = 0U;
+
+ static uint32_t a_debounce = 0U;
+ if (!!current_block->steps.a) a_debounce = (AXIS_DID_MOVE_DEB) * 400; // divide by 0.0025f
+ if (a_debounce) { SBI(axis_bits, A_AXIS); a_debounce--; }
+ #if HAS_Y_AXIS
+ static uint32_t b_debounce = 0U;
+ if (!!current_block->steps.b) b_debounce = (AXIS_DID_MOVE_DEB) * 400;
+ if (b_debounce) { SBI(axis_bits, B_AXIS); b_debounce--; }
+ #endif
+ #if HAS_Z_AXIS
+ static uint32_t c_debounce = 0U;
+ if (!!current_block->steps.c) c_debounce = (AXIS_DID_MOVE_DEB) * 400;
+ if (c_debounce) { SBI(axis_bits, C_AXIS); c_debounce--; }
+ #endif
+ #if HAS_EXTRUDERS
+ static uint32_t e_debounce = 0U;
+ if (!!current_block->steps.e) e_debounce = (AXIS_DID_MOVE_DEB) * 400;
+ if (e_debounce) { SBI(axis_bits, E_AXIS); e_debounce--; }
+ #endif
+
+ axis_did_move = axis_bits;
+ }
+
+#endif // FT_MOTION
+
#if ENABLED(BABYSTEPPING)
#define _ENABLE_AXIS(A) enable_axis(_AXIS(A))
diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h
index 4adbb01991b3..4c54a5dbf955 100644
--- a/Marlin/src/module/stepper.h
+++ b/Marlin/src/module/stepper.h
@@ -49,6 +49,10 @@
#include "stepper/speed_lookuptable.h"
#endif
+#if ENABLED(FT_MOTION)
+ #include "ft_types.h"
+#endif
+
//
// Estimate the amount of time the Stepper ISR will take to execute
//
@@ -470,6 +474,7 @@ constexpr ena_mask_t enable_overlap[] = {
//
class Stepper {
friend class Max7219;
+ friend class FxdTiCtrl;
friend void stepperTask(void *);
public:
@@ -817,6 +822,11 @@ class Stepper {
set_directions();
}
+ #if ENABLED(FT_MOTION)
+ // Manage the planner
+ static void fxdTiCtrl_BlockQueueUpdate();
+ #endif
+
#if HAS_ZV_SHAPING
static void set_shaping_damping_ratio(const AxisEnum axis, const_float_t zeta);
static float get_shaping_damping_ratio(const AxisEnum axis);
@@ -848,6 +858,11 @@ class Stepper {
static void microstep_init();
#endif
+ #if ENABLED(FT_MOTION)
+ static void fxdTiCtrl_stepper(const bool applyDir, const ft_command_t command);
+ static void fxdTiCtrl_refreshAxisDidMove();
+ #endif
+
};
extern Stepper stepper;
diff --git a/buildroot/tests/STM32F103RC_btt b/buildroot/tests/STM32F103RC_btt
index 16419cbfa232..95a18c615ff8 100755
--- a/buildroot/tests/STM32F103RC_btt
+++ b/buildroot/tests/STM32F103RC_btt
@@ -12,8 +12,8 @@ set -e
restore_configs
opt_set MOTHERBOARD BOARD_BTT_SKR_MINI_E3_V1_0 SERIAL_PORT 1 SERIAL_PORT_2 -1 \
X_DRIVER_TYPE TMC2209 Y_DRIVER_TYPE TMC2209 Z_DRIVER_TYPE TMC2209 E0_DRIVER_TYPE TMC2209
-opt_enable PINS_DEBUGGING Z_IDLE_HEIGHT
-exec_test $1 $2 "BigTreeTech SKR Mini E3 1.0 - Basic Config with TMC2209 HW Serial" "$3"
+opt_enable PINS_DEBUGGING Z_IDLE_HEIGHT FT_MOTION
+exec_test $1 $2 "BigTreeTech SKR Mini E3 1.0 - TMC2209 HW Serial, FT_MOTION" "$3"
# clean up
restore_configs
diff --git a/ini/features.ini b/ini/features.ini
index 4e378a8eaf65..d8ba74f2db20 100644
--- a/ini/features.ini
+++ b/ini/features.ini
@@ -186,6 +186,7 @@ AIR_EVACUATION = src_filter=+
SERVO_DETACH_GCODE = src_filter=+
HAS_DUPLICATION_MODE = src_filter=+
+FT_MOTION = src_filter=+ +
LIN_ADVANCE = src_filter=+
PHOTO_GCODE = src_filter=+
CONTROLLER_FAN_EDITABLE = src_filter=+
diff --git a/platformio.ini b/platformio.ini
index 9fbc589d2e54..3478dcc1fbad 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -192,6 +192,7 @@ default_src_filter = + - - + -
-
-
+ - -
-
-
-