From 86b9e7ee1324b504a11452297031c85730700e2b Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 7 Jun 2022 17:37:47 -0500 Subject: [PATCH] servo cleanup --- Marlin/src/HAL/AVR/Servo.cpp | 20 ++++++++-------- Marlin/src/HAL/DUE/Servo.cpp | 33 +++++++++++---------------- Marlin/src/HAL/SAMD51/Servo.cpp | 4 ++-- Marlin/src/HAL/STM32F1/Servo.cpp | 16 ++++++------- Marlin/src/HAL/shared/servo.cpp | 10 ++++---- Marlin/src/HAL/shared/servo_private.h | 12 +++++----- 6 files changed, 44 insertions(+), 51 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index f5359b394b..379eb08215 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -66,7 +66,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s /************ static functions common to all instances ***********************/ -static inline void handle_interrupts(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) { const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer @@ -81,11 +81,9 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t } else { // Finished all channels so wait for the refresh period to expire before starting over - const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL); - if (((unsigned)*TCNTn) + 4 < iticks) // allow a few ticks to ensure the next OCR1A not missed - *OCRnA = iticks; - else - *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed + const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL), // At least the refresh interval has elapsed + tctick = ((unsigned)*TCNTn) + 4; // allow a few ticks to ensure the next OCR1A not missed + *OCRnA = max(tctick, iticks); Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel } } @@ -123,7 +121,7 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t /****************** end of static functions ******************************/ -void initISR(timer16_Sequence_t timer) { +void initISR(const timer16_Sequence_t timer) { #ifdef _useTimer1 if (timer == _timer1) { TCCR1A = 0; // normal counting mode @@ -182,7 +180,7 @@ void initISR(timer16_Sequence_t timer) { #endif } -void finISR(timer16_Sequence_t timer) { +void finISR(const timer16_Sequence_t timer) { // Disable use of the given timer #ifdef WIRING if (timer == _timer1) { @@ -192,7 +190,8 @@ void finISR(timer16_Sequence_t timer) { #else TIMSK #endif - , OCIE1A); // disable timer 1 output compare interrupt + , OCIE1A // disable timer 1 output compare interrupt + ); timerDetach(TIMER1OUTCOMPAREA_INT); } else if (timer == _timer3) { @@ -202,7 +201,8 @@ void finISR(timer16_Sequence_t timer) { #else ETIMSK #endif - , OCIE3A); // disable the timer3 output compare A interrupt + , OCIE3A // disable the timer3 output compare A interrupt + ); timerDetach(TIMER3OUTCOMPAREA_INT); } #else // !WIRING diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 5d75eb6985..69e2cc1b2b 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -52,7 +52,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s // ------------------------ /// Interrupt handler for the TC0 channel 1. // ------------------------ -void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); +void Servo_Handler(const timer16_Sequence_t timer, Tc *pTc, const uint8_t channel); #ifdef _useTimer1 void HANDLER_FOR_TIMER1() { Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); } @@ -70,7 +70,7 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); void HANDLER_FOR_TIMER5() { Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); } #endif -void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { +void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) { const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; 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 @@ -82,16 +82,14 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { Channel[timer]++; // increment to the next channel if (good_servo && 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 + 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 } else { // finished all channels so wait for the refresh period to expire before starting over - const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL); - tc->TC_CHANNEL[channel].TC_RA = - tc->TC_CHANNEL[channel].TC_CV + 4 < iticks - ? iticks // allow a few ticks to ensure the next OCR1A not missed - : tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed + const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL), // At least the refresh interval has elapsed + tctick = tc->TC_CHANNEL[channel].TC_CV + 4; // allow a few ticks to ensure the next OCR1A not missed + tc->TC_CHANNEL[channel].TC_RA = max(tctick, iticks); Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel } @@ -116,30 +114,25 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { TC_Start(tc, channel); } -void initISR(timer16_Sequence_t timer) { +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); + 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); + 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); + 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); + 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); + if (timer == _timer5) _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) { #ifdef _useTimer1 TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); #endif diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp index 23ab21c615..04461653f9 100644 --- a/Marlin/src/HAL/SAMD51/Servo.cpp +++ b/Marlin/src/HAL/SAMD51/Servo.cpp @@ -124,7 +124,7 @@ HAL_SERVO_TIMER_ISR() { } } -void initISR(timer16_Sequence_t timer) { +void initISR(const timer16_Sequence_t timer) { Tc * const tc = timer_config[SERVO_TC].pTc; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); @@ -201,7 +201,7 @@ void initISR(timer16_Sequence_t timer) { } } -void finISR(timer16_Sequence_t timer) { +void finISR(const timer16_Sequence_t timer) { Tc * const tc = timer_config[SERVO_TC].pTc; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); diff --git a/Marlin/src/HAL/STM32F1/Servo.cpp b/Marlin/src/HAL/STM32F1/Servo.cpp index 8dc1ef7b6a..47ffb631cf 100644 --- a/Marlin/src/HAL/STM32F1/Servo.cpp +++ b/Marlin/src/HAL/STM32F1/Servo.cpp @@ -147,17 +147,17 @@ void libServo::move(const int32_t value) { uint16_t SR = timer_get_status(tdev); if (SR & TIMER_SR_CC1IF) { // channel 1 off #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(SERVO0_PIN, 1); // off + OUT_WRITE_OD(SERVO0_PIN, HIGH); // off #else - OUT_WRITE(SERVO0_PIN, 0); + OUT_WRITE(SERVO0_PIN, LOW); #endif timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT); } if (SR & TIMER_SR_CC2IF) { // channel 2 resume #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(SERVO0_PIN, 0); // on + OUT_WRITE_OD(SERVO0_PIN, LOW); // on #else - OUT_WRITE(SERVO0_PIN, 1); + OUT_WRITE(SERVO0_PIN, HIGH); #endif timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT); } @@ -167,9 +167,9 @@ void libServo::move(const int32_t value) { timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); if (!tdev) return false; #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(inPin, 1); + OUT_WRITE_OD(inPin, HIGH); #else - OUT_WRITE(inPin, 0); + OUT_WRITE(inPin, LOW); #endif timer_pause(tdev); @@ -200,9 +200,9 @@ void libServo::move(const int32_t value) { timer_disable_irq(tdev, 1); timer_disable_irq(tdev, 2); #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(pin, 1); // off + OUT_WRITE_OD(pin, HIGH); // off #else - OUT_WRITE(pin, 0); + OUT_WRITE(pin, LOW); #endif } } diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp index cfec6f3017..567ff81598 100644 --- a/Marlin/src/HAL/shared/servo.cpp +++ b/Marlin/src/HAL/shared/servo.cpp @@ -65,7 +65,7 @@ uint8_t ServoCount = 0; // the total number of attached /************ static functions common to all instances ***********************/ -static boolean isTimerActive(timer16_Sequence_t timer) { +static bool anyTimerChannelActive(const timer16_Sequence_t timer) { // returns true if any servo is active on this timer LOOP_L_N(channel, SERVOS_PER_TIMER) { if (SERVO(timer, channel).Pin.isActive) @@ -102,16 +102,16 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { // initialize the timer if it has not already been initialized timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if (!isTimerActive(timer)) initISR(timer); - servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive + if (!anyTimerChannelActive(timer)) initISR(timer); + servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for anyTimerChannelActive return servoIndex; } void Servo::detach() { servo_info[servoIndex].Pin.isActive = false; - timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if (!isTimerActive(timer)) finISR(timer); + const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (anyTimerChannelActive(timer)) finISR(timer); } void Servo::write(int value) { diff --git a/Marlin/src/HAL/shared/servo_private.h b/Marlin/src/HAL/shared/servo_private.h index d85d8da8ba..021e0cb81d 100644 --- a/Marlin/src/HAL/shared/servo_private.h +++ b/Marlin/src/HAL/shared/servo_private.h @@ -70,10 +70,10 @@ #define ticksToUs(_ticks) (unsigned(_ticks) * (SERVO_TIMER_PRESCALER) / clockCyclesPerMicrosecond()) // convenience macros -#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo -#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // returns the index of the servo on this timer -#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // macro to access servo index by timer and channel -#define SERVO(_timer,_channel) (servo_info[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel +#define SERVO_INDEX_TO_TIMER(_servo_nbr) timer16_Sequence_t(_servo_nbr / (SERVOS_PER_TIMER)) // the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // servo index by timer and channel +#define SERVO(_timer,_channel) servo_info[SERVO_INDEX(_timer,_channel)] // servo class by timer and channel // Types @@ -94,5 +94,5 @@ extern ServoInfo_t servo_info[MAX_SERVOS]; // Public functions -extern void initISR(timer16_Sequence_t timer); -extern void finISR(timer16_Sequence_t timer); +extern void initISR(const timer16_Sequence_t timer); +extern void finISR(const timer16_Sequence_t timer);