sravan/Marlin-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

⚡️ Optimize FT_MOTION (#26557)

Browse Source
This commit is contained in:
narno2202
2023-12-29 05:41:34 +01:00
committed by GitHub
parent 1aeee2cd1f
commit ba91fa09b7
3 changed files with 46 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
Marlin/src/module/ft_motion.cpp
View File

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

5
Marlin/src/module/ft_motion.h
View File

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

2
Marlin/src/module/ft_types.h
View File

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