Added unit tests for lid angle calculation and acos

Added unit test for motion_sense lid angle calculations.
Added unit test for math_util arc_cos() function.

BUG=none
Original-BUG=chrome-os-partner:25640
BRANCH=rambi
TEST=make buildall

Original-Change-Id: I35debf6fc0e2d9996debc2ede175acaa060df627
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/186720
Reviewed-by: Vic Yang <victoryang@chromium.org>
(cherry picked from commit 55c3e8ee14693d6d667b5d21f0dcbb14d185ccbe)

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

8 files changed, 242 insertions, 1 deletions

diff --git a/Makefile.toolchain b/Makefile.toolchain
index f29059a605..6d44331fc7 100644
--- a/Makefile.toolchain
+++ b/Makefile.toolchain
@@ -55,5 +55,5 @@ BUILD_CFLAGS= $(LIBFTDI_CFLAGS) $(CPPFLAGS) -O3 $(CFLAGS_DEBUG) $(CFLAGS_WARN)
 HOST_CFLAGS=$(CPPFLAGS) -O3 $(CFLAGS_DEBUG) $(CFLAGS_WARN)
 LDFLAGS=-nostdlib -X
 BUILD_LDFLAGS=$(LIBFTDI_LDLIBS)
-HOST_TEST_LDFLAGS=-T core/host/host_exe.lds -lrt -pthread -rdynamic\
+HOST_TEST_LDFLAGS=-T core/host/host_exe.lds -lrt -pthread -rdynamic -lm\
 		  $(if $(TEST_COVERAGE),-fprofile-arcs,)
diff --git a/board/host/board.c b/board/host/board.c
index 5d47f0d8df..daf78d8cb9 100644
--- a/board/host/board.c
+++ b/board/host/board.c
@@ -8,6 +8,7 @@
 #include "extpower.h"
 #include "gpio.h"
 #include "lid_switch.h"
+#include "motion_sense.h"
 #include "power_button.h"
 #include "temp_sensor.h"
 #include "timer.h"
@@ -62,3 +63,27 @@ const struct button_config buttons[] = {
 };
 BUILD_ASSERT(ARRAY_SIZE(buttons) == CONFIG_BUTTON_COUNT);
 #endif
+
+/* Define the accelerometer orientation matrices. */
+const struct accel_orientation acc_orient = {
+	/* Lid and base sensor are already aligned. */
+	.rot_align = {
+		{ 1,  0,  0},
+		{ 0,  1,  0},
+		{ 0,  0,  1}
+	},
+
+	/* Hinge aligns with y axis. */
+	.rot_hinge_90 = {
+		{ 0,  0,  1},
+		{ 0,  1,  0},
+		{ -1, 0,  0}
+	},
+	.rot_hinge_180 = {
+		{-1,  0,  0},
+		{ 0,  1,  0},
+		{ 0,  0, -1}
+	},
+	.hinge_axis = {0, 1, 0},
+};
+
diff --git a/board/host/board.h b/board/host/board.h
index a0c47fcee7..55d66afb7b 100644
--- a/board/host/board.h
+++ b/board/host/board.h
@@ -51,4 +51,13 @@ enum adc_channel {
 	ADC_CH_COUNT
 };
 
+/* Identifiers for each accelerometer used. */
+enum accel_id {
+	ACCEL_LID,
+	ACCEL_BASE,
+
+	/* Number of accelerometers. */
+	ACCEL_COUNT
+};
+
 #endif /* __BOARD_H */
diff --git a/test/build.mk b/test/build.mk
index 7ea82cc00c..f93b5897aa 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -23,6 +23,7 @@ test-list-host=mutex pingpong utils kb_scan kb_mkbp lid_sw power_button hooks
 test-list-host+=thermal flash queue kb_8042 extpwr_gpio console_edit system
 test-list-host+=sbs_charging adapter host_command thermal_falco led_spring
 test-list-host+=bklight_lid bklight_passthru interrupt timer_dos button
+test-list-host+=motion_sense math_util
 
 adapter-y=adapter.o
 button-y=button.o
@@ -39,6 +40,8 @@ kb_mkbp-y=kb_mkbp.o
 kb_scan-y=kb_scan.o
 led_spring-y=led_spring.o led_spring_impl.o
 lid_sw-y=lid_sw.o
+math_util-y=math_util.o
+motion_sense-y=motion_sense.o
 mutex-y=mutex.o
 pingpong-y=pingpong.o
 power_button-y=power_button.o
diff --git a/test/math_util.c b/test/math_util.c
new file mode 100644
index 0000000000..bfc0345655
--- /dev/null
+++ b/test/math_util.c
@@ -0,0 +1,60 @@
+/* Copyright (c) 2014 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.
+ *
+ * Test motion sense code.
+ */
+
+#include <math.h>
+
+#include "math_util.h"
+#include "motion_sense.h"
+#include "test_util.h"
+
+/*****************************************************************************/
+/* Mock functions */
+
+/* Need to define accelerometer init and read functions just to compile. */
+int accel_init(enum accel_id id)
+{
+	return EC_SUCCESS;
+}
+
+int accel_read(enum accel_id id, int *x_acc, int *y_acc, int *z_acc)
+{
+	return EC_SUCCESS;
+}
+
+/*****************************************************************************/
+/* Test utilities */
+
+/* Macro to compare two floats and check if they are equal within diff. */
+#define IS_FLOAT_EQUAL(a, b, diff) ((a) >= ((b) - diff) && (a) <= ((b) + diff))
+
+#define ACOS_TOLERANCE_DEG 0.5f
+#define RAD_TO_DEG (180.0f / 3.1415926f)
+
+static int test_acos(void)
+{
+	float a, b;
+	float test;
+
+	/* Test a handful of values. */
+	for (test = -1.0; test <= 1.0; test += 0.01) {
+		a = arc_cos(test);
+		b = acos(test) * RAD_TO_DEG;
+		TEST_ASSERT(IS_FLOAT_EQUAL(a, b, ACOS_TOLERANCE_DEG));
+	}
+
+	return EC_SUCCESS;
+}
+
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_acos);
+
+	test_print_result();
+}
diff --git a/test/math_util.tasklist b/test/math_util.tasklist
new file mode 100644
index 0000000000..60688f9c34
--- /dev/null
+++ b/test/math_util.tasklist
@@ -0,0 +1,18 @@
+/* Copyright (c) 2014 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  \
+  TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE)
diff --git a/test/motion_sense.c b/test/motion_sense.c
new file mode 100644
index 0000000000..1a5d808044
--- /dev/null
+++ b/test/motion_sense.c
@@ -0,0 +1,108 @@
+/* Copyright (c) 2014 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.
+ *
+ * Test motion sense code.
+ */
+
+#include <math.h>
+
+#include "common.h"
+#include "motion_sense.h"
+#include "task.h"
+#include "test_util.h"
+#include "timer.h"
+
+/* Mock acceleration values for motion sense task to read in. */
+int mock_x_acc[ACCEL_COUNT], mock_y_acc[ACCEL_COUNT], mock_z_acc[ACCEL_COUNT];
+
+/*****************************************************************************/
+/* Mock functions */
+
+int accel_init(enum accel_id id)
+{
+	return EC_SUCCESS;
+}
+
+int accel_read(enum accel_id id, int *x_acc, int *y_acc, int *z_acc)
+{
+	/* Return the mock values. */
+	*x_acc = mock_x_acc[id];
+	*y_acc = mock_y_acc[id];
+	*z_acc = mock_z_acc[id];
+
+	return EC_SUCCESS;
+}
+
+
+/*****************************************************************************/
+/* Test utilities */
+static int test_lid_angle(void)
+{
+	/*
+	 * Set the base accelerometer as if it were sitting flat on a desk
+	 * and set the lid to closed.
+	 */
+	mock_x_acc[ACCEL_BASE] = 0;
+	mock_y_acc[ACCEL_BASE] = 0;
+	mock_z_acc[ACCEL_BASE] = 1000;
+	mock_x_acc[ACCEL_LID] = 0;
+	mock_y_acc[ACCEL_LID] = 0;
+	mock_z_acc[ACCEL_LID] = 1000;
+	task_wake(TASK_ID_MOTIONSENSE);
+	msleep(5);
+	TEST_ASSERT(motion_get_lid_angle() == 0);
+
+	/* Set lid open to 90 degrees. */
+	mock_x_acc[ACCEL_LID] = -1000;
+	mock_y_acc[ACCEL_LID] = 0;
+	mock_z_acc[ACCEL_LID] = 0;
+	task_wake(TASK_ID_MOTIONSENSE);
+	msleep(5);
+	TEST_ASSERT(motion_get_lid_angle() == 90);
+
+	/* Set lid open to -135. */
+	mock_x_acc[ACCEL_LID] = 500;
+	mock_y_acc[ACCEL_LID] = 0;
+	mock_z_acc[ACCEL_LID] = -500;
+	task_wake(TASK_ID_MOTIONSENSE);
+	msleep(5);
+	TEST_ASSERT(motion_get_lid_angle() == -135);
+
+	/*
+	 * Align base with hinge and make sure it returns 0 for angle. In this
+	 * test it doesn't matter what the lid acceleration vector is.
+	 */
+	mock_x_acc[ACCEL_BASE] = 0;
+	mock_y_acc[ACCEL_BASE] = 1000;
+	mock_z_acc[ACCEL_BASE] = 0;
+	task_wake(TASK_ID_MOTIONSENSE);
+	msleep(5);
+	TEST_ASSERT(motion_get_lid_angle() == 0);
+
+	/*
+	 * Use all three axes and set lid to negative base and make sure
+	 * angle is 180.
+	 */
+	mock_x_acc[ACCEL_BASE] = 500;
+	mock_y_acc[ACCEL_BASE] = 400;
+	mock_z_acc[ACCEL_BASE] = 300;
+	mock_x_acc[ACCEL_LID] = -500;
+	mock_y_acc[ACCEL_LID] = -400;
+	mock_z_acc[ACCEL_LID] = -300;
+	task_wake(TASK_ID_MOTIONSENSE);
+	msleep(5);
+	TEST_ASSERT(motion_get_lid_angle() == 180);
+
+	return EC_SUCCESS;
+}
+
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_lid_angle);
+
+	test_print_result();
+}
diff --git a/test/motion_sense.tasklist b/test/motion_sense.tasklist
new file mode 100644
index 0000000000..60688f9c34
--- /dev/null
+++ b/test/motion_sense.tasklist
@@ -0,0 +1,18 @@
+/* Copyright (c) 2014 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  \
+  TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE)