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

🩹⚡️ Fix judder, optimize planner (#27035)

Browse Source
This commit is contained in:
Mihai
2024-07-01 21:56:06 +03:00
committed by GitHub
parent c81416438d
commit 6fed66dd42
2 changed files with 125 additions and 209 deletions
Download Patch File Download Diff File Expand all files Collapse all files

323
Marlin/src/module/planner.cpp
View File

@@ -128,7 +128,6 @@ Planner planner;
block_t Planner::block_buffer[BLOCK_BUFFER_SIZE]; block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
Planner::block_buffer_nonbusy, // Index of the first non-busy block Planner::block_buffer_nonbusy, // Index of the first non-busy block
Planner::block_buffer_planned, // Index of the optimally planned block
Planner::block_buffer_tail; // Index of the busy block, if any Planner::block_buffer_tail; // Index of the busy block, if any
uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks
uint8_t Planner::delay_before_delivering; // Delay block delivery so initial blocks in an empty queue may merge uint8_t Planner::delay_before_delivering; // Delay block delivery so initial blocks in an empty queue may merge
@@ -768,10 +767,6 @@ block_t* Planner::get_current_block() {
// As this block is busy, advance the nonbusy block pointer // As this block is busy, advance the nonbusy block pointer
block_buffer_nonbusy = next_block_index(block_buffer_tail); block_buffer_nonbusy = next_block_index(block_buffer_tail);
// Push block_buffer_planned pointer, if encountered.
if (block_buffer_tail == block_buffer_planned)
block_buffer_planned = block_buffer_nonbusy;
// Return the block // Return the block
return block; return block;
} }
@@ -784,29 +779,30 @@ block_t* Planner::get_current_block() {
/** /**
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors. Requires that initial_rate and final_rate are * by the provided factors. If entry_factor is 0 don't change the initial_rate.
* no less than sqrt(block->acceleration_steps_per_s2 / 2), which is ensured * Assumes that the implied initial_rate and final_rate are no less than
* through minimum_planner_speed_sqr in _populate_block(). * sqrt(block->acceleration_steps_per_s2 / 2). This is ensured through
* minimum_planner_speed_sqr / min_entry_speed_sqr though note there's one
* exception in recalculate_trapezoids().
** **
* ############ VERY IMPORTANT ############ * ############ VERY IMPORTANT ############
* NOTE that the PRECONDITION to call this function is that the block is * NOTE that the PRECONDITION to call this function is that the block is
* NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR * NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR
* is not and will not use the block while we modify it, so it is safe to * is not and will not use the block while we modify it.
* alter its values.
*/ */
void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t entry_factor, const_float_t exit_factor) { void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t entry_speed, const_float_t exit_speed) {
uint32_t initial_rate = LROUND(block->nominal_rate * entry_factor), const float spmm = block->steps_per_mm;
final_rate = LROUND(block->nominal_rate * exit_factor); // (steps per second) uint32_t initial_rate = entry_speed ? LROUND(entry_speed * spmm) : block->initial_rate,
final_rate = LROUND(exit_speed * spmm);
// Legacy check against supposed timer overflow. However Stepper::calc_timer_interval() already // Removing code to constrain values produces judder in direction-switching moves because the
// should protect against it. But removing this code produces judder in direction-switching // current discrete stepping math diverges from physical motion under constant acceleration
// moves. This is because the current discrete stepping math diverges from physical motion under // when acceleration_steps_per_s2 is large compared to initial/final_rate.
// constant acceleration when acceleration_steps_per_s2 is large compared to initial/final_rate. NOLESS(initial_rate, long(MINIMAL_STEP_RATE));
NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE)); // Enforce the minimum speed NOLESS(final_rate, long(MINIMAL_STEP_RATE));
NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE)); NOMORE(initial_rate, block->nominal_rate); // NOTE: The nominal rate may be less than MINIMAL_STEP_RATE!
NOMORE(initial_rate, block->nominal_rate); // NOTE: The nominal rate may be less than MINIMAL_STEP_RATE! NOMORE(final_rate, block->nominal_rate);
NOMORE(final_rate, block->nominal_rate);
#if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE) #if ANY(S_CURVE_ACCELERATION, LIN_ADVANCE)
// If we have some plateau time, the cruise rate will be the nominal rate // If we have some plateau time, the cruise rate will be the nominal rate
@@ -942,16 +938,16 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
* *
* Recalculates the motion plan according to the following basic guidelines: * Recalculates the motion plan according to the following basic guidelines:
* *
* 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds * 1. Go over blocks sequentially in reverse order and maximize the entry junction speed:
* (i.e. current->entry_speed) such that: * a. Entry speed should stay below/at the pre-computed maximum junction speed limit
* a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of * b. Aim for the maximum entry speed which is the one reverse-computed from its exit speed
* neighboring blocks. * (next->entry_speed) if assuming maximum deceleration over the full block travel distance
* b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) * c. The last (newest appended) block uses safe_exit_speed exit speed (there's no 'next')
* with a maximum allowable deceleration over the block travel distance. * 2. Go over blocks in chronological (forward) order and fix the exit junction speed:
* c. The last (or newest appended) block is planned from safe_exit_speed_sqr. * a. Exit speed (next->entry_speed) must be below/at the maximum exit speed forward-computed
* 2. Go over every block in chronological (forward) order and dial down junction speed values if * from its entry speed if assuming maximum acceleration over the full block travel distance
* a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable * b. Exit speed should stay above/at the pre-computed minimum junction speed limit
* acceleration over the block travel distance. * 3. Convert entry / exit speeds (mm/s) into final/initial steps/s
* *
* When these stages are complete, the planner will have maximized the velocity profiles throughout the all * When these stages are complete, the planner will have maximized the velocity profiles throughout the all
* of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In * of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
@@ -959,28 +955,22 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
* are possible. If a new block is added to the buffer, the plan is recomputed according to the said * are possible. If a new block is added to the buffer, the plan is recomputed according to the said
* guidelines for a new optimal plan. * guidelines for a new optimal plan.
* *
* To increase computational efficiency of these guidelines, a set of planner block pointers have been * To increase computational efficiency of these guidelines:
* created to indicate stop-compute points for when the planner guidelines cannot logically make any further * 1. We keep track of which blocks need calculation (block->flag.recalculate)
* changes or improvements to the plan when in normal operation and new blocks are streamed and added to the * 2. We stop the reverse pass on the first block whose entry_speed == max_entry_speed. As soon
* planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are * as that happens, there can be no further increases (ensured by the previous recalculate)
* bracketed by junction velocities at their maximums (or by the first planner block as well), no new block * 3. On the forward pass we skip through to the first block with a modified exit speed
* added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute * (next->entry_speed)
* them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute * 4. On the forward pass if we encounter a full acceleration block that limits its exit speed
* point) are all accelerating, they are all optimal and can not be altered by a new block added to the * (next->entry_speed) we also update the maximum for that junction (next->max_entry_speed)
* planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum * so it's never updated again
* junction velocity is reached. However, if the operational conditions of the plan changes from infrequently * 5. We use speed squared (ex: entry_speed_sqr in mm^2/s^2) in acceleration limit computations
* used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is * 6. We don't recompute sqrt(entry_speed_sqr) if the block's entry speed didn't change
* recomputed as stated in the general guidelines.
* *
* Planner buffer index mapping: * Planner buffer index mapping:
* - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. * - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
* - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether * - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
* the buffer is full or empty. As described for standard ring buffers, this block is always empty. * the buffer is full or empty. As described for standard ring buffers, this block is always empty.
* - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
* streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
* planner buffer that don't change with the addition of a new block, as describe above. In addition,
* this block can never be less than block_buffer_tail and will always be pushed forward and maintain
* this requirement when encountered by the Planner::release_current_block() routine during a cycle.
* *
* NOTE: Since the planner only computes on what's in the planner buffer, some motions with many short * NOTE: Since the planner only computes on what's in the planner buffer, some motions with many short
* segments (e.g., complex curves) may seem to move slowly. This is because there simply isn't * segments (e.g., complex curves) may seem to move slowly. This is because there simply isn't
@@ -1003,7 +993,8 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
*/ */
// The kernel called by recalculate() when scanning the plan from last to first entry. // The kernel called by recalculate() when scanning the plan from last to first entry.
void Planner::reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr) { // Returns true if it could increase the current block's entry speed.
bool Planner::reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr) {
// We need to recalculate only for the last block added or if next->entry_speed_sqr changed. // We need to recalculate only for the last block added or if next->entry_speed_sqr changed.
if (!next || next->flag.recalculate) { if (!next || next->flag.recalculate) {
// And only if we're not already at max entry speed. // And only if we're not already at max entry speed.
@@ -1021,196 +1012,136 @@ void Planner::reverse_pass_kernel(block_t * const current, const block_t * const
// become BUSY just before being marked RECALCULATE, so check for that! // become BUSY just before being marked RECALCULATE, so check for that!
if (stepper.is_block_busy(current)) { if (stepper.is_block_busy(current)) {
// Block became busy. Clear the RECALCULATE flag (no point in // Block became busy. Clear the RECALCULATE flag (no point in
// recalculating BUSY blocks). And don't set its speed, as it can't // recalculating BUSY blocks).
// be updated at this time.
current->flag.recalculate = false; current->flag.recalculate = false;
} }
else { else {
// Block is not BUSY so this is ahead of the Stepper ISR: // Block is not BUSY so this is ahead of the Stepper ISR:
// Just Set the new entry speed.
current->entry_speed_sqr = new_entry_speed_sqr; current->entry_speed_sqr = new_entry_speed_sqr;
return true;
} }
} }
} }
} }
return false;
} }
/** /**
* recalculate() needs to go over the current plan twice. * recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the reverse pass. * Once in reverse and once forward. This implements the reverse pass that
* coarsely maximizes the entry speeds starting from last block.
* Requires there's at least one block with flag.recalculate in the buffer.
*/ */
void Planner::reverse_pass(const_float_t safe_exit_speed_sqr) { void Planner::reverse_pass(const_float_t safe_exit_speed_sqr) {
// Initialize block index to the last block in the planner buffer. // Initialize block index to the last block in the planner buffer.
// This last block will have flag.recalculate set.
uint8_t block_index = prev_block_index(block_buffer_head); uint8_t block_index = prev_block_index(block_buffer_head);
// Read the index of the last buffer planned block. // The ISR may change block_buffer_nonbusy so get a stable local copy.
// The ISR may change it so get a stable local copy. uint8_t nonbusy_block_index = block_buffer_nonbusy;
uint8_t planned_block_index = block_buffer_planned;
// If there was a race condition and block_buffer_planned was incremented
// or was pointing at the head (queue empty) break loop now and avoid
// planning already consumed blocks
if (planned_block_index == block_buffer_head) return;
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
// block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
const block_t *next = nullptr; const block_t *next = nullptr;
while (block_index != planned_block_index) { // Don't try to change the entry speed of the first non-busy block.
while (block_index != nonbusy_block_index) {
// Perform the reverse pass
block_t *current = &block_buffer[block_index]; block_t *current = &block_buffer[block_index];
// Only process movement blocks // Only process movement blocks
if (current->is_move()) { if (current->is_move()) {
reverse_pass_kernel(current, next, safe_exit_speed_sqr); // If no entry speed increase was possible we end the reverse pass.
if (!reverse_pass_kernel(current, next, safe_exit_speed_sqr)) return;
next = current; next = current;
} }
// Advance to the next
block_index = prev_block_index(block_index); block_index = prev_block_index(block_index);
// The ISR could advance the block_buffer_planned while we were doing the reverse pass. // The ISR could advance block_buffer_nonbusy while we were doing the reverse pass.
// We must try to avoid using an already consumed block as the last one - So follow // We must try to avoid using an already consumed block as the last one - So follow
// changes to the pointer and make sure to limit the loop to the currently busy block // changes to the pointer and make sure to limit the loop to the currently busy block
while (planned_block_index != block_buffer_planned) { while (nonbusy_block_index != block_buffer_nonbusy) {
// If we reached the busy block or an already processed block, break the loop now // If we reached the busy block or an already processed block, break the loop now
if (block_index == planned_block_index) return; if (block_index == nonbusy_block_index) return;
// Advance the pointer, following the busy block // Advance the pointer, following the busy block
planned_block_index = next_block_index(planned_block_index); nonbusy_block_index = next_block_index(nonbusy_block_index);
} }
} }
} }
// The kernel called by recalculate() when scanning the plan from first to last entry. // The kernel called during the forward pass. Assumes current->flag.recalculate.
void Planner::forward_pass_kernel(const block_t * const previous, block_t * const current, const uint8_t block_index) { void Planner::forward_pass_kernel(const block_t * const previous, block_t * const current) {
// Check against previous speed only on current->entry_speed_sqr changes (or if first time). // Check if the previous block is accelerating.
if (current->flag.recalculate) { if (previous->entry_speed_sqr < current->entry_speed_sqr) {
// If the previous block is accelerating check if it's too short to complete the full speed // Compute the maximum achievable speed if the previous block was fully accelerating.
// change then adjust the entry speed accordingly. Entry speeds have already been maximized. float new_exit_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
if (previous->entry_speed_sqr < current->entry_speed_sqr) {
float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
// If true, previous block is full-acceleration and we can move the planned pointer forward. if (new_exit_speed_sqr < current->entry_speed_sqr) {
if (new_entry_speed_sqr < current->entry_speed_sqr) { // Current entry speed limited by full acceleration from previous entry speed.
// Current entry speed limited by full acceleration from previous entry speed.
// Make sure entry speed not lower than minimum_planner_speed_sqr.
NOLESS(new_entry_speed_sqr, current->min_entry_speed_sqr);
current->entry_speed_sqr = new_entry_speed_sqr;
// Set optimal plan pointer. // Make sure entry speed not lower than minimum_planner_speed_sqr.
block_buffer_planned = block_index; NOLESS(new_exit_speed_sqr, current->min_entry_speed_sqr);
} current->entry_speed_sqr = new_exit_speed_sqr;
else { // Ensure we don't try updating entry_speed_sqr again.
// Previous entry speed has been maximized. current->max_entry_speed_sqr = new_exit_speed_sqr;
block_buffer_planned = prev_block_index(block_index);
}
} }
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. When the plan is bracketed by either the beginning of the
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
if (current->entry_speed_sqr == current->max_entry_speed_sqr)
block_buffer_planned = block_index;
} }
// The fully optimized entry speed is our new minimum speed.
current->min_entry_speed_sqr = current->entry_speed_sqr;
} }
/** /**
* recalculate() needs to go over the current plan twice. * Do the forward pass and recalculate the trapezoid speed profiles for all blocks in the plan
* Once in reverse and once forward. This implements the forward pass. * according to entry/exit speeds.
*/
void Planner::forward_pass() {
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
// Begin at buffer planned pointer. Note that block_buffer_planned can be modified
// by the stepper ISR, so read it ONCE. It it guaranteed that block_buffer_planned
// will never lead head, so the loop is safe to execute. Also note that the forward
// pass will never modify the values at the tail.
uint8_t block_index = block_buffer_planned;
block_t *block;
const block_t * previous = nullptr;
while (block_index != block_buffer_head) {
// Perform the forward pass
block = &block_buffer[block_index];
// Only process movement blocks
if (block->is_move()) {
// If there's no previous block or the previous block is not
// BUSY (thus, modifiable) run the forward_pass_kernel. Otherwise,
// the previous block became BUSY, so assume the current block's
// entry speed can't be altered (since that would also require
// updating the exit speed of the previous block).
if (previous && !stepper.is_block_busy(previous))
forward_pass_kernel(previous, block, block_index);
previous = block;
}
// Advance to the previous
block_index = next_block_index(block_index);
}
}
/**
* Recalculate the trapezoid speed profiles for all blocks in the plan
* according to the entry_factor for each junction. Must be called by
* recalculate() after updating the blocks.
*/ */
void Planner::recalculate_trapezoids(const_float_t safe_exit_speed_sqr) { void Planner::recalculate_trapezoids(const_float_t safe_exit_speed_sqr) {
// The tail may be changed by the ISR so get a local copy. // Start with the block that's about to execute or is executing.
uint8_t block_index = block_buffer_tail, uint8_t block_index = block_buffer_tail,
head_block_index = block_buffer_head; head_block_index = block_buffer_head;
// Since there could be a sync block in the head of the queue, and the
// next loop must not recalculate the head block (as it needs to be
// specially handled), scan backwards to the first non-SYNC block.
while (head_block_index != block_index) {
// Go back (head always point to the first free block)
const uint8_t prev_index = prev_block_index(head_block_index);
// Get the pointer to the block
block_t *prev = &block_buffer[prev_index];
// It the block is a move, we're done with this loop
if (prev->is_move()) break;
// Examine the previous block. This and all following are SYNC blocks
head_block_index = prev_index;
}
// Go from the tail (currently executed block) to the first block, without including it)
block_t *block = nullptr, *next = nullptr; block_t *block = nullptr, *next = nullptr;
float current_entry_speed = 0.0f, next_entry_speed = 0.0f; float next_entry_speed = 0.0f;
while (block_index != head_block_index) { while (block_index != head_block_index) {
next = &block_buffer[block_index]; next = &block_buffer[block_index];
// Only process movement blocks
if (next->is_move()) { if (next->is_move()) {
next_entry_speed = SQRT(next->entry_speed_sqr); // Check if the next block's entry speed changed
if (next->flag.recalculate) {
if (!block) {
// 'next' is the first move due to either being the first added move or due to the planner
// having completely fallen behind. Revert any reverse pass change.
next->entry_speed_sqr = next->min_entry_speed_sqr;
next_entry_speed = SQRT(next->min_entry_speed_sqr);
}
else {
// Try to fix exit speed which requires trapezoid recalculation
block->flag.recalculate = true;
if (block) { // But there is an inherent race condition here, as the block may have
// become BUSY just before being marked RECALCULATE, so check for that!
if (stepper.is_block_busy(block)) {
// Block is BUSY so we can't change the exit speed. Revert any reverse pass change.
next->entry_speed_sqr = next->min_entry_speed_sqr;
if (!next->initial_rate) {
// 'next' was never calculated. Planner is falling behind so for maximum efficiency
// set next's stepping speed directly and forgo checking against min_entry_speed_sqr.
// calculate_trapezoid_for_block() can handle it, albeit sub-optimally.
next->initial_rate = block->final_rate;
}
// Note that at this point next_entry_speed is (still) 0.
}
else {
// Block is not BUSY: we won the race against the ISR or recalculate was already set
// If the next block is marked to RECALCULATE, also mark the previously-fetched one if (next->entry_speed_sqr != next->min_entry_speed_sqr)
if (next->flag.recalculate) block->flag.recalculate = true; forward_pass_kernel(block, next);
// Recalculate if current block entry or exit junction speed has changed. const float current_entry_speed = next_entry_speed;
if (block->flag.recalculate) { next_entry_speed = SQRT(next->entry_speed_sqr);
// But there is an inherent race condition here, as the block maybe calculate_trapezoid_for_block(block, current_entry_speed, next_entry_speed);
// became BUSY, just before it was marked as RECALCULATE, so check
// if that is the case!
if (!stepper.is_block_busy(block)) {
// Block is not BUSY, we won the race against the Stepper ISR:
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
const float nomr = 1.0f / block->nominal_speed;
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
} }
// Reset current only to ensure next trapezoid is computed - The // Reset current only to ensure next trapezoid is computed - The
@@ -1220,30 +1151,17 @@ void Planner::recalculate_trapezoids(const_float_t safe_exit_speed_sqr) {
} }
block = next; block = next;
current_entry_speed = next_entry_speed;
} }
block_index = next_block_index(block_index); block_index = next_block_index(block_index);
} }
// Last/newest block in buffer. Always recalculated. // Last/newest block in buffer. The above guarantees it's a move block.
if (block) { if (block && block->flag.recalculate) {
const float current_entry_speed = next_entry_speed;
next_entry_speed = SQRT(safe_exit_speed_sqr); next_entry_speed = SQRT(safe_exit_speed_sqr);
// Mark the next(last) block as RECALCULATE, to prevent the Stepper ISR running it. calculate_trapezoid_for_block(block, current_entry_speed, next_entry_speed);
// As the last block is always recalculated here, there is a chance the block isn't
// marked as RECALCULATE yet. That's the reason for the following line.
block->flag.recalculate = true;
// But there is an inherent race condition here, as the block maybe
// became BUSY, just before it was marked as RECALCULATE, so check
// if that is the case!
if (!stepper.is_block_busy(block)) {
// Block is not BUSY, we won the race against the Stepper ISR:
const float nomr = 1.0f / block->nominal_speed;
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
}
// Reset block to ensure its trapezoid is computed - The stepper is free to use // Reset block to ensure its trapezoid is computed - The stepper is free to use
// the block from now on. // the block from now on.
@@ -1251,14 +1169,10 @@ void Planner::recalculate_trapezoids(const_float_t safe_exit_speed_sqr) {
} }
} }
// Requires there's at least one block with flag.recalculate in the buffer
void Planner::recalculate(const_float_t safe_exit_speed_sqr) { void Planner::recalculate(const_float_t safe_exit_speed_sqr) {
// Initialize block index to the last block in the planner buffer. reverse_pass(safe_exit_speed_sqr);
const uint8_t block_index = prev_block_index(block_buffer_head); // The forward pass is done as part of recalculate_trapezoids()
// If there is just one block, no planning can be done. Avoid it!
if (block_index != block_buffer_planned) {
reverse_pass(safe_exit_speed_sqr);
forward_pass();
}
recalculate_trapezoids(safe_exit_speed_sqr); recalculate_trapezoids(safe_exit_speed_sqr);
} }
@@ -1667,7 +1581,7 @@ void Planner::quick_stop() {
const bool was_enabled = stepper.suspend(); const bool was_enabled = stepper.suspend();
// Drop all queue entries // Drop all queue entries
block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail; block_buffer_nonbusy = block_buffer_head = block_buffer_tail;
// Restart the block delay for the first movement - As the queue was // Restart the block delay for the first movement - As the queue was
// forced to empty, there's no risk the ISR will touch this. // forced to empty, there's no risk the ISR will touch this.
@@ -2436,6 +2350,7 @@ bool Planner::_populate_block(
// Compute and limit the acceleration rate for the trapezoid generator. // Compute and limit the acceleration rate for the trapezoid generator.
const float steps_per_mm = block->step_event_count * inverse_millimeters; const float steps_per_mm = block->step_event_count * inverse_millimeters;
block->steps_per_mm = steps_per_mm;
uint32_t accel; uint32_t accel;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
bool use_advance_lead = false; bool use_advance_lead = false;
@@ -2829,6 +2744,8 @@ bool Planner::_populate_block(
block->entry_speed_sqr = minimum_planner_speed_sqr; block->entry_speed_sqr = minimum_planner_speed_sqr;
// Set min entry speed. Rarely it could be higher than the previous nominal speed but that's ok. // Set min entry speed. Rarely it could be higher than the previous nominal speed but that's ok.
block->min_entry_speed_sqr = minimum_planner_speed_sqr; block->min_entry_speed_sqr = minimum_planner_speed_sqr;
// Zero the initial_rate to indicate that calculate_trapezoid_for_block() hasn't been called yet.
block->initial_rate = 0;
block->flag.recalculate = true; block->flag.recalculate = true;

11
Marlin/src/module/planner.h
View File

@@ -219,6 +219,7 @@ typedef struct PlannerBlock {
min_entry_speed_sqr, // Minimum allowable junction entry speed in (mm/sec)^2 min_entry_speed_sqr, // Minimum allowable junction entry speed in (mm/sec)^2
max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2 max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2
millimeters, // The total travel of this block in mm millimeters, // The total travel of this block in mm
steps_per_mm, // steps/mm
acceleration; // acceleration mm/sec^2 acceleration; // acceleration mm/sec^2
union { union {
@@ -442,7 +443,6 @@ class Planner {
static block_t block_buffer[BLOCK_BUFFER_SIZE]; static block_t block_buffer[BLOCK_BUFFER_SIZE];
static volatile uint8_t block_buffer_head, // Index of the next block to be pushed static volatile uint8_t block_buffer_head, // Index of the next block to be pushed
block_buffer_nonbusy, // Index of the first non busy block block_buffer_nonbusy, // Index of the first non busy block
block_buffer_planned, // Index of the optimally planned block
block_buffer_tail; // Index of the busy block, if any block_buffer_tail; // Index of the busy block, if any
static uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocks static uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocks
static uint8_t delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks static uint8_t delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
@@ -804,7 +804,7 @@ class Planner {
FORCE_INLINE static uint8_t nonbusy_movesplanned() { return block_dec_mod(block_buffer_head, block_buffer_nonbusy); } FORCE_INLINE static uint8_t nonbusy_movesplanned() { return block_dec_mod(block_buffer_head, block_buffer_nonbusy); }
// Remove all blocks from the buffer // Remove all blocks from the buffer
FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0; } FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_head = block_buffer_tail = 0; }
// Check if movement queue is full // Check if movement queue is full
FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); } FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
@@ -1081,13 +1081,12 @@ class Planner {
} }
#endif #endif
static void calculate_trapezoid_for_block(block_t * const block, const_float_t entry_factor, const_float_t exit_factor); static void calculate_trapezoid_for_block(block_t * const block, const_float_t entry_speed, const_float_t exit_speed);
static void reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr); static bool reverse_pass_kernel(block_t * const current, const block_t * const next, const_float_t safe_exit_speed_sqr);
static void forward_pass_kernel(const block_t * const previous, block_t * const current, uint8_t block_index); static void forward_pass_kernel(const block_t * const previous, block_t * const current);
static void reverse_pass(const_float_t safe_exit_speed_sqr); static void reverse_pass(const_float_t safe_exit_speed_sqr);
static void forward_pass();
static void recalculate_trapezoids(const_float_t safe_exit_speed_sqr); static void recalculate_trapezoids(const_float_t safe_exit_speed_sqr);