Clean up stepper and babystep (#16857)

This commit is contained in:
Scott Lahteine 2020-02-14 05:14:37 -06:00 committed by GitHub
parent 073e4443e8
commit 0b984519c3
Signed by: GitHub
GPG key ID: 4AEE18F83AFDEB23
8 changed files with 242 additions and 240 deletions

View file

@ -153,8 +153,8 @@ void stepperTask(void* parameter) {
remaining--; remaining--;
} }
else { else {
Stepper::stepper_pulse_phase_isr(); Stepper::pulse_phase_isr();
remaining = Stepper::stepper_block_phase_isr(); remaining = Stepper::block_phase_isr();
} }
} }
} }

View file

@ -49,14 +49,6 @@ void Babystep::step_axis(const AxisEnum axis) {
} }
} }
void Babystep::task() {
#if EITHER(BABYSTEP_XY, I2C_POSITION_ENCODERS)
LOOP_XYZ(axis) step_axis((AxisEnum)axis);
#else
step_axis(Z_AXIS);
#endif
}
void Babystep::add_mm(const AxisEnum axis, const float &mm) { void Babystep::add_mm(const AxisEnum axis, const float &mm) {
add_steps(axis, mm * planner.settings.axis_steps_per_mm[axis]); add_steps(axis, mm * planner.settings.axis_steps_per_mm[axis]);
} }

View file

@ -55,7 +55,15 @@ public:
static void add_steps(const AxisEnum axis, const int16_t distance); static void add_steps(const AxisEnum axis, const int16_t distance);
static void add_mm(const AxisEnum axis, const float &mm); static void add_mm(const AxisEnum axis, const float &mm);
static void task();
//
// Called by the Temperature ISR to
// apply accumulated babysteps to the axes.
//
static inline void task() {
LOOP_L_N(axis, BS_TODO_AXIS(Z_AXIS)) step_axis((AxisEnum)axis);
}
private: private:
static void step_axis(const AxisEnum axis); static void step_axis(const AxisEnum axis);
}; };

View file

@ -709,6 +709,59 @@ void Planner::init() {
#define MINIMAL_STEP_RATE 120 #define MINIMAL_STEP_RATE 120
/**
* Get the current block for processing
* and mark the block as busy.
* Return nullptr if the buffer is empty
* or if there is a first-block delay.
*
* WARNING: Called from Stepper ISR context!
*/
block_t* Planner::get_current_block() {
// Get the number of moves in the planner queue so far
const uint8_t nr_moves = movesplanned();
// If there are any moves queued ...
if (nr_moves) {
// If there is still delay of delivery of blocks running, decrement it
if (delay_before_delivering) {
--delay_before_delivering;
// If the number of movements queued is less than 3, and there is still time
// to wait, do not deliver anything
if (nr_moves < 3 && delay_before_delivering) return nullptr;
delay_before_delivering = 0;
}
// If we are here, there is no excuse to deliver the block
block_t * const block = &block_buffer[block_buffer_tail];
// No trapezoid calculated? Don't execute yet.
if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return nullptr;
#if HAS_SPI_LCD
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
// 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;
}
// The queue became empty
#if HAS_SPI_LCD
clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
#endif
return nullptr;
}
/** /**
* 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.
@ -1498,8 +1551,7 @@ void Planner::quick_stop() {
// must be handled: The tail could change between the read and the assignment // must be handled: The tail could change between the read and the assignment
// so this must be enclosed in a critical section // so this must be enclosed in a critical section
const bool was_enabled = STEPPER_ISR_ENABLED(); const bool was_enabled = stepper.suspend();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
// Drop all queue entries // Drop all queue entries
block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail; block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail;
@ -1517,7 +1569,7 @@ void Planner::quick_stop() {
cleaning_buffer_counter = 1000; cleaning_buffer_counter = 1000;
// Reenable Stepper ISR // Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) stepper.wake_up();
// And stop the stepper ISR // And stop the stepper ISR
stepper.quick_stop(); stepper.quick_stop();
@ -1548,13 +1600,12 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) { if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
// Protect the access to the position. // Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED(); const bool was_enabled = stepper.suspend();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
const int32_t p1 = stepper.position(CORE_AXIS_1), const int32_t p1 = stepper.position(CORE_AXIS_1),
p2 = stepper.position(CORE_AXIS_2); p2 = stepper.position(CORE_AXIS_2);
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) stepper.wake_up();
// ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1 // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
// ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2 // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
@ -2004,13 +2055,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
#if HAS_SPI_LCD #if HAS_SPI_LCD
// Protect the access to the position. // Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED(); const bool was_enabled = stepper.suspend();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
block_buffer_runtime_us += segment_time_us; block_buffer_runtime_us += segment_time_us;
block->segment_time_us = segment_time_us; block->segment_time_us = segment_time_us;
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) stepper.wake_up();
#endif #endif
block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0 block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
@ -2822,6 +2872,48 @@ void Planner::set_max_jerk(const AxisEnum axis, float targetValue) {
#endif #endif
} }
#if HAS_SPI_LCD
uint16_t Planner::block_buffer_runtime() {
#ifdef __AVR__
// Protect the access to the variable. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables
const bool was_enabled = stepper.suspend();
#endif
millis_t bbru = block_buffer_runtime_us;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) stepper.wake_up();
#endif
// To translate µs to ms a division by 1000 would be required.
// We introduce 2.4% error here by dividing by 1024.
// Doesn't matter because block_buffer_runtime_us is already too small an estimation.
bbru >>= 10;
// limit to about a minute.
NOMORE(bbru, 0xFFFFul);
return bbru;
}
void Planner::clear_block_buffer_runtime() {
#ifdef __AVR__
// Protect the access to the variable. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables
const bool was_enabled = stepper.suspend();
#endif
block_buffer_runtime_us = 0;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) stepper.wake_up();
#endif
}
#endif
#if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP)
void Planner::autotemp_M104_M109() { void Planner::autotemp_M104_M109() {

View file

@ -763,60 +763,18 @@ class Planner {
FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); } FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
/** /**
* The current block. nullptr if the buffer is empty. * Get the current block for processing
* This also marks the block as busy. * and mark the block as busy.
* Return nullptr if the buffer is empty
* or if there is a first-block delay.
*
* WARNING: Called from Stepper ISR context! * WARNING: Called from Stepper ISR context!
*/ */
static block_t* get_current_block() { static block_t* get_current_block();
// Get the number of moves in the planner queue so far
const uint8_t nr_moves = movesplanned();
// If there are any moves queued ...
if (nr_moves) {
// If there is still delay of delivery of blocks running, decrement it
if (delay_before_delivering) {
--delay_before_delivering;
// If the number of movements queued is less than 3, and there is still time
// to wait, do not deliver anything
if (nr_moves < 3 && delay_before_delivering) return nullptr;
delay_before_delivering = 0;
}
// If we are here, there is no excuse to deliver the block
block_t * const block = &block_buffer[block_buffer_tail];
// No trapezoid calculated? Don't execute yet.
if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return nullptr;
#if HAS_SPI_LCD
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
// 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;
}
// The queue became empty
#if HAS_SPI_LCD
clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
#endif
return nullptr;
}
/** /**
* "Discard" the block and "release" the memory. * "Discard" the block and "release" the memory.
* Called when the current block is no longer needed. * Called when the current block is no longer needed.
* 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()) if (has_blocks_queued())
@ -824,47 +782,8 @@ class Planner {
} }
#if HAS_SPI_LCD #if HAS_SPI_LCD
static uint16_t block_buffer_runtime();
static uint16_t block_buffer_runtime() { static void clear_block_buffer_runtime();
#ifdef __AVR__
// Protect the access to the variable. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables
bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif
millis_t bbru = block_buffer_runtime_us;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
// To translate µs to ms a division by 1000 would be required.
// We introduce 2.4% error here by dividing by 1024.
// Doesn't matter because block_buffer_runtime_us is already too small an estimation.
bbru >>= 10;
// limit to about a minute.
NOMORE(bbru, 0xFFFFul);
return bbru;
}
static void clear_block_buffer_runtime() {
#ifdef __AVR__
// Protect the access to the variable. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables
bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif
block_buffer_runtime_us = 0;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
}
#endif #endif
#if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP)

View file

@ -203,11 +203,8 @@ uint32_t Stepper::advance_divisor = 0,
bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not
#endif #endif
uint32_t Stepper::nextMainISR = 0;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
uint32_t Stepper::nextAdvanceISR = LA_ADV_NEVER, uint32_t Stepper::nextAdvanceISR = LA_ADV_NEVER,
Stepper::LA_isr_rate = LA_ADV_NEVER; Stepper::LA_isr_rate = LA_ADV_NEVER;
uint16_t Stepper::LA_current_adv_steps = 0, uint16_t Stepper::LA_current_adv_steps = 0,
@ -402,13 +399,13 @@ constexpr uint32_t NS_TO_PULSE_TIMER_TICKS(uint32_t NS) { return (NS + (NS_PER_P
#define PULSE_HIGH_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_HIGH_NS - _MIN(_MIN_PULSE_HIGH_NS, TIMER_SETUP_NS))) #define PULSE_HIGH_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_HIGH_NS - _MIN(_MIN_PULSE_HIGH_NS, TIMER_SETUP_NS)))
#define PULSE_LOW_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_LOW_NS - _MIN(_MIN_PULSE_LOW_NS, TIMER_SETUP_NS))) #define PULSE_LOW_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_LOW_NS - _MIN(_MIN_PULSE_LOW_NS, TIMER_SETUP_NS)))
#define USING_TIMED_PULSE() hal_timer_t end_tick_count = 0 #define USING_TIMED_PULSE() hal_timer_t start_pulse_count = 0
#define START_TIMED_PULSE(DIR) (end_tick_count = HAL_timer_get_count(PULSE_TIMER_NUM) + PULSE_##DIR##_TICK_COUNT) #define START_TIMED_PULSE(DIR) (start_pulse_count = HAL_timer_get_count(PULSE_TIMER_NUM))
#define AWAIT_TIMED_PULSE() while (HAL_timer_get_count(PULSE_TIMER_NUM) < end_tick_count) { } #define AWAIT_TIMED_PULSE(DIR) while (PULSE_##DIR##_TICK_COUNT > HAL_timer_get_count(PULSE_TIMER_NUM) - start_pulse_count) { }
#define START_HIGH_PULSE() START_TIMED_PULSE(HIGH) #define START_HIGH_PULSE() START_TIMED_PULSE(HIGH)
#define AWAIT_HIGH_PULSE() AWAIT_TIMED_PULSE(HIGH)
#define START_LOW_PULSE() START_TIMED_PULSE(LOW) #define START_LOW_PULSE() START_TIMED_PULSE(LOW)
#define AWAIT_HIGH_PULSE() AWAIT_TIMED_PULSE() #define AWAIT_LOW_PULSE() AWAIT_TIMED_PULSE(LOW)
#define AWAIT_LOW_PULSE() AWAIT_TIMED_PULSE()
#if MINIMUM_STEPPER_PRE_DIR_DELAY > 0 #if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
#define DIR_WAIT_BEFORE() DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY) #define DIR_WAIT_BEFORE() DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY)
@ -422,11 +419,6 @@ constexpr uint32_t NS_TO_PULSE_TIMER_TICKS(uint32_t NS) { return (NS + (NS_PER_P
#define DIR_WAIT_AFTER() #define DIR_WAIT_AFTER()
#endif #endif
void Stepper::wake_up() {
// TCNT1 = 0;
ENABLE_STEPPER_DRIVER_INTERRUPT();
}
/** /**
* Set the stepper direction of each axis * Set the stepper direction of each axis
* *
@ -1334,6 +1326,9 @@ HAL_STEP_TIMER_ISR() {
#endif #endif
void Stepper::isr() { void Stepper::isr() {
static uint32_t nextMainISR = 0; // Interval until the next main Stepper Pulse phase (0 = Now)
#ifndef __AVR__ #ifndef __AVR__
// Disable interrupts, to avoid ISR preemption while we reprogram the period // Disable interrupts, to avoid ISR preemption while we reprogram the period
// (AVR enters the ISR with global interrupts disabled, so no need to do it here) // (AVR enters the ISR with global interrupts disabled, so no need to do it here)
@ -1357,35 +1352,35 @@ void Stepper::isr() {
// Enable ISRs to reduce USART processing latency // Enable ISRs to reduce USART processing latency
ENABLE_ISRS(); ENABLE_ISRS();
// Run main stepping pulse phase ISR if we have to if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses
if (!nextMainISR) Stepper::stepper_pulse_phase_isr();
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
// Run linear advance stepper ISR if we have to if (!nextAdvanceISR) nextAdvanceISR = advance_isr(); // 0 = Do Linear Advance E Stepper pulses
if (!nextAdvanceISR) nextAdvanceISR = Stepper::advance_isr();
#endif #endif
// ^== Time critical. NOTHING besides pulse generation should be above here!!! // ^== Time critical. NOTHING besides pulse generation should be above here!!!
// Run main stepping block processing ISR if we have to if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block
if (!nextMainISR) nextMainISR = Stepper::stepper_block_phase_isr();
uint32_t interval = // Get the interval to the next ISR call
const uint32_t interval = _MIN(
nextMainISR // Time until the next Stepper ISR
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
_MIN(nextAdvanceISR, nextMainISR) // Nearest time interval , nextAdvanceISR // Come back early for Linear Advance?
#else
nextMainISR // Remaining stepper ISR time
#endif #endif
; , uint32_t(HAL_TIMER_TYPE_MAX) // Come back in a very long time
);
// Limit the value to the maximum possible value of the timer //
NOMORE(interval, uint32_t(HAL_TIMER_TYPE_MAX)); // Compute remaining time for each ISR phase
// NEVER : The phase is idle
// Zero : The phase will occur on the next ISR call
// Non-zero : The phase will occur on a future ISR call
//
// Compute the time remaining for the main isr
nextMainISR -= interval; nextMainISR -= interval;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
// Compute the time remaining for the advance isr
if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval; if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval;
#endif #endif
@ -1471,7 +1466,7 @@ void Stepper::isr() {
* call to this method that might cause variation in the timing. The aim * call to this method that might cause variation in the timing. The aim
* is to keep pulse timing as regular as possible. * is to keep pulse timing as regular as possible.
*/ */
void Stepper::stepper_pulse_phase_isr() { void Stepper::pulse_phase_isr() {
// If we must abort the current block, do so! // If we must abort the current block, do so!
if (abort_current_block) { if (abort_current_block) {
@ -1548,7 +1543,7 @@ void Stepper::stepper_pulse_phase_isr() {
// Don't step E here - But remember the number of steps to perform // Don't step E here - But remember the number of steps to perform
motor_direction(E_AXIS) ? --LA_steps : ++LA_steps; motor_direction(E_AXIS) ? --LA_steps : ++LA_steps;
#else #else
step_needed.e = delta_error.e >= 0; step_needed.e = true;
#endif #endif
} }
#elif HAS_E0_STEP #elif HAS_E0_STEP
@ -1604,20 +1599,14 @@ void Stepper::stepper_pulse_phase_isr() {
#if DISABLED(LIN_ADVANCE) #if DISABLED(LIN_ADVANCE)
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
if (delta_error.e >= 0) { if (delta_error.e >= 0) {
delta_error.e -= advance_divisor; delta_error.e -= advance_divisor;
E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN); E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN);
} }
#elif HAS_E0_STEP
#else // !MIXING_EXTRUDER PULSE_STOP(E);
#endif
#if HAS_E0_STEP #endif
PULSE_STOP(E);
#endif
#endif // !MIXING_EXTRUDER
#endif // !LIN_ADVANCE
#if ISR_MULTI_STEPS #if ISR_MULTI_STEPS
if (events_to_do) START_LOW_PULSE(); if (events_to_do) START_LOW_PULSE();
@ -1630,10 +1619,10 @@ void Stepper::stepper_pulse_phase_isr() {
// properly schedules blocks from the planner. This is executed after creating // properly schedules blocks from the planner. This is executed after creating
// the step pulses, so it is not time critical, as pulses are already done. // the step pulses, so it is not time critical, as pulses are already done.
uint32_t Stepper::stepper_block_phase_isr() { uint32_t Stepper::block_phase_isr() {
// If no queued movements, just wait 1ms for the next move // If no queued movements, just wait 1ms for the next block
uint32_t interval = (STEPPER_TIMER_RATE) / 1000; uint32_t interval = (STEPPER_TIMER_RATE) / 1000UL;
// If there is a current block // If there is a current block
if (current_block) { if (current_block) {
@ -1667,16 +1656,14 @@ uint32_t Stepper::stepper_block_phase_isr() {
// acc_step_rate is in steps/second // acc_step_rate is in steps/second
// step_rate to timer interval and steps per stepper isr // step_rate to timer interval and steps per stepper isr
interval = calc_timer_interval(acc_step_rate, oversampling_factor, &steps_per_isr); interval = calc_timer_interval(acc_step_rate, &steps_per_isr);
acceleration_time += interval; acceleration_time += interval;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (LA_use_advance_lead) { // Fire ISR if final adv_rate is reached
// Fire ISR if final adv_rate is reached if (LA_steps && (!LA_use_advance_lead || LA_isr_rate != current_block->advance_speed))
if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0; initiateLA();
} #endif
else if (LA_steps) nextAdvanceISR = 0;
#endif // LIN_ADVANCE
} }
// Are we in Deceleration phase ? // Are we in Deceleration phase ?
else if (step_events_completed > decelerate_after) { else if (step_events_completed > decelerate_after) {
@ -1712,32 +1699,32 @@ uint32_t Stepper::stepper_block_phase_isr() {
// step_rate is in steps/second // step_rate is in steps/second
// step_rate to timer interval and steps per stepper isr // step_rate to timer interval and steps per stepper isr
interval = calc_timer_interval(step_rate, oversampling_factor, &steps_per_isr); interval = calc_timer_interval(step_rate, &steps_per_isr);
deceleration_time += interval; deceleration_time += interval;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (LA_use_advance_lead) { if (LA_use_advance_lead) {
// Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached // Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached
if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) { if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) {
nextAdvanceISR = 0; initiateLA();
LA_isr_rate = current_block->advance_speed; LA_isr_rate = current_block->advance_speed;
} }
} }
else if (LA_steps) nextAdvanceISR = 0; else if (LA_steps) initiateLA();
#endif // LIN_ADVANCE #endif
} }
// We must be in cruise phase otherwise // We must be in cruise phase otherwise
else { else {
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
// If there are any esteps, fire the next advance_isr "now" // If there are any esteps, fire the next advance_isr "now"
if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0; if (LA_steps && LA_isr_rate != current_block->advance_speed) initiateLA();
#endif #endif
// Calculate the ticks_nominal for this nominal speed, if not done yet // Calculate the ticks_nominal for this nominal speed, if not done yet
if (ticks_nominal < 0) { if (ticks_nominal < 0) {
// step_rate to timer interval and loops for the nominal speed // step_rate to timer interval and loops for the nominal speed
ticks_nominal = calc_timer_interval(current_block->nominal_rate, oversampling_factor, &steps_per_isr); ticks_nominal = calc_timer_interval(current_block->nominal_rate, &steps_per_isr);
} }
// The timer interval is just the nominal value for the nominal speed // The timer interval is just the nominal value for the nominal speed
@ -1846,17 +1833,17 @@ uint32_t Stepper::stepper_block_phase_isr() {
// No acceleration / deceleration time elapsed so far // No acceleration / deceleration time elapsed so far
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 no axis smoothing (via oversampling)
#if ENABLED(ADAPTIVE_STEP_SMOOTHING) #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
// At this point, we must decide if we can use Stepper movement axis smoothing. // Decide if axis smoothing is possible
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)
while (max_rate < MIN_STEP_ISR_FREQUENCY) { while (max_rate < MIN_STEP_ISR_FREQUENCY) { // As long as more ISRs are possible...
max_rate <<= 1; max_rate <<= 1; // Try to double the rate
if (max_rate >= MAX_STEP_ISR_FREQUENCY_1X) break; if (max_rate >= MAX_STEP_ISR_FREQUENCY_1X) break; // Don't exceed the estimated ISR limit
++oversampling; ++oversampling; // Increase the oversampling (used for left-shift)
} }
oversampling_factor = oversampling; oversampling_factor = oversampling; // For all timer interval calculations
#endif #endif
// Based on the oversampling factor, do the calculations // Based on the oversampling factor, do the calculations
@ -1894,8 +1881,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
if ((LA_use_advance_lead = current_block->use_advance_lead)) { if ((LA_use_advance_lead = current_block->use_advance_lead)) {
LA_final_adv_steps = current_block->final_adv_steps; LA_final_adv_steps = current_block->final_adv_steps;
LA_max_adv_steps = current_block->max_adv_steps; LA_max_adv_steps = current_block->max_adv_steps;
//Start the ISR initiateLA(); // Start the ISR
nextAdvanceISR = 0;
LA_isr_rate = current_block->advance_speed; LA_isr_rate = current_block->advance_speed;
} }
else LA_isr_rate = LA_ADV_NEVER; else LA_isr_rate = LA_ADV_NEVER;
@ -1954,7 +1940,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
#endif #endif
// Calculate the initial timer interval // Calculate the initial timer interval
interval = calc_timer_interval(current_block->initial_rate, oversampling_factor, &steps_per_isr); interval = calc_timer_interval(current_block->initial_rate, &steps_per_isr);
} }
} }
@ -2054,6 +2040,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
return interval; return interval;
} }
#endif // LIN_ADVANCE #endif // LIN_ADVANCE
// Check if the given block is busy or not - Must not be called from ISR contexts // Check if the given block is busy or not - Must not be called from ISR contexts
@ -2093,7 +2080,7 @@ void Stepper::init() {
digipot_motor = 255 * (motor_current[i] / 2.5); digipot_motor = 255 * (motor_current[i] / 2.5);
dac084s085::setValue(i, digipot_motor); dac084s085::setValue(i, digipot_motor);
} }
#endif//MB(ALLIGATOR) #endif
// Init Microstepping Pins // Init Microstepping Pins
#if HAS_MICROSTEPS #if HAS_MICROSTEPS
@ -2287,7 +2274,7 @@ void Stepper::init() {
#if DISABLED(I2S_STEPPER_STREAM) #if DISABLED(I2S_STEPPER_STREAM)
HAL_timer_start(STEP_TIMER_NUM, 122); // Init Stepper ISR to 122 Hz for quick starting HAL_timer_start(STEP_TIMER_NUM, 122); // Init Stepper ISR to 122 Hz for quick starting
ENABLE_STEPPER_DRIVER_INTERRUPT(); wake_up();
sei(); sei();
#endif #endif
@ -2341,19 +2328,43 @@ int32_t Stepper::position(const AxisEnum axis) {
#ifdef __AVR__ #ifdef __AVR__
// Protect the access to the position. Only required for AVR, as // Protect the access to the position. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables // any 32bit CPU offers atomic access to 32bit variables
const bool was_enabled = STEPPER_ISR_ENABLED(); const bool was_enabled = suspend();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif #endif
const int32_t v = count_position[axis]; const int32_t v = count_position[axis];
#ifdef __AVR__ #ifdef __AVR__
// Reenable Stepper ISR // Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) wake_up();
#endif #endif
return v; return v;
} }
// Set the current position in steps
void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
planner.synchronize();
const bool was_enabled = suspend();
_set_position(a, b, c, e);
if (was_enabled) wake_up();
}
void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
planner.synchronize();
#ifdef __AVR__
// Protect the access to the position. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables
const bool was_enabled = suspend();
#endif
count_position[a] = v;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) wake_up();
#endif
}
// Signal endstops were triggered - This function can be called from // Signal endstops were triggered - This function can be called from
// an ISR context (Temperature, Stepper or limits ISR), so we must // an ISR context (Temperature, Stepper or limits ISR), so we must
// be very careful here. If the interrupt being preempted was the // be very careful here. If the interrupt being preempted was the
@ -2362,8 +2373,7 @@ int32_t Stepper::position(const AxisEnum axis) {
// is properly canceled // is properly canceled
void Stepper::endstop_triggered(const AxisEnum axis) { void Stepper::endstop_triggered(const AxisEnum axis) {
const bool was_enabled = STEPPER_ISR_ENABLED(); const bool was_enabled = suspend();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
endstops_trigsteps[axis] = ( endstops_trigsteps[axis] = (
#if IS_CORE #if IS_CORE
(axis == CORE_AXIS_2 (axis == CORE_AXIS_2
@ -2378,22 +2388,21 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
// Discard the rest of the move if there is a current block // Discard the rest of the move if there is a current block
quick_stop(); quick_stop();
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) wake_up();
} }
int32_t Stepper::triggered_position(const AxisEnum axis) { int32_t Stepper::triggered_position(const AxisEnum axis) {
#ifdef __AVR__ #ifdef __AVR__
// Protect the access to the position. Only required for AVR, as // Protect the access to the position. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables // any 32bit CPU offers atomic access to 32bit variables
const bool was_enabled = STEPPER_ISR_ENABLED(); const bool was_enabled = suspend();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif #endif
const int32_t v = endstops_trigsteps[axis]; const int32_t v = endstops_trigsteps[axis];
#ifdef __AVR__ #ifdef __AVR__
// Reenable Stepper ISR // Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) wake_up();
#endif #endif
return v; return v;
@ -2403,14 +2412,13 @@ void Stepper::report_positions() {
#ifdef __AVR__ #ifdef __AVR__
// Protect the access to the position. // Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED(); const bool was_enabled = suspend();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif #endif
const xyz_long_t pos = count_position; const xyz_long_t pos = count_position;
#ifdef __AVR__ #ifdef __AVR__
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); if (was_enabled) wake_up();
#endif #endif
#if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA #if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
@ -2571,16 +2579,21 @@ void Stepper::report_positions() {
Z_STEP_WRITE(INVERT_Z_STEP_PIN); Z_STEP_WRITE(INVERT_Z_STEP_PIN);
// Restore direction bits // Restore direction bits
DIR_WAIT_BEFORE();
X_DIR_WRITE(old_dir.x); X_DIR_WRITE(old_dir.x);
Y_DIR_WRITE(old_dir.y); Y_DIR_WRITE(old_dir.y);
Z_DIR_WRITE(old_dir.z); Z_DIR_WRITE(old_dir.z);
DIR_WAIT_AFTER();
#endif #endif
} break; } break;
default: break; default: break;
} }
sei(); sei();
} }

View file

@ -321,13 +321,13 @@ class Stepper {
static bool bezier_2nd_half; // If Bézier curve has been initialized or not static bool bezier_2nd_half; // If Bézier curve has been initialized or not
#endif #endif
static uint32_t nextMainISR; // time remaining for the next Step ISR
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
static uint32_t nextAdvanceISR, LA_isr_rate; static uint32_t nextAdvanceISR, LA_isr_rate;
static uint16_t LA_current_adv_steps, LA_final_adv_steps, LA_max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early". static uint16_t LA_current_adv_steps, LA_final_adv_steps, LA_max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
static int8_t LA_steps; static int8_t LA_steps;
static bool LA_use_advance_lead; static bool LA_use_advance_lead;
#endif // LIN_ADVANCE #endif
static int32_t ticks_nominal; static int32_t ticks_nominal;
#if DISABLED(S_CURVE_ACCELERATION) #if DISABLED(S_CURVE_ACCELERATION)
@ -351,28 +351,36 @@ class Stepper {
public: public:
//
// Constructor / initializer
//
Stepper() {};
// Initialize stepper hardware // Initialize stepper hardware
static void init(); static void init();
// Interrupt Service Routines // Interrupt Service Routine and phases
// The stepper subsystem goes to sleep when it runs out of things to execute.
// Call this to notify the subsystem that it is time to go to work.
static inline void wake_up() { ENABLE_STEPPER_DRIVER_INTERRUPT(); }
static inline bool is_awake() { return STEPPER_ISR_ENABLED(); }
static inline bool suspend() {
const bool awake = is_awake();
if (awake) DISABLE_STEPPER_DRIVER_INTERRUPT();
return awake;
}
// The ISR scheduler // The ISR scheduler
static void isr(); static void isr();
// The stepper pulse phase ISR // The stepper pulse ISR phase
static void stepper_pulse_phase_isr(); static void pulse_phase_isr();
// The stepper block processing phase ISR // The stepper block processing ISR phase
static uint32_t stepper_block_phase_isr(); static uint32_t block_phase_isr();
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
// The Linear advance stepper ISR // The Linear advance ISR phase
static uint32_t advance_isr(); static uint32_t advance_isr();
FORCE_INLINE static void initiateLA() { nextAdvanceISR = 0; }
#endif #endif
// Check if the given block is busy or not - Must not be called from ISR contexts // Check if the given block is busy or not - Must not be called from ISR contexts
@ -381,13 +389,14 @@ class Stepper {
// 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);
// Set the current position in steps
static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
static void set_axis_position(const AxisEnum a, const int32_t &v);
// Report the positions of the steppers, in steps // Report the positions of the steppers, in steps
static void report_positions(); static void report_positions();
// The stepper subsystem goes to sleep when it runs out of things to execute. Call this
// to notify the subsystem that it is time to go to work.
static void wake_up();
// Quickly stop all steppers // Quickly stop all steppers
FORCE_INLINE static void quick_stop() { abort_current_block = true; } FORCE_INLINE static void quick_stop() { abort_current_block = true; }
@ -453,34 +462,6 @@ class Stepper {
static void refresh_motor_power(); static void refresh_motor_power();
#endif #endif
// Set the current position in steps
static inline void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
planner.synchronize();
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
_set_position(a, b, c, e);
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
static inline void set_axis_position(const AxisEnum a, const int32_t &v) {
planner.synchronize();
#ifdef __AVR__
// Protect the access to the position. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#endif
count_position[a] = v;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
}
// Set direction bits for all steppers // Set direction bits for all steppers
static void set_directions(); static void set_directions();
@ -490,11 +471,11 @@ class Stepper {
static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e); static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
FORCE_INLINE static void _set_position(const abce_long_t &spos) { _set_position(spos.a, spos.b, spos.c, spos.e); } FORCE_INLINE static void _set_position(const abce_long_t &spos) { _set_position(spos.a, spos.b, spos.c, spos.e); }
FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t scale, uint8_t* loops) { FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t* loops) {
uint32_t timer; uint32_t timer;
// Scale the frequency, as requested by the caller // Scale the frequency, as requested by the caller
step_rate <<= scale; step_rate <<= oversampling_factor;
uint8_t multistep = 1; uint8_t multistep = 1;
#if DISABLED(DISABLE_MULTI_STEPPING) #if DISABLED(DISABLE_MULTI_STEPPING)

View file

@ -65,15 +65,12 @@
#include "../libs/private_spi.h" #include "../libs/private_spi.h"
#endif #endif
#if EITHER(BABYSTEPPING, PID_EXTRUSION_SCALING) #if ENABLED(PID_EXTRUSION_SCALING)
#include "stepper.h" #include "stepper.h"
#endif #endif
#if ENABLED(BABYSTEPPING) #if ENABLED(BABYSTEPPING)
#include "../feature/babystep.h" #include "../feature/babystep.h"
#if ENABLED(BABYSTEP_ALWAYS_AVAILABLE)
#include "../gcode/gcode.h"
#endif
#endif #endif
#include "printcounter.h" #include "printcounter.h"