diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index d1cf89ea5..14303804a 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -149,7 +149,7 @@ void Planner::init() { * Calculate trapezoid parameters, multiplying the entry- and exit-speeds * by the provided factors. */ -void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) { +void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) { uint32_t initial_rate = ceil(block->nominal_rate * entry_factor), final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second) @@ -203,29 +203,20 @@ void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor, // The kernel called by recalculate() when scanning the plan from last to first entry. -void Planner::reverse_pass_kernel(block_t* current, block_t* next) { - if (!current) return; - - if (next) { - // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. - // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and - // check for maximum allowable speed reductions to ensure maximum possible planned speed. - float max_entry_speed = current->max_entry_speed; - if (current->entry_speed != max_entry_speed) { - - // If nominal length true, max junction speed is guaranteed to be reached. Only compute - // for max allowable speed if block is decelerating and nominal length is false. - if (!current->nominal_length_flag && max_entry_speed > next->entry_speed) { - current->entry_speed = min(max_entry_speed, - max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters)); - } - else { - current->entry_speed = max_entry_speed; - } - current->recalculate_flag = true; - - } - } // Skip last block. Already initialized and set for recalculation. +void Planner::reverse_pass_kernel(block_t* const current, const block_t *next) { + if (!current || !next) return; + // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. + // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and + // check for maximum allowable speed reductions to ensure maximum possible planned speed. + float max_entry_speed = current->max_entry_speed; + if (current->entry_speed != max_entry_speed) { + // If nominal length true, max junction speed is guaranteed to be reached. Only compute + // for max allowable speed if block is decelerating and nominal length is false. + current->entry_speed = ((current->flag & BLOCK_FLAG_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed) + ? max_entry_speed + : min(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters)); + current->flag |= BLOCK_FLAG_RECALCULATE; + } } /** @@ -255,21 +246,21 @@ void Planner::reverse_pass() { } // The kernel called by recalculate() when scanning the plan from first to last entry. -void Planner::forward_pass_kernel(block_t* previous, block_t* current) { +void Planner::forward_pass_kernel(const block_t* previous, block_t* const current) { if (!previous) return; // If the previous block is an acceleration block, but it is not long enough to complete the // full speed change within the block, we need to adjust the entry speed accordingly. Entry // speeds have already been reset, maximized, and reverse planned by reverse planner. // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. - if (!previous->nominal_length_flag) { + if (!(previous->flag & BLOCK_FLAG_NOMINAL_LENGTH)) { if (previous->entry_speed < current->entry_speed) { float entry_speed = min(current->entry_speed, max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters)); // Check for junction speed change if (current->entry_speed != entry_speed) { current->entry_speed = entry_speed; - current->recalculate_flag = true; + current->flag |= BLOCK_FLAG_RECALCULATE; } } } @@ -298,19 +289,18 @@ void Planner::forward_pass() { */ void Planner::recalculate_trapezoids() { int8_t block_index = block_buffer_tail; - block_t* current; - block_t* next = NULL; + block_t *current, *next = NULL; while (block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { // Recalculate if current block entry or exit junction speed has changed. - if (current->recalculate_flag || next->recalculate_flag) { + if ((current->flag & BLOCK_FLAG_RECALCULATE) || (next->flag & BLOCK_FLAG_RECALCULATE)) { // NOTE: Entry and exit factors always > 0 by all previous logic operations. float nom = current->nominal_speed; calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom); - current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed + current->flag &= ~BLOCK_FLAG_RECALCULATE; // Reset current only to ensure next trapezoid is computed } } block_index = next_block_index(block_index); @@ -319,7 +309,7 @@ void Planner::recalculate_trapezoids() { if (next) { float nom = next->nominal_speed; calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom); - next->recalculate_flag = false; + next->flag &= ~BLOCK_FLAG_RECALCULATE; } } @@ -1119,8 +1109,9 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. - block->nominal_length_flag = (block->nominal_speed <= v_allowable); - block->recalculate_flag = true; // Always calculate trapezoid for new block + block->flag &= ~BLOCK_FLAG_NOMINAL_LENGTH; + if (block->nominal_speed <= v_allowable) block->flag |= BLOCK_FLAG_NOMINAL_LENGTH; + block->flag |= BLOCK_FLAG_RECALCULATE; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); diff --git a/Marlin/planner.h b/Marlin/planner.h index 80b20c478..2e18a7066 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -40,6 +40,19 @@ #include "vector_3.h" #endif +enum BlockFlag { + // Recalculate trapezoids on entry junction. For optimization. + BLOCK_FLAG_RECALCULATE = _BV(0), + + // Nominal speed always reached. + // i.e., The segment is long enough, so the nominal speed is reachable if accelerating + // from a safe speed (in consideration of jerking from zero speed). + BLOCK_FLAG_NOMINAL_LENGTH = _BV(1), + + // Start from a halt at the start of this block, respecting the maximum allowed jerk. + BLOCK_FLAG_START_FROM_FULL_HALT = _BV(2) +}; + /** * struct block_t * @@ -79,19 +92,18 @@ typedef struct { #endif // Fields used by the motion planner to manage acceleration - float nominal_speed, // The nominal speed for this block in mm/sec - entry_speed, // Entry speed at previous-current junction in mm/sec - max_entry_speed, // Maximum allowable junction entry speed in mm/sec - millimeters, // The total travel of this block in mm - acceleration; // acceleration mm/sec^2 - unsigned char recalculate_flag, // Planner flag to recalculate trapezoids on entry junction - nominal_length_flag; // Planner flag for nominal speed always reached + float nominal_speed, // The nominal speed for this block in mm/sec + entry_speed, // Entry speed at previous-current junction in mm/sec + max_entry_speed, // Maximum allowable junction entry speed in mm/sec + millimeters, // The total travel of this block in mm + acceleration; // acceleration mm/sec^2 + uint8_t flag; // Block flags (See BlockFlag enum above) // Settings for the trapezoid generator - unsigned long nominal_rate, // The nominal step rate for this block in step_events/sec - initial_rate, // The jerk-adjusted step rate at start of block - final_rate, // The minimal rate at exit - acceleration_steps_per_s2; // acceleration steps/sec^2 + uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec + initial_rate, // The jerk-adjusted step rate at start of block + final_rate, // The minimal rate at exit + acceleration_steps_per_s2; // acceleration steps/sec^2 #if FAN_COUNT > 0 unsigned long fan_speed[FAN_COUNT]; @@ -379,10 +391,10 @@ class Planner { return sqrt(sq(target_velocity) - 2 * accel * distance); } - static void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor); + 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* current, block_t* next); - static void forward_pass_kernel(block_t* previous, block_t* current); + static void reverse_pass_kernel(block_t* const current, const block_t *next); + static void forward_pass_kernel(const block_t *previous, block_t* const current); static void reverse_pass(); static void forward_pass();