🐛 Fix some FT Motion probing issues (#28096)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

Marlin/Configuration.h

@@ -1667,6 +1667,8 @@
   //#define PROBE_TOOLCHANGE_NO_MOVE  // Suppress motion on probe tool-change
 #endif
 
+//#define PROBE_WAKEUP_TIME_MS  30    // (ms) Time for the probe to wake up
+
 // Most probes should stay away from the edges of the bed, but
 // with NOZZLE_AS_PROBE this can be negative for a wider probing area.
 #define PROBING_MARGIN 10

Marlin/src/inc/Warnings.cpp

@@ -919,6 +919,13 @@
   #warning "The BEEPER cannot produce tones so you can disable SPEAKER."
 #endif
 
+/**
+ * Delay for probes that need time to boot up when enabled
+ */
+#if defined(DELAY_BEFORE_PROBING) && DELAY_BEFORE_PROBING < 25
+  #warning "The actual DELAY_BEFORE_PROBING will be the minimum 25 ms. Leave DELAY_BEFORE_PROBING disabled to use the minimum."
+#endif
+
 /**
  * Fixed-Time Motion
  */
@@ -926,17 +933,19 @@
   #if ENABLED(I2S_STEPPER_STREAM)
     #warning "FT_MOTION has not been tested with I2S_STEPPER_STREAM."
   #endif
-  #if ENABLED(FTM_HOME_AND_PROBE) && ANY(BIQU_MICROPROBE_V1, BIQU_MICROPROBE_V2)
-    #warning "FT_MOTION in known to have issues with BIQU Microprobe."
-  #endif
-  #if ENABLED(FTM_HOME_AND_PROBE) && DELAY_BEFORE_PROBING <= 25
-    #warning "A longer DELAY_BEFORE_PROBING is recommended when using a probe with FT_MOTION."
-  #endif
   #if ENABLED(NONLINEAR_EXTRUSION)
     #warning "NONLINEAR_EXTRUSION does not (currently) operate when FT_MOTION is the active motion system."
   #endif
   #if ENABLED(LIN_ADVANCE)
-    #warning "Be aware that FT_MOTION K factor (M493 K) is a separate setting from LIN_ADVANCE K factor (M900 K)."
+    #warning "Be aware that FT_MOTION K factor is now set with M900 K (same as LIN_ADVANCE)."
+  #endif
+#endif
+#if ENABLED(FTM_HOME_AND_PROBE)
+  #if ANY(BIQU_MICROPROBE_V1, BIQU_MICROPROBE_V2)
+    #warning "Let us know if you experience any issues with BIQU Microprobe and FT_MOTION."
+  #endif
+  #if DELAY_BEFORE_PROBING <= 25
+    #warning "A DELAY_BEFORE_PROBING over 25 ms is recommended with FT_MOTION."
   #endif
 #endif
 

Marlin/src/module/endstops.cpp

@@ -328,6 +328,9 @@ void Endstops::enable(const bool onoff) {
     #if PIN_EXISTS(PROBE_ENABLE)
       WRITE(PROBE_ENABLE_PIN, onoff);
     #endif
+    #if PROBE_WAKEUP_TIME_MS
+      if (onoff) safe_delay(PROBE_WAKEUP_TIME_MS);
+    #endif
     resync();
   }
 #endif
@@ -853,11 +856,14 @@ void Endstops::update() {
     #define PROCESS_ENDSTOP_Z(MINMAX) PROCESS_DUAL_ENDSTOP(Z, MINMAX)
   #endif
 
+  #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 ENABLED(G38_PROBE_TARGET)
     // For G38 moves check the probe's pin for ALL movement
     if (G38_move && TEST_ENDSTOP(Z_MIN_PROBE) == TERN1(G38_PROBE_AWAY, (G38_move < 4))) {
       G38_did_trigger = true;
-      #define _G38_SET(Q) | (stepper.axis_is_moving(_AXIS(Q)) << _AXIS(Q))
+      #define _G38_SET(Q) | (AXIS_IS_MOVING(Q) << _AXIS(Q))
       #define _G38_RESP(Q) if (moving[_AXIS(Q)]) { _ENDSTOP_HIT(Q, ENDSTOP); planner.endstop_triggered(_AXIS(Q)); }
       const Flags<NUM_AXES> moving = { uvalue_t(NUM_AXES)(0 MAIN_AXIS_MAP(_G38_SET)) };
       MAIN_AXIS_MAP(_G38_RESP);
@@ -872,9 +878,6 @@ 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 (AXIS_IS_MOVING(X)) {
       const AxisEnum x_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? X_AXIS : X_AXIS_HEAD;

Marlin/src/module/ft_motion.cpp

@@ -53,8 +53,8 @@ FTMotion ftMotion;
 ft_config_t FTMotion::cfg;
 bool FTMotion::busy; // = false
 
-XYZEval<millis_t> FTMotion::axis_move_end_ti = { 0 };
-AxisBits FTMotion::axis_move_dir;
+AxisBits FTMotion::moving_axis_flags,           // These axes are moving in the planner block being processed
+         FTMotion::axis_move_dir;               // ...in these directions
 
 // Private variables.
 
@@ -157,7 +157,10 @@ void FTMotion::loop() {
   fill_stepper_plan_buffer();
 
   // Set busy status for use by planner.busy()
+  const bool oldBusy = busy;
   busy = stepping.bresenham_iterations_pending > 0 || !stepper_plan_is_empty();
+  if (oldBusy && !busy) moving_axis_flags.reset();
+
 }
 
 #if HAS_FTM_SHAPING
@@ -211,7 +214,7 @@ void FTMotion::reset() {
   TERN_(HAS_EXTRUDERS, prev_traj_e = 0.0f);  // Reset linear advance variables.
   TERN_(DISTINCT_E_FACTORS, block_extruder_axis = E_AXIS);
 
-  axis_move_end_ti.reset();
+  moving_axis_flags.reset();
 
   if (did_suspend) stepper.wake_up();
 }
@@ -348,21 +351,20 @@ bool FTMotion::plan_next_block() {
 
     TERN_(FTM_HAS_LIN_ADVANCE, use_advance_lead = current_block->use_advance_lead);
 
-    // Watch endstops until the move ends
-    const millis_t move_end_ti = millis() + \
-      stepper_plan_count() * FTM_TS + // Time to empty stepper command buffer
-      SEC_TO_MS(currentGenerator->getTotalDuration()) + // Time to finish this block
-      SEC_TO_MS((FTM_TS) * calc_runout_samples()); // Time for a rounout block
-
     #define _SET_MOVE_END(A) do{ \
       if (moveDist.A) { \
-        axis_move_end_ti.A = move_end_ti; \
+        moving_axis_flags.A = true; \
         axis_move_dir.A = moveDist.A > 0; \
       } \
     }while(0);
 
     LOGICAL_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 marks the block for discard on the next ISR.
+    endstops.update();
+
     return true;
   }
 }

Marlin/src/module/ft_motion.h

@@ -135,8 +135,8 @@ class FTMotion {
       reset();
     }
 
-    static XYZEval<millis_t> axis_move_end_ti;
-    static AxisBits axis_move_dir;
+    static AxisBits moving_axis_flags,                    // These axes are moving in the planner block being processed
+                    axis_move_dir;                        // ...in these directions
 
     // Public methods
     static void init();
@@ -169,7 +169,7 @@ class FTMotion {
     static TrajectoryType getTrajectoryType() { return trajectoryType; }
 
     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);
+      return cfg.active ? moving_axis_flags[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];

Marlin/src/module/probe.cpp

@@ -595,6 +595,7 @@ bool Probe::set_deployed(const bool deploy, const bool no_return/*=false*/) {
   if (!no_return) do_blocking_move_to(old_xy); // Return to the original location unless handled externally
 
   endstops.enable_z_probe(deploy);
+
   return false;
 }
 

Marlin/src/module/probe.h

@@ -46,7 +46,8 @@
 #endif
 
 #if ENABLED(BD_SENSOR)
-  #define PROBE_READ() bdp_state
+  #include "endstops.h"
+  #define PROBE_READ() endstops.bdp_state
 #elif USE_Z_MIN_PROBE
   #define PROBE_READ() READ(Z_MIN_PROBE_PIN)
 #else

Marlin/src/module/stepper.cpp

@@ -3337,14 +3337,17 @@ void Stepper::init() {
  * derive the current XYZE position later on.
  */
 void Stepper::_set_position(const abce_long_t &spos) {
-  #if ENABLED(INPUT_SHAPING_X)
-    const int32_t x_shaping_delta = count_position.x - shaping_x.last_block_end_pos;
-  #endif
-  #if ENABLED(INPUT_SHAPING_Y)
-    const int32_t y_shaping_delta = count_position.y - shaping_y.last_block_end_pos;
-  #endif
-  #if ENABLED(INPUT_SHAPING_Z)
-    const int32_t z_shaping_delta = count_position.z - shaping_z.last_block_end_pos;
+  #if HAS_ZV_SHAPING
+    const bool ftMotionActive = TERN0(FT_MOTION, ftMotion.cfg.active);
+    #if ENABLED(INPUT_SHAPING_X)
+      const int32_t x_shaping_delta = ftMotionActive ? 0 : count_position.x - shaping_x.last_block_end_pos;
+    #endif
+    #if ENABLED(INPUT_SHAPING_Y)
+      const int32_t y_shaping_delta = ftMotionActive ? 0 : count_position.y - shaping_y.last_block_end_pos;
+    #endif
+    #if ENABLED(INPUT_SHAPING_Z)
+      const int32_t z_shaping_delta = ftMotionActive ? 0 : count_position.z - shaping_z.last_block_end_pos;
+    #endif
   #endif
 
   #if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX)
@@ -3541,13 +3544,11 @@ void Stepper::report_positions() {
 #if ENABLED(FT_MOTION)
 
   /**
-   * Run stepping from the Stepper ISR at regular short intervals.
+   * Run stepping for FT Motion from the Stepper ISR at regular short intervals.
    *
-   * - Set ftMotion.sts_stepperBusy state to reflect whether there are any commands in the circular buffer.
-   * - If there are no commands in the buffer, return.
-   * - Get the next command from the circular buffer ftMotion.stepperCmdBuff[].
-   * - If the block is being aborted, return without processing the command.
-   * - Apply STEP/DIR along with any delays required. A command may be empty, with no STEP/DIR.
+   * - If there are no STEP commands in the buffer, return.
+   * - Update the last_direction_bits for all stepping axes.
+   * - Apply STEP/DIR along with any delays required.
    */
   void Stepper::ftMotion_stepper() {
     AxisBits &step_bits = ftMotion.stepping.step_bits;            // Aliases for prettier code

buildroot/tests/mega2560

@@ -65,7 +65,7 @@ opt_set MOTHERBOARD BOARD_AZTEEG_X3_PRO NUM_SERVOS 1 \
         FIL_RUNOUT3_STATE HIGH FILAMENT_RUNOUT_SCRIPT '"M600 T%c"'
 opt_enable VIKI2 BOOT_MARLIN_LOGO_ANIMATED SDSUPPORT AUTO_REPORT_SD_STATUS \
            Z_PROBE_SERVO_NR Z_SERVO_ANGLES Z_SERVO_MEASURE_ANGLE DEACTIVATE_SERVOS_AFTER_MOVE Z_SERVO_DEACTIVATE_AFTER_STOW \
-           AUTO_BED_LEVELING_3POINT DEBUG_LEVELING_FEATURE PROBE_PT_1 PROBE_PT_2 PROBE_PT_3 \
+           AUTO_BED_LEVELING_3POINT DEBUG_LEVELING_FEATURE PROBE_PT_1 PROBE_PT_2 PROBE_PT_3 PROBE_WAKEUP_TIME_MS \
            EEPROM_SETTINGS EEPROM_CHITCHAT M114_DETAIL AUTO_REPORT_POSITION \
            NO_VOLUMETRICS EXTENDED_CAPABILITIES_REPORT AUTO_REPORT_TEMPERATURES AUTOTEMP G38_PROBE_TARGET JOYSTICK \
            DIRECT_STEPPING DETECT_BROKEN_ENDSTOP \