diff --git a/cores/arduino/trap_handler.c b/cores/arduino/trap_handler.c index af637d1..b491eae 100644 --- a/cores/arduino/trap_handler.c +++ b/cores/arduino/trap_handler.c @@ -9,7 +9,7 @@ void __attribute__((weak)) wire_interrupt_handler(void) { // dummy function for case when wire library is not in use } -void __attribute__((weak)) servo_handler_wrapper(void) +void __attribute__((weak)) servo_interrupt_handler(void) { // dummy function for case when wire library is not in use } @@ -27,7 +27,7 @@ void __attribute__((noinline, section(".ram_text"), optimize("O3"))) trap_handle // servo timer interrupt if (EPIC_CHECK_TIMER16_2()) - servo_handler_wrapper(); + servo_interrupt_handler(); // uart0 interrupt if (EPIC_CHECK_UART_0()) diff --git a/cores/arduino/wiring_LL.h b/cores/arduino/wiring_LL.h index 3649ee9..4e2723e 100644 --- a/cores/arduino/wiring_LL.h +++ b/cores/arduino/wiring_LL.h @@ -19,39 +19,40 @@ // ----------------- SYSTICK ----------------- // // get ticks from systick -#define SYSTICK_GET_TICKS() ((uint64_t)(SCR1_TIMER->MTIMEH)<<32 | SCR1_TIMER->MTIME) +#define SYSTICK_GET_TICKS() ((uint64_t)(SCR1_TIMER->MTIMEH)<<32 | SCR1_TIMER->MTIME) // ----------------- UART ----------------- // -#define UART_READ_BYTE(pUart) ((uint16_t)pUart->RXDATA) -#define UART_IS_RX_FIFO_EMPTY(pUart) ((pUart->FLAGS & UART_FLAGS_RXNE_M) == 0) +#define UART_READ_BYTE(pUart) ((uint16_t)pUart->RXDATA) +#define UART_IS_RX_FIFO_EMPTY(pUart) ((pUart->FLAGS & UART_FLAGS_RXNE_M) == 0) // ----------------- TIMER16 ----------------- // -#define TIM16_DISABLE(htim16) (htim16.Instance->CR &= ~TIMER16_CR_ENABLE_M) -#define TIM16_CLEAR_INT_MASK(htim16, intMask) (htim16.Instance->ICR = intMask) -#define TIM16_GET_ARRM_INT_STATUS(htim16) ((bool)((htim16.Instance->ISR & htim16.Instance->IER) & TIMER16_ISR_ARR_MATCH_M)) +#define TIM16_DISABLE(htim16) (htim16.Instance->CR &= ~TIMER16_CR_ENABLE_M) +#define TIM16_CLEAR_INT_MASK(htim16, intMask) (htim16.Instance->ICR = intMask) +#define TIM16_GET_ARRM_INT_STATUS(htim16) ((bool)((htim16.Instance->ISR & htim16.Instance->IER) & TIMER16_ISR_ARR_MATCH_M)) +#define TIM16_DISABLE_INT_BY_MASK(htim16,intMask) (htim16.Instance->IER &= ~(intMask)) // ----------------- EPIC ----------------- // -#define EPIC_LEVEL_CLEAR_BY_MASK(mask) (EPIC->MASK_LEVEL_CLEAR |= mask) -#define EPIC_CLEAR_ALL() (EPIC->CLEAR = 0xFFFFFFFF) +#define EPIC_LEVEL_CLEAR_BY_MASK(mask) (EPIC->MASK_LEVEL_CLEAR |= mask) +#define EPIC_CLEAR_ALL() (EPIC->CLEAR = 0xFFFFFFFF) // ----------------- IRQ ----------------- // -#define GLOBAL_IRQ_DISABLE() (clear_csr(mie, MIE_MEIE)) -#define GLOBAL_IRQ_ENABLE() set_csr(mstatus, MSTATUS_MIE); \ - set_csr(mie, MIE_MEIE) +#define GLOBAL_IRQ_DISABLE() (clear_csr(mie, MIE_MEIE)) +#define GLOBAL_IRQ_ENABLE() set_csr(mstatus, MSTATUS_MIE); \ + set_csr(mie, MIE_MEIE) // ----------------- GPIO ----------------- // -#define GPIO_SET_PIN(GPIO_x, pinMask) ((GPIO_x)->SET = (pinMask)) -#define GPIO_CLEAR_PIN(GPIO_x, pinMask) ((GPIO_x)->CLEAR = (pinMask)) -#define GPIO_TOGGLE_PIN(GPIO_x, pinMask) ((GPIO_x)->OUTPUT_ ^= pinMask) -#define GPIO_READ_PIN(GPIO_x, pinMask) (((GPIO_x)->SET & (pinMask)) != (uint32_t)GPIO_PIN_LOW ? GPIO_PIN_HIGH : GPIO_PIN_LOW) +#define GPIO_SET_PIN(GPIO_x, pinMask) ((GPIO_x)->SET = (pinMask)) +#define GPIO_CLEAR_PIN(GPIO_x, pinMask) ((GPIO_x)->CLEAR = (pinMask)) +#define GPIO_TOGGLE_PIN(GPIO_x, pinMask) ((GPIO_x)->OUTPUT_ ^= pinMask) +#define GPIO_READ_PIN(GPIO_x, pinMask) (((GPIO_x)->SET & (pinMask)) != (uint32_t)GPIO_PIN_LOW ? GPIO_PIN_HIGH : GPIO_PIN_LOW) // get pin state by it's number -#define GPIO_GET_PIN_STATE(GPIO_x, pinNumber) (((GPIO_x)->OUTPUT_ >> pinNumber) & 0b1) +#define GPIO_GET_PIN_STATE(GPIO_x, pinNumber) (((GPIO_x)->OUTPUT_ >> pinNumber) & 0b1) // ----------------- GPIO IRQ ----------------- // -#define GPIO_IRQ_LINE_ENABLE(lineMask) ( GPIO_IRQ->ENABLE_SET = (1 << (lineMask >> GPIO_IRQ_LINE_S)) ) -#define GPIO_IRQ_LINE_DISABLE(lineMask) ( GPIO_IRQ->ENABLE_CLEAR = (1 << (lineMask >> GPIO_IRQ_LINE_S)) ) -#define GPIO_IRQ_LINE_STATE(lineMask) ((GPIO_IRQ->INTERRUPT & (1 << (lineMask >> GPIO_IRQ_LINE_S))) != 0) -#define GPIO_IRQ_CLEAR_ALL() ( GPIO_IRQ->CLEAR = 0b11111111) +#define GPIO_IRQ_LINE_ENABLE(lineMask) ( GPIO_IRQ->ENABLE_SET = (1 << (lineMask >> GPIO_IRQ_LINE_S)) ) +#define GPIO_IRQ_LINE_DISABLE(lineMask) ( GPIO_IRQ->ENABLE_CLEAR = (1 << (lineMask >> GPIO_IRQ_LINE_S)) ) +#define GPIO_IRQ_LINE_STATE(lineMask) ((GPIO_IRQ->INTERRUPT & (1 << (lineMask >> GPIO_IRQ_LINE_S))) != 0) +#define GPIO_IRQ_CLEAR_ALL() ( GPIO_IRQ->CLEAR = 0b11111111) // ----------------- PIN CONFIG ----------------- // // return config of pin with pinNumber(0...16) in portReg (config, pupd, ds for ports 0...2) diff --git a/libraries/Servo/src/Servo.cpp b/libraries/Servo/src/Servo.cpp index f00a727..1911da0 100644 --- a/libraries/Servo/src/Servo.cpp +++ b/libraries/Servo/src/Servo.cpp @@ -4,6 +4,7 @@ #include "mik32_hal_timer16.h" #include "mik32_hal_irq.h" +#include "wiring_LL.h" #include "Servo.h" @@ -19,16 +20,16 @@ uint8_t prescaler = TIMER16_PRESCALER_16; #define usToTicks(_us) ((clockCyclesPerMicrosecond() * _us) / ( (1 << prescaler) )) // converts microseconds to ticks (TIMERx_PRESCALER_16) #define ticksToUs(_ticks) (((unsigned) _ticks * ( (1 << prescaler) )) / clockCyclesPerMicrosecond()) // converts from ticks back to microseconds -#define TRIM_DURATION 5 // compensation ticks to trim adjust for digitalWrite delays +#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in us for this servo +#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in us for this servo + +#define TRIM_DURATION 5 // compensation ticks to trim adjust for digitalWrite delays // 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) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel - -#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in us for this servo -#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in us for this servo +#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) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel // timer handler Timer16_HandleTypeDef htimer16; @@ -58,9 +59,9 @@ void Timer16_2_Init() /************ static functions common to all instances ***********************/ // irq handler -extern "C" void servo_handler_wrapper(void) +extern "C" void __attribute__((optimize("O3"))) servo_interrupt_handler(void) { - if ((htimer16.Instance->ISR & htimer16.Instance->IER) & TIMER16_ISR_ARR_MATCH_M) + if (TIM16_GET_ARRM_INT_STATUS(htimer16)) { timer16_Sequence_t timer = _timer1; @@ -89,7 +90,7 @@ extern "C" void servo_handler_wrapper(void) } // reset timer interrupt flags - htimer16.Instance->ICR = 0xFFFFFFFF; + TIM16_CLEAR_INT_MASK(htimer16, 0xFFFFFFFF); } // init isr @@ -108,11 +109,9 @@ static void finISR(timer16_Sequence_t timer) { if (timer == _timer1) { - HAL_Timer16_Disable(&htimer16); - htimer16.Instance->IER &= ~(TIMER16_IER_ARROKIE_M | TIMER16_IER_CMPOKIE_M - | TIMER16_IER_ARRMIE_M | TIMER16_IER_CMPMIE_M); - htimer16.Instance->CR &= ~TIMER16_CR_ENABLE_M; // disable timer - HAL_EPIC_MaskLevelClear(HAL_EPIC_TIMER16_1_MASK); + TIM16_DISABLE(htimer16); + TIM16_DISABLE_INT_BY_MASK(htimer16, TIMER16_IER_ARROKIE_M | TIMER16_IER_CMPOKIE_M | TIMER16_IER_ARRMIE_M | TIMER16_IER_CMPMIE_M); + EPIC_LEVEL_CLEAR_BY_MASK(HAL_EPIC_TIMER16_1_MASK); } }