mag_cal: Support fixed-point calculation.

Modified from floating point version.  This includes changes to
vec3, vec4, mat33, mat44, and mag_cal.
Now fixed-point type (fp_*) functions is a function wrapper for both
fixed-point and floating point version operations:

* define CONFIG_FPU to use floating version mag_cal
* undef CONFIG_FPU to use fixed-point version mag_cal

Also, add tests for both float and fp types operations.

TEST=define CONFIG_FPU; flash on reef; See ARC++ magnetmeter app moving.
TEST=undef CONFIG_FPU; flash on reef; See ARC++ magnetmeter app moving.
TEST=make runtests -j
TEST=make buildalltests -j
BUG=b:113364863
BRANCH=None

Change-Id: Ie695945acb666912babb2a603e09c602a0624d44
Signed-off-by: Yilun Lin <yllin@google.com>
Reviewed-on: https://chromium-review.googlesource.com/1260704
Commit-Ready: Yilun Lin <yllin@chromium.org>
Tested-by: Yilun Lin <yllin@chromium.org>
Reviewed-by: Nicolas Boichat <drinkcat@chromium.org>

1 files changed, 62 insertions, 59 deletions

diff --git a/common/mag_cal.c b/common/mag_cal.c
index 6e5a48b6b1..1e71059921 100644
--- a/common/mag_cal.c
+++ b/common/mag_cal.c
@@ -16,9 +16,9 @@
 /* Data from sensor is in 16th of uT */
 #define MAG_CAL_RAW_UT      16
 
-#define MAX_EIGEN_RATIO     25.0f
-#define MAX_EIGEN_MAG       (80.0f * MAG_CAL_RAW_UT)
-#define MIN_EIGEN_MAG       (10.0f * MAG_CAL_RAW_UT)
+#define MAX_EIGEN_RATIO     FLOAT_TO_FP(25.0f)
+#define MAX_EIGEN_MAG       FLOAT_TO_FP(80.0f * MAG_CAL_RAW_UT)
+#define MIN_EIGEN_MAG       FLOAT_TO_FP(10.0f * MAG_CAL_RAW_UT)
 
 #define MAX_FIT_MAG         MAX_EIGEN_MAG
 #define MIN_FIT_MAG         MIN_EIGEN_MAG
@@ -34,21 +34,24 @@
  */
 static int moc_eigen_test(struct mag_cal_t *moc)
 {
-	mat33_float_t S;
-	floatv3_t eigenvals;
-	mat33_float_t eigenvecs;
-	float evmax, evmin, evmag;
+	mat33_fp_t S;
+	fpv3_t eigenvals;
+	mat33_fp_t eigenvecs;
+	fp_t evmax, evmin, evmag;
 	int eigen_pass;
 
 	/* covariance matrix */
-	S[0][0] = moc->acc[0][0] - moc->acc[0][3] * moc->acc[0][3];
-	S[0][1] = S[1][0] = moc->acc[0][1] - moc->acc[0][3] * moc->acc[1][3];
-	S[0][2] = S[2][0] = moc->acc[0][2] - moc->acc[0][3] * moc->acc[2][3];
-	S[1][1] = moc->acc[1][1] - moc->acc[1][3] * moc->acc[1][3];
-	S[1][2] = S[2][1] = moc->acc[1][2] - moc->acc[1][3] * moc->acc[2][3];
-	S[2][2] = moc->acc[2][2] - moc->acc[2][3] * moc->acc[2][3];
-
-	mat33_float_get_eigenbasis(S, eigenvals, eigenvecs);
+	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]);
+
+	mat33_fp_get_eigenbasis(S, eigenvals, eigenvecs);
 
 	evmax = (eigenvals[X] > eigenvals[Y]) ? eigenvals[X] : eigenvals[Y];
 	evmax = (eigenvals[Z] > evmax) ? eigenvals[Z] : evmax;
@@ -56,9 +59,9 @@ static int moc_eigen_test(struct mag_cal_t *moc)
 	evmin = (eigenvals[X] < eigenvals[Y]) ? eigenvals[X] : eigenvals[Y];
 	evmin = (eigenvals[Z] < evmin) ? eigenvals[Z] : evmin;
 
-	evmag = sqrtf(eigenvals[X] + eigenvals[Y] + eigenvals[Z]);
+	evmag = fp_sqrtf(eigenvals[X] + eigenvals[Y] + eigenvals[Z]);
 
-	eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax)
+	eigen_pass = (fp_mul(evmin, MAX_EIGEN_RATIO) > evmax)
 		&& (evmag > MIN_EIGEN_MAG)
 		&& (evmag < MAX_EIGEN_MAG);
 
@@ -80,10 +83,10 @@ static int moc_eigen_test(struct mag_cal_t *moc)
 /*
  * Kasa sphere fitting with normal equation
  */
-static int moc_fit(struct mag_cal_t *moc, floatv3_t bias, float *radius)
+static int moc_fit(struct mag_cal_t *moc, fpv3_t bias, fp_t *radius)
 {
 	sizev4_t pivot;
-	floatv4_t out;
+	fpv4_t out;
 	int success = 0;
 
 	/*
@@ -100,16 +103,16 @@ static int moc_fit(struct mag_cal_t *moc, floatv3_t bias, float *radius)
 	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] = 1.0f;
+	moc->acc[3][3] = FLOAT_TO_FP(1.0f);
 
-	moc->acc_w[X] *= -1;
-	moc->acc_w[Y] *= -1;
-	moc->acc_w[Z] *= -1;
-	moc->acc_w[W] *= -1;
+	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_float_decompose_lup(moc->acc, pivot);
+	mat44_fp_decompose_lup(moc->acc, pivot);
 
-	mat44_float_solve(moc->acc, out, moc->acc_w, pivot);
+	mat44_fp_solve(moc->acc, out, moc->acc_w, pivot);
 
 	/*
 	 * spherei is defined by:
@@ -120,10 +123,10 @@ static int moc_fit(struct mag_cal_t *moc, floatv3_t bias, float *radius)
 	 * r = sqrt(xc^2 + yc^2 + zc^2 - out[W])
 	 */
 
-	memcpy(bias, out, sizeof(floatv3_t));
-	floatv3_scalar_mul(bias, -0.5f);
+	memcpy(bias, out, sizeof(fpv3_t));
+	fpv3_scalar_mul(bias, FLOAT_TO_FP(-0.5f));
 
-	*radius = sqrtf(floatv3_dot(bias, bias) - out[W]);
+	*radius = fp_sqrtf(fpv3_dot(bias, bias) - out[W]);
 
 #if 0
 	CPRINTF("mag cal: bias (%d, %d, %d), R %d uT\n",
@@ -152,60 +155,61 @@ int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
 	int new_bias = 0;
 
 	/* 1. run accumulators */
-	float w = v[X] * v[X] + v[Y] * v[Y] + v[Z] * v[Z];
+	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] += v[X] * v[X];
-	moc->acc[0][1] += v[X] * v[Y];
-	moc->acc[0][2] += v[X] * v[Z];
-	moc->acc_w[X] += v[X] * 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] += v[Y] * v[Y];
-	moc->acc[1][2] += v[Y] * v[Z];
-	moc->acc_w[Y] += v[Y] * 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] += v[Z] * v[Z];
-	moc->acc_w[Z] += v[Z] * 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++;
 
 	/* 2. batch has enough samples? */
 	if (moc->batch_size > 0 && moc->nsamples >= moc->batch_size) {
-		float inv = 1.0f / moc->nsamples;
+		fp_t inv = fp_div_dbz(FLOAT_TO_FP(1.0f),
+				  INT_TO_FP((int)moc->nsamples));
 
-		moc->acc[0][3] *= inv;
-		moc->acc[1][3] *= inv;
-		moc->acc[2][3] *= inv;
-		moc->acc_w[W] *= inv;
+		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] *= inv;
-		moc->acc[0][1] *= inv;
-		moc->acc[0][2] *= inv;
-		moc->acc_w[X] *= 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] *= inv;
-		moc->acc[1][2] *= inv;
-		moc->acc_w[Y] *= 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] *= inv;
-		moc->acc_w[Z] *= inv;
+		moc->acc[2][2] = fp_mul(moc->acc[2][2], inv);
+		moc->acc_w[Z] = fp_mul(moc->acc_w[Z], inv);
 
 		/* 3. eigen test */
 		if (moc_eigen_test(moc)) {
-			floatv3_t bias;
-			float radius;
+			fpv3_t bias;
+			fp_t radius;
 
 			/* 4. Kasa sphere fitting */
 			if (moc_fit(moc, bias, &radius)) {
 
-				moc->bias[X] = bias[X] * -1;
-				moc->bias[Y] = bias[Y] * -1;
-				moc->bias[Z] = bias[Z] * -1;
+				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));
 
 				moc->radius = radius;
 
@@ -218,4 +222,3 @@ int mag_cal_update(struct mag_cal_t *moc, const intv3_t v)
 
 	return new_bias;
 }
-