SunRed/Marlin-Artillery-M600
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Merge pull request #10688 from ejtagle/bugfix-2.0.x

Browse source 
[2.0.x] Refactor, optimization of core planner/stepper/endstops logic
This commit is contained in:
Scott Lahteine 2018-05-20 07:24:39 -05:00 committed by GitHub
commit 16f92dca44
Signed by: GitHub
GPG key ID: 4AEE18F83AFDEB23
37 changed files with 1699 additions and 1174 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

156
Marlin/src/HAL/HAL_AVR/HAL.h
View file

@ -162,24 +162,148 @@ extern "C" {
* (otherwise, characters will be lost due to UART overflow).
* Then: Stepper, Endstops, Temperature, and -finally- all others.
*/
#define HAL_timer_isr_prologue_0 do{ DISABLE_TEMPERATURE_INTERRUPT(); sei(); }while(0)
#define HAL_timer_isr_epilogue_0 do{ cli(); ENABLE_TEMPERATURE_INTERRUPT(); }while(0)
#define HAL_timer_isr_prologue(TIMER_NUM)
#define HAL_timer_isr_epilogue(TIMER_NUM)
#define HAL_timer_isr_prologue_1 \
const bool temp_isr_was_enabled = TEMPERATURE_ISR_ENABLED(); \
do{ \
DISABLE_TEMPERATURE_INTERRUPT(); \
DISABLE_STEPPER_DRIVER_INTERRUPT(); \
sei(); \
}while(0)
/* 18 cycles maximum latency */
#define HAL_STEP_TIMER_ISR \
extern "C" void TIMER1_COMPA_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
extern "C" void TIMER1_COMPA_vect_bottom (void) asm ("TIMER1_COMPA_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
void TIMER1_COMPA_vect (void) { \
__asm__ __volatile__ ( \
A("push r16") /* 2 Save R16 */ \
A("in r16, __SREG__") /* 1 Get SREG */ \
A("push r16") /* 2 Save SREG into stack */ \
A("lds r16, %[timsk0]") /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
A("push r16") /* 2 Save TIMSK0 into the stack */ \
A("andi r16,~%[msk0]") /* 1 Disable the temperature ISR */ \
A("sts %[timsk0], r16") /* 2 And set the new value */ \
A("lds r16, %[timsk1]") /* 2 Load into R0 the stepper timer Interrupt mask register [TIMSK1] */ \
A("andi r16,~%[msk1]") /* 1 Disable the stepper ISR */ \
A("sts %[timsk1], r16") /* 2 And set the new value */ \
A("sei") /* 1 Enable global interrupts - stepper and temperature ISRs are disabled, so no risk of reentry or being preempted by the temperature ISR */ \
A("push r16") /* 2 Save TIMSK1 into stack */ \
A("in r16, 0x3B") /* 1 Get RAMPZ register */ \
A("push r16") /* 2 Save RAMPZ into stack */ \
A("in r16, 0x3C") /* 1 Get EIND register */ \
A("push r0") /* C runtime can modify all the following registers without restoring them */ \
A("push r1") \
A("push r18") \
A("push r19") \
A("push r20") \
A("push r21") \
A("push r22") \
A("push r23") \
A("push r24") \
A("push r25") \
A("push r26") \
A("push r27") \
A("push r30") \
A("push r31") \
A("clr r1") /* C runtime expects this register to be 0 */ \
A("call TIMER1_COMPA_vect_bottom") /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */ \
A("pop r31") \
A("pop r30") \
A("pop r27") \
A("pop r26") \
A("pop r25") \
A("pop r24") \
A("pop r23") \
A("pop r22") \
A("pop r21") \
A("pop r20") \
A("pop r19") \
A("pop r18") \
A("pop r1") \
A("pop r0") \
A("out 0x3C, r16") /* 1 Restore EIND register */ \
A("pop r16") /* 2 Get the original RAMPZ register value */ \
A("out 0x3B, r16") /* 1 Restore RAMPZ register to its original value */ \
A("pop r16") /* 2 Get the original TIMSK1 value but with stepper ISR disabled */ \
A("ori r16,%[msk1]") /* 1 Reenable the stepper ISR */ \
A("cli") /* 1 Disable global interrupts - Reenabling Stepper ISR can reenter amd temperature can reenter, and we want that, if it happens, after this ISR has ended */ \
A("sts %[timsk1], r16") /* 2 And restore the old value - This reenables the stepper ISR */ \
A("pop r16") /* 2 Get the temperature timer Interrupt mask register [TIMSK0] */ \
A("sts %[timsk0], r16") /* 2 And restore the old value - This reenables the temperature ISR */ \
A("pop r16") /* 2 Get the old SREG value */ \
A("out __SREG__, r16") /* 1 And restore the SREG value */ \
A("pop r16") /* 2 Restore R16 value */ \
A("reti") /* 4 Return from interrupt */ \
: \
: [timsk0] "i" ((uint16_t)&TIMSK0), \
[timsk1] "i" ((uint16_t)&TIMSK1), \
[msk0] "M" ((uint8_t)(1<<OCIE0B)),\
[msk1] "M" ((uint8_t)(1<<OCIE1A)) \
: \
); \
} \
void TIMER1_COMPA_vect_bottom(void)
#define HAL_timer_isr_epilogue_1 do{ cli(); ENABLE_STEPPER_DRIVER_INTERRUPT(); if (temp_isr_was_enabled) ENABLE_TEMPERATURE_INTERRUPT(); }while(0)
#define HAL_timer_isr_prologue(TIMER_NUM) _CAT(HAL_timer_isr_prologue_, TIMER_NUM)
#define HAL_timer_isr_epilogue(TIMER_NUM) _CAT(HAL_timer_isr_epilogue_, TIMER_NUM)
#define HAL_STEP_TIMER_ISR ISR(TIMER1_COMPA_vect)
#define HAL_TEMP_TIMER_ISR ISR(TIMER0_COMPB_vect)
/* 14 cycles maximum latency */
#define HAL_TEMP_TIMER_ISR \
extern "C" void TIMER0_COMPB_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
extern "C" void TIMER0_COMPB_vect_bottom(void) asm ("TIMER0_COMPB_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
void TIMER0_COMPB_vect (void) { \
__asm__ __volatile__ ( \
A("push r16") /* 2 Save R16 */ \
A("in r16, __SREG__") /* 1 Get SREG */ \
A("push r16") /* 2 Save SREG into stack */ \
A("lds r16, %[timsk0]") /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
A("andi r16,~%[msk0]") /* 1 Disable the temperature ISR */ \
A("sts %[timsk0], r16") /* 2 And set the new value */ \
A("sei") /* 1 Enable global interrupts - It is safe, as the temperature ISR is disabled, so we cannot reenter it */ \
A("push r16") /* 2 Save TIMSK0 into stack */ \
A("in r16, 0x3B") /* 1 Get RAMPZ register */ \
A("push r16") /* 2 Save RAMPZ into stack */ \
A("in r16, 0x3C") /* 1 Get EIND register */ \
A("push r0") /* C runtime can modify all the following registers without restoring them */ \
A("push r1") \
A("push r18") \
A("push r19") \
A("push r20") \
A("push r21") \
A("push r22") \
A("push r23") \
A("push r24") \
A("push r25") \
A("push r26") \
A("push r27") \
A("push r30") \
A("push r31") \
A("clr r1") /* C runtime expects this register to be 0 */ \
A("call TIMER0_COMPB_vect_bottom") /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */ \
A("pop r31") \
A("pop r30") \
A("pop r27") \
A("pop r26") \
A("pop r25") \
A("pop r24") \
A("pop r23") \
A("pop r22") \
A("pop r21") \
A("pop r20") \
A("pop r19") \
A("pop r18") \
A("pop r1") \
A("pop r0") \
A("out 0x3C, r16") /* 1 Restore EIND register */ \
A("pop r16") /* 2 Get the original RAMPZ register value */ \
A("out 0x3B, r16") /* 1 Restore RAMPZ register to its original value */ \
A("pop r16") /* 2 Get the original TIMSK0 value but with temperature ISR disabled */ \
A("ori r16,%[msk0]") /* 1 Enable temperature ISR */ \
A("cli") /* 1 Disable global interrupts - We must do this, as we will reenable the temperature ISR, and we don´t want to reenter this handler until the current one is done */ \
A("sts %[timsk0], r16") /* 2 And restore the old value */ \
A("pop r16") /* 2 Get the old SREG */ \
A("out __SREG__, r16") /* 1 And restore the SREG value */ \
A("pop r16") /* 2 Restore R16 */ \
A("reti") /* 4 Return from interrupt */ \
: \
: [timsk0] "i"((uint16_t)&TIMSK0), \
[msk0] "M" ((uint8_t)(1<<OCIE0B)) \
: \
); \
} \
void TIMER0_COMPB_vect_bottom(void)
// ADC
#ifdef DIDR2

21
Marlin/src/HAL/HAL_AVR/endstop_interrupts.h
View file

@ -24,7 +24,7 @@
* Endstop Interrupts
*
* Without endstop interrupts the endstop pins must be polled continually in
* the stepper-ISR via endstops.update(), most of the time finding no change.
* the temperature-ISR via endstops.update(), most of the time finding no change.
* With this feature endstops.update() is called only when we know that at
* least one endstop has changed state, saving valuable CPU cycles.
*
@ -40,17 +40,10 @@
#include "../../core/macros.h"
#include <stdint.h>
volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
// Must be reset to 0 by the test function when finished.
// This is what is really done inside the interrupts.
FORCE_INLINE void endstop_ISR_worker( void ) {
e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
}
#include "../../module/endstops.h"
// One ISR for all EXT-Interrupts
void endstop_ISR(void) { endstop_ISR_worker(); }
void endstop_ISR(void) { endstops.check_possible_change(); }
/**
* Patch for pins_arduino.h (...\Arduino\hardware\arduino\avr\variants\mega\pins_arduino.h)
@ -95,19 +88,19 @@ void pciSetup(const int8_t pin) {
// Handlers for pin change interrupts
#ifdef PCINT0_vect
ISR(PCINT0_vect) { endstop_ISR_worker(); }
ISR(PCINT0_vect) { endstop_ISR(); }
#endif
#ifdef PCINT1_vect
ISR(PCINT1_vect) { endstop_ISR_worker(); }
ISR(PCINT1_vect) { endstop_ISR(); }
#endif
#ifdef PCINT2_vect
ISR(PCINT2_vect) { endstop_ISR_worker(); }
ISR(PCINT2_vect) { endstop_ISR(); }
#endif
#ifdef PCINT3_vect
ISR(PCINT3_vect) { endstop_ISR_worker(); }
ISR(PCINT3_vect) { endstop_ISR(); }
#endif
void setup_endstop_interrupts( void ) {

5
Marlin/src/HAL/HAL_DUE/DebugMonitor_Due.cpp
View file

@ -46,6 +46,11 @@ static void TXBegin(void) {
// Disable UART interrupt in NVIC
NVIC_DisableIRQ( UART_IRQn );
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
// Disable clock
pmc_disable_periph_clk( ID_UART );

27
Marlin/src/HAL/HAL_DUE/HAL_timers_Due.cpp
View file

@ -99,6 +99,11 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
// Disable interrupt, just in case it was already enabled
NVIC_DisableIRQ(irq);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
// Disable timer interrupt
tc->TC_CHANNEL[channel].TC_IDR = TC_IDR_CPCS;
@ -126,18 +131,28 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
}
void HAL_timer_enable_interrupt(const uint8_t timer_num) {
const tTimerConfig * const pConfig = &TimerConfig[timer_num];
pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IER = TC_IER_CPCS;
IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
NVIC_EnableIRQ(irq);
}
void HAL_timer_disable_interrupt(const uint8_t timer_num) {
const tTimerConfig * const pConfig = &TimerConfig[timer_num];
pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IDR = TC_IDR_CPCS;
IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
NVIC_DisableIRQ(irq);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
}
// missing from CMSIS: Check if interrupt is enabled or not
static bool NVIC_GetEnabledIRQ(IRQn_Type IRQn) {
return (NVIC->ISER[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F))) != 0;
}
bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {
const tTimerConfig * const pConfig = &TimerConfig[timer_num];
return (pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IMR & TC_IMR_CPCS) != 0;
IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
return NVIC_GetEnabledIRQ(irq);
}
#endif // ARDUINO_ARCH_SAM

2
Marlin/src/HAL/HAL_DUE/HAL_timers_Due.h
View file

@ -118,8 +118,6 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num);
void HAL_timer_disable_interrupt(const uint8_t timer_num);
bool HAL_timer_interrupt_enabled(const uint8_t timer_num);
//void HAL_timer_isr_prologue(const uint8_t timer_num);
FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) {
const tTimerConfig * const pConfig = &TimerConfig[timer_num];
// Reading the status register clears the interrupt flag

10
Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.cpp
View file

@ -245,6 +245,11 @@
// Disable UART interrupt in NVIC
NVIC_DisableIRQ( HWUART_IRQ );
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
// Disable clock
pmc_disable_periph_clk( HWUART_IRQ_ID );
@ -290,6 +295,11 @@
// Disable UART interrupt in NVIC
NVIC_DisableIRQ( HWUART_IRQ );
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
pmc_disable_periph_clk( HWUART_IRQ_ID );
}

12
Marlin/src/HAL/HAL_DUE/endstop_interrupts.h
View file

@ -24,7 +24,7 @@
* Endstop Interrupts
*
* Without endstop interrupts the endstop pins must be polled continually in
* the stepper-ISR via endstops.update(), most of the time finding no change.
* the temperature-ISR via endstops.update(), most of the time finding no change.
* With this feature endstops.update() is called only when we know that at
* least one endstop has changed state, saving valuable CPU cycles.
*
@ -37,16 +37,10 @@
#ifndef _ENDSTOP_INTERRUPTS_H_
#define _ENDSTOP_INTERRUPTS_H_
volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
// Must be reset to 0 by the test function when finished.
// This is what is really done inside the interrupts.
FORCE_INLINE void endstop_ISR_worker( void ) {
e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
}
#include "../../module/endstops.h"
// One ISR for all EXT-Interrupts
void endstop_ISR(void) { endstop_ISR_worker(); }
void endstop_ISR(void) { endstops.check_possible_change(); }
/**
* Endstop interrupts for Due based targets.

5
Marlin/src/HAL/HAL_DUE/watchdog_Due.cpp
View file

@ -68,6 +68,11 @@ void watchdogSetup(void) {
// Disable WDT interrupt (just in case, to avoid triggering it!)
NVIC_DisableIRQ(WDT_IRQn);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
// Initialize WDT with the given parameters
WDT_Enable(WDT, value);

57
Marlin/src/HAL/HAL_LPC1768/HAL_timers.cpp
View file

@ -23,7 +23,7 @@
/**
* Description:
*
* For TARGET_LPC1768
* Timers for LPC1768
*/
#ifdef TARGET_LPC1768
@ -32,62 +32,35 @@
#include "HAL_timers.h"
void HAL_timer_init(void) {
SBI(LPC_SC->PCONP, 1); // power on timer0
SBI(LPC_SC->PCONP, SBIT_TIMER0); // Power ON Timer 0
LPC_TIM0->PR = (HAL_TIMER_RATE) / (HAL_STEPPER_TIMER_RATE) - 1; // Use prescaler to set frequency if needed
SBI(LPC_SC->PCONP, 2); // power on timer1
SBI(LPC_SC->PCONP, SBIT_TIMER1); // Power ON Timer 1
LPC_TIM1->PR = (HAL_TIMER_RATE) / 1000000 - 1;
}
void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
switch (timer_num) {
case 0:
LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0
LPC_TIM0->MCR = _BV(SBIT_MR0I) | _BV(SBIT_MR0R); // Match on MR0, reset on MR0, interrupts when NVIC enables them
LPC_TIM0->MR0 = uint32_t(HAL_STEPPER_TIMER_RATE) / frequency; // Match value (period) to set frequency
LPC_TIM0->TCR = _BV(0); // enable
LPC_TIM0->TCR = _BV(SBIT_CNTEN); // Counter Enable
NVIC_SetPriority(TIMER0_IRQn, NVIC_EncodePriority(0, 1, 0));
NVIC_EnableIRQ(TIMER0_IRQn);
break;
case 1:
LPC_TIM1->MCR = 3;
LPC_TIM1->MCR = _BV(SBIT_MR0I) | _BV(SBIT_MR0R); // Match on MR0, reset on MR0, interrupts when NVIC enables them
LPC_TIM1->MR0 = uint32_t(HAL_TEMP_TIMER_RATE) / frequency;
LPC_TIM1->TCR = _BV(0);
LPC_TIM1->TCR = _BV(SBIT_CNTEN); // Counter Enable
NVIC_SetPriority(TIMER1_IRQn, NVIC_EncodePriority(0, 2, 0));
NVIC_EnableIRQ(TIMER1_IRQn);
break;
default: break;
}
}
void HAL_timer_enable_interrupt(const uint8_t timer_num) {
switch (timer_num) {
case 0:
NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
NVIC_SetPriority(TIMER0_IRQn, NVIC_EncodePriority(0, 1, 0));
break;
case 1:
NVIC_EnableIRQ(TIMER1_IRQn);
NVIC_SetPriority(TIMER1_IRQn, NVIC_EncodePriority(0, 2, 0));
break;
}
}
void HAL_timer_disable_interrupt(const uint8_t timer_num) {
switch (timer_num) {
case 0: NVIC_DisableIRQ(TIMER0_IRQn); break; // disable interrupt handler
case 1: NVIC_DisableIRQ(TIMER1_IRQn); break;
}
}
bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {
switch (timer_num) {
case 0: return NVIC_GetActive(TIMER0_IRQn);
case 1: return NVIC_GetActive(TIMER1_IRQn);
}
return false;
}
void HAL_timer_isr_prologue(const uint8_t timer_num) {
switch (timer_num) {
case 0: SBI(LPC_TIM0->IR, 0); break; // Clear the Interrupt
case 1: SBI(LPC_TIM1->IR, 0); break;
}
}
#endif // TARGET_LPC1768

114
Marlin/src/HAL/HAL_LPC1768/HAL_timers.h
View file

@ -34,18 +34,42 @@
#include <stdint.h>
#include "../../core/macros.h"
#define SBIT_TIMER0 1
#define SBIT_TIMER1 2
#define SBIT_CNTEN 0
#define SBIT_MR0I 0 // Timer 0 Interrupt when TC matches MR0
#define SBIT_MR0R 1 // Timer 0 Reset TC on Match
#define SBIT_MR0S 2 // Timer 0 Stop TC and PC on Match
#define SBIT_MR1I 3
#define SBIT_MR1R 4
#define SBIT_MR1S 5
#define SBIT_MR2I 6
#define SBIT_MR2R 7
#define SBIT_MR2S 8
#define SBIT_MR3I 9
#define SBIT_MR3R 10
#define SBIT_MR3S 11
// --------------------------------------------------------------------------
// Defines
// --------------------------------------------------------------------------
#define FORCE_INLINE __attribute__((always_inline)) inline
#define _HAL_TIMER(T) _CAT(LPC_TIM, T)
#define _HAL_TIMER_IRQ(T) TIMER##T##_IRQn
#define __HAL_TIMER_ISR(T) extern "C" void TIMER##T##_IRQHandler(void)
#define _HAL_TIMER_ISR(T) __HAL_TIMER_ISR(T)
typedef uint32_t hal_timer_t;
#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF
#define STEP_TIMER_NUM 0 // index of timer to use for stepper
#define TEMP_TIMER_NUM 1 // index of timer to use for temperature
#define STEP_TIMER_NUM 0 // Timer Index for Stepper
#define TEMP_TIMER_NUM 1 // Timer Index for Temperature
#define PULSE_TIMER_NUM STEP_TIMER_NUM
#define PWM_TIMER_NUM 3 // Timer Index for PWM
#define HAL_TIMER_RATE ((SystemCoreClock) / 4) // frequency of timers peripherals
#define HAL_STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE)
@ -66,21 +90,12 @@ typedef uint32_t hal_timer_t;
#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM)
#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM)
#define HAL_STEP_TIMER_ISR extern "C" void TIMER0_IRQHandler(void)
#define HAL_TEMP_TIMER_ISR extern "C" void TIMER1_IRQHandler(void)
#define HAL_STEP_TIMER_ISR _HAL_TIMER_ISR(STEP_TIMER_NUM)
#define HAL_TEMP_TIMER_ISR _HAL_TIMER_ISR(TEMP_TIMER_NUM)
// PWM timer
#define HAL_PWM_TIMER LPC_TIM3
#define HAL_PWM_TIMER_ISR extern "C" void TIMER3_IRQHandler(void)
#define HAL_PWM_TIMER_IRQn TIMER3_IRQn
// --------------------------------------------------------------------------
// Types
// --------------------------------------------------------------------------
// --------------------------------------------------------------------------
// Public Variables
// --------------------------------------------------------------------------
// Timer references by index
#define STEP_TIMER _HAL_TIMER(STEP_TIMER_NUM)
#define TEMP_TIMER _HAL_TIMER(TEMP_TIMER_NUM)
// --------------------------------------------------------------------------
// Public functions
@ -90,31 +105,23 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency);
FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) {
switch (timer_num) {
case 0:
LPC_TIM0->MR0 = compare;
if (LPC_TIM0->TC > compare)
LPC_TIM0->TC = compare - 5; // generate an immediate stepper ISR
break;
case 1:
LPC_TIM1->MR0 = compare;
if (LPC_TIM1->TC > compare)
LPC_TIM1->TC = compare - 5; // make sure we don't have one extra long period
break;
case 0: STEP_TIMER->MR0 = compare; break; // Stepper Timer Match Register 0
case 1: TEMP_TIMER->MR0 = compare; break; // Temp Timer Match Register 0
}
}
FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) {
switch (timer_num) {
case 0: return LPC_TIM0->MR0;
case 1: return LPC_TIM1->MR0;
case 0: return STEP_TIMER->MR0; // Stepper Timer Match Register 0
case 1: return TEMP_TIMER->MR0; // Temp Timer Match Register 0
}
return 0;
}
FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) {
switch (timer_num) {
case 0: return LPC_TIM0->TC;
case 1: return LPC_TIM1->TC;
case 0: return STEP_TIMER->TC; // Stepper Timer Count
case 1: return TEMP_TIMER->TC; // Temp Timer Count
}
return 0;
}
@ -124,10 +131,45 @@ FORCE_INLINE static void HAL_timer_restrain(const uint8_t timer_num, const uint1
if (HAL_timer_get_compare(timer_num) < mincmp) HAL_timer_set_compare(timer_num, mincmp);
}
void HAL_timer_enable_interrupt(const uint8_t timer_num);
void HAL_timer_disable_interrupt(const uint8_t timer_num);
bool HAL_timer_interrupt_enabled(const uint8_t timer_num);
void HAL_timer_isr_prologue(const uint8_t timer_num);
FORCE_INLINE static void HAL_timer_enable_interrupt(const uint8_t timer_num) {
switch (timer_num) {
case 0: NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
case 1: NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
}
}
FORCE_INLINE static void HAL_timer_disable_interrupt(const uint8_t timer_num) {
switch (timer_num) {
case 0: NVIC_DisableIRQ(TIMER0_IRQn); // Disable interrupt handler
case 1: NVIC_DisableIRQ(TIMER1_IRQn); // Disable interrupt handler
}
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
}
// This function is missing from CMSIS
FORCE_INLINE static bool NVIC_GetEnableIRQ(IRQn_Type IRQn) {
return (NVIC->ISER[((uint32_t)IRQn) >> 5] & (1 << ((uint32_t)IRQn) & 0x1F)) != 0;
}
FORCE_INLINE static bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {
switch (timer_num) {
case 0: return NVIC_GetEnableIRQ(TIMER0_IRQn); // Check if interrupt is enabled or not
case 1: return NVIC_GetEnableIRQ(TIMER1_IRQn); // Check if interrupt is enabled or not
}
return false;
}
FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) {
switch (timer_num) {
case 0: SBI(STEP_TIMER->IR, SBIT_CNTEN); break;
case 1: SBI(TEMP_TIMER->IR, SBIT_CNTEN); break;
}
}
#define HAL_timer_isr_epilogue(TIMER_NUM)
#endif // _HAL_TIMERS_DUE_H
#endif // _HAL_TIMERS_H

25
Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.cpp
View file

@ -78,12 +78,14 @@
#define NUM_ISR_PWMS 20
#define HAL_PWM_TIMER LPC_TIM3
#define HAL_PWM_TIMER_ISR extern "C" void TIMER3_IRQHandler(void)
#define HAL_PWM_TIMER_IRQn TIMER3_IRQn
#define LPC_PORT_OFFSET (0x0020)
#define LPC_PIN(pin) (1UL << pin)
#define LPC_GPIO(port) ((volatile LPC_GPIO_TypeDef *)(LPC_GPIO0_BASE + LPC_PORT_OFFSET * port))
typedef struct { // holds all data needed to control/init one of the PWM channels
bool active_flag; // THIS TABLE ENTRY IS ACTIVELY TOGGLING A PIN
pin_t pin;
@ -256,6 +258,11 @@ bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min /* = 1 */, uint32_t max /* =
// OK to update the active table because the
// ISR doesn't use any of the changed items
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
if (ISR_table_update) //use work table if that's the newest
temp_table = work_table;
else
@ -340,6 +347,11 @@ bool LPC1768_PWM_detach_pin(pin_t pin) {
//// interrupt controlled PWM code
NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
if (ISR_table_update) {
ISR_table_update = false; // don't update yet - have another update to do
NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn); // re-enable PWM interrupts
@ -426,6 +438,12 @@ bool LPC1768_PWM_write(pin_t pin, uint32_t value) {
//// interrupt controlled PWM code
NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
if (!ISR_table_update) // use the most up to date table
COPY_ACTIVE_TABLE; // copy active table into work table
@ -454,6 +472,11 @@ bool useable_hardware_PWM(pin_t pin) {
NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
bool return_flag = false;
for (uint8_t i = 0; i < NUM_ISR_PWMS; i++) // see if it's already setup
if (active_table[i].pin == pin) return_flag = true;

12
Marlin/src/HAL/HAL_LPC1768/endstop_interrupts.h
View file

@ -24,7 +24,7 @@
* Endstop Interrupts
*
* Without endstop interrupts the endstop pins must be polled continually in
* the stepper-ISR via endstops.update(), most of the time finding no change.
* the temperature-ISR via endstops.update(), most of the time finding no change.
* With this feature endstops.update() is called only when we know that at
* least one endstop has changed state, saving valuable CPU cycles.
*
@ -40,16 +40,10 @@
//Currently this is untested and broken
#error "Please disable Endstop Interrupts LPC176x is currently an unsupported platform"
volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
// Must be reset to 0 by the test function when finished.
// This is what is really done inside the interrupts.
FORCE_INLINE void endstop_ISR_worker( void ) {
e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
}
#include "../../module/endstops.h"
// One ISR for all EXT-Interrupts
void endstop_ISR(void) { endstop_ISR_worker(); }
void endstop_ISR(void) { endstops.check_possible_change(); }
void setup_endstop_interrupts(void) {
#if HAS_X_MAX

12
Marlin/src/HAL/HAL_STM32F1/endstop_interrupts.h
View file

@ -36,7 +36,7 @@
* Endstop Interrupts
*
* Without endstop interrupts the endstop pins must be polled continually in
* the stepper-ISR via endstops.update(), most of the time finding no change.
* the temperature-ISR via endstops.update(), most of the time finding no change.
* With this feature endstops.update() is called only when we know that at
* least one endstop has changed state, saving valuable CPU cycles.
*
@ -49,16 +49,10 @@
#ifndef _ENDSTOP_INTERRUPTS_H_
#define _ENDSTOP_INTERRUPTS_H_
volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
// Must be reset to 0 by the test function when finished.
// This is what is really done inside the interrupts.
FORCE_INLINE void endstop_ISR_worker( void ) {
e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
}
#include "../../module/endstops.h"
// One ISR for all EXT-Interrupts
void endstop_ISR(void) { endstop_ISR_worker(); }
void endstop_ISR(void) { endstops.check_possible_change(); }
void setup_endstop_interrupts(void) {
#if HAS_X_MAX

5
Marlin/src/HAL/HAL_STM32F4/HAL_timers_STM32F4.cpp
View file

@ -123,6 +123,11 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num) {
void HAL_timer_disable_interrupt(const uint8_t timer_num) {
HAL_NVIC_DisableIRQ(timerConfig[timer_num].IRQ_Id);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
}
hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) {

10
Marlin/src/HAL/HAL_STM32F4/endstop_interrupts.h
View file

@ -24,16 +24,10 @@
#ifndef _ENDSTOP_INTERRUPTS_H_
#define _ENDSTOP_INTERRUPTS_H_
volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
// Must be reset to 0 by the test function when finished.
// This is what is really done inside the interrupts.
FORCE_INLINE void endstop_ISR_worker( void ) {
e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
}
#include "../../module/endstops.h"
// One ISR for all EXT-Interrupts
void endstop_ISR(void) { endstop_ISR_worker(); }
void endstop_ISR(void) { endstops.check_possible_change(); }
void setup_endstop_interrupts(void) {
#if HAS_X_MAX

5
Marlin/src/HAL/HAL_STM32F7/HAL_timers_STM32F7.cpp
View file

@ -127,6 +127,11 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num) {
void HAL_timer_disable_interrupt(const uint8_t timer_num) {
HAL_NVIC_DisableIRQ(timerConfig[timer_num].IRQ_Id);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
}
hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) {

10
Marlin/src/HAL/HAL_STM32F7/endstop_interrupts.h
View file

@ -26,16 +26,10 @@
#ifndef _ENDSTOP_INTERRUPTS_H_
#define _ENDSTOP_INTERRUPTS_H_
volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
// Must be reset to 0 by the test function when finished.
// This is what is really done inside the interrupts.
FORCE_INLINE void endstop_ISR_worker( void ) {
e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
}
#include "../../module/endstops.h"
// One ISR for all EXT-Interrupts
void endstop_ISR(void) { endstop_ISR_worker(); }
void endstop_ISR(void) { endstops.check_possible_change(); }
void setup_endstop_interrupts(void) {
#if HAS_X_MAX

21
Marlin/src/HAL/HAL_TEENSY35_36/HAL_timers_Teensy.cpp
View file

@ -29,6 +29,22 @@
#include "HAL.h"
#include "HAL_timers_Teensy.h"
/** \brief Instruction Synchronization Barrier
Instruction Synchronization Barrier flushes the pipeline in the processor,
so that all instructions following the ISB are fetched from cache or
memory, after the instruction has been completed.
*/
FORCE_INLINE static void __ISB(void) {
__asm__ __volatile__("isb 0xF":::"memory");
}
/** \brief Data Synchronization Barrier
This function acts as a special kind of Data Memory Barrier.
It completes when all explicit memory accesses before this instruction complete.
*/
FORCE_INLINE static void __DSB(void) {
__asm__ __volatile__("dsb 0xF":::"memory");
}
void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
switch (timer_num) {
@ -65,6 +81,11 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num) {
case 0: NVIC_DISABLE_IRQ(IRQ_FTM0); break;
case 1: NVIC_DISABLE_IRQ(IRQ_FTM1); break;
}
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
}
bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {

12
Marlin/src/HAL/HAL_TEENSY35_36/endstop_interrupts.h
View file

@ -24,7 +24,7 @@
* Endstop Interrupts
*
* Without endstop interrupts the endstop pins must be polled continually in
* the stepper-ISR via endstops.update(), most of the time finding no change.
* the temperature-ISR via endstops.update(), most of the time finding no change.
* With this feature endstops.update() is called only when we know that at
* least one endstop has changed state, saving valuable CPU cycles.
*
@ -37,16 +37,10 @@
#ifndef _ENDSTOP_INTERRUPTS_H_
#define _ENDSTOP_INTERRUPTS_H_
volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
// Must be reset to 0 by the test function when finished.
// This is what is really done inside the interrupts.
FORCE_INLINE void endstop_ISR_worker( void ) {
e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
}
#include "../../module/endstops.h"
// One ISR for all EXT-Interrupts
void endstop_ISR(void) { endstop_ISR_worker(); }
void endstop_ISR(void) { endstops.check_possible_change(); }
/**
* Endstop interrupts for Due based targets.

14
Marlin/src/Marlin.cpp
View file

@ -95,10 +95,6 @@
#include "feature/I2CPositionEncoder.h"
#endif
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
#include HAL_PATH(HAL, endstop_interrupts.h)
#endif
#if HAS_TRINAMIC
#include "feature/tmc_util.h"
#endif
@ -269,7 +265,7 @@ bool pin_is_protected(const pin_t pin) {
}
void quickstop_stepper() {
stepper.quick_stop();
planner.quick_stop();
planner.synchronize();
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
@ -748,7 +744,9 @@ void setup() {
print_job_timer.init(); // Initial setup of print job timer
stepper.init(); // Initialize stepper, this enables interrupts!
endstops.init(); // Init endstops and pullups
stepper.init(); // Init stepper. This enables interrupts!
#if HAS_SERVOS
servo_init();
@ -860,10 +858,6 @@ void setup() {
i2c.onRequest(i2c_on_request);
#endif
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
setup_endstop_interrupts();
#endif
#if DO_SWITCH_EXTRUDER
move_extruder_servo(0); // Initialize extruder servo
#endif

4
Marlin/src/feature/Max7219_Debug_LEDs.cpp
View file

@ -73,7 +73,6 @@ static uint8_t LEDs[8] = { 0 };
#endif
void Max7219_PutByte(uint8_t data) {
CRITICAL_SECTION_START;
for (uint8_t i = 8; i--;) {
SIG_DELAY();
WRITE(MAX7219_CLK_PIN, LOW); // tick
@ -84,12 +83,10 @@ void Max7219_PutByte(uint8_t data) {
SIG_DELAY();
data <<= 1;
}
CRITICAL_SECTION_END;
}
void Max7219(const uint8_t reg, const uint8_t data) {
SIG_DELAY();
CRITICAL_SECTION_START;
WRITE(MAX7219_LOAD_PIN, LOW); // begin
SIG_DELAY();
Max7219_PutByte(reg); // specify register
@ -99,7 +96,6 @@ void Max7219(const uint8_t reg, const uint8_t data) {
WRITE(MAX7219_LOAD_PIN, LOW); // and tell the chip to load the data
SIG_DELAY();
WRITE(MAX7219_LOAD_PIN, HIGH);
CRITICAL_SECTION_END;
SIG_DELAY();
}

9
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View file

@ -262,7 +262,8 @@
z_position = end[Z_AXIS];
}
planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
if (!planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder))
break;
} //else printf("FIRST MOVE PRUNED ");
}
@ -319,7 +320,8 @@
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
if (!planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder))
break;
current_yi += dyi;
yi_cnt--;
}
@ -342,7 +344,8 @@
z_position = end[Z_AXIS];
}
planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
if (!planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder))
break;
current_xi += dxi;
xi_cnt--;
}

2
Marlin/src/gcode/config/M540.cpp
View file

@ -33,7 +33,7 @@
void GcodeSuite::M540() {
if (parser.seen('S'))
stepper.abort_on_endstop_hit = parser.value_bool();
planner.abort_on_endstop_hit = parser.value_bool();
}

2
Marlin/src/gcode/control/M17_M18_M84.cpp
View file

@ -47,7 +47,7 @@ void GcodeSuite::M18_M84() {
else {
bool all_axis = !(parser.seen('X') || parser.seen('Y') || parser.seen('Z') || parser.seen('E'));
if (all_axis) {
stepper.finish_and_disable();
planner.finish_and_disable();
}
else {
planner.synchronize();

2
Marlin/src/gcode/control/M80_M81.cpp
View file

@ -95,7 +95,7 @@
*/
void GcodeSuite::M81() {
thermalManager.disable_all_heaters();
stepper.finish_and_disable();
planner.finish_and_disable();
#if FAN_COUNT > 0
for (uint8_t i = 0; i < FAN_COUNT; i++) fanSpeeds[i] = 0;

9
Marlin/src/gcode/motion/G2_G3.cpp
View file

@ -197,14 +197,17 @@ void plan_arc(
// i.e., Complete the angular vector in the given time.
inverse_kinematics(raw);
ADJUST_DELTA(raw);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
break;
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#elif HAS_UBL_AND_CURVES
float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
planner.apply_leveling(pos);
planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder);
if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder))
break;
#else
planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder))
break;
#endif
}

6
Marlin/src/lcd/ultralcd.cpp
View file

@ -2421,12 +2421,10 @@ void lcd_quick_feedback(const bool clear_buttons) {
void _lcd_do_nothing() {}
void _lcd_hard_stop() {
stepper.quick_stop();
const screenFunc_t old_screen = currentScreen;
currentScreen = _lcd_do_nothing;
while (planner.movesplanned()) idle();
planner.quick_stop();
currentScreen = old_screen;
stepper.cleaning_buffer_counter = 0;
set_current_from_steppers_for_axis(ALL_AXES);
sync_plan_position();
}
@ -3856,7 +3854,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
// M540 S - Abort on endstop hit when SD printing
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &stepper.abort_on_endstop_hit);
MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &planner.abort_on_endstop_hit);
#endif
END_MENU();

346
Marlin/src/module/endstops.cpp
View file

@ -32,18 +32,27 @@
#include "../module/temperature.h"
#include "../lcd/ultralcd.h"
// TEST_ENDSTOP: test the old and the current status of an endstop
#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits & old_endstop_bits, ENDSTOP))
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
#include HAL_PATH(../HAL, endstop_interrupts.h)
#endif
// TEST_ENDSTOP: test the current status of an endstop
#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits, ENDSTOP))
#if HAS_BED_PROBE
#define ENDSTOPS_ENABLED (endstops.enabled || endstops.z_probe_enabled)
#else
#define ENDSTOPS_ENABLED endstops.enabled
#endif
Endstops endstops;
// public:
bool Endstops::enabled, Endstops::enabled_globally; // Initialized by settings.load()
volatile char Endstops::endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
volatile uint8_t Endstops::endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
Endstops::esbits_t Endstops::current_endstop_bits = 0,
Endstops::old_endstop_bits = 0;
Endstops::esbits_t Endstops::current_endstop_bits = 0;
#if HAS_BED_PROBE
volatile bool Endstops::z_probe_enabled = false;
@ -196,8 +205,93 @@ void Endstops::init() {
#endif
#endif
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
setup_endstop_interrupts();
#endif
// Enable endstops
enable_globally(
#if ENABLED(ENDSTOPS_ALWAYS_ON_DEFAULT)
true
#else
false
#endif
);
} // Endstops::init
// Called from ISR. A change was detected. Find out what happened!
void Endstops::check_possible_change() { if (ENDSTOPS_ENABLED) endstops.update(); }
// Called from ISR: Poll endstop state if required
void Endstops::poll() {
#if ENABLED(PINS_DEBUGGING)
endstops.run_monitor(); // report changes in endstop status
#endif
#if DISABLED(ENDSTOP_INTERRUPTS_FEATURE)
if (ENDSTOPS_ENABLED) endstops.update();
#endif
}
void Endstops::enable_globally(const bool onoff) {
enabled_globally = enabled = onoff;
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
if (onoff) endstops.update(); // If enabling, update state now
#endif
}
// Enable / disable endstop checking
void Endstops::enable(const bool onoff) {
enabled = onoff;
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
if (onoff) endstops.update(); // If enabling, update state now
#endif
}
// Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
void Endstops::not_homing() {
enabled = enabled_globally;
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
if (enabled) endstops.update(); // If enabling, update state now
#endif
}
// Clear endstops (i.e., they were hit intentionally) to suppress the report
void Endstops::hit_on_purpose() {
endstop_hit_bits = 0;
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
if (enabled) endstops.update(); // If enabling, update state now
#endif
}
// Enable / disable endstop z-probe checking
#if HAS_BED_PROBE
void Endstops::enable_z_probe(bool onoff) {
z_probe_enabled = onoff;
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
if (enabled) endstops.update(); // If enabling, update state now
#endif
}
#endif
#if ENABLED(PINS_DEBUGGING)
void Endstops::run_monitor() {
if (!monitor_flag) return;
static uint8_t monitor_count = 16; // offset this check from the others
monitor_count += _BV(1); // 15 Hz
monitor_count &= 0x7F;
if (!monitor_count) monitor(); // report changes in endstop status
}
#endif
void Endstops::report_state() {
if (endstop_hit_bits) {
#if ENABLED(ULTRA_LCD)
@ -208,7 +302,7 @@ void Endstops::report_state() {
#endif
#define _ENDSTOP_HIT_ECHO(A,C) do{ \
SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", stepper.triggered_position_mm(_AXIS(A))); \
SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", planner.triggered_position_mm(_AXIS(A))); \
_SET_STOP_CHAR(A,C); }while(0)
#define _ENDSTOP_HIT_TEST(A,C) \
@ -238,7 +332,7 @@ void Endstops::report_state() {
hit_on_purpose();
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && ENABLED(SDSUPPORT)
if (stepper.abort_on_endstop_hit) {
if (planner.abort_on_endstop_hit) {
card.sdprinting = false;
card.closefile();
quickstop_stepper();
@ -300,38 +394,41 @@ void Endstops::M119() {
#endif
} // Endstops::M119
// The following routines are called from an ISR context. It could be the temperature ISR, the
// endstop ISR or the Stepper ISR.
#if ENABLED(X_DUAL_ENDSTOPS)
void Endstops::test_dual_x_endstops(const EndstopEnum es1, const EndstopEnum es2) {
const byte x_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for X, bit 1 for X2
if (x_test && stepper.current_block->steps[X_AXIS] > 0) {
if (x_test && stepper.movement_non_null(X_AXIS)) {
SBI(endstop_hit_bits, X_MIN);
if (!stepper.performing_homing || (x_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
stepper.kill_current_block();
stepper.quick_stop();
}
}
#endif
#if ENABLED(Y_DUAL_ENDSTOPS)
void Endstops::test_dual_y_endstops(const EndstopEnum es1, const EndstopEnum es2) {
const byte y_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for Y, bit 1 for Y2
if (y_test && stepper.current_block->steps[Y_AXIS] > 0) {
if (y_test && stepper.movement_non_null(Y_AXIS)) {
SBI(endstop_hit_bits, Y_MIN);
if (!stepper.performing_homing || (y_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
stepper.kill_current_block();
stepper.quick_stop();
}
}
#endif
#if ENABLED(Z_DUAL_ENDSTOPS)
void Endstops::test_dual_z_endstops(const EndstopEnum es1, const EndstopEnum es2) {
const byte z_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for Z, bit 1 for Z2
if (z_test && stepper.current_block->steps[Z_AXIS] > 0) {
if (z_test && stepper.movement_non_null(Z_AXIS)) {
SBI(endstop_hit_bits, Z_MIN);
if (!stepper.performing_homing || (z_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
stepper.kill_current_block();
stepper.quick_stop();
}
}
#endif
// Check endstops - Called from ISR!
// Check endstops - Could be called from ISR!
void Endstops::update() {
#define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX
@ -349,7 +446,7 @@ void Endstops::update() {
UPDATE_ENDSTOP_BIT(AXIS, MINMAX); \
if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX))) { \
_ENDSTOP_HIT(AXIS, MINMAX); \
stepper.endstop_triggered(_AXIS(AXIS)); \
planner.endstop_triggered(_AXIS(AXIS)); \
} \
}while(0)
@ -358,9 +455,9 @@ void Endstops::update() {
if (G38_move) {
UPDATE_ENDSTOP_BIT(Z, MIN_PROBE);
if (TEST_ENDSTOP(_ENDSTOP(Z, MIN_PROBE))) {
if (stepper.current_block->steps[_AXIS(X)] > 0) { _ENDSTOP_HIT(X, MIN); stepper.endstop_triggered(_AXIS(X)); }
else if (stepper.current_block->steps[_AXIS(Y)] > 0) { _ENDSTOP_HIT(Y, MIN); stepper.endstop_triggered(_AXIS(Y)); }
else if (stepper.current_block->steps[_AXIS(Z)] > 0) { _ENDSTOP_HIT(Z, MIN); stepper.endstop_triggered(_AXIS(Z)); }
if (stepper.movement_non_null(_AXIS(X))) { _ENDSTOP_HIT(X, MIN); planner.endstop_triggered(_AXIS(X)); }
else if (stepper.movement_non_null(_AXIS(Y))) { _ENDSTOP_HIT(Y, MIN); planner.endstop_triggered(_AXIS(Y)); }
else if (stepper.movement_non_null(_AXIS(Z))) { _ENDSTOP_HIT(Z, MIN); planner.endstop_triggered(_AXIS(Z)); }
G38_endstop_hit = true;
}
}
@ -371,7 +468,7 @@ void Endstops::update() {
*/
#if IS_CORE
#define S_(N) stepper.current_block->steps[CORE_AXIS_##N]
#define S_(N) stepper.movement_non_null(CORE_AXIS_##N)
#define D_(N) stepper.motor_direction(CORE_AXIS_##N)
#endif
@ -391,7 +488,7 @@ void Endstops::update() {
#define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) X_CMP D_(2)) )
#define X_AXIS_HEAD X_HEAD
#else
#define X_MOVE_TEST stepper.current_block->steps[X_AXIS] > 0
#define X_MOVE_TEST stepper.movement_non_null(X_AXIS)
#define X_AXIS_HEAD X_AXIS
#endif
@ -411,7 +508,7 @@ void Endstops::update() {
#define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Y_CMP D_(2)) )
#define Y_AXIS_HEAD Y_HEAD
#else
#define Y_MOVE_TEST stepper.current_block->steps[Y_AXIS] > 0
#define Y_MOVE_TEST stepper.movement_non_null(Y_AXIS)
#define Y_AXIS_HEAD Y_AXIS
#endif
@ -431,13 +528,13 @@ void Endstops::update() {
#define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Z_CMP D_(2)) )
#define Z_AXIS_HEAD Z_HEAD
#else
#define Z_MOVE_TEST stepper.current_block->steps[Z_AXIS] > 0
#define Z_MOVE_TEST stepper.movement_non_null(Z_AXIS)
#define Z_AXIS_HEAD Z_AXIS
#endif
// With Dual X, endstops are only checked in the homing direction for the active extruder
#if ENABLED(DUAL_X_CARRIAGE)
#define E0_ACTIVE stepper.current_block->active_extruder == 0
#define E0_ACTIVE stepper.movement_extruder() == 0
#define X_MIN_TEST ((X_HOME_DIR < 0 && E0_ACTIVE) || (X2_HOME_DIR < 0 && !E0_ACTIVE))
#define X_MAX_TEST ((X_HOME_DIR > 0 && E0_ACTIVE) || (X2_HOME_DIR > 0 && !E0_ACTIVE))
#else
@ -448,126 +545,119 @@ void Endstops::update() {
/**
* Check and update endstops according to conditions
*/
if (stepper.current_block) {
if (X_MOVE_TEST) {
if (stepper.motor_direction(X_AXIS_HEAD)) { // -direction
#if HAS_X_MIN
#if ENABLED(X_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(X, MIN);
#if HAS_X2_MIN
UPDATE_ENDSTOP_BIT(X2, MIN);
#else
COPY_BIT(current_endstop_bits, X_MIN, X2_MIN);
#endif
test_dual_x_endstops(X_MIN, X2_MIN);
if (X_MOVE_TEST) {
if (stepper.motor_direction(X_AXIS_HEAD)) { // -direction
#if HAS_X_MIN
#if ENABLED(X_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(X, MIN);
#if HAS_X2_MIN
UPDATE_ENDSTOP_BIT(X2, MIN);
#else
if (X_MIN_TEST) UPDATE_ENDSTOP(X, MIN);
COPY_BIT(current_endstop_bits, X_MIN, X2_MIN);
#endif
test_dual_x_endstops(X_MIN, X2_MIN);
#else
if (X_MIN_TEST) UPDATE_ENDSTOP(X, MIN);
#endif
}
else { // +direction
#if HAS_X_MAX
#if ENABLED(X_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(X, MAX);
#if HAS_X2_MAX
UPDATE_ENDSTOP_BIT(X2, MAX);
#else
COPY_BIT(current_endstop_bits, X_MAX, X2_MAX);
#endif
test_dual_x_endstops(X_MAX, X2_MAX);
#else
if (X_MAX_TEST) UPDATE_ENDSTOP(X, MAX);
#endif
#endif
}
#endif
}
if (Y_MOVE_TEST) {
if (stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
#if HAS_Y_MIN
#if ENABLED(Y_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(Y, MIN);
#if HAS_Y2_MIN
UPDATE_ENDSTOP_BIT(Y2, MIN);
#else
COPY_BIT(current_endstop_bits, Y_MIN, Y2_MIN);
#endif
test_dual_y_endstops(Y_MIN, Y2_MIN);
else { // +direction
#if HAS_X_MAX
#if ENABLED(X_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(X, MAX);
#if HAS_X2_MAX
UPDATE_ENDSTOP_BIT(X2, MAX);
#else
UPDATE_ENDSTOP(Y, MIN);
COPY_BIT(current_endstop_bits, X_MAX, X2_MAX);
#endif
test_dual_x_endstops(X_MAX, X2_MAX);
#else
if (X_MAX_TEST) UPDATE_ENDSTOP(X, MAX);
#endif
}
else { // +direction
#if HAS_Y_MAX
#if ENABLED(Y_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(Y, MAX);
#if HAS_Y2_MAX
UPDATE_ENDSTOP_BIT(Y2, MAX);
#else
COPY_BIT(current_endstop_bits, Y_MAX, Y2_MAX);
#endif
test_dual_y_endstops(Y_MAX, Y2_MAX);
#else
UPDATE_ENDSTOP(Y, MAX);
#endif
#endif
}
#endif
}
}
if (Z_MOVE_TEST) {
if (stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.
#if HAS_Z_MIN
#if ENABLED(Z_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(Z, MIN);
#if HAS_Z2_MIN
UPDATE_ENDSTOP_BIT(Z2, MIN);
#else
COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
#endif
test_dual_z_endstops(Z_MIN, Z2_MIN);
if (Y_MOVE_TEST) {
if (stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
#if HAS_Y_MIN
#if ENABLED(Y_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(Y, MIN);
#if HAS_Y2_MIN
UPDATE_ENDSTOP_BIT(Y2, MIN);
#else
#if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
if (z_probe_enabled) UPDATE_ENDSTOP(Z, MIN);
#else
UPDATE_ENDSTOP(Z, MIN);
#endif
COPY_BIT(current_endstop_bits, Y_MIN, Y2_MIN);
#endif
test_dual_y_endstops(Y_MIN, Y2_MIN);
#else
UPDATE_ENDSTOP(Y, MIN);
#endif
// When closing the gap check the enabled probe
#if ENABLED(Z_MIN_PROBE_ENDSTOP)
if (z_probe_enabled) {
UPDATE_ENDSTOP(Z, MIN_PROBE);
if (TEST_ENDSTOP(Z_MIN_PROBE)) SBI(endstop_hit_bits, Z_MIN_PROBE);
}
#endif
}
else { // Z +direction. Gantry up, bed down.
#if HAS_Z_MAX
// Check both Z dual endstops
#if ENABLED(Z_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(Z, MAX);
#if HAS_Z2_MAX
UPDATE_ENDSTOP_BIT(Z2, MAX);
#else
COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
#endif
test_dual_z_endstops(Z_MAX, Z2_MAX);
// If this pin is not hijacked for the bed probe
// then it belongs to the Z endstop
#elif DISABLED(Z_MIN_PROBE_ENDSTOP) || Z_MAX_PIN != Z_MIN_PROBE_PIN
UPDATE_ENDSTOP(Z, MAX);
#endif
#endif
}
#endif
}
else { // +direction
#if HAS_Y_MAX
#if ENABLED(Y_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(Y, MAX);
#if HAS_Y2_MAX
UPDATE_ENDSTOP_BIT(Y2, MAX);
#else
COPY_BIT(current_endstop_bits, Y_MAX, Y2_MAX);
#endif
test_dual_y_endstops(Y_MAX, Y2_MAX);
#else
UPDATE_ENDSTOP(Y, MAX);
#endif
#endif
}
}
} // stepper.current_block
old_endstop_bits = current_endstop_bits;
if (Z_MOVE_TEST) {
if (stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.
#if HAS_Z_MIN
#if ENABLED(Z_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(Z, MIN);
#if HAS_Z2_MIN
UPDATE_ENDSTOP_BIT(Z2, MIN);
#else
COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
#endif
test_dual_z_endstops(Z_MIN, Z2_MIN);
#else
#if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
if (z_probe_enabled) UPDATE_ENDSTOP(Z, MIN);
#else
UPDATE_ENDSTOP(Z, MIN);
#endif
#endif
#endif
// When closing the gap check the enabled probe
#if ENABLED(Z_MIN_PROBE_ENDSTOP)
if (z_probe_enabled) {
UPDATE_ENDSTOP(Z, MIN_PROBE);
if (TEST_ENDSTOP(Z_MIN_PROBE)) SBI(endstop_hit_bits, Z_MIN_PROBE);
}
#endif
}
else { // Z +direction. Gantry up, bed down.
#if HAS_Z_MAX
// Check both Z dual endstops
#if ENABLED(Z_DUAL_ENDSTOPS)
UPDATE_ENDSTOP_BIT(Z, MAX);
#if HAS_Z2_MAX
UPDATE_ENDSTOP_BIT(Z2, MAX);
#else
COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
#endif
test_dual_z_endstops(Z_MAX, Z2_MAX);
// If this pin is not hijacked for the bed probe
// then it belongs to the Z endstop
#elif DISABLED(Z_MIN_PROBE_ENDSTOP) || Z_MAX_PIN != Z_MIN_PROBE_PIN
UPDATE_ENDSTOP(Z, MAX);
#endif
#endif
}
}
} // Endstops::update()
#if ENABLED(PINS_DEBUGGING)

49
Marlin/src/module/endstops.h
View file

@ -51,7 +51,7 @@ class Endstops {
public:
static bool enabled, enabled_globally;
static volatile char endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
static volatile uint8_t endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
typedef uint16_t esbits_t;
@ -68,23 +68,26 @@ class Endstops {
typedef byte esbits_t;
#endif
static esbits_t current_endstop_bits, old_endstop_bits;
static esbits_t current_endstop_bits;
Endstops() {
enable_globally(
#if ENABLED(ENDSTOPS_ALWAYS_ON_DEFAULT)
true
#else
false
#endif
);
};
Endstops() {};
/**
* Initialize the endstop pins
*/
static void init();
/**
* A change was detected or presumed to be in endstops pins. Find out what
* changed, if anything. Called from ISR contexts
*/
static void check_possible_change();
/**
* Periodic call to poll endstops if required. Called from temperature ISR
*/
static void poll();
/**
* Update the endstops bits from the pins
*/
@ -101,34 +104,28 @@ class Endstops {
static void M119();
// Enable / disable endstop checking globally
static void enable_globally(bool onoff=true) { enabled_globally = enabled = onoff; }
static void enable_globally(const bool onoff=true);
// Enable / disable endstop checking
static void enable(bool onoff=true) { enabled = onoff; }
static void enable(const bool onoff=true);
// Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
static void not_homing() { enabled = enabled_globally; }
static void not_homing();
// Clear endstops (i.e., they were hit intentionally) to suppress the report
static void hit_on_purpose() { endstop_hit_bits = 0; }
static void hit_on_purpose();
// Enable / disable endstop z-probe checking
#if HAS_BED_PROBE
static volatile bool z_probe_enabled;
static void enable_z_probe(bool onoff=true) { z_probe_enabled = onoff; }
static void enable_z_probe(bool onoff=true);
#endif
// Debugging of endstops
#if ENABLED(PINS_DEBUGGING)
static bool monitor_flag;
static void monitor();
FORCE_INLINE static void run_monitor() {
if (!monitor_flag) return;
static uint8_t monitor_count = 16; // offset this check from the others
monitor_count += _BV(1); // 15 Hz
monitor_count &= 0x7F;
if (!monitor_count) monitor(); // report changes in endstop status
}
static void run_monitor();
#endif
private:
@ -146,10 +143,4 @@ class Endstops {
extern Endstops endstops;
#if HAS_BED_PROBE
#define ENDSTOPS_ENABLED (endstops.enabled || endstops.z_probe_enabled)
#else
#define ENDSTOPS_ENABLED endstops.enabled
#endif
#endif // __ENDSTOPS_H__

21
Marlin/src/module/motion.cpp
View file

@ -644,7 +644,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
break;
/*
SERIAL_ECHO(segments);
SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
@ -654,7 +655,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
//*/
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#else
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm);
if (!planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm))
break;
#endif
}
@ -746,7 +748,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
idle();
}
LOOP_XYZE(i) raw[i] += segment_distance[i];
planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm);
if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm))
break;
}
// Since segment_distance is only approximate,
@ -848,14 +851,14 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
}
// unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
for (uint8_t i = 0; i < 3; i++)
planner.buffer_line(
if (!planner.buffer_line(
i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS],
i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS],
i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS],
current_position[E_AXIS],
i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS],
active_extruder
);
active_extruder)
) break;
delayed_move_time = 0;
active_extruder_parked = false;
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -872,11 +875,11 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#endif
// move duplicate extruder into correct duplication position.
planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
planner.buffer_line(
if (!planner.buffer_line(
current_position[X_AXIS] + duplicate_extruder_x_offset,
current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
planner.max_feedrate_mm_s[X_AXIS], 1
);
planner.max_feedrate_mm_s[X_AXIS], 1)
) break;
planner.synchronize();
SYNC_PLAN_POSITION_KINEMATIC();
extruder_duplication_enabled = true;

723
Marlin/src/module/planner.cpp
View file

@ -92,6 +92,10 @@
#include "../feature/power.h"
#endif
// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
// fewer movements. The delay is measured in milliseconds, and must be less than 250ms
#define BLOCK_DELAY_FOR_1ST_MOVE 50
Planner planner;
// public:
@ -100,13 +104,20 @@ 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;
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
float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
Planner::axis_steps_per_mm[XYZE_N],
Planner::steps_to_mm[XYZE_N];
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
bool Planner::abort_on_endstop_hit = false;
#endif
#if ENABLED(DISTINCT_E_FACTORS)
uint8_t Planner::last_extruder = 0; // Respond to extruder change
#endif
@ -175,7 +186,7 @@ int32_t Planner::position[NUM_AXIS] = { 0 };
uint32_t Planner::cutoff_long;
float Planner::previous_speed[NUM_AXIS],
Planner::previous_nominal_speed;
Planner::previous_nominal_speed_sqr;
#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
@ -212,11 +223,13 @@ void Planner::init() {
ZERO(position_float);
#endif
ZERO(previous_speed);
previous_nominal_speed = 0.0;
previous_nominal_speed_sqr = 0.0;
#if ABL_PLANAR
bed_level_matrix.set_to_identity();
#endif
clear_block_buffer();
block_buffer_planned = 0;
delay_before_delivering = 0;
}
#if ENABLED(BEZIER_JERK_CONTROL)
@ -363,7 +376,7 @@ void Planner::init() {
//
static uint32_t get_period_inverse(uint32_t d) {
static const uint8_t inv_tab[256] PROGMEM = {
static const uint8_t inv_tab[256] PROGMEM = {
255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227,
225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200,
199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176,
@ -727,12 +740,9 @@ void Planner::init() {
}
#else
// All the other 32 CPUs can easily perform the inverse using hardware division,
// so we don´t need to reduce precision or to use assembly language at all.
// so we don't need to reduce precision or to use assembly language at all.
// This routine, for all the other archs, returns 0x100000000 / d ~= 0xFFFFFFFF / d
static FORCE_INLINE uint32_t get_period_inverse(uint32_t d) {
return 0xFFFFFFFF / d;
}
static FORCE_INLINE uint32_t get_period_inverse(const uint32_t d) { return 0xFFFFFFFF / d; }
#endif
#endif
@ -743,12 +753,13 @@ void Planner::init() {
* by the provided factors.
*/
void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),
final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)
// Limit minimal step rate (Otherwise the timer will overflow.)
NOLESS(initial_rate, MINIMAL_STEP_RATE);
NOLESS(final_rate, MINIMAL_STEP_RATE);
NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE));
NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE));
#if ENABLED(BEZIER_JERK_CONTROL)
uint32_t cruise_rate = initial_rate;
@ -757,19 +768,18 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
const int32_t accel = block->acceleration_steps_per_s2;
// Steps required for acceleration, deceleration to/from nominal rate
int32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));
// Steps between acceleration and deceleration, if any
plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
// Does accelerate_steps + decelerate_steps exceed step_event_count?
// Then we can't possibly reach the nominal rate, there will be no cruising.
// Use intersection_distance() to calculate accel / braking time in order to
// reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) {
accelerate_steps = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
accelerate_steps = MIN(uint32_t(MAX(accelerate_steps_float, 0)), block->step_event_count);
plateau_steps = 0;
#if ENABLED(BEZIER_JERK_CONTROL)
@ -796,8 +806,12 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
#endif
CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy.
// 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 (!TEST(block->flag, BLOCK_BIT_BUSY)) {
block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps + plateau_steps;
block->initial_rate = initial_rate;
@ -810,32 +824,99 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
#endif
block->final_rate = final_rate;
}
CRITICAL_SECTION_END;
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
// This method will calculate the junction jerk as the euclidean distance between the nominal
// velocities of the respective blocks.
//inline float junction_jerk(block_t *before, block_t *after) {
// return SQRT(
// POW((before->speed_x-after->speed_x), 2)+POW((before->speed_y-after->speed_y), 2));
//}
/* PLANNER SPEED DEFINITION
+--------+ <- current->nominal_speed
/ \
current->entry_speed -> + \
| + <- next->entry_speed (aka exit speed)
+-------------+
time -->
Recalculates the motion plan according to the following basic guidelines:
1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
(i.e. current->entry_speed) such that:
a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
neighboring blocks.
b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
with a maximum allowable deceleration over the block travel distance.
c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
2. Go over every block in chronological (forward) order and dial down junction speed values if
a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
acceleration over the block travel distance.
When these stages are complete, the planner will have maximized the velocity profiles throughout the all
of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements
are possible. If a new block is added to the buffer, the plan is recomputed according to the said
guidelines for a new optimal plan.
To increase computational efficiency of these guidelines, a set of planner block pointers have been
created to indicate stop-compute points for when the planner guidelines cannot logically make any further
changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
point) are all accelerating, they are all optimal and can not be altered by a new block added to the
planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
recomputed as stated in the general guidelines.
Planner buffer index mapping:
- block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
- block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
the buffer is full or empty. As described for standard ring buffers, this block is always empty.
- block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
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.
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
enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
for the planner to compute over. It also increases the number of computations the planner has to perform
to compute an optimal plan, so select carefully.
*/
// 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* const next) {
if (current && next) {
// 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
// check for maximum allowable speed reductions to ensure maximum possible planned speed.
const float max_entry_speed = current->max_entry_speed;
if (current->entry_speed != max_entry_speed || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false.
const float new_entry_speed = (TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
? max_entry_speed
: MIN(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
if (new_entry_speed != current->entry_speed) {
current->entry_speed = new_entry_speed;
void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
if (current) {
// If entry speed is already at the maximum entry speed, and there was no change of speed
// in the next block, there is no need to recheck. Block is cruising and there is no need to
// compute anything for this block,
// If not, block entry speed needs to be recalculated to ensure maximum possible planned speed.
const float max_entry_speed_sqr = current->max_entry_speed_sqr;
// Compute maximum entry speed decelerating over the current block from its exit speed.
// If not at the maximum entry speed, or the previous block entry speed changed
if (current->entry_speed_sqr != max_entry_speed_sqr || (next && TEST(next->flag, BLOCK_BIT_RECALCULATE))) {
// If nominal length true, max junction speed is guaranteed to be reached.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
const float new_entry_speed_sqr = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
? max_entry_speed_sqr
: MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(MINIMUM_PLANNER_SPEED), current->millimeters));
if (current->entry_speed_sqr != new_entry_speed_sqr) {
current->entry_speed_sqr = new_entry_speed_sqr;
// Need to recalculate the block speed
SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
}
@ -847,51 +928,72 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t* const n
* Once in reverse and once forward. This implements the reverse pass.
*/
void Planner::reverse_pass() {
if (movesplanned() > 2) {
const uint8_t endnr = next_block_index(block_buffer_tail); // tail is running. tail+1 shouldn't be altered because it's connected to the running block.
uint8_t blocknr = prev_block_index(block_buffer_head);
block_t* current = &block_buffer[blocknr];
// Initialize block index to the last block in the planner buffer.
uint8_t block_index = prev_block_index(block_buffer_head);
// Last/newest block in buffer:
const float max_entry_speed = current->max_entry_speed;
if (current->entry_speed != max_entry_speed) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false.
const float new_entry_speed = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
? max_entry_speed
: MIN(max_entry_speed, max_allowable_speed(-current->acceleration, MINIMUM_PLANNER_SPEED, current->millimeters));
if (current->entry_speed != new_entry_speed) {
current->entry_speed = new_entry_speed;
SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
// Read the index of the last buffer planned block.
// The ISR may change it so get a stable local copy.
uint8_t planned_block_index = block_buffer_planned;
// If there was a race condition and block_buffer_planned was incremented
// or was pointing at the head (queue empty) break loop now and avoid
// planning already consumed blocks
if (planned_block_index == block_buffer_head) return;
// 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];
// Only consider non sync blocks
if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
reverse_pass_kernel(current, next);
next = current;
}
do {
const block_t * const next = current;
blocknr = prev_block_index(blocknr);
current = &block_buffer[blocknr];
reverse_pass_kernel(current, next);
} while (blocknr != endnr);
// Advance to the next
block_index = prev_block_index(block_index);
}
}
// The kernel called by recalculate() when scanning the plan from first to last entry.
void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current) {
void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index) {
if (previous) {
// If the previous block is an acceleration block, too short to complete the full speed
// change, adjust the entry speed accordingly. Entry speeds have already been reset,
// maximized, and reverse-planned. If nominal length is set, max junction speed is
// guaranteed to be reached. No need to recheck.
if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) {
if (previous->entry_speed < current->entry_speed) {
const float new_entry_speed = MIN(current->entry_speed, max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
// Check for junction speed change
if (current->entry_speed != new_entry_speed) {
current->entry_speed = new_entry_speed;
SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH) &&
previous->entry_speed_sqr < current->entry_speed_sqr) {
// Compute the maximum allowable speed
const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
// If true, current block is full-acceleration and we can move the planned pointer forward.
if (new_entry_speed_sqr < current->entry_speed_sqr) {
// 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;
// And mark we need to recompute the trapezoidal shape
SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
}
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. When the plan is bracketed by either the beginning of the
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
if (current->entry_speed_sqr == current->max_entry_speed_sqr)
block_buffer_planned = block_index;
}
}
@ -900,15 +1002,31 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
* Once in reverse and once forward. This implements the forward pass.
*/
void Planner::forward_pass() {
block_t* block[3] = { NULL, NULL, NULL };
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block[0] = block[1];
block[1] = block[2];
block[2] = &block_buffer[b];
forward_pass_kernel(block[0], block[1]);
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
// Begin at buffer planned pointer. Note that block_buffer_planned can be modified
// by the stepper ISR, so read it ONCE. It it guaranteed that block_buffer_planned
// will never lead head, so the loop is safe to execute. Also note that the forward
// pass will never modify the values at the tail.
uint8_t block_index = block_buffer_planned;
block_t *current;
const block_t * previous = NULL;
while (block_index != block_buffer_head) {
// Perform the forward pass
current = &block_buffer[block_index];
// Skip SYNC blocks
if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
forward_pass_kernel(previous, current, block_index);
previous = current;
}
// Advance to the previous
block_index = next_block_index(block_index);
}
forward_pass_kernel(block[1], block[2]);
}
/**
@ -917,38 +1035,73 @@ void Planner::forward_pass() {
* recalculate() after updating the blocks.
*/
void Planner::recalculate_trapezoids() {
int8_t block_index = block_buffer_tail;
block_t *current, *next = NULL;
// 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;
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);
// Get the pointer to the block
block_t *prev = &block_buffer[prev_index];
// If not dealing with a sync block, we are done. The last block is not a SYNC block
if (!TEST(prev->flag, BLOCK_BIT_SYNC_POSITION)) break;
// 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;
float current_entry_speed = 0.0, next_entry_speed = 0.0;
while (block_index != head_block_index) {
while (block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
if (current) {
// Recalculate if current block entry or exit junction speed has changed.
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.
const float 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
CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
// Skip sync blocks
if (!TEST(next->flag, BLOCK_BIT_SYNC_POSITION)) {
next_entry_speed = SQRT(next->entry_speed_sqr);
if (current) {
// Recalculate if current block entry or exit junction speed has changed.
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.
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
CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
}
}
current = next;
current_entry_speed = next_entry_speed;
}
block_index = next_block_index(block_index);
}
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
if (next) {
const float nomr = 1.0 / next->nominal_speed;
calculate_trapezoid_for_block(next, next->entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
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->max_adv_steps = next_nominal_speed * comp;
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
}
#endif
@ -956,33 +1109,14 @@ void Planner::recalculate_trapezoids() {
}
}
/**
* Recalculate the motion plan according to the following algorithm:
*
* 1. Go over every block in reverse order...
*
* Calculate a junction speed reduction (block_t.entry_factor) so:
*
* a. The junction jerk is within the set limit, and
*
* b. No speed reduction within one block requires faster
* deceleration than the one, true constant acceleration.
*
* 2. Go over every block in chronological order...
*
* Dial down junction speed reduction values if:
* a. The speed increase within one block would require faster
* acceleration than the one, true constant acceleration.
*
* After that, all blocks will have an entry_factor allowing all speed changes to
* be performed using only the one, true constant acceleration, and where no junction
* jerk is jerkier than the set limit, Jerky. Finally it will:
*
* 3. Recalculate "trapezoids" for all blocks.
*/
void Planner::recalculate() {
reverse_pass();
forward_pass();
// Initialize block index to the last block in the planner buffer.
const uint8_t block_index = prev_block_index(block_buffer_head);
// If there is just one block, no planning can be done. Avoid it!
if (block_index != block_buffer_planned) {
reverse_pass();
forward_pass();
}
recalculate_trapezoids();
}
@ -998,7 +1132,7 @@ void Planner::recalculate() {
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block_t* block = &block_buffer[b];
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
const float se = (float)block->steps[E_AXIS] / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
NOLESS(high, se);
}
}
@ -1299,6 +1433,53 @@ void Planner::check_axes_activity() {
#endif // PLANNER_LEVELING
void Planner::quick_stop() {
// Remove all the queued blocks. Note that this function is NOT
// called from the Stepper ISR, so we must consider tail as readonly!
// that is why we set head to tail - But there is a race condition that
// must be handled: The tail could change between the read and the assignment
// so this must be enclosed in a critical section
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
// Drop all queue entries
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.
delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
#if ENABLED(ULTRA_LCD)
// Clear the accumulated runtime
clear_block_buffer_runtime();
#endif
// Make sure to drop any attempt of queuing moves for at least 1 second
cleaning_buffer_counter = 1000;
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
// And stop the stepper ISR
stepper.quick_stop();
}
void Planner::endstop_triggered(const AxisEnum axis) {
// Record stepper position and discard the current block
stepper.endstop_triggered(axis);
}
float Planner::triggered_position_mm(const AxisEnum axis) {
return stepper.triggered_position(axis) * steps_to_mm[axis];
}
void Planner::finish_and_disable() {
while (has_blocks_queued() || cleaning_buffer_counter) idle();
disable_all_steppers();
}
/**
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
@ -1311,7 +1492,7 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
// Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED();
DISABLE_STEPPER_DRIVER_INTERRUPT();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
// ((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
@ -1333,18 +1514,79 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
/**
* Block until all buffered steps are executed / cleaned
*/
void Planner::synchronize() { while (has_blocks_queued() || stepper.cleaning_buffer_counter) idle(); }
void Planner::synchronize() { while (has_blocks_queued() || cleaning_buffer_counter) idle(); }
/**
* Planner::_buffer_steps
*
* Add a new linear movement to the buffer (in terms of steps).
* Add a new linear movement to the planner queue (in terms of steps).
*
* target - target position in steps units
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
*
* Returns true if movement was properly queued, false otherwise
*/
void Planner::_buffer_steps(const int32_t (&target)[XYZE]
bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters
) {
// If we are cleaning, do not accept queuing of movements
if (cleaning_buffer_counter) return false;
// Wait for the next available block
uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
// Fill the block with the specified movement
if (!_populate_block(block, false, target
#if HAS_POSITION_FLOAT
, target_float
#endif
, fr_mm_s, extruder, millimeters
)) {
// Movement was not queued, probably because it was too short.
// Simply accept that as movement queued and done
return true;
}
// If this is the first added movement, reload the delay, otherwise, cancel it.
if (block_buffer_head == block_buffer_tail) {
// If it was the first queued block, restart the 1st block delivery delay, to
// give the planner an opportunity to queue more movements and plan them
// As there are no queued movements, the Stepper ISR will not touch this
// variable, so there is no risk setting this here (but it MUST be done
// before the following line!!)
delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
}
// Move buffer head
block_buffer_head = next_buffer_head;
// Recalculate and optimize trapezoidal speed profiles
recalculate();
// Movement successfully queued!
return true;
}
/**
* Planner::_populate_block
*
* Fills a new linear movement in the block (in terms of steps).
*
* target - target position in steps units
* fr_mm_s - (target) speed of the move
* extruder - target extruder
*
* Returns true is movement is acceptable, false otherwise
*/
bool Planner::_populate_block(block_t * const block, bool split_move,
const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
@ -1358,7 +1600,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
int32_t de = target[E_AXIS] - position[E_AXIS];
/* <-- add a slash to enable
SERIAL_ECHOPAIR(" _buffer_steps FR:", fr_mm_s);
SERIAL_ECHOPAIR(" _populate_block FR:", fr_mm_s);
SERIAL_ECHOPAIR(" A:", target[A_AXIS]);
SERIAL_ECHOPAIR(" (", da);
SERIAL_ECHOPAIR(" steps) B:", target[B_AXIS]);
@ -1425,11 +1667,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
if (de < 0) SBI(dm, E_AXIS);
const float esteps_float = de * e_factor[extruder];
const int32_t esteps = ABS(esteps_float) + 0.5;
// Wait for the next available block
uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
const uint32_t esteps = ABS(esteps_float) + 0.5;
// Clear all flags, including the "busy" bit
block->flag = 0x00;
@ -1466,7 +1704,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
block->step_event_count = MAX4(block->steps[A_AXIS], block->steps[B_AXIS], block->steps[C_AXIS], esteps);
// Bail if this is a zero-length block
if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return false;
// For a mixing extruder, get a magnified step_event_count for each
#if ENABLED(MIXING_EXTRUDER)
@ -1706,12 +1944,16 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
#endif
#if ENABLED(ULTRA_LCD)
CRITICAL_SECTION_START
block_buffer_runtime_us += segment_time_us;
CRITICAL_SECTION_END
// Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
block_buffer_runtime_us += segment_time_us;
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0
block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
#if ENABLED(FILAMENT_WIDTH_SENSOR)
@ -1799,8 +2041,8 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// Correct the speed
if (speed_factor < 1.0) {
LOOP_XYZE(i) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
}
// Compute and limit the acceleration rate for the trapezoid generator.
@ -1895,13 +2137,13 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
block->acceleration_steps_per_s2 = accel;
block->acceleration = accel / steps_per_mm;
#if DISABLED(BEZIER_JERK_CONTROL)
block->acceleration_rate = (long)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
block->acceleration_rate = (uint32_t)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
#endif
#if ENABLED(LIN_ADVANCE)
if (block->use_advance_lead) {
block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]);
#if ENABLED(LA_DEBUG)
if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < block->nominal_speed * block->e_D_ratio)
if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio)
SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
if (block->advance_speed < 200)
SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
@ -1909,7 +2151,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
}
#endif
float vmax_junction; // Initial limit on the segment entry velocity
float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
#if ENABLED(JUNCTION_DEVIATION)
@ -1935,7 +2177,17 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
* changed dynamically during operation nor can the line move geometry. This must be kept in
* memory in the event of a feedrate override changing the nominal speeds of blocks, which can
* change the overall maximum entry speed conditions of all blocks.
*/
*
* #######
* https://github.com/MarlinFirmware/Marlin/issues/10341#issuecomment-388191754
*
* hoffbaked: on May 10 2018 tuned and improved the GRBL algorithm for Marlin:
Okay! It seems to be working good. I somewhat arbitrarily cut it off at 1mm
on then on anything with less sides than an octagon. With this, and the
reverse pass actually recalculating things, a corner acceleration value
of 1000 junction deviation of .05 are pretty reasonable. If the cycles
can be spared, a better acos could be used. For all I know, it may be
already calculated in a different place. */
// Unit vector of previous path line segment
static float previous_unit_vec[
@ -1956,7 +2208,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
};
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
@ -1970,21 +2222,33 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
if (junction_cos_theta > 0.999999) {
// For a 0 degree acute junction, just set minimum junction speed.
vmax_junction = MINIMUM_PLANNER_SPEED;
vmax_junction_sqr = sq(MINIMUM_PLANNER_SPEED);
}
else {
junction_cos_theta = MAX(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
// two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
vmax_junction = SQRT((block->acceleration * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2));
vmax_junction_sqr = (JUNCTION_ACCELERATION_FACTOR * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2);
if (block->millimeters < 1.0) {
// Fast acos approximation, minus the error bar to be safe
float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18;
// If angle is greater than 135 degrees (octagon), find speed for approximate arc
if (junction_theta > RADIANS(135)) {
const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * JUNCTION_ACCELERATION_FACTOR;
NOMORE(vmax_junction_sqr, limit_sqr);
}
}
}
vmax_junction = MIN3(vmax_junction, block->nominal_speed, previous_nominal_speed);
// Get the lowest speed
vmax_junction_sqr = MIN3(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
}
else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
vmax_junction = 0.0;
vmax_junction_sqr = 0.0;
COPY(previous_unit_vec, unit_vec);
@ -2000,13 +2264,15 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// Exit speed limited by a jerk to full halt of a previous last segment
static float previous_safe_speed;
float safe_speed = block->nominal_speed;
const float nominal_speed = SQRT(block->nominal_speed_sqr);
float safe_speed = nominal_speed;
uint8_t limited = 0;
LOOP_XYZE(i) {
const float jerk = ABS(current_speed[i]), maxj = max_jerk[i];
if (jerk > maxj) {
if (limited) {
const float mjerk = maxj * block->nominal_speed;
const float mjerk = maxj * nominal_speed;
if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
}
else {
@ -2016,19 +2282,21 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
}
}
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
float vmax_junction;
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
// Estimate a maximum velocity allowed at a joint of two successive segments.
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
// then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
// Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
vmax_junction = MIN(block->nominal_speed, previous_nominal_speed);
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
float v_factor = 1;
limited = 0;
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
// Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
const float previous_nominal_speed = SQRT(previous_nominal_speed_sqr);
vmax_junction = MIN(nominal_speed, previous_nominal_speed);
// Now limit the jerk in all axes.
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
LOOP_XYZE(axis) {
@ -2063,16 +2331,19 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
vmax_junction = safe_speed;
previous_safe_speed = safe_speed;
vmax_junction_sqr = sq(vmax_junction);
#endif // Classic Jerk Limiting
// Max entry speed of this block equals the max exit speed of the previous block.
block->max_entry_speed = vmax_junction;
block->max_entry_speed_sqr = vmax_junction_sqr;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
const float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
// If stepper ISR is disabled, this indicates buffer_segment wants to add a split block.
// In this case start with the max. allowed speed to avoid an interrupted first move.
block->entry_speed = STEPPER_ISR_ENABLED() ? MINIMUM_PLANNER_SPEED : MIN(vmax_junction, v_allowable);
const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(MINIMUM_PLANNER_SPEED), block->millimeters);
// If we are trying to add a split block, start with the
// max. allowed speed to avoid an interrupted first move.
block->entry_speed_sqr = !split_move ? sq(MINIMUM_PLANNER_SPEED) : MIN(vmax_junction_sqr, v_allowable_sqr);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
@ -2082,25 +2353,22 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->flag |= block->nominal_speed <= v_allowable ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
block->flag |= block->nominal_speed_sqr <= v_allowable_sqr ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
// Update previous path unit_vector and nominal speed
COPY(previous_speed, current_speed);
previous_nominal_speed = block->nominal_speed;
previous_nominal_speed_sqr = block->nominal_speed_sqr;
// Move buffer head
block_buffer_head = next_buffer_head;
// Update the position (only when a move was queued)
// Update the position
static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
COPY(position, target);
#if HAS_POSITION_FLOAT
COPY(position_float, target_float);
#endif
recalculate();
} // _buffer_steps()
// Movement was accepted
return true;
} // _populate_block()
/**
* Planner::buffer_sync_block
@ -2111,31 +2379,28 @@ void Planner::buffer_sync_block() {
uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
// Clear block
memset(block, 0, sizeof(block_t));
block->flag = BLOCK_FLAG_SYNC_POSITION;
block->steps[A_AXIS] = position[A_AXIS];
block->steps[B_AXIS] = position[B_AXIS];
block->steps[C_AXIS] = position[C_AXIS];
block->steps[E_AXIS] = position[E_AXIS];
block->position[A_AXIS] = position[A_AXIS];
block->position[B_AXIS] = position[B_AXIS];
block->position[C_AXIS] = position[C_AXIS];
block->position[E_AXIS] = position[E_AXIS];
#if ENABLED(LIN_ADVANCE)
block->use_advance_lead = false;
#endif
block->nominal_speed =
block->entry_speed =
block->max_entry_speed =
block->millimeters =
block->acceleration = 0;
block->step_event_count =
block->nominal_rate =
block->initial_rate =
block->final_rate =
block->acceleration_steps_per_s2 =
block->segment_time_us = 0;
// If this is the first added movement, reload the delay, otherwise, cancel it.
if (block_buffer_head == block_buffer_tail) {
// If it was the first queued block, restart the 1st block delivery delay, to
// give the planner an opportunity to queue more movements and plan them
// As there are no queued movements, the Stepper ISR will not touch this
// variable, so there is no risk setting this here (but it MUST be done
// before the following line!!)
delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
}
block_buffer_head = next_buffer_head;
stepper.wake_up();
} // buffer_sync_block()
@ -2151,7 +2416,11 @@ void Planner::buffer_sync_block() {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
// If we are cleaning, do not accept queuing of movements
if (cleaning_buffer_counter) return false;
// When changing extruders recalculate steps corresponding to the E position
#if ENABLED(DISTINCT_E_FACTORS)
if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
@ -2209,48 +2478,18 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
SERIAL_ECHOLNPGM(")");
//*/
// Always split the first move into two (if not homing or probing)
if (!has_blocks_queued()) {
#define _BETWEEN(A) (position[_AXIS(A)] + target[_AXIS(A)]) >> 1
const int32_t between[ABCE] = { _BETWEEN(A), _BETWEEN(B), _BETWEEN(C), _BETWEEN(E) };
#if HAS_POSITION_FLOAT
#define _BETWEEN_F(A) (position_float[_AXIS(A)] + target_float[_AXIS(A)]) * 0.5
const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) };
#endif
DISABLE_STEPPER_DRIVER_INTERRUPT();
_buffer_steps(between
#if HAS_POSITION_FLOAT
, between_float
#endif
, fr_mm_s, extruder, millimeters * 0.5
);
const uint8_t next = block_buffer_head;
_buffer_steps(target
#if HAS_POSITION_FLOAT
, target_float
#endif
, fr_mm_s, extruder, millimeters * 0.5
);
SBI(block_buffer[next].flag, BLOCK_BIT_CONTINUED);
ENABLE_STEPPER_DRIVER_INTERRUPT();
}
else
_buffer_steps(target
// Queue the movement
if (
!_buffer_steps(target
#if HAS_POSITION_FLOAT
, target_float
#endif
, fr_mm_s, extruder, millimeters
);
)
) return false;
stepper.wake_up();
return true;
} // buffer_segment()
/**
@ -2277,7 +2516,7 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
position_float[C_AXIS] = c;
position_float[E_AXIS] = e;
#endif
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
previous_nominal_speed_sqr = 0.0; // Resets planner junction speeds. Assumes start from rest.
ZERO(previous_speed);
buffer_sync_block();
}
@ -2297,22 +2536,6 @@ void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
#endif
}
/**
* Sync from the stepper positions. (e.g., after an interrupted move)
*/
void Planner::sync_from_steppers() {
LOOP_XYZE(i) {
position[i] = stepper.position((AxisEnum)i);
#if HAS_POSITION_FLOAT
position_float[i] = position[i] * steps_to_mm[i
#if ENABLED(DISTINCT_E_FACTORS)
+ (i == E_AXIS ? active_extruder : 0)
#endif
];
#endif
}
}
/**
* Setters for planner position (also setting stepper position).
*/

281
Marlin/src/module/planner.h
View file

@ -35,6 +35,7 @@
#include "../Marlin.h"
#include "motion.h"
#include "../gcode/queue.h"
#if ENABLED(DELTA)
#include "delta.h"
@ -53,7 +54,7 @@ enum BlockFlagBit : char {
// from a safe speed (in consideration of jerking from zero speed).
BLOCK_BIT_NOMINAL_LENGTH,
// The block is busy
// The block is busy, being interpreted by the stepper ISR
BLOCK_BIT_BUSY,
// The block is segment 2+ of a longer move
@ -84,19 +85,35 @@ typedef struct {
uint8_t flag; // Block flags (See BlockFlag enum above)
unsigned char active_extruder; // The extruder to move (if E move)
// Fields used by the motion planner to manage acceleration
float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2
entry_speed_sqr, // Entry speed at previous-current junction in (mm/sec)^2
max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2
// Fields used by the Bresenham algorithm for tracing the line
int32_t steps[NUM_AXIS]; // Step count along each axis
union {
// Data used by all move blocks
struct {
// Fields used by the Bresenham algorithm for tracing the line
uint32_t steps[NUM_AXIS]; // Step count along each axis
};
// Data used by all sync blocks
struct {
int32_t position[NUM_AXIS]; // New position to force when this sync block is executed
};
};
uint32_t step_event_count; // The number of step events required to complete this block
uint8_t active_extruder; // The extruder to move (if E move)
#if ENABLED(MIXING_EXTRUDER)
uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
#endif
// Settings for the trapezoid generator
int32_t accelerate_until, // The index of the step event on which to stop acceleration
decelerate_after; // The index of the step event on which to start decelerating
uint32_t accelerate_until, // The index of the step event on which to stop acceleration
decelerate_after; // The index of the step event on which to start decelerating
#if ENABLED(BEZIER_JERK_CONTROL)
uint32_t cruise_rate; // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
@ -105,7 +122,7 @@ typedef struct {
uint32_t acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
deceleration_time_inverse;
#else
int32_t acceleration_rate; // The acceleration rate used for acceleration calculation
uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation
#endif
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
@ -119,13 +136,6 @@ typedef struct {
float e_D_ratio;
#endif
// Fields used by the motion planner to manage acceleration
float nominal_speed, // The nominal speed for this block in mm/sec
entry_speed, // Entry speed at previous-current junction in mm/sec
max_entry_speed, // Maximum allowable junction entry speed in mm/sec
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2
uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec
initial_rate, // The jerk-adjusted step rate at start of block
final_rate, // The minimal rate at exit
@ -166,6 +176,10 @@ 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_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
#if ENABLED(DISTINCT_E_FACTORS)
static uint8_t last_extruder; // Respond to extruder change
@ -233,6 +247,10 @@ class Planner {
#endif
#endif
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
static bool abort_on_endstop_hit;
#endif
private:
/**
@ -247,9 +265,9 @@ class Planner {
static float previous_speed[NUM_AXIS];
/**
* Nominal speed of previous path line segment
* Nominal speed of previous path line segment (mm/s)^2
*/
static float previous_nominal_speed;
static float previous_nominal_speed_sqr;
/**
* Limit where 64bit math is necessary for acceleration calculation
@ -308,15 +326,6 @@ class Planner {
// Manage fans, paste pressure, etc.
static void check_axes_activity();
/**
* Number of moves currently in the planner
*/
FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
// Update multipliers based on new diameter measurements
static void calculate_volumetric_multipliers();
@ -424,16 +433,32 @@ class Planner {
#define ARG_Z const float &rz
#endif
// Number of moves currently in the planner
FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
// Remove all blocks from the buffer
FORCE_INLINE static void clear_block_buffer() { 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); }
// Get count of movement slots free
FORCE_INLINE static uint8_t moves_free() { return BLOCK_BUFFER_SIZE - 1 - movesplanned(); }
/**
* Planner::get_next_free_block
*
* - Get the next head index (passed by reference)
* - Wait for a space to open up in the planner
* - Return the head block
* - Get the next head indices (passed by reference)
* - Wait for the number of spaces to open up in the planner
* - Return the first head block
*/
FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) {
FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head, uint8_t count = 1) {
// Wait until there are enough slots free
while (moves_free() < count) { idle(); }
// Return the first available block
next_buffer_head = next_block_index(block_buffer_head);
while (block_buffer_tail == next_buffer_head) idle(); // while (is_full)
return &block_buffer[block_buffer_head];
}
@ -446,8 +471,30 @@ class Planner {
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
*
* Returns true if movement was buffered, false otherwise
*/
static void _buffer_steps(const int32_t (&target)[XYZE]
static bool _buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
/**
* Planner::_populate_block
*
* Fills a new linear movement in the block (in terms of steps).
*
* target - target position in steps units
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
*
* Returns true is movement is acceptable, false otherwise
*/
static bool _populate_block(block_t * const block, bool split_move,
const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
@ -472,7 +519,7 @@ class Planner {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
static bool buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
@ -489,11 +536,11 @@ class Planner {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
FORCE_INLINE static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz);
#endif
buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
return buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
}
/**
@ -506,7 +553,7 @@ class Planner {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
@ -515,9 +562,9 @@ class Planner {
#endif
#if IS_KINEMATIC
inverse_kinematics(raw);
buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
return buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#else
buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
return buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#endif
}
@ -541,11 +588,6 @@ class Planner {
FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
/**
* Sync from the stepper positions. (e.g., after an interrupted move)
*/
static void sync_from_steppers();
/**
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
@ -557,73 +599,116 @@ class Planner {
FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
#endif
// Called to force a quick stop of the machine (for example, when an emergency
// stop is required, or when endstops are hit)
static void quick_stop();
// Called when an endstop is triggered. Causes the machine to stop inmediately
static void endstop_triggered(const AxisEnum axis);
// Triggered position of an axis in mm (not core-savvy)
static float triggered_position_mm(const AxisEnum axis);
// Block until all buffered steps are executed / cleaned
static void synchronize();
// Wait for moves to finish and disable all steppers
static void finish_and_disable();
// Periodic tick to handle cleaning timeouts
// Called from the Temperature ISR at ~1kHz
static void tick() {
if (cleaning_buffer_counter) {
--cleaning_buffer_counter;
#if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
#endif
}
}
/**
* Does the buffer have any blocks queued?
*/
FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
//
// Block until all buffered steps are executed
//
static void synchronize();
/**
* "Discard" the block and "release" the memory.
* Called when the current block is no longer needed.
*/
FORCE_INLINE static void discard_current_block() {
if (has_blocks_queued())
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
}
/**
* "Discard" the next block if it's continued.
* Called after an interrupted move to throw away the rest of the move.
*/
FORCE_INLINE static bool discard_continued_block() {
const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED);
if (discard) discard_current_block();
return discard;
}
/**
* The current block. NULL if the buffer is empty.
* This also marks the block as busy.
* WARNING: Called from Stepper ISR context!
*/
static block_t* get_current_block() {
if (has_blocks_queued()) {
// Get the number of moves in the planner queue so far
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 NULL;
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];
// If the block has no trapezoid calculated, it's unsafe to execute.
if (movesplanned() > 1) {
const block_t * const next = &block_buffer[next_block_index(block_buffer_tail)];
if (TEST(block->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE))
return NULL;
}
else if (TEST(block->flag, BLOCK_BIT_RECALCULATE))
return NULL;
// No trapezoid calculated? Don't execute yet.
if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return NULL;
#if ENABLED(ULTRA_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
// Mark the block as busy, so the planner does not attempt to replan it
SBI(block->flag, BLOCK_BIT_BUSY);
return block;
}
else {
#if ENABLED(ULTRA_LCD)
clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
#endif
return NULL;
// The queue became empty
#if ENABLED(ULTRA_LCD)
clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
#endif
return NULL;
}
/**
* "Discard" the block and "release" the memory.
* 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() {
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 ENABLED(ULTRA_LCD)
static uint16_t block_buffer_runtime() {
CRITICAL_SECTION_START
millis_t bbru = block_buffer_runtime_us;
CRITICAL_SECTION_END
#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.
@ -634,9 +719,19 @@ class Planner {
}
static void clear_block_buffer_runtime() {
CRITICAL_SECTION_START
block_buffer_runtime_us = 0;
CRITICAL_SECTION_END
#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
@ -653,8 +748,8 @@ class Planner {
/**
* Get the index of the next / previous block in the ring buffer
*/
static constexpr int8_t next_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); }
static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); }
static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); }
/**
* Calculate the distance (not time) it takes to accelerate
@ -679,12 +774,12 @@ class Planner {
}
/**
* Calculate the maximum allowable speed at this point, in order
* to reach 'target_velocity' using 'acceleration' within a given
* Calculate the maximum allowable speed squared at this point, in order
* to reach 'target_velocity_sqr' using 'acceleration' within a given
* 'distance'.
*/
static float max_allowable_speed(const float &accel, const float &target_velocity, const float &distance) {
return SQRT(sq(target_velocity) - 2 * accel * distance);
static float max_allowable_speed_sqr(const float &accel, const float &target_velocity_sqr, const float &distance) {
return target_velocity_sqr - 2 * accel * distance;
}
#if ENABLED(BEZIER_JERK_CONTROL)
@ -699,7 +794,7 @@ class Planner {
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 * const next);
static void forward_pass_kernel(const block_t * const previous, block_t* const current);
static void forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index);
static void reverse_pass();
static void forward_pass();

6
Marlin/src/module/planner_bezier.cpp
View file

@ -194,9 +194,11 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
#if HAS_UBL_AND_CURVES
float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] };
planner.apply_leveling(pos);
planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder);
if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder))
break;
#else
planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder);
if (!planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder))
break;
#endif
}
}

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

@ -86,10 +86,6 @@ Stepper stepper; // Singleton
block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
bool Stepper::abort_on_endstop_hit = false;
#endif
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
bool Stepper::performing_homing = false;
#endif
@ -100,8 +96,10 @@ block_t* Stepper::current_block = NULL; // A pointer to the block currently bei
// private:
uint8_t Stepper::last_direction_bits = 0; // The next stepping-bits to be output
int16_t Stepper::cleaning_buffer_counter = 0;
uint8_t Stepper::last_direction_bits = 0, // The next stepping-bits to be output
Stepper::last_movement_extruder = 0xFF; // Last movement extruder, as computed when the last movement was fetched from planner
bool Stepper::abort_current_block, // Signals to the stepper that current block should be aborted
Stepper::last_movement_non_null[NUM_AXIS]; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
#if ENABLED(X_DUAL_ENDSTOPS)
bool Stepper::locked_x_motor = false, Stepper::locked_x2_motor = false;
@ -118,7 +116,7 @@ int32_t Stepper::counter_X = 0,
Stepper::counter_Z = 0,
Stepper::counter_E = 0;
volatile uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block
uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block
#if ENABLED(BEZIER_JERK_CONTROL)
int32_t __attribute__((used)) Stepper::bezier_A __asm__("bezier_A"); // A coefficient in Bézier speed curve with alias for assembler
@ -132,15 +130,16 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not
#endif
uint32_t Stepper::nextMainISR = 0;
bool Stepper::all_steps_done = false;
#if ENABLED(LIN_ADVANCE)
uint32_t Stepper::LA_decelerate_after;
constexpr hal_timer_t ADV_NEVER = HAL_TIMER_TYPE_MAX;
hal_timer_t Stepper::nextMainISR = 0,
Stepper::nextAdvanceISR = ADV_NEVER,
Stepper::eISR_Rate = ADV_NEVER;
constexpr uint32_t ADV_NEVER = 0xFFFFFFFF;
uint32_t Stepper::nextAdvanceISR = ADV_NEVER,
Stepper::eISR_Rate = ADV_NEVER;
uint16_t Stepper::current_adv_steps = 0,
Stepper::final_adv_steps,
Stepper::max_adv_steps;
@ -157,7 +156,7 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
#endif // LIN_ADVANCE
int32_t Stepper::acceleration_time, Stepper::deceleration_time;
uint32_t Stepper::acceleration_time, Stepper::deceleration_time;
volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
@ -166,11 +165,11 @@ volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
int32_t Stepper::counter_m[MIXING_STEPPERS];
#endif
uint32_t Stepper::ticks_nominal;
uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
hal_timer_t Stepper::OCR1A_nominal;
#if DISABLED(BEZIER_JERK_CONTROL)
hal_timer_t Stepper::acc_step_rate; // needed for deceleration start point
uint32_t Stepper::acc_step_rate; // needed for deceleration start point
#endif
volatile int32_t Stepper::endstops_trigsteps[XYZ];
@ -185,12 +184,12 @@ volatile int32_t Stepper::endstops_trigsteps[XYZ];
#define DUAL_ENDSTOP_APPLY_STEP(A,V) \
if (performing_homing) { \
if (A##_HOME_DIR < 0) { \
if (!(TEST(endstops.old_endstop_bits, A##_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V); \
if (!(TEST(endstops.old_endstop_bits, A##2_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V); \
if (!(TEST(endstops.current_endstop_bits, A##_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V); \
if (!(TEST(endstops.current_endstop_bits, A##2_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V); \
} \
else { \
if (!(TEST(endstops.old_endstop_bits, A##_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V); \
if (!(TEST(endstops.old_endstop_bits, A##2_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V); \
if (!(TEST(endstops.current_endstop_bits, A##_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V); \
if (!(TEST(endstops.current_endstop_bits, A##2_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V); \
} \
} \
else { \
@ -319,10 +318,6 @@ void Stepper::set_directions() {
#endif // !LIN_ADVANCE
}
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
extern volatile uint8_t e_hit;
#endif
#if ENABLED(BEZIER_JERK_CONTROL)
/**
* We are using a quintic (fifth-degree) Bézier polynomial for the velocity curve.
@ -379,7 +374,7 @@ void Stepper::set_directions() {
*
* Floating point arithmetic execution time cost is prohibitive, so we will transform the math to
* use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps
* per second (driver pulses should at least be 2uS hi/2uS lo), and allocating 2 bits to avoid
* per second (driver pulses should at least be 2µS hi/2µS lo), and allocating 2 bits to avoid
* overflows on the evaluation of the Bézier curve, means we can use
*
* t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned
@ -1149,11 +1144,27 @@ void Stepper::set_directions() {
HAL_STEP_TIMER_ISR {
HAL_timer_isr_prologue(STEP_TIMER_NUM);
#if ENABLED(LIN_ADVANCE)
Stepper::advance_isr_scheduler();
#else
Stepper::isr();
#endif
// Program timer compare for the maximum period, so it does NOT
// flag an interrupt while this ISR is running - So changes from small
// periods to big periods are respected and the timer does not reset to 0
HAL_timer_set_compare(STEP_TIMER_NUM, HAL_TIMER_TYPE_MAX);
// Call the ISR scheduler
hal_timer_t ticks = Stepper::isr_scheduler();
// Now 'ticks' contains the period to the next Stepper ISR.
// Potential problem: Since the timer continues to run, the requested
// compare value may already have passed.
//
// Assuming at least 6µs between calls to this ISR...
// On AVR the ISR epilogue is estimated at 40 instructions - close to 2.5µS.
// On ARM the ISR epilogue is estimated at 10 instructions - close to 200nS.
// In either case leave at least 4µS for other tasks to execute.
const hal_timer_t minticks = HAL_timer_get_count(STEP_TIMER_NUM) + hal_timer_t((HAL_TICKS_PER_US) * 4); // ISR never takes more than 1ms, so this shouldn't cause trouble
NOLESS(ticks, MAX(minticks, hal_timer_t((STEP_TIMER_MIN_INTERVAL) * (HAL_TICKS_PER_US))));
// Set the next ISR to fire at the proper time
HAL_timer_set_compare(STEP_TIMER_NUM, ticks);
HAL_timer_isr_epilogue(STEP_TIMER_NUM);
}
@ -1164,168 +1175,73 @@ HAL_STEP_TIMER_ISR {
#define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B)
#endif
void Stepper::isr() {
hal_timer_t Stepper::isr_scheduler() {
uint32_t interval;
#define ENDSTOP_NOMINAL_OCR_VAL 1500 * HAL_TICKS_PER_US // Check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch
#define OCR_VAL_TOLERANCE 500 * HAL_TICKS_PER_US // First max delay is 2.0ms, last min delay is 0.5ms, all others 1.5ms
// Run main stepping pulse phase ISR if we have to
if (!nextMainISR) Stepper::stepper_pulse_phase_isr();
hal_timer_t ocr_val;
static uint32_t step_remaining = 0; // SPLIT function always runs. This allows 16 bit timers to be
// used to generate the stepper ISR.
#define SPLIT(L) do { \
if (L > ENDSTOP_NOMINAL_OCR_VAL) { \
const uint32_t remainder = (uint32_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \
ocr_val = (remainder < OCR_VAL_TOLERANCE) ? ENDSTOP_NOMINAL_OCR_VAL + remainder : ENDSTOP_NOMINAL_OCR_VAL; \
step_remaining = (uint32_t)L - ocr_val; \
} \
else \
ocr_val = L;\
}while(0)
// Time remaining before the next step?
if (step_remaining) {
// Make sure endstops are updated
if (ENDSTOPS_ENABLED) endstops.update();
// Next ISR either for endstops or stepping
ocr_val = step_remaining <= ENDSTOP_NOMINAL_OCR_VAL ? step_remaining : ENDSTOP_NOMINAL_OCR_VAL;
step_remaining -= ocr_val;
_NEXT_ISR(ocr_val);
#if DISABLED(LIN_ADVANCE)
HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
#endif
return;
}
//
// When cleaning, discard the current block and run fast
//
if (cleaning_buffer_counter) {
if (cleaning_buffer_counter < 0) { // Count up for endstop hit
if (current_block) planner.discard_current_block(); // Discard the active block that led to the trigger
if (!planner.discard_continued_block()) // Discard next CONTINUED block
cleaning_buffer_counter = 0; // Keep discarding until non-CONTINUED
}
else {
planner.discard_current_block();
--cleaning_buffer_counter; // Count down for abort print
#if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
#endif
}
current_block = NULL; // Prep to get a new block after cleaning
_NEXT_ISR(HAL_STEPPER_TIMER_RATE / 10000); // Run at max speed - 10 KHz
return;
}
// If there is no current block, attempt to pop one from the buffer
if (!current_block) {
// Anything in the buffer?
if ((current_block = planner.get_current_block())) {
// Sync block? Sync the stepper counts and return
while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
_set_position(
current_block->steps[A_AXIS], current_block->steps[B_AXIS],
current_block->steps[C_AXIS], current_block->steps[E_AXIS]
);
planner.discard_current_block();
if (!(current_block = planner.get_current_block())) return;
}
// Initialize the trapezoid generator from the current block.
static int8_t last_extruder = -1;
#if ENABLED(LIN_ADVANCE)
#if E_STEPPERS > 1
if (current_block->active_extruder != last_extruder) {
current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
LA_active_extruder = current_block->active_extruder;
}
#endif
if ((use_advance_lead = current_block->use_advance_lead)) {
LA_decelerate_after = current_block->decelerate_after;
final_adv_steps = current_block->final_adv_steps;
max_adv_steps = current_block->max_adv_steps;
}
#endif
if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) {
last_direction_bits = current_block->direction_bits;
last_extruder = current_block->active_extruder;
set_directions();
}
// No acceleration / deceleration time elapsed so far
acceleration_time = deceleration_time = 0;
// No step events completed so far
step_events_completed = 0;
// step_rate to timer interval
OCR1A_nominal = calc_timer_interval(current_block->nominal_rate);
// make a note of the number of step loops required at nominal speed
step_loops_nominal = step_loops;
#if DISABLED(BEZIER_JERK_CONTROL)
// Set as deceleration point the initial rate of the block
acc_step_rate = current_block->initial_rate;
#endif
#if ENABLED(BEZIER_JERK_CONTROL)
// Initialize the Bézier speed curve
_calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
// We have not started the 2nd half of the trapezoid
bezier_2nd_half = false;
#endif
// Initialize Bresenham counters to 1/2 the ceiling
counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
#if ENABLED(MIXING_EXTRUDER)
MIXING_STEPPERS_LOOP(i)
counter_m[i] = -(current_block->mix_event_count[i] >> 1);
#endif
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins.
// No 'change' can be detected.
#endif
#if ENABLED(Z_LATE_ENABLE)
// If delayed Z enable, postpone move for 1mS
if (current_block->steps[Z_AXIS] > 0) {
enable_Z();
_NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
return;
}
#endif
}
else {
// If no more queued moves, postpone next check for 1mS
_NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
return;
}
}
// Update endstops state, if enabled
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
if (e_hit && ENDSTOPS_ENABLED) {
endstops.update();
e_hit--;
}
#else
if (ENDSTOPS_ENABLED) endstops.update();
#if ENABLED(LIN_ADVANCE)
// Run linear advance stepper ISR if we have to
if (!nextAdvanceISR) nextAdvanceISR = Stepper::advance_isr();
#endif
// ^== Time critical. NOTHING besides pulse generation should be above here!!!
// Run main stepping block processing ISR if we have to
if (!nextMainISR) nextMainISR = Stepper::stepper_block_phase_isr();
#if ENABLED(LIN_ADVANCE)
// Select the closest interval in time
interval = (nextAdvanceISR <= nextMainISR)
? nextAdvanceISR
: nextMainISR;
#else // !ENABLED(LIN_ADVANCE)
// The interval is just the remaining time to the stepper ISR
interval = nextMainISR;
#endif
// Limit the value to the maximum possible value of the timer
if (interval > HAL_TIMER_TYPE_MAX)
interval = HAL_TIMER_TYPE_MAX;
// Compute the time remaining for the main isr
nextMainISR -= interval;
#if ENABLED(LIN_ADVANCE)
// Compute the time remaining for the advance isr
if (nextAdvanceISR != ADV_NEVER)
nextAdvanceISR -= interval;
#endif
return (hal_timer_t)interval;
}
// This part of the ISR should ONLY create the pulses for the steppers
// -- Nothing more, nothing less -- We want to avoid jitter from where
// the pulses should be generated (when the interrupt triggers) to the
// time pulses are actually created. So, PLEASE DO NOT PLACE ANY CODE
// above this line that can conditionally change that time (we are trying
// to keep the delay between the interrupt triggering and pulse generation
// as constant as possible!!!!
void Stepper::stepper_pulse_phase_isr() {
// If we must abort the current block, do so!
if (abort_current_block) {
abort_current_block = false;
if (current_block) {
current_block = NULL;
planner.discard_current_block();
}
}
// If there is no current block, do nothing
if (!current_block) return;
// Take multiple steps per interrupt (For high speed moves)
bool all_steps_done = false;
all_steps_done = false;
for (uint8_t i = step_loops; i--;) {
#define _COUNTER(AXIS) counter_## AXIS
@ -1520,116 +1436,213 @@ void Stepper::isr() {
#endif
} // steps_loop
}
// Calculate new timer value
if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
// This is the last half of the stepper interrupt: This one processes and
// 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.
#if ENABLED(BEZIER_JERK_CONTROL)
// Get the next speed to use (Jerk limited!)
hal_timer_t acc_step_rate =
acceleration_time < current_block->acceleration_time
? _eval_bezier_curve(acceleration_time)
: current_block->cruise_rate;
#else
acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
NOMORE(acc_step_rate, current_block->nominal_rate);
#endif
uint32_t Stepper::stepper_block_phase_isr() {
// step_rate to timer interval
const hal_timer_t interval = calc_timer_interval(acc_step_rate);
// If no queued movements, just wait 1ms for the next move
uint32_t interval = (HAL_STEPPER_TIMER_RATE / 1000);
SPLIT(interval); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
_NEXT_ISR(ocr_val);
// If there is a current block
if (current_block) {
acceleration_time += interval;
// Calculate new timer value
if (step_events_completed <= current_block->accelerate_until) {
#if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) {
if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
eISR_Rate = current_block->advance_speed;
#if ENABLED(BEZIER_JERK_CONTROL)
// Get the next speed to use (Jerk limited!)
uint32_t acc_step_rate =
acceleration_time < current_block->acceleration_time
? _eval_bezier_curve(acceleration_time)
: current_block->cruise_rate;
#else
acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
NOMORE(acc_step_rate, current_block->nominal_rate);
#endif
// step_rate to timer interval
interval = calc_timer_interval(acc_step_rate);
acceleration_time += interval;
#if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) {
if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
eISR_Rate = current_block->advance_speed;
}
}
}
else {
eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0;
}
#endif // LIN_ADVANCE
else {
eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0;
}
#endif // LIN_ADVANCE
}
else if (step_events_completed > current_block->decelerate_after) {
uint32_t step_rate;
#if ENABLED(BEZIER_JERK_CONTROL)
// If this is the 1st time we process the 2nd half of the trapezoid...
if (!bezier_2nd_half) {
// Initialize the Bézier speed curve
_calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
bezier_2nd_half = true;
}
// Calculate the next speed to use
step_rate = deceleration_time < current_block->deceleration_time
? _eval_bezier_curve(deceleration_time)
: current_block->final_rate;
#else
// Using the old trapezoidal control
step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
if (step_rate < acc_step_rate) { // Still decelerating?
step_rate = acc_step_rate - step_rate;
NOLESS(step_rate, current_block->final_rate);
}
else
step_rate = current_block->final_rate;
#endif
// step_rate to timer interval
interval = calc_timer_interval(step_rate);
deceleration_time += interval;
#if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) {
if (step_events_completed <= current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
eISR_Rate = current_block->advance_speed;
}
}
else {
eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0;
}
#endif // LIN_ADVANCE
}
else {
#if ENABLED(LIN_ADVANCE)
// If there are any esteps, fire the next advance_isr "now"
if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
#endif
// The timer interval is just the nominal value for the nominal speed
interval = ticks_nominal;
// Ensure this runs at the correct step rate, even if it just came off an acceleration
step_loops = step_loops_nominal;
}
// If current block is finished, reset pointer
if (all_steps_done) {
current_block = NULL;
planner.discard_current_block();
}
}
else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
hal_timer_t step_rate;
#if ENABLED(BEZIER_JERK_CONTROL)
// If this is the 1st time we process the 2nd half of the trapezoid...
if (!bezier_2nd_half) {
// If there is no current block at this point, attempt to pop one from the buffer
// and prepare its movement
if (!current_block) {
// Anything in the buffer?
if ((current_block = planner.get_current_block())) {
// Sync block? Sync the stepper counts and return
while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
_set_position(
current_block->position[A_AXIS], current_block->position[B_AXIS],
current_block->position[C_AXIS], current_block->position[E_AXIS]
);
planner.discard_current_block();
// Try to get a new block
if (!(current_block = planner.get_current_block()))
return interval; // No more queued movements!
}
// Compute movement direction for proper endstop handling
LOOP_NA(i) last_movement_non_null[i] = !!current_block->steps[i];
// Initialize the trapezoid generator from the current block.
#if ENABLED(LIN_ADVANCE)
#if E_STEPPERS > 1
if (current_block->active_extruder != last_movement_extruder) {
current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
LA_active_extruder = current_block->active_extruder;
}
#endif
if ((use_advance_lead = current_block->use_advance_lead)) {
LA_decelerate_after = current_block->decelerate_after;
final_adv_steps = current_block->final_adv_steps;
max_adv_steps = current_block->max_adv_steps;
}
#endif
if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_movement_extruder) {
last_direction_bits = current_block->direction_bits;
last_movement_extruder = current_block->active_extruder;
set_directions();
}
// At this point, we must ensure the movement about to execute isn't
// trying to force the head against a limit switch. If using interrupt-
// driven change detection, and already against a limit then no call to
// the endstop_triggered method will be done and the movement will be
// done against the endstop. So, check the limits here: If the movement
// is against the limits, the block will be marked as to be killed, and
// on the next call to this ISR, will be discarded.
endstops.check_possible_change();
// No acceleration / deceleration time elapsed so far
acceleration_time = deceleration_time = 0;
// No step events completed so far
step_events_completed = 0;
// step_rate to timer interval for the nominal speed
ticks_nominal = calc_timer_interval(current_block->nominal_rate);
// make a note of the number of step loops required at nominal speed
step_loops_nominal = step_loops;
#if DISABLED(BEZIER_JERK_CONTROL)
// Set as deceleration point the initial rate of the block
acc_step_rate = current_block->initial_rate;
#endif
#if ENABLED(BEZIER_JERK_CONTROL)
// Initialize the Bézier speed curve
_calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
bezier_2nd_half = true;
}
_calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
// Calculate the next speed to use
step_rate = deceleration_time < current_block->deceleration_time
? _eval_bezier_curve(deceleration_time)
: current_block->final_rate;
#else
// We have not started the 2nd half of the trapezoid
bezier_2nd_half = false;
#endif
// Using the old trapezoidal control
step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
if (step_rate < acc_step_rate) { // Still decelerating?
step_rate = acc_step_rate - step_rate;
NOLESS(step_rate, current_block->final_rate);
}
else
step_rate = current_block->final_rate;
// Initialize Bresenham counters to 1/2 the ceiling
counter_X = counter_Y = counter_Z = counter_E = -((int32_t)(current_block->step_event_count >> 1));
#if ENABLED(MIXING_EXTRUDER)
MIXING_STEPPERS_LOOP(i)
counter_m[i] = -(current_block->mix_event_count[i] >> 1);
#endif
#endif
// step_rate to timer interval
const hal_timer_t interval = calc_timer_interval(step_rate);
SPLIT(interval); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
_NEXT_ISR(ocr_val);
deceleration_time += interval;
#if ENABLED(LIN_ADVANCE)
if (current_block->use_advance_lead) {
if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
eISR_Rate = current_block->advance_speed;
}
}
else {
eISR_Rate = ADV_NEVER;
if (e_steps) nextAdvanceISR = 0;
}
#endif // LIN_ADVANCE
}
else {
#if ENABLED(LIN_ADVANCE)
// If we have esteps to execute, fire the next advance_isr "now"
if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
#endif
SPLIT(OCR1A_nominal); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
_NEXT_ISR(ocr_val);
// ensure we're running at the correct step rate, even if we just came off an acceleration
step_loops = step_loops_nominal;
#if ENABLED(Z_LATE_ENABLE)
// If delayed Z enable, enable it now. This option will severely interfere with
// timing between pulses when chaining motion between blocks, and it could lead
// to lost steps in both X and Y axis, so avoid using it unless strictly necessary!!
if (current_block->steps[Z_AXIS]) enable_Z();
#endif
}
}
#if DISABLED(LIN_ADVANCE)
// Make sure stepper ISR doesn't monopolize the CPU
HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
#endif
// If current block is finished, reset pointer
if (all_steps_done) {
current_block = NULL;
planner.discard_current_block();
}
// Return the interval to wait
return interval;
}
#if ENABLED(LIN_ADVANCE)
@ -1638,8 +1651,8 @@ void Stepper::isr() {
#define EXTRA_CYCLES_E (STEP_PULSE_CYCLES - (CYCLES_EATEN_E))
// Timer interrupt for E. e_steps is set in the main routine;
void Stepper::advance_isr() {
uint32_t Stepper::advance_isr() {
uint32_t interval;
#if ENABLED(MK2_MULTIPLEXER) // For SNMM even-numbered steppers are reversed
#define SET_E_STEP_DIR(INDEX) do{ if (e_steps) E0_DIR_WRITE(e_steps < 0 ? !INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0) : INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0)); }while(0)
@ -1700,21 +1713,21 @@ void Stepper::isr() {
if (step_events_completed > LA_decelerate_after && current_adv_steps > final_adv_steps) {
e_steps--;
current_adv_steps--;
nextAdvanceISR = eISR_Rate;
interval = eISR_Rate;
}
else if (step_events_completed < LA_decelerate_after && current_adv_steps < max_adv_steps) {
//step_events_completed <= (uint32_t)current_block->accelerate_until) {
e_steps++;
current_adv_steps++;
nextAdvanceISR = eISR_Rate;
interval = eISR_Rate;
}
else {
nextAdvanceISR = ADV_NEVER;
interval = ADV_NEVER;
eISR_Rate = ADV_NEVER;
}
}
else
nextAdvanceISR = ADV_NEVER;
interval = ADV_NEVER;
switch (LA_active_extruder) {
case 0: SET_E_STEP_DIR(0); break;
@ -1787,39 +1800,9 @@ void Stepper::isr() {
#endif
} // e_steps
return interval;
}
void Stepper::advance_isr_scheduler() {
// Run main stepping ISR if flagged
if (!nextMainISR) isr();
// Run Advance stepping ISR if flagged
if (!nextAdvanceISR) advance_isr();
// Is the next advance ISR scheduled before the next main ISR?
if (nextAdvanceISR <= nextMainISR) {
// Set up the next interrupt
HAL_timer_set_compare(STEP_TIMER_NUM, nextAdvanceISR);
// New interval for the next main ISR
if (nextMainISR) nextMainISR -= nextAdvanceISR;
// Will call Stepper::advance_isr on the next interrupt
nextAdvanceISR = 0;
}
else {
// The next main ISR comes first
HAL_timer_set_compare(STEP_TIMER_NUM, nextMainISR);
// New interval for the next advance ISR, if any
if (nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
nextAdvanceISR -= nextMainISR;
// Will call Stepper::isr on the next interrupt
nextMainISR = 0;
}
// Make sure stepper ISR doesn't monopolize the CPU
HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
}
#endif // LIN_ADVANCE
void Stepper::init() {
@ -1924,9 +1907,6 @@ void Stepper::init() {
if (!E_ENABLE_ON) E4_ENABLE_WRITE(HIGH);
#endif
// Init endstops and pullups
endstops.init();
#define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
#define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
#define _DISABLE(AXIS) disable_## AXIS()
@ -2048,31 +2028,33 @@ void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c
* Get a stepper's position in steps.
*/
int32_t Stepper::position(const AxisEnum axis) {
CRITICAL_SECTION_START;
const int32_t count_pos = count_position[axis];
CRITICAL_SECTION_END;
return count_pos;
}
void Stepper::finish_and_disable() {
planner.synchronize();
disable_all_steppers();
}
void Stepper::quick_stop() {
DISABLE_STEPPER_DRIVER_INTERRUPT();
kill_current_block();
current_block = NULL;
cleaning_buffer_counter = 5000;
planner.clear_block_buffer();
ENABLE_STEPPER_DRIVER_INTERRUPT();
#if ENABLED(ULTRA_LCD)
planner.clear_block_buffer_runtime();
#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
int32_t v = count_position[axis];
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
return v;
}
// Signal endstops were triggered - This function can be called from
// an ISR context (Temperature, Stepper or limits ISR), so we must
// be very careful here. If the interrupt being preempted was the
// Stepper ISR (this CAN happen with the endstop limits ISR) then
// when the stepper ISR resumes, we must be very sure that the movement
// is properly cancelled
void Stepper::endstop_triggered(const AxisEnum axis) {
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
#if IS_CORE
endstops_trigsteps[axis] = 0.5f * (
@ -2086,16 +2068,41 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
#endif // !COREXY && !COREXZ && !COREYZ
kill_current_block();
cleaning_buffer_counter = -1; // Discard the rest of the move
// Discard the rest of the move if there is a current block
quick_stop();
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
int32_t Stepper::triggered_position(const AxisEnum axis) {
#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
const int32_t v = endstops_trigsteps[axis];
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
return v;
}
void Stepper::report_positions() {
CRITICAL_SECTION_START;
// Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED();
if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
const int32_t xpos = count_position[X_AXIS],
ypos = count_position[Y_AXIS],
zpos = count_position[Z_AXIS];
CRITICAL_SECTION_END;
if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#if CORE_IS_XY || CORE_IS_XZ || IS_DELTA || IS_SCARA
SERIAL_PROTOCOLPGM(MSG_COUNT_A);

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

@ -62,10 +62,6 @@ class Stepper {
static block_t* current_block; // A pointer to the block currently being traced
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
static bool abort_on_endstop_hit;
#endif
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
static bool performing_homing;
#endif
@ -77,11 +73,12 @@ class Stepper {
static uint32_t motor_current_setting[3];
#endif
static int16_t cleaning_buffer_counter;
private:
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
last_movement_extruder; // Last movement extruder, as computed when the last movement was fetched from planner
static bool abort_current_block, // Signals to the stepper that current block should be aborted
last_movement_non_null[NUM_AXIS]; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
#if ENABLED(X_DUAL_ENDSTOPS)
static bool locked_x_motor, locked_x2_motor;
@ -95,7 +92,7 @@ class Stepper {
// Counter variables for the Bresenham line tracer
static int32_t counter_X, counter_Y, counter_Z, counter_E;
static volatile uint32_t step_events_completed; // The number of step events executed in the current block
static uint32_t step_events_completed; // The number of step events executed in the current block
#if ENABLED(BEZIER_JERK_CONTROL)
static int32_t bezier_A, // A coefficient in Bézier speed curve
@ -109,12 +106,14 @@ class Stepper {
static bool bezier_2nd_half; // If Bézier curve has been initialized or not
#endif
static uint32_t nextMainISR; // time remaining for the next Step ISR
static bool all_steps_done; // all steps done
#if ENABLED(LIN_ADVANCE)
static uint32_t LA_decelerate_after; // Copy from current executed block. Needed because current_block is set to NULL "too early".
static hal_timer_t nextMainISR, nextAdvanceISR, eISR_Rate;
static uint32_t nextAdvanceISR, eISR_Rate;
static uint16_t current_adv_steps, final_adv_steps, max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
#define _NEXT_ISR(T) nextMainISR = T
static int8_t e_steps;
static bool use_advance_lead;
#if E_STEPPERS > 1
@ -123,18 +122,14 @@ class Stepper {
static constexpr int8_t LA_active_extruder = 0;
#endif
#else // !LIN_ADVANCE
#endif // LIN_ADVANCE
#define _NEXT_ISR(T) HAL_timer_set_compare(STEP_TIMER_NUM, T);
#endif // !LIN_ADVANCE
static int32_t acceleration_time, deceleration_time;
static uint32_t acceleration_time, deceleration_time;
static uint8_t step_loops, step_loops_nominal;
static hal_timer_t OCR1A_nominal;
static uint32_t ticks_nominal;
#if DISABLED(BEZIER_JERK_CONTROL)
static hal_timer_t acc_step_rate; // needed for deceleration start point
static uint32_t acc_step_rate; // needed for deceleration start point
#endif
static volatile int32_t endstops_trigsteps[XYZ];
@ -167,88 +162,53 @@ class Stepper {
//
Stepper() { };
//
// Initialize stepper hardware
//
static void init();
//
// Interrupt Service Routines
//
static void isr();
// The ISR scheduler
static hal_timer_t isr_scheduler();
// The stepper pulse phase ISR
static void stepper_pulse_phase_isr();
// The stepper block processing phase ISR
static uint32_t stepper_block_phase_isr();
#if ENABLED(LIN_ADVANCE)
static void advance_isr();
static void advance_isr_scheduler();
// The Linear advance stepper ISR
static uint32_t advance_isr();
#endif
//
// 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);
FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; }
FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
planner.synchronize();
CRITICAL_SECTION_START;
_set_position(a, b, c, e);
CRITICAL_SECTION_END;
}
static void set_position(const AxisEnum a, const int32_t &v) {
planner.synchronize();
CRITICAL_SECTION_START;
count_position[a] = v;
CRITICAL_SECTION_END;
}
FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; }
static void set_e_position(const int32_t &e) {
planner.synchronize();
CRITICAL_SECTION_START;
count_position[E_AXIS] = e;
CRITICAL_SECTION_END;
}
//
// Set direction bits for all steppers
//
static void set_directions();
//
// Get the position of a stepper, in steps
//
static int32_t position(const AxisEnum axis);
//
// Report the positions of the steppers, in steps
//
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();
//
// Wait for moves to finish and disable all steppers
//
static void finish_and_disable();
// Quickly stop all steppers
FORCE_INLINE static void quick_stop() { abort_current_block = true; }
//
// Quickly stop all steppers and clear the blocks queue
//
static void quick_stop();
//
// The direction of a single motor
//
FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
// The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same.
FORCE_INLINE static bool movement_non_null(const AxisEnum axis) { return last_movement_non_null[axis]; }
// The extruder associated to the last movement
FORCE_INLINE static uint8_t movement_extruder() { return last_movement_extruder; }
// Handle a triggered endstop
static void endstop_triggered(const AxisEnum axis);
// Triggered position of an axis in steps
static int32_t triggered_position(const AxisEnum axis);
#if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
static void digitalPotWrite(const int16_t address, const int16_t value);
static void digipot_current(const uint8_t driver, const int16_t current);
@ -280,34 +240,24 @@ class Stepper {
static void babystep(const AxisEnum axis, const bool direction); // perform a short step with a single stepper motor, outside of any convention
#endif
static inline void kill_current_block() {
step_events_completed = current_block->step_event_count;
}
//
// Handle a triggered endstop
//
static void endstop_triggered(const AxisEnum axis);
//
// Triggered position of an axis in mm (not core-savvy)
//
FORCE_INLINE static float triggered_position_mm(const AxisEnum axis) {
return endstops_trigsteps[axis] * planner.steps_to_mm[axis];
}
#if HAS_MOTOR_CURRENT_PWM
static void refresh_motor_power();
#endif
private:
FORCE_INLINE static hal_timer_t calc_timer_interval(hal_timer_t step_rate) {
hal_timer_t timer;
// 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);
NOMORE(step_rate, MAX_STEP_FREQUENCY);
// Set direction bits for all steppers
static void set_directions();
// TODO: HAL: tidy this up, use condtionals_post.h
FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate) {
uint32_t timer;
NOMORE(step_rate, uint32_t(MAX_STEP_FREQUENCY));
// TODO: HAL: tidy this up, use Conditionals_post.h
#ifdef CPU_32_BIT
#if ENABLED(DISABLE_MULTI_STEPPING)
step_loops = 1;
@ -344,20 +294,20 @@ class Stepper {
timer = uint32_t(HAL_STEPPER_TIMER_RATE) / step_rate;
NOLESS(timer, min_time_per_step); // (STEP_DOUBLER_FREQUENCY * 2 kHz - this should never happen)
#else
NOLESS(step_rate, F_CPU / 500000);
NOLESS(step_rate, uint32_t(F_CPU / 500000U));
step_rate -= F_CPU / 500000; // Correct for minimal speed
if (step_rate >= (8 * 256)) { // higher step rate
uint8_t tmp_step_rate = (step_rate & 0x00FF);
uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0];
uint16_t gain = (uint16_t)pgm_read_word_near(table_address + 2);
uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0],
gain = (uint16_t)pgm_read_word_near(table_address + 2);
timer = MultiU16X8toH16(tmp_step_rate, gain);
timer = (uint16_t)pgm_read_word_near(table_address) - timer;
}
else { // lower step rates
uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0];
table_address += ((step_rate) >> 1) & 0xFFFC;
timer = (uint16_t)pgm_read_word_near(table_address);
timer -= (((uint16_t)pgm_read_word_near(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3);
timer = (uint16_t)pgm_read_word_near(table_address)
- (((uint16_t)pgm_read_word_near(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3);
}
if (timer < 100) { // (20kHz - this should never happen)
timer = 100;

24
Marlin/src/module/temperature.cpp
View file

@ -25,6 +25,7 @@
*/
#include "temperature.h"
#include "endstops.h"
#include "../Marlin.h"
#include "../lcd/ultralcd.h"
@ -40,10 +41,6 @@
#include "stepper.h"
#endif
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) || ENABLED(PINS_DEBUGGING)
#include "endstops.h"
#endif
#include "printcounter.h"
#if ENABLED(FILAMENT_WIDTH_SENSOR)
@ -1085,9 +1082,7 @@ void Temperature::updateTemperaturesFromRawValues() {
watchdog_reset();
#endif
CRITICAL_SECTION_START;
temp_meas_ready = false;
CRITICAL_SECTION_END;
}
@ -1727,6 +1722,7 @@ void Temperature::set_current_temp_raw() {
* - Step the babysteps value for each axis towards 0
* - For PINS_DEBUGGING, monitor and report endstop pins
* - For ENDSTOP_INTERRUPTS_FEATURE check endstops if flagged
* - Call planner.tick to count down its "ignore" time
*/
HAL_TEMP_TIMER_ISR {
HAL_timer_isr_prologue(TEMP_TIMER_NUM);
@ -2247,19 +2243,11 @@ void Temperature::isr() {
}
#endif // BABYSTEPPING
#if ENABLED(PINS_DEBUGGING)
endstops.run_monitor(); // report changes in endstop status
#endif
// Poll endstops state, if required
endstops.poll();
#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
extern volatile uint8_t e_hit;
if (e_hit && ENDSTOPS_ENABLED) {
endstops.update(); // call endstop update routine
e_hit--;
}
#endif
// Periodically call the planner timer
planner.tick();
}
#if HAS_TEMP_SENSOR

3
Marlin/src/sd/cardreader.cpp
View file

@ -29,7 +29,6 @@
#include "../Marlin.h"
#include "../lcd/ultralcd.h"
#include "../module/planner.h"
#include "../module/stepper.h"
#include "../module/printcounter.h"
#include "../core/language.h"
#include "../gcode/queue.h"
@ -983,7 +982,7 @@ void CardReader::printingHasFinished() {
#endif
#if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
stepper.cleaning_buffer_counter = 1; // The command will fire from the Stepper ISR
planner.finish_and_disable();
#endif
print_job_timer.stop();
if (print_job_timer.duration() > 60)