bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

🐛 Resolve DUE Servo pulse issue (#24305)

Browse Source 
Co-authored-by: sjasonsmith <20053467+sjasonsmith@users.noreply.github.com>
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
InsanityAutomation 2022-06-21 22:09:24 -04:00 committed by Scott Lahteine
parent 0523874e9c
commit 0dc59311ec
9 changed files with 205 additions and 185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

192
Marlin/src/HAL/AVR/Servo.cpp
View File

@ -67,26 +67,25 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s
/************ static functions common to all instances ***********************/ /************ static functions common to all instances ***********************/
static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
if (Channel[timer] < 0) int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer if (cho < 0) // Channel -1 indicates the refresh interval completed...
else { *TCNTn = 0; // ...so reset the timer
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive) else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
}
Channel[timer]++; // increment to the next channel Channel[timer] = ++cho; // Handle the next channel (or 0)
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
*OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks; *OCRnA = *TCNTn + SERVO(timer, cho).ticks; // set compare to current ticks plus duration
if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated if (SERVO(timer, cho).Pin.isActive) // activated?
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
} }
else { else {
// finished all channels so wait for the refresh period to expire before starting over // 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 const unsigned int cval = ((unsigned)*TCNTn) + 32 / (SERVO_TIMER_PRESCALER), // allow 32 cycles to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
else *OCRnA = max(cval, ival);
*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 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 ******************************/ /****************** end of static functions ******************************/
void initISR(timer16_Sequence_t timer) { void initISR(const timer16_Sequence_t timer_index) {
#ifdef _useTimer1 switch (timer_index) {
if (timer == _timer1) { default: break;
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 #ifdef _useTimer1
if (timer == _timer3) { case _timer1:
TCCR3A = 0; // normal counting mode TCCR1A = 0; // normal counting mode
TCCR3B = _BV(CS31); // set prescaler of 8 TCCR1B = _BV(CS11); // set prescaler of 8
TCNT3 = 0; // clear the timer count TCNT1 = 0; // clear the timer count
#ifdef __AVR_ATmega128__ #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__)
SBI(TIFR, OCF3A); // clear any pending interrupts; SBI(TIFR, OCF1A); // clear any pending interrupts;
SBI(ETIMSK, OCIE3A); // enable the output compare interrupt SBI(TIMSK, OCIE1A); // enable the output compare interrupt
#else #else
SBI(TIFR3, OCF3A); // clear any pending interrupts; // here if not ATmega8 or ATmega128
SBI(TIMSK3, OCIE3A); // enable the output compare interrupt SBI(TIFR1, OCF1A); // clear any pending interrupts;
#endif SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
#ifdef WIRING #endif
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only #ifdef WIRING
#endif timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
} #endif
#endif break;
#endif
#ifdef _useTimer4 #ifdef _useTimer3
if (timer == _timer4) { case _timer3:
TCCR4A = 0; // normal counting mode TCCR3A = 0; // normal counting mode
TCCR4B = _BV(CS41); // set prescaler of 8 TCCR3B = _BV(CS31); // set prescaler of 8
TCNT4 = 0; // clear the timer count TCNT3 = 0; // clear the timer count
TIFR4 = _BV(OCF4A); // clear any pending interrupts; #ifdef __AVR_ATmega128__
TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt SBI(TIFR, OCF3A); // clear any pending interrupts;
} SBI(ETIMSK, OCIE3A); // enable the output compare interrupt
#endif #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 #ifdef _useTimer4
if (timer == _timer5) { case _timer4:
TCCR5A = 0; // normal counting mode TCCR4A = 0; // normal counting mode
TCCR5B = _BV(CS51); // set prescaler of 8 TCCR4B = _BV(CS41); // set prescaler of 8
TCNT5 = 0; // clear the timer count TCNT4 = 0; // clear the timer count
TIFR5 = _BV(OCF5A); // clear any pending interrupts; TIFR4 = _BV(OCF4A); // clear any pending interrupts;
TIMSK5 = _BV(OCIE5A); // enable the output compare interrupt TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt
} break;
#endif #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 // Disable use of the given timer
#ifdef WIRING #ifdef WIRING
if (timer == _timer1) { switch (timer_index) {
CBI( default: break;
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
TIMSK1 case _timer1:
#else CBI(
TIMSK #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
#endif TIMSK1
, OCIE1A); // disable timer 1 output compare interrupt #else
timerDetach(TIMER1OUTCOMPAREA_INT); TIMSK
} #endif
else if (timer == _timer3) { , OCIE1A // disable timer 1 output compare interrupt
CBI( );
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) timerDetach(TIMER1OUTCOMPAREA_INT);
TIMSK3 break;
#else
ETIMSK case _timer3:
#endif CBI(
, OCIE3A); // disable the timer3 output compare A interrupt #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
timerDetach(TIMER3OUTCOMPAREA_INT); TIMSK3
#else
ETIMSK
#endif
, OCIE3A // disable the timer3 output compare A interrupt
);
timerDetach(TIMER3OUTCOMPAREA_INT);
break;
} }
#else // !WIRING #else // !WIRING
// For arduino - in future: call here to a currently undefined function to reset the timer // For arduino - in future: call here to a currently undefined function to reset the timer
UNUSED(timer); UNUSED(timer_index);
#endif #endif
} }

123
Marlin/src/HAL/DUE/Servo.cpp
View File

@ -47,7 +47,7 @@
#include "../shared/servo.h" #include "../shared/servo.h"
#include "../shared/servo_private.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. /// Interrupt handler for the TC0 channel 1.
@ -71,82 +71,91 @@ void Servo_Handler(const timer16_Sequence_t, Tc*, const uint8_t);
#endif #endif
void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) { void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) {
// clear interrupt static int8_t Channel[_Nbr_16timers]; // Servo counters to pulse (or -1 for refresh interval)
tc->TC_CHANNEL[channel].TC_SR; int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
if (Channel[timer] < 0) if (cho < 0) { // Channel -1 indicates the refresh interval completed...
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // ...so reset the timer
else if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive) if (DisablePending[timer]) {
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated // 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 Channel[timer] = ++cho; // go to the next channel (or 0)
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks; tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer, cho).ticks;
if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated if (SERVO(timer, cho).Pin.isActive) // activated?
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
} }
else { else {
// finished all channels so wait for the refresh period to expire before starting over // finished all channels so wait for the refresh period to expire before starting over
tc->TC_CHANNEL[channel].TC_RA = const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + 128 / (SERVO_TIMER_PRESCALER), // allow 128 cycles to ensure the next CV not missed
tc->TC_CHANNEL[channel].TC_CV < usToTicks(REFRESH_INTERVAL) - 4 ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
? (unsigned int)usToTicks(REFRESH_INTERVAL) // allow a few ticks to ensure the next OCR1A not missed tc->TC_CHANNEL[channel].TC_RA = max(cval, ival);
: 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 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) { static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) {
pmc_enable_periph_clk(id); pmc_enable_periph_clk(id);
TC_Configure(tc, channel, TC_Configure(tc, channel,
TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32 TC_CMR_WAVE // Waveform mode
TC_CMR_WAVE | // Waveform mode | TC_CMR_WAVSEL_UP_RC // Counter running up and reset when equal to RC
TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals 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 */ // Wait 1ms before the first ISR
TC_SetRA(tc, channel, 2625); // 1ms TC_SetRA(tc, channel, (F_CPU) / (SERVO_TIMER_PRESCALER) / 1000UL); // 1ms
/* Configure and enable interrupt */ // Configure and enable interrupt
NVIC_EnableIRQ(irqn); NVIC_EnableIRQ(irqn);
// TC_IER_CPAS: RA Compare tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS; // TC_IER_CPAS: RA Compare
tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS;
// Enables the timer clock and performs a software reset to start the counting // Enables the timer clock and performs a software reset to start the counting
TC_Start(tc, channel); TC_Start(tc, channel);
} }
void initISR(const timer16_Sequence_t timer) { void initISR(const timer16_Sequence_t timer_index) {
#ifdef _useTimer1 CRITICAL_SECTION_START();
if (timer == _timer1) _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); const bool disable_soon = DisablePending[timer_index];
#endif DisablePending.clear(timer_index);
#ifdef _useTimer2 CRITICAL_SECTION_END();
if (timer == _timer2) _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
#endif if (!disable_soon) switch (timer_index) {
#ifdef _useTimer3 default: break;
if (timer == _timer3) _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); #ifdef _useTimer1
#endif case _timer1: return _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
#ifdef _useTimer4 #endif
if (timer == _timer4) _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); #ifdef _useTimer2
#endif case _timer2: return _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
#ifdef _useTimer5 #endif
if (timer == _timer5) _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); #ifdef _useTimer3
#endif 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) { void finISR(const timer16_Sequence_t timer_index) {
#ifdef _useTimer1 // Timer is disabled from the ISR, to ensure proper final pulse length.
TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); DisablePending.set(timer_index);
#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
} }
#endif // HAS_SERVOS #endif // HAS_SERVOS

2
Marlin/src/HAL/DUE/ServoTimers.h
View File

@ -37,7 +37,7 @@
#define _useTimer5 #define _useTimer5
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays #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 TC0, chan 0 => TC0_Handler

11
Marlin/src/HAL/DUE/timers.cpp
View File

@ -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); NVIC_SetPriority(irq, timer_config[timer_num].priority);
// wave mode, reset counter on match with RC, // 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 // Set compare value
TC_SetRC(tc, channel, VARIANT_MCK / 2 / frequency); TC_SetRC(tc, channel, VARIANT_MCK / (HAL_TIMER_PRESCALER) / frequency);
// And start timer // And start timer
TC_Start(tc, channel); TC_Start(tc, channel);

3
Marlin/src/HAL/DUE/timers.h
View File

@ -35,7 +35,8 @@
typedef uint32_t hal_timer_t; typedef uint32_t hal_timer_t;
#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF #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 #ifndef MF_TIMER_STEP
#define MF_TIMER_STEP 2 // Timer Index for Stepper #define MF_TIMER_STEP 2 // Timer Index for Stepper

43
Marlin/src/HAL/SAMD51/Servo.cpp
View File

@ -77,7 +77,8 @@ HAL_SERVO_TIMER_ISR() {
; ;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer); 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 defined(_useTimer1) && defined(_useTimer2)
if (currentServoIndex[timer ^ 1] >= 0) { if (currentServoIndex[timer ^ 1] >= 0) {
// Wait for both channels // Wait for both channels
@ -86,41 +87,33 @@ HAL_SERVO_TIMER_ISR() {
return; return;
} }
#endif #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); SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT);
} }
else if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive) else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
// Select the next servo controlled by this timer currentServoIndex[timer] = ++cho; // go to the next channel (or 0)
currentServoIndex[timer]++; 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) { tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, cho).ticks;
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;
} }
else { else {
// finished all channels so wait for the refresh period to expire before starting over // 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 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
const uint16_t tcCounterValue = getTimerCount(); 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 ((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
} }
if (tcChannel == 0) { if (tcChannel == 0) {
SYNC(tc->COUNT16.SYNCBUSY.bit.CC0); SYNC(tc->COUNT16.SYNCBUSY.bit.CC0);
// Clear the interrupt tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0; // Clear the interrupt
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0;
} }
else { else {
SYNC(tc->COUNT16.SYNCBUSY.bit.CC1); SYNC(tc->COUNT16.SYNCBUSY.bit.CC1);
// Clear the interrupt tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1; // Clear the interrupt
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1;
} }
} }
@ -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; 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 // Disable the match channel interrupt request
tc->COUNT16.INTENCLR.reg = (tcChannel == 0) ? TC_INTENCLR_MC0 : TC_INTENCLR_MC1; tc->COUNT16.INTENCLR.reg = (tcChannel == 0) ? TC_INTENCLR_MC0 : TC_INTENCLR_MC1;

4
Marlin/src/HAL/shared/servo_private.h
View File

@ -94,5 +94,5 @@ extern ServoInfo_t servo_info[MAX_SERVOS];
// Public functions // Public functions
extern void initISR(const timer16_Sequence_t timer); void initISR(const timer16_Sequence_t timer_index);
extern void finISR(const timer16_Sequence_t timer); void finISR(const timer16_Sequence_t timer_index);

4
Marlin/src/core/serial.cpp
View File

@ -72,8 +72,8 @@ void serial_print_P(PGM_P str) {
while (const char c = pgm_read_byte(str++)) SERIAL_CHAR(c); 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_echo_start() { serial_print(F("echo:")); }
void serial_error_start() { static PGMSTR(errormagic, "Error:"); serial_print_P(errormagic); } void serial_error_start() { serial_print(F("Error:")); }
void serial_spaces(uint8_t count) { count *= (PROPORTIONAL_FONT_RATIO); while (count--) SERIAL_CHAR(' '); } void serial_spaces(uint8_t count) { count *= (PROPORTIONAL_FONT_RATIO); while (count--) SERIAL_CHAR(' '); }

8
Marlin/src/core/types.h
View File

@ -99,8 +99,8 @@ struct Flags {
void set(const int n) { b |= (bits_t)_BV(n); } void set(const int n) { b |= (bits_t)_BV(n); }
void clear(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 test(const int n) const { return TEST(b, n); }
bool operator[](const int n) { return test(n); } const bool operator[](const int n) { return test(n); }
bool operator[](const int n) const { return test(n); } const bool operator[](const int n) const { return test(n); }
int size() const { return sizeof(b); } int size() const { return sizeof(b); }
}; };
@ -113,8 +113,8 @@ struct Flags<1> {
void set(const int) { b = true; } void set(const int) { b = true; }
void clear(const int) { b = false; } void clear(const int) { b = false; }
bool test(const int) const { return b; } bool test(const int) const { return b; }
bool operator[](const int) { return b; } bool& operator[](const int) { return b; }
bool operator[](const int) const { return b; } bool operator[](const int) const { return b; }
int size() const { return sizeof(b); } int size() const { return sizeof(b); }
}; };