🐛 Resolve DUE Servo pulse issue (#24305)
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
committed by
Scott Lahteine
Scott Lahteine
parent
0523874e9c
commit
0dc59311ec
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
Reference in New Issue
Block a user