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, 65 insertions, 49 deletions

diff --git a/common/mat33.c b/common/mat33.c
index 793173ee3a..87e335db26 100644
--- a/common/mat33.c
+++ b/common/mat33.c
@@ -10,31 +10,35 @@
 
 #define K_EPSILON 1E-5f
 
-void init_zero_matrix(mat33_float_t A)
+void mat33_fp_init_zero(mat33_fp_t A)
 {
-	memset(A, 0, sizeof(mat33_float_t));
+	memset(A, 0, sizeof(mat33_fp_t));
 }
 
-void init_diagonal_matrix(mat33_float_t A, float x)
+void mat33_fp_init_diagonal(mat33_fp_t A, fp_t x)
 {
+	const size_t N = 3;
 	size_t i;
-	init_zero_matrix(A);
 
-	for (i = 0; i < 3; ++i)
+	mat33_fp_init_zero(A);
+
+	for (i = 0; i < N; ++i)
 		A[i][i] = x;
 }
 
-void mat33_float_scalar_mul(mat33_float_t A, float c)
+void mat33_fp_scalar_mul(mat33_fp_t A, fp_t c)
 {
+	const size_t N = 3;
 	size_t i;
-	for (i = 0; i < 3; ++i) {
+
+	for (i = 0; i < N; ++i) {
 		size_t j;
-		for (j = 0; j < 3; ++j)
-			A[i][j] *= c;
+		for (j = 0; j < N; ++j)
+			A[i][j] = fp_mul(A[i][j], c);
 	}
 }
 
-void mat33_float_swap_rows(mat33_float_t A, const size_t i, const size_t j)
+void mat33_fp_swap_rows(mat33_fp_t A, const size_t i, const size_t j)
 {
 	const size_t N = 3;
 	size_t k;
@@ -43,7 +47,7 @@ void mat33_float_swap_rows(mat33_float_t A, const size_t i, const size_t j)
 		return;
 
 	for (k = 0; k < N; ++k) {
-		float tmp = A[i][k];
+		fp_t tmp = A[i][k];
 		A[i][k] = A[j][k];
 		A[j][k] = tmp;
 	}
@@ -55,79 +59,91 @@ void mat33_float_swap_rows(mat33_float_t A, const size_t i, const size_t j)
  * The i-th eigenvalue corresponds to the eigenvector in the i-th _row_ of
  * "eigenvecs".
  */
-void mat33_float_get_eigenbasis(mat33_float_t S, floatv3_t e_vals,
-				mat33_float_t e_vecs)
+void mat33_fp_get_eigenbasis(mat33_fp_t S, fpv3_t e_vals,
+			     mat33_fp_t e_vecs)
 {
 	const size_t N = 3;
 	sizev3_t ind;
 	size_t i, j, k, l, m;
 
 	for (k = 0; k < N; ++k) {
-		ind[k] = mat33_float_maxind(S, k);
+		ind[k] = mat33_fp_maxind(S, k);
 		e_vals[k] = S[k][k];
 	}
 
-	init_diagonal_matrix(e_vecs, 1.0f);
+	mat33_fp_init_diagonal(e_vecs, FLOAT_TO_FP(1.0f));
 
 	for (;;) {
-		float y, t, s, c, p, sum;
+		fp_t y, t, s, c, p, sum;
+
 		m = 0;
-		for (k = 1; k + 1 < N; ++k) {
-			if (fabsf(S[k][ind[k]]) >
-			    fabsf(S[m][ind[m]])) {
+		for (k = 1; k + 1 < N; ++k)
+			if (fp_abs(S[k][ind[k]]) > fp_abs(S[m][ind[m]]))
 				m = k;
-			}
-		}
 
 		k = m;
 		l = ind[m];
 		p = S[k][l];
 
-		if (fabsf(p) < K_EPSILON)
+		/*
+		 * Note: K_EPSILON(1E-5) is too small to fit into 32-bit
+		 * fixed-point(with 16 fp bits). The minimum positive value is
+		 * 1 which is approximately 1.52E-5, so the
+		 * FLOAT_TO_FP(K_EPSILON) becomes zero.
+		 */
+		if (fp_abs(p) <= FLOAT_TO_FP(K_EPSILON))
 			break;
 
-		y = (e_vals[l] - e_vals[k]) * 0.5f;
+		y = fp_mul(e_vals[l] - e_vals[k], FLOAT_TO_FP(0.5f));
 
-		t = fabsf(y) + sqrtf(p * p + y * y);
-		s = sqrtf(p * p + t * t);
-		c = t / s;
-		s = p / s;
-		t = p * p / t;
+		t = fp_abs(y) + fp_sqrtf(fp_sq(p) + fp_sq(y));
+		s = fp_sqrtf(fp_sq(p) + fp_sq(t));
+		c = fp_div_dbz(t, s);
+		s = fp_div_dbz(p, s);
+		t = fp_div_dbz(fp_sq(p), t);
 
-		if (y < 0.0f) {
+		if (y < FLOAT_TO_FP(0.0f)) {
 			s = -s;
 			t = -t;
 		}
 
-		S[k][l] = 0.0f;
+		S[k][l] = FLOAT_TO_FP(0.0f);
 
 		e_vals[k] -= t;
 		e_vals[l] += t;
 
 		for (i = 0; i < k; ++i)
-			mat33_float_rotate(S, c, s, i, k, i, l);
+			mat33_fp_rotate(S, c, s, i, k, i, l);
 
 		for (i = k + 1; i < l; ++i)
-			mat33_float_rotate(S, c, s, k, i, i, l);
+			mat33_fp_rotate(S, c, s, k, i, i, l);
 
 		for (i = l + 1; i < N; ++i)
-			mat33_float_rotate(S, c, s, k, i, l, i);
+			mat33_fp_rotate(S, c, s, k, i, l, i);
 
 		for (i = 0; i < N; ++i) {
-			float tmp = c * e_vecs[k][i] - s * e_vecs[l][i];
-			e_vecs[l][i] = s * e_vecs[k][i] + c * e_vecs[l][i];
+			fp_t tmp = fp_mul(c, e_vecs[k][i]) -
+				   fp_mul(s, e_vecs[l][i]);
+			e_vecs[l][i] = fp_mul(s, e_vecs[k][i]) +
+				       fp_mul(c, e_vecs[l][i]);
 			e_vecs[k][i] = tmp;
 		}
 
-		ind[k] = mat33_float_maxind(S, k);
-		ind[l] = mat33_float_maxind(S, l);
+		ind[k] = mat33_fp_maxind(S, k);
+		ind[l] = mat33_fp_maxind(S, l);
 
-		sum = 0.0f;
+		sum = FLOAT_TO_FP(0.0f);
 		for (i = 0; i < N; ++i)
 			for (j = i + 1; j < N; ++j)
-				sum += fabsf(S[i][j]);
-
-		if (sum < K_EPSILON)
+				sum += fp_abs(S[i][j]);
+
+		/*
+		 * Note: K_EPSILON(1E-5) is too small to fit into 32-bit
+		 * fixed-point(with 16 fp bits). The minimum positive value is
+		 * 1 which is approximately 1.52E-5, so the
+		 * FLOAT_TO_FP(K_EPSILON) becomes zero.
+		 */
+		if (sum <= FLOAT_TO_FP(K_EPSILON))
 			break;
 	}
 
@@ -138,32 +154,32 @@ void mat33_float_get_eigenbasis(mat33_float_t S, floatv3_t e_vals,
 				m = l;
 
 		if (k != m) {
-			float tmp = e_vals[k];
+			fp_t tmp = e_vals[k];
 			e_vals[k] = e_vals[m];
 			e_vals[m] = tmp;
 
-			mat33_float_swap_rows(e_vecs, k, m);
+			mat33_fp_swap_rows(e_vecs, k, m);
 		}
 	}
 }
 
 /* index of largest off-diagonal element in row k */
-size_t mat33_float_maxind(mat33_float_t A, size_t k)
+size_t mat33_fp_maxind(mat33_fp_t A, size_t k)
 {
 	const size_t N = 3;
 	size_t i, m = k + 1;
 
 	for (i = k + 2; i < N; ++i)
-		if (fabsf(A[k][i]) > fabsf(A[k][m]))
+		if (fp_abs(A[k][i]) > fp_abs(A[k][m]))
 			m = i;
 
 	return m;
 }
 
-void mat33_float_rotate(mat33_float_t A, float c, float s,
-		  size_t k, size_t l, size_t i, size_t j)
+void mat33_fp_rotate(mat33_fp_t A, fp_t c, fp_t s,
+		     size_t k, size_t l, size_t i, size_t j)
 {
-	float tmp = c * A[k][l] - s * A[i][j];
-	A[i][j] = s * A[k][l] + c * A[i][j];
+	fp_t tmp = fp_mul(c, A[k][l]) - fp_mul(s, A[i][j]);
+	A[i][j] = fp_mul(s, A[k][l]) + fp_mul(c, A[i][j]);
 	A[k][l] = tmp;
 }