summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--common/mag_cal.c121
-rw-r--r--common/mat33.c114
-rw-r--r--common/mat44.c37
-rw-r--r--common/vec3.c21
-rw-r--r--include/mag_cal.h6
-rw-r--r--include/mat33.h22
-rw-r--r--include/mat44.h13
-rw-r--r--include/vec3.h12
-rw-r--r--include/vec4.h3
-rw-r--r--test/build.mk4
-rw-r--r--test/float.tasklist17
-rw-r--r--test/fp.c319
-rw-r--r--test/fp.tasklist17
-rw-r--r--test/test_config.h10
14 files changed, 554 insertions, 162 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;
}
-
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;
}
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]);
}
diff --git a/common/vec3.c b/common/vec3.c
index 4c157467c1..9a3561365a 100644
--- a/common/vec3.c
+++ b/common/vec3.c
@@ -9,25 +9,24 @@
#include "vec3.h"
#include "util.h"
-void floatv3_scalar_mul(floatv3_t v, float c)
+void fpv3_scalar_mul(fpv3_t v, fp_t c)
{
- v[X] *= c;
- v[Y] *= c;
- v[Z] *= c;
+ v[X] = fp_mul(v[X], c);
+ v[Y] = fp_mul(v[Y], c);
+ v[Z] = fp_mul(v[Z], c);
}
-float floatv3_dot(const floatv3_t v, const floatv3_t w)
+fp_t fpv3_dot(const fpv3_t v, const fpv3_t w)
{
- return v[X] * w[X] + v[Y] * w[Y] + v[Z] * w[Z];
+ return fp_mul(v[X], w[X]) + fp_mul(v[Y], w[Y]) + fp_mul(v[Z], w[Z]);
}
-float floatv3_norm_squared(const floatv3_t v)
+fp_t fpv3_norm_squared(const fpv3_t v)
{
- return floatv3_dot(v, v);
+ return fpv3_dot(v, v);
}
-float floatv3_norm(const floatv3_t v)
+fp_t fpv3_norm(const fpv3_t v)
{
- return sqrtf(floatv3_norm_squared(v));
+ return fp_sqrtf(fpv3_norm_squared(v));
}
-
diff --git a/include/mag_cal.h b/include/mag_cal.h
index 753052f55f..b0b35ded6f 100644
--- a/include/mag_cal.h
+++ b/include/mag_cal.h
@@ -29,9 +29,9 @@ struct mag_cal_t {
* | x | y | z | 1 |
* +----+----+----+----+
*/
- mat44_float_t acc;
- floatv4_t acc_w;
- float radius;
+ mat44_fp_t acc;
+ fpv4_t acc_w;
+ fp_t radius;
intv3_t bias;
diff --git a/include/mat33.h b/include/mat33.h
index 0b8b0123cb..fdd7e954ac 100644
--- a/include/mat33.h
+++ b/include/mat33.h
@@ -7,25 +7,25 @@
#define __CROS_EC_MAT_33_H
-#include "vec3.h"
+#include "math_util.h"
#include "util.h"
+#include "vec3.h"
typedef float mat33_float_t[3][3];
typedef size_t sizev3_t[3];
-void init_zero_matrix(mat33_float_t A);
-void init_diagonal_matrix(mat33_float_t A, float x);
-
-void mat33_float_scalar_mul(mat33_float_t A, float c);
+void mat33_fp_init_zero(mat33_fp_t A);
+void mat33_fp_init_diagonal(mat33_fp_t A, fp_t x);
-void mat33_float_swap_rows(mat33_float_t A, const size_t i, const size_t j);
+void mat33_fp_scalar_mul(mat33_fp_t A, fp_t c);
-void mat33_float_get_eigenbasis(mat33_float_t S, floatv3_t eigenvals,
- mat33_float_t eigenvecs);
+void mat33_fp_swap_rows(mat33_fp_t A, const size_t i, const size_t j);
-size_t mat33_float_maxind(mat33_float_t A, size_t k);
+void mat33_fp_get_eigenbasis(mat33_fp_t S, fpv3_t eigenvals,
+ mat33_fp_t eigenvecs);
-void mat33_float_rotate(mat33_float_t A, float c, float s,
- size_t k, size_t l, size_t i, size_t j);
+size_t mat33_fp_maxind(mat33_fp_t A, size_t k);
+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);
#endif /* __CROS_EC_MAT_33_H */
diff --git a/include/mat44.h b/include/mat44.h
index d8a8209055..2faa093c8e 100644
--- a/include/mat44.h
+++ b/include/mat44.h
@@ -8,17 +8,18 @@
#define __CROS_EC_MAT_44_H
-#include "vec4.h"
+#include "math_util.h"
#include "util.h"
+#include "vec4.h"
typedef float mat44_float_t[4][4];
+typedef fp_t mat44_fp_t[4][4];
typedef size_t sizev4_t[4];
-void mat44_float_decompose_lup(mat44_float_t LU, sizev4_t pivot);
-
-void mat44_float_swap_rows(mat44_float_t A, const size_t i, const size_t j);
+void mat44_fp_decompose_lup(mat44_fp_t LU, sizev4_t pivot);
-void mat44_float_solve(mat44_float_t A, floatv4_t x, const floatv4_t b,
- const sizev4_t pivot);
+void mat44_fp_swap_rows(mat44_fp_t A, const size_t i, const size_t j);
+void mat44_fp_solve(mat44_fp_t A, fpv4_t x, const fpv4_t b,
+ const sizev4_t pivot);
#endif /* __CROS_EC_MAT_44_H */
diff --git a/include/vec3.h b/include/vec3.h
index 367956b2a1..39514157dd 100644
--- a/include/vec3.h
+++ b/include/vec3.h
@@ -7,11 +7,13 @@
#ifndef __CROS_EC_VEC_3_H
#define __CROS_EC_VEC_3_H
-typedef float floatv3_t[3];
+#include "math_util.h"
-void floatv3_scalar_mul(floatv3_t v, float c);
-float floatv3_dot(const floatv3_t v, const floatv3_t w);
-float floatv3_norm_squared(const floatv3_t v);
-float floatv3_norm(const floatv3_t v);
+typedef float floatv3_t[3];
+typedef fp_t fpv3_t[3];
+void fpv3_scalar_mul(fpv3_t v, fp_t c);
+fp_t fpv3_dot(const fpv3_t v, const fpv3_t w);
+fp_t fpv3_norm_squared(const fpv3_t v);
+fp_t fpv3_norm(const fpv3_t v);
#endif /* __CROS_EC_VEC_3_H */
diff --git a/include/vec4.h b/include/vec4.h
index 05257e584b..b35431c098 100644
--- a/include/vec4.h
+++ b/include/vec4.h
@@ -7,7 +7,10 @@
#define __CROS_EC_VEC_4_H
+#include "math_util.h"
+
typedef float floatv4_t[4];
+typedef fp_t fpv4_t[4];
#endif /* __CROS_EC_VEC_4_H */
diff --git a/test/build.mk b/test/build.mk
index b16b15e702..31cec03c9a 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -29,6 +29,8 @@ test-list-host += entropy
test-list-host += extpwr_gpio
test-list-host += fan
test-list-host += flash
+test-list-host += float
+test-list-host += fp
test-list-host += hooks
test-list-host += host_command
test-list-host += inductive_charging
@@ -124,4 +126,6 @@ usb_pd_rev30-y=usb_pd.o
utils-y=utils.o
utils_str-y=utils_str.o
vboot-y=vboot.o
+float-y=fp.o
+fp-y=fp.o
x25519-y=x25519.o
diff --git a/test/float.tasklist b/test/float.tasklist
new file mode 100644
index 0000000000..5e26d7102e
--- /dev/null
+++ b/test/float.tasklist
@@ -0,0 +1,17 @@
+/* Copyright 2018 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/**
+ * List of enabled tasks in the priority order
+ *
+ * The first one has the lowest priority.
+ *
+ * For each task, use the macro TASK_TEST(n, r, d, s) where :
+ * 'n' in the name of the task
+ * 'r' in the main routine of the task
+ * 'd' in an opaque parameter passed to the routine at startup
+ * 's' is the stack size in bytes; must be a multiple of 8
+ */
+#define CONFIG_TEST_TASK_LIST /* No test task */
diff --git a/test/fp.c b/test/fp.c
new file mode 100644
index 0000000000..296a78e4df
--- /dev/null
+++ b/test/fp.c
@@ -0,0 +1,319 @@
+/* Copyright 2018 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/*
+ * Explicitly include common.h to populate predefined macros in test_config.h
+ * early. e.g. CONFIG_FPU, which is needed in math_util.h
+ */
+#include "common.h"
+
+#include "mat33.h"
+#include "mat44.h"
+#include "math_util.h"
+#include "test_util.h"
+#include "vec3.h"
+
+#if defined(TEST_FP) && !defined(CONFIG_FPU)
+#define NORM_TOLERANCE FLOAT_TO_FP(0.01f)
+#define NORM_SQUARED_TOLERANCE FLOAT_TO_FP(0.0f)
+#define DOT_TOLERANCE FLOAT_TO_FP(0.001f)
+#define SCALAR_MUL_TOLERANCE FLOAT_TO_FP(0.005f)
+#define EIGENBASIS_TOLERANCE FLOAT_TO_FP(0.03f)
+#define LUP_TOLERANCE FLOAT_TO_FP(0.0005f)
+#define SOLVE_TOLERANCE FLOAT_TO_FP(0.0005f)
+#elif defined(TEST_FLOAT) && defined(CONFIG_FPU)
+#define NORM_TOLERANCE FLOAT_TO_FP(0.0f)
+#define NORM_SQUARED_TOLERANCE FLOAT_TO_FP(0.0f)
+#define DOT_TOLERANCE FLOAT_TO_FP(0.0f)
+#define SCALAR_MUL_TOLERANCE FLOAT_TO_FP(0.005f)
+#define EIGENBASIS_TOLERANCE FLOAT_TO_FP(0.02f)
+#define LUP_TOLERANCE FLOAT_TO_FP(0.0f)
+#define SOLVE_TOLERANCE FLOAT_TO_FP(0.0f)
+#else
+#error "No such test configuration."
+#endif
+
+#define IS_FPV3_VECTOR_EQUAL(a, b, diff) \
+ (IS_FP_EQUAL((a)[0], (b)[0], (diff)) && \
+ IS_FP_EQUAL((a)[1], (b)[1], (diff)) && \
+ IS_FP_EQUAL((a)[2], (b)[2], (diff)))
+#define IS_FP_EQUAL(a, b, diff) ((a) >= ((b)-diff) && (a) <= ((b) + diff))
+#define IS_FLOAT_EQUAL(a, b, diff) IS_FP_EQUAL(a, b, diff)
+
+static int test_fpv3_scalar_mul(void)
+{
+ const int N = 3;
+ const float s = 2.0f;
+ floatv3_t r = {1.0f, 2.0f, 4.0f};
+ /* Golden result g = s * r; */
+ const floatv3_t g = {2.0f, 4.0f, 8.0f};
+ int i;
+ fpv3_t a;
+
+ for (i = 0; i < N; ++i)
+ a[i] = FLOAT_TO_FP(r[i]);
+
+ fpv3_scalar_mul(a, FLOAT_TO_FP(s));
+
+ for (i = 0; i < N; ++i)
+ TEST_ASSERT(IS_FP_EQUAL(a[i], FLOAT_TO_FP(g[i]), 0));
+
+ return EC_SUCCESS;
+}
+
+static int test_fpv3_dot(void)
+{
+ const int N = 3;
+ int i;
+ floatv3_t a = {1.8f, 2.12f, 4.12f};
+ floatv3_t b = {3.1f, 4.3f, 5.8f};
+ /* Golden result g = dot(a, b) */
+ float g = 38.592f;
+ fpv3_t fpa, fpb;
+
+ for (i = 0; i < N; ++i) {
+ fpa[i] = FLOAT_TO_FP(a[i]);
+ fpb[i] = FLOAT_TO_FP(b[i]);
+ }
+
+ TEST_ASSERT(IS_FP_EQUAL(fpv3_dot(fpa, fpb), FLOAT_TO_FP(g),
+ DOT_TOLERANCE));
+
+ return EC_SUCCESS;
+}
+
+static int test_fpv3_norm_squared(void)
+{
+ const int N = 3;
+ int i;
+ floatv3_t a = {3.0f, 4.0f, 5.0f};
+ /* Golden result g = norm_squared(a) */
+ float g = 50.0f;
+ fpv3_t fpa;
+
+ for (i = 0; i < N; ++i)
+ fpa[i] = FLOAT_TO_FP(a[i]);
+
+ TEST_ASSERT(IS_FP_EQUAL(fpv3_norm_squared(fpa), FLOAT_TO_FP(g),
+ NORM_SQUARED_TOLERANCE));
+
+ return EC_SUCCESS;
+}
+
+static int test_fpv3_norm(void)
+{
+ const int N = 3;
+ floatv3_t a = {3.1f, 4.2f, 5.3f};
+ /* Golden result g = norm(a) */
+ float g = 7.439085483551025390625f;
+ int i;
+ fpv3_t fpa;
+
+ for (i = 0; i < N; ++i)
+ fpa[i] = FLOAT_TO_FP(a[i]);
+
+ TEST_ASSERT(
+ IS_FP_EQUAL(fpv3_norm(fpa), FLOAT_TO_FP(g), NORM_TOLERANCE));
+
+ return EC_SUCCESS;
+}
+
+static int test_mat33_fp_init_zero(void)
+{
+ const int N = 3;
+ int i, j;
+ mat33_fp_t a;
+
+ for (i = 0; i < N; ++i)
+ for (j = 0; j < N; ++j)
+ a[i][j] = FLOAT_TO_FP(55.66f);
+
+ mat33_fp_init_zero(a);
+
+ for (i = 0; i < N; ++i)
+ for (j = 0; j < N; ++j)
+ TEST_ASSERT(a[i][j] == FLOAT_TO_FP(0.0f));
+
+ return EC_SUCCESS;
+}
+
+static int test_mat33_fp_init_diagonal(void)
+{
+ const int N = 3;
+ int i, j;
+ mat33_fp_t a;
+ fp_t v = FLOAT_TO_FP(-3.45f);
+
+ for (i = 0; i < N; ++i)
+ for (j = 0; j < N; ++j)
+ a[i][j] = FLOAT_TO_FP(55.66f);
+
+ mat33_fp_init_diagonal(a, v);
+
+ for (i = 0; i < N; ++i)
+ for (j = 0; j < N; ++j) {
+ if (i == j)
+ TEST_ASSERT(a[i][j] == v);
+ else
+ TEST_ASSERT(a[i][j] == FLOAT_TO_FP(0.0f));
+ }
+
+ return EC_SUCCESS;
+}
+
+static int test_mat33_fp_scalar_mul(void)
+{
+ const int N = 3;
+ float scale = 3.11f;
+ mat33_float_t a = {
+ {1.0f, 2.0f, 3.0f},
+ {1.1f, 2.2f, 3.3f},
+ {0.38f, 13.2f, 88.3f}
+ };
+ /* Golden result g = scalar_mul(a, scale) */
+ mat33_float_t g = {{3.11f, 6.22f, 9.33f},
+ {3.421f, 6.842f, 10.263f},
+ {1.18179988861083984375f, 41.051998138427734375f,
+ 274.613006591796875f}
+ };
+ int i, j;
+ mat33_fp_t fpa;
+
+ for (i = 0; i < N; ++i)
+ for (j = 0; j < N; ++j)
+ fpa[i][j] = FLOAT_TO_FP(a[i][j]);
+
+ mat33_fp_scalar_mul(fpa, FLOAT_TO_FP(scale));
+
+ for (i = 0; i < N; ++i)
+ for (j = 0; j < N; ++j)
+ TEST_ASSERT(IS_FP_EQUAL(fpa[i][j], FLOAT_TO_FP(g[i][j]),
+ SCALAR_MUL_TOLERANCE));
+
+ return EC_SUCCESS;
+}
+
+static int test_mat33_fp_get_eigenbasis(void)
+{
+ mat33_fp_t s = {
+ {FLOAT_TO_FP(4.0f), FLOAT_TO_FP(2.0f), FLOAT_TO_FP(2.0f)},
+ {FLOAT_TO_FP(2.0f), FLOAT_TO_FP(4.0f), FLOAT_TO_FP(2.0f)},
+ {FLOAT_TO_FP(2.0f), FLOAT_TO_FP(2.0f), FLOAT_TO_FP(4.0f)}
+ };
+ fpv3_t e_vals;
+ mat33_fp_t e_vecs;
+ int i, j;
+
+ /* Golden result from float version. */
+ mat33_fp_t gold_vecs = {
+ {FLOAT_TO_FP(0.55735206f), FLOAT_TO_FP(0.55735206f),
+ FLOAT_TO_FP(0.55735206f)},
+ {FLOAT_TO_FP(0.70710677f), FLOAT_TO_FP(-0.70710677f),
+ FLOAT_TO_FP(0.0f)},
+ {FLOAT_TO_FP(-0.40824828f), FLOAT_TO_FP(-0.40824828f),
+ FLOAT_TO_FP(0.81649655f)}
+ };
+ fpv3_t gold_vals = {FLOAT_TO_FP(8.0f), FLOAT_TO_FP(2.0f),
+ FLOAT_TO_FP(2.0f)};
+
+ mat33_fp_get_eigenbasis(s, e_vals, e_vecs);
+
+ for (i = 0; i < 3; ++i) {
+ TEST_ASSERT(IS_FP_EQUAL(gold_vals[i], e_vals[i],
+ EIGENBASIS_TOLERANCE));
+ for (j = 0; j < 3; ++j) {
+ TEST_ASSERT(IS_FP_EQUAL(gold_vecs[i][j], e_vecs[i][j],
+ EIGENBASIS_TOLERANCE));
+ }
+ }
+
+ return EC_SUCCESS;
+}
+
+static int test_mat44_fp_decompose_lup(void)
+{
+ int i, j;
+ sizev4_t pivot;
+ mat44_fp_t fpa = {
+ {FLOAT_TO_FP(11.0f), FLOAT_TO_FP(9.0f),
+ FLOAT_TO_FP(24.0f), FLOAT_TO_FP(2.0f)},
+ {FLOAT_TO_FP(1.0f), FLOAT_TO_FP(5.0f),
+ FLOAT_TO_FP(2.0f), FLOAT_TO_FP(6.0f)},
+ {FLOAT_TO_FP(3.0f), FLOAT_TO_FP(17.0f),
+ FLOAT_TO_FP(18.0f), FLOAT_TO_FP(1.0f)},
+ {FLOAT_TO_FP(2.0f), FLOAT_TO_FP(5.0f),
+ FLOAT_TO_FP(7.0f), FLOAT_TO_FP(1.0f)}
+ };
+ /* Golden result from float version. */
+ mat44_fp_t gold_lu = {
+ {FLOAT_TO_FP(11.0f), FLOAT_TO_FP(0.8181818f),
+ FLOAT_TO_FP(2.1818182f), FLOAT_TO_FP(0.18181819f)},
+ {FLOAT_TO_FP(3.0f), FLOAT_TO_FP(14.545454),
+ FLOAT_TO_FP(0.7875f), FLOAT_TO_FP(0.03125f)},
+ {FLOAT_TO_FP(1.0f), FLOAT_TO_FP(4.181818f),
+ FLOAT_TO_FP(-3.4750001f), FLOAT_TO_FP(-1.6366906f)},
+ {FLOAT_TO_FP(2.0f), FLOAT_TO_FP(3.3636365f),
+ FLOAT_TO_FP(-0.012500286f), FLOAT_TO_FP(0.5107909f)}
+ };
+ sizev4_t gold_pivot = {0, 2, 2, 3};
+
+ mat44_fp_decompose_lup(fpa, pivot);
+
+ for (i = 0; i < 4; ++i) {
+ TEST_ASSERT(gold_pivot[i] == pivot[i]);
+ for (j = 0; j < 4; ++j)
+ TEST_ASSERT(IS_FP_EQUAL(gold_lu[i][j], fpa[i][j],
+ LUP_TOLERANCE));
+ }
+
+ return EC_SUCCESS;
+}
+
+static int test_mat44_fp_solve(void)
+{
+ int i;
+ fpv4_t x;
+ mat44_fp_t A = {
+ {FLOAT_TO_FP(11.0f), FLOAT_TO_FP(0.8181818f),
+ FLOAT_TO_FP(2.1818182f), FLOAT_TO_FP(0.18181819f)},
+ {FLOAT_TO_FP(3.0f), FLOAT_TO_FP(14.545454),
+ FLOAT_TO_FP(0.7875f), FLOAT_TO_FP(0.03125f)},
+ {FLOAT_TO_FP(1.0f), FLOAT_TO_FP(4.181818f),
+ FLOAT_TO_FP(-3.4750001f), FLOAT_TO_FP(-1.6366906f)},
+ {FLOAT_TO_FP(2.0f), FLOAT_TO_FP(3.3636365f),
+ FLOAT_TO_FP(-0.012500286f), FLOAT_TO_FP(0.5107909f)}
+ };
+ sizev4_t pivot = {0, 2, 2, 3};
+ fpv4_t b = {FLOAT_TO_FP(1.0f), FLOAT_TO_FP(3.3f), FLOAT_TO_FP(0.8f),
+ FLOAT_TO_FP(8.9f)};
+ /* Golden result from float version. */
+ fpv4_t gold_x = {FLOAT_TO_FP(-43.50743f), FLOAT_TO_FP(-21.459526f),
+ FLOAT_TO_FP(26.629248f), FLOAT_TO_FP(16.80776f)};
+
+ mat44_fp_solve(A, x, b, pivot);
+
+ for (i = 0; i < 4; ++i)
+ TEST_ASSERT(IS_FP_EQUAL(gold_x[i], x[i], SOLVE_TOLERANCE));
+
+ return EC_SUCCESS;
+}
+
+void run_test(void)
+{
+ test_reset();
+
+ RUN_TEST(test_fpv3_scalar_mul);
+ RUN_TEST(test_fpv3_dot);
+ RUN_TEST(test_fpv3_norm_squared);
+ RUN_TEST(test_fpv3_norm);
+ RUN_TEST(test_mat33_fp_init_zero);
+ RUN_TEST(test_mat33_fp_init_diagonal);
+ RUN_TEST(test_mat33_fp_scalar_mul);
+ RUN_TEST(test_mat33_fp_get_eigenbasis);
+ RUN_TEST(test_mat44_fp_decompose_lup);
+ RUN_TEST(test_mat44_fp_solve);
+
+ test_print_result();
+}
diff --git a/test/fp.tasklist b/test/fp.tasklist
new file mode 100644
index 0000000000..5e26d7102e
--- /dev/null
+++ b/test/fp.tasklist
@@ -0,0 +1,17 @@
+/* Copyright 2018 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/**
+ * List of enabled tasks in the priority order
+ *
+ * The first one has the lowest priority.
+ *
+ * For each task, use the macro TASK_TEST(n, r, d, s) where :
+ * 'n' in the name of the task
+ * 'r' in the main routine of the task
+ * 'd' in an opaque parameter passed to the routine at startup
+ * 's' is the stack size in bytes; must be a multiple of 8
+ */
+#define CONFIG_TEST_TASK_LIST /* No test task */
diff --git a/test/test_config.h b/test/test_config.h
index 8a972f10c3..a53ea87656 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -54,6 +54,16 @@
#define CONFIG_MATH_UTIL
#endif
+#ifdef TEST_FLOAT
+#define CONFIG_FPU
+#define CONFIG_MAG_CALIBRATE
+#endif
+
+#ifdef TEST_FP
+#undef CONFIG_FPU
+#define CONFIG_MAG_CALIBRATE
+#endif
+
#ifdef TEST_MOTION_LID
#define CONFIG_LID_ANGLE
#define CONFIG_LID_ANGLE_INVALID_CHECK
View Lorry Controller Status