/**************************************************************************** * drivers/sensors/lsm303agr.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 #include #if defined(CONFIG_I2C) && defined(CONFIG_SENSORS_LSM303AGR) /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ #ifndef CONFIG_LSM303AGR_I2C_FREQUENCY # define CONFIG_LSM303AGR_I2C_FREQUENCY 400000 #endif /**************************************************************************** * Private Function Prototypes ****************************************************************************/ /* I2C Helpers */ static int lsm303agr_readreg8(FAR struct lsm303agr_dev_s *priv, uint8_t regaddr, FAR uint8_t * regval); static int lsm303agr_writereg8(FAR struct lsm303agr_dev_s *priv, uint8_t regaddr, uint8_t regval); /* Other Helpers */ static int lsm303agr_find_minimum(int16_t a[], int n); static int lsm303agr_find_maximum(int16_t a[], int n); static bool lsm303agr_isbitset(int8_t b, int8_t n); /* Accelerometer Operations */ static int lsm303agr_sensor_config(FAR struct lsm303agr_dev_s *priv); static int lsm303agr_sensor_start(FAR struct lsm303agr_dev_s *priv); static int lsm303agr_sensor_stop(FAR struct lsm303agr_dev_s *priv); static int lsm303agr_sensor_read(FAR struct lsm303agr_dev_s *priv, FAR struct lsm303agr_sensor_data_s *sdata); static int lsm303agr_selftest(FAR struct lsm303agr_dev_s *priv, uint32_t mode); /* Character Driver Methods */ static ssize_t lsm303agr_read(FAR struct file *filep, FAR char *buffer, size_t buflen); static ssize_t lsm303agr_write(FAR struct file *filep, FAR const char *buffer, size_t buflen); static int lsm303agr_ioctl(FAR struct file *filep, int cmd, unsigned long arg); /* Common Register Function */ static int lsm303agr_register(FAR const char *devpath, FAR struct i2c_master_s *i2c, uint8_t addr, FAR const struct lsm303agr_ops_s *ops, uint8_t datareg, struct lsm303agr_sensor_data_s sensor_data); /**************************************************************************** * Private Data ****************************************************************************/ static double g_accelerofactor = 0; static double g_magnetofactor = 0; static const struct file_operations g_fops = { NULL, /* open */ NULL, /* close */ lsm303agr_read, /* read */ lsm303agr_write, /* write */ NULL, /* seek */ lsm303agr_ioctl, /* ioctl */ }; static const struct lsm303agr_ops_s g_lsm303agrsensor_ops = { lsm303agr_sensor_config, lsm303agr_sensor_start, lsm303agr_sensor_stop, lsm303agr_sensor_read, lsm303agr_selftest }; /**************************************************************************** * Private Functions ****************************************************************************/ /**************************************************************************** * Name: lsm303agr_resetsensor * * Description: * Reset sensor values * ****************************************************************************/ static void lsm303agr_resetsensor(FAR struct lsm303agr_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.m_x_data = 0; priv->sensor_data.m_y_data = 0; priv->sensor_data.m_z_data = 0; priv->sensor_data.timestamp = 0; } /**************************************************************************** * Name: lsm303agr_readreg8 * * Description: * Read from an 8-bit register. * ****************************************************************************/ static int lsm303agr_readreg8(FAR struct lsm303agr_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_LSM303AGR_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: lsm303agr_writereg8 * * Description: * Write to an 8-bit register. * ****************************************************************************/ static int lsm303agr_writereg8(FAR struct lsm303agr_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_LSM303AGR_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: lsm303agr_find_minimum * * Description: * Find the minimum value in an array of numbers. * ****************************************************************************/ static int lsm303agr_find_minimum(int16_t a[], int n) { int c; int min; int index; min = a[0]; index = 0; for (c = 1; c < n; c++) { if (a[c] < min) { index = c; min = a[c]; } } return index; } /**************************************************************************** * Name: lsm303agr_find_maximum * * Description: * Find the maximum value in an array of numbers. * ****************************************************************************/ static int lsm303agr_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: lsm303agr_sensor_config * * Description: * Configure the accelerometer and gyroscope. * ****************************************************************************/ static int lsm303agr_sensor_config(FAR struct lsm303agr_dev_s *priv) { int ret; uint8_t regval; /* Sanity check */ DEBUGASSERT(priv != NULL); /* Get the device identification */ ret = lsm303agr_readreg8(priv, LSM303AGR_WHO_AM_I, ®val); if (ret < 0) { snerr("ERROR: lsm303agr_readreg8 failed: %d\n", ret); return ret; } sninfo("WHO AM I check address regval: %02x reg: %d....\n", regval, ret); if (regval != LSM303AGR_WHO_AM_I_VALUE) { snerr("ERROR: Invalid device identification %02x\n", regval); return -ENODEV; } return OK; } /**************************************************************************** * Name: lsm303agr_isbitset * * Description: * Check if bit is set from mask, not bit number. * ****************************************************************************/ static bool lsm303agr_isbitset(int8_t b, int8_t m) { if ((b & m) != 0) { return true; } return false; } /**************************************************************************** * Name: lsm303agr_sensor_start * * Description: * Start the accelerometer. * ****************************************************************************/ static int lsm303agr_sensor_start(FAR struct lsm303agr_dev_s *priv) { /* readreg8 is not necessary to modify. Clearbits can be 0x00 or 0xff */ /* Enable the accelerometer */ /* Reset values */ lsm303agr_resetsensor(priv); /* Sanity check */ DEBUGASSERT(priv != NULL); sninfo("Starting...."); /* Accelerometer config registers: * Turn on the accelerometer: 833Hz, +- 16g */ lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG1_A, 0x77); lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG4_A, 0xb0); g_accelerofactor = 11.72; /* Gyro config registers Turn on the gyro: FS=2000dps, ODR=833Hz Not using * modifyreg with empty value!!!! Then read value first!!! * lsm303agr_modifyreg8(priv, lsm303agr_CTRL2_G, value, 0x7c); */ lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_A_M, 0x8c); g_magnetofactor = 1.5; return OK; } /**************************************************************************** * Name: lsm303agr_sensor_stop * * Description: * Stop the sensor. * ****************************************************************************/ static int lsm303agr_sensor_stop(FAR struct lsm303agr_dev_s *priv) { /* Sanity check */ DEBUGASSERT(priv != NULL); /* Stop accelero: not implemented [datasheet] */ /* Stop magneto: not implemented [datasheet] */ return OK; } /**************************************************************************** * Name: lsm303agr_selftest * * Description: * Selftesting the sensor. * Mode 0 = selftest accelerometer and mode 1 = selftest magneto * ****************************************************************************/ static int lsm303agr_selftest(FAR struct lsm303agr_dev_s *priv, uint32_t mode) { int samples = 5; int i; int i2; int i3; uint8_t value; 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_xst = 0; int16_t raw_yst = 0; int16_t raw_zst = 0; int16_t raw_x = 0; int16_t raw_y = 0; int16_t raw_z = 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 = 0x00; lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG2_A, 0x00); lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG3_A, 0x00); lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG4_A, 0x81); lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG1_A, 0x57); g_accelerofactor = 1; } else { registershift = 0x40; lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_A_M, 0x8c); lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_B_M, 0x02); lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_C_M, 0x10); g_magnetofactor = 1; } 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) { if (mode == 0) { lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_A, &value); checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_A_ZYXDA); } else { lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_M, &value); checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_M_ZYXDA); } } nxsig_usleep(100000); /* 100ms */ /* Read OUT registers Gyro is starting at 22h and Accelero at 28h */ lsm303agr_readreg8(priv, LSM303AGR_OUT_X_L_A + registershift, (FAR uint8_t *)&lox); lsm303agr_readreg8(priv, LSM303AGR_OUT_X_H_A + registershift, (FAR uint8_t *)&hix); lsm303agr_readreg8(priv, LSM303AGR_OUT_Y_L_A + registershift, (FAR uint8_t *)&loy); lsm303agr_readreg8(priv, LSM303AGR_OUT_Y_H_A + registershift, (FAR uint8_t *)&hiy); lsm303agr_readreg8(priv, LSM303AGR_OUT_Z_L_A + registershift, (FAR uint8_t *)&loz); lsm303agr_readreg8(priv, LSM303AGR_OUT_Z_H_A + registershift, (FAR uint8_t *)&hiz); /* check XLDA 5 times */ for (i = 0; i < samples; i++) { if (mode == 0) { lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_A, &value); checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_A_ZYXDA); } else { lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_M, &value); checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_M_ZYXDA); } /* Average the stored data on each axis * * http://ozzmaker.com/accelerometer-to-g/ */ lsm303agr_readreg8(priv, LSM303AGR_OUT_X_L_A + registershift, (FAR uint8_t *)&lox); lsm303agr_readreg8(priv, LSM303AGR_OUT_X_H_A + registershift, (FAR uint8_t *)&hix); raw_x = (int16_t) (((uint16_t) hix << 8U) | (uint16_t) lox); lsm303agr_readreg8(priv, LSM303AGR_OUT_Y_L_A + registershift, (FAR uint8_t *)&loy); lsm303agr_readreg8(priv, LSM303AGR_OUT_Y_H_A + registershift, (FAR uint8_t *)&hiy); raw_y = (int16_t) (((uint16_t) hiy << 8U) | (uint16_t) loy); lsm303agr_readreg8(priv, LSM303AGR_OUT_Z_L_A + registershift, (FAR uint8_t *)&loz); lsm303agr_readreg8(priv, LSM303AGR_OUT_Z_H_A + 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) { lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG4_A, 0x85); } else { lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_C_M, 0x12); } nxsig_usleep(100000); /* 100ms */ checkbit = false; while (checkbit) { if (mode == 0) { lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_A, &value); checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_A_ZYXDA); } else { lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_M, &value); checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_M_ZYXDA); } } nxsig_usleep(100000); /* 100ms */ /* Now do all the ST values */ lsm303agr_readreg8(priv, LSM303AGR_OUT_X_L_A + registershift, (FAR uint8_t *)&loxst); lsm303agr_readreg8(priv, LSM303AGR_OUT_X_H_A + registershift, (FAR uint8_t *)&hixst); lsm303agr_readreg8(priv, LSM303AGR_OUT_Y_L_A + registershift, (FAR uint8_t *)&loyst); lsm303agr_readreg8(priv, LSM303AGR_OUT_Y_H_A + registershift, (FAR uint8_t *)&hiyst); lsm303agr_readreg8(priv, LSM303AGR_OUT_Z_L_A + registershift, (FAR uint8_t *)&lozst); lsm303agr_readreg8(priv, LSM303AGR_OUT_Z_H_A + registershift, (FAR uint8_t *)&hizst); for (i2 = 0; i2 < samples; i2++) { if (mode == 0) { lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_A, &value); checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_A_ZYXDA); } else { lsm303agr_readreg8(priv, LSM303AGR_STATUS_REG_M, &value); checkbit = lsm303agr_isbitset(value, LSM303AGR_STATUS_REG_M_ZYXDA); } nxsig_usleep(100000); /* 100ms */ lsm303agr_readreg8(priv, LSM303AGR_OUT_X_L_A + registershift, (FAR uint8_t *)&loxst); lsm303agr_readreg8(priv, LSM303AGR_OUT_X_H_A + registershift, (FAR uint8_t *)&hixst); raw_xst = (int16_t) (((uint16_t) hixst << 8U) | (uint16_t) loxst); lsm303agr_readreg8(priv, LSM303AGR_OUT_Y_L_A + registershift, (FAR uint8_t *)&loyst); lsm303agr_readreg8(priv, LSM303AGR_OUT_Y_H_A + registershift, (FAR uint8_t *)&hiyst); raw_yst = (int16_t) (((uint16_t) hiyst << 8U) | (uint16_t) loyst); lsm303agr_readreg8(priv, LSM303AGR_OUT_Z_L_A + registershift, (FAR uint8_t *)&lozst); lsm303agr_readreg8(priv, LSM303AGR_OUT_Z_H_A + 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[lsm303agr_find_minimum(OUTX_NOST, samples)]; min_y = OUTY_NOST[lsm303agr_find_minimum(OUTY_NOST, samples)]; min_z = OUTZ_NOST[lsm303agr_find_minimum(OUTZ_NOST, samples)]; max_x = OUTX_NOST[lsm303agr_find_maximum(OUTX_NOST, samples)]; max_y = OUTY_NOST[lsm303agr_find_maximum(OUTY_NOST, samples)]; max_z = OUTZ_NOST[lsm303agr_find_maximum(OUTZ_NOST, samples)]; min_xst = OUTX_ST[lsm303agr_find_minimum(OUTX_ST, samples)]; min_yst = OUTY_ST[lsm303agr_find_minimum(OUTY_ST, samples)]; min_zst = OUTZ_ST[lsm303agr_find_minimum(OUTZ_ST, samples)]; max_xst = OUTX_ST[lsm303agr_find_maximum(OUTX_ST, samples)]; max_yst = OUTY_ST[lsm303agr_find_maximum(OUTY_ST, samples)]; max_zst = OUTZ_ST[lsm303agr_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 */ 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"); lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG1_A, 0x00); lsm303agr_writereg8(priv, LSM303AGR_CTRL_REG4_A, 0x01); } break; case 1: { sninfo("SELFTEST MAGNETO DISABLED\n"); lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_C_M, 0x10); lsm303agr_writereg8(priv, LSM303AGR_CFG_REG_A_M, 0x83); } break; default: break; } return OK; } /**************************************************************************** * Name: lsm303agr_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 lsm303agr_sensor_read(FAR struct lsm303agr_dev_s *priv, FAR struct lsm303agr_sensor_data_s *sdata) { int16_t loxg = 0; int16_t hixg = 0; int16_t loyg = 0; int16_t hiyg = 0; int16_t lozg = 0; int16_t hizg = 0; int16_t templ = 0; int16_t temph = 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; /* Magneto */ lsm303agr_readreg8(priv, LSM303AGR_OUTX_L_REG_M, (FAR uint8_t *)&loxg); lsm303agr_readreg8(priv, LSM303AGR_OUTX_H_REG_M, (FAR uint8_t *)&hixg); lsm303agr_readreg8(priv, LSM303AGR_OUTY_L_REG_M, (FAR uint8_t *)&loyg); lsm303agr_readreg8(priv, LSM303AGR_OUTY_H_REG_M, (FAR uint8_t *)&hiyg); lsm303agr_readreg8(priv, LSM303AGR_OUTZ_L_REG_M, (FAR uint8_t *)&lozg); lsm303agr_readreg8(priv, LSM303AGR_OUTZ_H_REG_M, (FAR uint8_t *)&hizg); /* Temperature */ lsm303agr_readreg8(priv, LSM303AGR_OUT_TEMP_L_A, (FAR uint8_t *)&templ); lsm303agr_readreg8(priv, LSM303AGR_OUT_TEMP_H_A, (FAR uint8_t *)&temph); tempi = (int16_t) ((((int16_t) temph << 8) | (int16_t) templ)); temp_val = (tempi / 256) + 25; /* TSen 256 LSB/°C */ sninfo("Data 16-bit TEMP--->: %d Celsius\n", temp_val); sdata->temperature = temp_val; 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 M_X--->: %d mguass\n", (short)(x_valg * g_magnetofactor)); sninfo("Data 16-bit M_Y--->: %d mguass\n", (short)(y_valg * g_magnetofactor)); sninfo("Data 16-bit M_Z--->: %d mguass\n", (short)(z_valg * g_magnetofactor)); sdata->m_x_data = x_valg; sdata->m_y_data = y_valg; sdata->m_z_data = z_valg; return OK; } /**************************************************************************** * Name: lsm303agr_read * * Description: * The standard read method. * ****************************************************************************/ static ssize_t lsm303agr_read(FAR struct file *filep, FAR char *buffer, size_t buflen) { FAR struct inode *inode; FAR struct lsm303agr_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 */ inode = filep->f_inode; priv = inode->i_private; DEBUGASSERT(priv != NULL); DEBUGASSERT(priv->datareg == LSM303AGR_OUTX_L_A_SHIFT || priv->datareg == LSM303AGR_OUTX_L_M_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 = lsm303agr_readreg8(priv, regaddr, &lo); if (ret < 0) { snerr("ERROR: lsm303agr_readreg8 failed: %d\n", ret); return (ssize_t) ret; } regaddr++; /* Read the high byte */ ret = lsm303agr_readreg8(priv, regaddr, &hi); if (ret < 0) { snerr("ERROR: lsm303agr_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: lsm303agr_write * * Description: * A dummy write method. * ****************************************************************************/ static ssize_t lsm303agr_write(FAR struct file *filep, FAR const char *buffer, size_t buflen) { return -ENOSYS; } /**************************************************************************** * Name: lsm303agr_ioctl * * Description: * The standard ioctl method. * ****************************************************************************/ static int lsm303agr_ioctl(FAR struct file *filep, int cmd, unsigned long arg) { FAR struct inode *inode; FAR struct lsm303agr_dev_s *priv; int ret; /* Sanity check */ inode = filep->f_inode; priv = 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_LSM303AGRSENSORREAD: { FAR struct lsm303agr_sensor_data_s *d = (FAR struct lsm303agr_sensor_data_s *)arg; ret = priv->ops->sensor_read(priv, d); 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: lsm303agr_register * * Description: * Register the LSM303AGR accelerometer, gyroscope device as 'devpath'. * * Input Parameters: * devpath - The full path to the driver to register, e.g. * "/dev/lsm303agr0", "/dev/gyro0" or "/dev/mag0". * i2c - An I2C driver instance. * addr - The I2C address of the LSM303AGR 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 lsm303agr_register(FAR const char *devpath, FAR struct i2c_master_s *i2c, uint8_t addr, FAR const struct lsm303agr_ops_s *ops, uint8_t datareg, struct lsm303agr_sensor_data_s sensor_data) { FAR struct lsm303agr_dev_s *priv; int ret; /* Sanity check */ DEBUGASSERT(devpath != NULL); DEBUGASSERT(i2c != NULL); DEBUGASSERT(datareg == LSM303AGR_OUTX_L_A_SHIFT || datareg == LSM303AGR_OUTX_L_M_SHIFT); /* Initialize the device's structure */ priv = 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: lsm303agr_sensor_register * * Description: * Register the LSM303AGR accelerometer character device as 'devpath'. * * Input Parameters: * devpath - The full path to the driver to register, * e.g. "/dev/lsm303agr0". * i2c - An I2C driver instance. * addr - The I2C address of the LSM303AGR accelerometer. * * Returned Value: * Zero (OK) on success; a negated errno value on failure. * ****************************************************************************/ int lsm303agr_sensor_register(FAR const char *devpath, FAR struct i2c_master_s *i2c, uint8_t addr) { struct lsm303agr_sensor_data_s sensor_data; DEBUGASSERT(addr == LSM303AGRACCELERO_ADDR || addr == LSM303AGRMAGNETO_ADDR); sninfo("Trying to register accel\n"); return lsm303agr_register(devpath, i2c, addr, &g_lsm303agrsensor_ops, LSM303AGR_OUTX_L_A_SHIFT, sensor_data); } #endif /* CONFIG_I2C && CONFIG_SENSORS_LSM303AGR */