From ddc1a4884493e4f9d7b991521f120eb571808e72 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 20 May 2018 08:36:56 -0500 Subject: [PATCH 1/6] Adjust formatting in stepper.cpp --- Marlin/stepper.cpp | 496 +++++++++++++++++++++++---------------------- 1 file changed, 249 insertions(+), 247 deletions(-) diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 9d7cb4df1..479166140 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -50,6 +50,8 @@ * Jerk controlled movements planner added Apr 2018 by Eduardo José Tagle. * Equations based on Synthethos TinyG2 sources, but the fixed-point * implementation is new, as we are running the ISR with a variable period. + * Also implemented the Bézier velocity curve evaluation in ARM assembler, + * to avoid impacting ISR speed. */ #include "Marlin.h" @@ -426,7 +428,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 @@ -632,65 +634,65 @@ void Stepper::set_directions() { /* Store initial velocity*/ A("sts bezier_F, %0") A("sts bezier_F+1, %1") - A("sts bezier_F+2, %10") /* bezier_F = %10:%1:%0 = v0 */ + A("sts bezier_F+2, %10") /* bezier_F = %10:%1:%0 = v0 */ /* Get delta speed */ - A("ldi %2,-1") /* %2 = 0xFF, means A_negative = true */ - A("clr %8") /* %8 = 0 */ + A("ldi %2,-1") /* %2 = 0xFF, means A_negative = true */ + A("clr %8") /* %8 = 0 */ A("sub %0,%3") A("sbc %1,%4") - A("sbc %10,%5") /* v0 -= v1, C=1 if result is negative */ - A("brcc 1f") /* branch if result is positive (C=0), that means v0 >= v1 */ + A("sbc %10,%5") /* v0 -= v1, C=1 if result is negative */ + A("brcc 1f") /* branch if result is positive (C=0), that means v0 >= v1 */ /* Result was negative, get the absolute value*/ A("com %10") A("com %1") A("neg %0") A("sbc %1,%2") - A("sbc %10,%2") /* %10:%1:%0 +1 -> %10:%1:%0 = -(v0 - v1) = (v1 - v0) */ - A("clr %2") /* %2 = 0, means A_negative = false */ + A("sbc %10,%2") /* %10:%1:%0 +1 -> %10:%1:%0 = -(v0 - v1) = (v1 - v0) */ + A("clr %2") /* %2 = 0, means A_negative = false */ /* Store negative flag*/ L("1") - A("sts A_negative, %2") /* Store negative flag */ + A("sts A_negative, %2") /* Store negative flag */ /* Compute coefficients A,B and C [20 cycles worst case]*/ - A("ldi %9,6") /* %9 = 6 */ - A("mul %0,%9") /* r1:r0 = 6*LO(v0-v1) */ + A("ldi %9,6") /* %9 = 6 */ + A("mul %0,%9") /* r1:r0 = 6*LO(v0-v1) */ A("sts bezier_A, r0") A("mov %6,r1") - A("clr %7") /* %7:%6:r0 = 6*LO(v0-v1) */ - A("mul %1,%9") /* r1:r0 = 6*MI(v0-v1) */ + A("clr %7") /* %7:%6:r0 = 6*LO(v0-v1) */ + A("mul %1,%9") /* r1:r0 = 6*MI(v0-v1) */ A("add %6,r0") - A("adc %7,r1") /* %7:%6:?? += 6*MI(v0-v1) << 8 */ - A("mul %10,%9") /* r1:r0 = 6*HI(v0-v1) */ - A("add %7,r0") /* %7:%6:?? += 6*HI(v0-v1) << 16 */ + A("adc %7,r1") /* %7:%6:?? += 6*MI(v0-v1) << 8 */ + A("mul %10,%9") /* r1:r0 = 6*HI(v0-v1) */ + A("add %7,r0") /* %7:%6:?? += 6*HI(v0-v1) << 16 */ A("sts bezier_A+1, %6") - A("sts bezier_A+2, %7") /* bezier_A = %7:%6:?? = 6*(v0-v1) [35 cycles worst] */ + A("sts bezier_A+2, %7") /* bezier_A = %7:%6:?? = 6*(v0-v1) [35 cycles worst] */ - A("ldi %9,15") /* %9 = 15 */ - A("mul %0,%9") /* r1:r0 = 5*LO(v0-v1) */ + A("ldi %9,15") /* %9 = 15 */ + A("mul %0,%9") /* r1:r0 = 5*LO(v0-v1) */ A("sts bezier_B, r0") A("mov %6,r1") - A("clr %7") /* %7:%6:?? = 5*LO(v0-v1) */ - A("mul %1,%9") /* r1:r0 = 5*MI(v0-v1) */ + A("clr %7") /* %7:%6:?? = 5*LO(v0-v1) */ + A("mul %1,%9") /* r1:r0 = 5*MI(v0-v1) */ A("add %6,r0") - A("adc %7,r1") /* %7:%6:?? += 5*MI(v0-v1) << 8 */ - A("mul %10,%9") /* r1:r0 = 5*HI(v0-v1) */ - A("add %7,r0") /* %7:%6:?? += 5*HI(v0-v1) << 16 */ + A("adc %7,r1") /* %7:%6:?? += 5*MI(v0-v1) << 8 */ + A("mul %10,%9") /* r1:r0 = 5*HI(v0-v1) */ + A("add %7,r0") /* %7:%6:?? += 5*HI(v0-v1) << 16 */ A("sts bezier_B+1, %6") - A("sts bezier_B+2, %7") /* bezier_B = %7:%6:?? = 5*(v0-v1) [50 cycles worst] */ + A("sts bezier_B+2, %7") /* bezier_B = %7:%6:?? = 5*(v0-v1) [50 cycles worst] */ - A("ldi %9,10") /* %9 = 10 */ - A("mul %0,%9") /* r1:r0 = 10*LO(v0-v1) */ + A("ldi %9,10") /* %9 = 10 */ + A("mul %0,%9") /* r1:r0 = 10*LO(v0-v1) */ A("sts bezier_C, r0") A("mov %6,r1") - A("clr %7") /* %7:%6:?? = 10*LO(v0-v1) */ - A("mul %1,%9") /* r1:r0 = 10*MI(v0-v1) */ + A("clr %7") /* %7:%6:?? = 10*LO(v0-v1) */ + A("mul %1,%9") /* r1:r0 = 10*MI(v0-v1) */ A("add %6,r0") - A("adc %7,r1") /* %7:%6:?? += 10*MI(v0-v1) << 8 */ - A("mul %10,%9") /* r1:r0 = 10*HI(v0-v1) */ - A("add %7,r0") /* %7:%6:?? += 10*HI(v0-v1) << 16 */ + A("adc %7,r1") /* %7:%6:?? += 10*MI(v0-v1) << 8 */ + A("mul %10,%9") /* r1:r0 = 10*HI(v0-v1) */ + A("add %7,r0") /* %7:%6:?? += 10*HI(v0-v1) << 16 */ A("sts bezier_C+1, %6") " sts bezier_C+2, %7" /* bezier_C = %7:%6:?? = 10*(v0-v1) [65 cycles worst] */ : "+r" (r2), @@ -723,357 +725,357 @@ void Stepper::set_directions() { __asm__ __volatile( /* umul24x24to16hi(t, bezier_AV, curr_step); t: Range 0 - 1^16 = 16 bits*/ - A("lds %9,bezier_AV") /* %9 = LO(AV)*/ - A("mul %9,%2") /* r1:r0 = LO(bezier_AV)*LO(curr_step)*/ - A("mov %7,r1") /* %7 = LO(bezier_AV)*LO(curr_step) >> 8*/ - A("clr %8") /* %8:%7 = LO(bezier_AV)*LO(curr_step) >> 8*/ - A("lds %10,bezier_AV+1") /* %10 = MI(AV)*/ - A("mul %10,%2") /* r1:r0 = MI(bezier_AV)*LO(curr_step)*/ + A("lds %9,bezier_AV") /* %9 = LO(AV)*/ + A("mul %9,%2") /* r1:r0 = LO(bezier_AV)*LO(curr_step)*/ + A("mov %7,r1") /* %7 = LO(bezier_AV)*LO(curr_step) >> 8*/ + A("clr %8") /* %8:%7 = LO(bezier_AV)*LO(curr_step) >> 8*/ + A("lds %10,bezier_AV+1") /* %10 = MI(AV)*/ + A("mul %10,%2") /* r1:r0 = MI(bezier_AV)*LO(curr_step)*/ A("add %7,r0") - A("adc %8,r1") /* %8:%7 += MI(bezier_AV)*LO(curr_step)*/ - A("lds r1,bezier_AV+2") /* r11 = HI(AV)*/ - A("mul r1,%2") /* r1:r0 = HI(bezier_AV)*LO(curr_step)*/ - A("add %8,r0") /* %8:%7 += HI(bezier_AV)*LO(curr_step) << 8*/ - A("mul %9,%3") /* r1:r0 = LO(bezier_AV)*MI(curr_step)*/ + A("adc %8,r1") /* %8:%7 += MI(bezier_AV)*LO(curr_step)*/ + A("lds r1,bezier_AV+2") /* r11 = HI(AV)*/ + A("mul r1,%2") /* r1:r0 = HI(bezier_AV)*LO(curr_step)*/ + A("add %8,r0") /* %8:%7 += HI(bezier_AV)*LO(curr_step) << 8*/ + A("mul %9,%3") /* r1:r0 = LO(bezier_AV)*MI(curr_step)*/ A("add %7,r0") - A("adc %8,r1") /* %8:%7 += LO(bezier_AV)*MI(curr_step)*/ - A("mul %10,%3") /* r1:r0 = MI(bezier_AV)*MI(curr_step)*/ - A("add %8,r0") /* %8:%7 += LO(bezier_AV)*MI(curr_step) << 8*/ - A("mul %9,%4") /* r1:r0 = LO(bezier_AV)*HI(curr_step)*/ - A("add %8,r0") /* %8:%7 += LO(bezier_AV)*HI(curr_step) << 8*/ + A("adc %8,r1") /* %8:%7 += LO(bezier_AV)*MI(curr_step)*/ + A("mul %10,%3") /* r1:r0 = MI(bezier_AV)*MI(curr_step)*/ + A("add %8,r0") /* %8:%7 += LO(bezier_AV)*MI(curr_step) << 8*/ + A("mul %9,%4") /* r1:r0 = LO(bezier_AV)*HI(curr_step)*/ + A("add %8,r0") /* %8:%7 += LO(bezier_AV)*HI(curr_step) << 8*/ /* %8:%7 = t*/ /* uint16_t f = t;*/ - A("mov %5,%7") /* %6:%5 = f*/ + A("mov %5,%7") /* %6:%5 = f*/ A("mov %6,%8") /* %6:%5 = f*/ /* umul16x16to16hi(f, f, t); / Range 16 bits (unsigned) [17] */ - A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ - A("mov %9,r1") /* store MIL(LO(f) * LO(t)) in %9, we need it for rounding*/ - A("clr %10") /* %10 = 0*/ - A("clr %11") /* %11 = 0*/ - A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ - A("add %9,r0") /* %9 += LO(LO(f) * HI(t))*/ - A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ - A("add %9,r0") /* %9 += LO(HI(f) * LO(t))*/ - A("adc %10,r1") /* %10 += HI(HI(f) * LO(t)) */ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ - A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ - A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ - A("mov %5,%10") /* %6:%5 = */ - A("mov %6,%11") /* f = %10:%11*/ + A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ + A("mov %9,r1") /* store MIL(LO(f) * LO(t)) in %9, we need it for rounding*/ + A("clr %10") /* %10 = 0*/ + A("clr %11") /* %11 = 0*/ + A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ + A("add %9,r0") /* %9 += LO(LO(f) * HI(t))*/ + A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ + A("add %9,r0") /* %9 += LO(HI(f) * LO(t))*/ + A("adc %10,r1") /* %10 += HI(HI(f) * LO(t)) */ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ + A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ + A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ + A("mov %5,%10") /* %6:%5 = */ + A("mov %6,%11") /* f = %10:%11*/ /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/ - A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ - A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ - A("clr %10") /* %10 = 0*/ - A("clr %11") /* %11 = 0*/ - A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ - A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ - A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ - A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ - A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ - A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ - A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ - A("mov %5,%10") /* %6:%5 =*/ - A("mov %6,%11") /* f = %10:%11*/ + A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ + A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ + A("clr %10") /* %10 = 0*/ + A("clr %11") /* %11 = 0*/ + A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ + A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ + A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ + A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ + A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ + A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ + A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ + A("mov %5,%10") /* %6:%5 =*/ + A("mov %6,%11") /* f = %10:%11*/ /* [15 +17*2] = [49]*/ /* %4:%3:%2 will be acc from now on*/ /* uint24_t acc = bezier_F; / Range 20 bits (unsigned)*/ - A("clr %9") /* "decimal place we get for free"*/ + A("clr %9") /* "decimal place we get for free"*/ A("lds %2,bezier_F") A("lds %3,bezier_F+1") - A("lds %4,bezier_F+2") /* %4:%3:%2 = acc*/ + A("lds %4,bezier_F+2") /* %4:%3:%2 = acc*/ /* if (A_negative) {*/ A("lds r0,A_negative") - A("or r0,%0") /* Is flag signalling negative? */ - A("brne 3f") /* If yes, Skip next instruction if A was negative*/ - A("rjmp 1f") /* Otherwise, jump */ + A("or r0,%0") /* Is flag signalling negative? */ + A("brne 3f") /* If yes, Skip next instruction if A was negative*/ + A("rjmp 1f") /* Otherwise, jump */ /* uint24_t v; */ /* umul16x24to24hi(v, f, bezier_C); / Range 21bits [29] */ /* acc -= v; */ L("3") - A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/ - A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/ + A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/ + A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/ A("sub %9,r1") A("sbc %2,%0") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_C) * LO(f))*/ - A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/ - A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_C) * LO(f))*/ + A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/ + A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/ A("sub %9,r0") A("sbc %2,r1") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * LO(f)*/ - A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/ - A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * LO(f)*/ + A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/ + A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/ A("sub %2,r0") A("sbc %3,r1") - A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 8*/ - A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 8*/ + A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/ A("sub %9,r0") A("sbc %2,r1") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_C) * MI(f)*/ - A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_C) * MI(f)*/ + A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/ A("sub %2,r0") A("sbc %3,r1") - A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * MI(f) << 8*/ - A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * MI(f) << 8*/ + A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/ A("sub %3,r0") - A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 16*/ + A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 16*/ /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/ - A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ - A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ - A("clr %10") /* %10 = 0*/ - A("clr %11") /* %11 = 0*/ - A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ - A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ - A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ - A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ - A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ - A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ - A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ - A("mov %5,%10") /* %6:%5 =*/ - A("mov %6,%11") /* f = %10:%11*/ + A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ + A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ + A("clr %10") /* %10 = 0*/ + A("clr %11") /* %11 = 0*/ + A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ + A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ + A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ + A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ + A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ + A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ + A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ + A("mov %5,%10") /* %6:%5 =*/ + A("mov %6,%11") /* f = %10:%11*/ /* umul16x24to24hi(v, f, bezier_B); / Range 22bits [29]*/ /* acc += v; */ - A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/ - A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/ + A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/ + A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/ A("add %9,r1") A("adc %2,%0") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_B) * LO(f))*/ - A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/ - A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_B) * LO(f))*/ + A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/ + A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/ A("add %9,r0") A("adc %2,r1") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * LO(f)*/ - A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/ - A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * LO(f)*/ + A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/ + A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/ A("add %2,r0") A("adc %3,r1") - A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 8*/ - A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 8*/ + A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/ A("add %9,r0") A("adc %2,r1") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_B) * MI(f)*/ - A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_B) * MI(f)*/ + A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/ A("add %2,r0") A("adc %3,r1") - A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * MI(f) << 8*/ - A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * MI(f) << 8*/ + A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/ A("add %3,r0") - A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 16*/ + A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 16*/ /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^5 (unsigned) [17]*/ - A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ - A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ - A("clr %10") /* %10 = 0*/ - A("clr %11") /* %11 = 0*/ - A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ - A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ - A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ - A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ - A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ - A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ - A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ - A("mov %5,%10") /* %6:%5 =*/ - A("mov %6,%11") /* f = %10:%11*/ + A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ + A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ + A("clr %10") /* %10 = 0*/ + A("clr %11") /* %11 = 0*/ + A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ + A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ + A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ + A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ + A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ + A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ + A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ + A("mov %5,%10") /* %6:%5 =*/ + A("mov %6,%11") /* f = %10:%11*/ /* umul16x24to24hi(v, f, bezier_A); / Range 21bits [29]*/ /* acc -= v; */ - A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/ - A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/ + A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/ + A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/ A("sub %9,r1") A("sbc %2,%0") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_A) * LO(f))*/ - A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/ - A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_A) * LO(f))*/ + A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/ + A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/ A("sub %9,r0") A("sbc %2,r1") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * LO(f)*/ - A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/ - A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * LO(f)*/ + A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/ + A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/ A("sub %2,r0") A("sbc %3,r1") - A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 8*/ - A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 8*/ + A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/ A("sub %9,r0") A("sbc %2,r1") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_A) * MI(f)*/ - A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_A) * MI(f)*/ + A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/ A("sub %2,r0") A("sbc %3,r1") - A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * MI(f) << 8*/ - A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * MI(f) << 8*/ + A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/ A("sub %3,r0") - A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 16*/ - A("jmp 2f") /* Done!*/ + A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 16*/ + A("jmp 2f") /* Done!*/ L("1") /* uint24_t v; */ /* umul16x24to24hi(v, f, bezier_C); / Range 21bits [29]*/ /* acc += v; */ - A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/ - A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/ + A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/ + A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/ A("add %9,r1") A("adc %2,%0") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_C) * LO(f))*/ - A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/ - A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_C) * LO(f))*/ + A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/ + A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/ A("add %9,r0") A("adc %2,r1") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * LO(f)*/ - A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/ - A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * LO(f)*/ + A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/ + A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/ A("add %2,r0") A("adc %3,r1") - A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 8*/ - A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 8*/ + A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/ A("add %9,r0") A("adc %2,r1") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_C) * MI(f)*/ - A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_C) * MI(f)*/ + A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/ A("add %2,r0") A("adc %3,r1") - A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * MI(f) << 8*/ - A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * MI(f) << 8*/ + A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/ A("add %3,r0") - A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 16*/ + A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 16*/ /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/ - A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ - A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ - A("clr %10") /* %10 = 0*/ - A("clr %11") /* %11 = 0*/ - A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ - A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ - A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ - A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ - A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ - A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ - A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ - A("mov %5,%10") /* %6:%5 =*/ - A("mov %6,%11") /* f = %10:%11*/ + A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ + A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ + A("clr %10") /* %10 = 0*/ + A("clr %11") /* %11 = 0*/ + A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ + A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ + A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ + A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ + A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ + A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ + A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ + A("mov %5,%10") /* %6:%5 =*/ + A("mov %6,%11") /* f = %10:%11*/ /* umul16x24to24hi(v, f, bezier_B); / Range 22bits [29]*/ /* acc -= v;*/ - A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/ - A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/ + A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/ + A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/ A("sub %9,r1") A("sbc %2,%0") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_B) * LO(f))*/ - A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/ - A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_B) * LO(f))*/ + A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/ + A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/ A("sub %9,r0") A("sbc %2,r1") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * LO(f)*/ - A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/ - A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * LO(f)*/ + A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/ + A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/ A("sub %2,r0") A("sbc %3,r1") - A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 8*/ - A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 8*/ + A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/ A("sub %9,r0") A("sbc %2,r1") A("sbc %3,%0") - A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_B) * MI(f)*/ - A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_B) * MI(f)*/ + A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/ A("sub %2,r0") A("sbc %3,r1") - A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * MI(f) << 8*/ - A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/ + A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * MI(f) << 8*/ + A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/ A("sub %3,r0") - A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 16*/ + A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 16*/ /* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^5 (unsigned) [17]*/ - A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ - A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ - A("clr %10") /* %10 = 0*/ - A("clr %11") /* %11 = 0*/ - A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ - A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ - A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ - A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ - A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ - A("adc %11,%0") /* %11 += carry*/ - A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ - A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ - A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ - A("mov %5,%10") /* %6:%5 =*/ - A("mov %6,%11") /* f = %10:%11*/ + A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/ + A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/ + A("clr %10") /* %10 = 0*/ + A("clr %11") /* %11 = 0*/ + A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/ + A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/ + A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/ + A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/ + A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/ + A("adc %11,%0") /* %11 += carry*/ + A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/ + A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/ + A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/ + A("mov %5,%10") /* %6:%5 =*/ + A("mov %6,%11") /* f = %10:%11*/ /* umul16x24to24hi(v, f, bezier_A); / Range 21bits [29]*/ /* acc += v; */ - A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/ - A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/ + A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/ + A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/ A("add %9,r1") A("adc %2,%0") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_A) * LO(f))*/ - A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/ - A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_A) * LO(f))*/ + A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/ + A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/ A("add %9,r0") A("adc %2,r1") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * LO(f)*/ - A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/ - A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * LO(f)*/ + A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/ + A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/ A("add %2,r0") A("adc %3,r1") - A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 8*/ - A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 8*/ + A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/ A("add %9,r0") A("adc %2,r1") A("adc %3,%0") - A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_A) * MI(f)*/ - A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_A) * MI(f)*/ + A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/ A("add %2,r0") A("adc %3,r1") - A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * MI(f) << 8*/ - A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/ + A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * MI(f) << 8*/ + A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/ A("add %3,r0") - A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 16*/ + A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 16*/ L("2") " clr __zero_reg__" /* C runtime expects r1 = __zero_reg__ = 0 */ : "+r"(r0), From 38e1823375b0bdcd6e703e9b39e4268896c406d9 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 20 May 2018 08:05:47 -0500 Subject: [PATCH 2/6] Add HAL.h for AVR for easier 2.0.x parity --- Marlin/HAL.h | 305 +++++++++++++++++++++++++++++++++++++++++ Marlin/MarlinConfig.h | 16 ++- Marlin/endstops.h | 1 - Marlin/fastio.h | 1 - Marlin/macros.h | 6 - Marlin/stepper.cpp | 27 ++-- Marlin/stepper.h | 5 - Marlin/temperature.cpp | 98 +++++-------- 8 files changed, 365 insertions(+), 94 deletions(-) create mode 100644 Marlin/HAL.h diff --git a/Marlin/HAL.h b/Marlin/HAL.h new file mode 100644 index 000000000..e4c2f805a --- /dev/null +++ b/Marlin/HAL.h @@ -0,0 +1,305 @@ +/* ************************************************************************** + + Marlin 3D Printer Firmware + Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + + Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +****************************************************************************/ + +/** + * Description: HAL for __AVR__ + */ + +#ifndef _HAL_AVR_H_ +#define _HAL_AVR_H_ + +// -------------------------------------------------------------------------- +// Includes +// -------------------------------------------------------------------------- + +#include "fastio.h" + +#include +#include +#include +#include +#include +#include +#include + +// -------------------------------------------------------------------------- +// Defines +// -------------------------------------------------------------------------- + +//#define analogInputToDigitalPin(IO) IO + +// Bracket code that shouldn't be interrupted +#ifndef CRITICAL_SECTION_START + #define CRITICAL_SECTION_START unsigned char _sreg = SREG; cli(); + #define CRITICAL_SECTION_END SREG = _sreg; +#endif + +// -------------------------------------------------------------------------- +// Types +// -------------------------------------------------------------------------- + +typedef uint16_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFF + +typedef int8_t pin_t; + +#define HAL_SERVO_LIB Servo + +// -------------------------------------------------------------------------- +// Public Variables +// -------------------------------------------------------------------------- + +//extern uint8_t MCUSR; + +// -------------------------------------------------------------------------- +// Public functions +// -------------------------------------------------------------------------- + +//void cli(void); + +//void _delay_ms(const int delay); + +inline void HAL_clear_reset_source(void) { MCUSR = 0; } +inline uint8_t HAL_get_reset_source(void) { return MCUSR; } + +// eeprom +//void eeprom_write_byte(unsigned char *pos, unsigned char value); +//unsigned char eeprom_read_byte(unsigned char *pos); + +// timers +#define HAL_TIMER_RATE ((F_CPU) / 8) // i.e., 2MHz or 2.5MHz + +#define STEP_TIMER_NUM 1 +#define TEMP_TIMER_NUM 0 +#define PULSE_TIMER_NUM TEMP_TIMER_NUM + +#define HAL_STEPPER_TIMER_RATE HAL_TIMER_RATE +#define HAL_TICKS_PER_US ((HAL_STEPPER_TIMER_RATE) / 1000000) // Cannot be of type double +#define STEPPER_TIMER_PRESCALE 8 +#define STEP_TIMER_MIN_INTERVAL 8 // minimum time in µs between stepper interrupts + +#define TEMP_TIMER_FREQUENCY ((F_CPU) / 64.0 / 256.0) + +#define TIMER_OCR_1 OCR1A +#define TIMER_COUNTER_1 TCNT1 + +#define TIMER_OCR_0 OCR0A +#define TIMER_COUNTER_0 TCNT0 + +#define PULSE_TIMER_PRESCALE 8 + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() SBI(TIMSK1, OCIE1A) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A) +#define STEPPER_ISR_ENABLED() TEST(TIMSK1, OCIE1A) + +#define ENABLE_TEMPERATURE_INTERRUPT() SBI(TIMSK0, OCIE0B) +#define DISABLE_TEMPERATURE_INTERRUPT() CBI(TIMSK0, OCIE0B) +#define TEMPERATURE_ISR_ENABLED() TEST(TIMSK0, OCIE0B) + +#define HAL_timer_start(timer_num, frequency) + +#define _CAT(a, ...) a ## __VA_ARGS__ +#define HAL_timer_set_compare(timer, compare) (_CAT(TIMER_OCR_, timer) = compare) +#define HAL_timer_restrain(timer, interval_ticks) NOLESS(_CAT(TIMER_OCR_, timer), _CAT(TIMER_COUNTER_, timer) + interval_ticks) + +#define HAL_timer_get_compare(timer) _CAT(TIMER_OCR_, timer) +#define HAL_timer_get_count(timer) _CAT(TIMER_COUNTER_, timer) + +/** + * On AVR there is no hardware prioritization and preemption of + * interrupts, so this emulates it. The UART has first priority + * (otherwise, characters will be lost due to UART overflow). + * Then: Stepper, Endstops, Temperature, and -finally- all others. + */ +#define HAL_timer_isr_prologue(TIMER_NUM) +#define HAL_timer_isr_epilogue(TIMER_NUM) + +/* 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< 7) ADCSRB = _BV(MUX5); else ADCSRB = 0; SET_ADMUX_ADCSRA(pin) +#else + #define HAL_START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin) +#endif + +#define HAL_READ_ADC ADC + +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +#define HAL_SENSITIVE_PINS 0, 1 + +#endif // _HAL_AVR_H_ diff --git a/Marlin/MarlinConfig.h b/Marlin/MarlinConfig.h index 5f77dba0e..47e6020d9 100644 --- a/Marlin/MarlinConfig.h +++ b/Marlin/MarlinConfig.h @@ -23,21 +23,25 @@ #ifndef MARLIN_CONFIG_H #define MARLIN_CONFIG_H -#include "fastio.h" -#include "macros.h" #include "boards.h" +#include "macros.h" #include "Version.h" #include "Configuration.h" #include "Conditionals_LCD.h" #include "Configuration_adv.h" -#include "pins.h" + #if defined(__AVR__) && !defined(USBCON) #define HardwareSerial_h // trick to disable the standard HWserial #endif -#include "Arduino.h" + +#include "types.h" +#include "HAL.h" +#include "pins.h" #include "Conditionals_post.h" #include "SanityCheck.h" - -#include +#include "enum.h" +#include "language.h" +#include "utility.h" +#include "serial.h" #endif // MARLIN_CONFIG_H diff --git a/Marlin/endstops.h b/Marlin/endstops.h index 96cb3d089..fdaf02ea7 100644 --- a/Marlin/endstops.h +++ b/Marlin/endstops.h @@ -27,7 +27,6 @@ #ifndef __ENDSTOPS_H__ #define __ENDSTOPS_H__ -#include "enum.h" #include "MarlinConfig.h" class Endstops { diff --git a/Marlin/fastio.h b/Marlin/fastio.h index 03a3a9036..839db9929 100644 --- a/Marlin/fastio.h +++ b/Marlin/fastio.h @@ -28,7 +28,6 @@ #include -typedef int8_t pin_t; #ifndef _FASTIO_ARDUINO_H_ #define _FASTIO_ARDUINO_H_ diff --git a/Marlin/macros.h b/Marlin/macros.h index 6b8e87812..1f72ba38d 100644 --- a/Marlin/macros.h +++ b/Marlin/macros.h @@ -47,12 +47,6 @@ #define _O2 __attribute__((optimize("O2"))) #define _O3 __attribute__((optimize("O3"))) -// Bracket code that shouldn't be interrupted -#ifndef CRITICAL_SECTION_START - #define CRITICAL_SECTION_START unsigned char _sreg = SREG; cli(); - #define CRITICAL_SECTION_END SREG = _sreg; -#endif - // Clock speed factors #define CYCLES_PER_MICROSECOND (F_CPU / 1000000L) // 16 or 20 #define INT0_PRESCALER 8 diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 479166140..1a2eefe3b 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -1394,7 +1394,7 @@ void Stepper::isr() { * 10µs = 160 or 200 cycles. */ #if EXTRA_CYCLES_XYZE > 20 - uint32_t pulse_start = TCNT0; + hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM); #endif #if HAS_X_STEP @@ -1459,8 +1459,8 @@ void Stepper::isr() { // For minimum pulse time wait before stopping pulses #if EXTRA_CYCLES_XYZE > 20 - while (EXTRA_CYCLES_XYZE > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ } - pulse_start = TCNT0; + while (EXTRA_CYCLES_XYZE > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ } + pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM); #elif EXTRA_CYCLES_XYZE > 0 DELAY_NS(EXTRA_CYCLES_XYZE * NANOSECONDS_PER_CYCLE); #endif @@ -1495,7 +1495,7 @@ void Stepper::isr() { // For minimum pulse time wait after stopping pulses also #if EXTRA_CYCLES_XYZE > 20 - if (i) while (EXTRA_CYCLES_XYZE > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ } + if (i) while (EXTRA_CYCLES_XYZE > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ } #elif EXTRA_CYCLES_XYZE > 0 if (i) DELAY_NS(EXTRA_CYCLES_XYZE * NANOSECONDS_PER_CYCLE); #endif @@ -1736,8 +1736,8 @@ void Stepper::isr() { // For minimum pulse time wait before stopping pulses #if EXTRA_CYCLES_E > 20 - while (EXTRA_CYCLES_E > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ } - pulse_start = TCNT0; + while (EXTRA_CYCLES_E > (hal_timer_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ } + pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM); #elif EXTRA_CYCLES_E > 0 DELAY_NS(EXTRA_CYCLES_E * NANOSECONDS_PER_CYCLE); #endif @@ -1760,7 +1760,7 @@ void Stepper::isr() { // For minimum pulse time wait before looping #if EXTRA_CYCLES_E > 20 - if (e_steps) while (EXTRA_CYCLES_E > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ } + if (e_steps) while (EXTRA_CYCLES_E > (hal_timer_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ } #elif EXTRA_CYCLES_E > 0 if (e_steps) DELAY_NS(EXTRA_CYCLES_E * NANOSECONDS_PER_CYCLE); #endif @@ -2056,11 +2056,16 @@ void Stepper::endstop_triggered(const AxisEnum axis) { } 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); @@ -2101,8 +2106,8 @@ void Stepper::report_positions() { #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true) #if EXTRA_CYCLES_BABYSTEP > 20 - #define _SAVE_START const uint32_t pulse_start = TCNT0 - #define _PULSE_WAIT while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ } + #define _SAVE_START const hal_timer_t pulse_start = HAL_timer_get_count(STEP_TIMER_NUM) + #define _PULSE_WAIT while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(HAL_timer_get_count(STEP_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ } #else #define _SAVE_START NOOP #if EXTRA_CYCLES_BABYSTEP > 0 diff --git a/Marlin/stepper.h b/Marlin/stepper.h index 5dec78390..198da72dd 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -52,11 +52,6 @@ class Stepper; extern Stepper stepper; -#define ENABLE_STEPPER_DRIVER_INTERRUPT() SBI(TIMSK1, OCIE1A) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A) -#define STEPPER_ISR_ENABLED() TEST(TIMSK1, OCIE1A) -#define HAL_STEPPER_TIMER_RATE ((F_CPU) * 0.125) - // intRes = intIn1 * intIn2 >> 16 // uses: // r26 to store 0 diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp index e94c6b03b..ca344aab1 100644 --- a/Marlin/temperature.cpp +++ b/Marlin/temperature.cpp @@ -1066,9 +1066,7 @@ void Temperature::updateTemperaturesFromRawValues() { watchdog_reset(); #endif - CRITICAL_SECTION_START; temp_meas_ready = false; - CRITICAL_SECTION_END; } @@ -1179,43 +1177,38 @@ void Temperature::init() { #endif // HEATER_0_USES_MAX6675 - #ifdef DIDR2 - #define ANALOG_SELECT(pin) do{ if (pin < 8) SBI(DIDR0, pin); else SBI(DIDR2, pin & 0x07); }while(0) - #else - #define ANALOG_SELECT(pin) do{ SBI(DIDR0, pin); }while(0) - #endif + HAL_adc_init(); - // Set analog inputs - ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF) | 0x07; - DIDR0 = 0; - #ifdef DIDR2 - DIDR2 = 0; - #endif #if HAS_TEMP_ADC_0 - ANALOG_SELECT(TEMP_0_PIN); + HAL_ANALOG_SELECT(TEMP_0_PIN); #endif #if HAS_TEMP_ADC_1 - ANALOG_SELECT(TEMP_1_PIN); + HAL_ANALOG_SELECT(TEMP_1_PIN); #endif #if HAS_TEMP_ADC_2 - ANALOG_SELECT(TEMP_2_PIN); + HAL_ANALOG_SELECT(TEMP_2_PIN); #endif #if HAS_TEMP_ADC_3 - ANALOG_SELECT(TEMP_3_PIN); + HAL_ANALOG_SELECT(TEMP_3_PIN); #endif #if HAS_TEMP_ADC_4 - ANALOG_SELECT(TEMP_4_PIN); + HAL_ANALOG_SELECT(TEMP_4_PIN); #endif #if HAS_HEATED_BED - ANALOG_SELECT(TEMP_BED_PIN); + HAL_ANALOG_SELECT(TEMP_BED_PIN); #endif #if HAS_TEMP_CHAMBER - ANALOG_SELECT(TEMP_CHAMBER_PIN); + HAL_ANALOG_SELECT(TEMP_CHAMBER_PIN); #endif #if ENABLED(FILAMENT_WIDTH_SENSOR) - ANALOG_SELECT(FILWIDTH_PIN); + HAL_ANALOG_SELECT(FILWIDTH_PIN); #endif + // Use timer0 for temperature measurement + // Interleave temperature interrupt with millies interrupt + OCR0B = 128; + ENABLE_TEMPERATURE_INTERRUPT(); + #if HAS_AUTO_FAN_0 #if E0_AUTO_FAN_PIN == FAN1_PIN SET_OUTPUT(E0_AUTO_FAN_PIN); @@ -1277,11 +1270,6 @@ void Temperature::init() { #endif #endif - // Use timer0 for temperature measurement - // Interleave temperature interrupt with millies interrupt - OCR0B = 128; - ENABLE_TEMPERATURE_INTERRUPT(); - // Wait for temperature measurement to settle delay(250); @@ -1793,23 +1781,12 @@ void Temperature::set_current_temp_raw() { * - For PINS_DEBUGGING, monitor and report endstop pins * - For ENDSTOP_INTERRUPTS_FEATURE check endstops if flagged */ -ISR(TIMER0_COMPB_vect) { - /** - * AVR has no hardware interrupt preemption, so emulate priorization - * and preemption of this ISR by all others by disabling the timer - * interrupt generation capability and reenabling global interrupts. - * Any interrupt can then interrupt this handler and preempt it. - * This ISR becomes the lowest priority one so the UART, Endstops - * and Stepper ISRs can all preempt it. - */ - DISABLE_TEMPERATURE_INTERRUPT(); - sei(); +HAL_TEMP_TIMER_ISR { + HAL_timer_isr_prologue(TEMP_TIMER_NUM); Temperature::isr(); - // Disable global interrupts and reenable this ISR - cli(); - ENABLE_TEMPERATURE_INTERRUPT(); + HAL_timer_isr_epilogue(TEMP_TIMER_NUM); } void Temperature::isr() { @@ -2107,13 +2084,6 @@ void Temperature::isr() { * This gives each ADC 0.9765ms to charge up. */ - #define SET_ADMUX_ADCSRA(pin) ADMUX = _BV(REFS0) | (pin & 0x07); SBI(ADCSRA, ADSC) - #ifdef MUX5 - #define START_ADC(pin) if (pin > 7) ADCSRB = _BV(MUX5); else ADCSRB = 0; SET_ADMUX_ADCSRA(pin) - #else - #define START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin) - #endif - switch (adc_sensor_state) { case SensorsReady: { @@ -2133,25 +2103,25 @@ void Temperature::isr() { #if HAS_TEMP_ADC_0 case PrepareTemp_0: - START_ADC(TEMP_0_PIN); + HAL_START_ADC(TEMP_0_PIN); break; case MeasureTemp_0: - raw_temp_value[0] += ADC; + raw_temp_value[0] += HAL_READ_ADC; break; #endif #if HAS_HEATED_BED case PrepareTemp_BED: - START_ADC(TEMP_BED_PIN); + HAL_START_ADC(TEMP_BED_PIN); break; case MeasureTemp_BED: - raw_temp_bed_value += ADC; + raw_temp_bed_value += HAL_READ_ADC; break; #endif #if HAS_TEMP_CHAMBER case PrepareTemp_CHAMBER: - START_ADC(TEMP_CHAMBER_PIN); + HAL_START_ADC(TEMP_CHAMBER_PIN); break; case MeasureTemp_CHAMBER: raw_temp_chamber_value += ADC; @@ -2160,55 +2130,55 @@ void Temperature::isr() { #if HAS_TEMP_ADC_1 case PrepareTemp_1: - START_ADC(TEMP_1_PIN); + HAL_START_ADC(TEMP_1_PIN); break; case MeasureTemp_1: - raw_temp_value[1] += ADC; + raw_temp_value[1] += HAL_READ_ADC; break; #endif #if HAS_TEMP_ADC_2 case PrepareTemp_2: - START_ADC(TEMP_2_PIN); + HAL_START_ADC(TEMP_2_PIN); break; case MeasureTemp_2: - raw_temp_value[2] += ADC; + raw_temp_value[2] += HAL_READ_ADC; break; #endif #if HAS_TEMP_ADC_3 case PrepareTemp_3: - START_ADC(TEMP_3_PIN); + HAL_START_ADC(TEMP_3_PIN); break; case MeasureTemp_3: - raw_temp_value[3] += ADC; + raw_temp_value[3] += HAL_READ_ADC; break; #endif #if HAS_TEMP_ADC_4 case PrepareTemp_4: - START_ADC(TEMP_4_PIN); + HAL_START_ADC(TEMP_4_PIN); break; case MeasureTemp_4: - raw_temp_value[4] += ADC; + raw_temp_value[4] += HAL_READ_ADC; break; #endif #if ENABLED(FILAMENT_WIDTH_SENSOR) case Prepare_FILWIDTH: - START_ADC(FILWIDTH_PIN); + HAL_START_ADC(FILWIDTH_PIN); break; case Measure_FILWIDTH: - if (ADC > 102) { // Make sure ADC is reading > 0.5 volts, otherwise don't read. + if (HAL_READ_ADC > 102) { // Make sure ADC is reading > 0.5 volts, otherwise don't read. raw_filwidth_value -= (raw_filwidth_value >> 7); // Subtract 1/128th of the raw_filwidth_value - raw_filwidth_value += ((unsigned long)ADC << 7); // Add new ADC reading, scaled by 128 + raw_filwidth_value += ((unsigned long)HAL_READ_ADC << 7); // Add new ADC reading, scaled by 128 } break; #endif #if ENABLED(ADC_KEYPAD) case Prepare_ADC_KEY: - START_ADC(ADC_KEYPAD_PIN); + HAL_START_ADC(ADC_KEYPAD_PIN); break; case Measure_ADC_KEY: if (ADCKey_count < 16) { From 8f26c3a6d37ca7284f11112c9f9c12595218176e Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 20 May 2018 08:19:11 -0500 Subject: [PATCH 3/6] Refactor and optimize Stepper/Planner --- Marlin/Marlin_main.cpp | 45 +-- Marlin/cardreader.cpp | 2 +- Marlin/endstops.cpp | 12 +- Marlin/planner.cpp | 599 ++++++++++++++++++++++----------- Marlin/planner.h | 238 ++++++++----- Marlin/planner_bezier.cpp | 10 +- Marlin/stepper.cpp | 690 +++++++++++++++++++------------------- Marlin/stepper.h | 148 +++----- Marlin/temperature.cpp | 20 +- Marlin/ubl_motion.cpp | 13 +- Marlin/ultralcd.cpp | 6 +- 11 files changed, 991 insertions(+), 792 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index cdaf65376..054ffa00b 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -8474,7 +8474,7 @@ inline void gcode_M111() { */ inline void gcode_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; @@ -8517,7 +8517,7 @@ inline void gcode_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(); @@ -9963,7 +9963,7 @@ inline void gcode_M400() { planner.synchronize(); } #endif // FILAMENT_WIDTH_SENSOR void quickstop_stepper() { - stepper.quick_stop(); + planner.quick_stop(); planner.synchronize(); set_current_from_steppers_for_axis(ALL_AXES); SYNC_PLAN_POSITION_KINEMATIC(); @@ -10342,7 +10342,7 @@ inline void gcode_M502() { * M540: Set whether SD card print should abort on endstop hit (M540 S<0|1>) */ inline void gcode_M540() { - if (parser.seen('S')) stepper.abort_on_endstop_hit = parser.value_bool(); + if (parser.seen('S')) planner.abort_on_endstop_hit = parser.value_bool(); } #endif // ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED @@ -12995,7 +12995,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { 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, @@ -13281,7 +13282,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { #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]); @@ -13291,7 +13293,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { //*/ 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 } @@ -13385,14 +13388,14 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { } // 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) @@ -13409,17 +13412,12 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { } #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( + planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); + 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; @@ -13652,14 +13650,17 @@ void prepare_move_to_destination() { // 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 } diff --git a/Marlin/cardreader.cpp b/Marlin/cardreader.cpp index 109ab428b..28fdf1152 100644 --- a/Marlin/cardreader.cpp +++ b/Marlin/cardreader.cpp @@ -941,7 +941,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) diff --git a/Marlin/endstops.cpp b/Marlin/endstops.cpp index 93fbd9a5a..15b601c52 100644 --- a/Marlin/endstops.cpp +++ b/Marlin/endstops.cpp @@ -181,7 +181,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) \ @@ -211,7 +211,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(); @@ -322,7 +322,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) @@ -331,9 +331,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.current_block->steps[_AXIS(X)] > 0) { _ENDSTOP_HIT(X, MIN); planner.endstop_triggered(_AXIS(X)); } + else if (stepper.current_block->steps[_AXIS(Y)] > 0) { _ENDSTOP_HIT(Y, MIN); planner.endstop_triggered(_AXIS(Y)); } + else if (stepper.current_block->steps[_AXIS(Z)] > 0) { _ENDSTOP_HIT(Z, MIN); planner.endstop_triggered(_AXIS(Z)); } G38_endstop_hit = true; } } diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 4d4b92e9d..3818570b4 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -56,6 +56,10 @@ * * IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) * + * -- + * + * The fast inverse function needed for Bézier interpolation for AVR + * was designed, written and tested by Eduardo José Tagle on April/2018 */ #include "planner.h" @@ -85,13 +89,18 @@ 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 -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 @@ -160,7 +169,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 }; @@ -197,7 +206,7 @@ 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 @@ -347,7 +356,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, @@ -520,7 +529,7 @@ void Planner::init() { A("rjmp 6f") // No, skip it A("mov %14,%15") A("clr %15") - L("6") // %16:%15:%14 = initial estimation of 0x1000000 / d) + L("6") // %16:%15:%14 = initial estimation of 0x1000000 / d // Now, we must refine the estimation present on %16:%15:%14 using 1 iteration // of Newton-Raphson. As it has a quadratic convergence, 1 iteration is enough @@ -709,7 +718,6 @@ void Planner::init() { // Return the result return r11 | (uint16_t(r12) << 8) | (uint32_t(r13) << 16); } - #endif // BEZIER_JERK_CONTROL #define MINIMAL_STEP_RATE 120 @@ -719,12 +727,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; @@ -733,19 +742,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) @@ -772,8 +780,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; @@ -786,32 +798,35 @@ 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)); -//} - // 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; SBI(current->flag, BLOCK_BIT_RECALCULATE); } } @@ -826,44 +841,37 @@ 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]; - // 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); - } - } - - do { - const block_t * const next = current; - blocknr = prev_block_index(blocknr); + // Perform the reverse pass + block_t *current, *next = NULL; + while (blocknr != endnr) { + // Perform the reverse pass - Only consider non sync blocks current = &block_buffer[blocknr]; - reverse_pass_kernel(current, next); - } while (blocknr != endnr); + if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) { + reverse_pass_kernel(current, next); + next = current; + } + // Advance to the next + blocknr = prev_block_index(blocknr); + } } } // 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) { 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; + if (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 + if (current->entry_speed_sqr > new_entry_speed_sqr) { + // Always <= max_entry_speed_sqr. Backward pass sets this. + current->entry_speed_sqr = new_entry_speed_sqr; SBI(current->flag, BLOCK_BIT_RECALCULATE); } } @@ -876,15 +884,21 @@ 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 }; + const uint8_t endnr = block_buffer_head; + uint8_t blocknr = block_buffer_tail; - 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]); + // Perform the forward pass + block_t *current, *previous = NULL; + while (blocknr != endnr) { + // Perform the forward pass - Only consider non-sync blocks + current = &block_buffer[blocknr]; + if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) { + forward_pass_kernel(previous, current); + previous = current; + } + // Advance to the previous + blocknr = next_block_index(blocknr); } - forward_pass_kernel(block[1], block[2]); } /** @@ -893,38 +907,72 @@ 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; + 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 @@ -974,7 +1022,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); } } @@ -1275,6 +1323,59 @@ 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! + block_buffer_head = block_buffer_tail; + + #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; + + // And stop the stepper ISR + stepper.quick_stop(); +} + +void Planner::endstop_triggered(const AxisEnum axis) { + + /*NB: This will be called via endstops.update() + and endstops.update() can be called from the temperature + ISR. So Stepper interrupts are enabled */ + + // Disable stepper ISR + bool stepper_isr_enabled = STEPPER_ISR_ENABLED(); + DISABLE_STEPPER_DRIVER_INTERRUPT(); + + // Record stepper position + stepper.endstop_triggered(axis); + + // Discard the active block that led to the trigger + discard_current_block(); + + // Discard the CONTINUED block, if any. Note the planner can only queue 1 continued + // block after a previous non continued block, as the condition to queue them + // is that there are no queued blocks at the time a new block is queued. + const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED); + if (discard) discard_current_block(); + + // Reenable stepper ISR if it was enabled + if (stepper_isr_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); +} + +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. @@ -1287,7 +1388,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 @@ -1309,18 +1410,69 @@ 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; + } + + // 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 @@ -1334,7 +1486,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]); @@ -1401,11 +1553,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; @@ -1442,7 +1590,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) @@ -1682,12 +1830,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) @@ -1775,8 +1927,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. @@ -1871,13 +2023,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))); // * 8.388608 + 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."); @@ -1885,7 +2037,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) @@ -1911,7 +2063,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[ @@ -1932,7 +2094,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] @@ -1946,21 +2108,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 + const 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); @@ -1976,13 +2150,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 { @@ -1992,19 +2168,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) { @@ -2039,16 +2217,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. @@ -2058,25 +2239,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 @@ -2087,29 +2265,15 @@ 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]; - - #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; + 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]; block_buffer_head = next_buffer_head; stepper.wake_up(); @@ -2127,7 +2291,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]) { @@ -2196,37 +2364,80 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) }; #endif - DISABLE_STEPPER_DRIVER_INTERRUPT(); + // The new head value is not assigned yet + uint8_t buffer_head = 0; + bool added = false; - _buffer_steps(between + uint8_t next_buffer_head; + block_t *block = get_next_free_block(next_buffer_head, 2); + + // Fill the block with the specified movement + if ( + _populate_block(block, true, between + #if HAS_POSITION_FLOAT + , between_float + #endif + , fr_mm_s, extruder, millimeters * 0.5 + ) + ) { + // Movement accepted - Point to the next reserved block + block = &block_buffer[next_buffer_head]; + + // Store into the new to be stored head + buffer_head = next_buffer_head; + added = true; + + // And advance the pointer to the next unused slot + next_buffer_head = next_block_index(next_buffer_head); + } + + // Fill the second part of the block with the 2nd part of the movement + if ( + _populate_block(block, true, target + #if HAS_POSITION_FLOAT + , target_float + #endif + , fr_mm_s, extruder, millimeters * 0.5 + ) + ) { + // Movement accepted - If this block is a continuation + // of the previous one, mark it as such + if (added) SBI(block->flag, BLOCK_BIT_CONTINUED); + + // Store into the new to be stored head + buffer_head = next_buffer_head; + added = true; + } + + // If any of the movements was added + if (added) { + + // Move buffer head and add all the blocks that were filled + // successfully to the movement queue. + block_buffer_head = buffer_head; + + // Update the position (only when a move was queued) + static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!"); + COPY(position, target); #if HAS_POSITION_FLOAT - , between_float + COPY(position_float, target_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(); + // Recalculate and optimize trapezoidal speed profiles + recalculate(); + } } - else - _buffer_steps(target + else 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() /** @@ -2253,7 +2464,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(); } @@ -2273,22 +2484,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). */ diff --git a/Marlin/planner.h b/Marlin/planner.h index 35ce6c280..d022eca08 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -49,7 +49,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 @@ -80,24 +80,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 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 - acceleration_steps_per_s2; // acceleration steps/sec^2 + 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 @@ -106,7 +117,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) @@ -120,12 +131,10 @@ 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 + acceleration_steps_per_s2; // acceleration steps/sec^2 #if FAN_COUNT > 0 uint16_t fan_speed[FAN_COUNT]; @@ -162,6 +171,7 @@ 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 #if ENABLED(DISTINCT_E_FACTORS) static uint8_t last_extruder; // Respond to extruder change @@ -229,6 +239,10 @@ class Planner { #endif #endif + #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) + static bool abort_on_endstop_hit; + #endif + private: /** @@ -243,9 +257,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 @@ -304,15 +318,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(); @@ -420,16 +425,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, const 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]; } @@ -442,8 +463,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 @@ -468,7 +511,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); @@ -485,11 +528,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); } /** @@ -502,7 +545,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); @@ -511,9 +554,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 } @@ -537,11 +580,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. @@ -553,35 +591,38 @@ 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. @@ -603,23 +644,42 @@ class Planner { #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() { + block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1); } #if ENABLED(ULTRA_LCD) static uint16_t block_buffer_runtime() { - CRITICAL_SECTION_START - millis_t bbru = block_buffer_runtime_us; - CRITICAL_SECTION_END + // 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(); + + millis_t bbru = block_buffer_runtime_us; + + // Reenable Stepper ISR + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); + // 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. @@ -630,9 +690,15 @@ class Planner { } static void clear_block_buffer_runtime() { - CRITICAL_SECTION_START - block_buffer_runtime_us = 0; - CRITICAL_SECTION_END + // 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(); + + block_buffer_runtime_us = 0; + + // Reenable Stepper ISR + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); } #endif @@ -675,12 +741,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) diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp index 6fc80c9ad..5ed7c043c 100644 --- a/Marlin/planner_bezier.cpp +++ b/Marlin/planner_bezier.cpp @@ -41,8 +41,7 @@ #define MAX_STEP 0.1 #define SIGMA 0.1 -/* Compute the linear interpolation between to real numbers. -*/ +// Compute the linear interpolation between two real numbers. inline static float interp(float a, float b, float t) { return (1.0 - t) * a + t * b; } /** @@ -188,12 +187,15 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t); bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t); clamp_to_software_endstops(bez_target); + #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 } } diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 1a2eefe3b..63bbdd132 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -75,10 +75,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 @@ -90,7 +86,6 @@ 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; #if ENABLED(X_DUAL_ENDSTOPS) bool Stepper::locked_x_motor = false, Stepper::locked_x2_motor = false; @@ -107,7 +102,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 @@ -119,16 +114,17 @@ 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 uint16_t ADV_NEVER = 65535; - - uint16_t Stepper::nextMainISR = 0, - Stepper::nextAdvanceISR = ADV_NEVER, - Stepper::eISR_Rate = ADV_NEVER, - Stepper::current_adv_steps = 0, + 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; @@ -144,7 +140,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 }; @@ -153,11 +149,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; -uint16_t Stepper::OCR1A_nominal; #if DISABLED(BEZIER_JERK_CONTROL) - uint16_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]; @@ -1112,201 +1108,95 @@ void Stepper::set_directions() { * 2000 1 KHz - sleep rate * 4000 500 Hz - init rate */ -ISR(TIMER1_COMPA_vect) { - /** - * On AVR there is no hardware prioritization and preemption of - * interrupts, so this emulates it. The UART has first priority - * (otherwise, characters will be lost due to UART overflow). - * Then: Stepper, Endstops, Temperature, and -finally- all others. - * - * This ISR needs to run with as little preemption as possible, so - * the Temperature ISR is disabled here. Now only the UART, Endstops, - * and Arduino-defined interrupts can preempt. - */ - const bool temp_isr_was_enabled = TEMPERATURE_ISR_ENABLED(); - DISABLE_TEMPERATURE_INTERRUPT(); - DISABLE_STEPPER_DRIVER_INTERRUPT(); - sei(); - #if ENABLED(LIN_ADVANCE) - Stepper::advance_isr_scheduler(); - #else - Stepper::isr(); - #endif +HAL_STEP_TIMER_ISR { + HAL_timer_isr_prologue(STEP_TIMER_NUM); - // Disable global interrupts and reenable this ISR - cli(); - ENABLE_STEPPER_DRIVER_INTERRUPT(); - // Reenable the temperature ISR (if it was enabled) - if (temp_isr_was_enabled) ENABLE_TEMPERATURE_INTERRUPT(); + // 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); } -void Stepper::isr() { +#define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B) - uint16_t ocr_val; +hal_timer_t Stepper::isr_scheduler() { + uint32_t interval; - #define ENDSTOP_NOMINAL_OCR_VAL 3000 // Check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch - #define OCR_VAL_TOLERANCE 1000 // 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(); - #define _SPLIT(L) (ocr_val = (uint16_t)L) - #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) - - #define SPLIT(L) _SPLIT(L) - - #else // !ENDSTOP_INTERRUPTS_FEATURE : Sample endstops between stepping ISRs - - static uint32_t step_remaining = 0; - - #define SPLIT(L) do { \ - _SPLIT(L); \ - if (ENDSTOPS_ENABLED && L > ENDSTOP_NOMINAL_OCR_VAL) { \ - const uint16_t remainder = (uint16_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \ - ocr_val = (remainder < OCR_VAL_TOLERANCE) ? ENDSTOP_NOMINAL_OCR_VAL + remainder : ENDSTOP_NOMINAL_OCR_VAL; \ - step_remaining = (uint16_t)L - ocr_val; \ - } \ - }while(0) - - if (step_remaining && ENDSTOPS_ENABLED) { // Just check endstops - not yet time for a step - 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); - NOLESS(OCR1A, TCNT1 + 16); - return; - } - - #endif // !ENDSTOP_INTERRUPTS_FEATURE - - // - // 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(200); // 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 - - // No step events completed so far - step_events_completed = 0; - - #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(2000); // Run at slow speed - 1 KHz - return; - } - #endif - } - else { - _NEXT_ISR(2000); // 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 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 @@ -1501,114 +1391,208 @@ 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!) - uint16_t acc_step_rate = - acceleration_time < current_block->acceleration_time - ? _eval_bezier_curve(acceleration_time) - : current_block->cruise_rate; - #else - acc_step_rate = MultiU24X32toH16(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 uint16_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 if (step_events_completed > (uint32_t)current_block->decelerate_after) { - uint16_t step_rate; + 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) { + #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(); + } + } + + // 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! + } + + // 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 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 = MultiU24X32toH16(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 + #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 - // step_rate to timer interval - const uint16_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) - NOLESS(OCR1A, TCNT1 + 16); - #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) @@ -1617,8 +1601,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) @@ -1679,21 +1663,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; @@ -1715,7 +1699,7 @@ void Stepper::isr() { while (e_steps) { #if EXTRA_CYCLES_E > 20 - uint32_t pulse_start = TCNT0; + hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM); #endif switch (LA_active_extruder) { @@ -1766,39 +1750,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 - OCR1A = 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 - OCR1A = 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; - } - - // Don't run the ISR faster than possible - NOLESS(OCR1A, TCNT1 + 16); - } - #endif // LIN_ADVANCE void Stepper::init() { @@ -2013,30 +1967,43 @@ 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; -} + // 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(); -void Stepper::finish_and_disable() { - planner.synchronize(); - disable_all_steppers(); + const int32_t v = count_position[axis]; + + // Reenable Stepper ISR + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); + return v; } void Stepper::quick_stop() { + const bool was_enabled = STEPPER_ISR_ENABLED(); 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(); - #endif + + if (current_block) { + step_events_completed = current_block->step_event_count; + current_block = NULL; + } + + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); +} + +void Stepper::kill_current_block() { + const bool was_enabled = STEPPER_ISR_ENABLED(); + DISABLE_STEPPER_DRIVER_INTERRUPT(); + + if (current_block) + step_events_completed = current_block->step_event_count; + + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); } void Stepper::endstop_triggered(const AxisEnum axis) { + const bool was_enabled = STEPPER_ISR_ENABLED(); + if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT(); #if IS_CORE @@ -2051,8 +2018,31 @@ 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 + if (current_block) { + + // Kill the current block being executed + step_events_completed = current_block->step_event_count; + + // Prep to get a new block after cleaning + current_block = NULL; + } + + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); +} + +int32_t Stepper::triggered_position(const AxisEnum axis) { + // 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(); + + const int32_t v = endstops_trigsteps[axis]; + + // Reenable Stepper ISR + if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); + + return v; } void Stepper::report_positions() { diff --git a/Marlin/stepper.h b/Marlin/stepper.h index 198da72dd..df640a058 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -85,10 +85,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 @@ -100,8 +96,6 @@ 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 @@ -118,7 +112,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 @@ -130,12 +124,14 @@ class Stepper { 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 uint16_t nextMainISR, nextAdvanceISR, eISR_Rate, 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 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". static int8_t e_steps; static bool use_advance_lead; #if E_STEPPERS > 1 @@ -144,18 +140,14 @@ class Stepper { static constexpr int8_t LA_active_extruder = 0; #endif - #else // !LIN_ADVANCE + #endif // LIN_ADVANCE - #define _NEXT_ISR(T) OCR1A = 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 uint16_t OCR1A_nominal; + static uint32_t ticks_nominal; #if DISABLED(BEZIER_JERK_CONTROL) - static uint16_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]; @@ -188,88 +180,50 @@ 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 and clear the blocks queue - // + // Quickly stop all steppers 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); } + // Kill current block + static void kill_current_block(); + + // 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); @@ -301,32 +255,22 @@ 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 uint16_t calc_timer_interval(uint16_t step_rate) { - uint16_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(); + + FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate) { + uint32_t timer; + + NOMORE(step_rate, uint32_t(MAX_STEP_FREQUENCY)); if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times step_rate >>= 2; @@ -340,12 +284,14 @@ class Stepper { step_loops = 1; } - 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 - 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 = (uint16_t)pgm_read_word_near(table_address) - MultiU16X8toH16(step_rate & 0x00FF, gain); + const uint8_t tmp_step_rate = (step_rate & 0x00FF); + const 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]; @@ -355,9 +301,9 @@ class Stepper { } if (timer < 100) { // (20kHz - this should never happen) timer = 100; - SERIAL_PROTOCOL(MSG_STEPPER_TOO_HIGH); - SERIAL_PROTOCOLLN(step_rate); + SERIAL_ECHOLNPAIR(MSG_STEPPER_TOO_HIGH, step_rate); } + return timer; } diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp index ca344aab1..38fdb00bc 100644 --- a/Marlin/temperature.cpp +++ b/Marlin/temperature.cpp @@ -1780,6 +1780,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); @@ -2301,25 +2302,22 @@ void Temperature::isr() { #endif // BABYSTEPPING #if ENABLED(PINS_DEBUGGING) - extern bool endstop_monitor_flag; - // run the endstop monitor at 15Hz - static uint8_t endstop_monitor_count = 16; // offset this check from the others - if (endstop_monitor_flag) { - endstop_monitor_count += _BV(1); // 15 Hz - endstop_monitor_count &= 0x7F; - if (!endstop_monitor_count) endstop_monitor(); // report changes in endstop status - } + endstops.run_monitor(); // report changes in endstop status #endif + // Update endstops state, if enabled #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) - extern volatile uint8_t e_hit; - if (e_hit && ENDSTOPS_ENABLED) { - endstops.update(); // call endstop update routine + endstops.update(); e_hit--; } + #else + if (ENDSTOPS_ENABLED) endstops.update(); #endif + + // Periodically call the planner timer + planner.tick(); } #if HAS_TEMP_SENSOR diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index 9ffdff3bb..0e8e7b909 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -257,7 +257,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 "); } @@ -314,7 +315,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--; } @@ -337,7 +339,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--; } @@ -366,7 +369,7 @@ inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) { #if ENABLED(SKEW_CORRECTION) - float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS], in_raw[E_AXIS] }; + float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] }; planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); #else const float (&raw)[XYZE] = in_raw; @@ -438,7 +441,7 @@ uint16_t segments = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length #endif - NOLESS(segments, 1); // must have at least one segment + NOLESS(segments, 1U); // must have at least one segment const float inv_segments = 1.0 / segments; // divide once, multiply thereafter #if IS_SCARA // scale the feed rate from mm/s to degrees/s diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index 95bef8e11..2940cbc0d 100644 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -2393,12 +2393,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(); } @@ -3806,7 +3804,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(); From a4f2f2fe54c0ab90fca1ae507c674d6555b3ce99 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 20 May 2018 10:39:00 -0500 Subject: [PATCH 4/6] Planner block HOLD flag --- Marlin/planner.cpp | 112 +++++++++++++-------------------------------- Marlin/planner.h | 31 +++++++++---- 2 files changed, 54 insertions(+), 89 deletions(-) diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 3818570b4..ae9f57e2e 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -81,6 +81,10 @@ #include "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: @@ -92,6 +96,7 @@ block_t Planner::block_buffer[BLOCK_BUFFER_SIZE]; volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed Planner::block_buffer_tail; // Index of the busy block, if any uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks +uint8_t Planner::delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second Planner::axis_steps_per_mm[XYZE_N], @@ -211,6 +216,7 @@ void Planner::init() { bed_level_matrix.set_to_identity(); #endif clear_block_buffer(); + delay_before_delivering = 0; } #if ENABLED(BEZIER_JERK_CONTROL) @@ -1329,6 +1335,10 @@ void Planner::quick_stop() { // that is why we set head to tail! 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(); @@ -1357,12 +1367,6 @@ void Planner::endstop_triggered(const AxisEnum axis) { // Discard the active block that led to the trigger discard_current_block(); - // Discard the CONTINUED block, if any. Note the planner can only queue 1 continued - // block after a previous non continued block, as the condition to queue them - // is that there are no queued blocks at the time a new block is queued. - const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED); - if (discard) discard_current_block(); - // Reenable stepper ISR if it was enabled if (stepper_isr_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); } @@ -1450,6 +1454,16 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE] 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; @@ -2275,7 +2289,18 @@ void Planner::buffer_sync_block() { block->position[C_AXIS] = position[C_AXIS]; block->position[E_AXIS] = position[E_AXIS]; + // 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() @@ -2353,81 +2378,8 @@ bool 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 - - // The new head value is not assigned yet - uint8_t buffer_head = 0; - bool added = false; - - uint8_t next_buffer_head; - block_t *block = get_next_free_block(next_buffer_head, 2); - - // Fill the block with the specified movement + // Queue the movement if ( - _populate_block(block, true, between - #if HAS_POSITION_FLOAT - , between_float - #endif - , fr_mm_s, extruder, millimeters * 0.5 - ) - ) { - // Movement accepted - Point to the next reserved block - block = &block_buffer[next_buffer_head]; - - // Store into the new to be stored head - buffer_head = next_buffer_head; - added = true; - - // And advance the pointer to the next unused slot - next_buffer_head = next_block_index(next_buffer_head); - } - - // Fill the second part of the block with the 2nd part of the movement - if ( - _populate_block(block, true, target - #if HAS_POSITION_FLOAT - , target_float - #endif - , fr_mm_s, extruder, millimeters * 0.5 - ) - ) { - // Movement accepted - If this block is a continuation - // of the previous one, mark it as such - if (added) SBI(block->flag, BLOCK_BIT_CONTINUED); - - // Store into the new to be stored head - buffer_head = next_buffer_head; - added = true; - } - - // If any of the movements was added - if (added) { - - // Move buffer head and add all the blocks that were filled - // successfully to the movement queue. - block_buffer_head = buffer_head; - - // Update the position (only when a move was queued) - 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 and optimize trapezoidal speed profiles - recalculate(); - } - } - else if ( !_buffer_steps(target #if HAS_POSITION_FLOAT , target_float diff --git a/Marlin/planner.h b/Marlin/planner.h index d022eca08..902a4a3e0 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -172,6 +172,7 @@ class Planner { 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 #if ENABLED(DISTINCT_E_FACTORS) static uint8_t last_extruder; // Respond to extruder change @@ -629,17 +630,29 @@ class Planner { * 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) + || (movesplanned() > 1 && TEST(block_buffer[next_block_index(block_buffer_tail)].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. From d2f897104555b3be1f257e7267e25221a6fd38fb Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 20 May 2018 10:50:15 -0500 Subject: [PATCH 5/6] Fix planner block optimization --- Marlin/planner.cpp | 221 ++++++++++++++++++++++++++++++++------------- Marlin/planner.h | 23 +++-- 2 files changed, 174 insertions(+), 70 deletions(-) diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index ae9f57e2e..3075f3433 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -96,7 +96,8 @@ block_t Planner::block_buffer[BLOCK_BUFFER_SIZE]; volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed Planner::block_buffer_tail; // Index of the busy block, if any uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks -uint8_t Planner::delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks +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 Planner::axis_steps_per_mm[XYZE_N], @@ -216,6 +217,7 @@ void Planner::init() { bed_level_matrix.set_to_identity(); #endif clear_block_buffer(); + block_buffer_planned = 0; delay_before_delivering = 0; } @@ -807,6 +809,68 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); } +/* 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) { @@ -833,6 +897,8 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const : 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); } } @@ -844,44 +910,72 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const * 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); + // Initialize block index to the last block in the planner buffer. + uint8_t block_index = prev_block_index(block_buffer_head); + + // 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 - block_t *current, *next = NULL; - while (blocknr != endnr) { - // Perform the reverse pass - Only consider non sync blocks - current = &block_buffer[blocknr]; - if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) { - reverse_pass_kernel(current, next); - next = current; - } - // Advance to the next - blocknr = prev_block_index(blocknr); + 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; } + + // 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, const 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_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 - if (current->entry_speed_sqr > new_entry_speed_sqr) { - // Always <= max_entry_speed_sqr. Backward pass sets this. - current->entry_speed_sqr = new_entry_speed_sqr; - 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; } } @@ -890,20 +984,30 @@ 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() { - const uint8_t endnr = block_buffer_head; - uint8_t blocknr = block_buffer_tail; - // Perform the forward pass - block_t *current, *previous = NULL; - while (blocknr != endnr) { - // Perform the forward pass - Only consider non-sync blocks - current = &block_buffer[blocknr]; + // 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); + forward_pass_kernel(previous, current, block_index); previous = current; } // Advance to the previous - blocknr = next_block_index(blocknr); + block_index = next_block_index(block_index); } } @@ -913,6 +1017,7 @@ void Planner::forward_pass() { * recalculate() after updating the blocks. */ void Planner::recalculate_trapezoids() { + // The tail may be changed by the ISR so get a local copy. uint8_t block_index = block_buffer_tail; // As there could be a sync block in the head of the queue, and the next loop must not @@ -986,33 +1091,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(); } @@ -1330,10 +1416,18 @@ 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! - block_buffer_head = block_buffer_tail; + // 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. @@ -1347,6 +1441,9 @@ void Planner::quick_stop() { // 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(); } diff --git a/Marlin/planner.h b/Marlin/planner.h index 902a4a3e0..1c4f2a1e8 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -172,7 +172,9 @@ class Planner { 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 + 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 @@ -650,9 +652,7 @@ class Planner { block_t * const block = &block_buffer[block_buffer_tail]; // No trapezoid calculated? Don't execute yet. - if ( TEST(block->flag, BLOCK_BIT_RECALCULATE) - || (movesplanned() > 1 && TEST(block_buffer[next_block_index(block_buffer_tail)].flag, BLOCK_BIT_RECALCULATE)) - ) return NULL; + 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. @@ -677,7 +677,14 @@ class Planner { * NB: There MUST be a current block to call this function!! */ FORCE_INLINE static void discard_current_block() { - block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1); + 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) @@ -728,8 +735,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 @@ -774,7 +781,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(); From 3a4a229721f30fbd882dc996cc9d94fcb3a115cf Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 20 May 2018 10:59:58 -0500 Subject: [PATCH 6/6] Fix interrupt-based endstop detection --- Marlin/Marlin_main.cpp | 12 +- Marlin/endstop_interrupts.h | 23 +-- Marlin/endstops.cpp | 340 +++++++++++++++++++++++------------- Marlin/endstops.h | 64 ++++--- Marlin/enum.h | 16 -- Marlin/planner.cpp | 17 +- Marlin/stepper.cpp | 108 ++++++------ Marlin/stepper.h | 14 +- Marlin/temperature.cpp | 21 +-- 9 files changed, 332 insertions(+), 283 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 054ffa00b..2fac62ab3 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -336,10 +336,6 @@ #include "I2CPositionEncoder.h" #endif -#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) - #include "endstop_interrupts.h" -#endif - #if ENABLED(M100_FREE_MEMORY_WATCHER) void gcode_M100(); void M100_dump_routine(const char * const title, const char *start, const char *end); @@ -14292,7 +14288,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! servo_init(); // Initialize all servos, stow servo probe @@ -14417,10 +14415,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 diff --git a/Marlin/endstop_interrupts.h b/Marlin/endstop_interrupts.h index c83bac23a..65f0d1a5b 100644 --- a/Marlin/endstop_interrupts.h +++ b/Marlin/endstop_interrupts.h @@ -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,6 +40,9 @@ #include "macros.h" +// One ISR for all EXT-Interrupts +void endstop_ISR(void) { endstops.check_possible_change(); } + /** * Patch for pins_arduino.h (...\Arduino\hardware\arduino\avr\variants\mega\pins_arduino.h) * @@ -72,8 +75,6 @@ 0 ) #endif -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. // Install Pin change interrupt for a pin. Can be called multiple times. void pciSetup(const int8_t pin) { @@ -82,30 +83,22 @@ void pciSetup(const int8_t pin) { SBI(PCICR, digitalPinToPCICRbit(pin)); // enable interrupt for the group } -// 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. -} - -// Use one Routine to handle each group -// One ISR for all EXT-Interrupts -void endstop_ISR(void) { endstop_ISR_worker(); } // 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 ) { diff --git a/Marlin/endstops.cpp b/Marlin/endstops.cpp index 15b601c52..734dcecfa 100644 --- a/Marlin/endstops.cpp +++ b/Marlin/endstops.cpp @@ -31,18 +31,27 @@ #include "stepper.h" #include "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 "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; @@ -169,8 +178,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) @@ -273,38 +367,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 @@ -331,9 +428,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); planner.endstop_triggered(_AXIS(X)); } - else if (stepper.current_block->steps[_AXIS(Y)] > 0) { _ENDSTOP_HIT(Y, MIN); planner.endstop_triggered(_AXIS(Y)); } - else if (stepper.current_block->steps[_AXIS(Z)] > 0) { _ENDSTOP_HIT(Z, MIN); planner.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; } } @@ -344,7 +441,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 @@ -364,7 +461,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 @@ -384,7 +481,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 @@ -404,13 +501,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 @@ -421,124 +518,117 @@ 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() diff --git a/Marlin/endstops.h b/Marlin/endstops.h index fdaf02ea7..903a407cd 100644 --- a/Marlin/endstops.h +++ b/Marlin/endstops.h @@ -29,12 +29,28 @@ #include "MarlinConfig.h" +enum EndstopEnum : char { + X_MIN, + Y_MIN, + Z_MIN, + Z_MIN_PROBE, + X_MAX, + Y_MAX, + Z_MAX, + X2_MIN, + X2_MAX, + Y2_MIN, + Y2_MAX, + Z2_MIN, + Z2_MAX +}; + 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; @@ -51,23 +67,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 */ @@ -84,21 +103,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(); + static void run_monitor(); #endif private: @@ -116,10 +142,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__ diff --git a/Marlin/enum.h b/Marlin/enum.h index 378e47f32..1d5f0fcfb 100644 --- a/Marlin/enum.h +++ b/Marlin/enum.h @@ -88,22 +88,6 @@ enum DebugFlags : unsigned char { DEBUG_ALL = 0xFF }; -enum EndstopEnum : char { - X_MIN, - Y_MIN, - Z_MIN, - Z_MIN_PROBE, - X_MAX, - Y_MAX, - Z_MAX, - X2_MIN, - X2_MAX, - Y2_MIN, - Y2_MAX, - Z2_MIN, - Z2_MAX -}; - #if ENABLED(ADVANCED_PAUSE_FEATURE) enum AdvancedPauseMenuResponse : char { ADVANCED_PAUSE_RESPONSE_WAIT_FOR, diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 3075f3433..ffbee93ad 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -1449,23 +1449,8 @@ void Planner::quick_stop() { } void Planner::endstop_triggered(const AxisEnum axis) { - - /*NB: This will be called via endstops.update() - and endstops.update() can be called from the temperature - ISR. So Stepper interrupts are enabled */ - - // Disable stepper ISR - bool stepper_isr_enabled = STEPPER_ISR_ENABLED(); - DISABLE_STEPPER_DRIVER_INTERRUPT(); - - // Record stepper position + // Record stepper position and discard the current block stepper.endstop_triggered(axis); - - // Discard the active block that led to the trigger - discard_current_block(); - - // Reenable stepper ISR if it was enabled - if (stepper_isr_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); } float Planner::triggered_position_mm(const AxisEnum axis) { diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index 63bbdd132..071e6e89b 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -85,7 +85,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 +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; @@ -165,20 +168,20 @@ volatile int32_t Stepper::endstops_trigsteps[XYZ]; #define LOCKED_X2_MOTOR locked_x2_motor #define LOCKED_Y2_MOTOR locked_y2_motor #define LOCKED_Z2_MOTOR locked_z2_motor - #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); \ - } \ - 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); \ - } \ - } \ - else { \ - A##_STEP_WRITE(V); \ - A##2_STEP_WRITE(V); \ + #define DUAL_ENDSTOP_APPLY_STEP(A,V) \ + if (performing_homing) { \ + if (A##_HOME_DIR < 0) { \ + 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.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 { \ + A##_STEP_WRITE(V); \ + A##2_STEP_WRITE(V); \ } #endif @@ -1192,6 +1195,15 @@ hal_timer_t Stepper::isr_scheduler() { // 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; @@ -1521,12 +1533,13 @@ uint32_t Stepper::stepper_block_phase_isr() { return interval; // No more queued movements! } - // Initialize the trapezoid generator from the current block. - static int8_t last_extruder = -1; + // 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_extruder) { + 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; } @@ -1539,12 +1552,21 @@ uint32_t Stepper::stepper_block_phase_isr() { } #endif - if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) { + if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_movement_extruder) { last_direction_bits = current_block->direction_bits; - last_extruder = current_block->active_extruder; + 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; @@ -1577,11 +1599,6 @@ uint32_t Stepper::stepper_block_phase_isr() { 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, enable it now. This option will severely interfere with // timing between pulses when chaining motion between blocks, and it could lead @@ -1848,9 +1865,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() @@ -1979,29 +1993,14 @@ int32_t Stepper::position(const AxisEnum axis) { return v; } -void Stepper::quick_stop() { - const bool was_enabled = STEPPER_ISR_ENABLED(); - DISABLE_STEPPER_DRIVER_INTERRUPT(); - - if (current_block) { - step_events_completed = current_block->step_event_count; - current_block = NULL; - } - - if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); -} - -void Stepper::kill_current_block() { - const bool was_enabled = STEPPER_ISR_ENABLED(); - DISABLE_STEPPER_DRIVER_INTERRUPT(); - - if (current_block) - step_events_completed = current_block->step_event_count; - - if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); -} - +// 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(); @@ -2019,14 +2018,7 @@ void Stepper::endstop_triggered(const AxisEnum axis) { #endif // !COREXY && !COREXZ && !COREYZ // Discard the rest of the move if there is a current block - if (current_block) { - - // Kill the current block being executed - step_events_completed = current_block->step_event_count; - - // Prep to get a new block after cleaning - current_block = NULL; - } + quick_stop(); if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT(); } diff --git a/Marlin/stepper.h b/Marlin/stepper.h index df640a058..1be3f94c9 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -98,7 +98,10 @@ class Stepper { 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; @@ -210,13 +213,16 @@ class Stepper { static void wake_up(); // Quickly stop all steppers - static void quick_stop(); + FORCE_INLINE static void quick_stop() { abort_current_block = true; } // The direction of a single motor FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); } - // Kill current block - static void kill_current_block(); + // 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); diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp index 38fdb00bc..f2bdfab9c 100644 --- a/Marlin/temperature.cpp +++ b/Marlin/temperature.cpp @@ -32,6 +32,7 @@ #include "language.h" #include "printcounter.h" #include "delay.h" +#include "endstops.h" #if ENABLED(HEATER_0_USES_MAX6675) #include "MarlinSPI.h" @@ -41,10 +42,6 @@ #include "stepper.h" #endif -#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) - #include "endstops.h" -#endif - #if ENABLED(USE_WATCHDOG) #include "watchdog.h" #endif @@ -2301,20 +2298,8 @@ void Temperature::isr() { } #endif // BABYSTEPPING - #if ENABLED(PINS_DEBUGGING) - endstops.run_monitor(); // report changes in endstop status - #endif - - // Update endstops state, if enabled - #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) - extern volatile uint8_t e_hit; - if (e_hit && ENDSTOPS_ENABLED) { - endstops.update(); - e_hit--; - } - #else - if (ENDSTOPS_ENABLED) endstops.update(); - #endif + // Poll endstops state, if required + endstops.poll(); // Periodically call the planner timer planner.tick();