This is an automated email from the ASF dual-hosted git repository. simbit18 pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/nuttx.git
commit 80f88fc3447b61668fe8438fbb96e8e5bfb42820 Author: raiden00pl <[email protected]> AuthorDate: Fri Jul 24 15:02:07 2026 +0200 drivers/sensors: add LIS3DSH accelerometer uORB driver add LIS3DSH accelerometer uORB driver Signed-off-by: raiden00pl <[email protected]> Assisted-by: Claude:Claude-Fable-5 --- .../drivers/special/sensors/sensors_uorb.rst | 1 + drivers/sensors/CMakeLists.txt | 4 + drivers/sensors/Kconfig | 33 ++ drivers/sensors/Make.defs | 4 + drivers/sensors/lis3dsh_uorb.c | 483 +++++++++++++++++++++ include/nuttx/sensors/lis3dsh.h | 24 +- 6 files changed, 548 insertions(+), 1 deletion(-) diff --git a/Documentation/components/drivers/special/sensors/sensors_uorb.rst b/Documentation/components/drivers/special/sensors/sensors_uorb.rst index 490beb1ed06..da61b475c10 100644 --- a/Documentation/components/drivers/special/sensors/sensors_uorb.rst +++ b/Documentation/components/drivers/special/sensors/sensors_uorb.rst @@ -548,6 +548,7 @@ Implemented Drivers - hyt271 - l3gd20 - :doc:`lis2mdl` +- lis3dsh - lsm9ds1 - ltr308 - :doc:`mpu6050` diff --git a/drivers/sensors/CMakeLists.txt b/drivers/sensors/CMakeLists.txt index 883566cd530..bfcf3614c25 100644 --- a/drivers/sensors/CMakeLists.txt +++ b/drivers/sensors/CMakeLists.txt @@ -380,6 +380,10 @@ if(CONFIG_SENSORS) endif() endif() + if(CONFIG_SENSORS_LIS3DSH_UORB) + list(APPEND SRCS lis3dsh_uorb.c) + endif() + if(CONFIG_LIS3DSH) list(APPEND SRCS lis3dsh.c) endif() diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig index a0c2f34b8e1..6de33906550 100644 --- a/drivers/sensors/Kconfig +++ b/drivers/sensors/Kconfig @@ -947,6 +947,39 @@ config LIS3DSH ---help--- Enable driver support for the STMicro LIS3DSH 3-Axis accelerometer. +config SENSORS_LIS3DSH_UORB + bool "STMicro LIS3DSH 3-Axis accelerometer support (UORB)" + default n + select SPI + ---help--- + Enable UORB driver support for the STMicro LIS3DSH 3-Axis + accelerometer. Registers /dev/uorb/sensor_accelN. + +if SENSORS_LIS3DSH_UORB + +config SENSORS_LIS3DSH_UORB_POLL + bool "Enable polling worker thread" + default n + ---help--- + Enables polling of sensor. + +config SENSORS_LIS3DSH_UORB_POLL_INTERVAL + int "Polling interval in microseconds" + depends on SENSORS_LIS3DSH_UORB_POLL + default 100000 + range 0 4294967295 + ---help--- + The interval until a new sensor measurement will be triggered. + +config SENSORS_LIS3DSH_UORB_STACKSIZE + int "Worker thread stack size" + depends on SENSORS_LIS3DSH_UORB_POLL + default 1024 + ---help--- + The stack size for the worker thread. + +endif # SENSORS_LIS3DSH_UORB + config LIS3DH bool "STMicro LIS3DH 3-Axis accelerometer support" default n diff --git a/drivers/sensors/Make.defs b/drivers/sensors/Make.defs index e4593b14543..13cd2adebea 100644 --- a/drivers/sensors/Make.defs +++ b/drivers/sensors/Make.defs @@ -368,6 +368,10 @@ else endif endif +ifeq ($(CONFIG_SENSORS_LIS3DSH_UORB),y) + CSRCS += lis3dsh_uorb.c +endif + ifeq ($(CONFIG_LIS3DSH),y) CSRCS += lis3dsh.c endif diff --git a/drivers/sensors/lis3dsh_uorb.c b/drivers/sensors/lis3dsh_uorb.c new file mode 100644 index 00000000000..581920bb4d6 --- /dev/null +++ b/drivers/sensors/lis3dsh_uorb.c @@ -0,0 +1,483 @@ +/**************************************************************************** + * drivers/sensors/lis3dsh_uorb.c + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <string.h> +#include <errno.h> + +#include <nuttx/debug.h> +#include <nuttx/kmalloc.h> +#include <nuttx/kthread.h> +#include <nuttx/signal.h> +#include <nuttx/semaphore.h> + +#include <nuttx/sensors/sensor.h> +#include <nuttx/sensors/lis3dsh.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define LIS3DSH_WHO_AM_I_VALUE 0x3f + +/* Full-scale is configured to +/-2 g. The nominal sensitivity is + * 0.06 mg/digit; convert LSBs to m/s^2. + */ + +#define LIS3DSH_ONE_G 9.80665f +#define LIS3DSH_SENS_2G 0.00006f /* g/LSB */ +#define LIS3DSH_SCALE (LIS3DSH_SENS_2G * LIS3DSH_ONE_G) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct lis3dsh_sensor_s +{ + struct sensor_lowerhalf_s lower; + FAR struct spi_dev_s *spi; + int devno; +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + bool enabled; + uint32_t interval; + sem_t run; +#endif +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static void lis3dsh_putreg8(FAR struct lis3dsh_sensor_s *priv, + uint8_t regaddr, uint8_t regval); +static uint8_t lis3dsh_getreg8(FAR struct lis3dsh_sensor_s *priv, + uint8_t regaddr); +static void lis3dsh_getregs(FAR struct lis3dsh_sensor_s *priv, + uint8_t regaddr, FAR uint8_t *regval, + int len); +static int lis3dsh_checkid(FAR struct lis3dsh_sensor_s *priv); +static void lis3dsh_start(FAR struct lis3dsh_sensor_s *priv); +static void lis3dsh_read_accel(FAR struct lis3dsh_sensor_s *priv, + FAR struct sensor_accel *accel); + +static int lis3dsh_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, bool enable); +static int lis3dsh_set_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us); +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL +static int lis3dsh_thread(int argc, FAR char **argv); +#else +static int lis3dsh_fetch(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR char *buffer, size_t buflen); +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const struct sensor_ops_s g_lis3dsh_accel_ops = +{ + NULL, /* open */ + NULL, /* close */ + lis3dsh_activate, + lis3dsh_set_interval, + NULL, /* batch */ +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + NULL, /* fetch */ +#else + lis3dsh_fetch, +#endif + NULL, /* flush */ + NULL, /* selftest */ + NULL, /* set_calibvalue */ + NULL, /* calibrate */ + NULL, /* get_info */ + NULL, /* control */ +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: lis3dsh_putreg8 + ****************************************************************************/ + +static void lis3dsh_putreg8(FAR struct lis3dsh_sensor_s *priv, + uint8_t regaddr, uint8_t regval) +{ + SPI_LOCK(priv->spi, true); + SPI_SETFREQUENCY(priv->spi, LIS3DSH_SPI_FREQUENCY); + SPI_SETMODE(priv->spi, LIS3DSH_SPI_MODE); + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(priv->devno), true); + SPI_SEND(priv->spi, regaddr); + SPI_SEND(priv->spi, regval); + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(priv->devno), false); + + SPI_LOCK(priv->spi, false); +} + +/**************************************************************************** + * Name: lis3dsh_getreg8 + ****************************************************************************/ + +static uint8_t lis3dsh_getreg8(FAR struct lis3dsh_sensor_s *priv, + uint8_t regaddr) +{ + uint8_t regval = 0; + + SPI_LOCK(priv->spi, true); + SPI_SETFREQUENCY(priv->spi, LIS3DSH_SPI_FREQUENCY); + SPI_SETMODE(priv->spi, LIS3DSH_SPI_MODE); + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(priv->devno), true); + + /* MSB set indicates a read */ + + SPI_SEND(priv->spi, regaddr | 0x80); + SPI_RECVBLOCK(priv->spi, ®val, 1); + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(priv->devno), false); + SPI_LOCK(priv->spi, false); + + return regval; +} + +/**************************************************************************** + * Name: lis3dsh_getregs + * + * Description: + * Read a block of registers. Address auto-increment (ADD_INC in + * CTRL_REG6) must be enabled for multi-byte transfers. + * + ****************************************************************************/ + +static void lis3dsh_getregs(FAR struct lis3dsh_sensor_s *priv, + uint8_t regaddr, FAR uint8_t *regval, int len) +{ + SPI_LOCK(priv->spi, true); + SPI_SETFREQUENCY(priv->spi, LIS3DSH_SPI_FREQUENCY); + SPI_SETMODE(priv->spi, LIS3DSH_SPI_MODE); + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(priv->devno), true); + + SPI_SEND(priv->spi, regaddr | 0x80); + SPI_RECVBLOCK(priv->spi, regval, len); + + SPI_SELECT(priv->spi, SPIDEV_ACCELEROMETER(priv->devno), false); + SPI_LOCK(priv->spi, false); +} + +/**************************************************************************** + * Name: lis3dsh_checkid + ****************************************************************************/ + +static int lis3dsh_checkid(FAR struct lis3dsh_sensor_s *priv) +{ + uint8_t id = lis3dsh_getreg8(priv, LIS3DSH_WHO_AM_I_REG); + + if (id != LIS3DSH_WHO_AM_I_VALUE) + { + snerr("ERROR: Wrong LIS3DSH WHO_AM_I %02x\n", id); + return -ENODEV; + } + + return OK; +} + +/**************************************************************************** + * Name: lis3dsh_start + * + * Description: + * Configure the device for continuous +/-2 g measurement at 100 Hz. + * + ****************************************************************************/ + +static void lis3dsh_start(FAR struct lis3dsh_sensor_s *priv) +{ + /* Full-scale +/-2 g, 4-wire SPI (CTRL_REG5 = 0) */ + + lis3dsh_putreg8(priv, LIS3DSH_CTRL_REG_5, 0); + + /* Enable register address auto-increment for block reads */ + + lis3dsh_putreg8(priv, LIS3DSH_CTRL_REG_6, + LIS3DSH_CTRL_REG_6_ADD_INC_BM); + + /* Enable X/Y/Z axes, block-data-update, ODR = 100 Hz (0101b) */ + + lis3dsh_putreg8(priv, LIS3DSH_CTRL_REG_4, + LIS3DSH_CTRL_REG_4_XEN_BM | + LIS3DSH_CTRL_REG_4_YEN_BM | + LIS3DSH_CTRL_REG_4_ZEN_BM | + LIS3DSH_CTRL_REG_4_BDU_BM | + LIS3DSH_CTRL_REG_4_ODR_2_BM | + LIS3DSH_CTRL_REG_4_ODR_0_BM); +} + +/**************************************************************************** + * Name: lis3dsh_read_accel + * + * Description: + * Read the current acceleration and convert it to a sensor_accel event. + * + ****************************************************************************/ + +static void lis3dsh_read_accel(FAR struct lis3dsh_sensor_s *priv, + FAR struct sensor_accel *accel) +{ + uint8_t data[6]; + int16_t x; + int16_t y; + int16_t z; + + lis3dsh_getregs(priv, LIS3DSH_OUT_X_L_REG, data, 6); + + x = (int16_t)((uint16_t)data[0] | ((uint16_t)data[1] << 8)); + y = (int16_t)((uint16_t)data[2] | ((uint16_t)data[3] << 8)); + z = (int16_t)((uint16_t)data[4] | ((uint16_t)data[5] << 8)); + + accel->timestamp = sensor_get_timestamp(); + accel->x = (float)x * LIS3DSH_SCALE; + accel->y = (float)y * LIS3DSH_SCALE; + accel->z = (float)z * LIS3DSH_SCALE; + accel->temperature = 0; +} + +/**************************************************************************** + * Name: lis3dsh_activate + ****************************************************************************/ + +static int lis3dsh_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, bool enable) +{ + FAR struct lis3dsh_sensor_s *priv = (FAR struct lis3dsh_sensor_s *)lower; +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + bool start_thread = false; +#endif + + if (enable) + { +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + if (!priv->enabled) + { + start_thread = true; + } +#endif + } + + UNUSED(priv); + +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + priv->enabled = enable; + + if (start_thread) + { + nxsem_post(&priv->run); + } +#endif + + return OK; +} + +/**************************************************************************** + * Name: lis3dsh_set_interval + ****************************************************************************/ + +static int lis3dsh_set_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us) +{ +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + FAR struct lis3dsh_sensor_s *priv = (FAR struct lis3dsh_sensor_s *)lower; + + priv->interval = *period_us; +#endif + + return OK; +} + +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL +/**************************************************************************** + * Name: lis3dsh_thread + * + * Description: + * Poll the sensor at the configured interval and push accelerometer + * samples to the uORB upper half (streaming mode). + * + ****************************************************************************/ + +static int lis3dsh_thread(int argc, FAR char **argv) +{ + FAR struct lis3dsh_sensor_s *priv = + (FAR struct lis3dsh_sensor_s *)((uintptr_t)strtoul(argv[1], NULL, 16)); + struct sensor_accel accel; + int ret; + + while (true) + { + if (!priv->enabled) + { + /* Wait to be woken up by activate() */ + + ret = nxsem_wait(&priv->run); + if (ret < 0) + { + continue; + } + } + + if (priv->enabled) + { + lis3dsh_read_accel(priv, &accel); + priv->lower.push_event(priv->lower.priv, &accel, sizeof(accel)); + } + + nxsig_usleep(priv->interval); + } + + return OK; +} + +#else +/**************************************************************************** + * Name: lis3dsh_fetch + * + * Description: + * Read the most recent accelerometer sample on demand. + * + ****************************************************************************/ + +static int lis3dsh_fetch(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR char *buffer, size_t buflen) +{ + FAR struct lis3dsh_sensor_s *priv = (FAR struct lis3dsh_sensor_s *)lower; + struct sensor_accel accel; + + if (buflen < sizeof(accel)) + { + return -EINVAL; + } + + lis3dsh_read_accel(priv, &accel); + + memcpy(buffer, &accel, sizeof(accel)); + return sizeof(accel); +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: lis3dsh_register_uorb + ****************************************************************************/ + +int lis3dsh_register_uorb(int devno, FAR struct spi_dev_s *spi) +{ + FAR struct lis3dsh_sensor_s *priv; + int ret; +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + FAR char *argv[2]; + char arg1[32]; +#endif + + DEBUGASSERT(spi != NULL); + + priv = kmm_zalloc(sizeof(struct lis3dsh_sensor_s)); + if (priv == NULL) + { + snerr("ERROR: Failed to allocate LIS3DSH instance\n"); + return -ENOMEM; + } + + priv->spi = spi; + priv->devno = devno; + priv->lower.ops = &g_lis3dsh_accel_ops; + priv->lower.type = SENSOR_TYPE_ACCELEROMETER; + priv->lower.nbuffer = 1; +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + priv->enabled = false; + priv->interval = CONFIG_SENSORS_LIS3DSH_UORB_POLL_INTERVAL; + nxsem_init(&priv->run, 0, 0); +#endif + + /* Configure SPI and verify the device id */ + + SPI_LOCK(priv->spi, true); + SPI_SETFREQUENCY(priv->spi, LIS3DSH_SPI_FREQUENCY); + SPI_SETMODE(priv->spi, LIS3DSH_SPI_MODE); + SPI_LOCK(priv->spi, false); + + ret = lis3dsh_checkid(priv); + if (ret < 0) + { + goto errout; + } + + /* Configure the device for continuous +/-2 g measurement at 100 Hz */ + + lis3dsh_start(priv); + + ret = sensor_register(&priv->lower, devno); + if (ret < 0) + { + snerr("ERROR: Failed to register LIS3DSH accel driver: %d\n", ret); + goto errout; + } + +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + snprintf(arg1, sizeof(arg1), "%p", priv); + argv[0] = arg1; + argv[1] = NULL; + + ret = kthread_create("lis3dsh_thread", SCHED_PRIORITY_DEFAULT, + CONFIG_SENSORS_LIS3DSH_UORB_STACKSIZE, + lis3dsh_thread, argv); + if (ret < 0) + { + snerr("ERROR: Failed to create LIS3DSH thread: %d\n", ret); + sensor_unregister(&priv->lower, devno); + goto errout; + } +#endif + + return OK; + +errout: +#ifdef CONFIG_SENSORS_LIS3DSH_UORB_POLL + nxsem_destroy(&priv->run); +#endif + kmm_free(priv); + return ret; +} diff --git a/include/nuttx/sensors/lis3dsh.h b/include/nuttx/sensors/lis3dsh.h index 9134033f75d..6efece046d8 100644 --- a/include/nuttx/sensors/lis3dsh.h +++ b/include/nuttx/sensors/lis3dsh.h @@ -32,7 +32,8 @@ #include <nuttx/fs/ioctl.h> #include <nuttx/spi/spi.h> -#if defined(CONFIG_SPI) && defined(CONFIG_LIS3DSH) +#if defined(CONFIG_SPI) && (defined(CONFIG_LIS3DSH) || \ + defined(CONFIG_SENSORS_LIS3DSH_UORB)) /**************************************************************************** * Pre-processor Definitions @@ -254,6 +255,27 @@ extern "C" int lis3dsh_register(FAR const char *devpath, FAR struct spi_dev_s *spi, FAR struct lis3dsh_config_s *config); +/**************************************************************************** + * Name: lis3dsh_register_uorb + * + * Description: + * Register the LIS3DSH accelerometer as a uORB sensor + * (/dev/uorb/sensor_accel<devno>). + * + * Input Parameters: + * devno - The device number, used to build the character device path. + * spi - An instance of the SPI interface to use to communicate with + * the LIS3DSH. + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +#ifdef CONFIG_SENSORS_LIS3DSH_UORB +int lis3dsh_register_uorb(int devno, FAR struct spi_dev_s *spi); +#endif + #undef EXTERN #ifdef __cplusplus }
