driver: sensor: adxl345: Bug fix for q31_t conv

This is a bug fix for adxl345_accel_convert_q31 functions.
Functions are used to convert samples received from
sensor to q31_t format when RTIO stream is used.

Signed-off-by: Dimitrije Lilic <[email protected]>

Author: dimitrije-lilic

drivers/sensor/adi/adxl345/adxl345.c

@@ -438,7 +438,7 @@ static int adxl345_init(const struct device *dev)
 #ifdef CONFIG_ADXL345_TRIGGER
 	const struct adxl345_dev_config *cfg = dev->config;
 #endif
-	uint8_t dev_id;
+	uint8_t dev_id, full_res;
 
 	data->sample_number = 0;
 
@@ -459,12 +459,14 @@ static int adxl345_init(const struct device *dev)
 		return -EIO;
 	}
 
-	rc = adxl345_reg_write_byte(dev, ADXL345_DATA_FORMAT_REG, ADXL345_RANGE_16G);
+	rc = adxl345_reg_write_byte(dev, ADXL345_DATA_FORMAT_REG, ADXL345_RANGE_8G);
 	if (rc < 0) {
 		LOG_ERR("Data format set failed\n");
 		return -EIO;
 	}
 
+	data->selected_range = ADXL345_RANGE_8G;
+
 	rc = adxl345_reg_write_byte(dev, ADXL345_RATE_REG, ADXL345_RATE_25HZ);
 	if (rc < 0) {
 		LOG_ERR("Rate setting failed\n");
@@ -497,11 +499,16 @@ static int adxl345_init(const struct device *dev)
 	if (rc) {
 		return rc;
 	}
-		rc = adxl345_interrupt_config(dev, ADXL345_INT_MAP_WATERMARK_MSK);
+	rc = adxl345_interrupt_config(dev, ADXL345_INT_MAP_WATERMARK_MSK);
 	if (rc) {
 		return rc;
 	}
 #endif
+
+	rc = adxl345_reg_read_byte(dev, ADXL345_DATA_FORMAT_REG, &full_res);
+	uint8_t is_full_res_set = (full_res & ADXL345_DATA_FORMAT_FULL_RES) != 0;
+
+	data->is_full_res = is_full_res_set;
 	return 0;
 }
 

drivers/sensor/adi/adxl345/adxl345.h

@@ -40,13 +40,14 @@
 #define SAMPLE_NUM  0x1F
 
 /* Registers */
-#define ADXL345_DEVICE_ID_REG      0x00
-#define ADXL345_RATE_REG           0x2c
-#define ADXL345_POWER_CTL_REG      0x2d
-#define ADXL345_DATA_FORMAT_REG    0x31
-#define ADXL345_X_AXIS_DATA_0_REG  0x32
-#define ADXL345_FIFO_CTL_REG       0x38
-#define ADXL345_FIFO_STATUS_REG    0x39
+#define ADXL345_DEVICE_ID_REG           0x00
+#define ADXL345_RATE_REG                0x2c
+#define ADXL345_POWER_CTL_REG           0x2d
+#define ADXL345_DATA_FORMAT_REG         0x31
+#define ADXL345_DATA_FORMAT_FULL_RES    0x08
+#define ADXL345_X_AXIS_DATA_0_REG       0x32
+#define ADXL345_FIFO_CTL_REG            0x38
+#define ADXL345_FIFO_STATUS_REG         0x39
 
 #define ADXL345_PART_ID            0xe5
 
@@ -58,6 +59,7 @@
 #define ADXL345_ENABLE_MEASURE_BIT (1 << 3)
 #define ADXL345_FIFO_STREAM_MODE   (1 << 7)
 #define ADXL345_FIFO_COUNT_MASK    0x3f
+#define ADXL345_COMPLEMENT_MASK(x) GENMASK(15, (x))
 #define ADXL345_COMPLEMENT         0xfc00
 
 #define ADXL345_MAX_FIFO_SIZE      32
@@ -149,6 +151,8 @@ struct adxl345_dev_data {
 	int16_t bufy[ADXL345_MAX_FIFO_SIZE];
 	int16_t bufz[ADXL345_MAX_FIFO_SIZE];
 	struct adxl345_fifo_config fifo_config;
+	uint8_t is_full_res;
+	uint8_t selected_range;
 #ifdef CONFIG_ADXL345_TRIGGER
 	struct gpio_callback gpio_cb;
 
@@ -182,6 +186,8 @@ struct adxl345_dev_data {
 
 struct adxl345_fifo_data {
 	uint8_t is_fifo: 1;
+	uint8_t is_full_res: 1;
+	uint8_t selected_range: 2;
 	uint8_t sample_set_size: 4;
 	uint8_t int_status;
 	uint16_t accel_odr: 4;
@@ -194,6 +200,7 @@ struct adxl345_sample {
 	uint8_t is_fifo: 1;
 	uint8_t res: 7;
 #endif /* CONFIG_ADXL345_STREAM */
+	uint8_t selected_range;
 	int16_t x;
 	int16_t y;
 	int16_t z;

drivers/sensor/adi/adxl345/adxl345_decoder.c

@@ -19,13 +19,70 @@ static const uint32_t accel_period_ns[] = {
 	[ADXL345_ODR_400HZ] = UINT32_C(1000000000) / 400,
 };
 
-static inline void adxl345_accel_convert_q31(q31_t *out, uint16_t sample)
+static const uint32_t range_to_shift[] = {
+	[ADXL345_RANGE_2G] = 5,
+	[ADXL345_RANGE_4G] = 6,
+	[ADXL345_RANGE_8G] = 7,
+	[ADXL345_RANGE_16G] = 8,
+};
+
+/* (1 / sensitivity) * (pow(2,31) / pow(2,shift)) * (unit_scaler) */
+static const uint32_t qscale_factor_no_full_res[] = {
+	/* (1.0 / ADXL362_ACCEL_2G_LSB_PER_G) * (2^31 / 2^5) * SENSOR_G / 1000000 */
+	[ADXL345_RANGE_2G] = UINT32_C(2569011),
+	/* (1.0 / ADXL362_ACCEL_4G_LSB_PER_G) * (2^31 / 2^6) * SENSOR_G / 1000000  */
+	[ADXL345_RANGE_4G] = UINT32_C(642253),
+	/* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^7) ) * SENSOR_G / 1000000 */
+	[ADXL345_RANGE_8G] = UINT32_C(160563),
+	/* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^8) ) * SENSOR_G / 1000000 */
+	[ADXL345_RANGE_16G] = UINT32_C(40141),
+};
+
+/* (1 / sensitivity) * (pow(2,31) / pow(2,shift)) * (unit_scaler) */
+static const uint32_t qscale_factor_full_res[] = {
+	/* (1.0 / ADXL362_ACCEL_2G_LSB_PER_G) * (2^31 / 2^5) * SENSOR_G / 1000000 */
+	[ADXL345_RANGE_2G] = UINT32_C(2569011),
+	/* (1.0 / ADXL362_ACCEL_4G_LSB_PER_G) * (2^31 / 2^6) * SENSOR_G / 1000000  */
+	[ADXL345_RANGE_4G] = UINT32_C(1284506),
+	/* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^7) ) * SENSOR_G / 1000000 */
+	[ADXL345_RANGE_8G] = UINT32_C(642253),
+	/* (1.0 / ADXL362_ACCEL_8G_LSB_PER_G) * (2^31 / 2^8) ) * SENSOR_G / 1000000 */
+	[ADXL345_RANGE_16G] = UINT32_C(321126),
+};
+
+static inline void adxl345_accel_convert_q31(q31_t *out, int16_t sample, int32_t range,
+					uint8_t is_full_res)
 {
-	if (sample & BIT(9)) {
-		sample |= ADXL345_COMPLEMENT;
+	if (is_full_res) {
+		switch (range) {
+		case ADXL345_RANGE_2G:
+			if (sample & BIT(9)) {
+				sample |= ADXL345_COMPLEMENT_MASK(10);
+			}
+			break;
+		case ADXL345_RANGE_4G:
+			if (sample & BIT(10)) {
+				sample |= ADXL345_COMPLEMENT_MASK(11);
+			}
+			break;
+		case ADXL345_RANGE_8G:
+			if (sample & BIT(11)) {
+				sample |= ADXL345_COMPLEMENT_MASK(12);
+			}
+			break;
+		case ADXL345_RANGE_16G:
+			if (sample & BIT(12)) {
+				sample |= ADXL345_COMPLEMENT_MASK(13);
+			}
+			break;
+		}
+	} else {
+		if (sample & BIT(9)) {
+			sample |= ADXL345_COMPLEMENT;
+		}
 	}
-	int32_t micro_ms2 = ((sample * SENSOR_G) / 32);
-	*out = CLAMP((((int64_t)micro_ms2) + (micro_ms2 % 1000000)), INT32_MIN, INT32_MAX);
+
+	*out = sample * qscale_factor_no_full_res[range];
 }
 
 static int adxl345_decode_stream(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
@@ -46,11 +103,13 @@ static int adxl345_decode_stream(const uint8_t *buffer, struct sensor_chan_spec
 	memset(data, 0, sizeof(struct sensor_three_axis_data));
 	data->header.base_timestamp_ns = enc_data->timestamp;
 	data->header.reading_count = 1;
+	data->shift = range_to_shift[enc_data->selected_range];
 
 	buffer += sizeof(struct adxl345_fifo_data);
 
 	uint8_t sample_set_size = enc_data->sample_set_size;
 	uint64_t period_ns = accel_period_ns[enc_data->accel_odr];
+	uint8_t is_full_res = enc_data->is_full_res;
 
 	/* Calculate which sample is decoded. */
 	if ((uint8_t *)*fit >= buffer) {
@@ -73,13 +132,16 @@ static int adxl345_decode_stream(const uint8_t *buffer, struct sensor_chan_spec
 			data->readings[count].timestamp_delta = sample_num * period_ns;
 			uint8_t buff_offset = 0;
 
-			adxl345_accel_convert_q31(&data->readings[count].x, *(int16_t *)buffer);
+			adxl345_accel_convert_q31(&data->readings[count].x, *(int16_t *)buffer,
+					enc_data->selected_range, is_full_res);
 			buff_offset = 2;
 			adxl345_accel_convert_q31(&data->readings[count].y,
-						*(int16_t *)(buffer + buff_offset));
+						*(int16_t *)(buffer + buff_offset),
+							enc_data->selected_range, is_full_res);
 			buff_offset += 2;
 			adxl345_accel_convert_q31(&data->readings[count].z,
-						*(int16_t *)(buffer + buff_offset));
+						*(int16_t *)(buffer + buff_offset),
+							enc_data->selected_range, is_full_res);
 			break;
 		default:
 			return -ENOTSUP;

drivers/sensor/adi/adxl345/adxl345_stream.c

@@ -157,6 +157,8 @@ static void adxl345_process_fifo_samples_cb(struct rtio *r, const struct rtio_sq
 	hdr->is_fifo = 1;
 	hdr->timestamp = data->timestamp;
 	hdr->int_status = data->status1;
+	hdr->is_full_res = data->is_full_res;
+	hdr->selected_range = data->selected_range;
 	hdr->accel_odr = cfg->odr;
 	hdr->sample_set_size = sample_set_size;