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>

10 files changed, 258 insertions, 125 deletions

diff --git a/board/samus/board.c b/board/samus/board.c
index 704b3102b8..9dfcd1a8b4 100644
--- a/board/samus/board.c
+++ b/board/samus/board.c
@@ -249,15 +249,15 @@ struct kxcj9_data g_kxcj9_data;
 /* Four Motion sensors */
 /* Matrix to rotate accelrator into standard reference frame */
 const matrix_3x3_t base_standard_ref = {
-	{-1,  0,  0},
-	{ 0, -1,  0},
-	{ 0,  0, -1}
+	{FLOAT_TO_FP(-1),  0,  0},
+	{ 0, FLOAT_TO_FP(-1),  0},
+	{ 0,  0, FLOAT_TO_FP(-1)}
 };
 
 const matrix_3x3_t lid_standard_ref = {
-	{ 0,  1,  0},
-	{-1,  0,  0},
-	{ 0,  0, -1}
+	{ 0,  FLOAT_TO_FP(1),  0},
+	{FLOAT_TO_FP(-1),  0,  0},
+	{ 0,  0, FLOAT_TO_FP(-1)}
 };
 
 struct motion_sensor_t motion_sensors[] = {
@@ -289,14 +289,14 @@ const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
 const struct accel_orientation acc_orient = {
 	/* Hinge aligns with y axis. */
 	.rot_hinge_90 = {
-		{ 1,  0,  0},
-		{ 0,  1,  0},
-		{ 0,  0,  1}
+		{ FLOAT_TO_FP(1),  0,  0},
+		{ 0,  FLOAT_TO_FP(1),  0},
+		{ 0,  0,  FLOAT_TO_FP(1)}
 	},
 	.rot_hinge_180 = {
-		{ 1,  0,  0},
-		{ 0,  1,  0},
-		{ 0,  0,  1}
+		{ FLOAT_TO_FP(1),  0,  0},
+		{ 0,  FLOAT_TO_FP(1),  0},
+		{ 0,  0,  FLOAT_TO_FP(1)}
 	},
 	.hinge_axis = {0, 1, 0},
 };
diff --git a/common/lid_angle.c b/common/lid_angle.c
index a55cd5d291..eb6f6c94b5 100644
--- a/common/lid_angle.c
+++ b/common/lid_angle.c
@@ -59,7 +59,7 @@ static const int kb_wake_small_angle = 13;
  *
  * @return true/false
  */
-static int lid_in_range_to_accept_keys(float ang)
+static int lid_in_range_to_accept_keys(int ang)
 {
 	/*
 	 * If the keyboard wake large angle is min or max, then this
@@ -82,7 +82,7 @@ static int lid_in_range_to_accept_keys(float ang)
  *
  * @return true/false
  */
-static int lid_in_range_to_ignore_keys(float ang)
+static int lid_in_range_to_ignore_keys(int ang)
 {
 	/*
 	 * If the keyboard wake large angle is min or max, then this
@@ -114,9 +114,9 @@ void lid_angle_set_kb_wake_angle(int ang)
 	kb_wake_large_angle = ang;
 }
 
-void lidangle_keyscan_update(float lid_ang)
+void lidangle_keyscan_update(int lid_ang)
 {
-	static float lidangle_buffer[KEY_SCAN_LID_ANGLE_BUFFER_SIZE];
+	static int lidangle_buffer[KEY_SCAN_LID_ANGLE_BUFFER_SIZE];
 	static int index;
 
 	int i;
diff --git a/common/math_util.c b/common/math_util.c
index 56688283f4..120d13d63f 100644
--- a/common/math_util.c
+++ b/common/math_util.c
@@ -10,44 +10,56 @@
 #include "math_util.h"
 #include "util.h"
 
+/* Some useful math functions.  Use with integers only! */
+#define SQ(x) ((x) * (x))
+
 /* For cosine lookup table, define the increment and the size of the table. */
 #define COSINE_LUT_INCR_DEG	5
 #define COSINE_LUT_SIZE		((180 / COSINE_LUT_INCR_DEG) + 1)
 
-#ifdef CONFIG_FPU
 /* Lookup table for the value of cosine from 0 degrees to 180 degrees. */
-static const float cos_lut[] = {
-	 1.00000,  0.99619,  0.98481,  0.96593,  0.93969,
-	 0.90631,  0.86603,  0.81915,  0.76604,  0.70711,
-	 0.64279,  0.57358,  0.50000,  0.42262,  0.34202,
-	 0.25882,  0.17365,  0.08716,  0.00000, -0.08716,
-	-0.17365, -0.25882, -0.34202, -0.42262, -0.50000,
-	-0.57358, -0.64279, -0.70711, -0.76604, -0.81915,
-	-0.86603, -0.90631, -0.93969, -0.96593, -0.98481,
-	-0.99619, -1.00000,
+static const fp_t cos_lut[] = {
+	FLOAT_TO_FP( 1.00000), FLOAT_TO_FP( 0.99619), FLOAT_TO_FP( 0.98481),
+	FLOAT_TO_FP( 0.96593), FLOAT_TO_FP( 0.93969), FLOAT_TO_FP( 0.90631),
+	FLOAT_TO_FP( 0.86603), FLOAT_TO_FP( 0.81915), FLOAT_TO_FP( 0.76604),
+	FLOAT_TO_FP( 0.70711), FLOAT_TO_FP( 0.64279), FLOAT_TO_FP( 0.57358),
+	FLOAT_TO_FP( 0.50000), FLOAT_TO_FP( 0.42262), FLOAT_TO_FP( 0.34202),
+	FLOAT_TO_FP( 0.25882), FLOAT_TO_FP( 0.17365), FLOAT_TO_FP( 0.08716),
+	FLOAT_TO_FP( 0.00000), FLOAT_TO_FP(-0.08716), FLOAT_TO_FP(-0.17365),
+	FLOAT_TO_FP(-0.25882), FLOAT_TO_FP(-0.34202), FLOAT_TO_FP(-0.42262),
+	FLOAT_TO_FP(-0.50000), FLOAT_TO_FP(-0.57358), FLOAT_TO_FP(-0.64279),
+	FLOAT_TO_FP(-0.70711), FLOAT_TO_FP(-0.76604), FLOAT_TO_FP(-0.81915),
+	FLOAT_TO_FP(-0.86603), FLOAT_TO_FP(-0.90631), FLOAT_TO_FP(-0.93969),
+	FLOAT_TO_FP(-0.96593), FLOAT_TO_FP(-0.98481), FLOAT_TO_FP(-0.99619),
+	FLOAT_TO_FP(-1.00000),
 };
 BUILD_ASSERT(ARRAY_SIZE(cos_lut) == COSINE_LUT_SIZE);
 
 
-float arc_cos(float x)
+fp_t arc_cos(fp_t x)
 {
 	int i;
 
 	/* Cap x if out of range. */
-	if (x < -1.0)
-		x = -1.0;
-	else if (x > 1.0)
-		x = 1.0;
+	if (x < FLOAT_TO_FP(-1.0))
+		x = FLOAT_TO_FP(-1.0);
+	else if (x > FLOAT_TO_FP(1.0))
+		x = FLOAT_TO_FP(1.0);
 
 	/*
 	 * Increment through lookup table to find index and then linearly
 	 * interpolate for precision.
 	 */
 	/* TODO(crosbug.com/p/25600): Optimize with binary search. */
-	for (i = 0; i < COSINE_LUT_SIZE-1; i++)
-		if (x >= cos_lut[i+1])
-			return COSINE_LUT_INCR_DEG *
-			(i + (cos_lut[i] - x) / (cos_lut[i] - cos_lut[i+1]));
+	for (i = 0; i < COSINE_LUT_SIZE-1; i++) {
+		if (x >= cos_lut[i+1]) {
+			const fp_t interp = fp_div(cos_lut[i] - x,
+						   cos_lut[i] - cos_lut[i + 1]);
+
+			return fp_mul(INT_TO_FP(COSINE_LUT_INCR_DEG),
+				      INT_TO_FP(i) + interp);
+		}
+	}
 
 	/*
 	 * Shouldn't be possible to get here because inputs are clipped to
@@ -59,56 +71,114 @@ float arc_cos(float x)
 	return 0;
 }
 
+/**
+ * Integer square root.
+ */
+int int_sqrtf(int64_t x)
+{
+	int rmax = 0x7fffffff;
+	int rmin = 0;
+
+	/* Short cut if x is 32-bit value */
+	if (x < rmax)
+		rmax = 0x7fff;
+
+	/*
+	 * Just binary-search.  There are better algorithms, but we call this
+	 * infrequently enough it doesn't matter.
+	 */
+	if (x <= 0)
+		return 0;  /* Yeah, for imaginary numbers too */
+	else if (x > (int64_t)rmax * rmax)
+		return rmax;
+
+	while (1) {
+		int r = (rmax + rmin) / 2;
+		int64_t r2 = (int64_t)r * r;
+
+		if (r2 > x) {
+			/* Guessed too high */
+			rmax = r;
+		} else if (r2 < x) {
+			/* Guessed too low.  Watch out for rounding! */
+			if (rmin == r)
+				return r;
+
+			rmin = r;
+		} else {
+			/* Bullseye! */
+			return r;
+		}
+	}
+}
+
 int vector_magnitude(const vector_3_t v)
 {
-	return sqrtf(SQ(v[0]) + SQ(v[1]) + SQ(v[2]));
+	int64_t sum =   (int64_t)v[0] * v[0] +
+			(int64_t)v[1] * v[1] +
+			(int64_t)v[2] * v[2];
+
+	return int_sqrtf(sum);
 }
 
-float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2)
+fp_t cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2)
 {
-	int dotproduct;
-	float denominator;
+	int64_t dotproduct;
+	int64_t denominator;
 
 	/*
 	 * Angle between two vectors is acos(A dot B / |A|*|B|). To return
 	 * cosine of angle between vectors, then don't do acos operation.
 	 */
 
-	dotproduct = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
+	dotproduct =	(int64_t)v1[0] * v2[0] +
+			(int64_t)v1[1] * v2[1] +
+			(int64_t)v1[2] * v2[2];
 
-	denominator = vector_magnitude(v1) * vector_magnitude(v2);
+	denominator = (int64_t)vector_magnitude(v1) * vector_magnitude(v2);
 
 	/* Check for divide by 0 although extremely unlikely. */
-	if (ABS(denominator) < 0.01F)
-		return 0.0;
+	if (!denominator)
+		return 0;
 
-	return (float)dotproduct / (denominator);
+	/*
+	 * We must shift the dot product first, so that we can represent
+	 * fractions.  The answer is always a number with magnitude < 1.0, so
+	 * if we don't shift, it will always round down to 0.
+	 *
+	 * Note that overflow is possible if the dot product is large (that is,
+	 * if the vector components are of size (31 - FP_BITS/2) bits.  If that
+	 * ever becomes a problem, we could detect this by counting the leading
+	 * zeroes of the dot product and shifting the denominator down
+	 * partially instead of shifting the dot product up.  With the current
+	 * FP_BITS=16, that happens if the vector components are ~2^23.  Which
+	 * we're a long way away from; the vector components used in
+	 * accelerometer calculations are ~2^11.
+	 */
+	return (dotproduct << FP_BITS) / denominator;
 }
-#endif
 
 /*
  * rotate a vector v
  *  - support input v and output res are the same 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)
 {
-	vector_3_t t;
-
-	/* copy input v to temp vector t */
-	t[0] = v[0];
-	t[1] = v[1];
-	t[2] = v[2];
-
-	/* start rotate */
-	res[0] =	t[0] * R[0][0] +
-			t[1] * R[1][0] +
-			t[2] * R[2][0];
-	res[1] =	t[0] * R[0][1] +
-			t[1] * R[1][1] +
-			t[2] * R[2][1];
-	res[2] =	t[0] * R[0][2] +
-			t[1] * R[1][2] +
-			t[2] * R[2][2];
+	int64_t t[3];
+
+	/* Rotate */
+	t[0] =	(int64_t)v[0] * R[0][0] +
+		(int64_t)v[1] * R[1][0] +
+		(int64_t)v[2] * R[2][0];
+	t[1] =	(int64_t)v[0] * R[0][1] +
+		(int64_t)v[1] * R[1][1] +
+		(int64_t)v[2] * R[2][1];
+	t[2] =	(int64_t)v[0] * R[0][2] +
+		(int64_t)v[1] * R[1][2] +
+		(int64_t)v[2] * R[2][2];
+
+	/* Scale by fixed point shift when writing back to result */
+	res[0] = t[0] >> FP_BITS;
+	res[1] = t[1] >> FP_BITS;
+	res[2] = t[2] >> FP_BITS;
 }
-
diff --git a/common/motion_lid.c b/common/motion_lid.c
index 4b991dee6b..0dfb8763d7 100644
--- a/common/motion_lid.c
+++ b/common/motion_lid.c
@@ -32,7 +32,7 @@ enum {
 };
 
 /* Current acceleration vectors and current lid angle. */
-static float lid_angle_deg;
+static int lid_angle_deg;
 static int lid_angle_is_reliable;
 
 /*
@@ -41,7 +41,7 @@ static int lid_angle_is_reliable;
  * efficiency, value is given unit-less, so if you want the threshold to be
  * at 15 degrees, the value would be cos(15 deg) = 0.96593.
  */
-#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F
+#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD FLOAT_TO_FP(0.96593)
 
 
 /* Pointer to constant acceleration orientation data. */
@@ -61,11 +61,12 @@ struct motion_sensor_t *accel_lid = &motion_sensors[CONFIG_SENSOR_LID];
  * @return flag representing if resulting lid angle calculation is reliable.
  */
 static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
-		float *lid_angle)
+			       int *lid_angle)
 {
 	vector_3_t v;
-	float ang_lid_to_base, ang_lid_90, ang_lid_270;
-	float lid_to_base, base_to_hinge;
+	fp_t ang_lid_to_base, cos_lid_90, cos_lid_270;
+	fp_t lid_to_base, base_to_hinge;
+	fp_t denominator;
 	int reliable = 1;
 
 	/*
@@ -82,64 +83,66 @@ static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid,
 	 * If hinge aligns too closely with gravity, then result may be
 	 * unreliable.
 	 */
-	if (ABS(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD)
+	if (fp_abs(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD)
 		reliable = 0;
 
-	base_to_hinge = SQ(base_to_hinge);
+	base_to_hinge = fp_sq(base_to_hinge);
 
 	/* Check divide by 0. */
-	if (ABS(1.0F - base_to_hinge) < 0.01F) {
-		*lid_angle = 0.0;
+	denominator = FLOAT_TO_FP(1.0) - base_to_hinge;
+	if (fp_abs(denominator) < FLOAT_TO_FP(0.01)) {
+		*lid_angle = 0;
 		return 0;
 	}
 
-	ang_lid_to_base = arc_cos(
-			(lid_to_base - base_to_hinge) / (1 - base_to_hinge));
+	ang_lid_to_base = arc_cos(fp_div(lid_to_base - base_to_hinge,
+					 denominator));
 
 	/*
 	 * The previous calculation actually has two solutions, a positive and
 	 * a negative solution. To figure out the sign of the answer, calculate
-	 * the angle between the actual lid angle and the estimated vector if
-	 * the lid were open to 90 deg, ang_lid_90. Also calculate the angle
-	 * between the actual lid angle and the estimated vector if the lid
-	 * were open to 270 deg, ang_lid_270. The smaller of the two angles
-	 * represents which one is closer. If the lid is closer to the
-	 * estimated 270 degree vector then the result is negative, otherwise
-	 * it is positive.
+	 * the cosine of the angle between the actual lid angle and the
+	 * estimated vector if the lid were open to 90 deg, cos_lid_90. Also
+	 * calculate the cosine of the angle between the actual lid angle and
+	 * the estimated vector if the lid were open to 270 deg,
+	 * cos_lid_270. The smaller of the two angles represents which one is
+	 * closer. If the lid is closer to the estimated 270 degree vector then
+	 * the result is negative, otherwise it is positive.
 	 */
 	rotate(base, p_acc_orient->rot_hinge_90, v);
-	ang_lid_90 = cosine_of_angle_diff(v, lid);
+	cos_lid_90 = cosine_of_angle_diff(v, lid);
 	rotate(v, p_acc_orient->rot_hinge_180, v);
-	ang_lid_270 = cosine_of_angle_diff(v, lid);
+	cos_lid_270 = cosine_of_angle_diff(v, lid);
 
 	/*
-	 * Note that ang_lid_90 and ang_lid_270 are not in degrees, because
+	 * Note that cos_lid_90 and cos_lid_270 are not in degrees, because
 	 * the arc_cos() was never performed. But, since arc_cos() is
 	 * monotonically decreasing, we can do this comparison without ever
 	 * taking arc_cos(). But, since the function is monotonically
 	 * decreasing, the logic of this comparison is reversed.
 	 */
-	if (ang_lid_270 > ang_lid_90)
+	if (cos_lid_270 > cos_lid_90)
 		ang_lid_to_base = -ang_lid_to_base;
 
 	/* Place lid angle between 0 and 360 degrees. */
 	if (ang_lid_to_base < 0)
-		ang_lid_to_base += 360;
+		ang_lid_to_base += FLOAT_TO_FP(360);
+
+	/*
+	 * Round to nearest int by adding 0.5. Note, only works because lid
+	 * angle is known to be positive.
+	 */
+	*lid_angle = FP_TO_INT(ang_lid_to_base + FLOAT_TO_FP(0.5));
 
-	*lid_angle = ang_lid_to_base;
 	return reliable;
 }
 
 int motion_lid_get_angle(void)
 {
 	if (lid_angle_is_reliable)
-		/*
-		 * Round to nearest int by adding 0.5. Note, only works because
-		 * lid angle is known to be positive.
-		 */
-		return (int)(lid_angle_deg + 0.5F);
+		return lid_angle_deg;
 	else
-		return (int)LID_ANGLE_UNRELIABLE;
+		return LID_ANGLE_UNRELIABLE;
 }
 
 /*
diff --git a/common/motion_sense.c b/common/motion_sense.c
index fd9c64bc7d..5e61b8fcec 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -285,6 +285,12 @@ void motion_sense_task(void)
 		gesture_calc();
 #endif
 #ifdef CONFIG_LID_ANGLE
+		/*
+		 * TODO (crosbug.com/p/36132): Checking for ALL sensors on is
+		 * overkill.  It should just check for ACCEL in BASE and ACCEL
+		 * in LID, since those are the only ones needed for the lid
+		 * calculation.
+		 */
 		if (rd_cnt == motion_sensor_count)
 			motion_lid_calc();
 #endif
@@ -299,7 +305,7 @@ void motion_sense_task(void)
 					sensor->xyz[Z]);
 			}
 #ifdef CONFIG_LID_ANGLE
-			CPRINTF("a=%-6.1d", 10 * motion_lid_get_angle());
+			CPRINTF("a=%-4d", motion_lid_get_angle());
 #endif
 			CPRINTF("]\n");
 		}
@@ -778,5 +784,3 @@ DECLARE_CONSOLE_COMMAND(accelint, command_accelerometer_interrupt,
 #endif /* CONFIG_ACCEL_INTERRUPTS */
 
 #endif /* CONFIG_CMD_ACCELS */
-
-
diff --git a/include/lid_angle.h b/include/lid_angle.h
index cd13265c93..e45a5683a4 100644
--- a/include/lid_angle.h
+++ b/include/lid_angle.h
@@ -15,7 +15,7 @@
  *
  * @lid_ang Lid angle.
  */
-void lidangle_keyscan_update(float lid_ang);
+void lidangle_keyscan_update(int lid_ang);
 
 /**
  * Getter and setter methods for the keyboard wake angle. In S3, when the
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);
 
 
 
diff --git a/include/motion_lid.h b/include/motion_lid.h
index b3348c020e..8ccf27191e 100644
--- a/include/motion_lid.h
+++ b/include/motion_lid.h
@@ -3,13 +3,13 @@
  * found in the LICENSE file.
  */
 
-/* Header for motion_sense.c */
+/* Header for motion_lid.h */
 
 #ifndef __CROS_EC_MOTION_LID_H
 #define __CROS_EC_MOTION_LID_H
 
 /* Anything outside of lid angle range [-180, 180] should work. */
-#define LID_ANGLE_UNRELIABLE 500.0F
+#define LID_ANGLE_UNRELIABLE 500
 
 /**
  * This structure defines all of the data needed to specify the orientation
@@ -37,7 +37,8 @@ extern const struct accel_orientation acc_orient;
  * Get last calculated lid angle. Note, the lid angle calculated by the EC
  * is un-calibrated and is an approximate angle.
  *
- * @return lid angle in degrees in range [0, 360].
+ * @return lid angle in degrees in range [0, 360], or LID_ANGLE_UNRELIABLE
+ * if the lid angle can't be determined.
  */
 int motion_lid_get_angle(void);
 
diff --git a/test/math_util.c b/test/math_util.c
index 46087e9f33..e563d8dd02 100644
--- a/test/math_util.c
+++ b/test/math_util.c
@@ -37,7 +37,7 @@ static int test_acos(void)
 
 	/* Test a handful of values. */
 	for (test = -1.0; test <= 1.0; test += 0.01) {
-		a = arc_cos(test);
+		a = FP_TO_FLOAT(arc_cos(FLOAT_TO_FP(test)));
 		b = acos(test) * RAD_TO_DEG;
 		TEST_ASSERT(IS_FLOAT_EQUAL(a, b, ACOS_TOLERANCE_DEG));
 	}
diff --git a/test/motion_lid.c b/test/motion_lid.c
index e0f1e68e7c..a358a5056c 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -6,6 +6,7 @@
  */
 
 #include <math.h>
+#include <stdio.h>
 
 #include "accelgyro.h"
 #include "common.h"
@@ -89,15 +90,15 @@ const struct accelgyro_drv test_motion_sense = {
 };
 
 const matrix_3x3_t base_standard_ref = {
-	{ 1, 0, 0},
-	{ 0, 1, 0},
-	{ 0, 0, 1}
+	{ FLOAT_TO_FP(1), 0, 0},
+	{ 0, FLOAT_TO_FP(1), 0},
+	{ 0, 0, FLOAT_TO_FP(1)}
 };
 
 const matrix_3x3_t lid_standard_ref = {
-	{ 1, 0, 0},
-	{ 1, 0, 0},
-	{ 0, 0, 1}
+	{ FLOAT_TO_FP(1), 0, 0},
+	{ FLOAT_TO_FP(1), 0, 0},
+	{ 0, 0, FLOAT_TO_FP(1)}
 };
 
 struct motion_sensor_t motion_sensors[] = {