Skip to content

FT_MOTION : code simplification and cleanup, fix ceil really return ceil #26557

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
thinkyhead merged 7 commits into from
Dec 29, 2023
Merged

FT_MOTION : code simplification and cleanup, fix ceil really return ceil #26557

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
485ad1e
Code simplication of FT_MOTION for unified buffer system. Memory foot…
narno2202 Dec 21, 2023
2e9cd6f
Fix ceil really return ceil, code cleanup
narno2202 Dec 28, 2023
5bef3ef
Code cleanup
narno2202 Dec 28, 2023
91c0659
Code cleanup
narno2202 Dec 28, 2023
2ea4aee
ws
thinkyhead Dec 29, 2023
0fd0801
Merge branch 'bugfix-2.1.x' into pr/26557
thinkyhead Dec 29, 2023
3d8dcc9
merge followup
thinkyhead Dec 29, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
91 changes: 44 additions & 47 deletions Marlin/src/module/ft_motion.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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