🧑‍💻 Misc. servo code cleanup

This commit is contained in:
Scott Lahteine 2022-06-09 00:51:47 -05:00 committed by Scott Lahteine
parent ea22640d78
commit 1156557a47
6 changed files with 32 additions and 36 deletions

View File

@ -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 {

View File

@ -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
}

View File

@ -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);

View File

@ -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
}
}

View File

@ -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) {

View File

@ -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);