You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@mynewt.apache.org by GitBox <gi...@apache.org> on 2018/05/20 06:48:15 UTC
[GitHub] vrahane closed pull request #1088: fix self test
vrahane closed pull request #1088: fix self test
URL: https://github.com/apache/mynewt-core/pull/1088
This is a PR merged from a forked repository.
As GitHub hides the original diff on merge, it is displayed below for
the sake of provenance:
As this is a foreign pull request (from a fork), the diff is supplied
below (as it won't show otherwise due to GitHub magic):
diff --git a/hw/drivers/sensors/lis2dw12/include/lis2dw12/lis2dw12.h b/hw/drivers/sensors/lis2dw12/include/lis2dw12/lis2dw12.h
index e264eaf5bc..49315acc23 100644
--- a/hw/drivers/sensors/lis2dw12/include/lis2dw12/lis2dw12.h
+++ b/hw/drivers/sensors/lis2dw12/include/lis2dw12/lis2dw12.h
@@ -106,6 +106,8 @@ extern "C" {
#define LIS2DW12_SIXD_SRC_YL 0x04
#define LIS2DW12_SIXD_SRC_XH 0x02
#define LIS2DW12_SIXD_SRC_XL 0x01
+#define LIS2DW12_ST_MAX 1500
+#define LIS2DW12_ST_MIN 70
enum lis2dw12_ths_6d {
LIS2DW12_6D_THS_80_DEG = 0,
diff --git a/hw/drivers/sensors/lis2dw12/src/lis2dw12.c b/hw/drivers/sensors/lis2dw12/src/lis2dw12.c
index 1811c4ab32..703ac4ffcb 100644
--- a/hw/drivers/sensors/lis2dw12/src/lis2dw12.c
+++ b/hw/drivers/sensors/lis2dw12/src/lis2dw12.c
@@ -104,33 +104,131 @@ static const struct sensor_driver g_lis2dw12_sensor_driver = {
};
/**
- * Writes a single byte to the specified register using i2c
+ * Write multiple length data to LIS2DW12 sensor over I2C (MAX: 19 bytes)
*
* @param The sensor interface
- * @param The register address to write to
- * @param The value to write
+ * @param register address
+ * @param variable length payload
+ * @param length of the payload to write
*
- * @return 0 on success, non-zero error on failure.
+ * @return 0 on success, non-zero on failure
*/
-int
-lis2dw12_i2c_write8(struct sensor_itf *itf, uint8_t reg, uint8_t value)
+static int
+lis2dw12_i2c_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *buffer,
+ uint8_t len)
{
int rc;
- uint8_t payload[2] = { reg, value };
+ uint8_t payload[20] = { addr, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0};
struct hal_i2c_master_data data_struct = {
.address = itf->si_addr,
- .len = 2,
+ .len = len + 1,
.buffer = payload
};
- rc = hal_i2c_master_write(itf->si_num, &data_struct,
- OS_TICKS_PER_SEC / 10, 1);
+ if (len > (sizeof(payload) - 1)) {
+ rc = OS_EINVAL;
+ goto err;
+ }
+
+ memcpy(&payload[1], buffer, len);
+ /* Register write */
+ rc = hal_i2c_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1);
if (rc) {
- LIS2DW12_ERR("Failed to write to 0x%02X:0x%02X with value 0x%02X\n",
- itf->si_addr, reg, value);
- STATS_INC(g_lis2dw12stats, read_errors);
+ LIS2DW12_ERR("I2C access failed at address 0x%02X\n", data_struct.address);
+ STATS_INC(g_lis2dw12stats, write_errors);
+ goto err;
+ }
+
+ return 0;
+err:
+ return rc;
+}
+
+/**
+ * Write multiple length data to LIS2DW12 sensor over SPI
+ *
+ * @param The sensor interface
+ * @param register address
+ * @param variable length payload
+ * @param length of the payload to write
+ *
+ * @return 0 on success, non-zero on failure
+ */
+static int
+lis2dw12_spi_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
+ uint8_t len)
+{
+ int i;
+ int rc;
+
+ /*
+ * Auto register address increment is needed if the length
+ * requested is moret than 1
+ */
+ if (len > 1) {
+ addr |= LIS2DW12_SPI_READ_CMD_BIT;
+ }
+
+ /* Select the device */
+ hal_gpio_write(itf->si_cs_pin, 0);
+
+
+ /* Send the address */
+ rc = hal_spi_tx_val(itf->si_num, addr);
+ if (rc == 0xFFFF) {
+ rc = SYS_EINVAL;
+ LIS2DW12_ERR("SPI_%u register write failed addr:0x%02X\n",
+ itf->si_num, addr);
+ STATS_INC(g_lis2dw12stats, write_errors);
+ goto err;
+ }
+
+ for (i = 0; i < len; i++) {
+ /* Read data */
+ rc = hal_spi_tx_val(itf->si_num, payload[i]);
+ if (rc == 0xFFFF) {
+ rc = SYS_EINVAL;
+ LIS2DW12_ERR("SPI_%u write failed addr:0x%02X:0x%02X\n",
+ itf->si_num, addr);
+ STATS_INC(g_lis2dw12stats, write_errors);
+ goto err;
+ }
+ }
+
+
+ rc = 0;
+
+err:
+ /* De-select the device */
+ hal_gpio_write(itf->si_cs_pin, 1);
+
+ return rc;
+}
+
+/**
+ * Write multiple length data to LIS2DW12 sensor over different interfaces
+ *
+ * @param The sensor interface
+ * @param register address
+ * @param variable length payload
+ * @param length of the payload to write
+ *
+ * @return 0 on success, non-zero on failure
+ */
+int
+lis2dw12_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
+ uint8_t len)
+{
+ int rc;
+
+ if (itf->si_type == SENSOR_ITF_I2C) {
+ rc = lis2dw12_i2c_writelen(itf, addr, payload, len);
+ } else {
+ rc = lis2dw12_spi_writelen(itf, addr, payload, len);
}
return rc;
@@ -180,52 +278,6 @@ lis2dw12_i2c_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer, uint8
return rc;
}
-/**
- * Writes a single byte to the specified register using SPI
- *
- * @param The sensor interface
- * @param The register address to write to
- * @param The value to write
- *
- * @return 0 on success, non-zero error on failure.
- */
-int
-lis2dw12_spi_write8(struct sensor_itf *itf, uint8_t reg, uint8_t value)
-{
- int rc;
-
- /* Select the device */
- hal_gpio_write(itf->si_cs_pin, 0);
-
- /* Send the address */
- rc = hal_spi_tx_val(itf->si_num, reg & ~LIS2DW12_SPI_READ_CMD_BIT);
- if (rc == 0xFFFF) {
- rc = SYS_EINVAL;
- LIS2DW12_ERR("SPI_%u register write failed addr:0x%02X\n",
- itf->si_num, reg);
- STATS_INC(g_lis2dw12stats, write_errors);
- goto err;
- }
-
- /* Read data */
- rc = hal_spi_tx_val(itf->si_num, value);
- if (rc == 0xFFFF) {
- rc = SYS_EINVAL;
- LIS2DW12_ERR("SPI_%u write failed addr:0x%02X\n",
- itf->si_num, reg);
- STATS_INC(g_lis2dw12stats, write_errors);
- goto err;
- }
-
- rc = 0;
-
-err:
- /* De-select the device */
- hal_gpio_write(itf->si_cs_pin, 1);
-
- return rc;
-}
-
/**
* Read multiple bytes starting from specified register over SPI
*
@@ -294,9 +346,9 @@ lis2dw12_write8(struct sensor_itf *itf, uint8_t reg, uint8_t value)
int rc;
if (itf->si_type == SENSOR_ITF_I2C) {
- rc = lis2dw12_i2c_write8(itf, reg, value);
+ rc = lis2dw12_i2c_writelen(itf, reg, &value, 1);
} else {
- rc = lis2dw12_spi_write8(itf, reg, value);
+ rc = lis2dw12_spi_writelen(itf, reg, &value, 1);
}
return rc;
@@ -1863,60 +1915,92 @@ int lis2dw12_get_int1_on_int2_map(struct sensor_itf *itf, uint8_t *val)
int lis2dw12_run_self_test(struct sensor_itf *itf, int *result)
{
int rc;
- int16_t base[3], pos[3];
+ /*configure min and max values for reading 5 samples, and accounting for
+ * both negative and positive offset */
+ int min = LIS2DW12_ST_MIN*5*2;
+ int max = LIS2DW12_ST_MAX*5*2;
+
+ int16_t data[3], diff[3] = {0,0,0};
int i;
- int16_t change;
- uint8_t fs;
+ uint8_t prev_config[6];
+ /* set config per datasheet, with positive self test mode enabled. */
+ uint8_t st_config[] = {0x44, 0x08, 0x40, 0x00, 0x00, 0x10};
- rc = lis2dw12_get_fs(itf, &fs);
+ rc = lis2dw12_readlen(itf, LIS2DW12_REG_CTRL_REG1, prev_config, 6);
if (rc) {
return rc;
}
-
- /* ensure self test mode is disabled */
- rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_DISABLE);
+ rc = lis2dw12_writelen(itf, LIS2DW12_REG_CTRL_REG2, &st_config[1], 5);
+ rc = lis2dw12_writelen(itf, LIS2DW12_REG_CTRL_REG1, st_config, 1);
if (rc) {
return rc;
}
- os_time_delay(OS_TICKS_PER_SEC / 10);
-
- /* take base reading */
- rc = lis2dw12_get_data(itf, fs, &(base[0]), &(base[1]), &(base[2]));
+ /* go into self test mode 1 */
+ rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_MODE1);
if (rc) {
return rc;
}
- /* set self test mode to positive self test */
- rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_MODE1);
+ /* wait 100ms */
+ os_time_delay(OS_TICKS_PER_SEC / 100);
+ rc = lis2dw12_get_data(itf, 2, &(data[0]), &(data[1]), &(data[2]));
if (rc) {
return rc;
}
- os_time_delay(OS_TICKS_PER_SEC / 10);
+ /* take positive offset reading */
+ for(i=0; i<5; i++) {
- /* take self test reading */
- rc = lis2dw12_get_data(itf, fs, &(pos[0]), &(pos[1]), &(pos[2]));
+ rc = lis2dw12_get_data(itf, 2, &(data[0]), &(data[1]), &(data[2]));
+ if (rc) {
+ return rc;
+ }
+ diff[0] += data[0];
+ diff[1] += data[1];
+ diff[2] += data[2];
+ /* wait at least 20 ms */
+ os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
+ }
+
+ /* go into self test mode 2 */
+ rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_MODE2);
if (rc) {
return rc;
}
- /* disable self test mod */
- rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_DISABLE);
+ os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
+ rc = lis2dw12_get_data(itf, 2, &(data[0]), &(data[1]), &(data[2]));
if (rc) {
return rc;
}
- /* calculate accel data difference */
- change = 0;
- for(i = 0; i < 3; i++) {
- change += pos[i] - base[i];
- }
+ /* take negative offset reading */
+ for (i=0; i<5; i++) {
- if ((change > 70) && (change < 1500)) {
- *result = 0;
- } else {
- *result = -1;
+ rc = lis2dw12_get_data(itf, 2, &(data[0]), &(data[1]), &(data[2]));
+ if (rc) {
+ return rc;
+ }
+ diff[0] -= data[0];
+ diff[1] -= data[1];
+ diff[2] -= data[2];
+ /* wait at least 20 ms */
+ os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
+ }
+
+ /* disable self test mod */
+ rc = lis2dw12_writelen(itf, LIS2DW12_REG_CTRL_REG1, prev_config, 6);
+ if (rc) {
+ return rc;
+ }
+
+ /* compare values to thresholds */
+ *result = 0;
+ for (i = 0; i < 3; i++) {
+ if ((diff[i] < min) || (diff[i] > max)) {
+ *result -= 1;
+ }
}
return 0;
----------------------------------------------------------------
This is an automated message from the Apache Git Service.
To respond to the message, please log on GitHub and use the
URL above to go to the specific comment.
For queries about this service, please contact Infrastructure at:
users@infra.apache.org
With regards,
Apache Git Services