From 60d1755e0431911dcd27557896cfaff9e8177254 Mon Sep 17 00:00:00 2001 From: dongjiuzhu Date: Tue, 17 Nov 2020 14:02:06 +0800 Subject: [PATCH] drivers: l3gd20 supports the sensor driver model Signed-off-by: dongjiuzhu --- drivers/sensors/Kconfig | 10 + drivers/sensors/l3gd20.c | 350 ++++++++++++++------------------- include/nuttx/sensors/l3gd20.h | 17 +- 3 files changed, 163 insertions(+), 214 deletions(-) diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig index 560cd6736c..f8000924a4 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 89a765b5b3..716d044861 100644 --- a/drivers/sensors/l3gd20.c +++ b/drivers/sensors/l3gd20.c @@ -29,13 +29,16 @@ #include #include #include +#include +#include #include #include #include #include +#include #include #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,37 +357,82 @@ 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); + l3gd20_read_measurement_data(priv, &temp); + + /* push data to upper half driver */ + + priv->lower.push_event(priv->lower.priv, &temp, + sizeof(struct sensor_event_gyro)); } +#else + /**************************************************************************** - * Name: l3gd20_open + * Name: l3gd20_fetch ****************************************************************************/ -static int l3gd20_open(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); + + /* Read out the latest sensor data */ + + l3gd20_read_measurement_data(priv, (FAR struct sensor_event_gyro *)buffer); + + return sizeof(struct sensor_event_gyro); +} +#endif + +/**************************************************************************** + * Name: l3gd20_activate + ****************************************************************************/ + +static int l3gd20_activate(FAR struct sensor_lowerhalf_s *lower, + bool enable) +{ + FAR struct l3gd20_dev_s *priv = container_of(lower, + FAR struct l3gd20_dev_s, + lower); + struct sensor_event_gyro temp; + #ifdef CONFIG_DEBUG_SENSORS_INFO uint8_t reg_content; uint8_t reg_addr; @@ -402,162 +440,72 @@ static int l3gd20_open(FAR struct file *filep) DEBUGASSERT(priv != NULL); - /* Perform a reset */ + if (enable == true) + { + /* Perform a reset */ - l3gd20_reset(priv); + l3gd20_reset(priv); - /* Enable DRDY signal on INT 2 */ + /* Enable DRDY signal on INT 2 */ - l3gd20_write_register(priv, - L3GD20_CTRL_REG_3, - L3GD20_CTRL_REG_3_I2_DRDY_BM); + 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. - */ + /* 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); + 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 - */ + /* 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); + 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 */ + /* 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 */ + /* Read back the content of all control registers for debug purposes */ #ifdef CONFIG_DEBUG_SENSORS_INFO - reg_content = 0; + reg_content = 0; - l3gd20_read_register(priv, L3GD20_WHO_AM_I, ®_content); - sninfo("WHO_AM_I_REG = %04x\n", reg_content); + 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); - } + 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); + l3gd20_read_register(priv, L3GD20_STATUS_REG, ®_content); + sninfo("STATUS_REG = %04x\n", reg_content); #endif - - return OK; -} - -/**************************************************************************** - * Name: l3gd20_close - ****************************************************************************/ - -static int l3gd20_close(FAR struct file *filep) -{ - FAR struct inode *inode = filep->f_inode; - FAR struct l3gd20_dev_s *priv = inode->i_private; - - DEBUGASSERT(priv != NULL); - - /* Perform a reset */ - - l3gd20_reset(priv); - return OK; -} - -/**************************************************************************** - * Name: l3gd20_read - ****************************************************************************/ - -static ssize_t l3gd20_read(FAR struct file *filep, 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_sensor_data_s *data; - int ret; - - DEBUGASSERT(priv != NULL); - - /* Check if enough memory was provided for the read call */ - - if (buflen < sizeof(FAR struct l3gd20_sensor_data_s)) + } + else { - snerr("ERROR: Not enough memory for reading out a sensor data" - " sample\n"); - return -ENOSYS; + /* Perform a reset */ + + l3gd20_reset(priv); } - /* Acquire the semaphore before the data is copied */ - - ret = nxsem_wait(&priv->datasem); - if (ret < 0) - { - snerr("ERROR: Could not acquire priv->datasem: %d\n", ret); - return ret; - } - - /* Copy the sensor data into the buffer */ - - data = (FAR struct l3gd20_sensor_data_s *)buffer; - memset(data, 0, sizeof(FAR struct l3gd20_sensor_data_s)); - - 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; - - /* Give back the semaphore */ - - nxsem_post(&priv->datasem); - - return sizeof(FAR struct l3gd20_sensor_data_s); -} - -/**************************************************************************** - * Name: l3gd20_write - ****************************************************************************/ - -static ssize_t l3gd20_write(FAR struct file *filep, FAR const char *buffer, - size_t buflen) -{ - return -ENOSYS; -} - -/**************************************************************************** - * Name: l3gd20_ioctl - ****************************************************************************/ - -static int l3gd20_ioctl(FAR struct file *filep, int cmd, unsigned long arg) -{ - int ret = OK; - - switch (cmd) - { - /* @TODO */ - - /* Command was not recognized */ - - default: - snerr("ERROR: Unrecognized cmd: %d\n", cmd); - ret = -ENOTTY; - break; - } - - 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 26faae5987..cb0aad6b51 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