common: mag_cal: update magnetometer to leverage kasa

Update magnetometer calibration algorithm to leverage
the new kasa standalone code.

BUG=b:138303429,chromium:1023858
TEST=added unit test
BRANCH=None

Change-Id: I5c0403b66d9fe7c2925b2ec6244cf9e32ad5ea5f
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1931464
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>

1 files changed, 50 insertions, 125 deletions

diff --git a/common/mag_cal.c b/common/mag_cal.c
index db09050007..1dc40e34db 100644
--- a/common/mag_cal.c
+++ b/common/mag_cal.c
@@ -26,6 +26,18 @@
 #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
 #define PRINTF_FLOAT(x)  ((int)((x) * 100.0f))
 
+/**
+ * Compute the covariance element: (avg(ab) - avg(a)*avg(b))
+ *
+ * @param sq The accumulated sum of a*b
+ * @param a The accumulated sum of a
+ * @param b The accumulated sum of b
+ * @return (sq - ((a * b) * inv)) * inv
+ */
+static inline fp_t covariance_element(fp_t sq, fp_t a, fp_t b, fp_t inv)
+{
+	return fp_mul(sq - fp_mul(fp_mul(a, b), inv), inv);
+}
 /*
  * eigen value magnitude and ratio test
  *
@@ -38,18 +50,35 @@ static int moc_eigen_test(struct mag_cal_t *moc)
 	fpv3_t eigenvals;
 	mat33_fp_t eigenvecs;
 	fp_t evmax, evmin, evmag;
+	fp_t inv = fp_div_dbz(FLOAT_TO_FP(1.0f),
+			      INT_TO_FP((int) moc->kasa_fit.nsamples));
 	int eigen_pass;
 
 	/* covariance matrix */
-	S[0][0] = moc->acc[0][0] - fp_sq(moc->acc[0][3]);
-	S[0][1] = S[1][0] =
-		moc->acc[0][1] - fp_mul(moc->acc[0][3], moc->acc[1][3]);
-	S[0][2] = S[2][0] =
-		moc->acc[0][2] - fp_mul(moc->acc[0][3], moc->acc[2][3]);
-	S[1][1] = moc->acc[1][1] - fp_sq(moc->acc[1][3]);
-	S[1][2] = S[2][1] =
-		moc->acc[1][2] - fp_mul(moc->acc[1][3], moc->acc[2][3]);
-	S[2][2] = moc->acc[2][2] - fp_sq(moc->acc[2][3]);
+	S[0][0] = covariance_element(moc->kasa_fit.acc_xx,
+				     moc->kasa_fit.acc_x,
+				     moc->kasa_fit.acc_x,
+				     inv);
+	S[0][1] = S[1][0] = covariance_element(moc->kasa_fit.acc_xy,
+					       moc->kasa_fit.acc_x,
+					       moc->kasa_fit.acc_y,
+					       inv);
+	S[0][2] = S[2][0] = covariance_element(moc->kasa_fit.acc_xz,
+					       moc->kasa_fit.acc_x,
+					       moc->kasa_fit.acc_z,
+					       inv);
+	S[1][1] = covariance_element(moc->kasa_fit.acc_yy,
+				     moc->kasa_fit.acc_y,
+				     moc->kasa_fit.acc_y,
+				     inv);
+	S[1][2] = S[2][1] = covariance_element(moc->kasa_fit.acc_yz,
+					       moc->kasa_fit.acc_y,
+					       moc->kasa_fit.acc_z,
+					       inv);
+	S[2][2] = covariance_element(moc->kasa_fit.acc_zz,
+				     moc->kasa_fit.acc_z,
+				     moc->kasa_fit.acc_z,
+				     inv);
 
 	mat33_fp_get_eigenbasis(S, eigenvals, eigenvecs);
 
@@ -66,88 +95,23 @@ static int moc_eigen_test(struct mag_cal_t *moc)
 		&& (evmag < MAX_EIGEN_MAG);
 
 #if 0
-	CPRINTF("mag eigenvalues: (%d %d %d), ",
+	CPRINTF("mag eigenvalues: (%.02d %.02d %.02d), ",
 		PRINTF_FLOAT(eigenvals[X]),
 		PRINTF_FLOAT(eigenvals[Y]),
 		PRINTF_FLOAT(eigenvals[Z]));
 
-	CPRINTF("ratio %d, mag %d: pass %d\r\n",
+	CPRINTF("ratio %.02d, mag %.02d: pass %d\r\n",
 		PRINTF_FLOAT(evmax / evmin),
 		PRINTF_FLOAT(evmag),
-		PRINTF_FLOAT(eigen_pass));
+		eigen_pass);
 #endif
 
 	return eigen_pass;
 }
 
-/*
- * Kasa sphere fitting with normal equation
- */
-static int moc_fit(struct mag_cal_t *moc, fpv3_t bias, fp_t *radius)
-{
-	sizev4_t pivot;
-	fpv4_t out;
-	int success = 0;
-
-	/*
-	 * To reduce stack size, moc->acc is A,
-	 * moc->acc_w is b: we are looking for out, where:
-	 *
-	 *    A    *   out   =    b
-	 * (4 x 4)   (4 x 1)   (4 x 1)
-	 */
-	/* complete the matrix: */
-	moc->acc[1][0] = moc->acc[0][1];
-	moc->acc[2][0] = moc->acc[0][2];
-	moc->acc[2][1] = moc->acc[1][2];
-	moc->acc[3][0] = moc->acc[0][3];
-	moc->acc[3][1] = moc->acc[1][3];
-	moc->acc[3][2] = moc->acc[2][3];
-	moc->acc[3][3] = FLOAT_TO_FP(1.0f);
-
-	moc->acc_w[X] = fp_mul(moc->acc_w[X], FLOAT_TO_FP(-1));
-	moc->acc_w[Y] = fp_mul(moc->acc_w[Y], FLOAT_TO_FP(-1));
-	moc->acc_w[Z] = fp_mul(moc->acc_w[Z], FLOAT_TO_FP(-1));
-	moc->acc_w[W] = fp_mul(moc->acc_w[W], FLOAT_TO_FP(-1));
-
-	mat44_fp_decompose_lup(moc->acc, pivot);
-
-	mat44_fp_solve(moc->acc, out, moc->acc_w, pivot);
-
-	/*
-	 * spherei is defined by:
-	 * (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
-	 *
-	 * Where r is:
-	 * xc = -out[X] / 2, yc = -out[Y] / 2, zc = -out[Z] / 2
-	 * r = sqrt(xc^2 + yc^2 + zc^2 - out[W])
-	 */
-
-	memcpy(bias, out, sizeof(fpv3_t));
-	fpv3_scalar_mul(bias, FLOAT_TO_FP(-0.5f));
-
-	*radius = fp_sqrtf(fpv3_dot(bias, bias) - out[W]);
-
-#if 0
-	CPRINTF("mag cal: bias (%d, %d, %d), R %d uT\n",
-		PRINTF_FLOAT(bias[X] / MAG_CAL_RAW_UT),
-		PRINTF_FLOAT(bias[Y] / MAG_CAL_RAW_UT),
-		PRINTF_FLOAT(bias[Z] / MAG_CAL_RAW_UT),
-		PRINTF_FLOAT(*radius / MAG_CAL_RAW_UT));
-#endif
-
-	/* TODO (menghsuan): bound on bias as well? */
-	if (*radius > MIN_FIT_MAG && *radius < MAX_FIT_MAG)
-		success = 1;
-
-	return success;
-}
-
 void init_mag_cal(struct mag_cal_t *moc)
 {
-	memset(moc->acc, 0, sizeof(moc->acc));
-	memset(moc->acc_w, 0, sizeof(moc->acc_w));
-	moc->nsamples = 0;
+	kasa_reset(&moc->kasa_fit);
 }
 
 int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
@@ -155,61 +119,22 @@ int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
 	int new_bias = 0;
 
 	/* 1. run accumulators */
-	fp_t w = fp_sq(v[X]) + fp_sq(v[Y]) + fp_sq(v[Z]);
-
-	moc->acc[0][3] += v[X];
-	moc->acc[1][3] += v[Y];
-	moc->acc[2][3] += v[Z];
-	moc->acc_w[W] += w;
-
-	moc->acc[0][0] += fp_sq(v[X]);
-	moc->acc[0][1] += fp_mul(v[X], v[Y]);
-	moc->acc[0][2] += fp_mul(v[X], v[Z]);
-	moc->acc_w[X] += fp_mul(v[X], w);
-
-	moc->acc[1][1] += fp_sq(v[Y]);
-	moc->acc[1][2] += fp_mul(v[Y], v[Z]);
-	moc->acc_w[Y] += fp_mul(v[Y], w);
-
-	moc->acc[2][2] += fp_sq(v[Z]);
-	moc->acc_w[Z] += fp_mul(v[Z], w);
-
-	if (moc->nsamples < MAG_CAL_MAX_SAMPLES)
-		moc->nsamples++;
+	kasa_accumulate(&moc->kasa_fit, INT_TO_FP(v[X]), INT_TO_FP(v[Y]),
+			INT_TO_FP(v[Z]));
 
 	/* 2. batch has enough samples? */
-	if (moc->batch_size > 0 && moc->nsamples >= moc->batch_size) {
-		fp_t inv = fp_div_dbz(FLOAT_TO_FP(1.0f),
-				  INT_TO_FP((int)moc->nsamples));
-
-		moc->acc[0][3] = fp_mul(moc->acc[0][3], inv);
-		moc->acc[1][3] = fp_mul(moc->acc[1][3], inv);
-		moc->acc[2][3] = fp_mul(moc->acc[2][3], inv);
-		moc->acc_w[W] = fp_mul(moc->acc_w[W], inv);
-
-		moc->acc[0][0] = fp_mul(moc->acc[0][0], inv);
-		moc->acc[0][1] = fp_mul(moc->acc[0][1], inv);
-		moc->acc[0][2] = fp_mul(moc->acc[0][2], inv);
-		moc->acc_w[X] = fp_mul(moc->acc_w[X], inv);
-
-		moc->acc[1][1] = fp_mul(moc->acc[1][1], inv);
-		moc->acc[1][2] = fp_mul(moc->acc[1][2], inv);
-		moc->acc_w[Y] = fp_mul(moc->acc_w[Y], inv);
-
-		moc->acc[2][2] = fp_mul(moc->acc[2][2], inv);
-		moc->acc_w[Z] = fp_mul(moc->acc_w[Z], inv);
-
+	if (moc->batch_size > 0 && moc->kasa_fit.nsamples >= moc->batch_size) {
 		/* 3. eigen test */
 		if (moc_eigen_test(moc)) {
 			fpv3_t bias;
 			fp_t radius;
 
 			/* 4. Kasa sphere fitting */
-			if (moc_fit(moc, bias, &radius)) {
-
-				moc->bias[X] = fp_mul(bias[X], FLOAT_TO_FP(-1));
-				moc->bias[Y] = fp_mul(bias[Y], FLOAT_TO_FP(-1));
-				moc->bias[Z] = fp_mul(bias[Z], FLOAT_TO_FP(-1));
+			kasa_compute(&moc->kasa_fit, bias, &radius);
+			if (radius > MIN_FIT_MAG && radius < MAX_FIT_MAG) {
+				moc->bias[X] = FP_TO_INT(bias[X]);
+				moc->bias[Y] = FP_TO_INT(bias[Y]);
+				moc->bias[Z] = FP_TO_INT(bias[Z]);
 
 				moc->radius = radius;