Skip to content

Commit

Permalink
WIP: icm42688 v2
Browse files Browse the repository at this point in the history 
icm42688 RTIO implementation

Signed-off-by: Al Semjonovs <[email protected]>
  • Loading branch information
@asemjonovs @yperess
asemjonovs authored and yperess committed May 16, 2023
1 parent b08e208 commit 288eaff
Show file tree
Hide file tree
Showing 16 changed files with 1,049 additions and 86 deletions.
9 changes: 9 additions & 0 deletions drivers/sensor/Kconfig
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,15 @@ config SENSOR_SHELL
help
This shell provides access to basic sensor data.

config SENSOR_SHELL_THREAD_STACK_SIZE
int "Stack size for the sensor shell data processing thread"
depends on SENSOR_SHELL
default 1024
help
The sensor shell uses a dedicated thread to process data coming from the
sensors in either one-shot or streaming mode. Use this config to control
the size of that thread's stack.

config SENSOR_SHELL_BATTERY
bool "Sensor shell 'battery' command"
depends on SHELL
Expand Down
2 changes: 1 addition & 1 deletion drivers/sensor/default_rtio_sensor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ static void sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe)

if (api->submit != NULL) {
api->submit(dev, iodev_sqe);
} else {
} else if (!cfg->is_streaming) {
sensor_submit_fallback(dev, iodev_sqe);
}
}
Expand Down
2 changes: 2 additions & 0 deletions drivers/sensor/icm42688/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,11 @@ zephyr_library()
zephyr_library_sources(
icm42688.c
icm42688_common.c
icm42688_decoder.c
icm42688_spi.c
)

zephyr_library_sources_ifdef(CONFIG_ICM42688_STREAM icm42688_rtio.c)
zephyr_library_sources_ifdef(CONFIG_ICM42688_TRIGGER icm42688_trigger.c)
zephyr_library_sources_ifdef(CONFIG_EMUL_ICM42688 icm42688_emul.c)
zephyr_include_directories_ifdef(CONFIG_EMUL_ICM42688 .)
15 changes: 15 additions & 0 deletions drivers/sensor/icm42688/Kconfig
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,13 @@ config EMUL_ICM42688
Enable the hardware emulator for the ICM42688. Doing so allows exercising
sensor APIs for this IMU in native_posix and qemu.

config ICM42688_DECODER
bool "ICM42688 decoder logic"
default y if ICM42688
help
Compile the ICM42688 decoder API which allows decoding raw data returned
from the sensor.

if ICM42688

choice
Expand All @@ -42,6 +49,14 @@ config ICM42688_TRIGGER_OWN_THREAD

endchoice

config ICM42688_STREAM
bool "Use hardware FIFO to stream data"
select ICM42688_TRIGGER
default y
depends on SPI_RTIO
help
Use this config option to enable streaming sensor data via RTIO subsystem.

config ICM42688_TRIGGER
bool

Expand Down
134 changes: 91 additions & 43 deletions drivers/sensor/icm42688/icm42688.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,23 +13,15 @@
#include <zephyr/sys/byteorder.h>

#include "icm42688.h"
#include "icm42688_decoder.h"
#include "icm42688_reg.h"
#include "icm42688_rtio.h"
#include "icm42688_spi.h"
#include "icm42688_trigger.h"

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(ICM42688, CONFIG_SENSOR_LOG_LEVEL);

struct icm42688_sensor_data {
struct icm42688_dev_data dev_data;

int16_t readings[7];
};

struct icm42688_sensor_config {
struct icm42688_dev_cfg dev_cfg;
};

static void icm42688_convert_accel(struct sensor_value *val, int16_t raw_val,
struct icm42688_cfg *cfg)
{
Expand All @@ -47,42 +39,81 @@ static inline void icm42688_convert_temp(struct sensor_value *val, int16_t raw_v
icm42688_temp_c((int32_t)raw_val, &val->val1, &val->val2);
}

static int icm42688_channel_get(const struct device *dev, enum sensor_channel chan,
struct sensor_value *val)
int icm42688_get_channel_reading(enum sensor_channel chan, int16_t readings[7], int16_t out[3])
{
struct icm42688_sensor_data *data = dev->data;
switch (chan) {
case SENSOR_CHAN_ACCEL_XYZ:
out[0] = readings[1];
out[1] = readings[2];
out[2] = readings[3];
break;
case SENSOR_CHAN_ACCEL_X:
out[0] = readings[1];
break;
case SENSOR_CHAN_ACCEL_Y:
out[0] = readings[2];
break;
case SENSOR_CHAN_ACCEL_Z:
out[0] = readings[3];
break;
case SENSOR_CHAN_GYRO_XYZ:
out[0] = readings[4];
out[1] = readings[5];
out[2] = readings[6];
break;
case SENSOR_CHAN_GYRO_X:
out[0] = readings[4];
break;
case SENSOR_CHAN_GYRO_Y:
out[0] = readings[5];
break;
case SENSOR_CHAN_GYRO_Z:
out[0] = readings[6];
break;
case SENSOR_CHAN_DIE_TEMP:
out[0] = readings[0];
break;
default:
return -ENOTSUP;
}

return 0;
}

int icm42688_channel_parse_readings(enum sensor_channel chan, int16_t readings[7],
struct icm42688_cfg *cfg, struct sensor_value *val)
{
switch (chan) {
case SENSOR_CHAN_ACCEL_XYZ:
icm42688_convert_accel(&val[0], data->readings[1], &data->dev_data.cfg);
icm42688_convert_accel(&val[1], data->readings[2], &data->dev_data.cfg);
icm42688_convert_accel(&val[2], data->readings[3], &data->dev_data.cfg);
icm42688_convert_accel(&val[0], readings[1], cfg);
icm42688_convert_accel(&val[1], readings[2], cfg);
icm42688_convert_accel(&val[2], readings[3], cfg);
break;
case SENSOR_CHAN_ACCEL_X:
icm42688_convert_accel(val, data->readings[1], &data->dev_data.cfg);
icm42688_convert_accel(val, readings[1], cfg);
break;
case SENSOR_CHAN_ACCEL_Y:
icm42688_convert_accel(val, data->readings[2], &data->dev_data.cfg);
icm42688_convert_accel(val, readings[2], cfg);
break;
case SENSOR_CHAN_ACCEL_Z:
icm42688_convert_accel(val, data->readings[3], &data->dev_data.cfg);
icm42688_convert_accel(val, readings[3], cfg);
break;
case SENSOR_CHAN_GYRO_XYZ:
icm42688_convert_gyro(&val[0], data->readings[4], &data->dev_data.cfg);
icm42688_convert_gyro(&val[1], data->readings[5], &data->dev_data.cfg);
icm42688_convert_gyro(&val[2], data->readings[6], &data->dev_data.cfg);
icm42688_convert_gyro(&val[0], readings[4], cfg);
icm42688_convert_gyro(&val[1], readings[5], cfg);
icm42688_convert_gyro(&val[2], readings[6], cfg);
break;
case SENSOR_CHAN_GYRO_X:
icm42688_convert_gyro(val, data->readings[4], &data->dev_data.cfg);
icm42688_convert_gyro(val, readings[4], cfg);
break;
case SENSOR_CHAN_GYRO_Y:
icm42688_convert_gyro(val, data->readings[5], &data->dev_data.cfg);
icm42688_convert_gyro(val, readings[5], cfg);
break;
case SENSOR_CHAN_GYRO_Z:
icm42688_convert_gyro(val, data->readings[6], &data->dev_data.cfg);
icm42688_convert_gyro(val, readings[6], cfg);
break;
case SENSOR_CHAN_DIE_TEMP:
icm42688_convert_temp(val, data->readings[0]);
icm42688_convert_temp(val, readings[0]);
break;
default:
return -ENOTSUP;
Expand All @@ -91,7 +122,15 @@ static int icm42688_channel_get(const struct device *dev, enum sensor_channel ch
return 0;
}

static int icm42688_sample_fetch(const struct device *dev, enum sensor_channel chan)
int icm42688_channel_get(const struct device *dev, enum sensor_channel chan,
struct sensor_value *val)
{
struct icm42688_sensor_data *data = dev->data;

return icm42688_channel_parse_readings(chan, data->readings, &data->cfg, val);
}

int icm42688_sample_fetch(const struct device *dev, enum sensor_channel chan)
{
uint8_t status;
struct icm42688_sensor_data *data = dev->data;
Expand Down Expand Up @@ -126,7 +165,7 @@ static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr, const struct sensor_value *val)
{
const struct icm42688_sensor_data *data = dev->data;
struct icm42688_cfg new_config = data->dev_data.cfg;
struct icm42688_cfg new_config = data->cfg;
int res = 0;

__ASSERT_NO_MSG(val != NULL);
Expand Down Expand Up @@ -174,7 +213,7 @@ static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan,
enum sensor_attribute attr, struct sensor_value *val)
{
const struct icm42688_sensor_data *data = dev->data;
const struct icm42688_cfg *cfg = &data->dev_data.cfg;
const struct icm42688_cfg *cfg = &data->cfg;
int res = 0;

__ASSERT_NO_MSG(val != NULL);
Expand Down Expand Up @@ -223,6 +262,8 @@ static const struct sensor_driver_api icm42688_driver_api = {
#ifdef CONFIG_ICM42688_TRIGGER
.trigger_set = icm42688_trigger_set,
#endif
.get_decoder = icm42688_get_decoder,
.submit = icm42688_submit,
};

int icm42688_init(const struct device *dev)
Expand All @@ -247,22 +288,17 @@ int icm42688_init(const struct device *dev)
LOG_ERR("Failed to initialize triggers");
return res;
}

res = icm42688_trigger_enable_interrupt(dev);
if (res != 0) {
LOG_ERR("Failed to enable triggers");
return res;
}
#endif

data->dev_data.cfg.accel_mode = ICM42688_ACCEL_LN;
data->dev_data.cfg.gyro_mode = ICM42688_GYRO_LN;
data->dev_data.cfg.accel_fs = ICM42688_ACCEL_FS_2G;
data->dev_data.cfg.gyro_fs = ICM42688_GYRO_FS_125;
data->dev_data.cfg.accel_odr = ICM42688_ACCEL_ODR_1000;
data->dev_data.cfg.gyro_odr = ICM42688_GYRO_ODR_1000;
data->cfg.accel_mode = ICM42688_ACCEL_LN;
data->cfg.gyro_mode = ICM42688_GYRO_LN;
data->cfg.accel_fs = ICM42688_ACCEL_FS_2G;
data->cfg.gyro_fs = ICM42688_GYRO_FS_125;
data->cfg.accel_odr = ICM42688_ACCEL_ODR_1000;
data->cfg.gyro_odr = ICM42688_GYRO_ODR_1000;
data->cfg.fifo_en = IS_ENABLED(CONFIG_ICM42688_STREAM);

res = icm42688_configure(dev, &data->dev_data.cfg);
res = icm42688_configure(dev, &data->cfg);
if (res != 0) {
LOG_ERR("Failed to configure");
return res;
Expand All @@ -286,9 +322,21 @@ void icm42688_unlock(const struct device *dev)
#define ICM42688_SPI_CFG \
SPI_OP_MODE_MASTER | SPI_MODE_CPOL | SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_TRANSFER_MSB

#define ICM42688_RTIO_DEFINE(inst) \
SPI_DT_IODEV_DEFINE(icm42688_spi_iodev_##inst, DT_DRV_INST(inst), ICM42688_SPI_CFG, 0U); \
RTIO_DEFINE(icm42688_rtio_##inst, 8, 4);

#define ICM42688_DEFINE_DATA(inst) \
IF_ENABLED(CONFIG_ICM42688_STREAM, (ICM42688_RTIO_DEFINE(inst))); \
static struct icm42688_sensor_data icm42688_driver_##inst = { \
IF_ENABLED(CONFIG_ICM42688_STREAM, ( \
.r = &icm42688_rtio_##inst, \
.spi_iodev = &icm42688_spi_iodev_##inst, \
)) \
};

#define ICM42688_INIT(inst) \
static struct icm42688_sensor_data icm42688_driver_##inst = { 0 }; \
ICM42688_DEFINE_DATA(inst); \
\
static const struct icm42688_sensor_config icm42688_cfg_##inst = { \
.dev_cfg = \
Expand Down
28 changes: 23 additions & 5 deletions drivers/sensor/icm42688/icm42688.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -385,17 +385,17 @@ struct icm42688_cfg {
/* TODO additional FIFO options */

/* TODO interrupt options */
bool interrupt1_drdy;
bool interrupt1_fifo_ths;
bool interrupt1_fifo_full;
};

struct icm42688_trigger_entry {
struct sensor_trigger trigger;
sensor_trigger_handler_t handler;
};

/**
* @brief Device data (struct device)
*/
struct icm42688_dev_data {
struct icm42688_sensor_data {
struct icm42688_cfg cfg;
#ifdef CONFIG_ICM42688_TRIGGER
#if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD)
Expand All @@ -405,12 +405,22 @@ struct icm42688_dev_data {
#elif defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD)
struct k_work work;
#endif
#ifdef CONFIG_ICM42688_STREAM
struct rtio_iodev_sqe *streaming_sqe;
struct rtio *r;
struct rtio_iodev *spi_iodev;
uint8_t int_status;
uint16_t fifo_count;
atomic_t reading_fifo;
#endif /* CONFIG_ICM42688_STREAM */
const struct device *dev;
struct gpio_callback gpio_cb;
sensor_trigger_handler_t data_ready_handler;
const struct sensor_trigger *data_ready_trigger;
struct k_mutex mutex;
#endif /* CONFIG_ICM42688_TRIGGER */

int16_t readings[7];
};

/**
Expand All @@ -422,6 +432,10 @@ struct icm42688_dev_cfg {
struct gpio_dt_spec gpio_int2;
};

struct icm42688_sensor_config {
struct icm42688_dev_cfg dev_cfg;
};

/**
* @brief Reset the sensor
*
Expand All @@ -443,7 +457,6 @@ int icm42688_reset(const struct device *dev);
*/
int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg);


/**
* @brief Safely (re)Configure the sensor with the given configuration
*
Expand Down Expand Up @@ -667,4 +680,9 @@ static inline void icm42688_temp_c(int32_t in, int32_t *out_c, uint32_t *out_uc)
*out_uc = ((in100 - (*out_c) * sensitivity) * INT64_C(1000000)) / sensitivity;
}

int icm42688_sample_fetch(const struct device *dev, enum sensor_channel chan);
int icm42688_channel_get(const struct device *dev, enum sensor_channel chan,
struct sensor_value *val);
int icm42688_get_channel_reading(enum sensor_channel chan, int16_t readings[7], int16_t out[3]);

#endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_H_ */
Loading

0 comments on commit 288eaff

Please sign in to comment.