Skip to content

Resolve DUE Servo pulse issue #24305

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
thinkyhead merged 26 commits into from
Jun 22, 2022
Merged

Resolve DUE Servo pulse issue #24305

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
e7a6c77
Resolve DUE Servo pulse issue
InsanityAutomation Jun 7, 2022
6566031
keep check for good SERVO_INDEX
thinkyhead Jun 7, 2022
86b9e7e
servo cleanup
thinkyhead Jun 7, 2022
6c39286
Try Servo::detach adjustment ??
thinkyhead Jun 7, 2022
04ffe94
Try adjusting finISR also ??
thinkyhead Jun 7, 2022
7f6622e
test this if nothing else works
thinkyhead Jun 7, 2022
dbd9321
use simple fix
thinkyhead Jun 8, 2022
c1775c9
Remove MOVE_SERVO, DETACH_SERVO
thinkyhead Jun 8, 2022
fb5dd40
Revise timer to MCK/2
InsanityAutomation Jun 8, 2022
0206af6
fix servo index check, add comments
thinkyhead Jun 9, 2022
5cdc452
Merge branch 'bugfix-2.1.x' into pr/24305
thinkyhead Jun 9, 2022
8af96b8
Update Servo.cpp
InsanityAutomation Jun 9, 2022
5fbb7cb
ws
thinkyhead Jun 11, 2022
9bb87da
apply SERVO_TIMER_PRESCALER to intervals
thinkyhead Jun 11, 2022
a03ee01
match samd51 to others
thinkyhead Jun 11, 2022
b3f2f44
set inactive servos LOW
thinkyhead Jun 11, 2022
701ab0d
define HAL_TIMER_PRESCALER
thinkyhead Jun 11, 2022
ad4cf3e
pick best clock for prescaler
thinkyhead Jun 11, 2022
9d4aff6
Fix DUE timer behavior when disabling servos
sjasonsmith Jun 12, 2022
425306b
Use Flags class
thinkyhead Jun 22, 2022
273a97e
move Channel
thinkyhead Jun 22, 2022
038dfa0
negate later
thinkyhead Jun 22, 2022
af1d342
use switch
thinkyhead Jun 22, 2022
f7f9a21
tweak serial echo
thinkyhead Jun 22, 2022
5bb8847
tweak flags types
thinkyhead Jun 22, 2022
a3b62a7
clean up, use 'timer_index'
thinkyhead Jun 22, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
200 changes: 105 additions & 95 deletions Marlin/src/HAL/AVR/Servo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
}

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
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] = ++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
}
}

Expand Down Expand Up @@ -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

#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 _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 _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
}

void finISR(timer16_Sequence_t timer) {
// Disable use of the given timer
#ifdef WIRING
if (timer == _timer1) {
CBI(
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
TIMSK1
void initISR(const timer16_Sequence_t timer_index) {
switch (timer_index) {
default: break;

#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
TIMSK
// here if not ATmega8 or ATmega128
SBI(TIFR1, OCF1A); // clear any pending interrupts;
SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
#endif
, OCIE1A); // disable timer 1 output compare interrupt
timerDetach(TIMER1OUTCOMPAREA_INT);
}
else if (timer == _timer3) {
CBI(
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
TIMSK3
#ifdef WIRING
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
#endif
break;
#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
ETIMSK
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
, OCIE3A); // disable the timer3 output compare A interrupt
timerDetach(TIMER3OUTCOMPAREA_INT);
break;
#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(const timer16_Sequence_t timer_index) {
// Disable use of the given timer
#ifdef WIRING
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
}

Expand Down
129 changes: 69 additions & 60 deletions Marlin/src/HAL/DUE/Servo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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

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
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] = ++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

/* 84MHz, MCK/32, for 1.5ms: 3937 */
TC_SetRA(tc, channel, 2625); // 1ms

/* Configure and enable interrupt */
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
);

// Wait 1ms before the first ISR
TC_SetRA(tc, channel, (F_CPU) / (SERVO_TIMER_PRESCALER) / 1000UL); // 1ms

// 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
Expand Down
2 changes: 1 addition & 1 deletion Marlin/src/HAL/DUE/ServoTimers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading