FT Motion updates

Marlin/src/module/endstops.cpp

@@ -31,8 +31,9 @@
 #include "temperature.h"
 #include "../lcd/marlinui.h"
 
-#define DEBUG_OUT ALL(USE_SENSORLESS, DEBUG_LEVELING_FEATURE)
-#include "../core/debug_out.h"
+#if ENABLED(FT_MOTION)
+  #include "ft_motion.h"
+#endif
 
 #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
   #include HAL_PATH(.., endstop_interrupts.h)
@@ -54,6 +55,9 @@
   #include "probe.h"
 #endif
 
+#define DEBUG_OUT ALL(USE_SENSORLESS, DEBUG_LEVELING_FEATURE)
+#include "../core/debug_out.h"
+
 Endstops endstops;
 
 // private:
@@ -830,9 +834,13 @@ void Endstops::update() {
 
   // Signal, after validation, if an endstop limit is pressed or not
 
+  #define AXIS_IS_MOVING(A) TERN(FT_MOTION, ftMotion, stepper).axis_is_moving(_AXIS(A))
+  #define AXIS_DIR_REV(A)  !TERN(FT_MOTION, ftMotion, stepper).motor_direction(A)
+
   #if HAS_X_AXIS
-    if (stepper.axis_is_moving(X_AXIS)) {
-      if (!stepper.motor_direction(X_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(X)) {
+      const AxisEnum x_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? X_AXIS : X_AXIS_HEAD;
+      if (AXIS_DIR_REV(x_head)) {
         #if HAS_X_MIN_STATE
           PROCESS_ENDSTOP_X(MIN);
           #if   CORE_DIAG(XY, Y, MIN)
@@ -864,8 +872,9 @@ void Endstops::update() {
   #endif // HAS_X_AXIS
 
   #if HAS_Y_AXIS
-    if (stepper.axis_is_moving(Y_AXIS)) {
-      if (!stepper.motor_direction(Y_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(Y)) {
+      const AxisEnum y_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? Y_AXIS : Y_AXIS_HEAD;
+      if (AXIS_DIR_REV(y_head)) {
         #if HAS_Y_MIN_STATE
           PROCESS_ENDSTOP_Y(MIN);
           #if   CORE_DIAG(XY, X, MIN)
@@ -897,8 +906,9 @@ void Endstops::update() {
   #endif // HAS_Y_AXIS
 
   #if HAS_Z_AXIS
-    if (stepper.axis_is_moving(Z_AXIS)) {
-      if (!stepper.motor_direction(Z_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(Z)) {
+      const AxisEnum z_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? Z_AXIS : Z_AXIS_HEAD;
+      if (AXIS_DIR_REV(z_head)) {
         // Z- : Gantry down, bed up
         #if HAS_Z_MIN_STATE
           // If the Z_MIN_PIN is being used for the probe there's no
@@ -944,8 +954,8 @@ void Endstops::update() {
   #endif // HAS_Z_AXIS
 
   #if HAS_I_AXIS && HAS_I_STATE
-    if (stepper.axis_is_moving(I_AXIS)) {
-      if (!stepper.motor_direction(I_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(I)) {
+      if (AXIS_DIR_REV(I_AXIS_HEAD)) {
         #if HAS_I_MIN_STATE
           PROCESS_ENDSTOP(I, MIN);
         #endif
@@ -959,8 +969,8 @@ void Endstops::update() {
   #endif // HAS_I_AXIS
 
   #if HAS_J_AXIS && HAS_J_STATE
-    if (stepper.axis_is_moving(J_AXIS)) {
-      if (!stepper.motor_direction(J_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(J)) {
+      if (AXIS_DIR_REV(J_AXIS_HEAD)) {
         #if HAS_J_MIN_STATE
           PROCESS_ENDSTOP(J, MIN);
         #endif
@@ -974,8 +984,8 @@ void Endstops::update() {
   #endif // HAS_J_AXIS
 
   #if HAS_K_AXIS && HAS_K_STATE
-    if (stepper.axis_is_moving(K_AXIS)) {
-      if (!stepper.motor_direction(K_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(K)) {
+      if (AXIS_DIR_REV(K_AXIS_HEAD)) {
         #if HAS_K_MIN_STATE
           PROCESS_ENDSTOP(K, MIN);
         #endif
@@ -989,8 +999,8 @@ void Endstops::update() {
   #endif // HAS_K_AXIS
 
   #if HAS_U_AXIS && HAS_U_STATE
-    if (stepper.axis_is_moving(U_AXIS)) {
-      if (!stepper.motor_direction(U_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(U)) {
+      if (AXIS_DIR_REV(U_AXIS_HEAD)) {
         #if HAS_U_MIN_STATE
           PROCESS_ENDSTOP(U, MIN);
         #endif
@@ -1004,8 +1014,8 @@ void Endstops::update() {
   #endif // HAS_U_AXIS
 
   #if HAS_V_AXIS && HAS_V_STATE
-    if (stepper.axis_is_moving(V_AXIS)) {
-      if (!stepper.motor_direction(V_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(V)) {
+      if (AXIS_DIR_REV(V_AXIS_HEAD)) {
         #if HAS_V_MIN_STATE
           PROCESS_ENDSTOP(V, MIN);
         #endif
@@ -1019,8 +1029,8 @@ void Endstops::update() {
   #endif // HAS_V_AXIS
 
   #if HAS_W_AXIS && HAS_W_STATE
-    if (stepper.axis_is_moving(W_AXIS)) {
-      if (!stepper.motor_direction(W_AXIS_HEAD)) {
+    if (AXIS_IS_MOVING(W)) {
+      if (AXIS_DIR_REV(W_AXIS_HEAD)) {
         #if HAS_W_MIN_STATE
           PROCESS_ENDSTOP(W, MIN);
         #endif

Marlin/src/module/ft_motion.cpp

@@ -44,6 +44,8 @@ int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper comman
 
 bool FTMotion::sts_stepperBusy = false;         // The stepper buffer has items and is in use.
 
+XYZEval<millis_t> FTMotion::axis_move_end_ti = { 0 };
+AxisBits FTMotion::axis_move_dir;
 
 // Private variables.
 
@@ -112,8 +114,6 @@ constexpr uint32_t BATCH_SIDX_IN_WINDOW = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
 
 // Public functions.
 
-static bool markBlockStart = false;
-
 // Controller main, to be invoked from non-isr task.
 void FTMotion::loop() {
 
@@ -141,7 +141,6 @@ void FTMotion::loop() {
       continue;
     }
     loadBlockData(stepper.current_block);
-    markBlockStart = true;
     blockProcRdy = true;
     // Some kinematics track axis motion in HX, HY, HZ
     #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX)
@@ -390,6 +389,8 @@ void FTMotion::reset() {
   #endif
 
   TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f);
+
+  axis_move_end_ti.reset();
 }
 
 // Private functions.
@@ -429,7 +430,11 @@ void FTMotion::runoutBlock() {
 
   const int32_t n_to_settle_and_fill_batch = n_to_settle_shaper + n_to_fill_batch_after_settling;
 
-  max_intervals = (PROP_BATCHES) * (FTM_BATCH_SIZE) + n_to_settle_and_fill_batch;
+  const int32_t N_needed_to_propagate_to_stepper = PROP_BATCHES;
+
+  const int32_t n_to_use = N_needed_to_propagate_to_stepper * (FTM_BATCH_SIZE) + n_to_settle_and_fill_batch;
+
+  max_intervals = n_to_use;
 
   blockProcRdy = true;
 }
@@ -549,6 +554,26 @@ void FTMotion::loadBlockData(block_t * const current_block) {
 
   endPosn_prevBlock += moveDist;
 
+  // Watch endstops until the move ends
+  const millis_t move_end_ti = millis() + SEC_TO_MS((FTM_TS) * float(max_intervals + num_samples_shaper_settle() + ((PROP_BATCHES) + 1) * (FTM_BATCH_SIZE)) + (float(FTM_STEPPERCMD_BUFF_SIZE) / float(FTM_STEPPER_FS)));
+
+  #define __SET_MOVE_END(A,V) do{ if (V) { axis_move_end_ti.A = move_end_ti; axis_move_dir.A = (V > 0); } }while(0);
+  #define _SET_MOVE_END(A) __SET_MOVE_END(A, moveDist[_AXIS(A)])
+  #if CORE_IS_XY
+    __SET_MOVE_END(X, moveDist.x + moveDist.y);
+    __SET_MOVE_END(Y, moveDist.x - moveDist.y);
+  #else
+    _SET_MOVE_END(X);
+    _SET_MOVE_END(Y);
+  #endif
+  TERN_(HAS_Z_AXIS, _SET_MOVE_END(Z));
+  SECONDARY_AXIS_MAP(_SET_MOVE_END);
+
+  // If the endstop is already pressed, endstop interrupts won't invoke
+  // endstop_triggered and the move will grind. So check here for a
+  // triggered endstop, which shortly marks the block for discard.
+  endstops.update();
+
 }
 
 // Generate data points of the trajectory.
@@ -565,6 +590,7 @@ void FTMotion::makeVector() {
   else if (makeVector_idx < (N1 + N2)) {
     // 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
@@ -717,12 +743,6 @@ void FTMotion::convertToSteps(const uint32_t idx) {
     // Init all step/dir bits to 0 (defaulting to reverse/negative motion)
     cmd = 0;
 
-    // Mark the start of a new block
-    if (markBlockStart) {
-      cmd = _BV(FT_BIT_START);
-      markBlockStart = false;
-    }
-
     // Accumulate the errors for all axes
     err_P += delta;
 

Marlin/src/module/ft_motion.h

@@ -23,6 +23,7 @@
 
 #include "../inc/MarlinConfigPre.h" // Access the top level configurations.
 #include "../module/planner.h"      // Access block type from planner.
+#include "../module/stepper.h"      // For stepper motion and direction
 
 #include "ft_types.h"
 
@@ -110,6 +111,9 @@ class FTMotion {
 
     static bool sts_stepperBusy;                          // The stepper buffer has items and is in use.
 
+    static XYZEval<millis_t> axis_move_end_ti;
+    static AxisBits axis_move_dir;
+
     // Public methods
     static void init();
     static void loop();                                   // Controller main, to be invoked from non-isr task.
@@ -121,6 +125,13 @@ class FTMotion {
 
     static void reset();                                  // Reset all states of the fixed time conversion to defaults.
 
+    FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) {
+      return cfg.active ? PENDING(millis(), axis_move_end_ti[axis]) : stepper.axis_is_moving(axis);
+    }
+    FORCE_INLINE static bool motor_direction(const AxisEnum axis) {
+      return cfg.active ? axis_move_dir[axis] : stepper.last_direction_bits[axis];
+    }
+
   private:
 
     static xyze_trajectory_t traj;

Marlin/src/module/ft_types.h

@@ -49,7 +49,6 @@ typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
 
 // TODO: Convert ft_command_t to a struct with bitfields instead of using a primitive type
 enum {
-  FT_BIT_START,
   LIST_N(DOUBLE(LOGICAL_AXES),
     FT_BIT_DIR_E, FT_BIT_STEP_E,
     FT_BIT_DIR_X, FT_BIT_STEP_X, FT_BIT_DIR_Y, FT_BIT_STEP_Y, FT_BIT_DIR_Z, FT_BIT_STEP_Z,

Marlin/src/module/stepper.cpp

@@ -1531,6 +1531,7 @@ void Stepper::isr() {
   uint8_t max_loops = 10;
 
   #if ENABLED(FT_MOTION)
+    static uint32_t ftMotion_nextAuxISR = 0U;  // Storage for the next ISR of the auxilliary tasks.
     const bool using_ftMotion = ftMotion.cfg.active;
   #else
     constexpr bool using_ftMotion = false;
@@ -1550,21 +1551,15 @@ void Stepper::isr() {
         if (!nextMainISR) {               // Main ISR is ready to fire during this iteration?
           nextMainISR = FTM_MIN_TICKS;    // Set to minimum interval (a limit on the top speed)
           ftMotion_stepper();             // Run FTM Stepping
-        }
-
-        #if ENABLED(BABYSTEPPING)
-          if (nextBabystepISR == 0) {                   // Avoid ANY stepping too soon after baby-stepping
-            nextBabystepISR = babystepping_isr();
-            NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8);  // FULL STOP for 125µs after a baby-step
+          // Define 2.5 msec task for auxilliary functions.
+          if (!ftMotion_nextAuxISR) {
+            TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
+            ftMotion_nextAuxISR = (STEPPER_TIMER_RATE) / 400;
           }
-          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
-
-        interval = nextMainISR;                         // Interval is either some old nextMainISR or FTM_MIN_TICKS
-        TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping?
-
-        nextMainISR = 0;                                // For FT Motion fire again ASAP
+        }
+        interval = _MIN(nextMainISR, ftMotion_nextAuxISR);
+        nextMainISR -= interval;
+        ftMotion_nextAuxISR -= interval;
       }
 
     #endif
@@ -3537,15 +3532,6 @@ void Stepper::report_positions() {
     #define _FTM_STEP(AXIS) TEST(command, FT_BIT_STEP_##AXIS)
     #define _FTM_DIR(AXIS) TEST(command, FT_BIT_DIR_##AXIS)
 
-    /**
-     * Set bits in axis_did_move for any axes moving in this block,
-     * clearing the bits at the start of each new segment.
-     */
-    if (TEST(command, FT_BIT_START)) axis_did_move.reset();
-
-    #define _FTM_AXIS_DID_MOVE(AXIS) axis_did_move.bset(_AXIS(AXIS), _FTM_STEP(AXIS));
-    LOGICAL_AXIS_MAP(_FTM_AXIS_DID_MOVE);
-
     /**
      * Update direction bits for steppers that were stepped by this command.
      * HX, HY, HZ direction bits were set for Core kinematics
@@ -3607,11 +3593,6 @@ void Stepper::report_positions() {
     #define _FTM_STEP_STOP(AXIS) AXIS##_APPLY_STEP(!STEP_STATE_##AXIS, false);
     LOGICAL_AXIS_MAP(_FTM_STEP_STOP);
 
-    // Check endstops on every step using axis_did_move as set by every step
-    // TODO: Update endstop states less frequently to save processing.
-    // NOTE: endstops.poll is still called at 1KHz by Temperature ISR.
-    IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, if ((bool)axis_did_move) endstops.update());
-
   } // Stepper::ftMotion_stepper
 
 #endif // FT_MOTION