michaelh/zephyr
1
0
Fork 0
Code Releases Activity

sensor: icm42688: Fix broken build and streaming.

Browse source 
Makes driver actually build again.
Fixes scaling, q32 encoding, bit shifts, and register mappings.
Also enables High Resolution mode (a unique and beneficial
feature of the chip when used with streaming).

Co-authored-by: James Goppert <james.goppert@gmail.com>
Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>
This commit is contained in:
Benjamin Perseghetti 2025-01-28 13:25:40 -05:00 • committed by Benjamin Cabé
commit e399c168a1
4 changed files with 27 additions and 72 deletions
Download patch file Download diff file Expand all files Collapse all files

7
drivers/sensor/tdk/icm42688/icm42688_common.c
View file

@ -255,8 +255,11 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg)
* temp/accel/gyro en fields in cfg
*/
uint8_t fifo_cfg1 =
FIELD_PREP(BIT_FIFO_TEMP_EN, 1) | FIELD_PREP(BIT_FIFO_GYRO_EN, 1) |
FIELD_PREP(BIT_FIFO_ACCEL_EN, 1) | FIELD_PREP(BIT_FIFO_TMST_FSYNC_EN, 1);
FIELD_PREP(BIT_FIFO_TEMP_EN, 1) |
FIELD_PREP(BIT_FIFO_GYRO_EN, 1) |
FIELD_PREP(BIT_FIFO_ACCEL_EN, 1) |
FIELD_PREP(BIT_FIFO_TMST_FSYNC_EN, 1) |
FIELD_PREP(BIT_FIFO_HIRES_EN, 1);
LOG_DBG("FIFO_CONFIG1 (0x%x) 0x%x", REG_FIFO_CONFIG1, fifo_cfg1);
res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG1, fifo_cfg1);

82
drivers/sensor/tdk/icm42688/icm42688_decoder.c
View file

@ -243,81 +243,33 @@ static inline q31_t icm42688_read_temperature_from_packet(const uint8_t *pkt)
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);
uint32_t unsigned_value;
int32_t signed_value;
bool is_hires = FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1;
int offset = 1 + (axis_offset * 2);
if (is_accel) {
switch (fs) {
case ICM42688_DT_ACCEL_FS_2:
scale = INT64_C(2) * BIT(31 - 5) * 9.80665;
break;
case ICM42688_DT_ACCEL_FS_4:
scale = INT64_C(4) * BIT(31 - 6) * 9.80665;
break;
case ICM42688_DT_ACCEL_FS_8:
scale = INT64_C(8) * BIT(31 - 7) * 9.80665;
break;
case ICM42688_DT_ACCEL_FS_16:
scale = INT64_C(16) * BIT(31 - 8) * 9.80665;
break;
}
} else {
switch (fs) {
case ICM42688_DT_GYRO_FS_2000:
scale = 164;
break;
case ICM42688_DT_GYRO_FS_1000:
scale = 328;
break;
case ICM42688_DT_GYRO_FS_500:
scale = 655;
break;
case ICM42688_DT_GYRO_FS_250:
scale = 1310;
break;
case ICM42688_DT_GYRO_FS_125:
scale = 2620;
break;
case ICM42688_DT_GYRO_FS_62_5:
scale = 5243;
break;
case ICM42688_DT_GYRO_FS_31_25:
scale = 10486;
break;
case ICM42688_DT_GYRO_FS_15_625:
scale = 20972;
break;
}
}
const uint32_t scale[2][2] = {
/* low-res, hi-res */
{35744, 8936}, /* gyro */
{40168, 2511}, /* accel */
};
if (!is_accel && FIELD_GET(FIFO_HEADER_ACCEL, pkt[0]) == 1) {
offset += 7;
offset += 6;
}
value = (int16_t)sys_le16_to_cpu((pkt[offset] << 8) | pkt[offset + 1]);
unsigned_value = (pkt[offset] << 8) | pkt[offset + 1];
if (FIELD_GET(FIFO_HEADER_20, pkt[0]) == 1) {
if (is_hires) {
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;
}
offset = 17 + axis_offset;
unsigned_value = (unsigned_value << 4) | FIELD_GET(mask, pkt[offset]);
signed_value = unsigned_value | (0 - (unsigned_value & BIT(19)));
} else {
if (value <= -32767) {
/* Invalid 16 bit value */
return -ENODATA;
}
signed_value = unsigned_value | (0 - (unsigned_value & BIT(16)));
}
*out = (q31_t)(value * scale / max);
*out = (q31_t)(signed_value * scale[is_accel][is_hires]);
return 0;
}
@ -434,7 +386,7 @@ 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 = accel_period_ns[edata->gyro_odr];
uint64_t period_ns = gyro_period_ns[edata->gyro_odr];
icm42688_get_shift(SENSOR_CHAN_GYRO_XYZ, edata->header.accel_fs,
edata->header.gyro_fs, &data->shift);

6
drivers/sensor/tdk/icm42688/icm42688_reg.h
View file

@ -254,9 +254,9 @@
#define BIT_FIFO_WM_GT_TH BIT(5)
#define BIT_FIFO_HIRES_EN BIT(4)
#define BIT_FIFO_TMST_FSYNC_EN BIT(3)
#define BIT_FIFO_GYRO_EN BIT(2)
#define BIT_FIFO_ACCEL_EN BIT(1)
#define BIT_FIFO_TEMP_EN BIT(0)
#define BIT_FIFO_TEMP_EN BIT(2)
#define BIT_FIFO_GYRO_EN BIT(1)
#define BIT_FIFO_ACCEL_EN BIT(0)
/* Bank0 INT_SOURCE0 */
#define BIT_UI_FSYNC_INT1_EN BIT(6)

4
drivers/sensor/tdk/icm42688/icm42688_rtio_stream.c
View file

@ -130,7 +130,7 @@ static void icm42688_fifo_count_cb(struct rtio *r, const struct rtio_sqe *sqe, v
read_len = pkts * packet_size;
((struct icm42688_fifo_data *)buf)->fifo_count = read_len;
__ASSERT_NO_MSG(read_len % pkt_size == 0);
__ASSERT_NO_MSG(read_len % packet_size == 0);
uint8_t *read_buf = buf + sizeof(hdr);
@ -302,7 +302,7 @@ void icm42688_fifo_event(const struct device *dev)
rc = sensor_clock_get_cycles(&cycles);
if (rc != 0) {
LOG_ERR("Failed to get sensor clock cycles");
rtio_iodev_sqe_err(drv_data->streaming_sqe, err);
rtio_iodev_sqe_err(drv_data->streaming_sqe, rc);
return;
}