bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Fix stepper/planner block handling, race conditions (#11098)

Browse Source 
- Allow planner to alter the deceleration phase of the currently executing block.
- Remove BUSY flag, as it is NON ATOMIC to set bits in the Stepper ISR and Planner at the same time.
This commit is contained in:
Eduardo José Tagle
2018-06-27 20:11:16 -03:00
committed by Scott Lahteine
parent 4d3a9930c5
commit edb21f349a
4 changed files with 177 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

194
Marlin/src/module/planner.cpp
View File

@@ -104,11 +104,12 @@ Planner planner;
* A ring buffer of moves described in steps
*/
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
Planner::block_buffer_planned; // Index of the optimally planned block
volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
Planner::block_buffer_nonbusy, // Index of the first non-busy block
Planner::block_buffer_planned, // Index of the optimally planned block
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
uint32_t Planner::max_acceleration_mm_per_s2[XYZE_N], // (mm/s^2) M201 XYZE
Planner::max_acceleration_steps_per_s2[XYZE_N], // (steps/s^2) Derived from mm_per_s2
@@ -240,7 +241,6 @@ void Planner::init() {
bed_level_matrix.set_to_identity();
#endif
clear_block_buffer();
block_buffer_planned = 0;
delay_before_delivering = 0;
}
@@ -703,6 +703,12 @@ void Planner::init() {
/**
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors.
**
* ############ VERY IMPORTANT ############
* NOTE that the PRECONDITION to call this function is that the block is
* NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR
* is not and will not use the block while we modify it, so it is safe to
* alter its values.
*/
void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
@@ -744,9 +750,6 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
cruise_rate = block->nominal_rate;
#endif
// block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps;
#if ENABLED(S_CURVE_ACCELERATION)
// Jerk controlled speed requires to express speed versus time, NOT steps
uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
@@ -755,32 +758,20 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
// And to offload calculations from the ISR, we also calculate the inverse of those times here
uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time);
uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time);
#endif
// Fill variables used by the stepper in a critical section
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
// Don't update variables if block is busy; it is being interpreted by the planner.
// If this happens, there's a problem... The block speed is inconsistent. Some values
// have already been updated, but the Stepper ISR is already using the block. Fortunately,
// the values being used by the Stepper ISR weren't touched, so just stop here...
// TODO: There may be a way to update a running block, depending on the stepper ISR position.
if (!TEST(block->flag, BLOCK_BIT_BUSY)) {
block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps + plateau_steps;
block->initial_rate = initial_rate;
#if ENABLED(S_CURVE_ACCELERATION)
block->acceleration_time = acceleration_time;
block->deceleration_time = deceleration_time;
block->acceleration_time_inverse = acceleration_time_inverse;
block->deceleration_time_inverse = deceleration_time_inverse;
block->cruise_rate = cruise_rate;
#endif
block->final_rate = final_rate;
}
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
// Store new block parameters
block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps + plateau_steps;
block->initial_rate = initial_rate;
#if ENABLED(S_CURVE_ACCELERATION)
block->acceleration_time = acceleration_time;
block->deceleration_time = deceleration_time;
block->acceleration_time_inverse = acceleration_time_inverse;
block->deceleration_time_inverse = deceleration_time_inverse;
block->cruise_rate = cruise_rate;
#endif
block->final_rate = final_rate;
}
/* PLANNER SPEED DEFINITION
@@ -831,7 +822,7 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
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.
this requirement when encountered by the Planner::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
@@ -875,8 +866,19 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const
// ISR does not consume the block before being recalculated
SBI(current->flag, BLOCK_BIT_RECALCULATE);
// Set the new entry speed
current->entry_speed_sqr = new_entry_speed_sqr;
// But there is an inherent race condition here, as the block may have
// become BUSY just before being marked RECALCULATE, so check for that!
if (stepper.is_block_busy(current)) {
// Block became busy. Clear the RECALCULATE flag (no point in
// recalculating BUSY blocks). And don't set its speed, as it can't
// be updated at this time.
CBI(current->flag, BLOCK_BIT_RECALCULATE);
}
else {
// Block is not BUSY so this is ahead of the Stepper ISR:
// Just Set the new entry speed.
current->entry_speed_sqr = new_entry_speed_sqr;
}
}
}
}
@@ -902,12 +904,11 @@ void Planner::reverse_pass() {
// 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
current = &block_buffer[block_index];
block_t *current = &block_buffer[block_index];
// Only consider non sync blocks
if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
@@ -917,6 +918,18 @@ void Planner::reverse_pass() {
// Advance to the next
block_index = prev_block_index(block_index);
// The ISR could advance the block_buffer_planned while we were doing the reverse pass.
// We must try to avoid using an already consumed block as the last one - So follow
// changes to the pointer and make sure to limit the loop to the currently busy block
while (planned_block_index != block_buffer_planned) {
// If we reached the busy block or an already processed block, break the loop now
if (block_index == planned_block_index) return;
// Advance the pointer, following the busy block
planned_block_index = next_block_index(planned_block_index);
}
}
}
@@ -940,11 +953,24 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
// so the stepper ISR does not consume the block before being recalculated
SBI(current->flag, BLOCK_BIT_RECALCULATE);
// 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.
// But there is an inherent race condition here, as the block maybe
// became BUSY, just before it was marked as RECALCULATE, so check
// if that is the case!
if (stepper.is_block_busy(current)) {
// Block became busy. Clear the RECALCULATE flag (no point in
// recalculating BUSY blocks and don't set its speed, as it can't
// be updated at this time.
CBI(current->flag, BLOCK_BIT_RECALCULATE);
}
else {
// Block is not BUSY, we won the race against the Stepper ISR:
// Set optimal plan pointer.
block_buffer_planned = block_index;
// 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;
}
}
}
@@ -981,7 +1007,13 @@ void Planner::forward_pass() {
// Skip SYNC blocks
if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
forward_pass_kernel(previous, current, block_index);
// If there's no previous block or the previous block is not
// BUSY (thus, modifiable) run the forward_pass_kernel. Otherwise,
// the previous block became BUSY, so assume the current block's
// entry speed can't be altered (since that would also require
// updating the exit speed of the previous block).
if (!previous || !stepper.is_block_busy(previous))
forward_pass_kernel(previous, current, block_index);
previous = current;
}
// Advance to the previous
@@ -996,16 +1028,15 @@ void Planner::forward_pass() {
*/
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
// recalculate the head block (as it needs to be specially handled), scan backwards until
// we find the first non SYNC block
uint8_t head_block_index = block_buffer_head;
uint8_t block_index = block_buffer_tail,
head_block_index = block_buffer_head;
// Since there could be a sync block in the head of the queue, and the
// next loop must not recalculate the head block (as it needs to be
// specially handled), scan backwards to the first non-SYNC block.
while (head_block_index != block_index) {
// Go back (head always point to the first free block)
uint8_t prev_index = prev_block_index(head_block_index);
const uint8_t prev_index = prev_block_index(head_block_index);
// Get the pointer to the block
block_t *prev = &block_buffer[prev_index];
@@ -1015,7 +1046,7 @@ void Planner::recalculate_trapezoids() {
// Examine the previous block. This and all following are SYNC blocks
head_block_index = prev_index;
};
}
// Go from the tail (currently executed block) to the first block, without including it)
block_t *current = NULL, *next = NULL;
@@ -1037,17 +1068,24 @@ void Planner::recalculate_trapezoids() {
// RECALCULATE yet, but the next one is. That's the reason for the following line.
SBI(current->flag, BLOCK_BIT_RECALCULATE);
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
nomr = 1.0 / current_nominal_speed;
calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
#if ENABLED(LIN_ADVANCE)
if (current->use_advance_lead) {
const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
current->max_adv_steps = current_nominal_speed * comp;
current->final_adv_steps = next_entry_speed * comp;
}
#endif
// But there is an inherent race condition here, as the block maybe
// became BUSY, just before it was marked as RECALCULATE, so check
// if that is the case!
if (!stepper.is_block_busy(current)) {
// Block is not BUSY, we won the race against the Stepper ISR:
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
nomr = 1.0 / current_nominal_speed;
calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
#if ENABLED(LIN_ADVANCE)
if (current->use_advance_lead) {
const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
current->max_adv_steps = current_nominal_speed * comp;
current->final_adv_steps = next_entry_speed * comp;
}
#endif
}
// Reset current only to ensure next trapezoid is computed - The
// stepper is free to use the block from now on.
@@ -1070,16 +1108,23 @@ void Planner::recalculate_trapezoids() {
// marked as RECALCULATE yet. That's the reason for the following line.
SBI(next->flag, BLOCK_BIT_RECALCULATE);
const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
nomr = 1.0 / next_nominal_speed;
calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
#if ENABLED(LIN_ADVANCE)
if (next->use_advance_lead) {
const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
next->max_adv_steps = next_nominal_speed * comp;
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
}
#endif
// But there is an inherent race condition here, as the block maybe
// became BUSY, just before it was marked as RECALCULATE, so check
// if that is the case!
if (!stepper.is_block_busy(current)) {
// Block is not BUSY, we won the race against the Stepper ISR:
const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
nomr = 1.0 / next_nominal_speed;
calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
#if ENABLED(LIN_ADVANCE)
if (next->use_advance_lead) {
const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
next->max_adv_steps = next_nominal_speed * comp;
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
}
#endif
}
// Reset next only to ensure its trapezoid is computed - The stepper is free to use
// the block from now on.
@@ -1423,7 +1468,7 @@ void Planner::quick_stop() {
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
// Drop all queue entries
block_buffer_planned = block_buffer_head = block_buffer_tail;
block_buffer_nonbusy = 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.
@@ -1906,7 +1951,8 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
float inverse_secs = fr_mm_s * inverse_millimeters;
const uint8_t moves_queued = movesplanned();
// Get the number of non busy movements in queue (non busy means that they can be altered)
const uint8_t moves_queued = nonbusy_movesplanned();
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
#if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)

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

@@ -54,9 +54,6 @@ enum BlockFlagBit : char {
// from a safe speed (in consideration of jerking from zero speed).
BLOCK_BIT_NOMINAL_LENGTH,
// The block is busy, being interpreted by the stepper ISR
BLOCK_BIT_BUSY,
// The block is segment 2+ of a longer move
BLOCK_BIT_CONTINUED,
@@ -67,7 +64,6 @@ enum BlockFlagBit : char {
enum BlockFlag : char {
BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE),
BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH),
BLOCK_FLAG_BUSY = _BV(BLOCK_BIT_BUSY),
BLOCK_FLAG_CONTINUED = _BV(BLOCK_BIT_CONTINUED),
BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION)
};
@@ -83,7 +79,7 @@ enum BlockFlag : char {
*/
typedef struct {
uint8_t flag; // Block flags (See BlockFlag enum above)
volatile uint8_t flag; // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!
// Fields used by the motion planner to manage acceleration
float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2
@@ -175,10 +171,12 @@ class Planner {
*/
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_nonbusy, // Index of the first non busy block
block_buffer_planned, // Index of the optimally planned block
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
block_buffer_planned; // Index of the optimally planned block
static uint8_t delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
#if ENABLED(DISTINCT_E_FACTORS)
static uint8_t last_extruder; // Respond to extruder change
@@ -443,11 +441,14 @@ class Planner {
#define ARG_Z const float &rz
#endif
// Number of moves currently in the planner
// Number of moves currently in the planner including the busy block, if any
FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
// Number of nonbusy moves currently in the planner
FORCE_INLINE static uint8_t nonbusy_movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_nonbusy); }
// Remove all blocks from the buffer
FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail = 0; }
// Check if movement queue is full
FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
@@ -649,7 +650,7 @@ class Planner {
static block_t* get_current_block() {
// Get the number of moves in the planner queue so far
uint8_t nr_moves = movesplanned();
const uint8_t nr_moves = movesplanned();
// If there are any moves queued ...
if (nr_moves) {
@@ -673,8 +674,14 @@ class Planner {
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.
#endif
// Mark the block as busy, so the planner does not attempt to replan it
SBI(block->flag, BLOCK_BIT_BUSY);
// As this block is busy, advance the nonbusy block pointer
block_buffer_nonbusy = next_block_index(block_buffer_tail);
// Push block_buffer_planned pointer, if encountered.
if (block_buffer_tail == block_buffer_planned)
block_buffer_planned = block_buffer_nonbusy;
// Return the block
return block;
}
@@ -692,14 +699,8 @@ class Planner {
* NB: There MUST be a current block to call this function!!
*/
FORCE_INLINE static void discard_current_block() {
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 (has_blocks_queued())
block_buffer_tail = next_block_index(block_buffer_tail);
}
#if ENABLED(ULTRA_LCD)

33
Marlin/src/module/stepper.cpp
View File

@@ -107,8 +107,6 @@ Stepper stepper; // Singleton
// public:
block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
bool Stepper::homing_dual_axis = false;
#endif
@@ -119,6 +117,8 @@ block_t* Stepper::current_block = NULL; // A pointer to the block currently bei
// private:
block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
uint8_t Stepper::last_direction_bits = 0,
Stepper::axis_did_move;
@@ -1665,6 +1665,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
acceleration_time = deceleration_time = 0;
uint8_t oversampling = 0; // Assume we won't use it
#if ENABLED(ADAPTIVE_STEP_SMOOTHING)
// At this point, we must decide if we can use Stepper movement axis smoothing.
uint32_t max_rate = current_block->nominal_rate; // Get the maximum rate (maximum event speed)
@@ -1874,6 +1875,34 @@ uint32_t Stepper::stepper_block_phase_isr() {
}
#endif // LIN_ADVANCE
// Check if the given block is busy or not - Must not be called from ISR contexts
// The current_block could change in the middle of the read by an Stepper ISR, so
// we must explicitly prevent that!
bool Stepper::is_block_busy(const block_t* const block) {
#ifdef __AVR__
// A SW memory barrier, to ensure GCC does not overoptimize loops
#define sw_barrier() asm volatile("": : :"memory");
// Keep reading until 2 consecutive reads return the same value,
// meaning there was no update in-between caused by an interrupt.
// This works because stepper ISRs happen at a slower rate than
// successive reads of a variable, so 2 consecutive reads with
// the same value means no interrupt updated it.
block_t* vold, *vnew = current_block;
sw_barrier();
do {
vold = vnew;
vnew = current_block;
sw_barrier();
} while (vold != vnew);
#else
block_t *vnew = current_block;
#endif
// Return if the block is busy or not
return block == vnew;
}
void Stepper::init() {
// Init Digipot Motor Current

7
Marlin/src/module/stepper.h
View File

@@ -234,8 +234,6 @@ class Stepper {
public:
static block_t* current_block; // A pointer to the block currently being traced
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
static bool homing_dual_axis;
#endif
@@ -249,6 +247,8 @@ class Stepper {
private:
static block_t* current_block; // A pointer to the block currently being traced
static uint8_t last_direction_bits, // The next stepping-bits to be output
axis_did_move; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
@@ -360,6 +360,9 @@ class Stepper {
static uint32_t advance_isr();
#endif
// Check if the given block is busy or not - Must not be called from ISR contexts
static bool is_block_busy(const block_t* const block);
// Get the position of a stepper, in steps
static int32_t position(const AxisEnum axis);