コード例 #1
0
ファイル: math_util.c プロジェクト: fourier49/BZ_DEV_EC
fp_t arc_cos(fp_t x)
{
	int i;

	/* Cap x if out of range. */
	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]) {
			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
	 * [-1, 1] and the cos_lut[] table goes over the same range. If we
	 * are here, throw an assert.
	 */
	ASSERT(0);

	return 0;
}
コード例 #2
0
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 = 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));
	}

	return EC_SUCCESS;
}
コード例 #3
0
ファイル: hwtimer.c プロジェクト: coreboot/chrome-ec
/* HWTimer event handlers */
void __hw_clock_event_set(uint32_t deadline)
{
	fp_t inv_evt_tick = FLOAT_TO_FP(INT_32K_CLOCK/(float)SECOND);
	int32_t  evt_cnt_us;
	/* Is deadline min value? */
	if (evt_expired_us != 0 && evt_expired_us < deadline)
		return;

	/* mark min event value */
	evt_expired_us = deadline;
	evt_cnt_us = deadline - __hw_clock_source_read();
#if DEBUG_TMR
	evt_cnt_us_dbg = deadline - __hw_clock_source_read();
#endif
	/* Deadline is behind current timer */
	if (evt_cnt_us < 0)
		evt_cnt_us = 1;

	/* Event module disable */
	CLEAR_BIT(NPCX_ITCTS(ITIM_EVENT_NO), NPCX_ITCTS_ITEN);

	/*
	 * ITIM count down : event expired : Unit: 1/32768 sec
	 * It must exceed evt_expired_us for process_timers function
	 */
	evt_cnt = FP_TO_INT((fp_inter_t)(evt_cnt_us) * inv_evt_tick);
	if (evt_cnt > TICK_EVT_MAX_CNT) {
		CPRINTS("Event overflow! 0x%08x, us is %d\r\n",
				evt_cnt, evt_cnt_us);
		evt_cnt = TICK_EVT_MAX_CNT;
	}

	/* Wait for module disable to take effect before updating count */
	while (IS_BIT_SET(NPCX_ITCTS(ITIM_EVENT_NO), NPCX_ITCTS_ITEN))
		;

	NPCX_ITCNT16(ITIM_EVENT_NO) = MAX(evt_cnt, 1);

	/* Event module enable */
	SET_BIT(NPCX_ITCTS(ITIM_EVENT_NO), NPCX_ITCTS_ITEN);

	/* Wait for module enable */
	while (!IS_BIT_SET(NPCX_ITCTS(ITIM_EVENT_NO), NPCX_ITCTS_ITEN))
		;

	/* Enable interrupt of ITIM */
	task_enable_irq(ITIM16_INT(ITIM_EVENT_NO));
}
コード例 #4
0
ファイル: hwtimer.c プロジェクト: coreboot/chrome-ec
/* Returns time delay cause of deep idle */
uint32_t __hw_clock_get_sleep_time(uint16_t pre_evt_cnt)
{
	fp_t evt_tick = FLOAT_TO_FP(SECOND/(float)INT_32K_CLOCK);
	uint32_t sleep_time;
	uint16_t cnt = __hw_clock_event_count();

	/* Event has been triggered but timer ISR doesn't handle it */
	if (IS_BIT_SET(NPCX_ITCTS(ITIM_EVENT_NO), NPCX_ITCTS_TO_STS))
		sleep_time = FP_TO_INT((fp_inter_t)(pre_evt_cnt+1) * evt_tick);
	/* Event hasn't been triggered */
	else
		sleep_time = FP_TO_INT((fp_inter_t)(pre_evt_cnt+1 - cnt) *
				       evt_tick);

	return sleep_time;
}
コード例 #5
0
ファイル: board.c プロジェクト: littlebabay/chrome-ec
	gpio_set_level(GPIO_PD_RST_L, 0);
	usleep(100);
	gpio_set_level(GPIO_PD_RST_L, 1);
}

/* Four Motion sensors */
/* kxcj9 mutex and local/private data*/
static struct mutex g_kxcj9_mutex[2];
struct kionix_accel_data g_kxcj9_data[2] = {
	{.variant = KXCJ9},
	{.variant = KXCJ9},
};

/* Matrix to rotate accelrator into standard reference frame */
const matrix_3x3_t base_standard_ref = {
	{ 0,  FLOAT_TO_FP(1),  0},
	{FLOAT_TO_FP(-1),  0,  0},
	{ 0,  0,  FLOAT_TO_FP(1)}
};

const matrix_3x3_t lid_standard_ref = {
	{FLOAT_TO_FP(-1),  0,  0},
	{ 0, FLOAT_TO_FP(-1),  0},
	{ 0,  0, FLOAT_TO_FP(-1)}
};

struct motion_sensor_t motion_sensors[] = {
	{.name = "Base Accel",
	 .active_mask = SENSOR_ACTIVE_S0,
	 .chip = MOTIONSENSE_CHIP_KXCJ9,
	 .type = MOTIONSENSE_TYPE_ACCEL,
コード例 #6
0
ファイル: math_util.c プロジェクト: fourier49/BZ_DEV_EC
#include "common.h"
#include "math.h"
#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)

/* Lookup table for the value of cosine from 0 degrees to 180 degrees. */
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);
コード例 #7
0
ファイル: motion_lid.c プロジェクト: fourier49/BZ_DEV_EC
/**
 * 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(const vector_3_t base, const vector_3_t lid,
			       int *lid_angle)
{
	vector_3_t v;
	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;

	/*
	 * 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 (fp_abs(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD)
		reliable = 0;

	base_to_hinge = fp_sq(base_to_hinge);

	/* Check divide by 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(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 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);
	cos_lid_90 = cosine_of_angle_diff(v, lid);
	rotate(v, p_acc_orient->rot_hinge_180, v);
	cos_lid_270 = cosine_of_angle_diff(v, lid);

	/*
	 * 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 (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 += 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));

	return reliable;
}
コード例 #8
0
	float a, b;
	float test;

	/* Test a handful of values. */
	for (test = -1.0; test <= 1.0; test += 0.01) {
		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));
	}

	return EC_SUCCESS;
}


const matrix_3x3_t test_matrices[] = {
	{{ 0, FLOAT_TO_FP(-1), 0},
	 {FLOAT_TO_FP(-1), 0, 0},
	 { 0, 0, FLOAT_TO_FP(1)} },
	{{ FLOAT_TO_FP(1), 0, FLOAT_TO_FP(5)},
	 { FLOAT_TO_FP(2), FLOAT_TO_FP(1), FLOAT_TO_FP(6)},
	 { FLOAT_TO_FP(3), FLOAT_TO_FP(4), 0} }
};


static int test_rotate(void)
{
	int i, j, k;
	vector_3_t v = {1, 2, 3};
	vector_3_t w;

	for (i = 0; i < ARRAY_SIZE(test_matrices); i++) {