diff options
Diffstat (limited to 'os')
-rw-r--r-- | os/hal/ports/NRF51/NRF51822/hal_qei_lld.c | 216 | ||||
-rw-r--r-- | os/hal/ports/NRF51/NRF51822/hal_qei_lld.h | 16 |
2 files changed, 150 insertions, 82 deletions
diff --git a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c index ffbed95..66694dc 100644 --- a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c +++ b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.c @@ -31,6 +31,130 @@ /* Driver local definitions. */
/*===========================================================================*/
+static inline
+bool qei_adjust_count(qeicnt_t *count, qeidelta_t *delta,
+ qeicnt_t min, qeicnt_t max, qeioverflow_t mode) {
+ // See: https://www.securecoding.cert.org/confluence/display/c/INT32-C.+Ensure+that+operations+on+signed+integers+do+not+result+in+overflow
+
+ // 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;
+ case QEI_OVERFLOW_DISCARD:
+ *delta = _delta;
+ *count = _count;
+ break;
+ case QEI_OVERFLOW_MINMAX:
+ *delta = _count - (max - _delta);
+ *count = max;
+ break;
+ }
+ 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;
+ case QEI_OVERFLOW_DISCARD:
+ *delta = _delta;
+ *count = _count;
+ break;
+ case QEI_OVERFLOW_MINMAX:
+ *delta = _count - (min - _delta);
+ *count = min;
+ break;
+ }
+ return true;
+
+ // Normal operation
+ } else {
+ *delta = 0;
+ *count = _count + _delta;
+ return false;
+ }
+}
+
+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;
+}
+
+
+/**
+ * @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 Set counter value
+ *
+ * @param[in] qeip pointer to the @p QEIDriver object
+ * @param[in] value the 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 Common IRQ handler.
*
@@ -42,88 +166,28 @@ static void serve_interrupt(QEIDriver *qeip) { /* Accumulator overflowed
*/
if (qdec->EVENTS_ACCOF) {
- qdec->EVENTS_ACCOF = 0;
+ qdec->EVENTS_ACCOF = 0;
- qeip->overflowed++;
- if (qeip->config->overflowed_cb)
- qeip->config->overflowed_cb(qeip);
+ qeip->overflowed++;
+ if (qeip->config->overflowed_cb)
+ qeip->config->overflowed_cb(qeip);
}
/* 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
-
- // 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);
+ 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);
}
}
diff --git a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h index e1485e3..813d169 100644 --- a/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h +++ b/os/hal/ports/NRF51/NRF51822/hal_qei_lld.h @@ -31,7 +31,6 @@ #if (HAL_USE_QEI == TRUE) || defined(__DOXYGEN__)
-
/*===========================================================================*/
/* Driver constants. */
/*===========================================================================*/
@@ -71,7 +70,7 @@ #endif
/**
- * @brief QEID interrupt priority level setting.
+ * @brief QEID interrupt priority level setting for QDEC0.
*/
#if !defined(NRF51_QEI_QDEC0_IRQ_PRIORITY) || defined(__DOXYGEN__)
#define NRF51_QEI_QDEC0_IRQ_PRIORITY 2
@@ -296,12 +295,12 @@ struct QEIDriver { /**
* @brief Counter
*/
- qeicnt_t count;
+ qeicnt_t count;
/**
* @brief Number of time the MCU discarded updates due to
* accumulator overflow
*/
- uint32_t overflowed;
+ uint32_t overflowed;
/**
* @brief Pointer to the QDECx registers block.
*/
@@ -332,8 +331,10 @@ struct QEIDriver { * @notapi
*/
#define qei_lld_set_count(qeip, value) \
- do { \
- (qeip)->count = value; \
+ if ((qeip)->count != ((qeicnt_t)value)) { \
+ (qeip)->count = value; \
+ if ((qeip)->config->notify_cb) \
+ (qeip)->config->notify_cb(qeip); \
} while(0)
@@ -357,6 +358,9 @@ extern "C" { }
#endif
+void qeiSetCount(QEIDriver *qeip, qeicnt_t value);
+qeidelta_t qeiAdjust(QEIDriver *qeip, qeidelta_t delta);
+
#endif /* HAL_USE_QEI */
#endif /* HAL_QEI_LLD_H */
|