diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp
index 6dc1e2fb073..0a1ef5337ae 100644
--- a/Marlin/src/HAL/AVR/Servo.cpp
+++ b/Marlin/src/HAL/AVR/Servo.cpp
@@ -67,26 +67,25 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s
/************ static functions common to all instances ***********************/
static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
- if (Channel[timer] < 0)
- *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
- else {
- if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
- extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
- }
+ int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
+ if (cho < 0) // Channel -1 indicates the refresh interval completed...
+ *TCNTn = 0; // ...so reset the timer
+ else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
+ extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
- Channel[timer]++; // increment to the next channel
- if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
- *OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks;
- if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated
- extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
+ Channel[timer] = ++cho; // Handle the next channel (or 0)
+ if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
+ *OCRnA = *TCNTn + SERVO(timer, cho).ticks; // set compare to current ticks plus duration
+ if (SERVO(timer, cho).Pin.isActive) // activated?
+ extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
}
else {
// finished all channels so wait for the refresh period to expire before starting over
- if (((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
- *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
- else
- *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
- Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
+ const unsigned int cval = ((unsigned)*TCNTn) + 32 / (SERVO_TIMER_PRESCALER), // allow 32 cycles to ensure the next OCR1A not missed
+ ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
+ *OCRnA = max(cval, ival);
+
+ Channel[timer] = -1; // reset the timer counter to 0 on the next call
}
}
@@ -123,91 +122,102 @@ static inline void handle_interrupts(const timer16_Sequence_t timer, volatile ui
/****************** end of static functions ******************************/
-void initISR(timer16_Sequence_t timer) {
- #ifdef _useTimer1
- if (timer == _timer1) {
- TCCR1A = 0; // normal counting mode
- TCCR1B = _BV(CS11); // set prescaler of 8
- TCNT1 = 0; // clear the timer count
- #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__)
- SBI(TIFR, OCF1A); // clear any pending interrupts;
- SBI(TIMSK, OCIE1A); // enable the output compare interrupt
- #else
- // here if not ATmega8 or ATmega128
- SBI(TIFR1, OCF1A); // clear any pending interrupts;
- SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
- #endif
- #ifdef WIRING
- timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
- #endif
- }
- #endif
+void initISR(const timer16_Sequence_t timer_index) {
+ switch (timer_index) {
+ default: break;
- #ifdef _useTimer3
- if (timer == _timer3) {
- TCCR3A = 0; // normal counting mode
- TCCR3B = _BV(CS31); // set prescaler of 8
- TCNT3 = 0; // clear the timer count
- #ifdef __AVR_ATmega128__
- SBI(TIFR, OCF3A); // clear any pending interrupts;
- SBI(ETIMSK, OCIE3A); // enable the output compare interrupt
- #else
- SBI(TIFR3, OCF3A); // clear any pending interrupts;
- SBI(TIMSK3, OCIE3A); // enable the output compare interrupt
- #endif
- #ifdef WIRING
- timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
- #endif
- }
- #endif
+ #ifdef _useTimer1
+ case _timer1:
+ TCCR1A = 0; // normal counting mode
+ TCCR1B = _BV(CS11); // set prescaler of 8
+ TCNT1 = 0; // clear the timer count
+ #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__)
+ SBI(TIFR, OCF1A); // clear any pending interrupts;
+ SBI(TIMSK, OCIE1A); // enable the output compare interrupt
+ #else
+ // here if not ATmega8 or ATmega128
+ SBI(TIFR1, OCF1A); // clear any pending interrupts;
+ SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
+ #endif
+ #ifdef WIRING
+ timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
+ #endif
+ break;
+ #endif
- #ifdef _useTimer4
- if (timer == _timer4) {
- TCCR4A = 0; // normal counting mode
- TCCR4B = _BV(CS41); // set prescaler of 8
- TCNT4 = 0; // clear the timer count
- TIFR4 = _BV(OCF4A); // clear any pending interrupts;
- TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt
- }
- #endif
+ #ifdef _useTimer3
+ case _timer3:
+ TCCR3A = 0; // normal counting mode
+ TCCR3B = _BV(CS31); // set prescaler of 8
+ TCNT3 = 0; // clear the timer count
+ #ifdef __AVR_ATmega128__
+ SBI(TIFR, OCF3A); // clear any pending interrupts;
+ SBI(ETIMSK, OCIE3A); // enable the output compare interrupt
+ #else
+ SBI(TIFR3, OCF3A); // clear any pending interrupts;
+ SBI(TIMSK3, OCIE3A); // enable the output compare interrupt
+ #endif
+ #ifdef WIRING
+ timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
+ #endif
+ break;
+ #endif
- #ifdef _useTimer5
- if (timer == _timer5) {
- TCCR5A = 0; // normal counting mode
- TCCR5B = _BV(CS51); // set prescaler of 8
- TCNT5 = 0; // clear the timer count
- TIFR5 = _BV(OCF5A); // clear any pending interrupts;
- TIMSK5 = _BV(OCIE5A); // enable the output compare interrupt
- }
- #endif
+ #ifdef _useTimer4
+ case _timer4:
+ TCCR4A = 0; // normal counting mode
+ TCCR4B = _BV(CS41); // set prescaler of 8
+ TCNT4 = 0; // clear the timer count
+ TIFR4 = _BV(OCF4A); // clear any pending interrupts;
+ TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt
+ break;
+ #endif
+
+ #ifdef _useTimer5
+ case _timer5:
+ TCCR5A = 0; // normal counting mode
+ TCCR5B = _BV(CS51); // set prescaler of 8
+ TCNT5 = 0; // clear the timer count
+ TIFR5 = _BV(OCF5A); // clear any pending interrupts;
+ TIMSK5 = _BV(OCIE5A); // enable the output compare interrupt
+ break;
+ #endif
+ }
}
-void finISR(timer16_Sequence_t timer) {
+void finISR(const timer16_Sequence_t timer_index) {
// Disable use of the given timer
#ifdef WIRING
- if (timer == _timer1) {
- CBI(
- #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
- TIMSK1
- #else
- TIMSK
- #endif
- , OCIE1A); // disable timer 1 output compare interrupt
- timerDetach(TIMER1OUTCOMPAREA_INT);
- }
- else if (timer == _timer3) {
- CBI(
- #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
- TIMSK3
- #else
- ETIMSK
- #endif
- , OCIE3A); // disable the timer3 output compare A interrupt
- timerDetach(TIMER3OUTCOMPAREA_INT);
+ switch (timer_index) {
+ default: break;
+
+ case _timer1:
+ CBI(
+ #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
+ TIMSK1
+ #else
+ TIMSK
+ #endif
+ , OCIE1A // disable timer 1 output compare interrupt
+ );
+ timerDetach(TIMER1OUTCOMPAREA_INT);
+ break;
+
+ case _timer3:
+ CBI(
+ #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
+ TIMSK3
+ #else
+ ETIMSK
+ #endif
+ , OCIE3A // disable the timer3 output compare A interrupt
+ );
+ timerDetach(TIMER3OUTCOMPAREA_INT);
+ break;
}
#else // !WIRING
// For arduino - in future: call here to a currently undefined function to reset the timer
- UNUSED(timer);
+ UNUSED(timer_index);
#endif
}
diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp
index 72a7d649ef0..2dab88238dd 100644
--- a/Marlin/src/HAL/DUE/Servo.cpp
+++ b/Marlin/src/HAL/DUE/Servo.cpp
@@ -47,7 +47,7 @@
#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 Flags<_Nbr_16timers> DisablePending; // ISR should disable the timer at the next timer reset
// ------------------------
/// Interrupt handler for the TC0 channel 1.
@@ -71,82 +71,91 @@ 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) {
- // clear interrupt
- tc->TC_CHANNEL[channel].TC_SR;
- if (Channel[timer] < 0)
- tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
- else if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
- extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
+ static int8_t Channel[_Nbr_16timers]; // Servo counters to pulse (or -1 for refresh interval)
+ int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
+ 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.clear(timer);
+ 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
- Channel[timer]++; // increment to the next channel
- if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
- tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
- if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated
- extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high
+ Channel[timer] = ++cho; // go to the next channel (or 0)
+ if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
+ tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer, cho).ticks;
+ if (SERVO(timer, cho).Pin.isActive) // activated?
+ extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
}
else {
// finished all channels so wait for the refresh period to expire before starting over
- tc->TC_CHANNEL[channel].TC_RA =
- tc->TC_CHANNEL[channel].TC_CV < usToTicks(REFRESH_INTERVAL) - 4
- ? (unsigned int)usToTicks(REFRESH_INTERVAL) // allow a few ticks to ensure the next OCR1A not missed
- : tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
- Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
+ const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + 128 / (SERVO_TIMER_PRESCALER), // allow 128 cycles to ensure the next CV not missed
+ ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
+ tc->TC_CHANNEL[channel].TC_RA = max(cval, ival);
+
+ 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) {
pmc_enable_periph_clk(id);
TC_Configure(tc, channel,
- TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32
- TC_CMR_WAVE | // Waveform mode
- TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC
+ TC_CMR_WAVE // Waveform mode
+ | TC_CMR_WAVSEL_UP_RC // Counter running up and reset when equal to RC
+ | (SERVO_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) // MCK/2
+ | (SERVO_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0) // MCK/8
+ | (SERVO_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) // MCK/32
+ | (SERVO_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0) // MCK/128
+ );
- /* 84MHz, MCK/32, for 1.5ms: 3937 */
- TC_SetRA(tc, channel, 2625); // 1ms
+ // Wait 1ms before the first ISR
+ TC_SetRA(tc, channel, (F_CPU) / (SERVO_TIMER_PRESCALER) / 1000UL); // 1ms
- /* Configure and enable interrupt */
+ // Configure and enable interrupt
NVIC_EnableIRQ(irqn);
- // TC_IER_CPAS: RA Compare
- tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS;
+ tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS; // TC_IER_CPAS: RA Compare
// Enables the timer clock and performs a software reset to start the counting
TC_Start(tc, channel);
}
-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
+void initISR(const timer16_Sequence_t timer_index) {
+ CRITICAL_SECTION_START();
+ const bool disable_soon = DisablePending[timer_index];
+ DisablePending.clear(timer_index);
+ CRITICAL_SECTION_END();
+
+ if (!disable_soon) switch (timer_index) {
+ default: break;
+ #ifdef _useTimer1
+ case _timer1: return _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
+ #endif
+ #ifdef _useTimer2
+ case _timer2: return _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
+ #endif
+ #ifdef _useTimer3
+ case _timer3: return _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
+ #endif
+ #ifdef _useTimer4
+ case _timer4: return _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
+ #endif
+ #ifdef _useTimer5
+ case _timer5: return _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(const timer16_Sequence_t timer_index) {
+ // Timer is disabled from the ISR, to ensure proper final pulse length.
+ DisablePending.set(timer_index);
}
#endif // HAS_SERVOS
diff --git a/Marlin/src/HAL/DUE/ServoTimers.h b/Marlin/src/HAL/DUE/ServoTimers.h
index c32c9382539..95bd404c806 100644
--- a/Marlin/src/HAL/DUE/ServoTimers.h
+++ b/Marlin/src/HAL/DUE/ServoTimers.h
@@ -37,7 +37,7 @@
#define _useTimer5
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
-#define SERVO_TIMER_PRESCALER 32 // timer prescaler
+#define SERVO_TIMER_PRESCALER 2 // timer prescaler
/*
TC0, chan 0 => TC0_Handler
diff --git a/Marlin/src/HAL/DUE/timers.cpp b/Marlin/src/HAL/DUE/timers.cpp
index a7bf7fbd6d8..e5647817b6f 100644
--- a/Marlin/src/HAL/DUE/timers.cpp
+++ b/Marlin/src/HAL/DUE/timers.cpp
@@ -89,10 +89,17 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
NVIC_SetPriority(irq, timer_config[timer_num].priority);
// wave mode, reset counter on match with RC,
- TC_Configure(tc, channel, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK1);
+ TC_Configure(tc, channel,
+ TC_CMR_WAVE
+ | TC_CMR_WAVSEL_UP_RC
+ | (HAL_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0)
+ | (HAL_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0)
+ | (HAL_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0)
+ | (HAL_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0)
+ );
// Set compare value
- TC_SetRC(tc, channel, VARIANT_MCK / 2 / frequency);
+ TC_SetRC(tc, channel, VARIANT_MCK / (HAL_TIMER_PRESCALER) / frequency);
// And start timer
TC_Start(tc, channel);
diff --git a/Marlin/src/HAL/DUE/timers.h b/Marlin/src/HAL/DUE/timers.h
index bcfd07e268c..dc35c77e638 100644
--- a/Marlin/src/HAL/DUE/timers.h
+++ b/Marlin/src/HAL/DUE/timers.h
@@ -35,7 +35,8 @@
typedef uint32_t hal_timer_t;
#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF
-#define HAL_TIMER_RATE ((F_CPU) / 2) // frequency of timers peripherals
+#define HAL_TIMER_PRESCALER 2
+#define HAL_TIMER_RATE ((F_CPU) / (HAL_TIMER_PRESCALER)) // frequency of timers peripherals
#ifndef MF_TIMER_STEP
#define MF_TIMER_STEP 2 // Timer Index for Stepper
diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp
index 04461653f94..665322fe24b 100644
--- a/Marlin/src/HAL/SAMD51/Servo.cpp
+++ b/Marlin/src/HAL/SAMD51/Servo.cpp
@@ -77,7 +77,8 @@ HAL_SERVO_TIMER_ISR() {
;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
- if (currentServoIndex[timer] < 0) {
+ int8_t cho = currentServoIndex[timer]; // Handle the prior servo first
+ if (cho < 0) { // Servo -1 indicates the refresh interval completed...
#if defined(_useTimer1) && defined(_useTimer2)
if (currentServoIndex[timer ^ 1] >= 0) {
// Wait for both channels
@@ -86,41 +87,33 @@ HAL_SERVO_TIMER_ISR() {
return;
}
#endif
- tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL;
+ tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; // ...so reset the timer
SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT);
}
- else if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive)
- digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
+ else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
+ digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
- // Select the next servo controlled by this timer
- currentServoIndex[timer]++;
+ currentServoIndex[timer] = ++cho; // go to the next channel (or 0)
+ if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
+ if (SERVO(timer, cho).Pin.isActive) // activated?
+ digitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
- if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
- if (SERVO(timer, currentServoIndex[timer]).Pin.isActive) // check if activated
- digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
-
- tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, currentServoIndex[timer]).ticks;
+ tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, cho).ticks;
}
else {
// finished all channels so wait for the refresh period to expire before starting over
- currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
-
- const uint16_t tcCounterValue = getTimerCount();
-
- if ((TC_COUNTER_START_VAL - tcCounterValue) + 4UL < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
- tc->COUNT16.CC[tcChannel].reg = TC_COUNTER_START_VAL - (uint16_t)usToTicks(REFRESH_INTERVAL);
- else
- tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - 4UL); // at least REFRESH_INTERVAL has elapsed
+ currentServoIndex[timer] = -1; // reset the timer COUNT.reg on the next call
+ const uint16_t cval = getTimerCount() - 256 / (SERVO_TIMER_PRESCALER), // allow 256 cycles to ensure the next CV not missed
+ ival = (TC_COUNTER_START_VAL) - (uint16_t)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
+ tc->COUNT16.CC[tcChannel].reg = min(cval, ival);
}
if (tcChannel == 0) {
SYNC(tc->COUNT16.SYNCBUSY.bit.CC0);
- // Clear the interrupt
- tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0;
+ tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0; // Clear the interrupt
}
else {
SYNC(tc->COUNT16.SYNCBUSY.bit.CC1);
- // Clear the interrupt
- tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1;
+ tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1; // Clear the interrupt
}
}
@@ -201,9 +194,9 @@ void initISR(const timer16_Sequence_t timer) {
}
}
-void finISR(const timer16_Sequence_t timer) {
+void finISR(const timer16_Sequence_t timer_index) {
Tc * const tc = timer_config[SERVO_TC].pTc;
- const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
+ const uint8_t tcChannel = TIMER_TCCHANNEL(timer_index);
// Disable the match channel interrupt request
tc->COUNT16.INTENCLR.reg = (tcChannel == 0) ? TC_INTENCLR_MC0 : TC_INTENCLR_MC1;
diff --git a/Marlin/src/HAL/shared/servo_private.h b/Marlin/src/HAL/shared/servo_private.h
index 021e0cb81d0..10cc5a19882 100644
--- a/Marlin/src/HAL/shared/servo_private.h
+++ b/Marlin/src/HAL/shared/servo_private.h
@@ -94,5 +94,5 @@ extern ServoInfo_t servo_info[MAX_SERVOS];
// Public functions
-extern void initISR(const timer16_Sequence_t timer);
-extern void finISR(const timer16_Sequence_t timer);
+void initISR(const timer16_Sequence_t timer_index);
+void finISR(const timer16_Sequence_t timer_index);
diff --git a/Marlin/src/core/serial.cpp b/Marlin/src/core/serial.cpp
index 49e792110ef..727b191d040 100644
--- a/Marlin/src/core/serial.cpp
+++ b/Marlin/src/core/serial.cpp
@@ -72,8 +72,8 @@ void serial_print_P(PGM_P str) {
while (const char c = pgm_read_byte(str++)) SERIAL_CHAR(c);
}
-void serial_echo_start() { static PGMSTR(echomagic, "echo:"); serial_print_P(echomagic); }
-void serial_error_start() { static PGMSTR(errormagic, "Error:"); serial_print_P(errormagic); }
+void serial_echo_start() { serial_print(F("echo:")); }
+void serial_error_start() { serial_print(F("Error:")); }
void serial_spaces(uint8_t count) { count *= (PROPORTIONAL_FONT_RATIO); while (count--) SERIAL_CHAR(' '); }
diff --git a/Marlin/src/core/types.h b/Marlin/src/core/types.h
index fc3ef178b6d..335aa3a3344 100644
--- a/Marlin/src/core/types.h
+++ b/Marlin/src/core/types.h
@@ -99,8 +99,8 @@ struct Flags {
void set(const int n) { b |= (bits_t)_BV(n); }
void clear(const int n) { b &= ~(bits_t)_BV(n); }
bool test(const int n) const { return TEST(b, n); }
- bool operator[](const int n) { return test(n); }
- bool operator[](const int n) const { return test(n); }
+ const bool operator[](const int n) { return test(n); }
+ const bool operator[](const int n) const { return test(n); }
int size() const { return sizeof(b); }
};
@@ -113,8 +113,8 @@ struct Flags<1> {
void set(const int) { b = true; }
void clear(const int) { b = false; }
bool test(const int) const { return b; }
- bool operator[](const int) { return b; }
- bool operator[](const int) const { return b; }
+ bool& operator[](const int) { return b; }
+ bool operator[](const int) const { return b; }
int size() const { return sizeof(b); }
};