common: online_calibration: Add support for magnetometer

Add support for magnetometers in online calibration.

BRANCH=None
BUG=b:138303797,chromium:1023858
TEST=Added new unit test

Change-Id: I3a6dafb2f5fab9b11ac8bd3b53ae4976002d18cd
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2095519

4 files changed, 79 insertions, 12 deletions

diff --git a/common/build.mk b/common/build.mk
index d21baf5aa7..8bb9197596 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -119,7 +119,8 @@ common-$(CONFIG_RWSIG_TYPE_RWSIG)+=vboot/vb21_lib.o
 common-$(CONFIG_MATH_UTIL)+=math_util.o
 common-$(CONFIG_ONLINE_CALIB)+=stillness_detector.o kasa.o math_util.o \
 	mat44.o vec3.o newton_fit.o accel_cal.o online_calibration.o \
-	mkbp_event.o
+	mkbp_event.o mag_cal.o math_util.o mat33.o
+common-$(CONFIG_SHA1)+= sha1.o
 common-$(CONFIG_SHA256)+=sha256.o
 common-$(CONFIG_SOFTWARE_CLZ)+=clz.o
 common-$(CONFIG_SOFTWARE_CTZ)+=ctz.o
diff --git a/common/online_calibration.c b/common/online_calibration.c
index dd2c154260..6c090a6abc 100644
--- a/common/online_calibration.c
+++ b/common/online_calibration.c
@@ -8,6 +8,7 @@
 #include "hwtimer.h"
 #include "online_calibration.h"
 #include "common.h"
+#include "mag_cal.h"
 #include "util.h"
 #include "vec3.h"
 #include "task.h"
@@ -108,8 +109,11 @@ void online_calibration_init(void)
 
 		switch (s->type) {
 		case MOTIONSENSE_TYPE_ACCEL: {
-			accel_cal_reset((struct accel_cal *)
-					type_specific_data);
+			accel_cal_reset((struct accel_cal *)type_specific_data);
+			break;
+		}
+		case MOTIONSENSE_TYPE_MAG: {
+			init_mag_cal((struct mag_cal_t *)type_specific_data);
 			break;
 		}
 		default:
@@ -153,14 +157,11 @@ int online_calibration_process_data(
 	struct motion_sensor_t *sensor,
 	uint32_t timestamp)
 {
+	size_t sensor_num = motion_sensors - sensor;
 	int rc;
 	int temperature;
 	struct online_calib_data *calib_data;
 
-	rc = get_temperature(sensor, &temperature);
-	if (rc != EC_SUCCESS)
-		return rc;
-
 	calib_data = sensor->online_calib_data;
 	switch (sensor->type) {
 	case MOTIONSENSE_TYPE_ACCEL: {
@@ -168,11 +169,14 @@ int online_calibration_process_data(
 			(struct accel_cal *)(calib_data->type_specific_data);
 		fpv3_t fdata;
 
+		/* Temperature is required for accelerometer calibration. */
+		rc = get_temperature(sensor, &temperature);
+		if (rc != EC_SUCCESS)
+			return rc;
+
 		data_int16_to_fp(sensor, data->data, fdata);
 		if (accel_cal_accumulate(cal, timestamp, fdata[X], fdata[Y],
 					 fdata[Z], temperature)) {
-			int sensor_num = motion_sensors - sensor;
-
 			mutex_lock(&g_calib_cache_mutex);
 			/* Convert result to the right scale. */
 			data_fp_to_int16(sensor, cal->bias, calib_data->cache);
@@ -185,6 +189,30 @@ int online_calibration_process_data(
 		}
 		break;
 	}
+	case MOTIONSENSE_TYPE_MAG: {
+		struct mag_cal_t *cal =
+			(struct mag_cal_t *) (calib_data->type_specific_data);
+		int idata[] = {
+			(int)data->data[X],
+			(int)data->data[Y],
+			(int)data->data[Z],
+		};
+
+		if (mag_cal_update(cal, idata)) {
+			mutex_lock(&g_calib_cache_mutex);
+			/* Copy the values */
+			calib_data->cache[X] = cal->bias[X];
+			calib_data->cache[Y] = cal->bias[Y];
+			calib_data->cache[Z] = cal->bias[Z];
+			/* Set valid and dirty. */
+			sensor_calib_cache_valid_map |= BIT(sensor_num);
+			sensor_calib_cache_dirty_map |= BIT(sensor_num);
+			mutex_unlock(&g_calib_cache_mutex);
+			/* Notify the AP. */
+			mkbp_send_event(EC_MKBP_EVENT_ONLINE_CALIBRATION);
+		}
+		break;
+	}
 	default:
 		break;
 	}
diff --git a/include/mag_cal.h b/include/mag_cal.h
index a730fcbe59..61b24c7da9 100644
--- a/include/mag_cal.h
+++ b/include/mag_cal.h
@@ -29,5 +29,13 @@ struct mag_cal_t {
 
 void init_mag_cal(struct mag_cal_t *moc);
 
+/**
+ * Update the magnetometer calibration structure and possibly compute the new
+ * bias.
+ *
+ * @param moc Pointer to the magnetometer struct to update.
+ * @param v   The new data.
+ * @return    1 if a new calibration value is available, 0 otherwise.
+ */
 int mag_cal_update(struct mag_cal_t *moc, const intv3_t v);
 #endif  /* __CROS_EC_MAG_CAL_H */
diff --git a/test/online_calibration.c b/test/online_calibration.c
index 622ea6232d..e9fff2430e 100644
--- a/test/online_calibration.c
+++ b/test/online_calibration.c
@@ -6,6 +6,7 @@
 #include "accel_cal.h"
 #include "accelgyro.h"
 #include "hwtimer.h"
+#include "mag_cal.h"
 #include "online_calibration.h"
 #include "test_util.h"
 #include "timer.h"
@@ -69,8 +70,6 @@ static struct accelgyro_drv mock_sensor_driver = {
 	.get_range = mock_get_range,
 };
 
-static struct accelgyro_drv empty_sensor_driver = {};
-
 static struct accel_cal_algo base_accel_cal_algos[] = {
 	{
 		.newton_fit = NEWTON_FIT(4, 15, FLOAT_TO_FP(0.01f),
@@ -86,6 +85,8 @@ static struct accel_cal base_accel_cal_data = {
 	.num_temp_windows = ARRAY_SIZE(base_accel_cal_algos),
 };
 
+static struct mag_cal_t lid_mag_cal_data;
+
 static bool next_accel_cal_accumulate_result;
 static fpv3_t next_accel_cal_bias;
 
@@ -110,7 +111,11 @@ struct motion_sensor_t motion_sensors[] = {
 		},
 	},
 	[LID] = {
-		.drv = &empty_sensor_driver,
+		.type = MOTIONSENSE_TYPE_MAG,
+		.drv = &mock_sensor_driver,
+		.online_calib_data[0] = {
+			.type_specific_data = &lid_mag_cal_data,
+		}
 	},
 };
 
@@ -216,6 +221,30 @@ static int test_new_calibration_value(void)
 	return EC_SUCCESS;
 }
 
+int test_mag_reading_updated_cal(void)
+{
+	struct mag_cal_t expected_results;
+	struct ec_response_motion_sensor_data data;
+	int rc;
+	int test_values[] = { 207, -17, -37 };
+
+	data.sensor_num = LID;
+	data.data[X] = test_values[X];
+	data.data[Y] = test_values[Y];
+	data.data[Z] = test_values[Z];
+
+	init_mag_cal(&expected_results);
+	mag_cal_update(&expected_results, test_values);
+
+	rc = online_calibration_process_data(
+		&data, &motion_sensors[LID], __hw_clock_source_read());
+	TEST_EQ(rc, EC_SUCCESS, "%d");
+	TEST_EQ(expected_results.kasa_fit.nsamples,
+		lid_mag_cal_data.kasa_fit.nsamples, "%d");
+
+	return EC_SUCCESS;
+}
+
 void before_test(void)
 {
 	mock_read_temp_results = NULL;
@@ -230,6 +259,7 @@ void run_test(int argc, char **argv)
 	RUN_TEST(test_read_temp_from_cache_on_stage);
 	RUN_TEST(test_read_temp_twice_after_cache_stale);
 	RUN_TEST(test_new_calibration_value);
+	RUN_TEST(test_mag_reading_updated_cal);
 
 	test_print_result();
 }