191 lines
7.6 KiB
C
191 lines
7.6 KiB
C
#include "zh_avr_encoder.h"
|
|
|
|
#define ENCODER_DIRECTION_CW 0x10
|
|
#define ENCODER_DIRECTION_CCW 0x20
|
|
|
|
static const uint8_t _encoder_matrix[7][4] PROGMEM = {
|
|
{0x03, 0x02, 0x01, 0x00},
|
|
{0x23, 0x00, 0x01, 0x00},
|
|
{0x13, 0x02, 0x00, 0x00},
|
|
{0x03, 0x05, 0x04, 0x00},
|
|
{0x03, 0x03, 0x04, 0x00},
|
|
{0x03, 0x05, 0x03, 0x00},
|
|
};
|
|
|
|
TaskHandle_t zh_avr_encoder = NULL;
|
|
static QueueHandle_t _queue_handle = NULL;
|
|
static bool _is_initialized = false;
|
|
|
|
static avr_err_t _zh_avr_encoder_validate_config(const zh_avr_encoder_init_config_t *config);
|
|
static avr_err_t _zh_avr_encoder_configure_interrupts(const zh_avr_encoder_init_config_t *config, zh_avr_encoder_handle_t *handle);
|
|
static void _zh_avr_encoder_isr_processing_task(void *pvParameter);
|
|
|
|
avr_err_t zh_avr_encoder_init(const zh_avr_encoder_init_config_t *config, zh_avr_encoder_handle_t *handle)
|
|
{
|
|
avr_err_t err = _zh_avr_encoder_validate_config(config);
|
|
ZH_ERROR_CHECK(err == AVR_OK, err);
|
|
handle->encoder_number = config->encoder_number;
|
|
handle->encoder_min_value = config->encoder_min_value;
|
|
handle->encoder_max_value = config->encoder_max_value;
|
|
handle->encoder_step = config->encoder_step;
|
|
handle->encoder_position = (handle->encoder_min_value + handle->encoder_max_value) / 2;
|
|
handle->gpio_port = config->gpio_port;
|
|
handle->a_gpio_number = config->a_gpio_number;
|
|
handle->b_gpio_number = config->b_gpio_number;
|
|
err = _zh_avr_encoder_configure_interrupts(config, handle);
|
|
ZH_ERROR_CHECK(err == AVR_OK, err);
|
|
handle->is_initialized = true;
|
|
_is_initialized = true;
|
|
return AVR_OK;
|
|
}
|
|
|
|
avr_err_t zh_avr_encoder_set(zh_avr_encoder_handle_t *handle, double position)
|
|
{
|
|
ZH_ERROR_CHECK(handle->is_initialized == true, AVR_FAIL);
|
|
ZH_ERROR_CHECK(position <= handle->encoder_max_value && position >= handle->encoder_min_value, AVR_ERR_INVALID_ARG);
|
|
handle->encoder_position = position;
|
|
return AVR_OK;
|
|
}
|
|
|
|
avr_err_t zh_avr_encoder_get(const zh_avr_encoder_handle_t *handle, double *position)
|
|
{
|
|
ZH_ERROR_CHECK(handle->is_initialized == true, AVR_FAIL);
|
|
*position = handle->encoder_position;
|
|
return AVR_OK;
|
|
}
|
|
|
|
avr_err_t zh_avr_encoder_reset(zh_avr_encoder_handle_t *handle)
|
|
{
|
|
ZH_ERROR_CHECK(handle->is_initialized == true, AVR_FAIL);
|
|
handle->encoder_position = (handle->encoder_min_value + handle->encoder_max_value) / 2;
|
|
return AVR_OK;
|
|
}
|
|
|
|
static avr_err_t _zh_avr_encoder_validate_config(const zh_avr_encoder_init_config_t *config)
|
|
{
|
|
ZH_ERROR_CHECK(config != NULL, AVR_ERR_INVALID_ARG);
|
|
ZH_ERROR_CHECK(config->task_priority > tskIDLE_PRIORITY && config->stack_size >= 124, AVR_ERR_INVALID_ARG);
|
|
ZH_ERROR_CHECK(config->queue_size > 0, AVR_ERR_INVALID_ARG);
|
|
ZH_ERROR_CHECK(config->encoder_max_value > config->encoder_min_value, AVR_ERR_INVALID_ARG);
|
|
ZH_ERROR_CHECK(config->encoder_step > 0, AVR_ERR_INVALID_ARG);
|
|
ZH_ERROR_CHECK(config->gpio_port >= AVR_PORTB && config->gpio_port <= AVR_PORTD, AVR_ERR_INVALID_ARG);
|
|
ZH_ERROR_CHECK(config->a_gpio_number >= 0 && config->a_gpio_number <= 7, AVR_ERR_INVALID_ARG);
|
|
ZH_ERROR_CHECK(config->b_gpio_number >= 0 && config->b_gpio_number <= 7, AVR_ERR_INVALID_ARG);
|
|
ZH_ERROR_CHECK(config->a_gpio_number != config->b_gpio_number, AVR_ERR_INVALID_ARG);
|
|
return AVR_OK;
|
|
}
|
|
|
|
static avr_err_t _zh_avr_encoder_configure_interrupts(const zh_avr_encoder_init_config_t *config, zh_avr_encoder_handle_t *handle)
|
|
{
|
|
switch (config->gpio_port)
|
|
{
|
|
case AVR_PORTB:
|
|
DDRB &= ~((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
if (config->pullup == true)
|
|
{
|
|
PORTB |= ((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
}
|
|
PCICR |= (1 << PCIE0);
|
|
PCMSK0 |= ((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
break;
|
|
case AVR_PORTC:
|
|
DDRC &= ~((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
if (config->pullup == true)
|
|
{
|
|
PORTC |= ((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
}
|
|
PCICR |= (1 << PCIE1);
|
|
PCMSK1 |= ((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
break;
|
|
case AVR_PORTD:
|
|
DDRD &= ~((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
if (config->pullup == true)
|
|
{
|
|
PORTD |= ((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
}
|
|
PCICR |= (1 << PCIE2);
|
|
PCMSK2 |= ((1 << config->a_gpio_number) | (1 << config->b_gpio_number));
|
|
break;
|
|
default:
|
|
return AVR_ERR_INVALID_ARG;
|
|
break;
|
|
}
|
|
if (_is_initialized == false)
|
|
{
|
|
_queue_handle = xQueueCreate(config->queue_size, sizeof(zh_avr_encoder_handle_t));
|
|
ZH_ERROR_CHECK(_queue_handle != NULL, AVR_ERR_NO_MEM);
|
|
BaseType_t x_err = xTaskCreate(_zh_avr_encoder_isr_processing_task, "zh_avr_encoder", config->stack_size, NULL, config->task_priority, &zh_avr_encoder);
|
|
if (x_err != pdPASS)
|
|
{
|
|
vQueueDelete(_queue_handle);
|
|
return AVR_FAIL;
|
|
}
|
|
}
|
|
return AVR_OK;
|
|
}
|
|
|
|
BaseType_t zh_avr_encoder_isr_handler(zh_avr_encoder_handle_t *handle)
|
|
{
|
|
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
|
uint8_t temp = 0;
|
|
switch (handle->gpio_port)
|
|
{
|
|
case AVR_PORTB:
|
|
temp = pgm_read_byte(&_encoder_matrix[handle->encoder_state & 0x0F][(((PINB & (1 << handle->b_gpio_number)) == 0 ? 0 : 1) << 1) | ((PINB & (1 << handle->a_gpio_number)) == 0 ? 0 : 1)]);
|
|
break;
|
|
case AVR_PORTC:
|
|
temp = pgm_read_byte(&_encoder_matrix[handle->encoder_state & 0x0F][(((PINC & (1 << handle->b_gpio_number)) == 0 ? 0 : 1) << 1) | ((PINC & (1 << handle->a_gpio_number)) == 0 ? 0 : 1)]);
|
|
break;
|
|
case AVR_PORTD:
|
|
temp = pgm_read_byte(&_encoder_matrix[handle->encoder_state & 0x0F][(((PIND & (1 << handle->b_gpio_number)) == 0 ? 0 : 1) << 1) | ((PIND & (1 << handle->a_gpio_number)) == 0 ? 0 : 1)]);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
if (temp != handle->encoder_state)
|
|
{
|
|
handle->encoder_state = temp;
|
|
switch (handle->encoder_state & 0x30)
|
|
{
|
|
case ENCODER_DIRECTION_CW:
|
|
if (handle->encoder_position < handle->encoder_max_value)
|
|
{
|
|
handle->encoder_position = handle->encoder_position + handle->encoder_step;
|
|
if (handle->encoder_position > handle->encoder_max_value)
|
|
{
|
|
handle->encoder_position = handle->encoder_max_value;
|
|
}
|
|
xQueueSendFromISR(_queue_handle, handle, &xHigherPriorityTaskWoken);
|
|
}
|
|
break;
|
|
case ENCODER_DIRECTION_CCW:
|
|
if (handle->encoder_position > handle->encoder_min_value)
|
|
{
|
|
handle->encoder_position = handle->encoder_position - handle->encoder_step;
|
|
if (handle->encoder_position < handle->encoder_min_value)
|
|
{
|
|
handle->encoder_position = handle->encoder_min_value;
|
|
}
|
|
xQueueSendFromISR(_queue_handle, handle, &xHigherPriorityTaskWoken);
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
return xHigherPriorityTaskWoken;
|
|
}
|
|
|
|
static void _zh_avr_encoder_isr_processing_task(void *pvParameter)
|
|
{
|
|
zh_avr_encoder_handle_t queue = {0};
|
|
zh_avr_encoder_event_on_isr_t event = {0};
|
|
while (xQueueReceive(_queue_handle, &queue, portMAX_DELAY) == pdTRUE)
|
|
{
|
|
event.encoder_number = queue.encoder_number;
|
|
event.encoder_position = queue.encoder_position;
|
|
extern void zh_avr_encoder_event_handler(zh_avr_encoder_event_on_isr_t * event);
|
|
zh_avr_encoder_event_handler(&event);
|
|
}
|
|
vTaskDelete(NULL);
|
|
} |