/**************************************************************************** * drivers/sensors/lsm6dsl.c * * Copyright (C) 2018 Inc. All rights reserved. * Author: Ben vd Veen * Alias: DisruptiveNL * * Based on: * * Copyright (C) 2016 Omni Hoverboards Inc. All rights reserved. * Author: Paul Alexander Patience * * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name NuttX nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /**************************************************************************** * Included Files ****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #if defined(CONFIG_I2C) && defined(CONFIG_SENSORS_LSM6DSL) /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ #ifndef CONFIG_LSM6DSL_I2C_FREQUENCY # define CONFIG_LSM6DSL_I2C_FREQUENCY 400000 #endif /**************************************************************************** * Private Function Prototypes ****************************************************************************/ /* I2C Helpers */ static int lsm6dsl_readreg8(FAR struct lsm6dsl_dev_s *priv, uint8_t regaddr, FAR uint8_t * regval); static int lsm6dsl_writereg8(FAR struct lsm6dsl_dev_s *priv, uint8_t regaddr, uint8_t regval); static int lsm6dsl_modifyreg8(FAR struct lsm6dsl_dev_s *priv, uint8_t regaddr, uint8_t clearbits, uint8_t setbits); /* Other Helpers */ static int lsm6dsl_find_minimum(int16_t a[], int n); static int lsm6dsl_find_maximum(int16_t a[], int n); static bool lsm6dsl_isbitset(int8_t b, int8_t n); /* Accelerometer Operations */ static int lsm6dsl_sensor_config(FAR struct lsm6dsl_dev_s *priv); static int lsm6dsl_sensor_start(FAR struct lsm6dsl_dev_s *priv); static int lsm6dsl_sensor_stop(FAR struct lsm6dsl_dev_s *priv); static int lsm6dsl_sensor_read(FAR struct lsm6dsl_dev_s *priv, FAR struct lsm6dsl_sensor_data_s *sdata); static int lsm6dsl_selftest(FAR struct lsm6dsl_dev_s *priv, uint32_t mode); /* Character Driver Methods */ static int lsm6dsl_open(FAR struct file *filep); static int lsm6dsl_close(FAR struct file *filep); static ssize_t lsm6dsl_read(FAR struct file *filep, FAR char *buffer, size_t buflen); static ssize_t lsm6dsl_write(FAR struct file *filep, FAR const char *buffer, size_t buflen); static int lsm6dsl_ioctl(FAR struct file *filep, int cmd, unsigned long arg); /* Common Register Function */ static int lsm6dsl_register(FAR const char *devpath, FAR struct i2c_master_s *i2c, uint8_t addr, FAR const struct lsm6dsl_ops_s *ops, uint8_t datareg, struct lsm6dsl_sensor_data_s sensor_data); /**************************************************************************** * Private Data ****************************************************************************/ static double g_accelerofactor = 0; static double g_gyrofactor = 0; static const struct file_operations g_fops = { lsm6dsl_open, lsm6dsl_close, lsm6dsl_read, lsm6dsl_write, NULL, lsm6dsl_ioctl, NULL #ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS , NULL # endif }; static const struct lsm6dsl_ops_s g_lsm6dsl_sensor_ops = { lsm6dsl_sensor_config, lsm6dsl_sensor_start, lsm6dsl_sensor_stop, lsm6dsl_sensor_read, lsm6dsl_selftest }; /**************************************************************************** * Private Functions ****************************************************************************/ /**************************************************************************** * Name: lsm6dsl_resetsensor * * Description: * Reset sensor values * ****************************************************************************/ static void lsm6dsl_resetsensor(FAR struct lsm6dsl_dev_s *priv) { priv->sensor_data.x_data = 0; priv->sensor_data.y_data = 0; priv->sensor_data.z_data = 0; priv->sensor_data.temperature = 0; priv->sensor_data.g_x_data = 0; priv->sensor_data.g_y_data = 0; priv->sensor_data.g_z_data = 0; priv->sensor_data.timestamp = 0; } /**************************************************************************** * Name: lsm6dsl_readreg8 * * Description: * Read from an 8-bit register. * ****************************************************************************/ static int lsm6dsl_readreg8(FAR struct lsm6dsl_dev_s *priv, uint8_t regaddr, FAR uint8_t * regval) { struct i2c_config_s config; int ret; /* Sanity check */ DEBUGASSERT(priv != NULL); DEBUGASSERT(regval != NULL); /* Set up the I2C configuration */ config.frequency = CONFIG_LSM6DSL_I2C_FREQUENCY; config.address = priv->addr; config.addrlen = 7; /* Write the register address */ ret = i2c_write(priv->i2c, &config, ®addr, sizeof(regaddr)); if (ret < 0) { snerr("ERROR: i2c_write failed: %d\n", ret); return ret; } /* Restart and read 8 bits from the register */ ret = i2c_read(priv->i2c, &config, regval, sizeof(*regval)); if (ret < 0) { snerr("ERROR: i2c_read failed: %d\n", ret); return ret; } sninfo("addr: %02x value: %02x\n", regaddr, *regval); return OK; } /**************************************************************************** * Name: lsm6dsl_writereg8 * * Description: * Write to an 8-bit register. * ****************************************************************************/ static int lsm6dsl_writereg8(FAR struct lsm6dsl_dev_s *priv, uint8_t regaddr, uint8_t regval) { struct i2c_config_s config; uint8_t buffer[2]; int ret; /* Sanity check */ DEBUGASSERT(priv != NULL); /* Set up a 2-byte message to send */ buffer[0] = regaddr; buffer[1] = regval; /* Set up the I2C configuration */ config.frequency = CONFIG_LSM6DSL_I2C_FREQUENCY; config.address = priv->addr; config.addrlen = 7; /* Write the register address followed by the data (no RESTART) */ ret = i2c_write(priv->i2c, &config, buffer, sizeof(buffer)); if (ret < 0) { snerr("ERROR: i2c_write failed: %d\n", ret); return ret; } sninfo("addr: %02x value: %02x\n", regaddr, regval); return OK; } /**************************************************************************** * Name: lsm6dsl_modifyreg8 * * Description: * Modify an 8-bit register. * ****************************************************************************/ static int lsm6dsl_modifyreg8(FAR struct lsm6dsl_dev_s *priv, uint8_t regaddr, uint8_t clearbits, uint8_t setbits) { int ret; uint8_t regval; /* Sanity check */ DEBUGASSERT(priv != NULL); ret = lsm6dsl_readreg8(priv, regaddr, ®val); if (ret < 0) { snerr("ERROR: lsm6dsl_readreg8 failed: %d\n", ret); return ret; } regval &= ~clearbits; regval |= setbits; ret = lsm6dsl_writereg8(priv, regaddr, regval); if (ret < 0) { snerr("ERROR: lsm6dsl_writereg8 failed: %d\n", ret); return ret; } return OK; } /**************************************************************************** * Name: lsm6dsl_find_minimum * * Description: * Find the minimum value in an array of numbers. * ****************************************************************************/ static int lsm6dsl_find_minimum(int16_t a[], int n) { int c; int min = a[0]; int index = 0; for (c = 1; c < n; c++) { if (a[c] < min) { index = c; min = a[c]; } } return index; } /**************************************************************************** * Name: lsm6dsl_find_maximum * * Description: * Find the maximum value in an array of numbers. * ****************************************************************************/ static int lsm6dsl_find_maximum(int16_t am[], int n) { int c; int max = am[0]; int index = 0; for (c = 1; c < n; c++) { if (am[c] > max) { index = c; max = am[c]; } } return index; } /**************************************************************************** * Name: lsm6dsl_sensor_config * * Description: * Configure the accelerometer and gyroscope. * ****************************************************************************/ static int lsm6dsl_sensor_config(FAR struct lsm6dsl_dev_s *priv) { int ret; uint8_t regval; /* Sanity check */ DEBUGASSERT(priv != NULL); /* Get the device identification */ ret = lsm6dsl_readreg8(priv, LSM6DSL_WHO_AM_I, ®val); if (ret < 0) { snerr("ERROR: lsm6dsl_readreg8 failed: %d\n", ret); return ret; } if (regval != LSM6DSL_WHO_AM_I_VALUE) { snerr("ERROR: Invalid device identification %02x\n", regval); return -ENODEV; } return OK; } /**************************************************************************** * Name: lsm6dsl_isbitset * * Description: * Check if bit is set from mask, not bit number. * ****************************************************************************/ static bool lsm6dsl_isbitset(int8_t b, int8_t m) { if ((b & m) != 0) { return true; } return false; } /**************************************************************************** * Name: lsm6dsl_sensor_start * * Description: * Start the accelerometer. * ****************************************************************************/ static int lsm6dsl_sensor_start(FAR struct lsm6dsl_dev_s *priv) { /* Enable the accelerometer */ /* Reset values */ lsm6dsl_resetsensor(priv); /* Sanity check */ DEBUGASSERT(priv != NULL); sninfo("Starting...."); /* Accelerometer config registers: * Turn on the accelerometer: 833Hz, +- 16g */ lsm6dsl_writereg8(priv, LSM6DSL_CTRL1_XL, 0x74); g_accelerofactor = 0.488; /* Gyro config registers Turn on the gyro: FS=2000dps, ODR=833Hz Not using * modifyreg with empty value!!!! Then read value first!!! */ lsm6dsl_writereg8(priv, LSM6DSL_CTRL2_G, 0x7c); g_gyrofactor = 70; lsm6dsl_writereg8(priv, LSM6DSL_CTRL6_C, 0x00); /* Timestamp registers */ lsm6dsl_writereg8(priv, LSM6DSL_CTRL10_C, 0x20); return OK; } /**************************************************************************** * Name: lsm6dsl_sensor_stop * * Description: * Stop the accelerometer. * ****************************************************************************/ static int lsm6dsl_sensor_stop(FAR struct lsm6dsl_dev_s *priv) { /* Sanity check */ DEBUGASSERT(priv != NULL); /* Stop accelerometer */ lsm6dsl_modifyreg8(priv, LSM6DSL_CTRL1_XL_ODR_XL_SHIFT, LSM6DSL_CTRL1_XL_ODR_XL_MASK, LSM6DSL_CTRL1_XL_ODR_XL_POWER_DOWN); /* Stop gyro */ lsm6dsl_modifyreg8(priv, LSM6DSL_CTRL2_G_ODR_G_SHIFT, LSM6DSL_CTRL2_G_ODR_G_MASK, LSM6DSL_CTRL2_G_ODR_G_POWER_DOWN); return OK; } /**************************************************************************** * Name: lsm6dsl_selftest * * Description: * Selftesting the sensor. * Mode 0 = selftest accelerometer and mode 1 = selftest gyro * ****************************************************************************/ static int lsm6dsl_selftest(FAR struct lsm6dsl_dev_s *priv, uint32_t mode) { int samples = 5; int i; int i2; int i3; uint8_t value = 0; int8_t lox = 0; int8_t loxst = 0; int8_t hix = 0; int8_t hixst = 0; int8_t loy = 0; int8_t loyst = 0; int8_t hiy = 0; int8_t hiyst = 0; int8_t loz = 0; int8_t lozst = 0; int8_t hiz = 0; int8_t hizst = 0; int16_t OUTX_NOST[samples]; int16_t OUTY_NOST[samples]; int16_t OUTZ_NOST[samples]; int16_t OUTX_ST[samples]; int16_t OUTY_ST[samples]; int16_t OUTZ_ST[samples]; int16_t avr_x = 0; int16_t avr_y = 0; int16_t avr_z = 0; int16_t avr_xst = 0; int16_t avr_yst = 0; int16_t avr_zst = 0; int16_t min_x = 0; int16_t min_y = 0; int16_t min_z = 0; int16_t max_x = 0; int16_t max_y = 0; int16_t max_z = 0; int16_t min_xst = 0; int16_t min_yst = 0; int16_t min_zst = 0; int16_t max_xst = 0; int16_t max_yst = 0; int16_t max_zst = 0; int16_t raw_x = 0; int16_t raw_y = 0; int16_t raw_z = 0; int16_t raw_xst = 0; int16_t raw_yst = 0; int16_t raw_zst = 0; /* mode = 0 then add hex 0x06 to OUT registers */ int8_t registershift; /* Keep the device still during the self-test procedure. Setting registers * Power up, wait for 100ms for stable output. */ if (mode == 0) { registershift = 0x06; /* Accelero ; power down gyro CTRL2_G 4g factor: 1000 for mg -> g value * is in mg/LSB FS=4g,52Hz 4000mg=65535 */ lsm6dsl_writereg8(priv, LSM6DSL_CTRL1_XL, 0x38); lsm6dsl_writereg8(priv, LSM6DSL_CTRL2_G, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL3_C, 0x44); g_accelerofactor = (0.122 / 1000); } else { registershift = 0x00; /* Gyro; power down accelero CTRL1_XL FS=2000dps,208Hz 2000dps=65535 */ lsm6dsl_writereg8(priv, LSM6DSL_CTRL1_XL, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL2_G, 0x5c); lsm6dsl_writereg8(priv, LSM6DSL_CTRL3_C, 0x44); g_gyrofactor = (70 / 1000); /* 2000dps */ } lsm6dsl_writereg8(priv, LSM6DSL_CTRL4_C, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL6_C, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL7_G, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL8_XL, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL9_XL, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL10_C, 0x00); nxsig_usleep(100000); /* 100ms */ /* Read the output registers after checking XLDA bit 5 times */ bool checkbit = false; /* Wait until first sample and data is available */ while (checkbit) { lsm6dsl_readreg8(priv, LSM6DSL_STATUS_REG, &value); if (mode == 0) { checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_XLDA); } else { checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_GDA); } } nxsig_usleep(100000); /* 100ms */ /* Read OUT registers Gyro is starting at 22h and Accelero at 28h */ lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G + registershift, (FAR uint8_t *)&lox); lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G + registershift, (FAR uint8_t *)&hix); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G + registershift, (FAR uint8_t *)&loy); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G + registershift, (FAR uint8_t *)&hiy); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G + registershift, (FAR uint8_t *)&loz); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G + registershift, (FAR uint8_t *)&hiz); /* check XLDA 5 times */ for (i = 0; i < samples; i++) { lsm6dsl_readreg8(priv, LSM6DSL_STATUS_REG, &value); if (mode == 0) { checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_XLDA); } else { checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_GDA); } /* Average the stored data on each axis * http://ozzmaker.com/accelerometer-to-g/ */ lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G + registershift, (FAR uint8_t *)&lox); lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G + registershift, (FAR uint8_t *)&hix); raw_x = (int16_t) (((uint16_t) hix << 8U) | (uint16_t) lox); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G + registershift, (FAR uint8_t *)&loy); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G + registershift, (FAR uint8_t *)&hiy); raw_y = (int16_t) (((uint16_t) hiy << 8U) | (uint16_t) loy); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G + registershift, (FAR uint8_t *)&loz); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G + registershift, (FAR uint8_t *)&hiz); raw_z = (int16_t) (((uint16_t) hiz << 8U) | (uint16_t) loz); /* Selftest only uses raw values */ OUTX_NOST[i] = raw_x; OUTY_NOST[i] = raw_y; OUTZ_NOST[i] = raw_z; } /* Enable Selftest */ if (mode == 0) { lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x01); } else { lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x04); } nxsig_usleep(100000); /* 100ms */ checkbit = false; while (checkbit) /* wait until first sample and data is * available */ { lsm6dsl_readreg8(priv, LSM6DSL_STATUS_REG, &value); if (mode == 0) { checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_XLDA); } else { checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_GDA); } } nxsig_usleep(100000); /* 100ms */ /* Now do all the ST values */ lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G + registershift, (FAR uint8_t *)&loxst); lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G + registershift, (FAR uint8_t *)&hixst); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G + registershift, (FAR uint8_t *)&loyst); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G + registershift, (FAR uint8_t *)&hiyst); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G + registershift, (FAR uint8_t *)&lozst); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G + registershift, (FAR uint8_t *)&hizst); for (i2 = 0; i2 < samples; i2++) { lsm6dsl_readreg8(priv, LSM6DSL_STATUS_REG, &value); if (mode == 0) { checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_XLDA); } else { checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_GDA); } nxsig_usleep(100000); /* 100ms */ lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G + registershift, (FAR uint8_t *)&loxst); lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G + registershift, (FAR uint8_t *)&hixst); raw_xst = (int16_t) (((uint16_t) hixst << 8U) | (uint16_t) loxst); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G + registershift, (FAR uint8_t *)&loyst); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G + registershift, (FAR uint8_t *)&hiyst); raw_yst = (int16_t) (((uint16_t) hiyst << 8U) | (uint16_t) loyst); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G + registershift, (FAR uint8_t *)&lozst); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G + registershift, (FAR uint8_t *)&hizst); raw_zst = (int16_t) (((uint16_t) hizst << 8U) | (uint16_t) lozst); /* Selftest only uses raw values */ OUTX_ST[i2] = raw_xst; OUTY_ST[i2] = raw_yst; OUTZ_ST[i2] = raw_zst; } /* Average stored data on each axis */ for (i3 = 0; i3 < samples; i3++) { avr_x = avr_x + (int16_t) OUTX_NOST[i3]; avr_y = avr_y + (int16_t) OUTY_NOST[i3]; avr_z = avr_z + (int16_t) OUTZ_NOST[i3]; avr_xst = avr_xst + (int16_t) OUTX_ST[i3]; avr_yst = avr_yst + (int16_t) OUTY_ST[i3]; avr_zst = avr_zst + (int16_t) OUTZ_ST[i3]; } avr_x = (int16_t) avr_x / samples; avr_y = (int16_t) avr_y / samples; avr_z = (int16_t) avr_z / samples; avr_xst = (int16_t) avr_xst / samples; avr_yst = (int16_t) avr_yst / samples; avr_zst = (int16_t) avr_zst / samples; min_x = OUTX_NOST[lsm6dsl_find_minimum(OUTX_NOST, samples)]; min_y = OUTY_NOST[lsm6dsl_find_minimum(OUTY_NOST, samples)]; min_z = OUTZ_NOST[lsm6dsl_find_minimum(OUTZ_NOST, samples)]; max_x = OUTX_NOST[lsm6dsl_find_maximum(OUTX_NOST, samples)]; max_y = OUTY_NOST[lsm6dsl_find_maximum(OUTY_NOST, samples)]; max_z = OUTZ_NOST[lsm6dsl_find_maximum(OUTZ_NOST, samples)]; min_xst = OUTX_ST[lsm6dsl_find_minimum(OUTX_ST, samples)]; min_yst = OUTY_ST[lsm6dsl_find_minimum(OUTY_ST, samples)]; min_zst = OUTZ_ST[lsm6dsl_find_minimum(OUTZ_ST, samples)]; max_xst = OUTX_ST[lsm6dsl_find_maximum(OUTX_ST, samples)]; max_yst = OUTY_ST[lsm6dsl_find_maximum(OUTY_ST, samples)]; max_zst = OUTZ_ST[lsm6dsl_find_maximum(OUTZ_ST, samples)]; sninfo("stdev_x: -%d %d +%d\n", avr_x - min_x, avr_x, max_x - avr_x); sninfo("stdev_y: -%d %d +%d\n", avr_y - min_y, avr_y, max_y - avr_y); sninfo("stdev_z: -%d %d +%d\n", avr_z - min_z, avr_z, max_z - avr_z); sninfo("stdev_xst: -%d %d +%d\n", avr_xst - min_xst, avr_xst, max_xst - avr_xst); sninfo("stdev_yst: -%d %d +%d\n", avr_yst - min_yst, avr_yst, max_yst - avr_yst); sninfo("stdev_zst: -%d %d +%d\n", avr_zst - min_zst, avr_zst, max_zst - avr_zst); sninfo("avr_x: %d\n", avr_x); sninfo("avr_y: %d\n", avr_y); sninfo("avr_z: %d\n", avr_z); sninfo("min_x: %d\n", min_x); sninfo("max_x: %d\n", max_x); sninfo("min_xst: %d\n", min_xst); sninfo("max_xst: %d\n", max_xst); /* Validation Question is placed at ST FAE because the equation in the * datasheet is doubtful. */ if ((avr_x >= min_x && avr_x <= max_x) && (avr_xst >= min_xst && avr_xst <= max_xst)) { sninfo("PASSED NOST AND ST FOR X!\n"); } else { sninfo("FAILED NOST AND ST FOR X!\n"); sninfo("[ %d - %d ]", min_x, min_xst); sninfo(" <=\n "); sninfo("[ %d - %d ]", avr_x, avr_xst); sninfo(" <=\n "); sninfo("[ %d - %d ]", max_x, max_xst); sninfo("\n"); } if ((avr_y >= min_y && avr_y <= max_y) && (avr_yst >= min_yst && avr_yst <= max_yst)) { sninfo("PASSED NOST AND ST FOR Y!\n"); } else { sninfo("FAILED NOST AND ST FOR Y!\n"); sninfo("[ %d - %d ]", min_y, min_yst); sninfo(" <=\n "); sninfo("[ %d - %d ]", avr_y, avr_yst); sninfo(" <=\n "); sninfo("[ %d - %d ]", max_y, max_yst); sninfo("\n"); } if ((avr_z >= min_z && avr_z <= max_z) && (avr_zst >= min_zst && avr_zst <= max_zst)) { sninfo("PASSED NOST AND ST FOR Z!\n"); } else { sninfo("FAILED NOST AND ST FOR Z!\n"); sninfo("[ %d - %d ]", min_z, min_zst); sninfo(" <=\n "); sninfo("[ %d - %d ]", avr_z, avr_zst); sninfo(" <=\n "); sninfo("[ %d - %d ]", max_z, max_zst); sninfo("\n"); } nxsig_sleep(2); /* Disable test */ switch (mode) { case 0: { sninfo("SELFTEST ACCELERO DISABLED\n"); lsm6dsl_writereg8(priv, LSM6DSL_CTRL1_XL, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x00); } break; case 1: { sninfo("SELFTEST GYRO DISABLED\n"); lsm6dsl_writereg8(priv, LSM6DSL_CTRL2_G, 0x00); lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x00); } break; default: break; } return OK; } /**************************************************************************** * Name: lsm6dsl_sensor_read * * Description: * Read the sensor. * A sensor in a steady state on a horizontal surface will * measure 0 g on both the X-axis and Y-axis, whereas the Z-axis will * measure 1 g. (page 30 datasheet). The X- and Y-axis have an offset * of 40 mg/LSB * ****************************************************************************/ static int lsm6dsl_sensor_read(FAR struct lsm6dsl_dev_s *priv, FAR struct lsm6dsl_sensor_data_s *sdata) { int16_t lox = 0; int16_t loxg = 0; int16_t hix = 0; int16_t hixg = 0; int16_t loy = 0; int16_t loyg = 0; int16_t hiy = 0; int16_t hiyg = 0; int16_t loz = 0; int16_t lozg = 0; int16_t hiz = 0; int16_t hizg = 0; int16_t templ = 0; int16_t temph = 0; uint8_t tstamp0 = 0; uint8_t tstamp1 = 0; uint8_t tstamp2 = 0; uint32_t ts = 0; int16_t tempi = 0; int16_t temp_val = 0; int16_t x_valg = 0; int16_t y_valg = 0; int16_t z_valg = 0; int16_t xf_val = 0; int16_t yf_val = 0; int16_t zf_val = 0; /* Accelerometer */ lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_XL, (FAR uint8_t *)&lox); lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_XL, (FAR uint8_t *)&hix); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_XL, (FAR uint8_t *)&loy); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_XL, (FAR uint8_t *)&hiy); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_XL, (FAR uint8_t *)&loz); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_XL, (FAR uint8_t *)&hiz); /* Gyro */ lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G, (FAR uint8_t *)&loxg); lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G, (FAR uint8_t *)&hixg); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G, (FAR uint8_t *)&loyg); lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G, (FAR uint8_t *)&hiyg); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G, (FAR uint8_t *)&lozg); lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G, (FAR uint8_t *)&hizg); /* Timestamp */ lsm6dsl_readreg8(priv, LSM6DSL_TIMESTAMP0_REG, &tstamp0); lsm6dsl_readreg8(priv, LSM6DSL_TIMESTAMP1_REG, &tstamp1); lsm6dsl_readreg8(priv, LSM6DSL_TIMESTAMP2_REG, &tstamp2); ts = (tstamp2 << 16) | (tstamp1 << 8) | tstamp0; /* Temperature */ lsm6dsl_readreg8(priv, LSM6DSL_OUT_TEMP_L, (FAR uint8_t *)&templ); lsm6dsl_readreg8(priv, LSM6DSL_OUT_TEMP_H, (FAR uint8_t *)&temph); xf_val = (int16_t) ((hix << 8) | lox); yf_val = (int16_t) ((hiy << 8) | loy); zf_val = (int16_t) ((hiz << 8) | loz); tempi = (int16_t) ((((int16_t) temph << 8) | (int16_t) templ)); temp_val = (tempi / 256) + 25; sninfo("Data 16-bit XL_X--->: %d mg\n", (short)(xf_val * g_accelerofactor)); sninfo("Data 16-bit XL_Y--->: %d mg\n", (short)(yf_val * g_accelerofactor)); sninfo("Data 16-bit XL_Z--->: %d mg\n", (short)(zf_val * g_accelerofactor)); sninfo("Data 16-bit TEMP--->: %d Celsius\n", temp_val); sdata->x_data = xf_val * g_accelerofactor; sdata->y_data = yf_val * g_accelerofactor; sdata->z_data = zf_val * g_accelerofactor; sdata->temperature = temp_val; sdata->timestamp = ts; x_valg = (int16_t) (((hixg) << 8) | loxg); y_valg = (int16_t) (((hiyg) << 8) | loyg); z_valg = (int16_t) (((hizg) << 8) | lozg); sninfo("Data 16-bit G_X--->: %d mdps\n", (short)(x_valg * g_gyrofactor)); sninfo("Data 16-bit G_Y--->: %d mdps\n", (short)(y_valg * g_gyrofactor)); sninfo("Data 16-bit G_Z--->: %d mdps\n", (short)(z_valg * g_gyrofactor)); sdata->g_x_data = x_valg * g_gyrofactor; sdata->g_y_data = y_valg * g_gyrofactor; sdata->g_z_data = z_valg * g_gyrofactor; return OK; } /**************************************************************************** * Name: lsm6dsl_open * * Description: * This method is called when the device is opened. * ****************************************************************************/ static int lsm6dsl_open(FAR struct file *filep) { sninfo("Device LSM6DSL opened!!\n"); return OK; } /**************************************************************************** * Name: lsm6dsl_close * * Description: * This method is called when the device is closed. * ****************************************************************************/ static int lsm6dsl_close(FAR struct file *filep) { return OK; } /**************************************************************************** * Name: lsm6dsl_read * * Description: * The standard read method. * ****************************************************************************/ static ssize_t lsm6dsl_read(FAR struct file *filep, FAR char *buffer, size_t buflen) { FAR struct inode *inode; FAR struct lsm6dsl_dev_s *priv; int ret; size_t i; size_t j; size_t samplesize; size_t nsamples; uint16_t data; FAR int16_t *ptr; uint8_t regaddr; uint8_t lo; uint8_t hi; uint32_t merge = 0; /* Sanity check */ DEBUGASSERT(filep != NULL); inode = filep->f_inode; DEBUGASSERT(inode != NULL); priv = (FAR struct lsm6dsl_dev_s *)inode->i_private; DEBUGASSERT(priv != NULL); DEBUGASSERT(priv->datareg == LSM6DSL_OUTX_L_G_SHIFT || priv->datareg == LSM6DSL_OUTX_L_XL_SHIFT); DEBUGASSERT(buffer != NULL); samplesize = 3 * sizeof(*ptr); nsamples = buflen / samplesize; ptr = (FAR int16_t *) buffer; /* Get the requested number of samples */ for (i = 0; i < nsamples; i++) { /* Reset the register address to the X low byte register */ regaddr = priv->datareg; /* Read the X, Y and Z data */ for (j = 0; j < 3; j++) { /* Read the low byte */ ret = lsm6dsl_readreg8(priv, regaddr, &lo); if (ret < 0) { snerr("ERROR: lsm6dsl_readreg8 failed: %d\n", ret); return (ssize_t) ret; } regaddr++; /* Read the high byte */ ret = lsm6dsl_readreg8(priv, regaddr, &hi); if (ret < 0) { snerr("ERROR: lsm6dsl_readreg8 failed: %d\n", ret); return (ssize_t) ret; } regaddr++; /* The data is 16 bits in two's complement representation */ data = ((uint16_t) hi << 8) | (uint16_t) lo; /* Collect entropy */ merge += data ^ (merge >> 16); /* The value is positive */ if (data < 0x8000) { ptr[j] = (int16_t) data; } /* The value is negative, so find its absolute value by taking the * two's complement */ else if (data > 0x8000) { data = ~data + 1; ptr[j] = -(int16_t) data; } /* The value is negative and can't be represented as a positive * int16_t value */ else { ptr[j] = (int16_t) (-32768); } } } /* Feed sensor data to entropy pool */ add_sensor_randomness(merge); return nsamples * samplesize; } /**************************************************************************** * Name: lsm6dsl_write * * Description: * A dummy write method. * ****************************************************************************/ static ssize_t lsm6dsl_write(FAR struct file *filep, FAR const char *buffer, size_t buflen) { return -ENOSYS; } /**************************************************************************** * Name: lsm6dsl_ioctl * * Description: * The standard ioctl method. * ****************************************************************************/ static int lsm6dsl_ioctl(FAR struct file *filep, int cmd, unsigned long arg) { FAR struct inode *inode; FAR struct lsm6dsl_dev_s *priv; int ret; /* Sanity check */ DEBUGASSERT(filep != NULL); inode = filep->f_inode; DEBUGASSERT(inode != NULL); priv = (FAR struct lsm6dsl_dev_s *)inode->i_private; DEBUGASSERT(priv != NULL); /* Handle ioctl commands */ switch (cmd) { /* Start converting. Arg: None. */ case SNIOC_START: ret = priv->ops->start(priv); break; /* Stop converting. Arg: None. */ case SNIOC_STOP: ret = priv->ops->stop(priv); break; case SNIOC_LSM6DSLSENSORREAD: ret = priv->ops->sensor_read(priv, (FAR struct lsm6dsl_sensor_data_s *) arg); break; case SNIOC_START_SELFTEST: ret = priv->ops->selftest(priv, (uint32_t) arg); break; /* Unrecognized commands */ default: { snerr("ERROR: Unrecognized cmd: %d arg: %lu\n", cmd, arg); ret = -ENOTTY; } break; } return ret; } /**************************************************************************** * Name: lsm6dsl_register * * Description: * Register the LSM6DSL accelerometer, gyroscope device as 'devpath'. * * Input Parameters: * devpath - The full path to the driver to register, e.g. * "/dev/lsm6dsl0", "/dev/gyro0" or "/dev/mag0". * i2c - An I2C driver instance. * addr - The I2C address of the LSM6DSL accelerometer, gyroscope or * magnetometer. * ops - The device operations structure. * datareg - The register address of the low byte of the X-coordinate data. * * Returned Value: * Zero (OK) on success; a negated errno value on failure. * ****************************************************************************/ static int lsm6dsl_register(FAR const char *devpath, FAR struct i2c_master_s *i2c, uint8_t addr, FAR const struct lsm6dsl_ops_s *ops, uint8_t datareg, struct lsm6dsl_sensor_data_s sensor_data) { FAR struct lsm6dsl_dev_s *priv; int ret; /* Sanity check */ DEBUGASSERT(devpath != NULL); DEBUGASSERT(i2c != NULL); DEBUGASSERT(datareg == LSM6DSL_OUTX_L_XL_SHIFT || datareg == LSM6DSL_OUTX_L_G_SHIFT); /* Initialize the device's structure */ priv = (FAR struct lsm6dsl_dev_s *)kmm_malloc(sizeof(*priv)); if (priv == NULL) { snerr("ERROR: Failed to allocate instance\n"); return -ENOMEM; } priv->i2c = i2c; priv->addr = addr; priv->ops = ops; priv->datareg = datareg; priv->sensor_data = sensor_data; /* Configure the device */ ret = priv->ops->config(priv); if (ret < 0) { snerr("ERROR: Failed to configure device: %d\n", ret); kmm_free(priv); return ret; } /* Register the character driver */ ret = register_driver(devpath, &g_fops, 0666, priv); if (ret < 0) { snerr("ERROR: Failed to register driver: %d\n", ret); kmm_free(priv); return ret; } return OK; } /**************************************************************************** * Public Functions ****************************************************************************/ /**************************************************************************** * Name: lsm6dsl_sensor_register * * Description: * Register the LSM6DSL accelerometer character device as 'devpath'. * * Input Parameters: * devpath - The full path to the driver to register, * e.g. "/dev/lsm6dsl0". * i2c - An I2C driver instance. * addr - The I2C address of the LSM6DSL accelerometer. * * Returned Value: * Zero (OK) on success; a negated errno value on failure. * ****************************************************************************/ int lsm6dsl_sensor_register(FAR const char *devpath, FAR struct i2c_master_s *i2c, uint8_t addr) { struct lsm6dsl_sensor_data_s sensor_data; DEBUGASSERT(addr == LSM6DSLACCEL_ADDR0 || addr == LSM6DSLACCEL_ADDR1); sninfo("Trying to register accel\n"); return lsm6dsl_register(devpath, i2c, addr, &g_lsm6dsl_sensor_ops, LSM6DSL_OUTX_L_XL_SHIFT, sensor_data); } #endif /* CONFIG_I2C && CONFIG_SENSORS_LSM6DSL */