diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index f693eb1edf..415cefb66b 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -47,7 +47,8 @@ #include "../shared/servo.h" #include "../shared/servo_private.h" -static volatile int8_t Channel[_Nbr_16timers]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) +static int8_t Channel[_Nbr_16timers]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) +static bool DisablePending[_Nbr_16timers]; // Instructs ISR to disable the timer at the next timer reset // ------------------------ /// Interrupt handler for the TC0 channel 1. @@ -71,10 +72,18 @@ void Servo_Handler(const timer16_Sequence_t, Tc*, const uint8_t); #endif void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) { - tc->TC_CHANNEL[channel].TC_SR; // clear interrupt int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first - if (cho < 0) // Channel -1 indicates the refresh interval completed... + if (cho < 0) { // Channel -1 indicates the refresh interval completed... tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // ...so reset the timer + if (DisablePending[timer]) { + // Disabling only after the full servo period expires prevents + // pulses being too close together if immediately re-enabled. + DisablePending[timer] = false; + TC_Stop(tc, channel); + tc->TC_CHANNEL[channel].TC_SR; // clear interrupt + return; + } + } else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled? extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW @@ -92,6 +101,8 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel Channel[timer] = -1; // reset the timer CCR on the next call } + + tc->TC_CHANNEL[channel].TC_SR; // clear interrupt } static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { @@ -117,39 +128,33 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { } void initISR(const timer16_Sequence_t timer) { - #ifdef _useTimer1 - if (timer == _timer1) _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); - #endif - #ifdef _useTimer2 - if (timer == _timer2) _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2); - #endif - #ifdef _useTimer3 - if (timer == _timer3) _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); - #endif - #ifdef _useTimer4 - if (timer == _timer4) _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); - #endif - #ifdef _useTimer5 - if (timer == _timer5) _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); - #endif + CRITICAL_SECTION_START(); + bool needsInit = !DisablePending[timer]; + DisablePending[timer] = false; + CRITICAL_SECTION_END(); + + if (needsInit) { + #ifdef _useTimer1 + if (timer == _timer1) _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); + #endif + #ifdef _useTimer2 + if (timer == _timer2) _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2); + #endif + #ifdef _useTimer3 + if (timer == _timer3) _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); + #endif + #ifdef _useTimer4 + if (timer == _timer4) _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); + #endif + #ifdef _useTimer5 + if (timer == _timer5) _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); + #endif + } } -void finISR(timer16_Sequence_t) { - #ifdef _useTimer1 - TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); - #endif - #ifdef _useTimer2 - TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2); - #endif - #ifdef _useTimer3 - TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3); - #endif - #ifdef _useTimer4 - TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4); - #endif - #ifdef _useTimer5 - TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); - #endif +void finISR(timer16_Sequence_t timer) { + // Timer is disabled from the ISR, to ensure proper final pulse length. + DisablePending[timer] = true; } #endif // HAS_SERVOS