driver: bmi: add get_rms_noise() for body detection

We will need the amount of noise for body detection.
The amount of noise in accelometer will depends on several thing, e.g.,
output data rate.

Add get_rms_noise() function to get the root mean square
of noise in BMI.

BRANCH=None
BUG=b:123434029
TEST=buildall

Signed-off-by: Ching-Kang Yen <chingkang@chromium.org>
Change-Id: Ia56bbd2cdb36bee771beb9df32451d3e56f4f028
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2329112
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-by: Heng-ruey Hsu <henryhsu@chromium.org>

7 files changed, 53 insertions, 2 deletions

diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index d7b08e0c9e..803e705f42 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -750,6 +750,9 @@ const struct accelgyro_drv bmi160_drv = {
 	.manage_activity = manage_activity,
 	.list_activities = list_activities,
 #endif
+#ifdef CONFIG_BODY_DETECTION
+	.get_rms_noise = bmi_get_rms_noise,
+#endif
 };
 
 #ifdef CONFIG_CMD_I2C_STRESS_TEST_ACCEL
diff --git a/driver/accelgyro_bmi160.h b/driver/accelgyro_bmi160.h
index e93d0c28c7..c0e8a7128b 100644
--- a/driver/accelgyro_bmi160.h
+++ b/driver/accelgyro_bmi160.h
@@ -379,6 +379,9 @@
 #define BMI160_FF_DATA_LEN_GYR          6
 #define BMI160_FF_DATA_LEN_MAG          8
 
+/* Root mean square noise of 100 Hz accelerometer, units: ug */
+#define BMI160_ACCEL_RMS_NOISE_100HZ    1300
+
 extern const struct accelgyro_drv bmi160_drv;
 
 void bmi160_interrupt(enum gpio_signal signal);
diff --git a/driver/accelgyro_bmi260.c b/driver/accelgyro_bmi260.c
index 6eeb80cb04..cd75de2d9d 100644
--- a/driver/accelgyro_bmi260.c
+++ b/driver/accelgyro_bmi260.c
@@ -565,6 +565,9 @@ const struct accelgyro_drv bmi260_drv = {
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	.irq_handler = irq_handler,
 #endif
+#ifdef CONFIG_BODY_DETECTION
+	.get_rms_noise = bmi_get_rms_noise,
+#endif
 };
 
 #ifdef CONFIG_CMD_I2C_STRESS_TEST_ACCEL
diff --git a/driver/accelgyro_bmi260.h b/driver/accelgyro_bmi260.h
index 4358e657fe..9c7edc0766 100644
--- a/driver/accelgyro_bmi260.h
+++ b/driver/accelgyro_bmi260.h
@@ -327,9 +327,11 @@
 #define BMI260_FF_FRAME_LEN_TS          4
 #define BMI260_FF_DATA_LEN_ACC          6
 #define BMI260_FF_DATA_LEN_GYR          6
-
 #define BMI260_FF_DATA_LEN_MAG          8
 
+/* Root mean square noise of 100Hz accelerometer, units: ug */
+#define BMI260_ACCEL_RMS_NOISE_100HZ    1060
+
 extern const struct accelgyro_drv bmi260_drv;
 
 void bmi260_interrupt(enum gpio_signal signal);
diff --git a/driver/accelgyro_bmi_common.c b/driver/accelgyro_bmi_common.c
index f348edb1c5..2d8cfa2c45 100644
--- a/driver/accelgyro_bmi_common.c
+++ b/driver/accelgyro_bmi_common.c
@@ -661,6 +661,34 @@ int bmi_get_offset(const struct motion_sensor_t *s,
 	return EC_SUCCESS;
 }
 
+#ifdef CONFIG_BODY_DETECTION
+int bmi_get_rms_noise(const struct motion_sensor_t *s)
+{
+	int ret;
+	fp_t noise_100hz, rate, sqrt_rate_ratio;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		/* change unit of ODR to Hz to prevent INT_TO_FP() overflow */
+		rate = INT_TO_FP(bmi_get_data_rate(s) / 1000);
+		/*
+		 * Since the noise is proportional to sqrt(ODR) in BMI, and we
+		 * have rms noise in 100 Hz, we multiply it with the sqrt(ratio
+		 * of ODR to 100Hz) to get current noise.
+		 */
+		noise_100hz = INT_TO_FP(BMI_ACCEL_RMS_NOISE_100HZ(V(s)));
+		sqrt_rate_ratio = fp_sqrtf(fp_div(rate,
+						  INT_TO_FP(BMI_ACCEL_100HZ)));
+		ret = FP_TO_INT(fp_mul(noise_100hz, sqrt_rate_ratio));
+		break;
+	default:
+		CPRINTS("%s with gyro/mag is not implemented", __func__);
+		return 0;
+	}
+	return ret;
+}
+#endif
+
 int bmi_get_resolution(const struct motion_sensor_t *s)
 {
 	return BMI_RESOLUTION;
diff --git a/driver/accelgyro_bmi_common.h b/driver/accelgyro_bmi_common.h
index 583d43b04d..8073cb1674 100644
--- a/driver/accelgyro_bmi_common.h
+++ b/driver/accelgyro_bmi_common.h
@@ -150,7 +150,9 @@ struct bmi_drv_data_t {
 	(v) * (BMI260_CMD_REG - BMI160_CMD_REG))
 #define BMI_CMD_FIFO_FLUSH 0xb0
 
-
+#define BMI_ACCEL_RMS_NOISE_100HZ(v) (BMI160_ACCEL_RMS_NOISE_100HZ + \
+	(v) * (BMI260_ACCEL_RMS_NOISE_100HZ - BMI160_ACCEL_RMS_NOISE_100HZ))
+#define BMI_ACCEL_100HZ 100
 
 /*
  * Struct for pairing an engineering value with the register value for a
@@ -298,6 +300,10 @@ int bmi_get_offset(const struct motion_sensor_t *s,
 
 int bmi_get_resolution(const struct motion_sensor_t *s);
 
+#ifdef CONFIG_BODY_DETECTION
+int bmi_get_rms_noise(const struct motion_sensor_t *s);
+#endif
+
 int bmi_set_scale(const struct motion_sensor_t *s,
 		  const uint16_t *scale, int16_t temp);
 
diff --git a/include/accelgyro.h b/include/accelgyro.h
index 1417188d1a..e1226bc3e4 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -159,6 +159,12 @@ struct accelgyro_drv {
 			       uint32_t *disabled);
 
 #endif
+#ifdef CONFIG_BODY_DETECTION
+	/**
+	 * Get the root mean square of current noise (ug/mdps) in the sensor.
+	 */
+	int (*get_rms_noise)(const struct motion_sensor_t *s);
+#endif
 };
 
 /* Index values for rgb_calibration_t.coeff array */