common/motion_sense.c - chromiumos/platform/ec - Git at Google

 /* 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 sense module to read from various motion sensors. */

 #include "accelgyro.h"
 #include "common.h"
 #include "console.h"
 #include "hooks.h"
 #include "host_command.h"
 #include "lid_angle.h"
 #include "math_util.h"
 #include "motion_sense.h"
 #include "timer.h"
 #include "task.h"
 #include "util.h"

 /* Console output macros */
 #define CPUTS(outstr) cputs(CC_MOTION_SENSE, outstr)
 #define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args)

 /* Minimum time in between running motion sense task loop. */
 #define MIN_MOTION_SENSE_WAIT_TIME (1 * MSEC)

 static const struct motion_sensor_t *base;
 static const struct motion_sensor_t *lid;

 /* Current acceleration vectors and current lid angle. */
 static vector_3_t acc_lid_raw, acc_lid, acc_base;
 static vector_3_t acc_lid_host, acc_base_host;
 static float lid_angle_deg;
 static int lid_angle_is_reliable;

 /* Bounds for setting the sensor polling interval. */
 #define MIN_POLLING_INTERVAL_MS 5
 #define MAX_POLLING_INTERVAL_MS 1000

 /* Accelerometer polling intervals based on chipset state. */
 static int accel_interval_ap_on_ms = 10;
 static const int accel_interval_ap_suspend_ms = 100;

 /*
  * Angle threshold for how close the hinge aligns with gravity before
  * considering the lid angle calculation unreliable. For computational
  * 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

 /* Sampling interval for measuring acceleration and calculating lid angle. */
 static int accel_interval_ms;

 #ifdef CONFIG_CMD_LID_ANGLE
 static int accel_disp;
 #endif

 /* For vector_3_t, define which coordinates are in which location. */
 enum {
 	X, Y, Z
 };

 /* Pointer to constant acceleration orientation data. */
 const struct accel_orientation * const p_acc_orient = &acc_orient;

 /**
  * Calculate the lid angle using two acceleration vectors, one recorded in
  * the base and one in the lid.
  *
  * @param base Base accel vector
  * @param lid  Lid accel vector
  * @param lid_angle Pointer to location to store lid angle result
  *
  * @return flag representing if resulting lid angle calculation is reliable.
  */
 static int calculate_lid_angle(vector_3_t base, vector_3_t lid,
 		float *lid_angle)
 {
 	vector_3_t v;
 	float ang_lid_to_base, ang_lid_90, ang_lid_270;
 	float lid_to_base, base_to_hinge;
 	int reliable = 1;

 	/*
 	 * The angle between lid and base is:
 	 * acos((cad(base, lid) - cad(base, hinge)^2) /(1 - cad(base, hinge)^2))
 	 * where cad() is the cosine_of_angle_diff() function.
 	 *
 	 * Make sure to check for divide by 0.
 	 */
 	lid_to_base = cosine_of_angle_diff(base, lid);
 	base_to_hinge = cosine_of_angle_diff(base, p_acc_orient->hinge_axis);

 	/*
 	 * If hinge aligns too closely with gravity, then result may be
 	 * unreliable.
 	 */
 	if (ABS(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD)
 		reliable = 0;

 	base_to_hinge = SQ(base_to_hinge);

 	/* Check divide by 0. */
 	if (ABS(1.0F - base_to_hinge) < 0.01F) {
 		*lid_angle = 0.0;
 		return 0;
 	}

 	ang_lid_to_base = arc_cos(
 			(lid_to_base - base_to_hinge) / (1 - base_to_hinge));

 	/*
 	 * 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.
 	 */
 	rotate(base, &p_acc_orient->rot_hinge_90, &v);
 	ang_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);

 	/*
 	 * Note that ang_lid_90 and ang_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)
 		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;

 	*lid_angle = ang_lid_to_base;
 	return reliable;
 }

 int motion_get_lid_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);
 	else
 		return (int)LID_ANGLE_UNRELIABLE;
 }

 #ifdef CONFIG_ACCEL_CALIBRATE
 void motion_get_accel_lid(vector_3_t *v, int adjusted)
 {
 	memcpy(v, adjusted ? &acc_lid : &acc_lid_raw, sizeof(vector_3_t));
 }

 void motion_get_accel_base(vector_3_t *v)
 {
 	memcpy(v, &acc_base, sizeof(vector_3_t));
 }
 #endif

 static void set_ap_suspend_polling(void)
 {
 	accel_interval_ms = accel_interval_ap_suspend_ms;
 }
 DECLARE_HOOK(HOOK_CHIPSET_SUSPEND, set_ap_suspend_polling, HOOK_PRIO_DEFAULT);

 static void set_ap_on_polling(void)
 {
 	accel_interval_ms = accel_interval_ap_on_ms;
 }
 DECLARE_HOOK(HOOK_CHIPSET_RESUME, set_ap_on_polling, HOOK_PRIO_DEFAULT);


 void motion_sense_task(void)
 {
 	static timestamp_t ts0, ts1;
 	int wait_us;
 	int ret;
 	uint8_t *lpc_status;
 	uint16_t *lpc_data;
 	int sample_id = 0;
 	int i;

 	lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
 	lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA);

 	/*
 	 * TODO(crosbug.com/p/27320): The motion_sense task currently assumes
 	 * one configuration of motion sensors. Namely, it assumes there is
 	 * one accel in the base, one in the lid. Eventually, these
 	 * assumptions will have to be removed when we have other
 	 * configurations of motion sensors.
 	 */
 	for (i = 0; i <  motion_sensor_count; ++i) {
 		if (motion_sensors[i].location == LOCATION_LID)
 			lid = &motion_sensors[i];
 		else if (motion_sensors[i].location == LOCATION_BASE)
 			base = &motion_sensors[i];
 	}

 	if (lid == NULL || base == NULL) {
 		CPRINTS("Invalid motion_sensors list, lid and base required");
 		return;
 	}

 	/* Initialize accelerometers. */
 	ret = lid->drv->init(lid->drv_data, lid->i2c_addr);
 	ret |= base->drv->init(base->drv_data, base->i2c_addr);

 	/* If accelerometers do not initialize, then end task. */
 	if (ret != EC_SUCCESS) {
 		CPRINTS("Accel init failed; stopping MS");
 		return;
 	}

 	/* Initialize sampling interval. */
 	accel_interval_ms = accel_interval_ap_suspend_ms;

 	/* Set default accelerometer parameters. */
 	lid->drv->set_range(lid->drv_data,  2, 1);
 	lid->drv->set_resolution(lid->drv_data,  12, 1);
 	lid->drv->set_datarate(lid->drv_data,  100000, 1);
 	base->drv->set_range(base->drv_data, 2, 1);
 	base->drv->set_resolution(base->drv_data, 12, 1);
 	base->drv->set_datarate(base->drv_data, 100000, 1);

 	/* Write to status byte to represent that accelerometers are present. */
 	*lpc_status |= EC_MEMMAP_ACC_STATUS_PRESENCE_BIT;

 	while (1) {
 		ts0 = get_time();

 		/* Read all accelerations. */
 		lid->drv->read(lid->drv_data, &acc_lid_raw[X], &acc_lid_raw[Y],
 			   &acc_lid_raw[Z]);
 		base->drv->read(base->drv_data, &acc_base[X], &acc_base[Y],
 			   &acc_base[Z]);

 		/*
 		 * Rotate the lid vector so the reference frame aligns with
 		 * the base sensor.
 		 */
 		rotate(acc_lid_raw, &p_acc_orient->rot_align, &acc_lid);

 		/* Calculate angle of lid. */
 		lid_angle_is_reliable = calculate_lid_angle(acc_base, acc_lid,
 				&lid_angle_deg);

 		/* TODO(crosbug.com/p/25597): Add filter to smooth lid angle. */

 		/* Rotate accels into standard reference frame for the host. */
 		rotate(acc_base, &p_acc_orient->rot_standard_ref,
 				&acc_base_host);
 		rotate(acc_lid, &p_acc_orient->rot_standard_ref,
 				&acc_lid_host);

 		/*
 		 * Set the busy bit before writing the sensor data. Increment
 		 * the counter and clear the busy bit after writing the sensor
 		 * data. On the host side, the host needs to make sure the busy
 		 * bit is not set and that the counter remains the same before
 		 * and after reading the data.
 		 */
 		*lpc_status |= EC_MEMMAP_ACC_STATUS_BUSY_BIT;

 		/*
 		 * Copy sensor data to shared memory. Note that this code
 		 * assumes little endian, which is what the host expects. Also,
 		 * note that we share the lid angle calculation with host only
 		 * for debugging purposes. The EC lid angle is an approximation
 		 * with un-calibrated accels. The AP calculates a separate,
 		 * more accurate lid angle.
 		 */
 		lpc_data[0] = motion_get_lid_angle();
 		lpc_data[1] = acc_base_host[X];
 		lpc_data[2] = acc_base_host[Y];
 		lpc_data[3] = acc_base_host[Z];
 		lpc_data[4] = acc_lid_host[X];
 		lpc_data[5] = acc_lid_host[Y];
 		lpc_data[6] = acc_lid_host[Z];

 		/*
 		 * Increment sample id and clear busy bit to signal we finished
 		 * updating data.
 		 */
 		sample_id = (sample_id + 1) &
 				EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
 		*lpc_status = EC_MEMMAP_ACC_STATUS_PRESENCE_BIT | sample_id;

 #ifdef CONFIG_LID_ANGLE_KEY_SCAN
 		lidangle_keyscan_update(motion_get_lid_angle());
 #endif

 #ifdef CONFIG_CMD_LID_ANGLE
 		if (accel_disp) {
 			CPRINTS("ACC base=%-5d, %-5d, %-5d  lid=%-5d, "
 					"%-5d, %-5d  a=%-6.1d r=%d",
 					acc_base[X], acc_base[Y], acc_base[Z],
 					acc_lid[X], acc_lid[Y], acc_lid[Z],
 					(int)(10*lid_angle_deg),
 					lid_angle_is_reliable);
 		}
 #endif

 		/* Delay appropriately to keep sampling time consistent. */
 		ts1 = get_time();
 		wait_us = accel_interval_ms * MSEC - (ts1.val-ts0.val);

 		/*
 		 * Guarantee some minimum delay to allow other lower priority
 		 * tasks to run.
 		 */
 		if (wait_us < MIN_MOTION_SENSE_WAIT_TIME)
 			wait_us = MIN_MOTION_SENSE_WAIT_TIME;

 		task_wait_event(wait_us);
 	}
 }

 void accel_int_lid(enum gpio_signal signal)
 {
 	/*
 	 * Print statement is here for testing with console accelint command.
 	 * Remove print statement when interrupt is used for real.
 	 */
 	CPRINTS("Accelerometer wake-up interrupt occurred on lid");
 }

 void accel_int_base(enum gpio_signal signal)
 {
 	/*
 	 * Print statement is here for testing with console accelint command.
 	 * Remove print statement when interrupt is used for real.
 	 */
 	CPRINTS("Accelerometer wake-up interrupt occurred on base");
 }

 /*****************************************************************************/
 /* Host commands */

 /* Function to map host sensor IDs to motion sensor. */
 static const struct motion_sensor_t
 	*host_sensor_id_to_motion_sensor(int host_id)
 {
 	switch (host_id) {
 	case EC_MOTION_SENSOR_ACCEL_BASE:
 		return base;
 	case EC_MOTION_SENSOR_ACCEL_LID:
 		return lid;
 	}

 	/* If no match then the EC currently doesn't support ID received. */
 	return NULL;
 }

 static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 {
 	const struct ec_params_motion_sense *in = args->params;
 	struct ec_response_motion_sense *out = args->response;
 	const struct motion_sensor_t *sensor;
 	int data;

 	switch (in->cmd) {
 	case MOTIONSENSE_CMD_DUMP:
 		/*
 		 * TODO(crosbug.com/p/27320): Need to remove hard coding and
 		 * use some motion_sense data structure from the board file to
 		 * help fill in this response.
 		 */
 		out->dump.module_flags =
 			(*(host_get_memmap(EC_MEMMAP_ACC_STATUS)) &
 				EC_MEMMAP_ACC_STATUS_PRESENCE_BIT) ?
 					MOTIONSENSE_MODULE_FLAG_ACTIVE : 0;
 		out->dump.sensor_flags[0] = MOTIONSENSE_SENSOR_FLAG_PRESENT;
 		out->dump.sensor_flags[1] = MOTIONSENSE_SENSOR_FLAG_PRESENT;
 		out->dump.sensor_flags[2] = 0;
 		out->dump.data[0] = acc_base_host[X];
 		out->dump.data[1] = acc_base_host[Y];
 		out->dump.data[2] = acc_base_host[Z];
 		out->dump.data[3] = acc_lid_host[X];
 		out->dump.data[4] = acc_lid_host[Y];
 		out->dump.data[5] = acc_lid_host[Z];

 		args->response_size = sizeof(out->dump);
 		break;

 	case MOTIONSENSE_CMD_INFO:
 		/*
 		 * TODO(crosbug.com/p/27320): Need to remove hard coding and
 		 * use some motion_sense data structure from the board file to
 		 * help fill in this response.
 		 */
 		sensor = host_sensor_id_to_motion_sensor(
 			in->sensor_odr.sensor_num);
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;

 		if (sensor->drv->sensor_type == SENSOR_ACCELEROMETER)
 			out->info.type = MOTIONSENSE_TYPE_ACCEL;
 		else if (sensor->drv->sensor_type == SENSOR_GYRO)
 			out->info.type = MOTIONSENSE_TYPE_GYRO;

 		if (sensor->location == LOCATION_BASE)
 			out->info.location = MOTIONSENSE_LOC_BASE;
 		else if (sensor->location == LOCATION_LID)
 			out->info.location = MOTIONSENSE_LOC_LID;

 		if (sensor->drv->chip_type == CHIP_KXCJ9)
 			out->info.chip = MOTIONSENSE_CHIP_KXCJ9;
 		else if (sensor->drv->chip_type == CHIP_LSM6DS0)
 			out->info.chip = MOTIONSENSE_CHIP_LSM6DS0;

 		args->response_size = sizeof(out->info);
 		break;

 	case MOTIONSENSE_CMD_EC_RATE:
 		/*
 		 * Set new sensor sampling rate when AP is on, if the data arg
 		 * has a value.
 		 */
 		if (in->ec_rate.data != EC_MOTION_SENSE_NO_VALUE) {
 			/* Bound the new sampling rate. */
 			data = in->ec_rate.data;
 			if (data < MIN_POLLING_INTERVAL_MS)
 				data = MIN_POLLING_INTERVAL_MS;
 			if (data > MAX_POLLING_INTERVAL_MS)
 				data = MAX_POLLING_INTERVAL_MS;

 			accel_interval_ap_on_ms = data;
 			accel_interval_ms = data;
 		}

 		out->ec_rate.ret = accel_interval_ap_on_ms;

 		args->response_size = sizeof(out->ec_rate);
 		break;

 	case MOTIONSENSE_CMD_SENSOR_ODR:
 		/* Verify sensor number is valid. */
 		sensor = host_sensor_id_to_motion_sensor(
 			in->sensor_odr.sensor_num);
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;

 		/* Set new datarate if the data arg has a value. */
 		if (in->sensor_odr.data != EC_MOTION_SENSE_NO_VALUE) {
 			if (sensor->drv->set_datarate(sensor->drv_data,
 						      in->sensor_odr.data,
 						      in->sensor_odr.roundup)
 						      != EC_SUCCESS) {
 				CPRINTS("MS bad sensor rate %d",
 						in->sensor_odr.data);
 				return EC_RES_INVALID_PARAM;
 			}
 		}

 		sensor->drv->get_datarate(sensor->drv_data, &data);
 		out->sensor_odr.ret = data;

 		args->response_size = sizeof(out->sensor_odr);
 		break;

 	case MOTIONSENSE_CMD_SENSOR_RANGE:
 		/* Verify sensor number is valid. */
 		sensor = host_sensor_id_to_motion_sensor(
 			in->sensor_odr.sensor_num);
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;

 		/* Set new datarate if the data arg has a value. */
 		if (in->sensor_range.data != EC_MOTION_SENSE_NO_VALUE) {
 			if (sensor->drv->set_range(sensor->drv_data,
 						   in->sensor_range.data,
 						   in->sensor_range.roundup)
 						   != EC_SUCCESS) {
 				CPRINTS("MS bad sensor range %d",
 						in->sensor_range.data);
 				return EC_RES_INVALID_PARAM;
 			}
 		}

 		sensor->drv->get_range(sensor->drv_data, &data);
 		out->sensor_range.ret = data;

 		args->response_size = sizeof(out->sensor_range);
 		break;

 	case MOTIONSENSE_CMD_KB_WAKE_ANGLE:
 #ifdef CONFIG_LID_ANGLE_KEY_SCAN
 		/* Set new keyboard wake lid angle if data arg has value. */
 		if (in->kb_wake_angle.data != EC_MOTION_SENSE_NO_VALUE)
 			lid_angle_set_kb_wake_angle(in->kb_wake_angle.data);

 		out->kb_wake_angle.ret = lid_angle_get_kb_wake_angle();
 #else
 		out->kb_wake_angle.ret = 0;
 #endif
 		args->response_size = sizeof(out->kb_wake_angle);

 		break;

 	default:
 		CPRINTS("MS bad cmd 0x%x", in->cmd);
 		return EC_RES_INVALID_PARAM;
 	}

 	return EC_RES_SUCCESS;
 }

 DECLARE_HOST_COMMAND(EC_CMD_MOTION_SENSE_CMD,
 		     host_cmd_motion_sense,
 		     EC_VER_MASK(0));

 /*****************************************************************************/
 /* Console commands */
 #ifdef CONFIG_CMD_LID_ANGLE
 static int command_ctrl_print_lid_angle_calcs(int argc, char **argv)
 {
 	char *e;
 	int val;

 	if (argc > 3)
 		return EC_ERROR_PARAM_COUNT;

 	/* First argument is on/off whether to display accel data. */
 	if (argc > 1) {
 		if (!parse_bool(argv[1], &val))
 			return EC_ERROR_PARAM1;

 		accel_disp = val;
 	}

 	/*
 	 * Second arg changes the accel task time interval. Note accel
 	 * sampling interval will be clobbered when chipset suspends or
 	 * resumes.
 	 */
 	if (argc > 2) {
 		val = strtoi(argv[2], &e, 0);
 		if (*e)
 			return EC_ERROR_PARAM2;

 		accel_interval_ms = val;
 	}

 	return EC_SUCCESS;
 }
 DECLARE_CONSOLE_COMMAND(lidangle, command_ctrl_print_lid_angle_calcs,
 	"on/off [interval]",
 	"Print lid angle calculations and set calculation frequency.", NULL);
 #endif /* CONFIG_CMD_LID_ANGLE */

 #ifdef CONFIG_CMD_ACCELS
 static int command_accelrange(int argc, char **argv)
 {
 	char *e;
 	int id, data, round = 1;
 	struct motion_sensor_t *sensor;

 	if (argc < 2 || argc > 4)
 		return EC_ERROR_PARAM_COUNT;

 	/* First argument is sensor id. */
 	id = strtoi(argv[1], &e, 0);
 	if (*e || id < 0 || id > motion_sensor_count)
 		return EC_ERROR_PARAM1;
 	sensor = motion_sensors[id];

 	if (argc >= 3) {
 		/* Second argument is data to write. */
 		data = strtoi(argv[2], &e, 0);
 		if (*e)
 			return EC_ERROR_PARAM2;

 		if (argc == 4) {
 			/* Third argument is rounding flag. */
 			round = strtoi(argv[3], &e, 0);
 			if (*e)
 				return EC_ERROR_PARAM3;
 		}

 		/*
 		 * Write new range, if it returns invalid arg, then return
 		 * a parameter error.
 		 */
 		if (sensor->drv->set_range(sensor->drv_data,
 					   data,
 					   round) == EC_ERROR_INVAL)
 			return EC_ERROR_PARAM2;
 	} else {
 		sensor->drv->get_range(sensor->drv_data, &data);
 		ccprintf("Range for sensor %d: %d\n", id, data);
 	}

 	return EC_SUCCESS;
 }
 DECLARE_CONSOLE_COMMAND(accelrange, command_accelrange,
 	"id [data [roundup]]",
 	"Read or write accelerometer range", NULL);

 static int command_accelresolution(int argc, char **argv)
 {
 	char *e;
 	int id, data, round = 1;
 	struct motion_sensor_t *sensor;

 	if (argc < 2 || argc > 4)
 		return EC_ERROR_PARAM_COUNT;

 	/* First argument is sensor id. */
 	id = strtoi(argv[1], &e, 0);
 	if (*e || id < 0 || id > motion_sensor_count)
 		return EC_ERROR_PARAM1;
 	sensor = motion_sensors[id];

 	if (argc >= 3) {
 		/* Second argument is data to write. */
 		data = strtoi(argv[2], &e, 0);
 		if (*e)
 			return EC_ERROR_PARAM2;

 		if (argc == 4) {
 			/* Third argument is rounding flag. */
 			round = strtoi(argv[3], &e, 0);
 			if (*e)
 				return EC_ERROR_PARAM3;
 		}

 		/*
 		 * Write new resolution, if it returns invalid arg, then
 		 * return a parameter error.
 		 */
 		if (sensor->drv->set_resolution(sensor->drv_data, data, round)
 			== EC_ERROR_INVAL)
 			return EC_ERROR_PARAM2;
 	} else {
 		sensor->drv->get_resolution(sensor->drv_data, &data);
 		ccprintf("Resolution for sensor %d: %d\n", id, data);
 	}

 	return EC_SUCCESS;
 }
 DECLARE_CONSOLE_COMMAND(accelres, command_accelresolution,
 	"id [data [roundup]]",
 	"Read or write accelerometer resolution", NULL);

 static int command_acceldatarate(int argc, char **argv)
 {
 	char *e;
 	int id, data, round = 1;
 	struct motion_sensor_t *sensor;

 	if (argc < 2 || argc > 4)
 		return EC_ERROR_PARAM_COUNT;

 	/* First argument is sensor id. */
 	id = strtoi(argv[1], &e, 0);
 	if (*e || id < 0 || id > motion_sensor_count)
 		return EC_ERROR_PARAM1;
 	sensor = motion_sensors[id];

 	if (argc >= 3) {
 		/* Second argument is data to write. */
 		data = strtoi(argv[2], &e, 0);
 		if (*e)
 			return EC_ERROR_PARAM2;

 		if (argc == 4) {
 			/* Third argument is rounding flag. */
 			round = strtoi(argv[3], &e, 0);
 			if (*e)
 				return EC_ERROR_PARAM3;
 		}

 		/*
 		 * Write new data rate, if it returns invalid arg, then
 		 * return a parameter error.
 		 */
 		if (sensor->drv->set_datarate(sensor->drv_data, data, round)
 			== EC_ERROR_INVAL)
 			return EC_ERROR_PARAM2;
 	} else {
 		sensor->drv->get_datarate(sensor->drv_data, &data);
 		ccprintf("Data rate for sensor %d: %d\n", id, data);
 	}

 	return EC_SUCCESS;
 }
 DECLARE_CONSOLE_COMMAND(accelrate, command_acceldatarate,
 	"id [data [roundup]]",
 	"Read or write accelerometer range", NULL);

 #ifdef CONFIG_ACCEL_INTERRUPTS
 static int command_accelerometer_interrupt(int argc, char **argv)
 {
 	char *e;
 	int id, thresh;
 	struct motion_sensor_t *sensor;

 	if (argc != 3)
 		return EC_ERROR_PARAM_COUNT;

 	/* First argument is id. */
 	id = strtoi(argv[1], &e, 0);
 	if (*e || id < 0 || id >= motion_sensor_count)
 		return EC_ERROR_PARAM1;
 	sensor = motion_sensors[id];

 	/* Second argument is interrupt threshold. */
 	thresh = strtoi(argv[2], &e, 0);
 	if (*e)
 		return EC_ERROR_PARAM2;

 	sensor->drv->set_interrupt(drv_data, thresh);

 	return EC_SUCCESS;
 }
 DECLARE_CONSOLE_COMMAND(accelint, command_accelerometer_interrupt,
 	"id threshold",
 	"Write interrupt threshold", NULL);
 #endif /* CONFIG_ACCEL_INTERRUPTS */

 #endif /* CONFIG_CMD_ACCELS */
	/* 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 sense module to read from various motion sensors. */

	#include "accelgyro.h"
	#include "common.h"
	#include "console.h"
	#include "hooks.h"
	#include "host_command.h"
	#include "lid_angle.h"
	#include "math_util.h"
	#include "motion_sense.h"
	#include "timer.h"
	#include "task.h"
	#include "util.h"

	/* Console output macros */
	#define CPUTS(outstr) cputs(CC_MOTION_SENSE, outstr)
	#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args)

	/* Minimum time in between running motion sense task loop. */
	#define MIN_MOTION_SENSE_WAIT_TIME (1 * MSEC)

	static const struct motion_sensor_t *base;
	static const struct motion_sensor_t *lid;

	/* Current acceleration vectors and current lid angle. */
	static vector_3_t acc_lid_raw, acc_lid, acc_base;
	static vector_3_t acc_lid_host, acc_base_host;
	static float lid_angle_deg;
	static int lid_angle_is_reliable;

	/* Bounds for setting the sensor polling interval. */
	#define MIN_POLLING_INTERVAL_MS 5
	#define MAX_POLLING_INTERVAL_MS 1000

	/* Accelerometer polling intervals based on chipset state. */
	static int accel_interval_ap_on_ms = 10;
	static const int accel_interval_ap_suspend_ms = 100;

	/*
	* Angle threshold for how close the hinge aligns with gravity before
	* considering the lid angle calculation unreliable. For computational
	* 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

	/* Sampling interval for measuring acceleration and calculating lid angle. */
	static int accel_interval_ms;

	#ifdef CONFIG_CMD_LID_ANGLE
	static int accel_disp;
	#endif

	/* For vector_3_t, define which coordinates are in which location. */
	enum {
	X, Y, Z
	};

	/* Pointer to constant acceleration orientation data. */
	const struct accel_orientation * const p_acc_orient = &acc_orient;

	/**
	* Calculate the lid angle using two acceleration vectors, one recorded in
	* the base and one in the lid.
	*
	* @param base Base accel vector
	* @param lid Lid accel vector
	* @param lid_angle Pointer to location to store lid angle result
	*
	* @return flag representing if resulting lid angle calculation is reliable.
	*/
	static int calculate_lid_angle(vector_3_t base, vector_3_t lid,
	float *lid_angle)
	{
	vector_3_t v;
	float ang_lid_to_base, ang_lid_90, ang_lid_270;
	float lid_to_base, base_to_hinge;
	int reliable = 1;

	/*
	* The angle between lid and base is:
	* acos((cad(base, lid) - cad(base, hinge)^2) /(1 - cad(base, hinge)^2))
	* where cad() is the cosine_of_angle_diff() function.
	*
	* Make sure to check for divide by 0.
	*/
	lid_to_base = cosine_of_angle_diff(base, lid);
	base_to_hinge = cosine_of_angle_diff(base, p_acc_orient->hinge_axis);

	/*
	* If hinge aligns too closely with gravity, then result may be
	* unreliable.
	*/
	if (ABS(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD)
	reliable = 0;

	base_to_hinge = SQ(base_to_hinge);

	/* Check divide by 0. */
	if (ABS(1.0F - base_to_hinge) < 0.01F) {
	*lid_angle = 0.0;
	return 0;
	}

	ang_lid_to_base = arc_cos(
	(lid_to_base - base_to_hinge) / (1 - base_to_hinge));

	/*
	* 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.
	*/
	rotate(base, &p_acc_orient->rot_hinge_90, &v);
	ang_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);

	/*
	* Note that ang_lid_90 and ang_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)
	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;

	*lid_angle = ang_lid_to_base;
	return reliable;
	}

	int motion_get_lid_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);
	else
	return (int)LID_ANGLE_UNRELIABLE;
	}

	#ifdef CONFIG_ACCEL_CALIBRATE
	void motion_get_accel_lid(vector_3_t *v, int adjusted)
	{
	memcpy(v, adjusted ? &acc_lid : &acc_lid_raw, sizeof(vector_3_t));
	}

	void motion_get_accel_base(vector_3_t *v)
	{
	memcpy(v, &acc_base, sizeof(vector_3_t));
	}
	#endif

	static void set_ap_suspend_polling(void)
	{
	accel_interval_ms = accel_interval_ap_suspend_ms;
	}
	DECLARE_HOOK(HOOK_CHIPSET_SUSPEND, set_ap_suspend_polling, HOOK_PRIO_DEFAULT);

	static void set_ap_on_polling(void)
	{
	accel_interval_ms = accel_interval_ap_on_ms;
	}
	DECLARE_HOOK(HOOK_CHIPSET_RESUME, set_ap_on_polling, HOOK_PRIO_DEFAULT);


	void motion_sense_task(void)
	{
	static timestamp_t ts0, ts1;
	int wait_us;
	int ret;
	uint8_t *lpc_status;
	uint16_t *lpc_data;
	int sample_id = 0;
	int i;

	lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
	lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA);

	/*
	* TODO(crosbug.com/p/27320): The motion_sense task currently assumes
	* one configuration of motion sensors. Namely, it assumes there is
	* one accel in the base, one in the lid. Eventually, these
	* assumptions will have to be removed when we have other
	* configurations of motion sensors.
	*/
	for (i = 0; i < motion_sensor_count; ++i) {
	if (motion_sensors[i].location == LOCATION_LID)
	lid = &motion_sensors[i];
	else if (motion_sensors[i].location == LOCATION_BASE)
	base = &motion_sensors[i];
	}

	if (lid == NULL \|\| base == NULL) {
	CPRINTS("Invalid motion_sensors list, lid and base required");
	return;
	}

	/* Initialize accelerometers. */
	ret = lid->drv->init(lid->drv_data, lid->i2c_addr);
	ret \|= base->drv->init(base->drv_data, base->i2c_addr);

	/* If accelerometers do not initialize, then end task. */
	if (ret != EC_SUCCESS) {
	CPRINTS("Accel init failed; stopping MS");
	return;
	}

	/* Initialize sampling interval. */
	accel_interval_ms = accel_interval_ap_suspend_ms;

	/* Set default accelerometer parameters. */
	lid->drv->set_range(lid->drv_data, 2, 1);
	lid->drv->set_resolution(lid->drv_data, 12, 1);
	lid->drv->set_datarate(lid->drv_data, 100000, 1);
	base->drv->set_range(base->drv_data, 2, 1);
	base->drv->set_resolution(base->drv_data, 12, 1);
	base->drv->set_datarate(base->drv_data, 100000, 1);

	/* Write to status byte to represent that accelerometers are present. */
	*lpc_status \|= EC_MEMMAP_ACC_STATUS_PRESENCE_BIT;

	while (1) {
	ts0 = get_time();

	/* Read all accelerations. */
	lid->drv->read(lid->drv_data, &acc_lid_raw[X], &acc_lid_raw[Y],
	&acc_lid_raw[Z]);
	base->drv->read(base->drv_data, &acc_base[X], &acc_base[Y],
	&acc_base[Z]);

	/*
	* Rotate the lid vector so the reference frame aligns with
	* the base sensor.
	*/
	rotate(acc_lid_raw, &p_acc_orient->rot_align, &acc_lid);

	/* Calculate angle of lid. */
	lid_angle_is_reliable = calculate_lid_angle(acc_base, acc_lid,
	&lid_angle_deg);

	/* TODO(crosbug.com/p/25597): Add filter to smooth lid angle. */

	/* Rotate accels into standard reference frame for the host. */
	rotate(acc_base, &p_acc_orient->rot_standard_ref,
	&acc_base_host);
	rotate(acc_lid, &p_acc_orient->rot_standard_ref,
	&acc_lid_host);

	/*
	* Set the busy bit before writing the sensor data. Increment
	* the counter and clear the busy bit after writing the sensor
	* data. On the host side, the host needs to make sure the busy
	* bit is not set and that the counter remains the same before
	* and after reading the data.
	*/
	*lpc_status \|= EC_MEMMAP_ACC_STATUS_BUSY_BIT;

	/*
	* Copy sensor data to shared memory. Note that this code
	* assumes little endian, which is what the host expects. Also,
	* note that we share the lid angle calculation with host only
	* for debugging purposes. The EC lid angle is an approximation
	* with un-calibrated accels. The AP calculates a separate,
	* more accurate lid angle.
	*/
	lpc_data[0] = motion_get_lid_angle();
	lpc_data[1] = acc_base_host[X];
	lpc_data[2] = acc_base_host[Y];
	lpc_data[3] = acc_base_host[Z];
	lpc_data[4] = acc_lid_host[X];
	lpc_data[5] = acc_lid_host[Y];
	lpc_data[6] = acc_lid_host[Z];

	/*
	* Increment sample id and clear busy bit to signal we finished
	* updating data.
	*/
	sample_id = (sample_id + 1) &
	EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
	*lpc_status = EC_MEMMAP_ACC_STATUS_PRESENCE_BIT \| sample_id;

	#ifdef CONFIG_LID_ANGLE_KEY_SCAN
	lidangle_keyscan_update(motion_get_lid_angle());
	#endif

	#ifdef CONFIG_CMD_LID_ANGLE
	if (accel_disp) {
	CPRINTS("ACC base=%-5d, %-5d, %-5d lid=%-5d, "
	"%-5d, %-5d a=%-6.1d r=%d",
	acc_base[X], acc_base[Y], acc_base[Z],
	acc_lid[X], acc_lid[Y], acc_lid[Z],
	(int)(10*lid_angle_deg),
	lid_angle_is_reliable);
	}
	#endif

	/* Delay appropriately to keep sampling time consistent. */
	ts1 = get_time();
	wait_us = accel_interval_ms * MSEC - (ts1.val-ts0.val);

	/*
	* Guarantee some minimum delay to allow other lower priority
	* tasks to run.
	*/
	if (wait_us < MIN_MOTION_SENSE_WAIT_TIME)
	wait_us = MIN_MOTION_SENSE_WAIT_TIME;

	task_wait_event(wait_us);
	}
	}

	void accel_int_lid(enum gpio_signal signal)
	{
	/*
	* Print statement is here for testing with console accelint command.
	* Remove print statement when interrupt is used for real.
	*/
	CPRINTS("Accelerometer wake-up interrupt occurred on lid");
	}

	void accel_int_base(enum gpio_signal signal)
	{
	/*
	* Print statement is here for testing with console accelint command.
	* Remove print statement when interrupt is used for real.
	*/
	CPRINTS("Accelerometer wake-up interrupt occurred on base");
	}

	/*****************************************************************************/
	/* Host commands */

	/* Function to map host sensor IDs to motion sensor. */
	static const struct motion_sensor_t
	*host_sensor_id_to_motion_sensor(int host_id)
	{
	switch (host_id) {
	case EC_MOTION_SENSOR_ACCEL_BASE:
	return base;
	case EC_MOTION_SENSOR_ACCEL_LID:
	return lid;
	}

	/* If no match then the EC currently doesn't support ID received. */
	return NULL;
	}

	static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
	{
	const struct ec_params_motion_sense *in = args->params;
	struct ec_response_motion_sense *out = args->response;
	const struct motion_sensor_t *sensor;
	int data;

	switch (in->cmd) {
	case MOTIONSENSE_CMD_DUMP:
	/*
	* TODO(crosbug.com/p/27320): Need to remove hard coding and
	* use some motion_sense data structure from the board file to
	* help fill in this response.
	*/
	out->dump.module_flags =
	(*(host_get_memmap(EC_MEMMAP_ACC_STATUS)) &
	EC_MEMMAP_ACC_STATUS_PRESENCE_BIT) ?
	MOTIONSENSE_MODULE_FLAG_ACTIVE : 0;
	out->dump.sensor_flags[0] = MOTIONSENSE_SENSOR_FLAG_PRESENT;
	out->dump.sensor_flags[1] = MOTIONSENSE_SENSOR_FLAG_PRESENT;
	out->dump.sensor_flags[2] = 0;
	out->dump.data[0] = acc_base_host[X];
	out->dump.data[1] = acc_base_host[Y];
	out->dump.data[2] = acc_base_host[Z];
	out->dump.data[3] = acc_lid_host[X];
	out->dump.data[4] = acc_lid_host[Y];
	out->dump.data[5] = acc_lid_host[Z];

	args->response_size = sizeof(out->dump);
	break;

	case MOTIONSENSE_CMD_INFO:
	/*
	* TODO(crosbug.com/p/27320): Need to remove hard coding and
	* use some motion_sense data structure from the board file to
	* help fill in this response.
	*/
	sensor = host_sensor_id_to_motion_sensor(
	in->sensor_odr.sensor_num);
	if (sensor == NULL)
	return EC_RES_INVALID_PARAM;

	if (sensor->drv->sensor_type == SENSOR_ACCELEROMETER)
	out->info.type = MOTIONSENSE_TYPE_ACCEL;
	else if (sensor->drv->sensor_type == SENSOR_GYRO)
	out->info.type = MOTIONSENSE_TYPE_GYRO;

	if (sensor->location == LOCATION_BASE)
	out->info.location = MOTIONSENSE_LOC_BASE;
	else if (sensor->location == LOCATION_LID)
	out->info.location = MOTIONSENSE_LOC_LID;

	if (sensor->drv->chip_type == CHIP_KXCJ9)
	out->info.chip = MOTIONSENSE_CHIP_KXCJ9;
	else if (sensor->drv->chip_type == CHIP_LSM6DS0)
	out->info.chip = MOTIONSENSE_CHIP_LSM6DS0;

	args->response_size = sizeof(out->info);
	break;

	case MOTIONSENSE_CMD_EC_RATE:
	/*
	* Set new sensor sampling rate when AP is on, if the data arg
	* has a value.
	*/
	if (in->ec_rate.data != EC_MOTION_SENSE_NO_VALUE) {
	/* Bound the new sampling rate. */
	data = in->ec_rate.data;
	if (data < MIN_POLLING_INTERVAL_MS)
	data = MIN_POLLING_INTERVAL_MS;
	if (data > MAX_POLLING_INTERVAL_MS)
	data = MAX_POLLING_INTERVAL_MS;

	accel_interval_ap_on_ms = data;
	accel_interval_ms = data;
	}

	out->ec_rate.ret = accel_interval_ap_on_ms;

	args->response_size = sizeof(out->ec_rate);
	break;

	case MOTIONSENSE_CMD_SENSOR_ODR:
	/* Verify sensor number is valid. */
	sensor = host_sensor_id_to_motion_sensor(
	in->sensor_odr.sensor_num);
	if (sensor == NULL)
	return EC_RES_INVALID_PARAM;

	/* Set new datarate if the data arg has a value. */
	if (in->sensor_odr.data != EC_MOTION_SENSE_NO_VALUE) {
	if (sensor->drv->set_datarate(sensor->drv_data,
	in->sensor_odr.data,
	in->sensor_odr.roundup)
	!= EC_SUCCESS) {
	CPRINTS("MS bad sensor rate %d",
	in->sensor_odr.data);
	return EC_RES_INVALID_PARAM;
	}
	}

	sensor->drv->get_datarate(sensor->drv_data, &data);
	out->sensor_odr.ret = data;

	args->response_size = sizeof(out->sensor_odr);
	break;

	case MOTIONSENSE_CMD_SENSOR_RANGE:
	/* Verify sensor number is valid. */
	sensor = host_sensor_id_to_motion_sensor(
	in->sensor_odr.sensor_num);
	if (sensor == NULL)
	return EC_RES_INVALID_PARAM;

	/* Set new datarate if the data arg has a value. */
	if (in->sensor_range.data != EC_MOTION_SENSE_NO_VALUE) {
	if (sensor->drv->set_range(sensor->drv_data,
	in->sensor_range.data,
	in->sensor_range.roundup)
	!= EC_SUCCESS) {
	CPRINTS("MS bad sensor range %d",
	in->sensor_range.data);
	return EC_RES_INVALID_PARAM;
	}
	}

	sensor->drv->get_range(sensor->drv_data, &data);
	out->sensor_range.ret = data;

	args->response_size = sizeof(out->sensor_range);
	break;

	case MOTIONSENSE_CMD_KB_WAKE_ANGLE:
	#ifdef CONFIG_LID_ANGLE_KEY_SCAN
	/* Set new keyboard wake lid angle if data arg has value. */
	if (in->kb_wake_angle.data != EC_MOTION_SENSE_NO_VALUE)
	lid_angle_set_kb_wake_angle(in->kb_wake_angle.data);

	out->kb_wake_angle.ret = lid_angle_get_kb_wake_angle();
	#else
	out->kb_wake_angle.ret = 0;
	#endif
	args->response_size = sizeof(out->kb_wake_angle);

	break;

	default:
	CPRINTS("MS bad cmd 0x%x", in->cmd);
	return EC_RES_INVALID_PARAM;
	}

	return EC_RES_SUCCESS;
	}

	DECLARE_HOST_COMMAND(EC_CMD_MOTION_SENSE_CMD,
	host_cmd_motion_sense,
	EC_VER_MASK(0));

	/*****************************************************************************/
	/* Console commands */
	#ifdef CONFIG_CMD_LID_ANGLE
	static int command_ctrl_print_lid_angle_calcs(int argc, char **argv)
	{
	char *e;
	int val;

	if (argc > 3)
	return EC_ERROR_PARAM_COUNT;

	/* First argument is on/off whether to display accel data. */
	if (argc > 1) {
	if (!parse_bool(argv[1], &val))
	return EC_ERROR_PARAM1;

	accel_disp = val;
	}

	/*
	* Second arg changes the accel task time interval. Note accel
	* sampling interval will be clobbered when chipset suspends or
	* resumes.
	*/
	if (argc > 2) {
	val = strtoi(argv[2], &e, 0);
	if (*e)
	return EC_ERROR_PARAM2;

	accel_interval_ms = val;
	}

	return EC_SUCCESS;
	}
	DECLARE_CONSOLE_COMMAND(lidangle, command_ctrl_print_lid_angle_calcs,
	"on/off [interval]",
	"Print lid angle calculations and set calculation frequency.", NULL);
	#endif /* CONFIG_CMD_LID_ANGLE */

	#ifdef CONFIG_CMD_ACCELS
	static int command_accelrange(int argc, char **argv)
	{
	char *e;
	int id, data, round = 1;
	struct motion_sensor_t *sensor;

	if (argc < 2 \|\| argc > 4)
	return EC_ERROR_PARAM_COUNT;

	/* First argument is sensor id. */
	id = strtoi(argv[1], &e, 0);
	if (*e \|\| id < 0 \|\| id > motion_sensor_count)
	return EC_ERROR_PARAM1;
	sensor = motion_sensors[id];

	if (argc >= 3) {
	/* Second argument is data to write. */
	data = strtoi(argv[2], &e, 0);
	if (*e)
	return EC_ERROR_PARAM2;

	if (argc == 4) {
	/* Third argument is rounding flag. */
	round = strtoi(argv[3], &e, 0);
	if (*e)
	return EC_ERROR_PARAM3;
	}

	/*
	* Write new range, if it returns invalid arg, then return
	* a parameter error.
	*/
	if (sensor->drv->set_range(sensor->drv_data,
	data,
	round) == EC_ERROR_INVAL)
	return EC_ERROR_PARAM2;
	} else {
	sensor->drv->get_range(sensor->drv_data, &data);
	ccprintf("Range for sensor %d: %d\n", id, data);
	}

	return EC_SUCCESS;
	}
	DECLARE_CONSOLE_COMMAND(accelrange, command_accelrange,
	"id [data [roundup]]",
	"Read or write accelerometer range", NULL);

	static int command_accelresolution(int argc, char **argv)
	{
	char *e;
	int id, data, round = 1;
	struct motion_sensor_t *sensor;

	if (argc < 2 \|\| argc > 4)
	return EC_ERROR_PARAM_COUNT;

	/* First argument is sensor id. */
	id = strtoi(argv[1], &e, 0);
	if (*e \|\| id < 0 \|\| id > motion_sensor_count)
	return EC_ERROR_PARAM1;
	sensor = motion_sensors[id];

	if (argc >= 3) {
	/* Second argument is data to write. */
	data = strtoi(argv[2], &e, 0);
	if (*e)
	return EC_ERROR_PARAM2;

	if (argc == 4) {
	/* Third argument is rounding flag. */
	round = strtoi(argv[3], &e, 0);
	if (*e)
	return EC_ERROR_PARAM3;
	}

	/*
	* Write new resolution, if it returns invalid arg, then
	* return a parameter error.
	*/
	if (sensor->drv->set_resolution(sensor->drv_data, data, round)
	== EC_ERROR_INVAL)
	return EC_ERROR_PARAM2;
	} else {
	sensor->drv->get_resolution(sensor->drv_data, &data);
	ccprintf("Resolution for sensor %d: %d\n", id, data);
	}

	return EC_SUCCESS;
	}
	DECLARE_CONSOLE_COMMAND(accelres, command_accelresolution,
	"id [data [roundup]]",
	"Read or write accelerometer resolution", NULL);

	static int command_acceldatarate(int argc, char **argv)
	{
	char *e;
	int id, data, round = 1;
	struct motion_sensor_t *sensor;

	if (argc < 2 \|\| argc > 4)
	return EC_ERROR_PARAM_COUNT;

	/* First argument is sensor id. */
	id = strtoi(argv[1], &e, 0);
	if (*e \|\| id < 0 \|\| id > motion_sensor_count)
	return EC_ERROR_PARAM1;
	sensor = motion_sensors[id];

	if (argc >= 3) {
	/* Second argument is data to write. */
	data = strtoi(argv[2], &e, 0);
	if (*e)
	return EC_ERROR_PARAM2;

	if (argc == 4) {
	/* Third argument is rounding flag. */
	round = strtoi(argv[3], &e, 0);
	if (*e)
	return EC_ERROR_PARAM3;
	}

	/*
	* Write new data rate, if it returns invalid arg, then
	* return a parameter error.
	*/
	if (sensor->drv->set_datarate(sensor->drv_data, data, round)
	== EC_ERROR_INVAL)
	return EC_ERROR_PARAM2;
	} else {
	sensor->drv->get_datarate(sensor->drv_data, &data);
	ccprintf("Data rate for sensor %d: %d\n", id, data);
	}

	return EC_SUCCESS;
	}
	DECLARE_CONSOLE_COMMAND(accelrate, command_acceldatarate,
	"id [data [roundup]]",
	"Read or write accelerometer range", NULL);

	#ifdef CONFIG_ACCEL_INTERRUPTS
	static int command_accelerometer_interrupt(int argc, char **argv)
	{
	char *e;
	int id, thresh;
	struct motion_sensor_t *sensor;

	if (argc != 3)
	return EC_ERROR_PARAM_COUNT;

	/* First argument is id. */
	id = strtoi(argv[1], &e, 0);
	if (*e \|\| id < 0 \|\| id >= motion_sensor_count)
	return EC_ERROR_PARAM1;
	sensor = motion_sensors[id];

	/* Second argument is interrupt threshold. */
	thresh = strtoi(argv[2], &e, 0);
	if (*e)
	return EC_ERROR_PARAM2;

	sensor->drv->set_interrupt(drv_data, thresh);

	return EC_SUCCESS;
	}
	DECLARE_CONSOLE_COMMAND(accelint, command_accelerometer_interrupt,
	"id threshold",
	"Write interrupt threshold", NULL);
	#endif /* CONFIG_ACCEL_INTERRUPTS */

	#endif /* CONFIG_CMD_ACCELS */