🐛 Refine FT Motion, I2S Stepping (MarlinFirmware#26628)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
smiksky · Jan 8, 2024 · b106f59 · b106f59
1 parent 38f483c
commit b106f59
Show file tree

Hide file tree

Showing 15 changed files with 133 additions and 72 deletions.
diff --git a/Marlin/src/HAL/ESP32/i2s.cpp b/Marlin/src/HAL/ESP32/i2s.cpp
@@ -34,6 +34,10 @@
 #include <freertos/queue.h>
 #include "../../module/stepper.h"
 
+#if ENABLED(FT_MOTION)
+  #include "../../module/ft_motion.h"
+#endif
+
 #define DMA_BUF_COUNT 8                                // number of DMA buffers to store data
 #define DMA_BUF_LEN   4092                             // maximum size in bytes
 #define I2S_SAMPLE_SIZE 4                              // 4 bytes, 32 bits per sample
@@ -134,8 +138,8 @@ static void IRAM_ATTR i2s_intr_handler_default(void *arg) {
 
   if (high_priority_task_awoken == pdTRUE) portYIELD_FROM_ISR();
 
-  // clear interrupt
-  I2S0.int_clr.val = I2S0.int_st.val; //clear pending interrupt
+  // Clear pending interrupt
+  I2S0.int_clr.val = I2S0.int_st.val;
 }
 
 void stepperTask(void *parameter) {
@@ -148,29 +152,43 @@ void stepperTask(void *parameter) {
     xQueueReceive(dma.queue, &dma.current, portMAX_DELAY);
     dma.rw_pos = 0;
 
+    const bool using_ftMotion = TERN0(FT_MOTION, ftMotion.cfg.mode);
+
     while (dma.rw_pos < DMA_SAMPLE_COUNT) {
-      if (!nextMainISR) {
-        Stepper::pulse_phase_isr();
-        nextMainISR = Stepper::block_phase_isr();
-      }
-      #if ENABLED(LIN_ADVANCE)
-        else if (!nextAdvanceISR) {
-          Stepper::advance_isr();
-          nextAdvanceISR = Stepper::la_interval;
-        }
-      #endif
-      else
-        i2s_push_sample();
 
-      nextMainISR--;
+      #if ENABLED(FT_MOTION)
 
-      #if ENABLED(LIN_ADVANCE)
-        if (nextAdvanceISR == Stepper::LA_ADV_NEVER)
-          nextAdvanceISR = Stepper::la_interval;
+        if (using_ftMotion) {
+          if (!nextMainISR) stepper.ftMotion_stepper();
+          nextMainISR = 0;
+        }
 
-        if (nextAdvanceISR && nextAdvanceISR != Stepper::LA_ADV_NEVER)
-          nextAdvanceISR--;
       #endif
+
+      if (!using_ftMotion) {
+        if (!nextMainISR) {
+          Stepper::pulse_phase_isr();
+          nextMainISR = Stepper::block_phase_isr();
+        }
+        #if ENABLED(LIN_ADVANCE)
+          else if (!nextAdvanceISR) {
+            Stepper::advance_isr();
+            nextAdvanceISR = Stepper::la_interval;
+          }
+        #endif
+        else
+          i2s_push_sample();
+
+        nextMainISR--;
+
+        #if ENABLED(LIN_ADVANCE)
+          if (nextAdvanceISR == Stepper::LA_ADV_NEVER)
+            nextAdvanceISR = Stepper::la_interval;
+
+          if (nextAdvanceISR && nextAdvanceISR != Stepper::LA_ADV_NEVER)
+            nextAdvanceISR--;
+        #endif
+      }
     }
   }
 }

diff --git a/Marlin/src/inc/Conditionals_adv.h b/Marlin/src/inc/Conditionals_adv.h
@@ -1309,6 +1309,12 @@
   #define HAS_ZV_SHAPING 1
 #endif
 
+// FT Motion unified window and batch size
+#if ALL(FT_MOTION, FTM_UNIFIED_BWS)
+  #define FTM_WINDOW_SIZE FTM_BW_SIZE
+  #define FTM_BATCH_SIZE  FTM_BW_SIZE
+#endif
+
 // Toolchange Event G-code
 #if !HAS_MULTI_EXTRUDER || !(defined(EVENT_GCODE_TOOLCHANGE_T0) || defined(EVENT_GCODE_TOOLCHANGE_T1) || defined(EVENT_GCODE_TOOLCHANGE_T2) || defined(EVENT_GCODE_TOOLCHANGE_T3) || defined(EVENT_GCODE_TOOLCHANGE_T4) || defined(EVENT_GCODE_TOOLCHANGE_T5) || defined(EVENT_GCODE_TOOLCHANGE_T6) || defined(EVENT_GCODE_TOOLCHANGE_T7))
   #undef TC_GCODE_USE_GLOBAL_X

diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h
@@ -4123,8 +4123,12 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive."
 /**
  * Fixed-Time Motion limitations
  */
-#if ALL(FT_MOTION, MIXING_EXTRUDER)
-  #error "FT_MOTION does not currently support MIXING_EXTRUDER."
+#if ENABLED(FT_MOTION)
+  #if ENABLED(MIXING_EXTRUDER)
+    #error "FT_MOTION does not currently support MIXING_EXTRUDER."
+  #elif DISABLED(FTM_UNIFIED_BWS)
+    #error "FT_MOTION requires FTM_UNIFIED_BWS to be enabled because FBS is not yet implemented."
+  #endif
 #endif
 
 // Multi-Stepping Limit

diff --git a/Marlin/src/inc/Warnings.cpp b/Marlin/src/inc/Warnings.cpp
@@ -826,3 +826,10 @@
 #if PIN_EXISTS(BEEPER) && ALL(SPEAKER, NO_SPEAKER)
   #warning "The BEEPER cannot produce tones so you can disable SPEAKER."
 #endif
+
+/**
+ * Fixed-Time Motion
+ */
+#if ALL(FT_MOTION, I2S_STEPPER_STREAM)
+  #warning "FT_MOTION has not been tested with I2S_STEPPER_STREAM."
+#endif
diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp
@@ -55,8 +55,8 @@ FTMotion ftMotion;
 ft_config_t FTMotion::cfg;
 bool FTMotion::busy; // = false
 ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepper commands buffer.
-uint32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer.
-         FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
+int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer.
+        FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
 
 bool FTMotion::sts_stepperBusy = false;         // The stepper buffer has items and is in use.
 
@@ -123,6 +123,8 @@ uint32_t FTMotion::interpIdx = 0,               // Index of current data point b
   float FTMotion::e_advanced_z1 = 0.0f;   // (ms) Unit delay of advanced extruder position.
 #endif
 
+constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
+
 //-----------------------------------------------------------------
 // Function definitions.
 //-----------------------------------------------------------------
@@ -145,8 +147,16 @@ void FTMotion::runoutBlock() {
   ratio.reset();
 
   max_intervals = cfg.modeHasShaper() ? shaper_intervals : 0;
-  if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, min_max_intervals - (FTM_BATCH_SIZE))) max_intervals = min_max_intervals;
-  max_intervals += TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_WINDOW_SIZE) - makeVector_batchIdx;
+  if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BATCH_SIZE, min_max_intervals - (FTM_BATCH_SIZE)))
+    max_intervals = min_max_intervals;
+
+  max_intervals += (
+    #if ENABLED(FTM_UNIFIED_BWS)
+      FTM_WINDOW_SIZE - makeVector_batchIdx
+    #else
+      FTM_WINDOW_SIZE - ((last_batchIdx < (FTM_BATCH_SIZE)) ? 0 : makeVector_batchIdx)
+    #endif
+  );
   blockProcRdy = blockDataIsRunout = true;
   runoutEna = blockProcDn = false;
 }
@@ -198,7 +208,7 @@ void FTMotion::loop() {
       );
 
       // Shift the time series back in the window
-      #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], (FTM_WINDOW_SIZE - FTM_BATCH_SIZE) * sizeof(traj.A[0]))
+      #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], last_batchIdx * sizeof(traj.A[0]))
       LOGICAL_AXIS_CODE(
         TSHIFT(e),
         TSHIFT(x), TSHIFT(y), TSHIFT(z),
@@ -219,7 +229,7 @@ void FTMotion::loop() {
     && (interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP))
   ) {
     convertToSteps(interpIdx);
-    if (++interpIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE)) {
+    if (++interpIdx == FTM_BATCH_SIZE) {
       batchRdyForInterp = false;
       interpIdx = 0;
     }
@@ -449,7 +459,7 @@ void FTMotion::reset() {
   endPosn_prevBlock.reset();
 
   makeVector_idx = makeVector_idx_z1 = 0;
-  makeVector_batchIdx = 0;
+  makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MAX(last_batchIdx, FTM_BATCH_SIZE));
 
   steps.reset();
   interpIdx = interpIdx_z1 = 0;
@@ -464,10 +474,11 @@ void FTMotion::reset() {
 }
 
 // Private functions.
+
 // Auxiliary function to get number of step commands in the buffer.
-uint32_t FTMotion::stepperCmdBuffItems() {
-  const uint32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx;
-  return stepperCmdBuff_produceIdx < stepperCmdBuff_consumeIdx ? (FTM_STEPPERCMD_BUFF_SIZE) + udiff : udiff;
+int32_t FTMotion::stepperCmdBuffItems() {
+  const int32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx;
+  return (udiff < 0) ? udiff + (FTM_STEPPERCMD_BUFF_SIZE) : udiff;
 }
 
 // Initializes storage variables before startup.
@@ -677,8 +688,8 @@ void FTMotion::makeVector() {
   #endif
 
   // Filled up the queue with regular and shaped steps
-  if (++makeVector_batchIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE))) {
-    makeVector_batchIdx = 0;
+  if (++makeVector_batchIdx == FTM_WINDOW_SIZE) {
+    makeVector_batchIdx = last_batchIdx;
     batchRdy = true;
   }
 

diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h
@@ -102,12 +102,11 @@ class FTMotion {
     }
 
     static ft_command_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of stepper commands.
-    static uint32_t stepperCmdBuff_produceIdx,            // Index of next stepper command write to the buffer.
-                    stepperCmdBuff_consumeIdx;            // Index of next stepper command read from the buffer.
+    static int32_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 init();
     static void startBlockProc();                         // Set controller states to begin processing a block.
@@ -153,10 +152,10 @@ class FTMotion {
     static uint32_t N1, N2, N3;
     static uint32_t max_intervals;
 
-    static constexpr uint32_t _ftm_size = TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE),
-                              _ftm_wind = TERN(FTM_UNIFIED_BWS, 2, CEIL((FTM_WINDOW_SIZE) / _ftm_size)),
-                              shaper_intervals = _ftm_size * CEIL((FTM_ZMAX) / _ftm_size),
-                              min_max_intervals = _ftm_size * _ftm_wind;
+    #define _DIVCEIL(A,B) (((A) + (B) - 1) / (B))
+    static constexpr uint32_t _ftm_ratio = TERN(FTM_UNIFIED_BWS, 2, _DIVCEIL(FTM_WINDOW_SIZE, FTM_BATCH_SIZE)),
+                              shaper_intervals = (FTM_BATCH_SIZE) * _DIVCEIL(FTM_ZMAX, FTM_BATCH_SIZE),
+                              min_max_intervals = (FTM_BATCH_SIZE) * _ftm_ratio;
 
     // Make vector variables.
     static uint32_t makeVector_idx,
@@ -203,7 +202,7 @@ class FTMotion {
     #endif
 
     // Private methods
-    static uint32_t stepperCmdBuffItems();
+    static int32_t stepperCmdBuffItems();
     static void loadBlockData(block_t *const current_block);
     static void makeVector();
     static void convertToSteps(const uint32_t idx);

diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h
@@ -44,13 +44,8 @@ enum dynFreqMode_t : uint8_t {
 
 #define IS_EI_MODE(N) WITHIN(N, ftMotionMode_EI, ftMotionMode_3HEI)
 
-#if ENABLED(FTM_UNIFIED_BWS)
-  typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectory_t;
-  typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryMod_t;
-#else
-  typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
-  typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
-#endif
+typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
+typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
 
 enum {
   LIST_N(DOUBLE(LOGICAL_AXES),

diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
@@ -1508,14 +1508,12 @@ void Stepper::isr() {
     #if ENABLED(FT_MOTION)
 
       if (using_ftMotion) {
-        if (!nextMainISR) {
-          nextMainISR = FTM_MIN_TICKS;
-          ftMotion_stepper();
-          endstops.update();
-          TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_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
         }
-        interval = nextMainISR;
-        nextMainISR -= interval;
+        interval = nextMainISR;           // Interval is either some old nextMainISR or FTM_MIN_TICKS
+        nextMainISR = 0;                  // For FT Motion fire again ASAP
       }
 
     #endif
@@ -3448,7 +3446,8 @@ void Stepper::report_positions() {
     // Use one byte to restore one stepper command in the format:
     // |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction|
     const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];
-    if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE)) ftMotion.stepperCmdBuff_consumeIdx = 0U;
+    if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE))
+      ftMotion.stepperCmdBuff_consumeIdx = 0;
 
     if (abort_current_block) return;
 
@@ -3492,6 +3491,8 @@ void Stepper::report_positions() {
       U_APPLY_STEP(axis_did_move.u, false), V_APPLY_STEP(axis_did_move.v, false), W_APPLY_STEP(axis_did_move.w, false)
     );
 
+    TERN_(I2S_STEPPER_STREAM, i2s_push_sample());
+
     // Begin waiting for the minimum pulse duration
     START_TIMED_PULSE();
 
@@ -3533,6 +3534,12 @@ void Stepper::report_positions() {
       U_APPLY_STEP(!STEP_STATE_U, false), V_APPLY_STEP(!STEP_STATE_V, false), W_APPLY_STEP(!STEP_STATE_W, false)
     );
 
+    // Check endstops on every step
+    IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, endstops.update());
+
+    // Also handle babystepping here
+    TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
+
   } // Stepper::ftMotion_stepper
 
   void Stepper::ftMotion_blockQueueUpdate() {

diff --git a/Marlin/src/pins/esp32/pins_ENWI_ESPNP.h b/Marlin/src/pins/esp32/pins_ENWI_ESPNP.h
@@ -34,7 +34,9 @@
 //
 // I2S (steppers & other output-only pins)
 //
-#define I2S_STEPPER_STREAM
+#ifndef I2S_STEPPER_STREAM
+  #define I2S_STEPPER_STREAM
+#endif
 #if ENABLED(I2S_STEPPER_STREAM)
   #define I2S_WS                              17
   #define I2S_BCK                             22

diff --git a/Marlin/src/pins/esp32/pins_ESP32.h b/Marlin/src/pins/esp32/pins_ESP32.h
@@ -32,7 +32,9 @@
 //
 // I2S (steppers & other output-only pins)
 //
-#define I2S_STEPPER_STREAM
+#ifndef I2S_STEPPER_STREAM
+  #define I2S_STEPPER_STREAM
+#endif
 #if ENABLED(I2S_STEPPER_STREAM)
   #define I2S_WS                              25
   #define I2S_BCK                             26

diff --git a/Marlin/src/pins/esp32/pins_MKS_TINYBEE.h b/Marlin/src/pins/esp32/pins_MKS_TINYBEE.h
@@ -56,7 +56,9 @@
 //
 // Enable I2S stepper stream
 //
-#define I2S_STEPPER_STREAM
+#ifndef I2S_STEPPER_STREAM
+  #define I2S_STEPPER_STREAM
+#endif
 #if ENABLED(I2S_STEPPER_STREAM)
   #define I2S_WS                              26
   #define I2S_BCK                             25

diff --git a/Marlin/src/pins/esp32/pins_MM_JOKER.h b/Marlin/src/pins/esp32/pins_MM_JOKER.h
@@ -85,7 +85,9 @@
 //
 // Enable I2S stepper stream
 //
-#define I2S_STEPPER_STREAM
+#ifndef I2S_STEPPER_STREAM
+  #define I2S_STEPPER_STREAM
+#endif
 #if ENABLED(I2S_STEPPER_STREAM)
   #define I2S_WS                              26
   #define I2S_BCK                             25

diff --git a/Marlin/src/pins/esp32/pins_MRR_ESPE.h b/Marlin/src/pins/esp32/pins_MRR_ESPE.h
@@ -51,7 +51,9 @@
 //
 // Enable I2S stepper stream
 //
-#define I2S_STEPPER_STREAM
+#ifndef I2S_STEPPER_STREAM
+  #define I2S_STEPPER_STREAM
+#endif
 #if ENABLED(I2S_STEPPER_STREAM)
   #define I2S_WS                              26
   #define I2S_BCK                             25

diff --git a/Marlin/src/pins/esp32/pins_RESP32_CUSTOM.h b/Marlin/src/pins/esp32/pins_RESP32_CUSTOM.h
@@ -34,4 +34,6 @@
 //
 // I2S (steppers & other output-only pins)
 //
-#define I2S_STEPPER_STREAM
+#ifndef I2S_STEPPER_STREAM
+  #define I2S_STEPPER_STREAM
+#endif