From 3cdd1360d4e71edabad003f7652a0abc0b1aaad1 Mon Sep 17 00:00:00 2001 From: Stephane D'Alu Date: Tue, 28 Jun 2016 21:53:12 +0200 Subject: QEI driver for NRF51 --- os/hal/ports/NRF51/NRF51822/hal_qei_lld.c | 303 +++++++++++++++++++++++++++ os/hal/ports/NRF51/NRF51822/hal_qei_lld.h | 334 ++++++++++++++++++++++++++++++ os/hal/ports/NRF51/NRF51822/platform.mk | 6 +- 3 files changed, 642 insertions(+), 1 deletion(-) create mode 100644 os/hal/ports/NRF51/NRF51822/hal_qei_lld.c create mode 100644 os/hal/ports/NRF51/NRF51822/hal_qei_lld.h diff --git a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c new file mode 100644 index 0000000..0f8043d --- /dev/null +++ b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c @@ -0,0 +1,303 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Martino Migliavacca + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +/** + * @file TIMv1/hal_qei_lld.c + * @brief STM32 QEI subsystem low level driver header. + * + * @addtogroup QEI + * @{ + */ + +#include "hal.h" + +#if (HAL_USE_QEI == TRUE) || defined(__DOXYGEN__) + + +/*===========================================================================*/ +/* Driver local definitions. */ +/*===========================================================================*/ + + +/*===========================================================================*/ +/* Driver exported variables. */ +/*===========================================================================*/ + +/** + * @brief QEID1 driver identifier. + */ +#if NRF51_QEI_USE_QDEC1 || defined(__DOXYGEN__) +QEIDriver QEID1; +#endif + + +/*===========================================================================*/ +/* Driver local variables and types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Driver local functions. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Driver interrupt handlers. */ +/*===========================================================================*/ + +#if NRF51_QEI_USE_QDEC1 == TRUE +/** + * @brief Quadrature decoder vector (QDEC) + * + * @isr + */ +OSAL_IRQ_HANDLER(Vector88) { + QEIDriver *qeip = &QEID1; + NRF_QDEC_Type *qdec = qeip->qdec; + + OSAL_IRQ_PROLOGUE(); + + + osalSysLockFromISR(); + + if (qdec->EVENTS_ACCOF) { + qdec->EVENTS_ACCOF = 0; + qeip->overflowed++; + if (qeip->config->overflowed_cb) + qeip->config->overflowed_cb(qeip); + } + + if (qdec->EVENTS_REPORTRDY) { + qdec->EVENTS_REPORTRDY = 0; + + // Read and clear counters (due to shortcut) + int16_t acc = ( int16_t)qdec->ACCREAD; + uint16_t accdbl = (uint16_t)qdec->ACCDBLREAD; + + // Inverse direction if requested + if (qeip->config->dirinv) + acc = -acc; // acc is [-1024..+1023], its okay on int16_t + + // Get boundaries + qeicnt_t min = QEI_COUNT_MIN; + qeicnt_t max = QEI_COUNT_MAX; + if (qeip->config->min != qeip->config->max) { + min = qeip->config->min; + max = qeip->config->max; + } + + // Compute new value + qeidelta_t delta = 0; + qeicnt_t count = qeip->count + acc; + bool overflowed = false; + + // See: https://www.securecoding.cert.org/confluence/display/c/INT32-C.+Ensure+that+operations+on+signed+integers+do+not+result+in+overflow + + // Overflow + if ((acc > 0) && (qeip->count > (max - acc))) { + overflowed = true; + switch(qeip->config->overflow) { + case QEI_OVERFLOW_WRAP: + delta = 0; + count = (min + (qeip->count - (max - acc))) - 1; + break; + case QEI_OVERFLOW_DISCARD: + delta = acc; + count = qeip->count; + break; + case QEI_OVERFLOW_MINMAX: + delta = qeip->count - (max - acc); + count = max; + break; + } + + // Underflow + } else if ((acc < 0) && (qeip->count < (min - acc))) { + overflowed = true; + switch(qeip->config->overflow) { + case QEI_OVERFLOW_WRAP: + delta = 0; + count = (max + (qeip->count - (min - acc))) + 1; + break; + case QEI_OVERFLOW_DISCARD: + delta = acc; + count = qeip->count; + break; + case QEI_OVERFLOW_MINMAX: + delta = qeip->count - (min - acc); + count = min; + break; + } + } + + // Set value and notify + if (qeip->count != count) { + qeip->count = count; + if (qeip->config->notify_cb) + qeip->config->notify_cb(qeip); + } + + // Notify for overflow (passing the remaining delta) + if (overflowed && qeip->config->overflow_cb) + qeip->config->overflow_cb(qeip, delta); + } + + + osalSysUnlockFromISR(); + + OSAL_IRQ_EPILOGUE(); +} +#endif + +/*===========================================================================*/ +/* Driver exported functions. */ +/*===========================================================================*/ + +/** + * @brief Low level QEI driver initialization. + * + * @notapi + */ +void qei_lld_init(void) { + +#if NRF51_QEI_USE_QDEC1 + /* Driver initialization.*/ + qeiObjectInit(&QEID1); + QEID1.qdec = NRF_QDEC; +#endif +} + +/** + * @brief Configures and activates the QEI peripheral. + * + * @param[in] qeip pointer to the @p QEIDriver object + * + * @notapi + */ +void qei_lld_start(QEIDriver *qeip) { + NRF_QDEC_Type *qdec = qeip->qdec; + const QEIConfig *cfg = qeip->config; + + if (qeip->state == QEI_STOP) { + // Set Pins + palSetLineMode(cfg->phase_a, PAL_MODE_INPUT); + palSetLineMode(cfg->phase_b, PAL_MODE_INPUT); + if (cfg->led != PAL_NOLINE) { + palSetLineMode(cfg->led, PAL_MODE_INPUT); + } + + // Set interrupt masks and enable interrupt + qdec->INTENSET = QDEC_INTENSET_REPORTRDY_Msk | + QDEC_INTENSET_ACCOF_Msk; +#if NRF51_QEI_USE_QDEC1 + if (&QEID1 == qeip) { + nvicEnableVector(QDEC_IRQn, NRF51_QEI_IRQ_PRIORITY); + } +#endif + + // Select pin for Phase A and Phase B + qdec->PSELA = PAL_PAD(cfg->phase_a); + qdec->PSELB = PAL_PAD(cfg->phase_b); + + // Select (optional) pin for LED, and configure it + qdec->PSELLED = (cfg->led == PAL_NOLINE) ? (uint32_t)-1 + : PAL_PAD(cfg->led); + qdec->LEDPOL = ((cfg->led_polarity == QEI_LED_POLARITY_LOW) + ? QDEC_LEDPOL_LEDPOL_ActiveLow + : QDEC_LEDPOL_LEDPOL_ActiveHigh) + << QDEC_LEDPOL_LEDPOL_Pos; + qdec->LEDPRE = cfg->led_warming; + + // Set sampling resolution and debouncing + qdec->SAMPLEPER = cfg->resolution; + qdec->DBFEN = (cfg->debouncing ? QDEC_DBFEN_DBFEN_Enabled + : QDEC_DBFEN_DBFEN_Disabled) + << QDEC_DBFEN_DBFEN_Pos; + + // Define minimum sampling before reporting + // and create shortcut to clear accumulation + qdec->REPORTPER = cfg->report; + qdec->SHORTS = QDEC_SHORTS_REPORTRDY_READCLRACC_Msk; + + // Enable peripheric + qdec->ENABLE = 1; + } + + // Initially state is stopped, events cleared + qdec->TASKS_STOP = 1; + qdec->EVENTS_SAMPLERDY = 0; + qdec->EVENTS_REPORTRDY = 0; + qdec->EVENTS_ACCOF = 0; +} + +/** + * @brief Deactivates the QEI peripheral. + * + * @param[in] qeip pointer to the @p QEIDriver object + * + * @notapi + */ +void qei_lld_stop(QEIDriver *qeip) { + + NRF_QDEC_Type *qdec = qeip->qdec; + const QEIConfig *cfg = qeip->config; + + if (qeip->state == QEI_READY) { + qdec->TASKS_STOP = 1; + qdec->ENABLE = 0; +#if NRF51_QEI_USE_QDEC1 + if (&QEID1 == qeip) { + nvicDisableVector(QDEC_IRQn); + } +#endif + qdec->INTENCLR = QDEC_INTENSET_REPORTRDY_Msk | + QDEC_INTENSET_ACCOF_Msk; + } +} + +/** + * @brief Enables the input capture. + * + * @param[in] qeip pointer to the @p QEIDriver object + * + * @notapi + */ +void qei_lld_enable(QEIDriver *qeip) { + qeip->overflowed = 0; + + qeip->qdec->EVENTS_SAMPLERDY = 0; + qeip->qdec->EVENTS_REPORTRDY = 0; + qeip->qdec->EVENTS_ACCOF = 0; + qeip->qdec->TASKS_START = 1; +} + +/** + * @brief Disables the input capture. + * + * @param[in] qeip pointer to the @p QEIDriver object + * + * @notapi + */ +void qei_lld_disable(QEIDriver *qeip) { + qeip->qdec->TASKS_STOP = 1; +} + + + + + + +#endif /* HAL_USE_QEI */ + +/** @} */ diff --git a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h new file mode 100644 index 0000000..f07a60a --- /dev/null +++ b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h @@ -0,0 +1,334 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Martino Migliavacca + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +/** + * @file TIMv1/hal_qei_lld.h + * @brief STM32 QEI subsystem low level driver header. + * + * @addtogroup QEI + * @{ + */ + +#ifndef HAL_QEI_LLD_H +#define HAL_QEI_LLD_H + +#if (HAL_USE_QEI == TRUE) || defined(__DOXYGEN__) + + + +/*===========================================================================*/ +/* Driver constants. */ +/*===========================================================================*/ + +#define QEI_LED_POLARITY_LOW 0 +#define QEI_LED_POLARITY_HIGH 1 + +#define QEI_COUNT_MIN 0 +#define QEI_COUNT_MAX 65535 + + + + +/*===========================================================================*/ +/* Driver pre-compile time settings. */ +/*===========================================================================*/ + +/** + * @name Configuration options + * @{ + */ +/** + * @brief QEID1 driver enable switch. + * @details If set to @p TRUE the support for QEID1 is included. + * @note The default is @p FALSE. + */ +#if !defined(NRF51_QEI_USE_QDEC1) || defined(__DOXYGEN__) +#define NRF51_QEI_USE_QDEC1 FALSE +#endif + +/** + * @brief QEID interrupt priority level setting. + */ +#if !defined(NRF51_QEI_IRQ_PRIORITY) || defined(__DOXYGEN__) +#define NRF51_QEI_IRQ_PRIORITY 2 +#endif +/** @} */ + +/*===========================================================================*/ +/* Derived constants and error checks. */ +/*===========================================================================*/ + +#if NRF51_QEI_USE_QDEC1 && \ + !OSAL_IRQ_IS_VALID_PRIORITY(NRF51_QEI_IRQ_PRIORITY) +#error "Invalid IRQ priority assigned to QDEC1" +#endif + +/*===========================================================================*/ +/* Driver data structures and types. */ +/*===========================================================================*/ + + + +/** + * @brief QEI count mode. + */ +typedef enum { + QEI_MODE_QUADRATURE = 0, /**< Quadrature encoder mode. */ +} qeimode_t; + +/** + * @brief QEI resolution. + */ +typedef enum { + QEI_RESOLUTION_128us = 0x00UL, /**< 128us sample period. */ + QEI_RESOLUTION_256us = 0x01UL, /**< 256us sample period. */ + QEI_RESOLUTION_512us = 0x02UL, /**< 512us sample period. */ + QEI_RESOLUTION_1024us = 0x03UL, /**< 1024us sample period. */ + QEI_RESOLUTION_2048us = 0x04UL, /**< 2048us sample period. */ + QEI_RESOLUTION_4096us = 0x05UL, /**< 4096us sample period. */ + QEI_RESOLUTION_8192us = 0x06UL, /**< 8192us sample period. */ + QEI_RESOLUTION_16384us = 0x07UL, /**< 16384us sample period. */ +} qeiresolution_t; + +/** + * + */ +typedef enum { + QEI_REPORT_10 = 0x00UL, /**< 10 samples per report. */ + QEI_REPORT_40 = 0x01UL, /**< 40 samples per report. */ + QEI_REPORT_80 = 0x02UL, /**< 80 samples per report. */ + QEI_REPORT_120 = 0x03UL, /**< 120 samples per report. */ + QEI_REPORT_160 = 0x04UL, /**< 160 samples per report. */ + QEI_REPORT_200 = 0x05UL, /**< 200 samples per report. */ + QEI_REPORT_240 = 0x06UL, /**< 240 samples per report. */ + QEI_REPORT_280 = 0x07UL, /**< 280 samples per report. */ +} qeireport_t; + + +/** + * @brief Handling of counter overflow/underflow + */ +typedef enum { + QEI_OVERFLOW_WRAP = 0, /**< Counter value will wrap around. */ + QEI_OVERFLOW_DISCARD = 1, /**< Counter doesn't change. */ + QEI_OVERFLOW_MINMAX = 2, /**< Counter will be updated to min or max. */ +} qeioverflow_t; + + + +/** + * @brief QEI direction inversion. + */ +typedef enum { + QEI_DIRINV_FALSE = 0, /**< Do not invert counter direction. */ + QEI_DIRINV_TRUE = 1, /**< Invert counter direction. */ +} qeidirinv_t; + +/** + * @brief QEI counter type. + */ +typedef uint16_t qeicnt_t; + +/** + * @brief QEI delta type. + */ +typedef int16_t qeidelta_t; + +/** + * @brief Driver configuration structure. + * @note It could be empty on some architectures. + */ +typedef struct { + /** + * @brief Count mode. + */ + qeimode_t mode; + /** + * @brief Resolution. + */ + qeiresolution_t resolution; + /** + * @brief Direction inversion. + */ + qeidirinv_t dirinv; + /** + * @brief Handling of counter overflow/underflow + * + * @details When overflow callback is called, the counter value + * is not updated, the decision on how to update is left + * to the callback. + * + * Three implementation are provided + * - QEI_OVERFLOW_DISCARD: + * discard the update value, counter doesn't change + * - QEI_OVERFLOW_MINMAX + * counter will be updated to reach min or max + * - QEI_OVERFLOW_WRAP: + * counter value will wrap around + */ + qeioverflow_t overflow; + /** + * @brief Min count value. + * + * @note If min == max, the QEI_COUNT_MIN is used as default + */ + qeicnt_t min; + /** + * @brief Max count value. + * + * @note If min == max, the QEI_COUNT_MAX is used as default + */ + qeicnt_t max; + /** + * @brief Notify of value change + */ + qeicallback_t notify_cb; + /** + * @brief Notify of overflow + * + * @note Overflow notification is performed after + * value changed notification. + */ + void (*overflow_cb)(QEIDriver *qeip, qeidelta_t delta); + /* End of the mandatory fields.*/ + /** + * @brief Line for reading Phase A + */ + ioline_t phase_a; + /** + * @brief Line for reading Phase B + */ + ioline_t phase_b; + /** + * @brief Line to use to control LED + * + * @note If LED is not controlled by MCU, you need to use the + * PAL_NOLINE value. + */ + ioline_t led; + /** + * @brief Period in µs the LED is switched on prior to sampling. + * + * @details LED warming is between 0 and 511 (including boundaries) + * + * @note 31µs is the recommanded default. + * + * @note If debouncing is activated, LED is always on for the + * whole sampling period (aka: resolution) + */ + uint16_t led_warming; + /** + * @brief LED polarity to used (when LED is controlled by MCU) + */ + uint8_t led_polarity; + /** + * @brief Activate debouncing filter + * + * @note If LED is controlled by MCU, the led_warming is ignored and, + * LED is always on for the whole sampling period (aka: resolution) + */ + bool debouncing; + /** + * @brief Number of sample per report + * + * @details Default to QEI_REPORT_10 + */ + qeireport_t report; + /** + * @brief Notify of internal accumulator overflowed + * + * @note MCU has discarded some of the samples. + */ + qeicallback_t overflowed_cb; +} QEIConfig; + +/** + * @brief Structure representing an QEI driver. + */ +struct QEIDriver { + /** + * @brief Driver state. + */ + qeistate_t state; + /** + * @brief Last count value. + */ + qeicnt_t last; + /** + * @brief Current configuration data. + */ + const QEIConfig *config; +#if defined(QEI_DRIVER_EXT_FIELDS) + QEI_DRIVER_EXT_FIELDS +#endif + /* End of the mandatory fields.*/ + /** + */ + qeidelta_t delta; + /** + */ + qeicnt_t count; + /** + * @brief Number of time the MCU discarded updates due to + * accumulator overflow + */ + uint32_t overflowed; + /** + * @brief Pointer to the ADCx registers block. + */ + NRF_QDEC_Type *qdec; +}; + +/*===========================================================================*/ +/* Driver macros. */ +/*===========================================================================*/ + +/** + * @brief Returns the counter value. + * + * @param[in] qeip pointer to the @p QEIDriver object + * @return The current counter value. + * + * @notapi + */ +#define qei_lld_get_count(qeip) ((qeip)->count) + + +/*===========================================================================*/ +/* External declarations. */ +/*===========================================================================*/ + +#if NRF51_QEI_USE_QDEC1 && !defined(__DOXYGEN__) +extern QEIDriver QEID1; +#endif + +#ifdef __cplusplus +extern "C" { +#endif + void qei_lld_init(void); + void qei_lld_start(QEIDriver *qeip); + void qei_lld_stop(QEIDriver *qeip); + void qei_lld_enable(QEIDriver *qeip); + void qei_lld_disable(QEIDriver *qeip); +#ifdef __cplusplus +} +#endif + +#endif /* HAL_USE_QEI */ + +#endif /* HAL_QEI_LLD_H */ + +/** @} */ diff --git a/os/hal/ports/NRF51/NRF51822/platform.mk b/os/hal/ports/NRF51/NRF51822/platform.mk index b937e39..ad5b2c4 100644 --- a/os/hal/ports/NRF51/NRF51822/platform.mk +++ b/os/hal/ports/NRF51/NRF51822/platform.mk @@ -37,6 +37,9 @@ endif ifneq ($(findstring HAL_USE_PWM TRUE,$(HALCONF)),) PLATFORMSRC += ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_pwm_lld.c endif +ifneq ($(findstring HAL_USE_QEI TRUE,$(HALCONF)),) +PLATFORMSRC += ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c +endif else PLATFORMSRC = ${CHIBIOS}/os/hal/ports/common/ARMCMx/nvic.c \ ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_lld.c \ @@ -51,7 +54,8 @@ PLATFORMSRC = ${CHIBIOS}/os/hal/ports/common/ARMCMx/nvic.c \ ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_gpt_lld.c \ ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_wdg_lld.c \ ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_rng_lld.c \ - ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_pwm_lld.c + ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_pwm_lld.c \ + ${CHIBIOS_CONTRIB}/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c endif # Required include directories -- cgit v1.2.3