diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index 674b66f581cb..0e1ade69f0fb 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/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; diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index f4a65fdd5e92..8c8098873f42 100644 --- a/Marlin/src/module/ft_motion.h +++ b/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. diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h index 06680488754f..e7bcf37ac688 100644 --- a/Marlin/src/module/ft_types.h +++ b/Marlin/src/module/ft_types.h @@ -49,11 +49,9 @@ enum dynFreqMode_t : uint8_t { #if ENABLED(FTM_UNIFIED_BWS) typedef struct XYZEarray xyze_trajectory_t; typedef struct XYZEarray xyze_trajectoryMod_t; - typedef struct XYZEarray xyze_trajectoryWin_t; #else typedef struct XYZEarray xyze_trajectory_t; typedef struct XYZEarray xyze_trajectoryMod_t; - typedef struct XYZEarray xyze_trajectoryWin_t; #endif enum {