This is an automated email from the ASF dual-hosted git repository. jerzy pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/mynewt-core.git
commit e9e6c717bc364fe99707bcb7769b41a42606c154 Author: Jerzy Kasenberg <[email protected]> AuthorDate: Fri Dec 14 11:33:00 2018 +0100 hw/drivers/lis2dh12: Add self test procedure Added code for self-test procedure described in application note. --- .../sensors/lis2dh12/include/lis2dh12/lis2dh12.h | 13 ++ hw/drivers/sensors/lis2dh12/src/lis2dh12.c | 179 +++++++++++++++++++++ hw/drivers/sensors/lis2dh12/src/lis2dh12_priv.h | 3 + 3 files changed, 195 insertions(+) diff --git a/hw/drivers/sensors/lis2dh12/include/lis2dh12/lis2dh12.h b/hw/drivers/sensors/lis2dh12/include/lis2dh12/lis2dh12.h index 0cbdc4b..b5b96cf 100644 --- a/hw/drivers/sensors/lis2dh12/include/lis2dh12/lis2dh12.h +++ b/hw/drivers/sensors/lis2dh12/include/lis2dh12/lis2dh12.h @@ -640,6 +640,19 @@ lis2dh12_set_activity_duration(struct sensor_itf *itf, uint8_t duration); int lis2dh12_get_fifo_samples(struct sensor_itf *itf, uint8_t *samples); +/** + * Run Self test on sensor + * + * Self test sequence as described in lis2dh12 application note AN5005. + * + * @param the sensor interface + * @param pointer to return test result in (0 on pass, non-zero on failure) + * + * @return 0 on sucess, non-zero on failure + */ +int +lis2dh12_run_self_test(struct sensor_itf *itf, int *result); + #if MYNEWT_VAL(LIS2DH12_CLI) int lis2dh12_shell_init(void); #endif diff --git a/hw/drivers/sensors/lis2dh12/src/lis2dh12.c b/hw/drivers/sensors/lis2dh12/src/lis2dh12.c index b676489..ea9f107 100644 --- a/hw/drivers/sensors/lis2dh12/src/lis2dh12.c +++ b/hw/drivers/sensors/lis2dh12/src/lis2dh12.c @@ -1480,6 +1480,185 @@ lis2dh12_set_activity_duration(struct sensor_itf *itf, uint8_t duration) } static int +lis2dh12_wait_for_data(struct sensor_itf *itf, int timeout_ms) +{ + int rc; + uint8_t status; + os_time_t time_limit = os_time_get() + os_time_ms_to_ticks32(timeout_ms); + + while (1) { + rc = lis2dh12_read8(itf, LIS2DH12_REG_STATUS_REG, &status); + if (rc != OS_OK || (status & LIS2DH12_STATUS_ZYXDA) != 0) { + break; + } + if (os_time_get() > time_limit) { + rc = OS_TIMEOUT; + break; + } + os_time_delay(1); + } + return rc; +} + +int lis2dh12_run_self_test(struct sensor_itf *itf, int *result) +{ + int rc, rc2; + int i; + int16_t no_st[3], st[3], data[3]; + int32_t scratch[3] = { 0 }; + uint8_t prev_config[4]; + uint8_t config[4] = { LIS2DH12_CTRL_REG1_XPEN | + LIS2DH12_CTRL_REG1_YPEN | + LIS2DH12_CTRL_REG1_ZPEN | + LIS2DH12_DATA_RATE_50HZ, + 0, 0, + LIS2DH12_ST_MODE_DISABLE | + LIS2DH12_FS_2G | + LIS2DH12_CTRL_REG4_BDU }; + int16_t diff; + uint8_t fifo_ctrl = 0; + const int read_count = 5; + + *result = 0; + + rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG1, prev_config, 4); + if (rc) { + return rc; + } + + rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG2, &config[1], 3); + if (rc) { + goto end; + } + + rc = lis2dh12_write8(itf, LIS2DH12_REG_CTRL_REG1, config[0]); + if (rc) { + goto end; + } + + /* Turn on bypass mode for fifo */ + rc = lis2dh12_read8(itf, LIS2DH12_REG_FIFO_CTRL_REG, &fifo_ctrl); + if (rc) { + goto end; + } + + if (fifo_ctrl) { + rc = lis2dh12_write8(itf, LIS2DH12_REG_FIFO_CTRL_REG, 0); + if (rc) { + goto end; + } + } + /* Wait 90ms */ + os_time_delay(90 * OS_TICKS_PER_SEC / 1000 + 1); + + /* Wait 1ms for first sample */ + rc = lis2dh12_wait_for_data(itf, 1); + if (rc) { + goto end; + } + + /* Read and discard data */ + rc = lis2dh12_get_data(itf, 2, &data[0], &data[1], &data[2]); + if (rc) { + goto end; + } + + /* Take no st offset reading */ + for (i = 0; i < read_count; i++) { + /* Wait at least 20 ms, ODR is 50Hz */ + os_time_delay(20 * OS_TICKS_PER_SEC / 1000 + 1); + + rc = lis2dh12_wait_for_data(itf, 1); + if (rc) { + goto end; + } + + rc = lis2dh12_get_data(itf, 2, &data[0], &data[1], &data[2]); + if (rc) { + goto end; + } + scratch[0] += data[0]; + scratch[1] += data[1]; + scratch[2] += data[2]; + } + + /* Average the stored data on each axis */ + no_st[0] = scratch[0] / read_count; + no_st[1] = scratch[1] / read_count; + no_st[2] = scratch[2] / read_count; + + memset(&scratch, 0, sizeof(scratch)); + + /* Self test mode 0 */ + rc = lis2dh12_set_self_test_mode(itf, LIS2DH12_ST_MODE_MODE0); + if (rc) { + goto end; + } + + /* Wait 90ms */ + os_time_delay(90 * OS_TICKS_PER_SEC / 1000 + 1); + + /* Wait 1 ms for first sample */ + rc = lis2dh12_wait_for_data(itf, 1); + if (rc) { + goto end; + } + + /* Read and discard data */ + rc = lis2dh12_get_data(itf, 2, &data[0], &data[1], &data[2]); + if (rc) { + goto end; + } + + for (i = 0; i < read_count; i++) { + /* Wait at least 20 ms, ODR is 50Hz */ + os_time_delay(20 * OS_TICKS_PER_SEC / 1000 + 1); + + rc = lis2dh12_wait_for_data(itf, 1); + if (rc) { + goto end; + } + + rc = lis2dh12_get_data(itf, 2, &data[0], &data[1], &data[2]); + if (rc) { + goto end; + } + scratch[0] += data[0]; + scratch[1] += data[1]; + scratch[2] += data[2]; + } + + /* Average the stored data on each axis */ + st[0] = scratch[0] / read_count; + st[1] = scratch[1] / read_count; + st[2] = scratch[2] / read_count; + + /* |Min(ST_X)| <=|OUTX_AVG_ST - OUTX_AVG_NO_ST| <= |Max(ST_X)| */ + /* Compare values to thresholds */ + for (i = 0; i < 3; i++) { + diff = abs(st[i] - no_st[i]); + if (diff < LIS2DH12_ST_MIN || diff > LIS2DH12_ST_MAX) { + *result -= 1; + } + } +end: + /* Restore fifo mode if was set */ + if (fifo_ctrl) { + rc2 = lis2dh12_write8(itf, LIS2DH12_REG_FIFO_CTRL_REG, fifo_ctrl); + if (rc == OS_OK && rc2 != OS_OK) { + rc = rc2; + } + } + /* Disable self test mode, and restore other settings */ + rc2 = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG1, prev_config, 4); + if (rc == OS_OK && rc2 != OS_OK) { + rc = rc2; + } + + return rc; +} + +static int disable_interrupt(struct sensor *sensor, uint8_t int_to_disable, uint8_t int_num) { struct lis2dh12 *lis2dh12; diff --git a/hw/drivers/sensors/lis2dh12/src/lis2dh12_priv.h b/hw/drivers/sensors/lis2dh12/src/lis2dh12_priv.h index 035a2ce..643e7f7 100644 --- a/hw/drivers/sensors/lis2dh12/src/lis2dh12_priv.h +++ b/hw/drivers/sensors/lis2dh12/src/lis2dh12_priv.h @@ -174,6 +174,9 @@ extern "C" { #define LIS2DH12_SPI_ADDR_INC 0x40 #define LIS2DH12_I2C_ADDR_INC 0x80 +#define LIS2DH12_ST_MAX 1440 +#define LIS2DH12_ST_MIN 68 + int lis2dh12_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload, uint8_t len); int lis2dh12_readlen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload, uint8_t len); int lis2dh12_write8(struct sensor_itf *itf, uint8_t reg, uint8_t value);
