microlab
/
scube
1
1
Fork 0
Code Issues Pull Requests Releases Activity

[2.0.x] better reverse pass (#8722)

Browse Source 
* repair reverse_pass()

And make it readeble.
This was broken a long time ago.
Not competely unfunctional but far from optimal.

* Minor speedup when calling calculate_trapezoid_for_block

2 float / to 1 foat / and 2 float *

* Various style changes
2.0.x
AnHardt 7 years ago committed by Scott Lahteine
parent f58eb391aa
commit b3daf6b5db
2 changed files with 46 additions and 42 deletions
Show Stats Download Patch File Download Diff File

51
Marlin/src/module/planner.cpp
Unescape Escape View File

@ -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. // 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 (!current || !next) return;
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // 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 // 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. * Once in reverse and once forward. This implements the reverse pass.
*/ */
void Planner::reverse_pass() { void Planner::reverse_pass() {
if (movesplanned() > 3) { 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 }; do {
const block_t * const next = current;
// Make a local copy of block_buffer_tail, because the interrupt can alter it blocknr = prev_block_index(blocknr);
// Is a critical section REALLY needed for a single byte change? current = &block_buffer[blocknr];
//CRITICAL_SECTION_START; if (TEST(current->flag, BLOCK_BIT_START_FROM_FULL_HALT)) // Up to this every block is already optimized.
uint8_t tail = block_buffer_tail; break;
//CRITICAL_SECTION_END reverse_pass_kernel(current, next);
} while (blocknr != endnr);
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]);
}
} }
} }
// The kernel called by recalculate() when scanning the plan from first to last entry. // 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 (!previous) return;
// If the previous block is an acceleration block, but it is not long enough to complete the // 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. // Recalculate if current block entry or exit junction speed has changed.
if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) { 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. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
float nom = current->nominal_speed; const float nomr = 1.0 / current->nominal_speed;
calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom); 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 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. // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
if (next) { if (next) {
float nom = next->nominal_speed; const float nomr = 1.0 / next->nominal_speed;
calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom); calculate_trapezoid_for_block(next, next->entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
CBI(next->flag, BLOCK_BIT_RECALCULATE); 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 #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. // 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. // 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 //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 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 // increment counters with next move in e axis
filwidth_e_count += delta_mm[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 #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 // Move buffer head
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
// Update the position (only when a move was queued) // 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); COPY(position, target);
recalculate(); recalculate();

27
Marlin/src/module/planner.h
Unescape Escape View File

@ -134,15 +134,24 @@ typedef struct {
#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1)) #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
class Planner { class Planner {
public: 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 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_tail; block_buffer_tail; // Index of the busy block, if any
#if ENABLED(DISTINCT_E_FACTORS) #if ENABLED(DISTINCT_E_FACTORS)
static uint8_t last_extruder; // Respond to extruder change static uint8_t last_extruder; // Respond to extruder change
@ -277,9 +286,9 @@ class Planner {
/** /**
* Number of moves currently in the 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 // Update multipliers based on new diameter measurements
static void calculate_volumetric_multipliers(); static void calculate_volumetric_multipliers();
@ -533,8 +542,8 @@ class Planner {
/** /**
* Get the index of the next / previous block in the ring buffer * 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 constexpr 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 prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); }
/** /**
* Calculate the distance (not time) it takes to accelerate * 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 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 reverse_pass_kernel(block_t* const current, const block_t * const next);
static void forward_pass_kernel(const block_t *previous, block_t* const current); static void forward_pass_kernel(const block_t * const previous, block_t* const current);
static void reverse_pass(); static void reverse_pass();
static void forward_pass(); static void forward_pass();

Write Preview
Loading…
Cancel
Save