Convert motion sense calculations to fixed point.

Motion sense calculations do not require huge amounts of precision, so
fixed point is plenty accurate.  And fixed point works on Cortex-M0,
which lacks a FPU.

BUG=chrome-os-partner:36126
BRANCH=minnie (samus already works with the FPU, but could grab this if we
       want additional testing)
TEST=manual
    1. Boot system
    2. At EC console: accelinfo on 250
    3. Move lid through several different angles (30 degrees to max open) and
       see that it updates correctly and relatively smoothly.  A few degrees
       of angle jitter is normal.
    4. At several angles, rotate the chromebook around and see that the lid
       angle remains relatively stable.
    5. If the hinge is made normal to the ground (or within 15 degrees of
       vertical), the angle should read 500, since the acceleration vectors
       don't yield good results in that orientation (for either fixed or float
       math).
    And run 'make buildall -j', which tests arc_cos() and lid angle calculations

Change-Id: I70a0d08b8914629a3e21ae5578cbe8e50f29ad68
Signed-off-by: Randall Spangler <rspangler@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/244116
Reviewed-by: Alec Berg <alecaberg@chromium.org>

1 files changed, 70 insertions, 16 deletions

diff --git a/include/math_util.h b/include/math_util.h
index 6ae36a995b..dc0eb431c3 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -8,20 +8,74 @@
 #ifndef __CROS_MATH_UTIL_H
 #define __CROS_MATH_UTIL_H
 
-#ifdef CONFIG_FPU
-typedef float matrix_3x3_t[3][3];
-#else
-typedef int matrix_3x3_t[3][3];
-#endif
+/* Fixed-point type */
+typedef int32_t fp_t;
 
-typedef int vector_3_t[3];
+/* Number of bits left of decimal point for fixed-point */
+#define FP_BITS 16
 
+/* Conversion to/from fixed-point */
+#define INT_TO_FP(x) ((fp_t)(x) << FP_BITS)
+#define FP_TO_INT(x) ((int32_t)((x) >> FP_BITS))
+/* Float to fixed-point, only for compile-time constants and unit tests */
+#define FLOAT_TO_FP(x) ((fp_t)((x) * (float)(1<<FP_BITS)))
+/* Fixed-point to float, for unit tests */
+#define FP_TO_FLOAT(x) ((float)(x) / (float)(1<<FP_BITS))
 
-/* Some useful math functions. */
-#define SQ(x) ((x) * (x))
-#define ABS(x) ((x) >= 0 ? (x) : -(x))
+/*
+ * Fixed-point addition and subtraction can be done directly, because they
+ * work identically.
+ */
+
+/**
+ * Multiplication - return (a * b)
+ */
+static inline fp_t fp_mul(fp_t a, fp_t b)
+{
+	return (fp_t)(((int64_t)a * b) >> FP_BITS);
+}
+
+/**
+ * Division - return (a / b)
+ */
+static inline fp_t fp_div(fp_t a, fp_t b)
+{
+	return (fp_t)(((int64_t)a << FP_BITS) / b);
+}
+
+/**
+ * Square (a * a)
+ */
+static inline fp_t fp_sq(fp_t a)
+{
+	return fp_mul(a, a);
+}
 
-#ifdef CONFIG_FPU
+/**
+ * Absolute value
+ */
+static inline fp_t fp_abs(fp_t a)
+{
+	return (a >= 0 ? a : -a);
+}
+
+/*
+ * Fixed point matrix
+ *
+ * Note that constant matrices MUST be initialized using FLOAT_TO_FP()
+ * or INT_TO_FP() for all non-zero values.
+ */
+typedef fp_t matrix_3x3_t[3][3];
+
+/* Integer vector */
+typedef int vector_3_t[3];
+
+/*
+ * Return absolute value of x.  Note that as a macro expansion, this may have
+ * side effects if x includes function calls, which is why inline functions
+ * like fp_abs() are preferred.
+ */
+#define ABS(x) ((x) >= 0 ? (x) : -(x))
 
 /**
  * Find acos(x) in degrees. Argument is clipped to [-1.0, 1.0].
@@ -30,19 +84,20 @@ typedef int vector_3_t[3];
  *
  * @return acos(x) in degrees.
  */
-float arc_cos(float x);
+fp_t arc_cos(fp_t x);
 
 /**
  * Find the cosine of the angle between two vectors.
  *
+ * The implementation assumes no vector component is greater than
+ * 2^(31 - FP_BITS/2).  For example, 2^23, for FP_BITS=16.
+ *
  * @param v1
  * @param v2
  *
  * @return Cosine of the angle between v1 and v2.
  */
-float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
-
-#endif
+fp_t cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
 
 /**
  * Rotate vector v by rotation matrix R.
@@ -51,8 +106,7 @@ float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2);
  * @param R Rotation matrix.
  * @param res Resultant vector.
  */
-void rotate(const vector_3_t v, const matrix_3x3_t R,
-		vector_3_t res);
+void rotate(const vector_3_t v, const matrix_3x3_t R, vector_3_t res);