andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

* 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
pull/1/head
AnHardt 7 years ago
committed by Scott Lahteine
parent
f58eb391aa
commit
b3daf6b5db
2 changed files with 46 additions and 42 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 53
      Marlin/src/module/planner.cpp
  2. 35
      Marlin/src/module/planner.h

53
Marlin/src/module/planner.cpp
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.
block_t* block[3] = { NULL, NULL, NULL }; // tail+2 because the index is not yet advanced when checked
uint8_t blocknr = prev_block_index(block_buffer_head);
// Make a local copy of block_buffer_tail, because the interrupt can alter it block_t* current = &block_buffer[blocknr];
// Is a critical section REALLY needed for a single byte change?
//CRITICAL_SECTION_START; do {
uint8_t tail = block_buffer_tail; const block_t * const next = current;
//CRITICAL_SECTION_END blocknr = prev_block_index(blocknr);
current = &block_buffer[blocknr];
uint8_t b = BLOCK_MOD(block_buffer_head - 3); if (TEST(current->flag, BLOCK_BIT_START_FROM_FULL_HALT)) // Up to this every block is already optimized.
while (b != tail) { break;
if (block[0] && TEST(block[0]->flag, BLOCK_BIT_START_FROM_FULL_HALT)) break; reverse_pass_kernel(current, next);
b = prev_block_index(b); } while (blocknr != endnr);
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();

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

@ -134,21 +134,30 @@ 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
#endif #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 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 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 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 // 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], axis_steps_per_mm[XYZE_N],
steps_to_mm[XYZE_N]; steps_to_mm[XYZE_N];
static uint32_t max_acceleration_steps_per_s2[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 * 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