From a4af975873c7b0c75d986cb829eb5d869a4a6b8e Mon Sep 17 00:00:00 2001 From: etagle Date: Fri, 18 May 2018 04:04:01 -0300 Subject: [PATCH] Fix planner block optimization - Fixed the planner incorrectly avoiding optimization of the block following the active one. - Added extra conditions to terminate planner early and avoid redundant computations. --- Marlin/src/module/planner.cpp | 223 ++++++++++++++++++++++++---------- Marlin/src/module/planner.h | 37 +++--- 2 files changed, 182 insertions(+), 78 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 78634f799..0171f3d21 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -107,7 +107,8 @@ block_t Planner::block_buffer[BLOCK_BUFFER_SIZE]; volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed Planner::block_buffer_tail; // Index of the busy block, if any uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks -uint8_t Planner::delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks +uint8_t Planner::delay_before_delivering, // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks + Planner::block_buffer_planned; // Index of the optimally planned block float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second Planner::axis_steps_per_mm[XYZE_N], @@ -227,6 +228,7 @@ void Planner::init() { bed_level_matrix.set_to_identity(); #endif clear_block_buffer(); + block_buffer_planned = 0; delay_before_delivering = 0; } @@ -825,6 +827,68 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); } +/* PLANNER SPEED DEFINITION + +--------+ <- current->nominal_speed + / \ + current->entry_speed -> + \ + | + <- next->entry_speed (aka exit speed) + +-------------+ + time --> + + 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 + (i.e. current->entry_speed) such that: + a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of + neighboring blocks. + b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed) + with a maximum allowable deceleration over the block travel distance. + c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero). + 2. Go over every block in chronological (forward) order and dial down junction speed values if + a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable + acceleration over the block travel distance. + + 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 + other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements + 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. + + To increase computational efficiency of these guidelines, a set of planner block pointers have been + created to indicate stop-compute points for when the planner guidelines cannot logically make any further + changes or improvements to the plan when in normal operation and new blocks are streamed and added to the + planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are + bracketed by junction velocities at their maximums (or by the first planner block as well), no new block + added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute + them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute + point) are all accelerating, they are all optimal and can not be altered by a new block added to the + planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum + junction velocity is reached. However, if the operational conditions of the plan changes from infrequently + used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is + recomputed as stated in the general guidelines. + + Planner buffer index mapping: + - 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 + 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 plan_discard_current_block() routine during a cycle. + + NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short + line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't + enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then + decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and + becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner + will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line + motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use, + the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance + for the planner to compute over. It also increases the number of computations the planner has to perform + to compute an optimal plan, so select carefully. +*/ + // 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) { if (current) { @@ -851,6 +915,8 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const : MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(MINIMUM_PLANNER_SPEED), current->millimeters)); if (current->entry_speed_sqr != new_entry_speed_sqr) { current->entry_speed_sqr = new_entry_speed_sqr; + + // Need to recalculate the block speed SBI(current->flag, BLOCK_BIT_RECALCULATE); } } @@ -862,44 +928,72 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const * Once in reverse and once forward. This implements the reverse pass. */ void Planner::reverse_pass() { - if (movesplanned() > 2) { - const uint8_t endnr = next_block_index(block_buffer_tail); // tail is running. tail+1 shouldn't be altered because it's connected to the running block. - uint8_t blocknr = prev_block_index(block_buffer_head); + // Initialize block index to the last block in the planner buffer. + uint8_t block_index = prev_block_index(block_buffer_head); + + // Read the index of the last buffer planned block. + // The ISR may change it so get a stable local copy. + 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. + block_t *current; + const block_t *next = NULL; + while (block_index != planned_block_index) { // Perform the reverse pass - block_t *current, *next = NULL; - while (blocknr != endnr) { - // Perform the reverse pass - Only consider non sync blocks - current = &block_buffer[blocknr]; - if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) { - reverse_pass_kernel(current, next); - next = current; - } - // Advance to the next - blocknr = prev_block_index(blocknr); + current = &block_buffer[block_index]; + + // Only consider non sync blocks + if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) { + reverse_pass_kernel(current, next); + next = current; } + + // Advance to the next + block_index = prev_block_index(block_index); } } // The kernel called by recalculate() when scanning the plan from first to last entry. -void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current) { +void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index) { if (previous) { // If the previous block is an acceleration block, too short to complete the full speed // change, adjust the entry speed accordingly. Entry speeds have already been reset, // maximized, and reverse-planned. If nominal length is set, max junction speed is // guaranteed to be reached. No need to recheck. - if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) { - if (previous->entry_speed_sqr < current->entry_speed_sqr) { - // Compute the maximum allowable speed - const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters); - // If true, current block is full-acceleration - if (current->entry_speed_sqr > new_entry_speed_sqr) { - // Always <= max_entry_speed_sqr. Backward pass sets this. - current->entry_speed_sqr = new_entry_speed_sqr; - SBI(current->flag, BLOCK_BIT_RECALCULATE); - } + if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH) && + previous->entry_speed_sqr < current->entry_speed_sqr) { + + // Compute the maximum allowable speed + const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters); + + // If true, current block is full-acceleration and we can move the planned pointer forward. + if (new_entry_speed_sqr < current->entry_speed_sqr) { + + // Always <= max_entry_speed_sqr. Backward pass sets this. + current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + + // Set optimal plan pointer. + block_buffer_planned = block_index; + + // And mark we need to recompute the trapezoidal shape + SBI(current->flag, BLOCK_BIT_RECALCULATE); } } + + // 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; } } @@ -908,20 +1002,30 @@ void Planner::forward_pass_kernel(const block_t * const previous, block_t* const * Once in reverse and once forward. This implements the forward pass. */ void Planner::forward_pass() { - const uint8_t endnr = block_buffer_head; - uint8_t blocknr = block_buffer_tail; - - // Perform the forward pass - block_t *current, *previous = NULL; - while (blocknr != endnr) { - // Perform the forward pass - Only consider non-sync blocks - current = &block_buffer[blocknr]; + + // 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 *current; + const block_t * previous = NULL; + while (block_index != block_buffer_head) { + + // Perform the forward pass + current = &block_buffer[block_index]; + + // Skip SYNC blocks if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) { - forward_pass_kernel(previous, current); + forward_pass_kernel(previous, current, block_index); previous = current; } // Advance to the previous - blocknr = next_block_index(blocknr); + block_index = next_block_index(block_index); } } @@ -931,6 +1035,7 @@ void Planner::forward_pass() { * recalculate() after updating the blocks. */ void Planner::recalculate_trapezoids() { + // The tail may be changed by the ISR so get a local copy. uint8_t block_index = block_buffer_tail; // As there could be a sync block in the head of the queue, and the next loop must not @@ -1004,33 +1109,14 @@ void Planner::recalculate_trapezoids() { } } -/** - * Recalculate the motion plan according to the following algorithm: - * - * 1. Go over every block in reverse order... - * - * Calculate a junction speed reduction (block_t.entry_factor) so: - * - * a. The junction jerk is within the set limit, and - * - * b. No speed reduction within one block requires faster - * deceleration than the one, true constant acceleration. - * - * 2. Go over every block in chronological order... - * - * Dial down junction speed reduction values if: - * a. The speed increase within one block would require faster - * acceleration than the one, true constant acceleration. - * - * After that, all blocks will have an entry_factor allowing all speed changes to - * be performed using only the one, true constant acceleration, and where no junction - * jerk is jerkier than the set limit, Jerky. Finally it will: - * - * 3. Recalculate "trapezoids" for all blocks. - */ void Planner::recalculate() { - reverse_pass(); - forward_pass(); + // Initialize block index to the last block in the planner buffer. + const uint8_t block_index = prev_block_index(block_buffer_head); + // If there is just one block, no planning can be done. Avoid it! + if (block_index != block_buffer_planned) { + reverse_pass(); + forward_pass(); + } recalculate_trapezoids(); } @@ -1348,10 +1434,18 @@ void Planner::check_axes_activity() { #endif // PLANNER_LEVELING void Planner::quick_stop() { + // Remove all the queued blocks. Note that this function is NOT // called from the Stepper ISR, so we must consider tail as readonly! - // that is why we set head to tail! - block_buffer_head = block_buffer_tail; + // that is why we set head to tail - But there is a race condition that + // must be handled: The tail could change between the read and the assignment + // so this must be enclosed in a critical section + + const bool was_enabled = STEPPER_ISR_ENABLED(); + if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT(); + + // Drop all queue entries + block_buffer_planned = block_buffer_head = block_buffer_tail; // Restart the block delay for the first movement - As the queue was // forced to empty, there's no risk the ISR will touch this. @@ -1365,6 +1459,9 @@ void Planner::quick_stop() { // Make sure to drop any attempt of queuing moves for at least 1 second cleaning_buffer_counter = 1000; + // Reenable Stepper ISR + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); + // And stop the stepper ISR stepper.quick_stop(); } diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 34288c14d..86707610b 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -177,7 +177,9 @@ class Planner { static volatile uint8_t block_buffer_head, // Index of the next block to be pushed block_buffer_tail; // Index of the busy block, if any 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 + block_buffer_planned; // Index of the optimally planned block + #if ENABLED(DISTINCT_E_FACTORS) static uint8_t last_extruder; // Respond to extruder change @@ -655,9 +657,7 @@ class Planner { block_t * const block = &block_buffer[block_buffer_tail]; // No trapezoid calculated? Don't execute yet. - if ( TEST(block->flag, BLOCK_BIT_RECALCULATE) - || (movesplanned() > 1 && TEST(block_buffer[next_block_index(block_buffer_tail)].flag, BLOCK_BIT_RECALCULATE)) - ) return NULL; + if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return NULL; #if ENABLED(ULTRA_LCD) block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it. @@ -667,13 +667,13 @@ class Planner { SBI(block->flag, BLOCK_BIT_BUSY); return block; } - else { - // The queue became empty - #if ENABLED(ULTRA_LCD) - clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero. - #endif - return NULL; - } + + // The queue became empty + #if ENABLED(ULTRA_LCD) + clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero. + #endif + + return NULL; } /** @@ -682,7 +682,14 @@ class Planner { * NB: There MUST be a current block to call this function!! */ FORCE_INLINE static void discard_current_block() { - block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1); + if (has_blocks_queued()) { // Discard non-empty buffer. + uint8_t block_index = next_block_index( block_buffer_tail ); + + // Push block_buffer_planned pointer, if encountered. + if (!has_blocks_queued()) block_buffer_planned = block_index; + + block_buffer_tail = block_index; + } } #if ENABLED(ULTRA_LCD) @@ -741,8 +748,8 @@ class Planner { /** * Get the index of the next / previous block in the ring buffer */ - static constexpr int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); } - static constexpr int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); } + static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); } + static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); } /** * Calculate the distance (not time) it takes to accelerate @@ -787,7 +794,7 @@ class Planner { static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor); static void reverse_pass_kernel(block_t* const current, const block_t * const next); - static void forward_pass_kernel(const block_t * const previous, block_t* const current); + static void forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index); static void reverse_pass(); static void forward_pass();