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, 19 insertions, 18 deletions

diff --git a/common/mat44.c b/common/mat44.c
index 2ca3b69c32..a4232bf8d4 100644
--- a/common/mat44.c
+++ b/common/mat44.c
@@ -10,37 +10,38 @@
 
 #define K_EPSILON 1E-5f
 
-void mat44_float_decompose_lup(mat44_float_t LU, sizev4_t pivot)
+void mat44_fp_decompose_lup(mat44_fp_t LU, sizev4_t pivot)
 {
 	const size_t N = 4;
 	size_t i, j, k;
 
 	for (k = 0; k < N; ++k) {
-		float max = fabsf(LU[k][k]);
+		fp_t max = fp_abs(LU[k][k]);
 		pivot[k] = k;
 		for (j = k + 1; j < N; ++j) {
-			if (max < fabsf(LU[j][k])) {
-				max = fabsf(LU[j][k]);
+			const fp_t lu_jk = fp_abs(LU[j][k]);
+			if (max < lu_jk) {
+				max = lu_jk;
 				pivot[k] = j;
 			}
 		}
 
 		if (pivot[k] != k)
-			mat44_float_swap_rows(LU, k, pivot[k]);
+			mat44_fp_swap_rows(LU, k, pivot[k]);
 
-		if (fabsf(LU[k][k]) < K_EPSILON)
+		if (fp_abs(LU[k][k]) < FLOAT_TO_FP(K_EPSILON))
 			continue;
 
 		for (j = k + 1; j < N; ++j)
-			LU[k][j] /= LU[k][k];
+			LU[k][j] = fp_div_dbz(LU[k][j], LU[k][k]);
 
 		for (i = k + 1; i < N; ++i)
 			for (j = k + 1; j < N; ++j)
-				LU[i][j] -= LU[i][k] * LU[k][j];
+				LU[i][j] -= fp_mul(LU[i][k], LU[k][j]);
 	}
 }
 
-void mat44_float_swap_rows(mat44_float_t A, const size_t i, const size_t j)
+void mat44_fp_swap_rows(mat44_fp_t A, const size_t i, const size_t j)
 {
 	const size_t N = 4;
 	size_t k;
@@ -49,35 +50,35 @@ void mat44_float_swap_rows(mat44_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;
 	}
 }
 
-void mat44_float_solve(mat44_float_t A, floatv4_t x, const floatv4_t b,
-		 const sizev4_t pivot)
+void mat44_fp_solve(mat44_fp_t A, fpv4_t x, const fpv4_t b,
+		    const sizev4_t pivot)
 {
 	const size_t N = 4;
-	floatv4_t b_copy;
+	fpv4_t b_copy;
 	size_t i, k;
 
-	memcpy(b_copy, b, sizeof(floatv4_t));
+	memcpy(b_copy, b, sizeof(fpv4_t));
 
 	for (k = 0; k < N; ++k) {
 		if (pivot[k] != k) {
-			float tmp = b_copy[k];
+			fp_t tmp = b_copy[k];
 			b_copy[k] = b_copy[pivot[k]];
 			b_copy[pivot[k]] = tmp;
 		}
 
 		x[k] = b_copy[k];
 		for (i = 0; i < k; ++i)
-			x[k] -= x[i] * A[k][i];
-		x[k] /= A[k][k];
+			x[k] -= fp_mul(x[i], A[k][i]);
+		x[k] = fp_div_dbz(x[k], A[k][k]);
 	}
 
 	for (k = N; k-- > 0;)
 		for (i = k + 1; i < N; ++i)
-			x[k] -= x[i] * A[k][i];
+			x[k] -= fp_mul(x[i], A[k][i]);
 }