diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index e459b244fc..d5865913ab 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -42,7 +42,7 @@ ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepp int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer. FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer. -bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use. +bool FTMotion::stepperCmdBuffHasData = false; // The stepper buffer has items and is in use. XYZEval FTMotion::axis_move_end_ti = { 0 }; AxisBits FTMotion::axis_move_dir; @@ -64,9 +64,9 @@ bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done b // ... if applicable, and is ready to be converted to step commands. // Trapezoid data variables. -xyze_pos_t FTMotion::startPosn, // (mm) Start position of block - FTMotion::endPosn_prevBlock = { 0.0f }; // (mm) End position of previous block -xyze_float_t FTMotion::ratio; // (ratio) Axis move ratio of block +xyze_pos_t FTMotion::startPos, // (mm) Start position of block + FTMotion::endPos_prevBlock = { 0.0f }; // (mm) End position of previous block +xyze_float_t FTMotion::ratio; // (ratio) Axis move ratio of block float FTMotion::accel_P, // Acceleration prime of block. [mm/sec/sec] FTMotion::decel_P, // Deceleration prime of block. [mm/sec/sec] FTMotion::F_P, // Feedrate prime of block. [mm/sec] @@ -135,7 +135,7 @@ void FTMotion::loop() { * 4. Signal ready for new block. */ if (stepper.abort_current_block) { - if (sts_stepperBusy) return; // Wait until motion buffers are emptied + if (stepperCmdBuffHasData) return; // Wait until motion buffers are emptied discard_planner_block_protected(); reset(); stepper.abort_current_block = false; // Abort finished. @@ -149,13 +149,13 @@ void FTMotion::loop() { continue; } loadBlockData(stepper.current_block); + blockProcRdy = true; // If the endstop is already pressed, endstop interrupts won't invoke // endstop_triggered and the move will grind. So check here for a // triggered endstop, which shortly marks the block for discard. endstops.update(); - blockProcRdy = true; // Some kinematics track axis motion in HX, HY, HZ #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX) stepper.last_direction_bits.hx = stepper.current_block->direction_bits.hx; @@ -170,18 +170,14 @@ void FTMotion::loop() { if (blockProcRdy) { - if (!batchRdy) makeVector(); // Caution: Do not consolidate checks on blockProcRdy/batchRdy, as they are written by makeVector(). - // When makeVector is finished: either blockProcRdy has been set false (because the block is - // done being processed) or batchRdy is set true, or both. + if (!batchRdy) makeVector(); // may clear blockProcRdy // Check if the block has been completely converted: if (!blockProcRdy) { discard_planner_block_protected(); - - // Check if the block needs to be runout: if (!batchRdy && !planner.has_blocks_queued()) { runoutBlock(); - makeVector(); // Do an additional makeVector call to guarantee batchRdy set this loop. + makeVector(); // Additional call to guarantee batchRdy is set this loop. } } } @@ -220,7 +216,7 @@ void FTMotion::loop() { } // Report busy status to planner. - busy = (sts_stepperBusy || blockProcRdy || batchRdy || batchRdyForInterp); + busy = (stepperCmdBuffHasData || blockProcRdy || batchRdy || batchRdyForInterp); } @@ -386,7 +382,7 @@ void FTMotion::reset() { blockProcRdy = batchRdy = batchRdyForInterp = false; - endPosn_prevBlock.reset(); + endPos_prevBlock.reset(); makeVector_idx = 0; makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MIN(BATCH_SIDX_IN_WINDOW, FTM_BATCH_SIZE)); @@ -424,13 +420,13 @@ void FTMotion::discard_planner_block_protected() { /** * Set up a pseudo block to allow motion to settle and buffers to empty. * Called when the planner has one block left. The buffers will be filled - * with the last commanded position by setting the startPosn block variable to + * with the last commanded position by setting the startPos block variable to * the last position of the previous block and all ratios to zero such that no * axes' positions are incremented. */ void FTMotion::runoutBlock() { - startPosn = endPosn_prevBlock; + startPos = endPos_prevBlock; ratio.reset(); const int32_t n_to_fill_batch = (FTM_WINDOW_SIZE) - makeVector_batchIdx; @@ -466,7 +462,7 @@ void FTMotion::loadBlockData(block_t * const current_block) { const float totalLength = current_block->millimeters, oneOverLength = 1.0f / totalLength; - startPosn = endPosn_prevBlock; + startPos = endPos_prevBlock; const xyze_pos_t& moveDist = current_block->dist_mm; @@ -551,7 +547,7 @@ void FTMotion::loadBlockData(block_t * const current_block) { // Accel + Coasting + Decel datapoints max_intervals = N1 + N2 + N3; - endPosn_prevBlock += moveDist; + endPos_prevBlock += moveDist; // Watch endstops until the move ends const millis_t move_end_ti = millis() + SEC_TO_MS((FTM_TS) * float(max_intervals + num_samples_shaper_settle() + ((PROP_BATCHES) + 1) * (FTM_BATCH_SIZE)) + (float(FTM_STEPPERCMD_BUFF_SIZE) / float(FTM_STEPPER_FS))); @@ -592,7 +588,7 @@ void FTMotion::makeVector() { TERN_(HAS_EXTRUDERS, accel_k = decel_P); // (mm/s^2) Acceleration K factor from Decel phase } - #define _SET_TRAJ(q) traj.q[makeVector_batchIdx] = startPosn.q + ratio.q * dist; + #define _SET_TRAJ(q) traj.q[makeVector_batchIdx] = startPos.q + ratio.q * dist; LOGICAL_AXIS_MAP_LC(_SET_TRAJ); #if HAS_EXTRUDERS diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index feaea55ca1..9d3d5c9833 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -113,7 +113,7 @@ class FTMotion { static int32_t stepperCmdBuff_produceIdx, // Index of next stepper command write to the buffer. stepperCmdBuff_consumeIdx; // Index of next stepper command read from the buffer. - static bool sts_stepperBusy; // The stepper buffer has items and is in use. + static bool stepperCmdBuffHasData; // The stepper buffer has items and is in use. static XYZEval axis_move_end_ti; static AxisBits axis_move_dir; @@ -145,9 +145,9 @@ class FTMotion { static bool batchRdy, batchRdyForInterp; // Trapezoid data variables. - static xyze_pos_t startPosn, // (mm) Start position of block - endPosn_prevBlock; // (mm) End position of previous block - static xyze_float_t ratio; // (ratio) Axis move ratio of block + static xyze_pos_t startPos, // (mm) Start position of block + endPos_prevBlock; // (mm) End position of previous block + static xyze_float_t ratio; // (ratio) Axis move ratio of block static float accel_P, decel_P, F_P, f_s, diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 8ad79aad1b..f2698b3da5 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -3565,8 +3565,8 @@ void Stepper::report_positions() { void Stepper::ftMotion_stepper() { // Check if the buffer is empty. - ftMotion.sts_stepperBusy = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx); - if (!ftMotion.sts_stepperBusy) return; + ftMotion.stepperCmdBuffHasData = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx); + if (!ftMotion.stepperCmdBuffHasData) return; // "Pop" one command from current motion buffer const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];