diff --git a/os/hal/include/hal_qei.h b/os/hal/include/hal_qei.h index 92f03fc5..1032c840 100644 --- a/os/hal/include/hal_qei.h +++ b/os/hal/include/hal_qei.h @@ -65,8 +65,36 @@ typedef struct QEIDriver QEIDriver; */ typedef void (*qeicallback_t)(QEIDriver *qeip); +/** + * @brief Driver possible handling of counter overflow/underflow. + * + * @details When counter is going to overflow, the new value is + * computed according to this mode in such a way that + * the counter will either wrap around, stay unchange + * or reach min/max + * + * @note All driver implementation should support the + * QEI_OVERFLOW_WRAP mode. + * + * @note Mode QEI_OVERFLOW_DISCARD and QEI_OVERFLOW_MINMAX are included + * if QEI_USE_OVERFLOW_DISCARD and QEI_USE_OVERFLOW_MINMAX are + * set to TRUE in halconf_community.h and are not necessary supported + * by all drivers + */ +typedef enum { + QEI_OVERFLOW_WRAP = 0, /**< Counter value will wrap around. */ +#if QEI_USE_OVERFLOW_DISCARD == TRUE + QEI_OVERFLOW_DISCARD = 1, /**< Counter doesn't change. */ +#endif +#if QEI_USE_OVERFLOW_MINMAX == TRUE + QEI_OVERFLOW_MINMAX = 2, /**< Counter will be updated upto min or max.*/ +#endif +} qeioverflow_t; + + #include "hal_qei_lld.h" + /*===========================================================================*/ /* Driver macros. */ /*===========================================================================*/ @@ -119,6 +147,8 @@ extern "C" { qeicnt_t qeiGetCount(QEIDriver *qeip); qeidelta_t qeiUpdate(QEIDriver *qeip); qeidelta_t qeiUpdateI(QEIDriver *qeip); + bool qei_adjust_count(qeicnt_t *count, qeidelta_t *delta, + qeicnt_t min, qeicnt_t max, qeioverflow_t mode); #ifdef __cplusplus } #endif 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 00000000..fbaf3aae --- /dev/null +++ b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c @@ -0,0 +1,308 @@ +/* + ChibiOS - Copyright (C) 2016..2016 Stéphane D'Alu + + 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 NRF51/hal_qei_lld.c + * @brief NRF51 QEI subsystem low level driver. + * + * @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_QDEC0 || defined(__DOXYGEN__) +QEIDriver QEID1; +#endif + + +/*===========================================================================*/ +/* Driver local variables and types. */ +/*===========================================================================*/ + +/*===========================================================================*/ +/* Driver local functions. */ +/*===========================================================================*/ + +/** + * @brief Common IRQ handler. + * + * @param[in] qeip pointer to an QEIDriver + */ +static void serve_interrupt(QEIDriver *qeip) { + NRF_QDEC_Type *qdec = qeip->qdec; + +#if NRF51_QEI_USE_ACC_OVERFLOWED_CB == TRUE + /* Accumulator overflowed + */ + if (qdec->EVENTS_ACCOF) { + qdec->EVENTS_ACCOF = 0; + + qeip->overflowed++; + if (qeip->config->overflowed_cb) + qeip->config->overflowed_cb(qeip); + } +#endif + + /* Report ready + */ + 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 + + /* Adjust counter */ + qei_lld_adjust_count(qeip, acc); + } +} + +/*===========================================================================*/ +/* Driver interrupt handlers. */ +/*===========================================================================*/ + +#if NRF51_QEI_USE_QDEC0 == TRUE +/** + * @brief Quadrature decoder vector (QDEC) + * + * @isr + */ +OSAL_IRQ_HANDLER(Vector88) { + + OSAL_IRQ_PROLOGUE(); + serve_interrupt(&QEID1); + OSAL_IRQ_EPILOGUE(); +} +#endif + +/*===========================================================================*/ +/* Driver exported functions. */ +/*===========================================================================*/ + +/** + * @brief Low level QEI driver initialization. + * + * @notapi + */ +void qei_lld_init(void) { + +#if NRF51_QEI_USE_QDEC0 == TRUE + /* 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 NRF51_QEI_USE_LED == TRUE + if (cfg->led != PAL_NOLINE) { + palSetLineMode(cfg->led, PAL_MODE_INPUT); + } +#endif + + /* Set interrupt masks and enable interrupt */ +#if NRF51_QEI_USE_ACC_OVERFLOWED_CB == TRUE + qdec->INTENSET = QDEC_INTENSET_REPORTRDY_Msk | + QDEC_INTENSET_ACCOF_Msk; +#else + qdec->INTENSET = QDEC_INTENSET_REPORTRDY_Msk; +#endif +#if NRF51_QEI_USE_QDEC0 == TRUE + if (&QEID1 == qeip) { + nvicEnableVector(QDEC_IRQn, NRF51_QEI_QDEC0_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 */ +#if NRF51_QEI_USE_LED == TRUE + qdec->PSELLED = 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; +#else + qdec->PSELLED = (uint32_t)-1; +#endif + + /* 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; + + /* Unset interrupt masks and disable interrupt */ +#if NRF51_QEI_USE_QDEC0 == TRUE + if (&QEID1 == qeip) { + nvicDisableVector(QDEC_IRQn); + } +#endif +#if NRF51_QEI_USE_ACC_OVERFLOWED_CB == TRUE + qdec->INTENCLR = QDEC_INTENCLR_REPORTRDY_Msk | + QDEC_INTENCLR_ACCOF_Msk; +#else + qdec->INTENCLR = QDEC_INTENCLR_REPORTRDY_Msk; +#endif + + /* Return pins to reset state */ + palSetLineMode(cfg->phase_a, PAL_MODE_RESET); + palSetLineMode(cfg->phase_b, PAL_MODE_RESET); +#if NRF51_QEI_USE_LED == TRUE + if (cfg->led != PAL_NOLINE) { + palSetLineMode(cfg->led, PAL_MODE_RESET); + } +#endif + } +} + +/** + * @brief Enables the input capture. + * + * @param[in] qeip pointer to the @p QEIDriver object + * + * @notapi + */ +void qei_lld_enable(QEIDriver *qeip) { +#if NRF51_QEI_USE_ACC_OVERFLOWED_CB == TRUE + qeip->overflowed = 0; +#endif + + 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; +} + +/** + * @brief Adjust counter + * + * @param[in] qeip pointer to the @p QEIDriver object + * @param[in] delta value to use for adjustement + * @return remaining adjustement that were not applied + * + * @notapi + */ +qeidelta_t qei_lld_adjust_count(QEIDriver *qeip, qeidelta_t delta) { + /* 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; + } + + /* Snapshot counter for later comparison */ + qeicnt_t count = qeip->count; + + /* Adjust counter value */ + bool overflowed = qei_adjust_count(&qeip->count, &delta, + min, max, qeip->config->overflow); + + /* Notify for value change */ + if ((qeip->count != count) && 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); + + /* Remaining delta */ + return delta; +} + +#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 00000000..5037591f --- /dev/null +++ b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h @@ -0,0 +1,390 @@ +/* + ChibiOS - Copyright (C) 2016..2016 Stéphane D'Alu + + 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 NRF51/hal_qei_lld.h + * @brief NRF51 QEI subsystem low level driver header. + * + * @note Not tested with LED pin + * + * @note Pins are configured as input with no pull. + * + * @addtogroup QEI + * @{ + */ + +#ifndef HAL_QEI_LLD_H +#define HAL_QEI_LLD_H + +#if (HAL_USE_QEI == TRUE) || defined(__DOXYGEN__) + +/*===========================================================================*/ +/* Driver constants. */ +/*===========================================================================*/ + +/** + * @brief For LED active on LOW + */ +#define QEI_LED_POLARITY_LOW 0 + +/** + * @brief For LED active on HIGH + */ +#define QEI_LED_POLARITY_HIGH 1 + +/** + * @brief Mininum usable value for defining counter underflow + */ +#define QEI_COUNT_MIN (-2147483648) + +/** + * @brief Maximum usable value for defining counter overflow + */ +#define QEI_COUNT_MAX ( 2147483647) + + + +/*===========================================================================*/ +/* Driver pre-compile time settings. */ +/*===========================================================================*/ + +/** + * @name Configuration options + * @{ + */ +/** + * @brief LED control enable switch. + * @details If set to @p TRUE the support for LED control + * is included. + * @note The default is @p FALSE. + */ +#if !defined(NRF51_QEI_USE_LED) || defined(__DOXYGEN__) +#define NRF51_QEI_USE_LED FALSE +#endif + +/** + * @brief Accumulator overflow notification enable switch. + * @details If set to @p TRUE the support for accumulator overflow + * is included. + * @note The default is @p FALSE. + */ +#if !defined(NRF51_QEI_USE_ACC_OVERFLOWED_CB) || defined(__DOXYGEN__) +#define NRF51_QEI_USE_ACC_OVERFLOWED_CB FALSE +#endif + +/** + * @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_QDEC0) || defined(__DOXYGEN__) +#define NRF51_QEI_USE_QDEC0 FALSE +#endif + +/** + * @brief QEID interrupt priority level setting for QDEC0. + */ +#if !defined(NRF51_QEI_QDEC0_IRQ_PRIORITY) || defined(__DOXYGEN__) +#define NRF51_QEI_QDEC0_IRQ_PRIORITY 2 +#endif +/** @} */ + +/*===========================================================================*/ +/* Derived constants and error checks. */ +/*===========================================================================*/ + +#if NRF51_QEI_USE_QDEC0 && \ + !OSAL_IRQ_IS_VALID_PRIORITY(NRF51_QEI_QDEC0_IRQ_PRIORITY) +#error "Invalid IRQ priority assigned to QDEC0" +#endif + +#if NRF51_QEI_USE_QDEC0 == FALSE +#error "Requesting QEI driver, but no QDEC peripheric attached" +#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; + +/** + * @brief Clusters of samples. + */ +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 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 int32_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 occurs, the counter value is updated + * according to: + * - 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, then QEI_COUNT_MIN is used. + */ + qeicnt_t min; + /** + * @brief Max count value. + * + * @note If min == max, then QEI_COUNT_MAX is used. + */ + qeicnt_t max; + /** + * @brief Notify of value change + * + * @note Called from ISR context. + */ + qeicallback_t notify_cb; + /** + * @brief Notify of overflow + * + * @note Overflow notification is performed after + * value changed notification. + * @note Called from ISR context. + */ + 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; +#if (NRF51_QEI_USE_LED == TRUE) || defined(__DOXYGEN__) + /** + * @brief Line used 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 expressed in micro-seconds and value + * is [0..511] + * + * @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; +#endif + /** + * @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 samples per report + * + * @details Default to QEI_REPORT_10 + */ + qeireport_t report; +#if NRF51_QEI_USE_ACC_OVERFLOWED_CB == TRUE + /** + * @brief Notify of internal accumulator overflowed + * (ie: MCU discarding samples) + * + * @note Called from ISR context. + */ + qeicallback_t overflowed_cb; +#endif +} 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.*/ + /** + * @brief Counter + */ + qeicnt_t count; +#if NRF51_QEI_USE_ACC_OVERFLOWED_CB == TRUE + /** + * @brief Number of time the MCU discarded updates due to + * accumulator overflow + */ + uint32_t overflowed; +#endif + /** + * @brief Pointer to the QDECx 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) + + +/** + * @brief Set the counter value. + * + * @param[in] qeip pointer to the @p QEIDriver object + * @param[in] value counter value + * + * @notapi + */ +#define qei_lld_set_count(qeip, value) \ + if ((qeip)->count != ((qeicnt_t)value)) { \ + (qeip)->count = value; \ + if ((qeip)->config->notify_cb) \ + (qeip)->config->notify_cb(qeip); \ + } while(0) + + +/*===========================================================================*/ +/* External declarations. */ +/*===========================================================================*/ + +#if NRF51_QEI_USE_QDEC0 && !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); + qeidelta_t qei_lld_adjust_count(QEIDriver *qeip, qeidelta_t delta); +#ifdef __cplusplus +} +#endif + +/*===========================================================================*/ +/* To be moved in hal_qei */ +/*===========================================================================*/ + +void qeiSetCount(QEIDriver *qeip, qeicnt_t value); +qeidelta_t qeiAdjust(QEIDriver *qeip, qeidelta_t delta); + +#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 b937e391..ad5b2c45 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 diff --git a/os/hal/src/hal_qei.c b/os/hal/src/hal_qei.c index a2b73037..abecdf8e 100644 --- a/os/hal/src/hal_qei.c +++ b/os/hal/src/hal_qei.c @@ -46,6 +46,91 @@ /* Driver exported functions. */ /*===========================================================================*/ +/** + * @brief Helper for correclty handling overflow/underflow + * + * @details Underflow/overflow will be handled according to mode: + * QEI_OVERFLOW_WRAP: counter value will wrap around. + * QEI_OVERFLOW_DISCARD: counter will not change + * QEI_OVERFLOW_MINMAX: counter will be updated upto min or max. + * + * @note This function is for use by low level driver. + * + * @param[in,out] count counter value + * @param[in,out] delta adjustment value + * @param[in] min minimum allowed value for counter + * @param[in] max maximum allowed value for counter + * @param[in] mode how to handle overflow + * + * @return true if counter underflow/overflow occured or + * was due to occur + * + */ +bool qei_adjust_count(qeicnt_t *count, qeidelta_t *delta, + qeicnt_t min, qeicnt_t max, qeioverflow_t mode) { + /* For information on signed integer overflow see: + * https://www.securecoding.cert.org/confluence/x/RgE + */ + + /* Get values */ + const qeicnt_t _count = *count; + const qeidelta_t _delta = *delta; + + /* Overflow operation + */ + if ((_delta > 0) && (_count > (max - _delta))) { + switch(mode) { + case QEI_OVERFLOW_WRAP: + *delta = 0; + *count = (min + (_count - (max - _delta))) - 1; + break; +#if QEI_USE_OVERFLOW_DISCARD == TRUE + case QEI_OVERFLOW_DISCARD: + *delta = _delta; + *count = _count; + break; +#endif +#if QEI_USE_OVERFLOW_MINMAX == TRUE + case QEI_OVERFLOW_MINMAX: + *delta = _count - (max - _delta); + *count = max; + break; +#endif + } + return true; + + /* Underflow operation + */ + } else if ((_delta < 0) && (_count < (min - _delta))) { + switch(mode) { + case QEI_OVERFLOW_WRAP: + *delta = 0; + *count = (max + (_count - (min - _delta))) + 1; + break; +#if QEI_USE_OVERFLOW_DISCARD == TRUE + case QEI_OVERFLOW_DISCARD: + *delta = _delta; + *count = _count; + break; +#endif +#if QEI_USE_OVERFLOW_MINMAX == TRUE + case QEI_OVERFLOW_MINMAX: + *delta = _count - (min - _delta); + *count = min; + break; +#endif + } + return true; + + /* Normal operation + */ + } else { + *delta = 0; + *count = _count + _delta; + return false; + } +} + /** * @brief QEI Driver initialization. * @note This function is implicitly invoked by @p halInit(), there is @@ -167,6 +252,44 @@ qeicnt_t qeiGetCount(QEIDriver *qeip) { return cnt; } +/** + * @brief Set counter value. + * + * @param[in] qeip pointer to the @p QEIDriver object. + * @param[in] value the new counter value. + * + * @api + */ +void qeiSetCount(QEIDriver *qeip, qeicnt_t value) { + osalDbgCheck(qeip != NULL); + osalDbgAssert((qeip->state == QEI_READY) || (qeip->state == QEI_ACTIVE), + "invalid state"); + + osalSysLock(); + qei_lld_set_count(qeip, value); + osalSysUnlock(); +} + +/** + * @brief Adjust the counter by delta. + * + * @param[in] qeip pointer to the @p QEIDriver object. + * @param[in] delta the adjustement value. + * @return the remaining delta (can occur during overflow). + * + * @api + */ +qeidelta_t qeiAdjust(QEIDriver *qeip, qeidelta_t delta) { + osalDbgCheck(qeip != NULL); + osalDbgAssert((qeip->state == QEI_ACTIVE), "invalid state"); + + osalSysLock(); + delta = qei_lld_adjust_count(qeip, delta); + osalSysUnlock(); + + return delta; +} + /** * @brief Returns the counter delta from last reading. *