From 00527f38d59d8b466d515da801844197073c7bcc Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 9 Jun 2022 00:51:47 -0500 Subject: [PATCH] =?UTF-8?q?=F0=9F=A7=91=E2=80=8D=F0=9F=92=BB=20Misc.=20ser?= =?UTF-8?q?vo=20code=20cleanup?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/HAL/AVR/Servo.cpp | 2 +- Marlin/src/HAL/DUE/Servo.cpp | 21 ++++++++------------- Marlin/src/HAL/SAMD51/Servo.cpp | 4 ++-- Marlin/src/HAL/STM32F1/Servo.cpp | 16 ++++++++-------- Marlin/src/HAL/shared/servo.cpp | 13 +++++++------ Marlin/src/HAL/shared/servo_private.h | 12 ++++++------ 6 files changed, 32 insertions(+), 36 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index 526352b773..6dc1e2fb07 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) { if (Channel[timer] < 0) *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer else { diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 5524aa9cef..72a7d649ef 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, Tc*, const uint8_t); #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) { // clear interrupt tc->TC_CHANNEL[channel].TC_SR; if (Channel[timer] < 0) @@ -113,26 +113,21 @@ 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 } 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..b838800de6 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) @@ -101,17 +101,18 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { max = (MAX_PULSE_WIDTH - inMax) / 4; // 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 + const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + 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); + //pinMode(servo_info[servoIndex].Pin.nbr, INPUT); // set servo pin to input } 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);