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;
}
void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
struct motion_sensor_t *sensor,
int valid_data)
{
struct ec_response_motion_sensor_data vector;
int i;
data->sensor_num = sensor - motion_sensors;
mutex_lock(&g_sensor_mutex);
if (queue_space(&motion_sense_fifo) == 0) {
queue_remove_unit(&motion_sense_fifo, &vector);
motion_sense_fifo_lost++;
motion_sensors[vector.sensor_num].lost++;
if (vector.flags & MOTIONSENSE_SENSOR_FLAG_FLUSH)
CPRINTS("Lost flush for sensor %d", vector.sensor_num);
}
for (i = 0; i < valid_data; i++)
sensor->xyz[i] = data->data[i];
mutex_unlock(&g_sensor_mutex);
if (valid_data) {
int ap_odr = sensor->config[SENSOR_CONFIG_AP].odr &
~ROUND_UP_FLAG;
int rate = INT_TO_FP(sensor->drv->get_data_rate(sensor));
/* If the AP does not want sensor info, skip */
if (ap_odr == 0)
return;
/* Skip if EC is oversampling */
if (sensor->oversampling < 0) {
sensor->oversampling += fp_div(INT_TO_FP(1000), rate);
return;
}
sensor->oversampling += fp_div(INT_TO_FP(1000), rate) -
fp_div(INT_TO_FP(1000), INT_TO_FP(ap_odr));
}
queue_add_unit(&motion_sense_fifo, data);
}
static double
calc_output (SINC_FILTER *filter, increment_t increment, increment_t start_filter_index, int ch)
{ double fraction, left, right, icoeff ;
increment_t filter_index, max_filter_index ;
int data_index, coeff_count, indx ;
/* Convert input parameters into fixed point. */
max_filter_index = INT_TO_FP (filter->coeff_half_len) ;
/* First apply the left half of the filter. */
filter_index = start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current - filter->channels * coeff_count ;
left = 0.0 ;
do
{ fraction = FP_TO_DOUBLE (filter_index) ;
indx = FP_TO_INT (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
left += icoeff * filter->buffer [data_index + ch] ;
filter_index -= increment ;
data_index = data_index + filter->channels ;
}
while (filter_index >= MAKE_INCREMENT_T (0)) ;
/* Now apply the right half of the filter. */
filter_index = increment - start_filter_index ;
coeff_count = (max_filter_index - filter_index) / increment ;
filter_index = filter_index + coeff_count * increment ;
data_index = filter->b_current + filter->channels * (1 + coeff_count) ;
right = 0.0 ;
do
{ fraction = FP_TO_DOUBLE (filter_index) ;
indx = FP_TO_INT (filter_index) ;
icoeff = filter->coeffs [indx] + fraction * (filter->coeffs [indx + 1] - filter->coeffs [indx]) ;
right += icoeff * filter->buffer [data_index + ch] ;
filter_index -= increment ;
data_index = data_index - filter->channels ;
}
while (filter_index > MAKE_INCREMENT_T (0)) ;
return (left + right) ;
} /* calc_output */
