bmi160: fix value calculation

As an example, when the gyro range is set to 250 deg/sec the scale was set to 133 where it really should be 133.160058662. This leads to a 0.12% error in the value returned. By separating the numerator and denominator, we're able to drastically reduce the error. Signed-off-by: Yuval Peress <peress@google.com>
zephyrproject-rtos · Dec 12, 2023 · 00b9683 · 00b9683
1 parent f218a78
commit 00b9683
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 23 deletions.
diff --git a/drivers/sensor/bmi160/bmi160.c b/drivers/sensor/bmi160/bmi160.c
@@ -403,7 +403,7 @@ static int bmi160_acc_range_set(const struct device *dev, const struct sensor_va
 		return -EIO;
 	}
 
-	data->scale.acc = BMI160_ACC_SCALE(range_g);
+	data->scale.acc_numerator = BMI160_ACC_SCALE_NUMERATOR(range_g);
 	return 0;
 }
 #endif
@@ -586,7 +586,7 @@ static int bmi160_gyr_range_set(const struct device *dev, const struct sensor_va
 		return -EIO;
 	}
 
-	data->scale.gyr = BMI160_GYR_SCALE(range);
+	data->scale.gyr_numerator = BMI160_GYR_SCALE_NUMERATOR(range);
 
 	return 0;
 }
@@ -899,24 +899,18 @@ static int bmi160_sample_fetch(const struct device *dev,
 	return ret;
 }
 
-static void bmi160_to_fixed_point(int16_t raw_val, uint16_t scale,
-				  struct sensor_value *val)
+static void bmi160_to_fixed_point(int16_t raw_val, int64_t scale_numerator,
+				  uint32_t scale_denominator, struct sensor_value *val)
 {
-	int32_t converted_val;
+	int64_t converted_val = (int64_t)raw_val * scale_numerator / scale_denominator;
 
-	/*
-	 * maximum converted value we can get is: max(raw_val) * max(scale)
-	 *	max(raw_val) = +/- 2^15
-	 *	max(scale) = 4785
-	 *	max(converted_val) = 156794880 which is less than 2^31
-	 */
-	converted_val = raw_val * scale;
 	val->val1 = converted_val / 1000000;
 	val->val2 = converted_val % 1000000;
 }
 
 static void bmi160_channel_convert(enum sensor_channel chan,
-				   uint16_t scale,
+				   int64_t scale_numerator,
+				   uint32_t scale_denominator,
 				   uint16_t *raw_xyz,
 				   struct sensor_value *val)
 {
@@ -942,7 +936,7 @@ static void bmi160_channel_convert(enum sensor_channel chan,
 	}
 
 	for (i = ofs_start; i <= ofs_stop ; i++, val++) {
-		bmi160_to_fixed_point(raw_xyz[i], scale, val);
+		bmi160_to_fixed_point(raw_xyz[i], scale_numerator, scale_denominator, val);
 	}
 }
 
@@ -953,7 +947,8 @@ static inline void bmi160_gyr_channel_get(const struct device *dev,
 {
 	struct bmi160_data *data = dev->data;
 
-	bmi160_channel_convert(chan, data->scale.gyr, data->sample.gyr, val);
+	bmi160_channel_convert(chan, data->scale.gyr_numerator, BMI160_GYR_SCALE_DENOMINATOR,
+			       data->sample.gyr, val);
 }
 #endif
 
@@ -964,7 +959,8 @@ static inline void bmi160_acc_channel_get(const struct device *dev,
 {
 	struct bmi160_data *data = dev->data;
 
-	bmi160_channel_convert(chan, data->scale.acc, data->sample.acc, val);
+	bmi160_channel_convert(chan, data->scale.acc_numerator, BMI160_ACC_SCALE_DENOMINATOR,
+			       data->sample.acc, val);
 }
 #endif
 
@@ -1130,7 +1126,7 @@ int bmi160_init(const struct device *dev)
 
 	acc_range = bmi160_acc_reg_val_to_range(BMI160_DEFAULT_RANGE_ACC);
 
-	data->scale.acc = BMI160_ACC_SCALE(acc_range);
+	data->scale.acc_numerator = BMI160_ACC_SCALE_NUMERATOR(acc_range);
 
 	/* set gyro default range */
 	if (bmi160_byte_write(dev, BMI160_REG_GYR_RANGE,
@@ -1141,7 +1137,7 @@ int bmi160_init(const struct device *dev)
 
 	gyr_range = bmi160_gyr_reg_val_to_range(BMI160_DEFAULT_RANGE_GYR);
 
-	data->scale.gyr = BMI160_GYR_SCALE(gyr_range);
+	data->scale.gyr_numerator = BMI160_GYR_SCALE_NUMERATOR(gyr_range);
 
 	if (bmi160_reg_field_update(dev, BMI160_REG_ACC_CONF,
 				    BMI160_ACC_CONF_ODR_POS,

diff --git a/drivers/sensor/bmi160/bmi160.h b/drivers/sensor/bmi160/bmi160.h
@@ -300,9 +300,11 @@ enum bmi160_odr {
 #define BMI160_GYR_RANGE_250DPS		3
 #define BMI160_GYR_RANGE_125DPS		4
 
-#define BMI160_ACC_SCALE(range_g)	((2 * range_g * SENSOR_G) / 65536LL)
-#define BMI160_GYR_SCALE(range_dps)\
-				((2 * range_dps * SENSOR_PI) / 180LL / 65536LL)
+#define BMI160_ACC_SCALE_NUMERATOR(range_g) (2 * (range_g) * SENSOR_G)
+#define BMI160_ACC_SCALE_DENOMINATOR UINT16_MAX
+
+#define BMI160_GYR_SCALE_NUMERATOR(range_dps) (2 * (range_dps) * SENSOR_PI)
+#define BMI160_GYR_SCALE_DENOMINATOR (UINT32_C(180) * UINT16_MAX)
 
 /* default settings, based on menuconfig options */
 
@@ -483,8 +485,10 @@ union bmi160_sample {
 };
 
 struct bmi160_scale {
-	uint16_t acc; /* micro m/s^2/lsb */
-	uint16_t gyr; /* micro radians/s/lsb */
+	/* numerator / denominator => micro m/s^2/lsb */
+	int32_t acc_numerator;
+	/* numerator / denominator => micro radians/s/lsb */
+	int64_t gyr_numerator;
 };
 
 struct bmi160_data {