⚡️ Optimize FT_MOTION (MarlinFirmware#26557)

Author: narno2202

Marlin/src/module/ft_motion.cpp

@@ -46,9 +46,9 @@ FTMotion ftMotion;
   static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_MASS_BASED, "dynFreqMode_MASS_BASED requires an X axis and an extruder.");
 #endif
 
-//-----------------------------------------------------------------//
+//-----------------------------------------------------------------
 // Variables.
-//-----------------------------------------------------------------//
+//-----------------------------------------------------------------
 
 // Public variables.
 
@@ -61,10 +61,10 @@ uint32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper comma
 bool FTMotion::sts_stepperBusy = false;         // The stepper buffer has items and is in use.
 
 // Private variables.
+
 // NOTE: These are sized for Ulendo FBS use.
-  xyze_trajectory_t FTMotion::traj;               // = {0.0f} Storage for fixed-time-based trajectory.
-  xyze_trajectoryMod_t FTMotion::trajMod;         // = {0.0f} Storage for modified fixed-time-based trajectory.
-  xyze_trajectoryWin_t FTMotion::trajWin;         // = {0.0f} Storage for fixed time trajectory window.
+xyze_trajectory_t    FTMotion::traj;            // = {0.0f} Storage for fixed-time-based trajectory.
+xyze_trajectoryMod_t FTMotion::trajMod;         // = {0.0f} Storage for fixed time trajectory window.
 
 bool FTMotion::blockProcRdy = false,            // Indicates a block is ready to be processed.
      FTMotion::blockProcRdy_z1 = false,         // Storage for the previous indicator.
@@ -96,23 +96,23 @@ uint32_t FTMotion::N1,                          // Number of data points in the
 uint32_t FTMotion::max_intervals;               // Total number of data points that will be generated from block.
 
 // Make vector variables.
-uint32_t FTMotion::makeVector_idx = 0,                    // Index of fixed time trajectory generation of the overall block.
-         FTMotion::makeVector_idx_z1 = 0,                 // Storage for the previously calculated index above.
-         FTMotion::makeVector_batchIdx = 0;               // Index of fixed time trajectory generation within the batch.
+uint32_t FTMotion::makeVector_idx = 0,          // Index of fixed time trajectory generation of the overall block.
+         FTMotion::makeVector_idx_z1 = 0,       // Storage for the previously calculated index above.
+         FTMotion::makeVector_batchIdx = 0;     // Index of fixed time trajectory generation within the batch.
 
 // Interpolation variables.
-xyze_long_t FTMotion::steps = { 0 };                  // Step count accumulator.
+xyze_long_t FTMotion::steps = { 0 };            // Step count accumulator.
 
-uint32_t FTMotion::interpIdx = 0,                     // Index of current data point being interpolated.
-         FTMotion::interpIdx_z1 = 0;                  // Storage for the previously calculated index above.
+uint32_t FTMotion::interpIdx = 0,               // Index of current data point being interpolated.
+         FTMotion::interpIdx_z1 = 0;            // Storage for the previously calculated index above.
 
 // Shaping variables.
 #if HAS_X_AXIS
   FTMotion::shaping_t FTMotion::shaping = {
     0, 0,
-    x:{ { 0.0f }, { 0.0f }, { 0 } },                  // d_zi, Ai, Ni
+    x:{ { 0.0f }, { 0.0f }, { 0 } },            // d_zi, Ai, Ni
     #if HAS_Y_AXIS
-      y:{ { 0.0f }, { 0.0f }, { 0 } }                 // d_zi, Ai, Ni
+      y:{ { 0.0f }, { 0.0f }, { 0 } }           // d_zi, Ai, Ni
     #endif
   };
 #endif
@@ -123,9 +123,9 @@ uint32_t FTMotion::interpIdx = 0,                     // Index of current data p
   float FTMotion::e_advanced_z1 = 0.0f;   // (ms) Unit delay of advanced extruder position.
 #endif
 
-//-----------------------------------------------------------------//
+//-----------------------------------------------------------------
 // Function definitions.
-//-----------------------------------------------------------------//
+//-----------------------------------------------------------------
 
 // Public functions.
 
@@ -186,8 +186,7 @@ void FTMotion::loop() {
     // Call Ulendo FBS here.
 
     #if ENABLED(FTM_UNIFIED_BWS)
-      trajMod = traj; // Copy the uncompensated vectors.
-      traj = trajWin; // Move the window to traj
+      trajMod = traj; // Move the window to traj
     #else
       // Copy the uncompensated vectors.
       #define TCOPY(A) memcpy(trajMod.A, traj.A, sizeof(trajMod.A))
@@ -199,7 +198,7 @@ void FTMotion::loop() {
       );
 
       // Shift the time series back in the window
-      #define TSHIFT(A) memcpy(traj.A, trajWin.A, sizeof(trajWin.A))
+      #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], (FTM_WINDOW_SIZE - FTM_BATCH_SIZE) * sizeof(traj.A[0]))
       LOGICAL_AXIS_CODE(
         TSHIFT(e),
         TSHIFT(x), TSHIFT(y), TSHIFT(z),
@@ -215,12 +214,11 @@ void FTMotion::loop() {
   }
 
   // Interpolation.
-  while ( batchRdyForInterp
-          && ( stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME) )
-          && ( interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP) )
+  while (batchRdyForInterp
+    && (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))
+    && (interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP))
   ) {
     convertToSteps(interpIdx);
-
     if (++interpIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE)) {
       batchRdyForInterp = false;
       interpIdx = 0;
@@ -443,7 +441,6 @@ void FTMotion::reset() {
   stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;
 
   traj.reset();
-  trajWin.reset();
 
   blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
   batchRdy = batchRdyForInterp = false;
@@ -611,26 +608,26 @@ void FTMotion::makeVector() {
   }
 
   LOGICAL_AXIS_CODE(
-    trajWin.e[makeVector_batchIdx] = startPosn.e + ratio.e * dist,
-    trajWin.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist,
-    trajWin.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist,
-    trajWin.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist,
-    trajWin.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist,
-    trajWin.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist,
-    trajWin.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist,
-    trajWin.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist,
-    trajWin.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist,
-    trajWin.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist
+    traj.e[makeVector_batchIdx] = startPosn.e + ratio.e * dist,
+    traj.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist,
+    traj.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist,
+    traj.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist,
+    traj.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist,
+    traj.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist,
+    traj.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist,
+    traj.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist,
+    traj.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist,
+    traj.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist
   );
 
   #if HAS_EXTRUDERS
     if (cfg.linearAdvEna) {
-      float dedt_adj = (trajWin.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS);
+      float dedt_adj = (traj.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS);
       if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK;
 
-      e_raw_z1 = trajWin.e[makeVector_batchIdx];
+      e_raw_z1 = traj.e[makeVector_batchIdx];
       e_advanced_z1 += dedt_adj * (FTM_TS);
-      trajWin.e[makeVector_batchIdx] = e_advanced_z1;
+      traj.e[makeVector_batchIdx] = e_advanced_z1;
     }
   #endif
 
@@ -640,9 +637,9 @@ void FTMotion::makeVector() {
 
     #if HAS_DYNAMIC_FREQ_MM
       case dynFreqMode_Z_BASED:
-        if (trajWin.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed.
-                 const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * trajWin.z[makeVector_batchIdx]
-          OPTARG(HAS_Y_AXIS, yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * trajWin.z[makeVector_batchIdx]);
+        if (traj.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed.
+                 const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx]
+          OPTARG(HAS_Y_AXIS, yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx]);
           updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ) OPTARG(HAS_Y_AXIS, _MAX(yf, FTM_MIN_SHAPE_FREQ)));
         }
         break;
@@ -652,8 +649,8 @@ void FTMotion::makeVector() {
       case dynFreqMode_MASS_BASED:
         // Update constantly. The optimization done for Z value makes
         // less sense for E, as E is expected to constantly change.
-        updateShapingN(      cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * trajWin.e[makeVector_batchIdx]
-          OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * trajWin.e[makeVector_batchIdx]) );
+        updateShapingN(      cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx]
+          OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx]) );
         break;
     #endif
 
@@ -663,18 +660,18 @@ void FTMotion::makeVector() {
   // Apply shaping if in mode.
   #if HAS_X_AXIS
     if (cfg.modeHasShaper()) {
-      shaping.x.d_zi[shaping.zi_idx] = trajWin.x[makeVector_batchIdx];
-      trajWin.x[makeVector_batchIdx] *= shaping.x.Ai[0];
+      shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx];
+      traj.x[makeVector_batchIdx] *= shaping.x.Ai[0];
       #if HAS_Y_AXIS
-        shaping.y.d_zi[shaping.zi_idx] = trajWin.y[makeVector_batchIdx];
-        trajWin.y[makeVector_batchIdx] *= shaping.y.Ai[0];
+        shaping.y.d_zi[shaping.zi_idx] = traj.y[makeVector_batchIdx];
+        traj.y[makeVector_batchIdx] *= shaping.y.Ai[0];
       #endif
       for (uint32_t i = 1U; i <= shaping.max_i; i++) {
         const uint32_t udiffx = shaping.zi_idx - shaping.x.Ni[i];
-        trajWin.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
+        traj.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
         #if HAS_Y_AXIS
           const uint32_t udiffy = shaping.zi_idx - shaping.y.Ni[i];
-          trajWin.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
+          traj.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
         #endif
       }
       if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0;

Marlin/src/module/ft_motion.h

@@ -134,7 +134,6 @@ class FTMotion {
 
     static xyze_trajectory_t traj;
     static xyze_trajectoryMod_t trajMod;
-    static xyze_trajectoryWin_t trajWin;
 
     static bool blockProcRdy, blockProcRdy_z1, blockProcDn;
     static bool batchRdy, batchRdyForInterp;
@@ -155,8 +154,8 @@ class FTMotion {
     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),
+                              _ftm_wind = TERN(FTM_UNIFIED_BWS, 2, CEIL((FTM_WINDOW_SIZE) / _ftm_size)),
+                              shaper_intervals = _ftm_size * CEIL((FTM_ZMAX) / _ftm_size),
                               min_max_intervals = _ftm_size * _ftm_wind;
 
     // Make vector variables.

Marlin/src/module/ft_types.h

@@ -49,11 +49,9 @@ enum dynFreqMode_t : uint8_t {
 #if ENABLED(FTM_UNIFIED_BWS)
   typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectory_t;
   typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryMod_t;
-  typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryWin_t;
 #else
   typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
   typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
-  typedef struct XYZEarray<float, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE)> xyze_trajectoryWin_t;
 #endif
 
 enum {