summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--board/host/board.c5
-rw-r--r--board/host/board.h1
-rw-r--r--board/samus/board.c5
-rw-r--r--common/build.mk1
-rw-r--r--common/math_util.c92
-rw-r--r--common/motion_calibrate.c381
-rw-r--r--common/motion_sense.c37
-rw-r--r--include/config.h7
-rw-r--r--include/math_util.h42
-rw-r--r--include/motion_sense.h23
10 files changed, 3 insertions, 591 deletions
diff --git a/board/host/board.c b/board/host/board.c
index 8c70053ac2..c43a36ae8b 100644
--- a/board/host/board.c
+++ b/board/host/board.c
@@ -53,10 +53,7 @@ BUILD_ASSERT(ARRAY_SIZE(buttons) == CONFIG_BUTTON_COUNT);
#endif
/* Define the accelerometer orientation matrices. */
-#ifndef CONFIG_ACCEL_CALIBRATE
-const
-#endif
-struct accel_orientation acc_orient = {
+const struct accel_orientation acc_orient = {
/* Lid and base sensor are already aligned. */
.rot_align = {
{ 1, 0, 0},
diff --git a/board/host/board.h b/board/host/board.h
index 19890b6504..3c11f58b8f 100644
--- a/board/host/board.h
+++ b/board/host/board.h
@@ -9,7 +9,6 @@
#define __BOARD_H
/* Optional features */
-#undef CONFIG_ACCEL_CALIBRATE
#define CONFIG_EXTPOWER_GPIO
#undef CONFIG_FMAP
#define CONFIG_POWER_BUTTON
diff --git a/board/samus/board.c b/board/samus/board.c
index a75682ad29..d6b77430d6 100644
--- a/board/samus/board.c
+++ b/board/samus/board.c
@@ -268,10 +268,7 @@ struct motion_sensor_t motion_sensors[] = {
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
/* Define the accelerometer orientation matrices. */
-#ifndef CONFIG_ACCEL_CALIBRATE
-const
-#endif
-struct accel_orientation acc_orient = {
+const struct accel_orientation acc_orient = {
/* Lid and base sensor are already aligned. */
.rot_align = {
{ 0, -1, 0},
diff --git a/common/build.mk b/common/build.mk
index 79a8f3c235..cf76e5a0a7 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -9,7 +9,6 @@
common-y=util.o
common-y+=version.o printf.o queue.o
-common-$(CONFIG_ACCEL_CALIBRATE)+=motion_calibrate.o
common-$(CONFIG_ADC)+=adc.o
common-$(CONFIG_ALS)+=als.o
common-$(CONFIG_AP_HANG_DETECT)+=ap_hang_detect.o
diff --git a/common/math_util.c b/common/math_util.c
index 56acec16dc..83bfc6cdfb 100644
--- a/common/math_util.c
+++ b/common/math_util.c
@@ -110,95 +110,3 @@ void rotate(const vector_3_t v, const matrix_3x3_t R,
t[2] * R[2][2];
}
-#ifdef CONFIG_ACCEL_CALIBRATE
-
-void matrix_multiply(matrix_3x3_t *m1, matrix_3x3_t *m2, matrix_3x3_t *res)
-{
- (*res)[0][0] = (*m1)[0][0] * (*m2)[0][0] + (*m1)[0][1] * (*m2)[1][0] +
- (*m1)[0][2] * (*m2)[2][0];
- (*res)[0][1] = (*m1)[0][0] * (*m2)[0][1] + (*m1)[0][1] * (*m2)[1][1] +
- (*m1)[0][2] * (*m2)[2][1];
- (*res)[0][2] = (*m1)[0][0] * (*m2)[0][2] + (*m1)[0][1] * (*m2)[1][2] +
- (*m1)[0][2] * (*m2)[2][2];
-
- (*res)[1][0] = (*m1)[1][0] * (*m2)[0][0] + (*m1)[1][1] * (*m2)[1][0] +
- (*m1)[1][2] * (*m2)[2][0];
- (*res)[1][1] = (*m1)[1][0] * (*m2)[0][1] + (*m1)[1][1] * (*m2)[1][1] +
- (*m1)[1][2] * (*m2)[2][1];
- (*res)[1][2] = (*m1)[1][0] * (*m2)[0][2] + (*m1)[1][1] * (*m2)[1][2] +
- (*m1)[1][2] * (*m2)[2][2];
-
- (*res)[2][0] = (*m1)[2][0] * (*m2)[0][0] + (*m1)[2][1] * (*m2)[1][0] +
- (*m1)[2][2] * (*m2)[2][0];
- (*res)[2][1] = (*m1)[2][0] * (*m2)[0][1] + (*m1)[2][1] * (*m2)[1][1] +
- (*m1)[2][2] * (*m2)[2][1];
- (*res)[2][2] = (*m1)[2][0] * (*m2)[0][2] + (*m1)[2][1] * (*m2)[1][2] +
- (*m1)[2][2] * (*m2)[2][2];
-}
-
-/**
- * Find determinant of matrix.
- *
- * @param m Pointer to matrix to take determinant of.
- *
- * @return Determinant of matrix m.
- */
-static float matrix_determinant(matrix_3x3_t *m)
-{
- return (*m)[0][0]*(*m)[1][1]*(*m)[2][2] +
- (*m)[1][0]*(*m)[2][1]*(*m)[0][2] +
- (*m)[2][0]*(*m)[0][1]*(*m)[1][2] -
- (*m)[0][0]*(*m)[2][1]*(*m)[1][2] -
- (*m)[2][0]*(*m)[1][1]*(*m)[0][2] -
- (*m)[1][0]*(*m)[0][1]*(*m)[2][2];
-}
-
-/**
- * Find inverse of matrix.
- *
- * @param m Pointer to matrix to invert.
- * @param res Pointer to resultant matrix.
- */
-static int matrix_inverse(matrix_3x3_t *m, matrix_3x3_t *res)
-{
- float det = matrix_determinant(m);
-
- /*
- * If determinant is too close to zero, then input is not linearly
- * independent enough, return an error.
- */
- if (ABS(det) < 1e7)
- return EC_ERROR_UNKNOWN;
-
- /* Find inverse of input matrix. */
- (*res)[0][0] = ((*m)[1][1]*(*m)[2][2] - (*m)[1][2]*(*m)[2][1]) / det;
- (*res)[0][1] = ((*m)[0][2]*(*m)[2][1] - (*m)[0][1]*(*m)[2][2]) / det;
- (*res)[0][2] = ((*m)[0][1]*(*m)[1][2] - (*m)[0][2]*(*m)[1][1]) / det;
-
- (*res)[1][0] = ((*m)[1][2]*(*m)[2][0] - (*m)[1][0]*(*m)[2][2]) / det;
- (*res)[1][1] = ((*m)[0][0]*(*m)[2][2] - (*m)[0][2]*(*m)[2][0]) / det;
- (*res)[1][2] = ((*m)[0][2]*(*m)[1][0] - (*m)[0][0]*(*m)[1][2]) / det;
-
- (*res)[2][0] = ((*m)[1][0]*(*m)[2][1] - (*m)[1][1]*(*m)[2][0]) / det;
- (*res)[2][1] = ((*m)[0][1]*(*m)[2][0] - (*m)[0][0]*(*m)[2][1]) / det;
- (*res)[2][2] = ((*m)[0][0]*(*m)[1][1] - (*m)[0][1]*(*m)[1][0]) / det;
-
- return EC_SUCCESS;
-}
-
-int solve_rotation_matrix(matrix_3x3_t *in, matrix_3x3_t *out, matrix_3x3_t *R)
-{
- static matrix_3x3_t i;
- int ret;
-
- /* Calculate inverse of input matrix. */
- ret = matrix_inverse(in, &i);
- if (ret != EC_SUCCESS)
- return ret;
-
- /* Multiply inverse of in matrix by out matrix and store into R. */
- matrix_multiply(&i, out, R);
-
- return EC_SUCCESS;
-}
-#endif /* CONFIG_ACCEL_CALIBRATE */
diff --git a/common/motion_calibrate.c b/common/motion_calibrate.c
deleted file mode 100644
index 3103a5fc3c..0000000000
--- a/common/motion_calibrate.c
+++ /dev/null
@@ -1,381 +0,0 @@
-/* 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.
- */
-
-/* Motion sensor calibration code. */
-
-#include "common.h"
-#include "console.h"
-#include "math_util.h"
-#include "motion_sense.h"
-#include "accelgyro.h"
-#include "timer.h"
-#include "task.h"
-#include "uart.h"
-#include "util.h"
-
-/*
- * Threshold to capture a sample when performing auto-calibrate. The units are
- * the same as the units of the accelerometer acceleration values.
- */
-#define AUTO_CAL_DIR_THRESHOLD (ACCEL_G * 3 / 4)
-#define AUTO_CAL_MAG_THRESHOLD (ACCEL_G / 20)
-
-/*
- * Solution to standard reference frame calibration equation. Note, this matrix
- * depends on the exact instructions regarding the orientation given to the user
- * for calibrating the standard reference frame.
- */
-static matrix_3x3_t standard_ref_calib = {
- { 1024, 0, 0},
- { 0, -1024, 0},
- { 0, 0, 1024}
-};
-
-/*****************************************************************************/
-/* Console commands */
-
-/**
- * Print all orientation calibration data.
- */
-static int command_print_orientation(int argc, char **argv)
-{
- matrix_3x3_t (*R);
-
- R = &acc_orient.rot_align;
- ccprintf("Lid to base alignment R:\n%.2d\t%.2d\t%.2d\n%.2d\t%.2d\t%.2d"
- "\n%.2d\t%.2d\t%.2d\n\n",
- (int)((*R)[0][0]*100), (int)((*R)[0][1]*100), (int)((*R)[0][2]*100),
- (int)((*R)[1][0]*100), (int)((*R)[1][1]*100), (int)((*R)[1][2]*100),
- (int)((*R)[2][0]*100), (int)((*R)[2][1]*100), (int)((*R)[2][2]*100));
-
- R = &acc_orient.rot_hinge_90;
- ccprintf("Hinge rotation 90 R:\n%.2d\t%.2d\t%.2d\n%.2d\t%.2d\t%.2d\n"
- "%.2d\t%.2d\t%.2d\n\n",
- (int)((*R)[0][0]*100), (int)((*R)[0][1]*100), (int)((*R)[0][2]*100),
- (int)((*R)[1][0]*100), (int)((*R)[1][1]*100), (int)((*R)[1][2]*100),
- (int)((*R)[2][0]*100), (int)((*R)[2][1]*100), (int)((*R)[2][2]*100));
-
- R = &acc_orient.rot_hinge_180;
- ccprintf("Hinge rotation 180 R:\n%.2d\t%.2d\t%.2d\n%.2d\t%.2d\t%.2d\n"
- "%.2d\t%.2d\t%.2d\n\n",
- (int)((*R)[0][0]*100), (int)((*R)[0][1]*100), (int)((*R)[0][2]*100),
- (int)((*R)[1][0]*100), (int)((*R)[1][1]*100), (int)((*R)[1][2]*100),
- (int)((*R)[2][0]*100), (int)((*R)[2][1]*100), (int)((*R)[2][2]*100));
-
- R = &acc_orient.rot_standard_ref;
- ccprintf("Standard ref frame R:\n%.2d\t%.2d\t%.2d\n%.2d\t%.2d\t%.2d\n"
- "%.2d\t%.2d\t%.2d\n\n",
- (int)((*R)[0][0]*100), (int)((*R)[0][1]*100), (int)((*R)[0][2]*100),
- (int)((*R)[1][0]*100), (int)((*R)[1][1]*100), (int)((*R)[1][2]*100),
- (int)((*R)[2][0]*100), (int)((*R)[2][1]*100), (int)((*R)[2][2]*100));
-
- ccprintf("Hinge Axis:\t%d\t%d\t%d\n", acc_orient.hinge_axis[0],
- acc_orient.hinge_axis[1],
- acc_orient.hinge_axis[2]);
-
- return EC_SUCCESS;
-}
-DECLARE_CONSOLE_COMMAND(accelorient, command_print_orientation,
- "",
- "Print all orientation calibration data", NULL);
-
-/**
- * Calibrate the orientation and print results to console.
- *
- * @param type 0 is for calibrating lid to base alignment,
- * 1 is for calibrating hinge 90 rotation
- */
-static int calibrate_orientation(int type)
-{
- int mag, ret, i, j;
-
- /* Captured flags. Set true when the corresponding axis is captured. */
- int captured[3] = {0, 0, 0};
-
- /* Current acceleration vectors. */
- vector_3_t base, lid;
-
- static matrix_3x3_t rec_base, rec_lid;
-
- while (1) {
- /* Capture the current acceleration vectors. */
- motion_get_accel_lid(&lid, type);
- motion_get_accel_base(&base);
-
- /* Measure magnitude of base accelerometer. */
- mag = vector_magnitude(base);
-
- /*
- * Only capture a sample if the magnitude of the acceleration
- * is close to G, because this assures we won't calibrate with
- * values biased by motion.
- */
- if ((mag > ACCEL_G - AUTO_CAL_MAG_THRESHOLD) &&
- (mag < ACCEL_G + AUTO_CAL_MAG_THRESHOLD)) {
-
- /*
- * Capture a sample when each axis exceeds some
- * threshold. This guarantees linear independence.
- */
- for (i = 0; i < 3; i++) {
- if (!captured[i] &&
- ABS(base[i]) > AUTO_CAL_DIR_THRESHOLD) {
-
- for (j = 0; j < 3; j++) {
- rec_base[i][j] = (float)base[j];
- rec_lid[i][j] = (float)lid[j];
- }
- ccprintf("Captured axis %d\n", i);
- captured[i] = 1;
- }
- }
-
- /* If all axes are captured, we are done. */
- if (captured[0] && captured[1] && captured[2])
- break;
- }
-
- /* Wait until next reading. */
- task_wait_event(50 * MSEC);
- }
-
- /* Solve for the rotation matrix and display final rotation matrix. */
- if (type == 0)
- ret = solve_rotation_matrix(&rec_lid, &rec_base,
- &acc_orient.rot_align);
- else
- ret = solve_rotation_matrix(&rec_base, &rec_lid,
- &acc_orient.rot_hinge_90);
-
- if (ret != EC_SUCCESS)
- ccprintf("Failed to find rotation matrix.\n");
-
- return ret;
-}
-
-/**
- * Calibrate the hinge axis and hinge 180 rotation matrix.
- */
-static int calibrate_hinge(void)
-{
- static matrix_3x3_t tmp;
- float d;
- int i, j;
- vector_3_t base;
-
- motion_get_accel_base(&base);
- memcpy(&acc_orient.hinge_axis, &base, sizeof(vector_3_t));
-
- /*
- * Calculate a rotation matrix to rotate 180 degrees about hinge axis.
- * The formula is:
- *
- * rot_hinge_180 = I + 2 * tmp^2 / d^2,
- * where tmp is a matrix formed from the hinge axis, d is the sqrt
- * of the hinge axis vector used in tmp, and I is the 3x3 identity
- * matrix.
- *
- */
- tmp[0][0] = 0;
- tmp[0][1] = base[2];
- tmp[0][2] = -base[1];
- tmp[1][0] = -base[2];
- tmp[1][1] = 0;
- tmp[1][2] = base[0];
- tmp[2][0] = base[1];
- tmp[2][1] = -base[0];
- tmp[2][2] = 0;
-
- matrix_multiply(&tmp, &tmp, &acc_orient.rot_hinge_180);
- d = (float)(SQ(base[0]) + SQ(base[1]) + SQ(base[2]));
-
- for (i = 0; i < 3; i++) {
- for (j = 0; j < 3; j++) {
- acc_orient.rot_hinge_180[i][j] *= 2.0F / d;
-
- /* Add identity matrix. */
- if (i == j)
- acc_orient.rot_hinge_180[i][j] += 1;
- }
- }
-
- return EC_SUCCESS;
-}
-
-/**
- * Calibrate the standard reference frame.
- */
-static int calibrate_standard_frame(vector_3_t *v_x, vector_3_t *v_y,
- vector_3_t *v_z)
-{
- static matrix_3x3_t m;
- int j;
-
- for (j = 0; j < 3; j++) {
- m[0][j] = (*v_x)[j];
- m[1][j] = (*v_y)[j];
- m[2][j] = (*v_z)[j];
- }
-
- return solve_rotation_matrix(&m, &standard_ref_calib,
- &acc_orient.rot_standard_ref);
-}
-
-/**
- * Wait until a specific set of keys is pressed: enter, 'q', or 's'. Return
- * key that was pressed.
- */
-static int wait_for_key(void)
-{
- int c = uart_getc();
-
- /* Loop until previous character was a new line char, 'q', or 's'. */
- while (c != '\r' && c != '\n' && c != 'q' && c != 's') {
- task_wait_event(50 * MSEC);
- c = uart_getc();
- }
-
- return c;
-}
-
-static int command_auto_calibrate(int argc, char **argv)
-{
- int c;
- vector_3_t v_x, v_y, v_z;
-
- if (argc > 1)
- return EC_ERROR_PARAM_COUNT;
-
- ccprintf("Calibrating... press 'q' at any time to quit, and 's' "
- "to skip step.\n");
-
- /*
- * Part 1: Calibrate the lid to base alignment rotation matrix.
- */
- ccprintf("\nStep 1: close lid, press enter, and rotate the machine\n"
- "in space until all 3 directions are captured.\n");
-
- /* Wait for user to press enter, quit, or skip. */
- c = wait_for_key();
- if (c == 'q') {
- ccprintf("Calibration exited.\n");
- return EC_SUCCESS;
- }
-
- /* If step is not skipped, perform calibration. */
- if (c != 's') {
- if (calibrate_orientation(0) != EC_SUCCESS) {
- ccprintf("Calibration error.\n");
- return EC_SUCCESS;
- }
- }
-
- /*
- * Part 2: Calibrate the hinge 90 rotation matrix.
- */
- ccprintf("\nStep 2: open lid to 90 degrees, press enter, and rotate\n"
- "in space until all 3 directions are captured.\n");
-
-
- /* Wait for user to press enter, quit, or skip. */
- c = wait_for_key();
- if (c == 'q') {
- ccprintf("Calibration exited.\n");
- return EC_SUCCESS;
- }
-
- /* If step is not skipped, perform calibration. */
- if (c != 's') {
- if (calibrate_orientation(1) != EC_SUCCESS) {
- ccprintf("Calibration error.\n");
- return EC_SUCCESS;
- }
- }
-
- /*
- * Part 3: Calibrate the hinge axis and hinge 180 rotation matrix.
- */
- ccprintf("\nStep 3: align hinge with gravity, and press enter.\n");
-
- /* Wait for user to press enter, quit, or skip. */
- c = wait_for_key();
- if (c == 'q') {
- ccprintf("Calibration exited.\n");
- return EC_SUCCESS;
- }
-
- /* If step is not skipped, perform calibration. */
- if (c != 's') {
- if (calibrate_hinge() != EC_SUCCESS) {
- ccprintf("Calibration error.\n");
- return EC_SUCCESS;
- }
- }
-
- /*
- * Part 4: Calibrate the standard reference frame rotation matrix.
- */
- ccprintf("\nStep 4a: place machine on right side, with hinge\n"
- "aligned with gravity, and press enter.\n");
-
- /* Wait for user to press enter, quit, or skip. */
- c = wait_for_key();
- if (c == 'q') {
- ccprintf("Calibration exited.\n");
- return EC_SUCCESS;
- }
-
- if (c == 's')
- goto auto_calib_done;
-
- /* In this orientation, the Y axis should be highest. Capture data. */
- motion_get_accel_base(&v_y);
-
- ccprintf("\nStep 4b: place machine flat on table, with keyboard\n"
- "up, and press enter.\n");
-
- /* Wait for user to press enter, quit, or skip. */
- c = wait_for_key();
- if (c == 'q') {
- ccprintf("Calibration exited.\n");
- return EC_SUCCESS;
- }
-
- if (c == 's')
- goto auto_calib_done;
-
- /* In this orientation, the Z axis should be highest. Capture data. */
- motion_get_accel_base(&v_z);
-
- ccprintf("\nStep 4c: hold machine perpendicular to table with\n"
- "the hinge up, and press enter.\n");
-
- /* Wait for user to press enter, quit, or skip. */
- c = wait_for_key();
- if (c == 'q') {
- ccprintf("Calibration exited.\n");
- return EC_SUCCESS;
- }
-
- if (c == 's')
- goto auto_calib_done;
-
- /* In this orientation, the X axis should be highest. Capture data. */
- motion_get_accel_base(&v_x);
-
- if (calibrate_standard_frame(&v_x, &v_y, &v_z) != EC_SUCCESS) {
- ccprintf("Calibration error.\n");
- return EC_SUCCESS;
- }
-
-auto_calib_done:
- /* Print results of all calibration. */
- command_print_orientation(0, NULL);
-
- return EC_SUCCESS;
-}
-DECLARE_CONSOLE_COMMAND(accelcalib, command_auto_calibrate,
- "",
- "Auto calibrate the accelerometers", NULL);
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 62be9ad181..f157838f12 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -156,43 +156,6 @@ int motion_get_lid_angle(void)
return (int)LID_ANGLE_UNRELIABLE;
}
-#ifdef CONFIG_ACCEL_CALIBRATE
-void motion_get_accel_lid(vector_3_t *v, int adjusted)
-{
- int i;
- struct motion_sensor_t *sensor;
- struct motion_sensor_t *accel_lid = NULL;
- for (i = 0; i < motion_sensor_count; ++i) {
- sensor = &motion_sensors[i];
- if ((LOCATION_LID == sensor->location)
- && (SENSOR_ACCELEROMETER == sensor->type)) {
- accel_lid = sensor;
- break;
- }
- }
- if (accel_lid)
- memcpy(v, (adjusted ? accel_lid->xyz : accel_lid->raw_xyz),
- sizeof(vector_3_t));
-}
-
-void motion_get_accel_base(vector_3_t *v)
-{
- int i;
- struct motion_sensor_t *sensor;
- struct motion_sensor_t *accel_base = NULL;
- for (i = 0; i < motion_sensor_count; ++i) {
- sensor = &motion_sensors[i];
- if ((LOCATION_BASE == sensor->location)
- && (SENSOR_ACCELEROMETER == sensor->type)) {
- accel_base = sensor;
- break;
- }
- }
- if (accel_base)
- memcpy(v, accel_base->xyz, sizeof(vector_3_t));
-}
-#endif
-
static void clock_chipset_shutdown(void)
{
int i;
diff --git a/include/config.h b/include/config.h
index bf2c76618d..cbc460408a 100644
--- a/include/config.h
+++ b/include/config.h
@@ -36,9 +36,6 @@
* BOARD_*, CHIP_*, and CHIP_FAMILY_*.
*/
-/* Enable EC console functions for calibrating accelerometers. */
-#undef CONFIG_ACCEL_CALIBRATE
-
/* Enable accelerometer interrupts. */
#undef CONFIG_ACCEL_INTERRUPTS
@@ -1170,10 +1167,6 @@
#undef CONFIG_KEYBOARD_PROTOCOL_MKBP
#endif
-#ifndef HAS_TASK_MOTIONSENSE
-#undef CONFIG_ACCEL_CALIBRATE
-#endif
-
/*****************************************************************************/
/*
* Apply test config overrides last, since tests need to override some of the
diff --git a/include/math_util.h b/include/math_util.h
index 1dc92c4999..044c411b23 100644
--- a/include/math_util.h
+++ b/include/math_util.h
@@ -47,47 +47,5 @@ void rotate(const vector_3_t v, const matrix_3x3_t R,
vector_3_t res);
-#ifdef CONFIG_ACCEL_CALIBRATE
-
-/**
- * Multiply two 3x3 matrices.
- *
- * @param m1
- * @param m2
- * @param res Pointer to resultant matrix R = a1*a2;
- */
-void matrix_multiply(matrix_3x3_t *m1, matrix_3x3_t *m2, matrix_3x3_t *res);
-
-/**
- * Given an input matrix and an output matrix, solve for the rotation
- * matrix to get from the input matrix to the output matrix. Note, that this
- * operation is not guaranteed. In order to successfully calculate the rotation
- * matrix, the input must be linearly independent so that the matrix can be
- * inverted.
- *
- * This function solves the following matrix equation for R:
- * in * R = out
- *
- * If input matrix is invertible the resulting rotation matrix is stored in R.
- *
- * @param in
- * @param out
- * @param R Pointer to resultant matrix.
- *
- * @return EC_SUCCESS if successful
- */
-int solve_rotation_matrix(matrix_3x3_t *in, matrix_3x3_t *out, matrix_3x3_t *R);
-
-/**
- * Calculate magnitude of a vector.
- *
- * @param v Vector to be measured.
- *
- * @return Magnitued of vector v.
- */
-int vector_magnitude(const vector_3_t v);
-
-#endif
-
#endif /* __CROS_MATH_UTIL_H */
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 4991fcaba4..c08083be8c 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -45,11 +45,7 @@ struct accel_orientation {
};
/* Link global structure for orientation. This must be defined in board.c. */
-extern
-#ifndef CONFIG_ACCEL_CALIBRATE
-const
-#endif
-struct accel_orientation acc_orient;
+extern const struct accel_orientation acc_orient;
/**
@@ -61,23 +57,6 @@ struct accel_orientation acc_orient;
int motion_get_lid_angle(void);
-#ifdef CONFIG_ACCEL_CALIBRATE
-/**
- * Get the last measured lid acceleration vector.
- *
- * @param v Pointer to location to store vector.
- * @param adjusted If false use the raw vector, if true use the adjusted vector.
- */
-void motion_get_accel_lid(vector_3_t *v, int adjusted);
-
-/**
- * Get the last measured base acceleration vector.
- *
- * @param v Pointer to location to store vector.
- */
-void motion_get_accel_base(vector_3_t *v);
-#endif
-
/**
* Interrupt function for lid accelerometer.
*