diff --git a/drivers/sensor/tdk/icm42688/icm42688.c b/drivers/sensor/tdk/icm42688/icm42688.c index a46f8ce2eadd..1a815156ede5 100644 --- a/drivers/sensor/tdk/icm42688/icm42688.c +++ b/drivers/sensor/tdk/icm42688/icm42688.c @@ -9,6 +9,7 @@ #define DT_DRV_COMPAT invensense_icm42688 #include +#include #include #include @@ -162,6 +163,29 @@ static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan, return -EINVAL; } new_config.batch_ticks = val->val1; + } else if ((enum sensor_attribute_icm42688)attr == + SENSOR_ATTR_ICM42688_PIN9_FUNCTION) { + if (val->val1 != ICM42688_PIN9_FUNCTION_INT2 && + val->val1 != ICM42688_PIN9_FUNCTION_FSYNC && + val->val1 != ICM42688_PIN9_FUNCTION_CLKIN) { + LOG_ERR("Unknown pin function"); + return -EINVAL; + } + + if (val->val2 < 31000 || val->val2 > 50000) { + LOG_ERR("RTC frequency must be between 31kHz and 50kHz"); + return -EINVAL; + } + + /* TODO: Allow this if FSYNC is configurable later. */ + if (val->val1 == ICM42688_PIN9_FUNCTION_FSYNC) { + LOG_ERR("FSYNC is disabled, PIN9_FUNCTION should not be set to " + "FSYNC"); + return -ENOTSUP; + } + + new_config.pin9_function = val->val1; + new_config.rtc_freq = val->val2; } else { LOG_ERR("Unsupported attribute"); res = -EINVAL; @@ -219,6 +243,10 @@ static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan, if (attr == SENSOR_ATTR_BATCH_DURATION) { val->val1 = cfg->batch_ticks; val->val2 = 0; + } else if ((enum sensor_attribute_icm42688)attr == + SENSOR_ATTR_ICM42688_PIN9_FUNCTION) { + val->val1 = cfg->pin9_function; + val->val2 = cfg->rtc_freq; } else { LOG_ERR("Unsupported attribute"); res = -EINVAL; @@ -313,7 +341,9 @@ void icm42688_unlock(const struct device *dev) .fifo_hires = false, \ .interrupt1_drdy = false, \ .interrupt1_fifo_ths = false, \ - .interrupt1_fifo_full = false \ + .interrupt1_fifo_full = false, \ + .pin9_function = ICM42688_PIN9_FUNCTION_INT2, \ + .rtc_freq = 0 \ } #define ICM42688_DEFINE_DATA(inst) \ diff --git a/drivers/sensor/tdk/icm42688/icm42688.h b/drivers/sensor/tdk/icm42688/icm42688.h index 65c534495d29..a24069a2cd85 100644 --- a/drivers/sensor/tdk/icm42688/icm42688.h +++ b/drivers/sensor/tdk/icm42688/icm42688.h @@ -308,6 +308,9 @@ struct icm42688_cfg { bool interrupt1_drdy; bool interrupt1_fifo_ths; bool interrupt1_fifo_full; + + uint8_t pin9_function; + uint16_t rtc_freq; }; struct icm42688_trigger_entry { diff --git a/drivers/sensor/tdk/icm42688/icm42688_common.c b/drivers/sensor/tdk/icm42688/icm42688_common.c index b674b07009a8..e04e638ffc2f 100644 --- a/drivers/sensor/tdk/icm42688/icm42688_common.c +++ b/drivers/sensor/tdk/icm42688/icm42688_common.c @@ -7,6 +7,7 @@ */ #include +#include #include #include #include "icm42688.h" @@ -148,6 +149,47 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) /* TODO maybe do the next few steps intelligently by checking current config */ + /* Select register bank 1 */ + res = icm42688_spi_single_write(&dev_cfg->spi, REG_BANK_SEL, BIT_BANK1); + if (res != 0) { + LOG_ERR("Error selecting register bank 1"); + return -EINVAL; + } + + k_busy_wait(50); + + /* Set pin 9 function */ + uint8_t intf_config5 = FIELD_PREP(MASK_PIN9_FUNCTION, cfg->pin9_function); + + LOG_DBG("INTF_CONFIG5 (0x%lx) 0x%x", FIELD_GET(REG_ADDRESS_MASK, REG_INTF_CONFIG5), + intf_config5); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_INTF_CONFIG5, intf_config5); + if (res != 0) { + LOG_ERR("Error writing INTF_CONFIG5"); + return -EINVAL; + } + + /* Select register bank 0 */ + res = icm42688_spi_single_write(&dev_cfg->spi, REG_BANK_SEL, BIT_BANK0); + if (res != 0) { + LOG_ERR("Error selecting register bank 0"); + return -EINVAL; + } + + k_busy_wait(50); + + bool is_pin9_clkin = cfg->pin9_function == ICM42688_PIN9_FUNCTION_CLKIN; + uint8_t intf_config1 = 0x91 | FIELD_PREP(BIT_RTC_MODE, is_pin9_clkin); + + LOG_DBG("INTF_CONFIG1 (0x%x) 0x%x", REG_INTF_CONFIG1, intf_config1); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_INTF_CONFIG1, intf_config1); + if (res != 0) { + LOG_ERR("Error writing INTF_CONFIG1"); + return -EINVAL; + } + + k_busy_wait(250); + /* Power management to set gyro/accel modes */ uint8_t pwr_mgmt0 = FIELD_PREP(MASK_GYRO_MODE, cfg->gyro_pwr_mode) | FIELD_PREP(MASK_ACCEL_MODE, cfg->accel_pwr_mode) | diff --git a/drivers/sensor/tdk/icm42688/icm42688_decoder.c b/drivers/sensor/tdk/icm42688/icm42688_decoder.c index 4a96fcd2ca27..afd5485ee201 100644 --- a/drivers/sensor/tdk/icm42688/icm42688_decoder.c +++ b/drivers/sensor/tdk/icm42688/icm42688_decoder.c @@ -290,7 +290,7 @@ static uint32_t accel_period_ns[] = { [ICM42688_DT_ACCEL_ODR_1_5625] = UINT32_C(10000000000000) / 15625, [ICM42688_DT_ACCEL_ODR_3_125] = UINT32_C(10000000000000) / 31250, [ICM42688_DT_ACCEL_ODR_6_25] = UINT32_C(10000000000000) / 62500, - [ICM42688_DT_ACCEL_ODR_12_5] = UINT32_C(10000000000000) / 12500, + [ICM42688_DT_ACCEL_ODR_12_5] = UINT32_C(10000000000000) / 125000, [ICM42688_DT_ACCEL_ODR_25] = UINT32_C(1000000000) / 25, [ICM42688_DT_ACCEL_ODR_50] = UINT32_C(1000000000) / 50, [ICM42688_DT_ACCEL_ODR_100] = UINT32_C(1000000000) / 100, @@ -305,7 +305,7 @@ static uint32_t accel_period_ns[] = { }; static uint32_t gyro_period_ns[] = { - [ICM42688_DT_GYRO_ODR_12_5] = UINT32_C(10000000000000) / 12500, + [ICM42688_DT_GYRO_ODR_12_5] = UINT32_C(10000000000000) / 125000, [ICM42688_DT_GYRO_ODR_25] = UINT32_C(1000000000) / 25, [ICM42688_DT_GYRO_ODR_50] = UINT32_C(1000000000) / 50, [ICM42688_DT_GYRO_ODR_100] = UINT32_C(1000000000) / 100, @@ -319,6 +319,28 @@ static uint32_t gyro_period_ns[] = { [ICM42688_DT_GYRO_ODR_32000] = UINT32_C(1000000) / 32, }; +static int icm42688_calc_timestamp_delta(int rtc_freq, int chan_type, int dt_odr, int frame_count, + uint64_t *out_delta) +{ + uint32_t period; + + if (IS_ACCEL(chan_type)) { + period = accel_period_ns[dt_odr]; + } else if (IS_GYRO(chan_type)) { + period = gyro_period_ns[dt_odr]; + } else { + return -EINVAL; + } + + /* + * When ODR is set to r and an external clock with frequency f is used, + * the actual ODR = f * r / 32000. + */ + *out_delta = (uint64_t)period * frame_count * 32000 / rtc_freq; + + return 0; +} + static int icm42688_fifo_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec, uint32_t *fit, uint16_t max_count, void *data_out) { @@ -363,27 +385,66 @@ static int icm42688_fifo_decode(const uint8_t *buffer, struct sensor_chan_spec c } if (chan_spec.chan_type == SENSOR_CHAN_DIE_TEMP) { struct sensor_q31_data *data = (struct sensor_q31_data *)data_out; + uint64_t ts_delta; - data->shift = 9; if (has_accel) { - data->readings[count].timestamp_delta = - accel_period_ns[edata->accel_odr] * (accel_frame_count - 1); + rc = icm42688_calc_timestamp_delta( + edata->rtc_freq, SENSOR_CHAN_ACCEL_XYZ, edata->accel_odr, + accel_frame_count - 1, &ts_delta); } else { - data->readings[count].timestamp_delta = - gyro_period_ns[edata->gyro_odr] * (gyro_frame_count - 1); + rc = icm42688_calc_timestamp_delta( + edata->rtc_freq, SENSOR_CHAN_GYRO_XYZ, edata->gyro_odr, + gyro_frame_count - 1, &ts_delta); + } + if (rc < 0) { + buffer = frame_end; + continue; + } + + /* + * TODO: For some extreme combination of ODR and FIFO count, using uint32_t + * to store timestamp delta will overflow. Better error reporting? + */ + if (ts_delta > UINT32_MAX) { + LOG_ERR("Timestamp delta overflow"); + buffer = frame_end; + continue; } + + data->readings[count].timestamp_delta = ts_delta; + + data->shift = 9; data->readings[count].temperature = icm42688_read_temperature_from_packet(buffer); } else if (IS_ACCEL(chan_spec.chan_type) && 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]; + uint64_t ts_delta; 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_calc_timestamp_delta(edata->rtc_freq, SENSOR_CHAN_ACCEL_XYZ, + edata->accel_odr, accel_frame_count - 1, + &ts_delta); + if (rc < 0) { + buffer = frame_end; + continue; + } + + /* + * TODO: For some extreme combination of ODR and FIFO count, using uint32_t + * to store timestamp delta will overflow. Better error reporting? + */ + if (ts_delta > UINT32_MAX) { + LOG_ERR("Timestamp delta overflow"); + buffer = frame_end; + continue; + } + + data->readings[count].timestamp_delta = ts_delta; + 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, @@ -399,12 +460,31 @@ static int icm42688_fifo_decode(const uint8_t *buffer, struct sensor_chan_spec c /* Decode gyro */ struct sensor_three_axis_data *data = (struct sensor_three_axis_data *)data_out; - uint64_t period_ns = gyro_period_ns[edata->gyro_odr]; + uint64_t ts_delta; 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_calc_timestamp_delta(edata->rtc_freq, SENSOR_CHAN_GYRO_XYZ, + edata->gyro_odr, gyro_frame_count - 1, + &ts_delta); + if (rc < 0) { + buffer = frame_end; + continue; + } + + /* + * TODO: For some extreme combination of ODR and FIFO count, using uint32_t + * to store timestamp delta will overflow. Better error reporting? + */ + if (ts_delta > UINT32_MAX) { + LOG_ERR("Timestamp delta overflow"); + buffer = frame_end; + continue; + } + + data->readings[count].timestamp_delta = ts_delta; + 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, diff --git a/drivers/sensor/tdk/icm42688/icm42688_decoder.h b/drivers/sensor/tdk/icm42688/icm42688_decoder.h index 9eadcbcecad5..92b0c9a33bb5 100644 --- a/drivers/sensor/tdk/icm42688/icm42688_decoder.h +++ b/drivers/sensor/tdk/icm42688/icm42688_decoder.h @@ -21,10 +21,11 @@ struct icm42688_decoder_header { struct icm42688_fifo_data { struct icm42688_decoder_header header; uint8_t int_status; - uint16_t gyro_odr: 4; - uint16_t accel_odr: 4; + uint8_t gyro_odr: 4; + uint8_t accel_odr: 4; uint16_t fifo_count: 11; - uint16_t reserved: 5; + uint16_t padding1: 5; + uint16_t rtc_freq; } __attribute__((__packed__)); struct icm42688_encoded_data { diff --git a/drivers/sensor/tdk/icm42688/icm42688_reg.h b/drivers/sensor/tdk/icm42688/icm42688_reg.h index 486a745ad53c..04f249bd9b2a 100644 --- a/drivers/sensor/tdk/icm42688/icm42688_reg.h +++ b/drivers/sensor/tdk/icm42688/icm42688_reg.h @@ -276,6 +276,9 @@ #define BIT_INT_TDEASSERT_DISABLE BIT(5) #define BIT_INT_ASYNC_RESET BIT(4) +/* Bank1 REG_INTF_CONFIG5 */ +#define MASK_PIN9_FUNCTION GENMASK(2, 1) + /* misc. defines */ #define WHO_AM_I_ICM42688 0x47 #define MIN_ACCEL_SENS_SHIFT 11 diff --git a/drivers/sensor/tdk/icm42688/icm42688_rtio_stream.c b/drivers/sensor/tdk/icm42688/icm42688_rtio_stream.c index 0657b855ecec..b7be214f5d44 100644 --- a/drivers/sensor/tdk/icm42688/icm42688_rtio_stream.c +++ b/drivers/sensor/tdk/icm42688/icm42688_rtio_stream.c @@ -118,6 +118,7 @@ static void icm42688_fifo_count_cb(struct rtio *r, const struct rtio_sqe *sqe, v .int_status = drv_data->int_status, .gyro_odr = drv_data->cfg.gyro_odr, .accel_odr = drv_data->cfg.accel_odr, + .rtc_freq = drv_data->cfg.rtc_freq, }; uint32_t buf_avail = buf_len; diff --git a/include/zephyr/drivers/sensor/icm42688.h b/include/zephyr/drivers/sensor/icm42688.h new file mode 100644 index 000000000000..da9c00528fe4 --- /dev/null +++ b/include/zephyr/drivers/sensor/icm42688.h @@ -0,0 +1,32 @@ +/* + * Copyright The Zephyr Project Contributors + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_INCLUDE_DRIVERS_SENSOR_ICM42688_H_ +#define ZEPHYR_INCLUDE_DRIVERS_SENSOR_ICM42688_H_ + +#include + +/** + * @file + * @brief Extended public API for ICM42688 + * + * Pin function configuration via attributes under the current sensor driver abstraction. + */ + +#define ICM42688_PIN9_FUNCTION_INT2 0 +#define ICM42688_PIN9_FUNCTION_FSYNC 1 +#define ICM42688_PIN9_FUNCTION_CLKIN 2 + +/** + * @brief Extended sensor attributes for ICM42688 + * + * Attributes for setting pin function. + */ +enum sensor_attribute_icm42688 { + SENSOR_ATTR_ICM42688_PIN9_FUNCTION = SENSOR_ATTR_PRIV_START +}; + +#endif /* ZEPHYR_INCLUDE_DRIVERS_SENSOR_ICM42688_H_ */