icm42688: Implement streaming APIs
Add streaming implementation for icm42688 using both threshold and full FIFO triggers. Signed-off-by: Yuval Peress <peress@google.com> topic#sensor_stream
This commit is contained in:
Yuval Peress
Maureen Helm
parent
94dc05b3f2
commit
1326c7c454
|
|
@ -10,6 +10,7 @@ zephyr_library_sources(
|
|||
|
||||
zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API icm42688_rtio.c)
|
||||
zephyr_library_sources_ifdef(CONFIG_ICM42688_DECODER icm42688_decoder.c)
|
||||
zephyr_library_sources_ifdef(CONFIG_ICM42688_STREAM icm42688_rtio_stream.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 .)
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@ if ICM42688
|
|||
|
||||
choice
|
||||
prompt "Trigger mode"
|
||||
default ICM42688_TRIGGER_NONE if ICM42688_STREAM
|
||||
default ICM42688_TRIGGER_GLOBAL_THREAD
|
||||
help
|
||||
Specify the type of triggering to be used by the driver
|
||||
|
|
@ -50,6 +51,15 @@ 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
|
||||
depends on SENSOR_ASYNC_API
|
||||
help
|
||||
Use this config option to enable streaming sensor data via RTIO subsystem.
|
||||
|
||||
config ICM42688_TRIGGER
|
||||
bool
|
||||
|
||||
|
|
|
|||
|
|
@ -82,7 +82,7 @@ int icm42688_channel_parse_readings(enum sensor_channel chan, int16_t readings[7
|
|||
}
|
||||
|
||||
static int icm42688_channel_get(const struct device *dev, enum sensor_channel chan,
|
||||
struct sensor_value *val)
|
||||
struct sensor_value *val)
|
||||
{
|
||||
struct icm42688_dev_data *data = dev->data;
|
||||
|
||||
|
|
@ -156,6 +156,19 @@ static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan,
|
|||
res = -EINVAL;
|
||||
}
|
||||
break;
|
||||
case SENSOR_CHAN_ALL:
|
||||
if (attr == SENSOR_ATTR_FIFO_WATERMARK) {
|
||||
int64_t mval = sensor_value_to_micro(val);
|
||||
|
||||
if (mval < 0 || mval > 1000000) {
|
||||
return -EINVAL;
|
||||
}
|
||||
new_config.fifo_wm = CLAMP(mval * 2048 / 1000000, 0, 2048);
|
||||
} else {
|
||||
LOG_ERR("Unsupported attribute");
|
||||
res = -EINVAL;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
LOG_ERR("Unsupported channel");
|
||||
res = -EINVAL;
|
||||
|
|
@ -257,7 +270,13 @@ int icm42688_init(const struct device *dev)
|
|||
data->cfg.gyro_mode = ICM42688_GYRO_LN;
|
||||
data->cfg.gyro_fs = ICM42688_GYRO_FS_125;
|
||||
data->cfg.gyro_odr = ICM42688_GYRO_ODR_1000;
|
||||
data->cfg.fifo_en = false;
|
||||
data->cfg.temp_dis = false;
|
||||
data->cfg.fifo_en = IS_ENABLED(CONFIG_ICM42688_STREAM);
|
||||
data->cfg.fifo_wm = 0;
|
||||
data->cfg.fifo_hires = 0;
|
||||
data->cfg.interrupt1_drdy = 0;
|
||||
data->cfg.interrupt1_fifo_ths = 0;
|
||||
data->cfg.interrupt1_fifo_full = 0;
|
||||
|
||||
res = icm42688_configure(dev, &data->cfg);
|
||||
if (res != 0) {
|
||||
|
|
@ -283,8 +302,15 @@ 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) \
|
||||
static struct icm42688_dev_data icm42688_driver_##inst;
|
||||
IF_ENABLED(CONFIG_ICM42688_STREAM, (ICM42688_RTIO_DEFINE(inst))); \
|
||||
static struct icm42688_dev_data icm42688_driver_##inst = { \
|
||||
IF_ENABLED(CONFIG_ICM42688_STREAM, (.r = &icm42688_rtio_##inst, \
|
||||
.spi_iodev = &icm42688_spi_iodev_##inst,))};
|
||||
|
||||
#define ICM42688_INIT(inst) \
|
||||
ICM42688_DEFINE_DATA(inst); \
|
||||
|
|
|
|||
|
|
@ -385,6 +385,9 @@ 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 {
|
||||
|
|
@ -405,6 +408,15 @@ 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;
|
||||
uint64_t timestamp;
|
||||
atomic_t reading_fifo;
|
||||
#endif /* CONFIG_ICM42688_STREAM */
|
||||
const struct device *dev;
|
||||
struct gpio_callback gpio_cb;
|
||||
sensor_trigger_handler_t data_ready_handler;
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@
|
|||
#include "icm42688.h"
|
||||
#include "icm42688_reg.h"
|
||||
#include "icm42688_spi.h"
|
||||
#include "icm42688_trigger.h"
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_REGISTER(ICM42688_LL, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
|
@ -165,8 +166,12 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg)
|
|||
}
|
||||
|
||||
/* Pulse mode with async reset (resets interrupt line on int status read) */
|
||||
res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_CONFIG,
|
||||
BIT_INT1_DRIVE_CIRCUIT | BIT_INT1_POLARITY);
|
||||
if (IS_ENABLED(CONFIG_ICM42688_TRIGGER)) {
|
||||
res = icm42688_trigger_enable_interrupt(dev, cfg);
|
||||
} else {
|
||||
res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_CONFIG,
|
||||
BIT_INT1_DRIVE_CIRCUIT | BIT_INT1_POLARITY);
|
||||
}
|
||||
if (res) {
|
||||
LOG_ERR("Error writing to INT_CONFIG");
|
||||
return res;
|
||||
|
|
|
|||
|
|
@ -198,9 +198,262 @@ int icm42688_encode(const struct device *dev, const enum sensor_channel *const c
|
|||
return 0;
|
||||
}
|
||||
|
||||
#define IS_ACCEL(chan) ((chan) >= SENSOR_CHAN_ACCEL_X && (chan) <= SENSOR_CHAN_ACCEL_XYZ)
|
||||
#define IS_GYRO(chan) ((chan) >= SENSOR_CHAN_GYRO_X && (chan) <= SENSOR_CHAN_GYRO_XYZ)
|
||||
|
||||
static inline q31_t icm42688_read_temperature_from_packet(const uint8_t *pkt)
|
||||
{
|
||||
int32_t temperature;
|
||||
int32_t whole;
|
||||
int32_t fraction;
|
||||
|
||||
/* Temperature always assumes a shift of 9 for a range of (-273,273) C */
|
||||
if (FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1) {
|
||||
temperature = (pkt[0xd] << 8) | pkt[0xe];
|
||||
|
||||
icm42688_temp_c(temperature, &whole, &fraction);
|
||||
} else {
|
||||
if (FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1 &&
|
||||
FIELD_GET(FIFO_HEADER_GYRO, pkt[0]) == 1) {
|
||||
temperature = pkt[0xd];
|
||||
} else {
|
||||
temperature = pkt[0x7];
|
||||
}
|
||||
|
||||
int64_t sensitivity = 207;
|
||||
int64_t temperature100 = (temperature * 100) + (25 * sensitivity);
|
||||
|
||||
whole = temperature100 / sensitivity;
|
||||
fraction =
|
||||
((temperature100 - whole * sensitivity) * INT64_C(1000000)) / sensitivity;
|
||||
}
|
||||
__ASSERT_NO_MSG(whole >= -512 && whole <= 511);
|
||||
return FIELD_PREP(GENMASK(31, 22), whole) | (fraction * GENMASK64(21, 0) / 1000000);
|
||||
}
|
||||
|
||||
static int icm42688_read_imu_from_packet(const uint8_t *pkt, bool is_accel, int fs,
|
||||
uint8_t axis_offset, q31_t *out)
|
||||
{
|
||||
int32_t value;
|
||||
int64_t scale = 0;
|
||||
int32_t max = BIT(15);
|
||||
int offset = 1 + (axis_offset * 2);
|
||||
|
||||
if (is_accel) {
|
||||
switch (fs) {
|
||||
case ICM42688_ACCEL_FS_2G:
|
||||
scale = INT64_C(2) * BIT(31 - 5) * 9.80665;
|
||||
break;
|
||||
case ICM42688_ACCEL_FS_4G:
|
||||
scale = INT64_C(4) * BIT(31 - 6) * 9.80665;
|
||||
break;
|
||||
case ICM42688_ACCEL_FS_8G:
|
||||
scale = INT64_C(8) * BIT(31 - 7) * 9.80665;
|
||||
break;
|
||||
case ICM42688_ACCEL_FS_16G:
|
||||
scale = INT64_C(16) * BIT(31 - 8) * 9.80665;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
switch (fs) {
|
||||
case ICM42688_GYRO_FS_2000:
|
||||
scale = 164;
|
||||
break;
|
||||
case ICM42688_GYRO_FS_1000:
|
||||
scale = 328;
|
||||
break;
|
||||
case ICM42688_GYRO_FS_500:
|
||||
scale = 655;
|
||||
break;
|
||||
case ICM42688_GYRO_FS_250:
|
||||
scale = 1310;
|
||||
break;
|
||||
case ICM42688_GYRO_FS_125:
|
||||
scale = 2620;
|
||||
break;
|
||||
case ICM42688_GYRO_FS_62_5:
|
||||
scale = 5243;
|
||||
break;
|
||||
case ICM42688_GYRO_FS_31_25:
|
||||
scale = 10486;
|
||||
break;
|
||||
case ICM42688_GYRO_FS_15_625:
|
||||
scale = 20972;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!is_accel && FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1) {
|
||||
offset += 7;
|
||||
}
|
||||
|
||||
value = (int16_t)sys_le16_to_cpu((pkt[offset] << 8) | pkt[offset + 1]);
|
||||
|
||||
if (FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1) {
|
||||
uint32_t mask = is_accel ? GENMASK(7, 4) : GENMASK(3, 0);
|
||||
|
||||
offset = 0x11 + axis_offset;
|
||||
value = (value << 4) | FIELD_GET(mask, pkt[offset]);
|
||||
/* In 20 bit mode, FS can only be +/-16g and +/-2000dps */
|
||||
scale = is_accel ? (INT64_C(16) * BIT(8) * 9.80665) : 131;
|
||||
max = is_accel ? BIT(18) : BIT(19);
|
||||
if (value == -524288) {
|
||||
/* Invalid 20 bit value */
|
||||
return -ENODATA;
|
||||
}
|
||||
} else {
|
||||
if (value <= -32767) {
|
||||
/* Invalid 16 bit value */
|
||||
return -ENODATA;
|
||||
}
|
||||
}
|
||||
|
||||
*out = (q31_t)(value * scale / max);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static uint32_t accel_period_ns[] = {
|
||||
[ICM42688_ACCEL_ODR_1_5625] = UINT32_C(10000000000000) / 15625,
|
||||
[ICM42688_ACCEL_ODR_3_125] = UINT32_C(10000000000000) / 31250,
|
||||
[ICM42688_ACCEL_ODR_6_25] = UINT32_C(10000000000000) / 62500,
|
||||
[ICM42688_ACCEL_ODR_12_5] = UINT32_C(10000000000000) / 12500,
|
||||
[ICM42688_ACCEL_ODR_25] = UINT32_C(1000000000) / 25,
|
||||
[ICM42688_ACCEL_ODR_50] = UINT32_C(1000000000) / 50,
|
||||
[ICM42688_ACCEL_ODR_100] = UINT32_C(1000000000) / 100,
|
||||
[ICM42688_ACCEL_ODR_200] = UINT32_C(1000000000) / 200,
|
||||
[ICM42688_ACCEL_ODR_500] = UINT32_C(1000000000) / 500,
|
||||
[ICM42688_ACCEL_ODR_1000] = UINT32_C(1000000),
|
||||
[ICM42688_ACCEL_ODR_2000] = UINT32_C(1000000) / 2,
|
||||
[ICM42688_ACCEL_ODR_4000] = UINT32_C(1000000) / 4,
|
||||
[ICM42688_ACCEL_ODR_8000] = UINT32_C(1000000) / 8,
|
||||
[ICM42688_ACCEL_ODR_16000] = UINT32_C(1000000) / 16,
|
||||
[ICM42688_ACCEL_ODR_32000] = UINT32_C(1000000) / 32,
|
||||
};
|
||||
|
||||
static uint32_t gyro_period_ns[] = {
|
||||
[ICM42688_GYRO_ODR_12_5] = UINT32_C(10000000000000) / 12500,
|
||||
[ICM42688_GYRO_ODR_25] = UINT32_C(1000000000) / 25,
|
||||
[ICM42688_GYRO_ODR_50] = UINT32_C(1000000000) / 50,
|
||||
[ICM42688_GYRO_ODR_100] = UINT32_C(1000000000) / 100,
|
||||
[ICM42688_GYRO_ODR_200] = UINT32_C(1000000000) / 200,
|
||||
[ICM42688_GYRO_ODR_500] = UINT32_C(1000000000) / 500,
|
||||
[ICM42688_GYRO_ODR_1000] = UINT32_C(1000000),
|
||||
[ICM42688_GYRO_ODR_2000] = UINT32_C(1000000) / 2,
|
||||
[ICM42688_GYRO_ODR_4000] = UINT32_C(1000000) / 4,
|
||||
[ICM42688_GYRO_ODR_8000] = UINT32_C(1000000) / 8,
|
||||
[ICM42688_GYRO_ODR_16000] = UINT32_C(1000000) / 16,
|
||||
[ICM42688_GYRO_ODR_32000] = UINT32_C(1000000) / 32,
|
||||
};
|
||||
|
||||
static int icm42688_fifo_decode(const uint8_t *buffer, enum sensor_channel channel,
|
||||
size_t channel_idx, uint32_t *fit, uint16_t max_count,
|
||||
void *data_out)
|
||||
{
|
||||
const struct icm42688_fifo_data *edata = (const struct icm42688_fifo_data *)buffer;
|
||||
const uint8_t *buffer_end = buffer + sizeof(struct icm42688_fifo_data) + edata->fifo_count;
|
||||
int accel_frame_count = 0;
|
||||
int gyro_frame_count = 0;
|
||||
int count = 0;
|
||||
int rc;
|
||||
|
||||
if ((uintptr_t)buffer_end <= *fit || channel_idx != 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
((struct sensor_data_header *)data_out)->base_timestamp_ns = edata->header.timestamp;
|
||||
|
||||
buffer += sizeof(struct icm42688_fifo_data);
|
||||
while (count < max_count && buffer < buffer_end) {
|
||||
const bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]) == 1;
|
||||
const bool has_accel = FIELD_GET(FIFO_HEADER_ACCEL, buffer[0]) == 1;
|
||||
const bool has_gyro = FIELD_GET(FIFO_HEADER_GYRO, buffer[0]) == 1;
|
||||
const uint8_t *frame_end = buffer;
|
||||
|
||||
if (is_20b) {
|
||||
frame_end += 20;
|
||||
} else if (has_accel && has_gyro) {
|
||||
frame_end += 16;
|
||||
} else {
|
||||
frame_end += 8;
|
||||
}
|
||||
if (has_accel) {
|
||||
accel_frame_count++;
|
||||
}
|
||||
if (has_gyro) {
|
||||
gyro_frame_count++;
|
||||
}
|
||||
|
||||
if ((uintptr_t)buffer < *fit) {
|
||||
/* This frame was already decoded, move on to the next frame */
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
if (channel == SENSOR_CHAN_DIE_TEMP) {
|
||||
struct sensor_q31_data *data = (struct sensor_q31_data *)data_out;
|
||||
|
||||
data->shift = 9;
|
||||
if (has_accel) {
|
||||
data->readings[count].timestamp_delta =
|
||||
accel_period_ns[edata->accel_odr] * (accel_frame_count - 1);
|
||||
} else {
|
||||
data->readings[count].timestamp_delta =
|
||||
gyro_period_ns[edata->gyro_odr] * (gyro_frame_count - 1);
|
||||
}
|
||||
data->readings[count].temperature =
|
||||
icm42688_read_temperature_from_packet(buffer);
|
||||
} else if (IS_ACCEL(channel) && has_accel) {
|
||||
/* Decode accel */
|
||||
struct sensor_three_axis_data *data =
|
||||
(struct sensor_three_axis_data *)data_out;
|
||||
uint64_t period_ns = accel_period_ns[edata->accel_odr];
|
||||
|
||||
icm42688_get_shift(SENSOR_CHAN_ACCEL_XYZ, edata->header.accel_fs,
|
||||
edata->header.gyro_fs, &data->shift);
|
||||
|
||||
data->readings[count].timestamp_delta = (accel_frame_count - 1) * period_ns;
|
||||
rc = icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 0,
|
||||
&data->readings[count].x);
|
||||
rc |= icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 1,
|
||||
&data->readings[count].y);
|
||||
rc |= icm42688_read_imu_from_packet(buffer, true, edata->header.accel_fs, 2,
|
||||
&data->readings[count].z);
|
||||
if (rc != 0) {
|
||||
accel_frame_count--;
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
} else if (IS_GYRO(channel) && has_gyro) {
|
||||
/* Decode gyro */
|
||||
struct sensor_three_axis_data *data =
|
||||
(struct sensor_three_axis_data *)data_out;
|
||||
uint64_t period_ns = accel_period_ns[edata->gyro_odr];
|
||||
|
||||
icm42688_get_shift(SENSOR_CHAN_GYRO_XYZ, edata->header.accel_fs,
|
||||
edata->header.gyro_fs, &data->shift);
|
||||
|
||||
data->readings[count].timestamp_delta = (gyro_frame_count - 1) * period_ns;
|
||||
rc = icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 0,
|
||||
&data->readings[count].x);
|
||||
rc |= icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 1,
|
||||
&data->readings[count].y);
|
||||
rc |= icm42688_read_imu_from_packet(buffer, false, edata->header.gyro_fs, 2,
|
||||
&data->readings[count].z);
|
||||
if (rc != 0) {
|
||||
gyro_frame_count--;
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
buffer = frame_end;
|
||||
*fit = (uintptr_t)frame_end;
|
||||
count++;
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
static int icm42688_one_shot_decode(const uint8_t *buffer, enum sensor_channel channel,
|
||||
size_t channel_idx, uint32_t *fit,
|
||||
uint16_t max_count, void *data_out)
|
||||
size_t channel_idx, uint32_t *fit, uint16_t max_count,
|
||||
void *data_out)
|
||||
{
|
||||
const struct icm42688_encoded_data *edata = (const struct icm42688_encoded_data *)buffer;
|
||||
const struct icm42688_decoder_header *header = &edata->header;
|
||||
|
|
@ -318,34 +571,76 @@ static int icm42688_one_shot_decode(const uint8_t *buffer, enum sensor_channel c
|
|||
}
|
||||
|
||||
static int icm42688_decoder_decode(const uint8_t *buffer, enum sensor_channel channel,
|
||||
size_t channel_idx, uint32_t *fit,
|
||||
uint16_t max_count, void *data_out)
|
||||
size_t channel_idx, uint32_t *fit, uint16_t max_count,
|
||||
void *data_out)
|
||||
{
|
||||
const struct icm42688_decoder_header *header =
|
||||
(const struct icm42688_decoder_header *)buffer;
|
||||
|
||||
if (header->is_fifo) {
|
||||
return icm42688_fifo_decode(buffer, channel, channel_idx, fit, max_count, data_out);
|
||||
}
|
||||
return icm42688_one_shot_decode(buffer, channel, channel_idx, fit, max_count, data_out);
|
||||
}
|
||||
|
||||
static int icm42688_decoder_get_frame_count(const uint8_t *buffer, enum sensor_channel channel,
|
||||
size_t channel_idx, uint16_t *frame_count)
|
||||
{
|
||||
ARG_UNUSED(buffer);
|
||||
const struct icm42688_fifo_data *data = (const struct icm42688_fifo_data *)buffer;
|
||||
const struct icm42688_decoder_header *header = &data->header;
|
||||
|
||||
if (channel_idx != 0) {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
switch (channel) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
case SENSOR_CHAN_GYRO_X:
|
||||
case SENSOR_CHAN_GYRO_Y:
|
||||
case SENSOR_CHAN_GYRO_Z:
|
||||
case SENSOR_CHAN_GYRO_XYZ:
|
||||
case SENSOR_CHAN_DIE_TEMP:
|
||||
*frame_count = 1;
|
||||
|
||||
if (!header->is_fifo) {
|
||||
switch (channel) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
case SENSOR_CHAN_GYRO_X:
|
||||
case SENSOR_CHAN_GYRO_Y:
|
||||
case SENSOR_CHAN_GYRO_Z:
|
||||
case SENSOR_CHAN_GYRO_XYZ:
|
||||
case SENSOR_CHAN_DIE_TEMP:
|
||||
*frame_count = 1;
|
||||
return 0;
|
||||
default:
|
||||
return -ENOTSUP;
|
||||
}
|
||||
return 0;
|
||||
default:
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
/* Skip the header */
|
||||
buffer += sizeof(struct icm42688_fifo_data);
|
||||
|
||||
uint16_t count = 0;
|
||||
const uint8_t *end = buffer + data->fifo_count;
|
||||
|
||||
while (buffer < end) {
|
||||
bool is_20b = FIELD_GET(FIFO_HEADER_20, buffer[0]);
|
||||
int size = is_20b ? 3 : 2;
|
||||
|
||||
if (FIELD_GET(FIFO_HEADER_ACCEL, buffer[0])) {
|
||||
size += 6;
|
||||
}
|
||||
if (FIELD_GET(FIFO_HEADER_GYRO, buffer[0])) {
|
||||
size += 6;
|
||||
}
|
||||
if (FIELD_GET(FIFO_HEADER_TIMESTAMP_FSYNC, buffer[0])) {
|
||||
size += 2;
|
||||
}
|
||||
if (is_20b) {
|
||||
size += 3;
|
||||
}
|
||||
|
||||
buffer += size;
|
||||
++count;
|
||||
}
|
||||
|
||||
*frame_count = count;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int icm42688_decoder_get_size_info(enum sensor_channel channel, size_t *base_size,
|
||||
|
|
@ -372,10 +667,31 @@ static int icm42688_decoder_get_size_info(enum sensor_channel channel, size_t *b
|
|||
}
|
||||
}
|
||||
|
||||
static bool icm24688_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
|
||||
{
|
||||
const struct icm42688_fifo_data *edata = (const struct icm42688_fifo_data *)buffer;
|
||||
|
||||
if (!edata->header.is_fifo) {
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (trigger) {
|
||||
case SENSOR_TRIG_DATA_READY:
|
||||
return FIELD_GET(BIT_INT_STATUS_DATA_RDY, edata->int_status);
|
||||
case SENSOR_TRIG_FIFO_WATERMARK:
|
||||
return FIELD_GET(BIT_INT_STATUS_FIFO_THS, edata->int_status);
|
||||
case SENSOR_TRIG_FIFO_FULL:
|
||||
return FIELD_GET(BIT_INT_STATUS_FIFO_FULL, edata->int_status);
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
SENSOR_DECODER_API_DT_DEFINE() = {
|
||||
.get_frame_count = icm42688_decoder_get_frame_count,
|
||||
.get_size_info = icm42688_decoder_get_size_info,
|
||||
.decode = icm42688_decoder_decode,
|
||||
.has_trigger = icm24688_decoder_has_trigger,
|
||||
};
|
||||
|
||||
int icm42688_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
|
||||
|
|
|
|||
|
|
@ -18,6 +18,15 @@ struct icm42688_decoder_header {
|
|||
uint8_t reserved: 2;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct icm42688_fifo_data {
|
||||
struct icm42688_decoder_header header;
|
||||
uint8_t int_status;
|
||||
uint16_t gyro_odr: 4;
|
||||
uint16_t accel_odr: 4;
|
||||
uint16_t fifo_count: 11;
|
||||
uint16_t reserved: 5;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct icm42688_encoded_data {
|
||||
struct icm42688_decoder_header header;
|
||||
struct {
|
||||
|
|
|
|||
|
|
@ -286,4 +286,12 @@
|
|||
#define MCLK_POLL_ATTEMPTS 100
|
||||
#define SOFT_RESET_TIME_MS 2 /* 1ms + elbow room */
|
||||
|
||||
/* FIFO header */
|
||||
#define FIFO_HEADER_ACCEL BIT(6)
|
||||
#define FIFO_HEADER_GYRO BIT(5)
|
||||
#define FIFO_HEADER_20 BIT(4)
|
||||
#define FIFO_HEADER_TIMESTAMP_FSYNC GENMASK(3, 2)
|
||||
#define FIFO_HEADER_ODR_ACCEL BIT(1)
|
||||
#define FIFO_HEADER_ODR_GYRO BIT(0)
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_REG_H_ */
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
#include "icm42688.h"
|
||||
#include "icm42688_decoder.h"
|
||||
#include "icm42688_reg.h"
|
||||
#include "icm42688_rtio.h"
|
||||
#include "icm42688_spi.h"
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
|
|
@ -42,7 +43,7 @@ static int icm42688_rtio_sample_fetch(const struct device *dev, int16_t readings
|
|||
return 0;
|
||||
}
|
||||
|
||||
int icm42688_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
static int icm42688_submit_one_shot(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
const enum sensor_channel *const channels = cfg->channels;
|
||||
|
|
@ -78,4 +79,17 @@ int icm42688_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
|||
return 0;
|
||||
}
|
||||
|
||||
int icm42688_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
|
||||
if (!cfg->is_streaming) {
|
||||
return icm42688_submit_one_shot(dev, iodev_sqe);
|
||||
} else if (IS_ENABLED(CONFIG_ICM42688_STREAM)) {
|
||||
return icm42688_submit_stream(dev, iodev_sqe);
|
||||
} else {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
}
|
||||
|
||||
BUILD_ASSERT(sizeof(struct icm42688_decoder_header) == 9);
|
||||
|
|
|
|||
|
|
@ -7,6 +7,13 @@
|
|||
#ifndef ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_
|
||||
#define ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_
|
||||
|
||||
#include <zephyr/device.h>
|
||||
#include <zephyr/rtio/rtio.h>
|
||||
|
||||
int icm42688_submit(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe);
|
||||
|
||||
int icm42688_submit_stream(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe);
|
||||
|
||||
void icm42688_fifo_event(const struct device *dev);
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_ */
|
||||
|
|
|
|||
319
drivers/sensor/icm42688/icm42688_rtio_stream.c
Normal file
319
drivers/sensor/icm42688/icm42688_rtio_stream.c
Normal file
|
|
@ -0,0 +1,319 @@
|
|||
/*
|
||||
* Copyright (c) 2023 Google LLC
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
|
||||
#include "icm42688.h"
|
||||
#include "icm42688_decoder.h"
|
||||
#include "icm42688_reg.h"
|
||||
#include "icm42688_rtio.h"
|
||||
|
||||
LOG_MODULE_DECLARE(ICM42688_RTIO);
|
||||
|
||||
int icm42688_submit_stream(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
struct icm42688_dev_data *data = sensor->data;
|
||||
struct icm42688_cfg new_config = data->cfg;
|
||||
|
||||
new_config.interrupt1_drdy = false;
|
||||
new_config.interrupt1_fifo_ths = false;
|
||||
new_config.interrupt1_fifo_full = false;
|
||||
for (int i = 0; i < cfg->count; ++i) {
|
||||
switch (cfg->triggers[i].trigger) {
|
||||
case SENSOR_TRIG_DATA_READY:
|
||||
new_config.interrupt1_drdy = true;
|
||||
break;
|
||||
case SENSOR_TRIG_FIFO_WATERMARK:
|
||||
new_config.interrupt1_fifo_ths = true;
|
||||
break;
|
||||
case SENSOR_TRIG_FIFO_FULL:
|
||||
new_config.interrupt1_fifo_full = true;
|
||||
break;
|
||||
default:
|
||||
LOG_DBG("Trigger (%d) not supported", cfg->triggers[i].trigger);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (new_config.interrupt1_drdy != data->cfg.interrupt1_drdy ||
|
||||
new_config.interrupt1_fifo_ths != data->cfg.interrupt1_fifo_ths ||
|
||||
new_config.interrupt1_fifo_full != data->cfg.interrupt1_fifo_full) {
|
||||
int rc = icm42688_safely_configure(sensor, &new_config);
|
||||
|
||||
if (rc != 0) {
|
||||
LOG_ERR("Failed to configure sensor");
|
||||
return rc;
|
||||
}
|
||||
}
|
||||
|
||||
data->streaming_sqe = iodev_sqe;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void icm42688_complete_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg)
|
||||
{
|
||||
const struct device *dev = arg;
|
||||
struct icm42688_dev_data *drv_data = dev->data;
|
||||
const struct icm42688_dev_cfg *drv_cfg = dev->config;
|
||||
struct rtio_iodev_sqe *iodev_sqe = sqe->userdata;
|
||||
|
||||
rtio_iodev_sqe_ok(iodev_sqe, drv_data->fifo_count);
|
||||
|
||||
gpio_pin_interrupt_configure_dt(&drv_cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
}
|
||||
|
||||
static void icm42688_fifo_count_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg)
|
||||
{
|
||||
const struct device *dev = arg;
|
||||
struct icm42688_dev_data *drv_data = dev->data;
|
||||
const struct icm42688_dev_cfg *drv_cfg = dev->config;
|
||||
struct rtio_iodev *spi_iodev = drv_data->spi_iodev;
|
||||
uint8_t *fifo_count_buf = (uint8_t *)&drv_data->fifo_count;
|
||||
uint16_t fifo_count = ((fifo_count_buf[0] << 8) | fifo_count_buf[1]);
|
||||
|
||||
drv_data->fifo_count = fifo_count;
|
||||
|
||||
/* Pull a operation from our device iodev queue, validated to only be reads */
|
||||
struct rtio_iodev_sqe *iodev_sqe = drv_data->streaming_sqe;
|
||||
|
||||
drv_data->streaming_sqe = NULL;
|
||||
|
||||
/* Not inherently an underrun/overrun as we may have a buffer to fill next time */
|
||||
if (iodev_sqe == NULL) {
|
||||
LOG_DBG("No pending SQE");
|
||||
gpio_pin_interrupt_configure_dt(&drv_cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
const size_t packet_size = drv_data->cfg.fifo_hires ? 20 : 16;
|
||||
const size_t min_read_size = sizeof(struct icm42688_fifo_data) + packet_size;
|
||||
const size_t ideal_read_size = sizeof(struct icm42688_fifo_data) + fifo_count;
|
||||
uint8_t *buf;
|
||||
uint32_t buf_len;
|
||||
|
||||
if (rtio_sqe_rx_buf(iodev_sqe, min_read_size, ideal_read_size, &buf, &buf_len) != 0) {
|
||||
LOG_ERR("Failed to get buffer");
|
||||
rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
|
||||
return;
|
||||
}
|
||||
LOG_DBG("Requesting buffer [%u, %u] got %u", (unsigned int)min_read_size,
|
||||
(unsigned int)ideal_read_size, buf_len);
|
||||
|
||||
/* Read FIFO and call back to rtio with rtio_sqe completion */
|
||||
/* TODO is packet format even needed? the fifo has a header per packet
|
||||
* already
|
||||
*/
|
||||
struct icm42688_fifo_data hdr = {
|
||||
.header = {
|
||||
.is_fifo = true,
|
||||
.gyro_fs = drv_data->cfg.gyro_fs,
|
||||
.accel_fs = drv_data->cfg.accel_fs,
|
||||
.timestamp = drv_data->timestamp,
|
||||
},
|
||||
.int_status = drv_data->int_status,
|
||||
.gyro_odr = drv_data->cfg.gyro_odr,
|
||||
.accel_odr = drv_data->cfg.accel_odr,
|
||||
};
|
||||
uint32_t buf_avail = buf_len;
|
||||
|
||||
memcpy(buf, &hdr, sizeof(hdr));
|
||||
buf_avail -= sizeof(hdr);
|
||||
|
||||
uint32_t read_len = MIN(fifo_count, buf_avail);
|
||||
uint32_t pkts = read_len / packet_size;
|
||||
|
||||
read_len = pkts * packet_size;
|
||||
((struct icm42688_fifo_data *)buf)->fifo_count = read_len;
|
||||
|
||||
__ASSERT_NO_MSG(read_len % pkt_size == 0);
|
||||
|
||||
uint8_t *read_buf = buf + sizeof(hdr);
|
||||
|
||||
/* Flush out completions */
|
||||
struct rtio_cqe *cqe;
|
||||
|
||||
do {
|
||||
cqe = rtio_cqe_consume(r);
|
||||
if (cqe != NULL) {
|
||||
rtio_cqe_release(r, cqe);
|
||||
}
|
||||
} while (cqe != NULL);
|
||||
|
||||
/* Setup new rtio chain to read the fifo data and report then check the
|
||||
* result
|
||||
*/
|
||||
struct rtio_sqe *write_fifo_addr = rtio_sqe_acquire(r);
|
||||
struct rtio_sqe *read_fifo_data = rtio_sqe_acquire(r);
|
||||
struct rtio_sqe *complete_op = rtio_sqe_acquire(r);
|
||||
const uint8_t reg_addr = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_FIFO_DATA);
|
||||
|
||||
rtio_sqe_prep_tiny_write(write_fifo_addr, spi_iodev, RTIO_PRIO_NORM, ®_addr, 1, NULL);
|
||||
write_fifo_addr->flags = RTIO_SQE_TRANSACTION;
|
||||
rtio_sqe_prep_read(read_fifo_data, spi_iodev, RTIO_PRIO_NORM, read_buf, read_len,
|
||||
iodev_sqe);
|
||||
|
||||
rtio_sqe_prep_callback(complete_op, icm42688_complete_cb, (void *)dev, iodev_sqe);
|
||||
|
||||
rtio_submit(r, 0);
|
||||
}
|
||||
|
||||
static struct sensor_stream_trigger *
|
||||
icm42688_get_read_config_trigger(const struct sensor_read_config *cfg,
|
||||
enum sensor_trigger_type trig)
|
||||
{
|
||||
for (int i = 0; i < cfg->count; ++i) {
|
||||
if (cfg->triggers[i].trigger == trig) {
|
||||
return &cfg->triggers[i];
|
||||
}
|
||||
}
|
||||
LOG_DBG("Unsupported trigger (%d)", trig);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static void icm42688_int_status_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg)
|
||||
{
|
||||
const struct device *dev = arg;
|
||||
struct icm42688_dev_data *drv_data = dev->data;
|
||||
const struct icm42688_dev_cfg *drv_cfg = dev->config;
|
||||
struct rtio_iodev *spi_iodev = drv_data->spi_iodev;
|
||||
struct rtio_iodev_sqe *streaming_sqe = drv_data->streaming_sqe;
|
||||
struct sensor_read_config *read_config;
|
||||
|
||||
if (streaming_sqe == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
read_config = (struct sensor_read_config *)streaming_sqe->sqe.iodev->data;
|
||||
__ASSERT_NO_MSG(read_config != NULL);
|
||||
|
||||
if (!read_config->is_streaming) {
|
||||
/* Oops, not really configured for streaming data */
|
||||
return;
|
||||
}
|
||||
|
||||
struct sensor_stream_trigger *fifo_ths_cfg =
|
||||
icm42688_get_read_config_trigger(read_config, SENSOR_TRIG_FIFO_WATERMARK);
|
||||
bool has_fifo_ths_trig = fifo_ths_cfg != NULL &&
|
||||
FIELD_GET(BIT_INT_STATUS_FIFO_THS, drv_data->int_status) != 0;
|
||||
|
||||
struct sensor_stream_trigger *fifo_full_cfg =
|
||||
icm42688_get_read_config_trigger(read_config, SENSOR_TRIG_FIFO_FULL);
|
||||
bool has_fifo_full_trig = fifo_full_cfg != NULL &&
|
||||
FIELD_GET(BIT_INT_STATUS_FIFO_FULL, drv_data->int_status) != 0;
|
||||
|
||||
if (!has_fifo_ths_trig && !has_fifo_full_trig) {
|
||||
gpio_pin_interrupt_configure_dt(&drv_cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Flush completions */
|
||||
struct rtio_cqe *cqe;
|
||||
|
||||
do {
|
||||
cqe = rtio_cqe_consume(r);
|
||||
if (cqe != NULL) {
|
||||
rtio_cqe_release(r, cqe);
|
||||
}
|
||||
} while (cqe != NULL);
|
||||
|
||||
enum sensor_stream_data_opt data_opt;
|
||||
|
||||
if (has_fifo_ths_trig && !has_fifo_full_trig) {
|
||||
/* Only care about fifo threshold */
|
||||
data_opt = fifo_ths_cfg->opt;
|
||||
} else if (!has_fifo_ths_trig && has_fifo_full_trig) {
|
||||
/* Only care about fifo full */
|
||||
data_opt = fifo_full_cfg->opt;
|
||||
} else {
|
||||
/* Both fifo threshold and full */
|
||||
data_opt = MIN(fifo_ths_cfg->opt, fifo_full_cfg->opt);
|
||||
}
|
||||
|
||||
if (data_opt == SENSOR_STREAM_DATA_NOP || data_opt == SENSOR_STREAM_DATA_DROP) {
|
||||
uint8_t *buf;
|
||||
uint32_t buf_len;
|
||||
|
||||
/* Clear streaming_sqe since we're done with the call */
|
||||
drv_data->streaming_sqe = NULL;
|
||||
if (rtio_sqe_rx_buf(streaming_sqe, sizeof(struct icm42688_fifo_data),
|
||||
sizeof(struct icm42688_fifo_data), &buf, &buf_len) != 0) {
|
||||
rtio_iodev_sqe_err(streaming_sqe, -ENOMEM);
|
||||
return;
|
||||
}
|
||||
|
||||
struct icm42688_fifo_data *data = (struct icm42688_fifo_data *)buf;
|
||||
|
||||
memset(buf, 0, buf_len);
|
||||
data->header.timestamp = drv_data->timestamp;
|
||||
data->int_status = drv_data->int_status;
|
||||
data->fifo_count = 0;
|
||||
rtio_iodev_sqe_ok(streaming_sqe, 0);
|
||||
gpio_pin_interrupt_configure_dt(&drv_cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
if (data_opt == SENSOR_STREAM_DATA_DROP) {
|
||||
/* Flush the FIFO */
|
||||
struct rtio_sqe *write_signal_path_reset = rtio_sqe_acquire(r);
|
||||
uint8_t write_buffer[] = {
|
||||
FIELD_GET(REG_ADDRESS_MASK, REG_SIGNAL_PATH_RESET),
|
||||
BIT_FIFO_FLUSH,
|
||||
};
|
||||
|
||||
rtio_sqe_prep_tiny_write(write_signal_path_reset, spi_iodev, RTIO_PRIO_NORM,
|
||||
write_buffer, ARRAY_SIZE(write_buffer), NULL);
|
||||
/* TODO Add a new flag for fire-and-forget so we don't have to block here */
|
||||
rtio_submit(r, 1);
|
||||
ARG_UNUSED(rtio_cqe_consume(r));
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/* We need the data, read the fifo length */
|
||||
struct rtio_sqe *write_fifo_count_reg = rtio_sqe_acquire(r);
|
||||
struct rtio_sqe *read_fifo_count = rtio_sqe_acquire(r);
|
||||
struct rtio_sqe *check_fifo_count = rtio_sqe_acquire(r);
|
||||
uint8_t reg = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_FIFO_COUNTH);
|
||||
uint8_t *read_buf = (uint8_t *)&drv_data->fifo_count;
|
||||
|
||||
rtio_sqe_prep_tiny_write(write_fifo_count_reg, spi_iodev, RTIO_PRIO_NORM, ®, 1, NULL);
|
||||
write_fifo_count_reg->flags = RTIO_SQE_TRANSACTION;
|
||||
rtio_sqe_prep_read(read_fifo_count, spi_iodev, RTIO_PRIO_NORM, read_buf, 2, NULL);
|
||||
rtio_sqe_prep_callback(check_fifo_count, icm42688_fifo_count_cb, arg, NULL);
|
||||
|
||||
rtio_submit(r, 0);
|
||||
}
|
||||
|
||||
void icm42688_fifo_event(const struct device *dev)
|
||||
{
|
||||
struct icm42688_dev_data *drv_data = dev->data;
|
||||
struct rtio_iodev *spi_iodev = drv_data->spi_iodev;
|
||||
struct rtio *r = drv_data->r;
|
||||
|
||||
if (drv_data->streaming_sqe == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
drv_data->timestamp = k_ticks_to_ns_floor64(k_uptime_ticks());
|
||||
|
||||
/*
|
||||
* Setup rtio chain of ops with inline calls to make decisions
|
||||
* 1. read int status
|
||||
* 2. call to check int status and get pending RX operation
|
||||
* 4. read fifo len
|
||||
* 5. call to determine read len
|
||||
* 6. read fifo
|
||||
* 7. call to report completion
|
||||
*/
|
||||
struct rtio_sqe *write_int_reg = rtio_sqe_acquire(r);
|
||||
struct rtio_sqe *read_int_reg = rtio_sqe_acquire(r);
|
||||
struct rtio_sqe *check_int_status = rtio_sqe_acquire(r);
|
||||
uint8_t reg = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_INT_STATUS);
|
||||
|
||||
rtio_sqe_prep_tiny_write(write_int_reg, spi_iodev, RTIO_PRIO_NORM, ®, 1, NULL);
|
||||
write_int_reg->flags = RTIO_SQE_TRANSACTION;
|
||||
rtio_sqe_prep_read(read_int_reg, spi_iodev, RTIO_PRIO_NORM, &drv_data->int_status, 1, NULL);
|
||||
rtio_sqe_prep_callback(check_int_status, icm42688_int_status_cb, (void *)dev, NULL);
|
||||
rtio_submit(r, 0);
|
||||
}
|
||||
|
|
@ -12,6 +12,7 @@
|
|||
|
||||
#include "icm42688.h"
|
||||
#include "icm42688_reg.h"
|
||||
#include "icm42688_rtio.h"
|
||||
#include "icm42688_spi.h"
|
||||
#include "icm42688_trigger.h"
|
||||
|
||||
|
|
@ -30,6 +31,9 @@ static void icm42688_gpio_callback(const struct device *dev, struct gpio_callbac
|
|||
#elif defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD)
|
||||
k_work_submit(&data->work);
|
||||
#endif
|
||||
if (IS_ENABLED(CONFIG_ICM42688_STREAM)) {
|
||||
icm42688_fifo_event(data->dev);
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD) || defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD)
|
||||
|
|
@ -90,6 +94,8 @@ int icm42688_trigger_set(const struct device *dev, const struct sensor_trigger *
|
|||
|
||||
switch (trig->type) {
|
||||
case SENSOR_TRIG_DATA_READY:
|
||||
case SENSOR_TRIG_FIFO_WATERMARK:
|
||||
case SENSOR_TRIG_FIFO_FULL:
|
||||
data->data_ready_handler = handler;
|
||||
data->data_ready_trigger = trig;
|
||||
|
||||
|
|
@ -146,7 +152,7 @@ int icm42688_trigger_init(const struct device *dev)
|
|||
return gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
}
|
||||
|
||||
int icm42688_trigger_enable_interrupt(const struct device *dev)
|
||||
int icm42688_trigger_enable_interrupt(const struct device *dev, struct icm42688_cfg *new_cfg)
|
||||
{
|
||||
int res;
|
||||
const struct icm42688_dev_cfg *cfg = dev->config;
|
||||
|
|
@ -164,9 +170,19 @@ int icm42688_trigger_enable_interrupt(const struct device *dev)
|
|||
return res;
|
||||
}
|
||||
|
||||
/* enable data ready interrupt on INT1 pin */
|
||||
return icm42688_spi_single_write(&cfg->spi, REG_INT_SOURCE0,
|
||||
FIELD_PREP(BIT_UI_DRDY_INT1_EN, 1));
|
||||
/* enable interrupts on INT1 pin */
|
||||
uint8_t value = 0;
|
||||
|
||||
if (new_cfg->interrupt1_drdy) {
|
||||
value |= FIELD_PREP(BIT_UI_DRDY_INT1_EN, 1);
|
||||
}
|
||||
if (new_cfg->interrupt1_fifo_ths) {
|
||||
value |= FIELD_PREP(BIT_FIFO_THS_INT1_EN, 1);
|
||||
}
|
||||
if (new_cfg->interrupt1_fifo_full) {
|
||||
value |= FIELD_PREP(BIT_FIFO_FULL_INT1_EN, 1);
|
||||
}
|
||||
return icm42688_spi_single_write(&cfg->spi, REG_INT_SOURCE0, value);
|
||||
}
|
||||
|
||||
void icm42688_lock(const struct device *dev)
|
||||
|
|
|
|||
|
|
@ -26,9 +26,10 @@ int icm42688_trigger_init(const struct device *dev);
|
|||
* @brief enable the trigger gpio interrupt
|
||||
*
|
||||
* @param dev icm42688 device pointer
|
||||
* @param new_cfg New configuration to use for the device
|
||||
* @return int 0 on success, negative error code otherwise
|
||||
*/
|
||||
int icm42688_trigger_enable_interrupt(const struct device *dev);
|
||||
int icm42688_trigger_enable_interrupt(const struct device *dev, struct icm42688_cfg *new_cfg);
|
||||
|
||||
/**
|
||||
* @brief lock access to the icm42688 device driver
|
||||
|
|
|
|||
Loading…
Reference in a new issue