jessikitty/Marlin
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix DUE timer behavior when disabling servos

Browse Source
This commit is contained in:
Jason Smith
2022-06-12 14:36:49 -07:00
parent ad4cf3e332
commit 9d4aff6eab
1 changed files with 39 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Marlin/src/HAL/DUE/Servo.cpp
+39 -34
View File
@@ -47,7 +47,8 @@
#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 int8_t Channel[_Nbr_16timers]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
static bool DisablePending[_Nbr_16timers]; // Instructs ISR to disable the timer at the next timer reset
// ------------------------
/// Interrupt handler for the TC0 channel 1.
@@ -71,10 +72,18 @@ 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) {
tc->TC_CHANNEL[channel].TC_SR; // clear interrupt
int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
if (cho < 0) // Channel -1 indicates the refresh interval completed...
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[timer] = false;
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
@@ -92,6 +101,8 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t 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) {
@@ -117,39 +128,33 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) {
}
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
CRITICAL_SECTION_START();
bool needsInit = !DisablePending[timer];
DisablePending[timer] = false;
CRITICAL_SECTION_END();
if (needsInit) {
#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 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(timer16_Sequence_t timer) {
// Timer is disabled from the ISR, to ensure proper final pulse length.
DisablePending[timer] = true;
}
#endif // HAS_SERVOS