diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 7f6ba21272..3bf6fa5a76 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -258,7 +258,7 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e // 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 *next) { +void Planner::reverse_pass_kernel(block_t* const current, const block_t * const 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 @@ -279,31 +279,25 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t *next) { * Once in reverse and once forward. This implements the reverse pass. */ void Planner::reverse_pass() { - if (movesplanned() > 3) { + const uint8_t endnr = BLOCK_MOD(block_buffer_tail + 2); // tail is running. tail+1 shouldn't be altered because it's connected to the running block. + // tail+2 because the index is not yet advanced when checked + uint8_t blocknr = prev_block_index(block_buffer_head); + block_t* current = &block_buffer[blocknr]; - block_t* block[3] = { NULL, NULL, NULL }; - - // Make a local copy of block_buffer_tail, because the interrupt can alter it - // Is a critical section REALLY needed for a single byte change? - //CRITICAL_SECTION_START; - uint8_t tail = block_buffer_tail; - //CRITICAL_SECTION_END - - uint8_t b = BLOCK_MOD(block_buffer_head - 3); - while (b != tail) { - if (block[0] && TEST(block[0]->flag, BLOCK_BIT_START_FROM_FULL_HALT)) break; - b = prev_block_index(b); - block[2] = block[1]; - block[1] = block[0]; - block[0] = &block_buffer[b]; - reverse_pass_kernel(block[1], block[2]); - } + do { + const block_t * const next = current; + blocknr = prev_block_index(blocknr); + current = &block_buffer[blocknr]; + if (TEST(current->flag, BLOCK_BIT_START_FROM_FULL_HALT)) // Up to this every block is already optimized. + break; + reverse_pass_kernel(current, next); + } while (blocknr != endnr); } } // The kernel called by recalculate() when scanning the plan from first to last entry. -void Planner::forward_pass_kernel(const block_t* previous, block_t* const current) { +void Planner::forward_pass_kernel(const block_t * const 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 @@ -355,8 +349,8 @@ void Planner::recalculate_trapezoids() { // Recalculate if current block entry or exit junction speed has changed. if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_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); + const float nomr = 1.0 / current->nominal_speed; + calculate_trapezoid_for_block(current, current->entry_speed * nomr, next->entry_speed * nomr); CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed } } @@ -364,8 +358,8 @@ void Planner::recalculate_trapezoids() { } // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. if (next) { - float nom = next->nominal_speed; - calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom); + const float nomr = 1.0 / next->nominal_speed; + calculate_trapezoid_for_block(next, next->entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr); CBI(next->flag, BLOCK_BIT_RECALCULATE); } } @@ -1020,7 +1014,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const #endif ); } - float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides + const float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides // Calculate inverse time for this move. No divide by zero due to previous checks. // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0. @@ -1059,7 +1053,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const //FMM update ring buffer used for delay with filament measurements if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && filwidth_delay_index[1] >= 0) { //only for extruder with filament sensor and if ring buffer is initialized - const int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10; + constexpr int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10; // increment counters with next move in e axis filwidth_e_count += delta_mm[E_AXIS]; @@ -1356,13 +1350,14 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const #endif // LIN_ADVANCE - calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed); + const float bnsr = 1.0 / block->nominal_speed; + calculate_trapezoid_for_block(block, block->entry_speed * bnsr, safe_speed * bnsr); // Move buffer head block_buffer_head = next_buffer_head; // Update the position (only when a move was queued) - static_assert(COUNT(target) > 1, "array as function parameter should be declared as reference and with count"); + static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!"); COPY(position, target); recalculate(); diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 2872ef53b8..cb0315c761 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -134,21 +134,30 @@ typedef struct { #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1)) class Planner { - public: /** - * A ring buffer of moves described in steps + * The move buffer, calculated in stepper steps + * + * block_buffer is a ring buffer... + * + * head,tail : indexes for write,read + * head==tail : the buffer is empty + * head!=tail : blocks are in the buffer + * head==(tail-1)%size : the buffer is full + * + * Writer of head is Planner::_buffer_line(). + * Reader of tail is Stepper::isr(). Always consider tail busy / read-only */ static block_t block_buffer[BLOCK_BUFFER_SIZE]; - static volatile uint8_t block_buffer_head, // Index of the next block to be pushed - block_buffer_tail; + 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 #if ENABLED(DISTINCT_E_FACTORS) - static uint8_t last_extruder; // Respond to extruder change + static uint8_t last_extruder; // Respond to extruder change #endif - static int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder + static int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder static float e_factor[EXTRUDERS], // The flow percentage and volumetric multiplier combine to scale E movement filament_size[EXTRUDERS], // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder @@ -156,7 +165,7 @@ class Planner { volumetric_multiplier[EXTRUDERS]; // Reciprocal of cross-sectional area of filament (in mm^2). Pre-calculated to reduce computation in the planner // May be auto-adjusted by a filament width sensor - static float max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second + static float max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second axis_steps_per_mm[XYZE_N], steps_to_mm[XYZE_N]; static uint32_t max_acceleration_steps_per_s2[XYZE_N], @@ -277,9 +286,9 @@ class Planner { /** * Number of moves currently in the planner */ - static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); } + FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); } - static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); } + FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); } // Update multipliers based on new diameter measurements static void calculate_volumetric_multipliers(); @@ -533,8 +542,8 @@ class Planner { /** * Get the index of the next / previous block in the ring buffer */ - static int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); } - static int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); } + 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); } /** * Calculate the distance (not time) it takes to accelerate @@ -569,8 +578,8 @@ 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 *next); - static void forward_pass_kernel(const block_t *previous, block_t* const current); + 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 reverse_pass(); static void forward_pass();