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