sensor: icm42688: Fix broken build and streaming.

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 <[email protected]>
Signed-off-by: Benjamin Perseghetti <[email protected]>

Author: 2 people

drivers/sensor/tdk/icm42688/icm42688_common.c

@@ -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);

drivers/sensor/tdk/icm42688/icm42688_decoder.c

@@ -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);

drivers/sensor/tdk/icm42688/icm42688_reg.h

@@ -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)

drivers/sensor/tdk/icm42688/icm42688_rtio_stream.c

@@ -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;
 	}