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;
}
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;
}
/* 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));
}
/* 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;
}
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,
#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);
/**
* 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;
}
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++) {
