Skip to content

Commit

Permalink
⚡️ Optimize FT_MOTION (MarlinFirmware#26557)
Browse files Browse the repository at this point in the history
  • Loading branch information
@narno2202
narno2202 authored Dec 29, 2023
1 parent 1aeee2c commit ba91fa0
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 52 deletions.
91 changes: 44 additions & 47 deletions Marlin/src/module/ft_motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand All @@ -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.
Expand Down Expand Up @@ -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
Expand All @@ -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.

Expand Down Expand Up @@ -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))
Expand All @@ -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),
Expand All @@ -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;
Expand Down Expand Up @@ -443,7 +441,6 @@ void FTMotion::reset() {
stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;

traj.reset();
trajWin.reset();

blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
batchRdy = batchRdyForInterp = false;
Expand Down Expand Up @@ -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

Expand All @@ -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;
Expand All @@ -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

Expand All @@ -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;
Expand Down
5 changes: 2 additions & 3 deletions Marlin/src/module/ft_motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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.
Expand Down
2 changes: 0 additions & 2 deletions Marlin/src/module/ft_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down

0 comments on commit ba91fa0

Please sign in to comment.