SunRed/Marlin-Artillery-M600
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases 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
Show stats 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 * A ring buffer of moves described in steps
*/ */
block_t Planner::block_buffer[BLOCK_BUFFER_SIZE]; block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed 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 Planner::block_buffer_nonbusy, // Index of the first non-busy block
uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks Planner::block_buffer_planned, // Index of the optimally planned block
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_tail; // Index of the busy block, if any
Planner::block_buffer_planned; // Index of the optimally planned block 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 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 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(); bed_level_matrix.set_to_identity();
#endif #endif
clear_block_buffer(); clear_block_buffer();
block_buffer_planned = 0;
delay_before_delivering = 0; delay_before_delivering = 0;
} }
@ -703,6 +703,12 @@ void Planner::init() {
/** /**
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors. * 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) { 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; cruise_rate = block->nominal_rate;
#endif #endif
// block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps;
#if ENABLED(S_CURVE_ACCELERATION) #if ENABLED(S_CURVE_ACCELERATION)
// Jerk controlled speed requires to express speed versus time, NOT steps // Jerk controlled speed requires to express speed versus time, NOT steps
uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE), 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 // 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 acceleration_time_inverse = get_period_inverse(acceleration_time);
uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time); uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time);
#endif #endif
// Fill variables used by the stepper in a critical section // Store new block parameters
const bool was_enabled = STEPPER_ISR_ENABLED(); block->accelerate_until = accelerate_steps;
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT(); block->decelerate_after = accelerate_steps + plateau_steps;
block->initial_rate = initial_rate;
// Don't update variables if block is busy; it is being interpreted by the planner. #if ENABLED(S_CURVE_ACCELERATION)
// If this happens, there's a problem... The block speed is inconsistent. Some values block->acceleration_time = acceleration_time;
// have already been updated, but the Stepper ISR is already using the block. Fortunately, block->deceleration_time = deceleration_time;
// the values being used by the Stepper ISR weren't touched, so just stop here... block->acceleration_time_inverse = acceleration_time_inverse;
// TODO: There may be a way to update a running block, depending on the stepper ISR position. block->deceleration_time_inverse = deceleration_time_inverse;
if (!TEST(block->flag, BLOCK_BIT_BUSY)) { block->cruise_rate = cruise_rate;
block->accelerate_until = accelerate_steps; #endif
block->decelerate_after = accelerate_steps + plateau_steps; block->final_rate = final_rate;
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();
} }
/* PLANNER SPEED DEFINITION /* 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 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, 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 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 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 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 // ISR does not consume the block before being recalculated
SBI(current->flag, BLOCK_BIT_RECALCULATE); SBI(current->flag, BLOCK_BIT_RECALCULATE);
// Set the new entry speed // But there is an inherent race condition here, as the block may have
current->entry_speed_sqr = new_entry_speed_sqr; // 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 // 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. // 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. // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
block_t *current;
const block_t *next = NULL; const block_t *next = NULL;
while (block_index != planned_block_index) { while (block_index != planned_block_index) {
// Perform the reverse pass // Perform the reverse pass
current = &block_buffer[block_index]; block_t *current = &block_buffer[block_index];
// Only consider non sync blocks // Only consider non sync blocks
if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) { if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
@ -917,6 +918,18 @@ void Planner::reverse_pass() {
// Advance to the next // Advance to the next
block_index = prev_block_index(block_index); 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 // so the stepper ISR does not consume the block before being recalculated
SBI(current->flag, BLOCK_BIT_RECALCULATE); SBI(current->flag, BLOCK_BIT_RECALCULATE);
// Always <= max_entry_speed_sqr. Backward pass sets this. // But there is an inherent race condition here, as the block maybe
current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. // 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. // Always <= max_entry_speed_sqr. Backward pass sets this.
block_buffer_planned = block_index; 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 // Skip SYNC blocks
if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) { 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; previous = current;
} }
// Advance to the previous // Advance to the previous
@ -996,16 +1028,15 @@ void Planner::forward_pass() {
*/ */
void Planner::recalculate_trapezoids() { void Planner::recalculate_trapezoids() {
// The tail may be changed by the ISR so get a local copy. // The tail may be changed by the ISR so get a local copy.
uint8_t block_index = block_buffer_tail; uint8_t block_index = block_buffer_tail,
head_block_index = block_buffer_head;
// As there could be a sync block in the head of the queue, and the next loop must not // Since there could be a sync block in the head of the queue, and the
// recalculate the head block (as it needs to be specially handled), scan backwards until // next loop must not recalculate the head block (as it needs to be
// we find the first non SYNC block // specially handled), scan backwards to the first non-SYNC block.
uint8_t head_block_index = block_buffer_head;
while (head_block_index != block_index) { while (head_block_index != block_index) {
// Go back (head always point to the first free block) // 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 // Get the pointer to the block
block_t *prev = &block_buffer[prev_index]; 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 // Examine the previous block. This and all following are SYNC blocks
head_block_index = prev_index; head_block_index = prev_index;
}; }
// Go from the tail (currently executed block) to the first block, without including it) // Go from the tail (currently executed block) to the first block, without including it)
block_t *current = NULL, *next = NULL; 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. // RECALCULATE yet, but the next one is. That's the reason for the following line.
SBI(current->flag, BLOCK_BIT_RECALCULATE); SBI(current->flag, BLOCK_BIT_RECALCULATE);
// NOTE: Entry and exit factors always > 0 by all previous logic operations. // But there is an inherent race condition here, as the block maybe
const float current_nominal_speed = SQRT(current->nominal_speed_sqr), // became BUSY, just before it was marked as RECALCULATE, so check
nomr = 1.0 / current_nominal_speed; // if that is the case!
calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr); if (!stepper.is_block_busy(current)) {
#if ENABLED(LIN_ADVANCE) // Block is not BUSY, we won the race against the Stepper ISR:
if (current->use_advance_lead) {
const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS]; // NOTE: Entry and exit factors always > 0 by all previous logic operations.
current->max_adv_steps = current_nominal_speed * comp; const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
current->final_adv_steps = next_entry_speed * comp; nomr = 1.0 / current_nominal_speed;
} calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
#endif #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 // Reset current only to ensure next trapezoid is computed - The
// stepper is free to use the block from now on. // 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. // marked as RECALCULATE yet. That's the reason for the following line.
SBI(next->flag, BLOCK_BIT_RECALCULATE); SBI(next->flag, BLOCK_BIT_RECALCULATE);
const float next_nominal_speed = SQRT(next->nominal_speed_sqr), // But there is an inherent race condition here, as the block maybe
nomr = 1.0 / next_nominal_speed; // became BUSY, just before it was marked as RECALCULATE, so check
calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr); // if that is the case!
#if ENABLED(LIN_ADVANCE) if (!stepper.is_block_busy(current)) {
if (next->use_advance_lead) { // Block is not BUSY, we won the race against the Stepper ISR:
const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
next->max_adv_steps = next_nominal_speed * comp; const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp; nomr = 1.0 / next_nominal_speed;
} calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
#endif #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 // Reset next only to ensure its trapezoid is computed - The stepper is free to use
// the block from now on. // the block from now on.
@ -1423,7 +1468,7 @@ void Planner::quick_stop() {
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
// Drop all queue entries // 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 // Restart the block delay for the first movement - As the queue was
// forced to empty, there's no risk the ISR will touch this. // 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. // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
float inverse_secs = fr_mm_s * inverse_millimeters; 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 // 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) #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). // from a safe speed (in consideration of jerking from zero speed).
BLOCK_BIT_NOMINAL_LENGTH, 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 // The block is segment 2+ of a longer move
BLOCK_BIT_CONTINUED, BLOCK_BIT_CONTINUED,
@ -67,7 +64,6 @@ enum BlockFlagBit : char {
enum BlockFlag : char { enum BlockFlag : char {
BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE), BLOCK_FLAG_RECALCULATE = _BV(BLOCK_BIT_RECALCULATE),
BLOCK_FLAG_NOMINAL_LENGTH = _BV(BLOCK_BIT_NOMINAL_LENGTH), 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_CONTINUED = _BV(BLOCK_BIT_CONTINUED),
BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION) BLOCK_FLAG_SYNC_POSITION = _BV(BLOCK_BIT_SYNC_POSITION)
}; };
@ -83,7 +79,7 @@ enum BlockFlag : char {
*/ */
typedef struct { 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 // Fields used by the motion planner to manage acceleration
float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2 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 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_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 block_buffer_tail; // Index of the busy block, if any
static uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocks 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 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
#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
@ -443,11 +441,14 @@ class Planner {
#define ARG_Z const float &rz #define ARG_Z const float &rz
#endif #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); } 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 // 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 // Check if movement queue is full
FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); } 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() { static block_t* get_current_block() {
// Get the number of moves in the planner queue so far // 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 there are any moves queued ...
if (nr_moves) { 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. 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 #endif
// Mark the block as busy, so the planner does not attempt to replan it // As this block is busy, advance the nonbusy block pointer
SBI(block->flag, BLOCK_BIT_BUSY); 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; return block;
} }
@ -692,14 +699,8 @@ class Planner {
* NB: There MUST be a current block to call this function!! * NB: There MUST be a current block to call this function!!
*/ */
FORCE_INLINE static void discard_current_block() { FORCE_INLINE static void discard_current_block() {
if (has_blocks_queued()) { // Discard non-empty buffer. if (has_blocks_queued())
uint8_t block_index = next_block_index( block_buffer_tail ); block_buffer_tail = 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 ENABLED(ULTRA_LCD) #if ENABLED(ULTRA_LCD)

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

@ -107,8 +107,6 @@ Stepper stepper; // Singleton
// public: // 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) #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
bool Stepper::homing_dual_axis = false; bool Stepper::homing_dual_axis = false;
#endif #endif
@ -119,6 +117,8 @@ block_t* Stepper::current_block = NULL; // A pointer to the block currently bei
// private: // private:
block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
uint8_t Stepper::last_direction_bits = 0, uint8_t Stepper::last_direction_bits = 0,
Stepper::axis_did_move; Stepper::axis_did_move;
@ -1665,6 +1665,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
acceleration_time = deceleration_time = 0; acceleration_time = deceleration_time = 0;
uint8_t oversampling = 0; // Assume we won't use it uint8_t oversampling = 0; // Assume we won't use it
#if ENABLED(ADAPTIVE_STEP_SMOOTHING) #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
// At this point, we must decide if we can use Stepper movement axis 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) 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 #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() { void Stepper::init() {
// Init Digipot Motor Current // Init Digipot Motor Current

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

@ -234,8 +234,6 @@ class Stepper {
public: 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) #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
static bool homing_dual_axis; static bool homing_dual_axis;
#endif #endif
@ -249,6 +247,8 @@ class Stepper {
private: 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 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 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(); static uint32_t advance_isr();
#endif #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 // Get the position of a stepper, in steps
static int32_t position(const AxisEnum axis); static int32_t position(const AxisEnum axis);