Skip to content

Commit a4d795e

Browse files
InsanityAutomationsjasonsmiththinkyhead
authored and
LCh-77
committed
🐛 Resolve DUE Servo pulse issue (MarlinFirmware#24305)
Co-authored-by: sjasonsmith <[email protected]> Co-authored-by: Scott Lahteine <[email protected]>
1 parent d1edd32 commit a4d795e

File tree

9 files changed

+212
-192
lines changed

9 files changed

+212
-192
lines changed

‎Marlin/src/HAL/AVR/Servo.cpp

+105-95
Original file line numberDiff line numberDiff line change
@@ -67,26 +67,25 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s
6767
/************ static functions common to all instances ***********************/
6868

6969
static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
70-
if (Channel[timer] < 0)
71-
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
72-
else {
73-
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
74-
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
75-
}
76-
77-
Channel[timer]++; // increment to the next channel
78-
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
79-
*OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks;
80-
if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated
81-
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
70+
int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
71+
if (cho < 0) // Channel -1 indicates the refresh interval completed...
72+
*TCNTn = 0; // ...so reset the timer
73+
else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
74+
extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
75+
76+
Channel[timer] = ++cho; // Handle the next channel (or 0)
77+
if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
78+
*OCRnA = *TCNTn + SERVO(timer, cho).ticks; // set compare to current ticks plus duration
79+
if (SERVO(timer, cho).Pin.isActive) // activated?
80+
extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
8281
}
8382
else {
8483
// finished all channels so wait for the refresh period to expire before starting over
85-
if (((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
86-
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
87-
else
88-
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
89-
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
84+
const unsigned int cval = ((unsigned)*TCNTn) + 32 / (SERVO_TIMER_PRESCALER), // allow 32 cycles to ensure the next OCR1A not missed
85+
ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
86+
*OCRnA = max(cval, ival);
87+
88+
Channel[timer] = -1; // reset the timer counter to 0 on the next call
9089
}
9190
}
9291

@@ -123,91 +122,102 @@ static inline void handle_interrupts(const timer16_Sequence_t timer, volatile ui
123122

124123
/****************** end of static functions ******************************/
125124

126-
void initISR(timer16_Sequence_t timer) {
127-
#ifdef _useTimer1
128-
if (timer == _timer1) {
129-
TCCR1A = 0; // normal counting mode
130-
TCCR1B = _BV(CS11); // set prescaler of 8
131-
TCNT1 = 0; // clear the timer count
132-
#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__)
133-
SBI(TIFR, OCF1A); // clear any pending interrupts;
134-
SBI(TIMSK, OCIE1A); // enable the output compare interrupt
135-
#else
136-
// here if not ATmega8 or ATmega128
137-
SBI(TIFR1, OCF1A); // clear any pending interrupts;
138-
SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
139-
#endif
140-
#ifdef WIRING
141-
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
142-
#endif
143-
}
144-
#endif
145-
146-
#ifdef _useTimer3
147-
if (timer == _timer3) {
148-
TCCR3A = 0; // normal counting mode
149-
TCCR3B = _BV(CS31); // set prescaler of 8
150-
TCNT3 = 0; // clear the timer count
151-
#ifdef __AVR_ATmega128__
152-
SBI(TIFR, OCF3A); // clear any pending interrupts;
153-
SBI(ETIMSK, OCIE3A); // enable the output compare interrupt
154-
#else
155-
SBI(TIFR3, OCF3A); // clear any pending interrupts;
156-
SBI(TIMSK3, OCIE3A); // enable the output compare interrupt
157-
#endif
158-
#ifdef WIRING
159-
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
160-
#endif
161-
}
162-
#endif
163-
164-
#ifdef _useTimer4
165-
if (timer == _timer4) {
166-
TCCR4A = 0; // normal counting mode
167-
TCCR4B = _BV(CS41); // set prescaler of 8
168-
TCNT4 = 0; // clear the timer count
169-
TIFR4 = _BV(OCF4A); // clear any pending interrupts;
170-
TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt
171-
}
172-
#endif
173-
174-
#ifdef _useTimer5
175-
if (timer == _timer5) {
176-
TCCR5A = 0; // normal counting mode
177-
TCCR5B = _BV(CS51); // set prescaler of 8
178-
TCNT5 = 0; // clear the timer count
179-
TIFR5 = _BV(OCF5A); // clear any pending interrupts;
180-
TIMSK5 = _BV(OCIE5A); // enable the output compare interrupt
181-
}
182-
#endif
183-
}
184-
185-
void finISR(timer16_Sequence_t timer) {
186-
// Disable use of the given timer
187-
#ifdef WIRING
188-
if (timer == _timer1) {
189-
CBI(
190-
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
191-
TIMSK1
125+
void initISR(const timer16_Sequence_t timer_index) {
126+
switch (timer_index) {
127+
default: break;
128+
129+
#ifdef _useTimer1
130+
case _timer1:
131+
TCCR1A = 0; // normal counting mode
132+
TCCR1B = _BV(CS11); // set prescaler of 8
133+
TCNT1 = 0; // clear the timer count
134+
#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__)
135+
SBI(TIFR, OCF1A); // clear any pending interrupts;
136+
SBI(TIMSK, OCIE1A); // enable the output compare interrupt
192137
#else
193-
TIMSK
138+
// here if not ATmega8 or ATmega128
139+
SBI(TIFR1, OCF1A); // clear any pending interrupts;
140+
SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
194141
#endif
195-
, OCIE1A); // disable timer 1 output compare interrupt
196-
timerDetach(TIMER1OUTCOMPAREA_INT);
197-
}
198-
else if (timer == _timer3) {
199-
CBI(
200-
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
201-
TIMSK3
142+
#ifdef WIRING
143+
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
144+
#endif
145+
break;
146+
#endif
147+
148+
#ifdef _useTimer3
149+
case _timer3:
150+
TCCR3A = 0; // normal counting mode
151+
TCCR3B = _BV(CS31); // set prescaler of 8
152+
TCNT3 = 0; // clear the timer count
153+
#ifdef __AVR_ATmega128__
154+
SBI(TIFR, OCF3A); // clear any pending interrupts;
155+
SBI(ETIMSK, OCIE3A); // enable the output compare interrupt
202156
#else
203-
ETIMSK
157+
SBI(TIFR3, OCF3A); // clear any pending interrupts;
158+
SBI(TIMSK3, OCIE3A); // enable the output compare interrupt
159+
#endif
160+
#ifdef WIRING
161+
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
204162
#endif
205-
, OCIE3A); // disable the timer3 output compare A interrupt
206-
timerDetach(TIMER3OUTCOMPAREA_INT);
163+
break;
164+
#endif
165+
166+
#ifdef _useTimer4
167+
case _timer4:
168+
TCCR4A = 0; // normal counting mode
169+
TCCR4B = _BV(CS41); // set prescaler of 8
170+
TCNT4 = 0; // clear the timer count
171+
TIFR4 = _BV(OCF4A); // clear any pending interrupts;
172+
TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt
173+
break;
174+
#endif
175+
176+
#ifdef _useTimer5
177+
case _timer5:
178+
TCCR5A = 0; // normal counting mode
179+
TCCR5B = _BV(CS51); // set prescaler of 8
180+
TCNT5 = 0; // clear the timer count
181+
TIFR5 = _BV(OCF5A); // clear any pending interrupts;
182+
TIMSK5 = _BV(OCIE5A); // enable the output compare interrupt
183+
break;
184+
#endif
185+
}
186+
}
187+
188+
void finISR(const timer16_Sequence_t timer_index) {
189+
// Disable use of the given timer
190+
#ifdef WIRING
191+
switch (timer_index) {
192+
default: break;
193+
194+
case _timer1:
195+
CBI(
196+
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
197+
TIMSK1
198+
#else
199+
TIMSK
200+
#endif
201+
, OCIE1A // disable timer 1 output compare interrupt
202+
);
203+
timerDetach(TIMER1OUTCOMPAREA_INT);
204+
break;
205+
206+
case _timer3:
207+
CBI(
208+
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
209+
TIMSK3
210+
#else
211+
ETIMSK
212+
#endif
213+
, OCIE3A // disable the timer3 output compare A interrupt
214+
);
215+
timerDetach(TIMER3OUTCOMPAREA_INT);
216+
break;
207217
}
208218
#else // !WIRING
209219
// For arduino - in future: call here to a currently undefined function to reset the timer
210-
UNUSED(timer);
220+
UNUSED(timer_index);
211221
#endif
212222
}
213223

‎Marlin/src/HAL/DUE/Servo.cpp

+69-60
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@
4747
#include "../shared/servo.h"
4848
#include "../shared/servo_private.h"
4949

50-
static volatile int8_t Channel[_Nbr_16timers]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
50+
static Flags<_Nbr_16timers> DisablePending; // ISR should disable the timer at the next timer reset
5151

5252
// ------------------------
5353
/// Interrupt handler for the TC0 channel 1.
@@ -71,82 +71,91 @@ void Servo_Handler(const timer16_Sequence_t, Tc*, const uint8_t);
7171
#endif
7272

7373
void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) {
74-
// clear interrupt
75-
tc->TC_CHANNEL[channel].TC_SR;
76-
if (Channel[timer] < 0)
77-
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
78-
else if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
79-
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
80-
81-
Channel[timer]++; // increment to the next channel
82-
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
83-
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
84-
if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated
85-
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high
74+
static int8_t Channel[_Nbr_16timers]; // Servo counters to pulse (or -1 for refresh interval)
75+
int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
76+
if (cho < 0) { // Channel -1 indicates the refresh interval completed...
77+
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // ...so reset the timer
78+
if (DisablePending[timer]) {
79+
// Disabling only after the full servo period expires prevents
80+
// pulses being too close together if immediately re-enabled.
81+
DisablePending.clear(timer);
82+
TC_Stop(tc, channel);
83+
tc->TC_CHANNEL[channel].TC_SR; // clear interrupt
84+
return;
85+
}
86+
}
87+
else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
88+
extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
89+
90+
Channel[timer] = ++cho; // go to the next channel (or 0)
91+
if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
92+
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer, cho).ticks;
93+
if (SERVO(timer, cho).Pin.isActive) // activated?
94+
extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
8695
}
8796
else {
8897
// finished all channels so wait for the refresh period to expire before starting over
89-
tc->TC_CHANNEL[channel].TC_RA =
90-
tc->TC_CHANNEL[channel].TC_CV < usToTicks(REFRESH_INTERVAL) - 4
91-
? (unsigned int)usToTicks(REFRESH_INTERVAL) // allow a few ticks to ensure the next OCR1A not missed
92-
: tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
93-
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
98+
const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + 128 / (SERVO_TIMER_PRESCALER), // allow 128 cycles to ensure the next CV not missed
99+
ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
100+
tc->TC_CHANNEL[channel].TC_RA = max(cval, ival);
101+
102+
Channel[timer] = -1; // reset the timer CCR on the next call
94103
}
104+
105+
tc->TC_CHANNEL[channel].TC_SR; // clear interrupt
95106
}
96107

97108
static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) {
98109
pmc_enable_periph_clk(id);
99110
TC_Configure(tc, channel,
100-
TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32
101-
TC_CMR_WAVE | // Waveform mode
102-
TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC
103-
104-
/* 84MHz, MCK/32, for 1.5ms: 3937 */
105-
TC_SetRA(tc, channel, 2625); // 1ms
106-
107-
/* Configure and enable interrupt */
111+
TC_CMR_WAVE // Waveform mode
112+
| TC_CMR_WAVSEL_UP_RC // Counter running up and reset when equal to RC
113+
| (SERVO_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) // MCK/2
114+
| (SERVO_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0) // MCK/8
115+
| (SERVO_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) // MCK/32
116+
| (SERVO_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0) // MCK/128
117+
);
118+
119+
// Wait 1ms before the first ISR
120+
TC_SetRA(tc, channel, (F_CPU) / (SERVO_TIMER_PRESCALER) / 1000UL); // 1ms
121+
122+
// Configure and enable interrupt
108123
NVIC_EnableIRQ(irqn);
109-
// TC_IER_CPAS: RA Compare
110-
tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS;
124+
tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS; // TC_IER_CPAS: RA Compare
111125

112126
// Enables the timer clock and performs a software reset to start the counting
113127
TC_Start(tc, channel);
114128
}
115129

116-
void initISR(const timer16_Sequence_t timer) {
117-
#ifdef _useTimer1
118-
if (timer == _timer1) _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
119-
#endif
120-
#ifdef _useTimer2
121-
if (timer == _timer2) _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
122-
#endif
123-
#ifdef _useTimer3
124-
if (timer == _timer3) _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
125-
#endif
126-
#ifdef _useTimer4
127-
if (timer == _timer4) _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
128-
#endif
129-
#ifdef _useTimer5
130-
if (timer == _timer5) _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
131-
#endif
130+
void initISR(const timer16_Sequence_t timer_index) {
131+
CRITICAL_SECTION_START();
132+
const bool disable_soon = DisablePending[timer_index];
133+
DisablePending.clear(timer_index);
134+
CRITICAL_SECTION_END();
135+
136+
if (!disable_soon) switch (timer_index) {
137+
default: break;
138+
#ifdef _useTimer1
139+
case _timer1: return _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
140+
#endif
141+
#ifdef _useTimer2
142+
case _timer2: return _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
143+
#endif
144+
#ifdef _useTimer3
145+
case _timer3: return _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
146+
#endif
147+
#ifdef _useTimer4
148+
case _timer4: return _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
149+
#endif
150+
#ifdef _useTimer5
151+
case _timer5: return _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
152+
#endif
153+
}
132154
}
133155

134-
void finISR(timer16_Sequence_t) {
135-
#ifdef _useTimer1
136-
TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
137-
#endif
138-
#ifdef _useTimer2
139-
TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
140-
#endif
141-
#ifdef _useTimer3
142-
TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
143-
#endif
144-
#ifdef _useTimer4
145-
TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
146-
#endif
147-
#ifdef _useTimer5
148-
TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
149-
#endif
156+
void finISR(const timer16_Sequence_t timer_index) {
157+
// Timer is disabled from the ISR, to ensure proper final pulse length.
158+
DisablePending.set(timer_index);
150159
}
151160

152161
#endif // HAS_SERVOS

‎Marlin/src/HAL/DUE/ServoTimers.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@
3737
#define _useTimer5
3838

3939
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
40-
#define SERVO_TIMER_PRESCALER 32 // timer prescaler
40+
#define SERVO_TIMER_PRESCALER 2 // timer prescaler
4141

4242
/*
4343
TC0, chan 0 => TC0_Handler

0 commit comments

Comments
 (0)