created more _lld_ fonctions.

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 */