You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2020/11/20 02:13:24 UTC
[incubator-nuttx] 01/02: drivers: l3gd20 supports the sensor driver
model
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git
commit 60d1755e0431911dcd27557896cfaff9e8177254
Author: dongjiuzhu <do...@xiaomi.com>
AuthorDate: Tue Nov 17 14:02:06 2020 +0800
drivers: l3gd20 supports the sensor driver model
Signed-off-by: dongjiuzhu <do...@xiaomi.com>
---
drivers/sensors/Kconfig | 10 ++
drivers/sensors/l3gd20.c | 336 +++++++++++++++++------------------------
include/nuttx/sensors/l3gd20.h | 17 +--
3 files changed, 156 insertions(+), 207 deletions(-)
diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig
index 560cd67..f800092 100644
--- a/drivers/sensors/Kconfig
+++ b/drivers/sensors/Kconfig
@@ -222,9 +222,19 @@ config SENSORS_L3GD20
bool "STMicro L3GD20 Gyroscope Sensor support"
default n
select SPI
+ select SCHED_HPWORK if SENSORS_L3GD20_BUFFER_SIZE > 0
---help---
Enable driver support for the STMicro L3GD20 gyroscope sensor.
+config SENSORS_L3GD20_BUFFER_SIZE
+ int "size of buffer"
+ default 1
+ depends on SENSORS_L3GD20
+ ---help---
+ The size of the circular buffer used. If the value equal to zero,
+ indicates that the circular buffer is disabled.
+
+
config SENSOR_KXTJ9
bool "Kionix KXTJ9 Accelerometer support"
default n
diff --git a/drivers/sensors/l3gd20.c b/drivers/sensors/l3gd20.c
index 89a765b..716d044 100644
--- a/drivers/sensors/l3gd20.c
+++ b/drivers/sensors/l3gd20.c
@@ -29,13 +29,16 @@
#include <debug.h>
#include <stdlib.h>
#include <string.h>
+#include <math.h>
+#include <nuttx/nuttx.h>
#include <nuttx/kmalloc.h>
#include <nuttx/wqueue.h>
#include <nuttx/random.h>
#include <nuttx/fs/fs.h>
+#include <nuttx/sensors/sensor.h>
#include <nuttx/sensors/l3gd20.h>
#if defined(CONFIG_SPI) && defined(CONFIG_SENSORS_L3GD20)
@@ -44,10 +47,6 @@
* Pre-processor Definitions
****************************************************************************/
-#if !defined(CONFIG_SCHED_HPWORK)
-# error Hi-priority work queue support is required (CONFIG_SCHED_HPWORK)
-#endif
-
/****************************************************************************
* Private Types
****************************************************************************/
@@ -59,13 +58,14 @@ struct l3gd20_dev_s
FAR struct spi_dev_s *spi; /* Pointer to the SPI instance */
FAR struct l3gd20_config_s *config; /* Pointer to the configuration of the
* L3GD20 sensor */
- sem_t datasem; /* Manages exclusive access to this
- * structure */
- struct l3gd20_sensor_data_s data; /* The data as measured by the sensor */
+ uint64_t timestamp; /* Units is microseconds */
+ struct sensor_lowerhalf_s lower; /* The struct of lower half driver */
+#if CONFIG_SENSORS_L3GD20_BUFFER_SIZE > 0
struct work_s work; /* The work queue is responsible for
* retrieving the data from the sensor
* after the arrival of new data was
* signalled in an interrupt */
+#endif
};
/****************************************************************************
@@ -80,7 +80,8 @@ static void l3gd20_write_register(FAR struct l3gd20_dev_s *dev,
uint8_t const reg_addr,
uint8_t const reg_data);
static void l3gd20_reset(FAR struct l3gd20_dev_s *dev);
-static void l3gd20_read_measurement_data(FAR struct l3gd20_dev_s *dev);
+static void l3gd20_read_measurement_data(FAR struct l3gd20_dev_s *dev,
+ FAR struct sensor_event_gyro *data);
static void l3gd20_read_gyroscope_data(FAR struct l3gd20_dev_s *dev,
uint16_t *x_gyr, uint16_t *y_gyr,
uint16_t *z_gyr);
@@ -88,32 +89,32 @@ static void l3gd20_read_temperature(FAR struct l3gd20_dev_s *dev,
uint8_t * temperature);
static int l3gd20_interrupt_handler(int irq, FAR void *context,
FAR void *arg);
+static int l3gd20_activate(FAR struct sensor_lowerhalf_s *lower,
+ bool enable);
+#if CONFIG_SENSORS_L3GD20_BUFFER_SIZE > 0
static void l3gd20_worker(FAR void *arg);
-
-static int l3gd20_open(FAR struct file *filep);
-static int l3gd20_close(FAR struct file *filep);
-static ssize_t l3gd20_read(FAR struct file *filep, FAR char *buffer,
- size_t buflen);
-static ssize_t l3gd20_write(FAR struct file *filep, FAR const char *buffer,
- size_t buflen);
-static int l3gd20_ioctl(FAR struct file *filep, int cmd, unsigned long arg);
+#else
+static int l3gd20_fetch(FAR struct sensor_lowerhalf_s *lower,
+ FAR char *buffer, size_t buflen);
+#endif
/****************************************************************************
* Private Data
****************************************************************************/
-static const struct file_operations g_l3gd20_fops =
+/* The lower half sensor driver operations for sensor register */
+
+static const struct sensor_ops_s g_l2gd20_ops =
{
- l3gd20_open,
- l3gd20_close,
- l3gd20_read,
- l3gd20_write,
- NULL,
- l3gd20_ioctl,
- NULL
-#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS
- , NULL
+ .activate = l3gd20_activate,
+ .set_interval = NULL,
+ .batch = NULL,
+#if CONFIG_SENSORS_L3GD20_BUFFER_SIZE > 0
+ .fetch = NULL,
+#else
+ .fetch = l3gd20_fetch,
#endif
+ .control = NULL
};
/* Single linked list to store instances of drivers */
@@ -214,13 +215,13 @@ static void l3gd20_reset(FAR struct l3gd20_dev_s *dev)
* Name: l3gd20_read_measurement_data
****************************************************************************/
-static void l3gd20_read_measurement_data(FAR struct l3gd20_dev_s *dev)
+static void l3gd20_read_measurement_data(FAR struct l3gd20_dev_s *dev,
+ FAR struct sensor_event_gyro *data)
{
uint16_t x_gyr = 0;
uint16_t y_gyr = 0;
uint16_t z_gyr = 0;
uint8_t temperature = 0;
- int ret;
/* Read Gyroscope */
@@ -230,24 +231,11 @@ static void l3gd20_read_measurement_data(FAR struct l3gd20_dev_s *dev)
l3gd20_read_temperature(dev, &temperature);
- /* Acquire the semaphore before the data is copied */
-
- ret = nxsem_wait(&dev->datasem);
- if (ret < 0)
- {
- snerr("ERROR: Could not acquire dev->datasem: %d\n", ret);
- return;
- }
-
- /* Copy retrieve data to internal data structure */
-
- dev->data.x_gyr = (int16_t) (x_gyr);
- dev->data.y_gyr = (int16_t) (y_gyr);
- dev->data.z_gyr = (int16_t) (z_gyr);
-
- /* Give back the semaphore */
-
- nxsem_post(&dev->datasem);
+ data->x = ((int16_t)x_gyr / 180.0f) * (float)M_PI;
+ data->y = ((int16_t)y_gyr / 180.0f) * (float)M_PI;
+ data->z = ((int16_t)z_gyr / 180.0f) * (float)M_PI;
+ data->temperature = temperature;
+ data->timestamp = dev->timestamp;
/* Feed sensor data to entropy pool */
@@ -357,6 +345,11 @@ static int l3gd20_interrupt_handler(int irq, FAR void *context,
DEBUGASSERT(priv != NULL);
}
+ /* Get the timestamp */
+
+ priv->timestamp = sensor_get_timestamp();
+
+#if CONFIG_SENSORS_L3GD20_BUFFER_SIZE > 0
/* Task the worker with retrieving the latest sensor data. We should not do
* this in a interrupt since it might take too long. Also we cannot lock
* the SPI bus from within an interrupt.
@@ -364,200 +357,155 @@ static int l3gd20_interrupt_handler(int irq, FAR void *context,
DEBUGASSERT(priv->work.worker == NULL);
ret = work_queue(HPWORK, &priv->work, l3gd20_worker, priv, 0);
+
if (ret < 0)
{
snerr("ERROR: Failed to queue work: %d\n", ret);
return ret;
}
+#else
+ /* notify event to upper half driver */
+
+ priv->lower.notify_event(priv->lower.priv);
+
+#endif
return OK;
}
+#if CONFIG_SENSORS_L3GD20_BUFFER_SIZE > 0
/****************************************************************************
* Name: l3gd20_worker
****************************************************************************/
static void l3gd20_worker(FAR void *arg)
{
+ struct sensor_event_gyro temp;
+
FAR struct l3gd20_dev_s *priv = (FAR struct l3gd20_dev_s *)(arg);
DEBUGASSERT(priv != NULL);
/* Read out the latest sensor data */
- l3gd20_read_measurement_data(priv);
-}
-
-/****************************************************************************
- * Name: l3gd20_open
- ****************************************************************************/
-
-static int l3gd20_open(FAR struct file *filep)
-{
- FAR struct inode *inode = filep->f_inode;
- FAR struct l3gd20_dev_s *priv = inode->i_private;
-#ifdef CONFIG_DEBUG_SENSORS_INFO
- uint8_t reg_content;
- uint8_t reg_addr;
-#endif
-
- DEBUGASSERT(priv != NULL);
-
- /* Perform a reset */
-
- l3gd20_reset(priv);
-
- /* Enable DRDY signal on INT 2 */
-
- l3gd20_write_register(priv,
- L3GD20_CTRL_REG_3,
- L3GD20_CTRL_REG_3_I2_DRDY_BM);
-
- /* Enable the maximum full scale mode.
- * Enable block data update for gyro sensor data.
- * This should prevent race conditions when reading sensor data.
- */
-
- l3gd20_write_register(priv,
- L3GD20_CTRL_REG_4,
- L3GD20_CTRL_REG_4_BDU_BM |
- L3GD20_CTRL_REG_4_FS_1_BM |
- L3GD20_CTRL_REG_4_FS_0_BM);
-
- /* Enable X,Y,Z axis
- * DR=00 -> Output data rate = 95 Hz, Cut-off = 12.5
- */
-
- l3gd20_write_register(priv,
- L3GD20_CTRL_REG_1,
- L3GD20_CTRL_REG_1_POWERDOWN_BM |
- L3GD20_CTRL_REG_1_X_EN_BM |
- L3GD20_CTRL_REG_1_Y_EN_BM |
- L3GD20_CTRL_REG_1_Z_EN_BM);
-
- /* Read measurement data to ensure DRDY is low */
-
- l3gd20_read_measurement_data(priv);
+ l3gd20_read_measurement_data(priv, &temp);
- /* Read back the content of all control registers for debug purposes */
+ /* push data to upper half driver */
-#ifdef CONFIG_DEBUG_SENSORS_INFO
- reg_content = 0;
-
- l3gd20_read_register(priv, L3GD20_WHO_AM_I, ®_content);
- sninfo("WHO_AM_I_REG = %04x\n", reg_content);
-
- for (reg_addr = L3GD20_CTRL_REG_1;
- reg_addr <= L3GD20_CTRL_REG_5;
- reg_addr++)
- {
- l3gd20_read_register(priv, reg_addr, ®_content);
- sninfo("R#%04x = %04x\n", reg_addr, reg_content);
- }
-
- l3gd20_read_register(priv, L3GD20_STATUS_REG, ®_content);
- sninfo("STATUS_REG = %04x\n", reg_content);
-#endif
-
- return OK;
+ priv->lower.push_event(priv->lower.priv, &temp,
+ sizeof(struct sensor_event_gyro));
}
+#else
+
/****************************************************************************
- * Name: l3gd20_close
+ * Name: l3gd20_fetch
****************************************************************************/
-static int l3gd20_close(FAR struct file *filep)
+static int l3gd20_fetch(FAR struct sensor_lowerhalf_s *lower,
+ FAR char *buffer, size_t buflen)
{
- FAR struct inode *inode = filep->f_inode;
- FAR struct l3gd20_dev_s *priv = inode->i_private;
+ FAR struct l3gd20_dev_s *priv = container_of(lower,
+ FAR struct l3gd20_dev_s,
+ lower);
+
+ if (buflen != sizeof(struct sensor_event_gyro))
+ return 0;
DEBUGASSERT(priv != NULL);
- /* Perform a reset */
+ /* Read out the latest sensor data */
- l3gd20_reset(priv);
- return OK;
+ l3gd20_read_measurement_data(priv, (FAR struct sensor_event_gyro *)buffer);
+
+ return sizeof(struct sensor_event_gyro);
}
+#endif
/****************************************************************************
- * Name: l3gd20_read
+ * Name: l3gd20_activate
****************************************************************************/
-static ssize_t l3gd20_read(FAR struct file *filep, FAR char *buffer,
- size_t buflen)
+static int l3gd20_activate(FAR struct sensor_lowerhalf_s *lower,
+ bool enable)
{
- FAR struct inode *inode = filep->f_inode;
- FAR struct l3gd20_dev_s *priv = inode->i_private;
- FAR struct l3gd20_sensor_data_s *data;
- int ret;
+ FAR struct l3gd20_dev_s *priv = container_of(lower,
+ FAR struct l3gd20_dev_s,
+ lower);
+ struct sensor_event_gyro temp;
- DEBUGASSERT(priv != NULL);
+#ifdef CONFIG_DEBUG_SENSORS_INFO
+ uint8_t reg_content;
+ uint8_t reg_addr;
+#endif
- /* Check if enough memory was provided for the read call */
+ DEBUGASSERT(priv != NULL);
- if (buflen < sizeof(FAR struct l3gd20_sensor_data_s))
+ if (enable == true)
{
- snerr("ERROR: Not enough memory for reading out a sensor data"
- " sample\n");
- return -ENOSYS;
- }
+ /* Perform a reset */
- /* Acquire the semaphore before the data is copied */
+ l3gd20_reset(priv);
- ret = nxsem_wait(&priv->datasem);
- if (ret < 0)
- {
- snerr("ERROR: Could not acquire priv->datasem: %d\n", ret);
- return ret;
- }
+ /* Enable DRDY signal on INT 2 */
- /* Copy the sensor data into the buffer */
+ l3gd20_write_register(priv,
+ L3GD20_CTRL_REG_3,
+ L3GD20_CTRL_REG_3_I2_DRDY_BM);
- data = (FAR struct l3gd20_sensor_data_s *)buffer;
- memset(data, 0, sizeof(FAR struct l3gd20_sensor_data_s));
+ /* Enable the maximum full scale mode.
+ * Enable block data update for gyro sensor data.
+ * This should prevent race conditions when reading sensor data.
+ */
- data->x_gyr = priv->data.x_gyr;
- data->y_gyr = priv->data.y_gyr;
- data->z_gyr = priv->data.z_gyr;
- data->temperature = priv->data.temperature;
+ l3gd20_write_register(priv,
+ L3GD20_CTRL_REG_4,
+ L3GD20_CTRL_REG_4_BDU_BM |
+ L3GD20_CTRL_REG_4_FS_1_BM |
+ L3GD20_CTRL_REG_4_FS_0_BM);
- /* Give back the semaphore */
+ /* Enable X,Y,Z axis
+ * DR=00 -> Output data rate = 95 Hz, Cut-off = 12.5
+ */
- nxsem_post(&priv->datasem);
+ l3gd20_write_register(priv,
+ L3GD20_CTRL_REG_1,
+ L3GD20_CTRL_REG_1_POWERDOWN_BM |
+ L3GD20_CTRL_REG_1_X_EN_BM |
+ L3GD20_CTRL_REG_1_Y_EN_BM |
+ L3GD20_CTRL_REG_1_Z_EN_BM);
- return sizeof(FAR struct l3gd20_sensor_data_s);
-}
+ /* Read measurement data to ensure DRDY is low */
-/****************************************************************************
- * Name: l3gd20_write
- ****************************************************************************/
+ l3gd20_read_measurement_data(priv, &temp);
-static ssize_t l3gd20_write(FAR struct file *filep, FAR const char *buffer,
- size_t buflen)
-{
- return -ENOSYS;
-}
+ /* Read back the content of all control registers for debug purposes */
-/****************************************************************************
- * Name: l3gd20_ioctl
- ****************************************************************************/
+#ifdef CONFIG_DEBUG_SENSORS_INFO
+ reg_content = 0;
-static int l3gd20_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
-{
- int ret = OK;
+ l3gd20_read_register(priv, L3GD20_WHO_AM_I, ®_content);
+ sninfo("WHO_AM_I_REG = %04x\n", reg_content);
- switch (cmd)
- {
- /* @TODO */
+ for (reg_addr = L3GD20_CTRL_REG_1;
+ reg_addr <= L3GD20_CTRL_REG_5;
+ reg_addr++)
+ {
+ l3gd20_read_register(priv, reg_addr, ®_content);
+ sninfo("R#%04x = %04x\n", reg_addr, reg_content);
+ }
- /* Command was not recognized */
+ l3gd20_read_register(priv, L3GD20_STATUS_REG, ®_content);
+ sninfo("STATUS_REG = %04x\n", reg_content);
+#endif
+ }
+ else
+ {
+ /* Perform a reset */
- default:
- snerr("ERROR: Unrecognized cmd: %d\n", cmd);
- ret = -ENOTTY;
- break;
+ l3gd20_reset(priv);
}
- return ret;
+ return 0;
}
/****************************************************************************
@@ -571,7 +519,8 @@ static int l3gd20_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
* Register the L3DF20 character device as 'devpath'.
*
* Input Parameters:
- * devpath - The full path to the driver to register, e.g., "/dev/gyr0".
+ * devno - The device number, used to build the device path
+ * as /dev/sensor/gyro_uncalN
* spi - An SPI driver instance.
* config - configuration for the L3GD20 driver.
*
@@ -580,7 +529,7 @@ static int l3gd20_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
*
****************************************************************************/
-int l3gd20_register(FAR const char *devpath, FAR struct spi_dev_s *spi,
+int l3gd20_register(int devno, FAR struct spi_dev_s *spi,
FAR struct l3gd20_config_s *config)
{
FAR struct l3gd20_dev_s *priv;
@@ -603,16 +552,16 @@ int l3gd20_register(FAR const char *devpath, FAR struct spi_dev_s *spi,
priv->spi = spi;
priv->config = config;
+#if CONFIG_SENSORS_L3GD20_BUFFER_SIZE > 0
priv->work.worker = NULL;
+#endif
+ priv->timestamp = 0;
- priv->data.x_gyr = 0;
- priv->data.y_gyr = 0;
- priv->data.z_gyr = 0;
- priv->data.temperature = 0;
-
- /* Initialize sensor data access semaphore */
-
- nxsem_init(&priv->datasem, 0, 1);
+ priv->lower.type = SENSOR_TYPE_GYROSCOPE;
+ priv->lower.buffer_size = sizeof(struct sensor_event_gyro) *
+ CONFIG_SENSORS_L3GD20_BUFFER_SIZE;
+ priv->lower.ops = &g_l2gd20_ops;
+ priv->lower.uncalibrated = true;
/* Setup SPI frequency and mode */
@@ -628,14 +577,13 @@ int l3gd20_register(FAR const char *devpath, FAR struct spi_dev_s *spi,
goto errout;
}
- /* Register the character driver */
+ /* Register the sensor driver */
- ret = register_driver(devpath, &g_l3gd20_fops, 0666, priv);
+ ret = sensor_register(&priv->lower, devno);
if (ret < 0)
{
snerr("ERROR: Failed to register driver: %d\n", ret);
kmm_free(priv);
- nxsem_destroy(&priv->datasem);
goto errout;
}
diff --git a/include/nuttx/sensors/l3gd20.h b/include/nuttx/sensors/l3gd20.h
index 26faae5..cb0aad6 100644
--- a/include/nuttx/sensors/l3gd20.h
+++ b/include/nuttx/sensors/l3gd20.h
@@ -229,16 +229,6 @@ struct l3gd20_config_s
int (*attach)(FAR struct l3gd20_config_s *, xcpt_t);
};
-/* Data returned by reading from the L3GD20 is returned in this format. */
-
-struct l3gd20_sensor_data_s
-{
- int16_t x_gyr; /* Measurement result for x axis */
- int16_t y_gyr; /* Measurement result for y axis */
- int16_t z_gyr; /* Measurement result for z axis */
- int8_t temperature; /* Measurement result for temperature sensor */
-};
-
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
@@ -258,8 +248,9 @@ extern "C"
* Register the L3DF20 character device as 'devpath'.
*
* Input Parameters:
- * devpath - The full path to the driver to register, e.g., "/dev/gyr0".
- * i2c - An SPI driver instance.
+ * devno - The device number, used to build the device path
+ * as /dev/sensor/gyro_uncalN
+ * spi - An SPI driver instance.
* config - configuration for the L3GD20 driver. For details see
* description above.
*
@@ -268,7 +259,7 @@ extern "C"
*
****************************************************************************/
-int l3gd20_register(FAR const char *devpath, FAR struct spi_dev_s *spi,
+int l3gd20_register(int devno, FAR struct spi_dev_s *spi,
FAR struct l3gd20_config_s *config);
#undef EXTERN