diff options
author | Yilun Lin <yllin@google.com> | 2018-10-04 10:19:57 +0800 |
---|---|---|
committer | chrome-bot <chrome-bot@chromium.org> | 2018-10-04 12:55:53 -0700 |
commit | 315aaca9467f49bc432ef5f2de9c0e3bb56f0251 (patch) | |
tree | 3ea739aca1db340e93b9daee7958d5afc5c9f31f | |
parent | ece03ab4d09b157c5e6f3c4fe0446678c0d8684b (diff) | |
download | chrome-ec-315aaca9467f49bc432ef5f2de9c0e3bb56f0251.tar.gz |
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>
-rw-r--r-- | common/mag_cal.c | 121 | ||||
-rw-r--r-- | common/mat33.c | 114 | ||||
-rw-r--r-- | common/mat44.c | 37 | ||||
-rw-r--r-- | common/vec3.c | 21 | ||||
-rw-r--r-- | include/mag_cal.h | 6 | ||||
-rw-r--r-- | include/mat33.h | 22 | ||||
-rw-r--r-- | include/mat44.h | 13 | ||||
-rw-r--r-- | include/vec3.h | 12 | ||||
-rw-r--r-- | include/vec4.h | 3 | ||||
-rw-r--r-- | test/build.mk | 4 | ||||
-rw-r--r-- | test/float.tasklist | 17 | ||||
-rw-r--r-- | test/fp.c | 319 | ||||
-rw-r--r-- | test/fp.tasklist | 17 | ||||
-rw-r--r-- | test/test_config.h | 10 |
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 |