rusefi
/
ChibiOS-Contrib
mirror of https://github.com/rusefi/ChibiOS-Contrib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

created more _lld_ fonctions.

This commit is contained in:
Stephane D'Alu 2016-06-29 20:14:40 +02:00
parent 5fee893778
commit b20bf874d0
2 changed files with 147 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

210
os/hal/ports/NRF51/NRF51822/hal_qei_lld.c
View File

@ -31,6 +31,130 @@
/* Driver local definitions. */ /* 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. * @brief Common IRQ handler.
* *
@ -42,88 +166,28 @@ static void serve_interrupt(QEIDriver *qeip) {
/* Accumulator overflowed /* Accumulator overflowed
*/ */
if (qdec->EVENTS_ACCOF) { if (qdec->EVENTS_ACCOF) {
qdec->EVENTS_ACCOF = 0; qdec->EVENTS_ACCOF = 0;
qeip->overflowed++; qeip->overflowed++;
if (qeip->config->overflowed_cb) if (qeip->config->overflowed_cb)
qeip->config->overflowed_cb(qeip); qeip->config->overflowed_cb(qeip);
} }
/* Report ready /* Report ready
*/ */
if (qdec->EVENTS_REPORTRDY) { if (qdec->EVENTS_REPORTRDY) {
qdec->EVENTS_REPORTRDY = 0; qdec->EVENTS_REPORTRDY = 0;
// Read and clear counters (due to shortcut) // Read (and clear counters due to shortcut)
int16_t acc = ( int16_t)qdec->ACCREAD; int16_t acc = ( int16_t)qdec->ACCREAD;
uint16_t accdbl = (uint16_t)qdec->ACCDBLREAD; uint16_t accdbl = (uint16_t)qdec->ACCDBLREAD;
// Inverse direction if requested // Inverse direction if requested
if (qeip->config->dirinv) if (qeip->config->dirinv)
acc = -acc; // acc is [-1024..+1023], its okay on int16_t acc = -acc; // acc is [-1024..+1023], its okay on int16_t
// Get boundaries // Adjust counter
qeicnt_t min = QEI_COUNT_MIN; qei_lld_adjust_count(qeip, acc);
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);
} }
} }

16
os/hal/ports/NRF51/NRF51822/hal_qei_lld.h
View File

@ -31,7 +31,6 @@
#if (HAL_USE_QEI == TRUE) || defined(__DOXYGEN__) #if (HAL_USE_QEI == TRUE) || defined(__DOXYGEN__)
/*===========================================================================*/ /*===========================================================================*/
/* Driver constants. */ /* Driver constants. */
/*===========================================================================*/ /*===========================================================================*/
@ -71,7 +70,7 @@
#endif #endif
/** /**
* @brief QEID interrupt priority level setting. * @brief QEID interrupt priority level setting for QDEC0.
*/ */
#if !defined(NRF51_QEI_QDEC0_IRQ_PRIORITY) || defined(__DOXYGEN__) #if !defined(NRF51_QEI_QDEC0_IRQ_PRIORITY) || defined(__DOXYGEN__)
#define NRF51_QEI_QDEC0_IRQ_PRIORITY 2 #define NRF51_QEI_QDEC0_IRQ_PRIORITY 2
@ -296,12 +295,12 @@ struct QEIDriver {
/** /**
* @brief Counter * @brief Counter
*/ */
qeicnt_t count; qeicnt_t count;
/** /**
* @brief Number of time the MCU discarded updates due to * @brief Number of time the MCU discarded updates due to
* accumulator overflow * accumulator overflow
*/ */
uint32_t overflowed; uint32_t overflowed;
/** /**
* @brief Pointer to the QDECx registers block. * @brief Pointer to the QDECx registers block.
*/ */
@ -332,8 +331,10 @@ struct QEIDriver {
* @notapi * @notapi
*/ */
#define qei_lld_set_count(qeip, value) \ #define qei_lld_set_count(qeip, value) \
do { \ if ((qeip)->count != ((qeicnt_t)value)) { \
(qeip)->count = value; \ (qeip)->count = value; \
if ((qeip)->config->notify_cb) \
(qeip)->config->notify_cb(qeip); \
} while(0) } while(0)
@ -357,6 +358,9 @@ extern "C" {
} }
#endif #endif
void qeiSetCount(QEIDriver *qeip, qeicnt_t value);
qeidelta_t qeiAdjust(QEIDriver *qeip, qeidelta_t delta);
#endif /* HAL_USE_QEI */ #endif /* HAL_USE_QEI */
#endif /* HAL_QEI_LLD_H */ #endif /* HAL_QEI_LLD_H */