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 */
/* 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;
}
/* 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));
}
/**
* 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;
}
