linguini1 commented on code in PR #15789: URL: https://github.com/apache/nuttx/pull/15789#discussion_r1947855269
########## drivers/sensors/lsm6dso32_uorb.c: ########## @@ -0,0 +1,1986 @@ +/**************************************************************************** + * drivers/sensors/lsm6dso32_uorb.c + * + * Contributed by Carleton University InSpace + * + * 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 <nuttx/nuttx.h> + +#include <debug.h> + +#include <nuttx/fs/fs.h> +#include <nuttx/i2c/i2c_master.h> +#include <nuttx/kmalloc.h> +#include <nuttx/kthread.h> +#include <nuttx/mutex.h> +#include <nuttx/random.h> +#include <nuttx/semaphore.h> +#include <nuttx/sensors/lsm6dso32.h> +#include <nuttx/sensors/sensor.h> +#include <nuttx/signal.h> +#include <stdio.h> +#include <sys/types.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* The value that should be in the WHO_AM_I register. */ + +#define WHO_AM_I_VAL 0x6c + +/* Convert milli Gs to m/s^2 */ + +#define MILLIG_TO_MS2 (0.0098067f) + +/* Convert milli-dps to rad/s */ + +#define MDPS_TO_RADS (3.141592653f / (180.0f * 1000.0f)) + +#ifndef CONFIG_SENSORS_LSM6DSO32_I2C_FREQUENCY +#define CONFIG_SENSORS_LSM6DSO32_I2C_FREQUENCY 400000 +#endif /* CONFIG_SENSORS_LSM6DSO32_I2C_FREQUENCY */ + +/* Number of measurement rounds for gyro self test */ + +#define GYRO_SELFTEST_ROUNDS 5 + +/* Minimum and maximum values for self-test at +/-2000dps, converted from + * 1dps/LSB to 70mdps/LSB + */ + +#define GYRO_ST_MIN ((150 * 1000) / 70) +#define GYRO_ST_MAX ((700 * 1000) / 70) + +#define XL_SELFTEST_ROUNDS 5 + +/* Minimum and maximum values for self-test at +/-4g in 1mg/LSB converted to + * 0.122mg/LSB + */ + +#define XL_ST_MIN (50 / 0.122f) +#define XL_ST_MAX (1700 / 0.122f) + +/* Min & max representable accel offset in g using 2^{-10}g/LSB */ + +#define XL_10_W_MAX (127.0f / (1 << 10)) +#define XL_10_W_MIN (-127.0f / (1 << 10)) + +/* Min & max representable accel offset in g using 2^{-6}g/LSB */ + +#define XL_6_W_MAX (127.0f / (1 << 6)) +#define XL_6_W_MIN (-127.0f / (1 << 6)) + +/* Registers */ + +#define WHO_AM_I 0x0f /* Hard-coded address on I2C bus. */ +#define TIMESTAMP0 0x40 /* First timestamp register (32 bits) */ +#define STATUS_REG 0x1e /* The status register */ +#define CTRL1_XL 0x10 /* Accel control reg 1 */ +#define CTRL2_G 0x11 /* Gyro control reg 2 */ +#define CTRL3_C 0x12 /* Control reg 3 */ +#define CTRL4_C 0x13 /* Control reg 4 */ +#define CTRL5_C 0x14 /* Control reg 5 */ +#define CTRL6_C 0x15 /* Control reg 6 */ +#define CTRL7_G 0x16 /* Control reg 7 */ +#define CTRL8_XL 0x17 /* Control reg 8 */ +#define CTRL9_XL 0x18 /* Control reg 9 */ +#define CTRL10_C 0x19 /* Control reg 10 */ +#define FIFO_CTRL4 0x0a /* The fourth FIFO control reg */ +#define INT1_CTRL 0x0d /* INT1 pin control */ +#define INT2_CTRL 0x0e /* INT2 pin control */ +#define OUT_TEMP_L 0x20 /* Temp output low byte. */ +#define OUT_TEMP_H 0x21 /* Temp output high byte. */ +#define OUTX_L_G 0x22 /* Gyro pitch axis (X) low byte. */ +#define OUTX_H_G 0x23 /* Gyro pitch axis (X) high byte. */ +#define OUTY_L_G 0x24 /* Gyro roll axis (Y) low byte. */ +#define OUTY_H_G 0x25 /* Gyro roll axis (Y) high byte. */ +#define OUTZ_L_G 0x26 /* Gyro yaw axis (Z) low byte. */ +#define OUTZ_H_G 0x27 /* Gyro yaw axis (Z) high byte. */ +#define OUTX_L_A 0x28 /* Accel (X) low byte. */ +#define OUTX_H_A 0x29 /* Accel (X) high byte. */ +#define OUTY_L_A 0x2a /* Accel (Y) low byte. */ +#define OUTY_H_A 0x2b /* Accel (Y) high byte. */ +#define OUTZ_L_A 0x2c /* Accel (Z) low byte. */ +#define OUTZ_H_A 0x2d /* Accel (Z) high byte. */ +#define X_OFS_USR 0x73 /* X offset correction accel */ +#define Y_OFS_USR 0x74 /* Y offset correction accel */ +#define Z_OFS_USR 0x75 /* Z offset correction accel */ + +/* Bits */ + +#define BIT_STATUS_XLDA (1 << 0) /* Accel data ready */ +#define BIT_STATUS_GDA (1 << 1) /* Gyro data ready */ +#define BIT_STATUS_TDA (1 << 2) /* Temp data ready */ +#define BIT_G_ST_POS (1 << 2) /* Enable gyro positive self-test */ +#define BIT_XL_ST_POS (1 << 0) /* Enable accel positive self-test */ +#define BIT_USR_OFF_W (1 << 3) /* User offset weight */ +#define BIT_USR_OFF_ON_OUT (1 << 1) /* User offset enable */ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* ODRs common to the accelerometer and the gyroscope. NOTE: this driver does + * implement the 1.6Hz low power ODR for the accelerometer. + */ + +enum lsm6dso32_odr_e +{ + ODR_OFF = 0x0, /* Sensor Deactivated */ + ODR_12_5HZ = 0x1, /* 12.5Hz Rate. */ + ODR_26HZ = 0x2, /* 26Hz Rate. */ + ODR_52HZ = 0x3, /* 52Hz Rate. */ + ODR_104HZ = 0x4, /* 104Hz Rate */ + ODR_208HZ = 0x5, /* 208Hz Rate */ + ODR_416HZ = 0x6, /* 416Hz Rate */ + ODR_833HZ = 0x7, /* 833Hz Rate */ + ODR_1660HZ = 0x8, /* 1.66kHz Rate */ + ODR_3330HZ = 0x9, /* 3.33kHz Rate */ + ODR_6660HZ = 0xa, /* 6.66kHz Rate */ + ODR_1_6HZ = 0xb, /* 1.6Hz Rate */ +}; + +/* Represents a lower half sensor driver of the LSM6DSO32 */ + +struct lsm6dso32_sens_s +{ + FAR struct sensor_lowerhalf_s lower; /* Lower-half sensor driver */ + FAR struct lsm6dso32_dev_s *dev; /* Reference to parent device */ + bool enabled; /* If this sensor is enabled */ + enum lsm6dso32_odr_e odr; /* Measurement interval of this + * sensor */ + int fsr; /* Full scale range of this sensor. + * Can be from either gyro or accel + * FSR enum. */ + sem_t run; /* Polling cycle lock */ + enum lsm6dso32_int_e intpin; /* The interrupt pin for this device */ + bool interrupts; /* Whether or not interrupts are + * enabled. */ + struct work_s work; /* Interrupt work queue + * structure */ +}; + +/* Represents the LSM6DSO23 IMU device */ + +struct lsm6dso32_dev_s +{ + struct lsm6dso32_sens_s gyro; /* Gyroscope */ + struct lsm6dso32_sens_s accel; /* Accelerometer lower half */ + FAR struct i2c_master_s *i2c; /* I2C interface. */ + uint8_t addr; /* I2C address. */ + float gy_off[3]; /* Offsets for gyroscope measurements */ + mutex_t devlock; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int lsm6dso32_control(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, int cmd, + unsigned long arg); +static int lsm6dso32_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, bool enable); +static int lsm6dso32_set_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us); +static int lsm6dso32_selftest(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, unsigned long arg); +static int lsm6dso32_set_calibvalue(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + unsigned long arg); +static int lsm6dso32_get_info(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR struct sensor_device_info_s *info); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* ODR frequencies to measurement intervals in microseconds */ + +static const uint32_t ODR_INTERVAL[] = +{ + [ODR_OFF] = 0, [ODR_12_5HZ] = 80000, [ODR_26HZ] = 38462, + [ODR_52HZ] = 19230, [ODR_104HZ] = 9615, [ODR_208HZ] = 4807, + [ODR_416HZ] = 2403, [ODR_833HZ] = 1200, [ODR_1660HZ] = 602, + [ODR_3330HZ] = 300, [ODR_6660HZ] = 150, [ODR_1_6HZ] = 625000, +}; + +/* Accelerometer FSR sensitivities in m/s^2 per LSB */ + +static const float FSR_XL_SENS[] = +{ + [LSM6DSO32_FSR_XL_4G] = 0.122f * MILLIG_TO_MS2, + [LSM6DSO32_FSR_XL_8G] = 0.244f * MILLIG_TO_MS2, + [LSM6DSO32_FSR_XL_16G] = 0.488f * MILLIG_TO_MS2, + [LSM6DSO32_FSR_XL_32G] = 0.976f * MILLIG_TO_MS2, +}; + +/* Gyro FSR sensitivities in rad/s per LSB */ + +static const float FSR_GYRO_SENS[] = +{ + [LSM6DSO32_FSR_GY_125DPS] = 4.375f * MDPS_TO_RADS, + [LSM6DSO32_FSR_GY_250DPS] = 8.75f * MDPS_TO_RADS, + [LSM6DSO32_FSR_GY_500DPS] = 17.50f * MDPS_TO_RADS, + [LSM6DSO32_FSR_GY_1000DPS] = 35.0f * MDPS_TO_RADS, + [LSM6DSO32_FSR_GY_2000DPS] = 70.0f * MDPS_TO_RADS, +}; + +/* Interrupt control registers */ + +static const uint8_t INT_CTRL[] = +{ + [LSM6DSO32_INT1] = INT1_CTRL, + [LSM6DSO32_INT2] = INT2_CTRL, +}; + +/* Sensor operations */ + +static const struct sensor_ops_s g_sensor_ops = +{ + .fetch = NULL, + .activate = lsm6dso32_activate, + .control = lsm6dso32_control, + .set_interval = lsm6dso32_set_interval, + .selftest = lsm6dso32_selftest, + .set_calibvalue = lsm6dso32_set_calibvalue, + .calibrate = NULL, + .get_info = lsm6dso32_get_info, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: lsm6dso32_write_bytes + * + * Description: + * Write bytes to the LSM6DSO32 sensor. Providing more than one byte will + * write to sequential registers starting at the provided address. + * + * Input Parameters: + * priv - The instance of the LSM6DSO32 sensor. + * addr - The register address to write to. + * buf - The buffer of data to write. + * nbytes - The number of bytes in the buffer to write. + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +static int lsm6dso32_write_bytes(struct lsm6dso32_dev_s *priv, uint8_t addr, + void *buf, size_t nbytes) +{ + struct i2c_msg_s cmd[2]; + + /* Register addressing part of command. */ + + cmd[0].frequency = CONFIG_SENSORS_LSM6DSO32_I2C_FREQUENCY; + cmd[0].addr = priv->addr; + cmd[0].flags = I2C_M_NOSTOP; + cmd[0].buffer = &addr; + cmd[0].length = sizeof(addr); + + /* Data to write. */ + + cmd[1].frequency = CONFIG_SENSORS_LSM6DSO32_I2C_FREQUENCY; + cmd[1].addr = priv->addr; + cmd[1].flags = I2C_M_NOSTART; + cmd[1].buffer = buf; + cmd[1].length = nbytes; + + /* Send command over the wire */ + + return I2C_TRANSFER(priv->i2c, cmd, 2); +} + +/**************************************************************************** + * Name: lsm6dso32_read_bytes + * + * Description: + * Read bytes from the LSM6DSO32 sensor. Reading more than one byte will + * read from sequential registers starting at the provided address. + * + * Input Parameters: + * priv - The instance of the LSM6DSO32 sensor. + * addr - The register address to read from. + * buf - The buffer of data to read into. + * nbytes - The number of bytes to read into the buffer. + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +static int lsm6dso32_read_bytes(struct lsm6dso32_dev_s *priv, uint8_t addr, + void *buf, size_t nbytes) +{ + struct i2c_msg_s cmd[2]; + + /* Register addressing part of command. */ + + cmd[0].frequency = CONFIG_SENSORS_LSM6DSO32_I2C_FREQUENCY; + cmd[0].addr = priv->addr; + cmd[0].flags = I2C_M_NOSTOP; + cmd[0].buffer = &addr; + cmd[0].length = sizeof(addr); + + /* Read data into buffer. */ + + cmd[1].frequency = CONFIG_SENSORS_LSM6DSO32_I2C_FREQUENCY; + cmd[1].addr = priv->addr; + cmd[1].flags = I2C_M_NOSTART | I2C_M_READ; + cmd[1].buffer = buf; + cmd[1].length = nbytes; + + /* Send command over the wire */ + + return I2C_TRANSFER(priv->i2c, cmd, 2); +} + +/**************************************************************************** + * Name: lsm6dso32_set_bits + * + * Description: + * Read current value of desired register and change specified bits + * while preserving previous ones. + * NOTE: Clear operation performed before set operation. + * + * Input Parameters: + * priv - The instance of the LSM6DSO32 sensor. + * addr - The register address being changed. + * set_bits - A mask of the bits to be set to 1. + * clear_bits - A mask of the bits to be set to 0. + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +static int lsm6dso32_set_bits(struct lsm6dso32_dev_s *priv, uint8_t addr, + uint8_t set_bits, uint8_t clear_bits) +{ + int err; + uint8_t reg; + + err = lsm6dso32_read_bytes(priv, addr, ®, sizeof(reg)); + + if (err < 0) + { + return err; + } + + reg = (reg & ~clear_bits) | set_bits; + err = lsm6dso32_write_bytes(priv, addr, ®, sizeof(reg)); + + if (err < 0) + { + return err; + } + + return err; +} + +/**************************************************************************** + * Name: accel_set_odr + * + * Description: + * Sets the accelerometer ODR. + ****************************************************************************/ + +static int accel_set_odr(FAR struct lsm6dso32_dev_s *dev, + enum lsm6dso32_odr_e odr) +{ + int err; + + err = lsm6dso32_set_bits(dev, CTRL1_XL, (odr & 0xf) << 4, 0xf0); + + if (err < 0) + { + return err; + } + + dev->accel.odr = odr; + return err; +} + +/**************************************************************************** + * Name: gyro_set_odr + * + * Description: + * Sets the gyroscope ODR. + ****************************************************************************/ + +static int gyro_set_odr(FAR struct lsm6dso32_dev_s *dev, + enum lsm6dso32_odr_e odr) +{ + DEBUGASSERT(odr != ODR_1_6HZ); /* Invalid setting for gyroscope */ + + int err; + + err = lsm6dso32_set_bits(dev, CTRL2_G, (odr & 0x0f) << 4, 0xf0); + + if (err < 0) + { + return err; + } + + dev->gyro.odr = odr; + + return err; +} + +/**************************************************************************** + * Name: accel_set_fsr + * + * Description: + * Sets the accelerometer FSR. + ****************************************************************************/ + +static int accel_set_fsr(FAR struct lsm6dso32_dev_s *dev, + enum lsm6dso32_fsr_xl_e fsr) +{ + int err; + + err = lsm6dso32_set_bits(dev, CTRL1_XL, (fsr & 0x3) << 2, 0x0c); + + if (err < 0) + { + return err; + } + + dev->accel.fsr = fsr; + return err; +} + +/**************************************************************************** + * Name: gyro_set_fsr + * + * Description: + * Sets the gyroscope FSR. + ****************************************************************************/ + +static int gyro_set_fsr(FAR struct lsm6dso32_dev_s *dev, + enum lsm6dso32_fsr_gyro_e fsr) +{ + int err; + + err = lsm6dso32_set_bits(dev, CTRL2_G, (fsr & 0x7) << 1, 0x0c); + + if (err < 0) + { + return err; + } + + dev->gyro.fsr = fsr; + return err; +} + +/**************************************************************************** + * Name: gyro_int_enable + * + * Description: + * Enables/disables the gyroscope interrupt. + ****************************************************************************/ + +static int gyro_int_enable(FAR struct lsm6dso32_dev_s *dev, bool enable) +{ + int err; + uint8_t enable_bits = enable ? 0x2 : 0x0; + uint8_t disable_bits = enable ? 0x0 : 0x2; + + err = lsm6dso32_set_bits(dev, INT_CTRL[dev->gyro.intpin], enable_bits, + disable_bits); + if (err < 0) + { + return err; + } + + dev->gyro.interrupts = enable; /* We succeeded, update state */ + return err; +} + +/**************************************************************************** + * Name: accel_int_enable + * + * Description: + * Enables/disables the accelerometer interrupt. + ****************************************************************************/ + +static int accel_int_enable(FAR struct lsm6dso32_dev_s *dev, bool enable) +{ + int err; + uint8_t enable_bits = enable ? 0x1 : 0x0; + uint8_t disable_bits = enable ? 0x0 : 0x1; + + err = lsm6dso32_set_bits(dev, INT_CTRL[dev->accel.intpin], enable_bits, + disable_bits); + if (err < 0) + { + return err; + } + + dev->accel.interrupts = enable; /* We succeeded, update state */ + return err; +} + +/**************************************************************************** + * Name: lsm6dso32_convert_temp + * + * Description: + * Converts raw temperature reading into units of degrees Celsius. + ****************************************************************************/ + +static float lsm6dso32_convert_temp(int16_t temp) +{ + return (float)((temp / 256) + 25); +} + +/**************************************************************************** + * Name: lsm6dso32_read_gyro + * + * Description: + * Reads gyroscope data into UORB structure. + ****************************************************************************/ + +static int lsm6dso32_read_gyro(FAR struct lsm6dso32_dev_s *dev, + FAR struct sensor_gyro *data) +{ + int16_t raw_data[4]; /* Holds 1 temp, 3 gyro (xyz) */ + int err; + + err = lsm6dso32_read_bytes(dev, OUT_TEMP_L, raw_data, sizeof(raw_data)); + if (err < 0) + { + return err; + } + + /* Convert data into the format required */ + + data->timestamp = sensor_get_timestamp(); + data->temperature = lsm6dso32_convert_temp(raw_data[0]); + data->x = + (float)(raw_data[1]) * FSR_GYRO_SENS[dev->gyro.fsr] - dev->gy_off[0]; + data->y = + (float)(raw_data[2]) * FSR_GYRO_SENS[dev->gyro.fsr] - dev->gy_off[1]; + data->z = + (float)(raw_data[3]) * FSR_GYRO_SENS[dev->gyro.fsr] - dev->gy_off[2]; + + return err; +} + +/**************************************************************************** + * Name: lsm6dso32_read_accel + * + * Description: + * Reads accelerometer data into UORB structure. + ****************************************************************************/ + +static int lsm6dso32_read_accel(FAR struct lsm6dso32_dev_s *dev, + FAR struct sensor_accel *data) +{ + int16_t raw_data[3]; /* 3 accel (xyz) */ + int16_t raw_temp; /* Temperature */ + int err; + + /* Get accelerometer data */ + + err = lsm6dso32_read_bytes(dev, OUTX_L_A, raw_data, sizeof(raw_data)); + if (err < 0) + { + return err; + } + + /* Get temperature data TODO can I bundle this with gyro by decoupling? */ + + err = lsm6dso32_read_bytes(dev, OUT_TEMP_L, &raw_temp, sizeof(raw_temp)); + if (err < 0) + { + return err; + } + + /* Convert data into the required format */ + + data->timestamp = sensor_get_timestamp(); + data->temperature = lsm6dso32_convert_temp(raw_temp); + data->x = (float)(raw_data[0]) * FSR_XL_SENS[dev->accel.fsr]; + data->y = (float)(raw_data[1]) * FSR_XL_SENS[dev->accel.fsr]; + data->z = (float)(raw_data[2]) * FSR_XL_SENS[dev->accel.fsr]; + + return err; +} + +/**************************************************************************** + * Name: push_gyro + * + * Description: + * Push gyro data to the UORB upper half. + ****************************************************************************/ + +static int push_gyro(FAR struct lsm6dso32_dev_s *dev) +{ + int err; + struct sensor_gyro data; + + err = nxmutex_lock(&dev->devlock); + if (err < 0) + { + return err; + } + + err = lsm6dso32_read_gyro(dev, &data); + if (err < 0) + { + goto early_ret; + } + + dev->gyro.lower.push_event(dev->gyro.lower.priv, &data, sizeof(data)); + +early_ret: + nxmutex_unlock(&dev->devlock); + return err; +} + +/**************************************************************************** + * Name: push_accel + * + * Description: + * Push accelerometer data to the UORB upper half. + ****************************************************************************/ + +static int push_accel(FAR struct lsm6dso32_dev_s *dev) +{ + int err; + struct sensor_accel data; + + err = nxmutex_lock(&dev->devlock); + if (err < 0) + { + return err; + } + + err = lsm6dso32_read_accel(dev, &data); + if (err < 0) + { + goto early_ret; + } + + dev->accel.lower.push_event(dev->accel.lower.priv, &data, sizeof(data)); + +early_ret: + nxmutex_unlock(&dev->devlock); + return err; +} + +/**************************************************************************** + * Name: gyro_worker + * + * Description: + * Worker thread called by gyroscope interrupt handler. + ****************************************************************************/ + +static void gyro_worker(FAR void *arg) +{ + FAR struct lsm6dso32_dev_s *dev = (FAR struct lsm6dso32_dev_s *)(arg); + push_gyro(dev); +} + +/**************************************************************************** + * Name: accel_worker + * + * Description: + * Worker thread called by accelerometer interrupt handler. + ****************************************************************************/ + +static void accel_worker(FAR void *arg) +{ + FAR struct lsm6dso32_dev_s *dev = (FAR struct lsm6dso32_dev_s *)(arg); + push_accel(dev); +} + +/**************************************************************************** + * Name: gyro_int_handler + * + * Description: + * Interrupt handler for gyroscope interrupts. + ****************************************************************************/ + +static int gyro_int_handler(int irq, FAR void *context, FAR void *arg) +{ + FAR struct lsm6dso32_dev_s *dev = (FAR struct lsm6dso32_dev_s *)(arg); + int err; + (void)(context); + + DEBUGASSERT(arg != NULL); + + /* Start high priority worker thread */ + + err = work_queue(HPWORK, &dev->gyro.work, &gyro_worker, dev, 0); + + if (err < 0) + { + snerr("Could not queue LSM6DSO32 gyro work queue: %d\n", err); + } + + return err; +} + +/**************************************************************************** + * Name: accel_int_handler + * + * Description: + * Interrupt handler for accelerometer interrupts. + ****************************************************************************/ + +static int accel_int_handler(int irq, FAR void *context, FAR void *arg) +{ + FAR struct lsm6dso32_dev_s *dev = (FAR struct lsm6dso32_dev_s *)(arg); + int err; + (void)(context); + + DEBUGASSERT(arg != NULL); + + /* Start high priority worker thread */ + + err = work_queue(HPWORK, &dev->accel.work, &accel_worker, dev, 0); + + if (err < 0) + { + snerr("Could not queue LSM6DSO32 accel work queue: %d\n", err); + } + + return err; +} + +/**************************************************************************** + * Name: gyro_thread + * + * Description: + * Polling thread for gyroscope measurements + ****************************************************************************/ + +static int gyro_thread(int argc, char **argv) +{ + FAR struct lsm6dso32_dev_s *dev = + (FAR struct lsm6dso32_dev_s *)((uintptr_t)strtoul(argv[1], NULL, 16)); + int err = 0; + + while (true) + { + /* If the sensor is disabled we wait indefinitely */ + + if (!dev->gyro.enabled) + { + err = nxsem_wait(&dev->gyro.run); + if (err < 0) + { + continue; + } + } + + /* If the sensor is enabled, grab some data */ + + err = push_gyro(dev); + if (err < 0) + { + continue; + } + + /* Wait for next measurement cycle */ + + nxsig_usleep(ODR_INTERVAL[dev->gyro.odr]); + } + + return err; +} + +/**************************************************************************** + * Name: accel_thread + * + * Description: + * Polling thread for accelerometer measurements. + ****************************************************************************/ + +static int accel_thread(int argc, char **argv) +{ + FAR struct lsm6dso32_dev_s *dev = + (FAR struct lsm6dso32_dev_s *)((uintptr_t)strtoul(argv[1], NULL, 16)); + int err = 0; + + while (true) + { + /* If the sensor is disabled we wait indefinitely */ + + if (!dev->accel.enabled) + { + err = nxsem_wait(&dev->accel.run); + if (err < 0) + { + continue; + } + } + + /* If the sensor is enabled, grab some data */ + + err = push_accel(dev); + if (err < 0) + { + continue; + } + + /* Wait for next measurement cycle */ + + nxsig_usleep(ODR_INTERVAL[dev->accel.odr]); + } + + return err; +} + +/**************************************************************************** + * Name: lsm6dso32_activate + ****************************************************************************/ + +static int lsm6dso32_activate(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, bool enable) +{ + FAR struct lsm6dso32_sens_s *sens = + container_of(lower, FAR struct lsm6dso32_sens_s, lower); + FAR struct lsm6dso32_dev_s *dev = sens->dev; + bool start_thread = false; + int err = 0; + + err = nxmutex_lock(&dev->devlock); + if (err < 0) + { + return err; + snerr("Failed to deactivate/activate LSM6DSO32"); + } + + /* Start collecting data continuously and enable thread if not already + * enabled + */ + + if (enable && !sens->enabled) + { + start_thread = true; + + /* Set to a relatively low sampling rate to start up */ + + if (lower->type == SENSOR_TYPE_GYROSCOPE) + { + err = gyro_set_odr(dev, ODR_12_5HZ); + } + else + { + err = accel_set_odr(dev, ODR_12_5HZ); + } + + if (err < 0) + { + goto early_ret; + } + } + + /* Turn off sensor if we're disabling */ + + if (!enable && sens->enabled) + { + if (lower->type == SENSOR_TYPE_GYROSCOPE) + { + err = gyro_set_odr(dev, ODR_OFF); + } + else + { + err = accel_set_odr(dev, ODR_OFF); + } + + if (err < 0) + { + goto early_ret; + } + } + + /* If we got here, there was no error, we can record the activation state */ + + sens->enabled = enable; Review Comment: I'm not sure what you mean, as opposed to polling from a thread? -- This is an automated message from the Apache Git Service. To respond to the message, please log on to GitHub and use the URL above to go to the specific comment. To unsubscribe, e-mail: commits-unsubscr...@nuttx.apache.org For queries about this service, please contact Infrastructure at: us...@infra.apache.org