Example #1
0
static void surround_update_filter(unsigned int fout)
{
    tcoef1 = fp_div(cutoff_l, fout, 31);
    tcoef2 = fp_div(cutoff_h, fout, 31);
    bcoef  = fp_div(cutoff_l / 2, fout, 31);
    hcoef  = fp_div(cutoff_h * 2, fout, 31);
}
Example #2
0
/* c = [a, b] */
void fp_lcm(fp_int *a, fp_int *b, fp_int *c)
{
   fp_int  t1, t2;

   fp_init(&t1);
   fp_init(&t2);
   fp_gcd(a, b, &t1);
   if (fp_cmp_mag(a, b) == FP_GT) {
      fp_div(a, &t1, &t2, NULL);
      fp_mul(b, &t2, c);
   } else {
      fp_div(b, &t1, &t2, NULL);
      fp_mul(a, &t2, c);
   }   
}
Example #3
0
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;
}
Example #4
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);
}
Example #5
0
static void pbe_update_filter(unsigned int fout)
{
    tcoef1 = fp_div(160, fout, 31);
    tcoef2 = fp_div(500, fout, 31);
    tcoef3 = fp_div(1150, fout, 31);
    /* Biophonic EQ */
    filter_bishelf_coefs(fp_div(20, fout, 32),
                         fp_div(16000, fout, 32),
                         0, 53, -5 + pbe_precut,
                         &pbe_filter[0]);
    filter_pk_coefs(fp_div(64, fout, 32), 28, 53,
                     &pbe_filter[1]);
    filter_pk_coefs(fp_div(2000, fout, 32), 28, 58,
                     &pbe_filter[2]);
    filter_pk_coefs(fp_div(7500, fout, 32), 43, -82,
                     &pbe_filter[3]);
    filter_pk_coefs(fp_div(10000, fout, 32), 43, -29,
                     &pbe_filter[4]);

}
Example #6
0
/** 
 * Calculate first order shelving filter. Filter is not directly usable by the
 * filter_process() function.
 * @param cutoff shelf midpoint frequency. See eq_pk_coefs for format.
 * @param A decibel value multiplied by ten, describing gain/attenuation of
 * shelf. Max value is 24 dB.
 * @param low true for low-shelf filter, false for high-shelf filter.
 * @param c pointer to coefficient storage. Coefficients are s4.27 format.
 */
void filter_shelf_coefs(unsigned long cutoff, long A, bool low, int32_t *c)
{
    long sin, cos;
    int32_t b0, b1, a0, a1; /* s3.28 */
    const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */

    sin = fp_sincos(cutoff/2, &cos);
    if (low) {
        const int32_t sin_div_g = fp_div(sin, g, 25);
        const int32_t sin_g = FRACMUL(sin, g);
        cos >>= 3;
        b0 = sin_g + cos;             /* 0.25 .. 4.10 */
        b1 = sin_g - cos;             /* -1 .. 3.98 */
        a0 = sin_div_g + cos;         /* 0.25 .. 4.10 */
        a1 = sin_div_g - cos;         /* -1 .. 3.98 */
    } else {
Example #7
0
/* c = a mod b, 0 <= c < b  */
int fp_mod(fp_int *a, fp_int *b, fp_int *c)
{
   fp_int t;
   int    err;

   fp_zero(&t);
   if ((err = fp_div(a, b, NULL, &t)) != FP_OKAY) {
      return err;
   }
   if (t.sign != b->sign) {
      fp_add(&t, b, c);
   } else {
      fp_copy(&t, c);
  }
  return FP_OKAY;
}
Example #8
0
int
main()
{
  if (fp_add (1, 1) != 2) fail ("fp_add 1+1");
  if (fp_sub (3, 2) != 1) fail ("fp_sub 3-2");
  if (fp_mul (2, 3) != 6) fail ("fp_mul 2*3");
  if (fp_div (3, 2) != 1.5) fail ("fp_div 3/2");
  if (fp_neg (1) != -1) fail ("fp_neg 1");

  if (dp_add (1, 1) != 2) fail ("dp_add 1+1");
  if (dp_sub (3, 2) != 1) fail ("dp_sub 3-2");
  if (dp_mul (2, 3) != 6) fail ("dp_mul 2*3");
  if (dp_div (3, 2) != 1.5) fail ("dp_div 3/2");
  if (dp_neg (1) != -1) fail ("dp_neg 1");

  if (fp_to_dp (1.5) != 1.5) fail ("fp_to_dp 1.5");
  if (dp_to_fp (1.5) != 1.5) fail ("dp_to_fp 1.5");

  if (floatsisf (1) != 1) fail ("floatsisf 1");
  if (floatsidf (1) != 1) fail ("floatsidf 1");
  if (fixsfsi (1.42) != 1) fail ("fixsfsi 1.42");
  if (fixunssfsi (1.42) != 1) fail ("fixunssfsi 1.42");
  if (fixdfsi (1.42) != 1) fail ("fixdfsi 1.42");
  if (fixunsdfsi (1.42) != 1) fail ("fixunsdfsi 1.42");

  if (eqsf2 (1, 1) == 0) fail ("eqsf2 1==1");
  if (eqsf2 (1, 2) != 0) fail ("eqsf2 1==2");
  if (nesf2 (1, 2) == 0) fail ("nesf2 1!=1");
  if (nesf2 (1, 1) != 0) fail ("nesf2 1!=1");
  if (gtsf2 (2, 1) == 0) fail ("gtsf2 2>1");
  if (gtsf2 (1, 1) != 0) fail ("gtsf2 1>1");
  if (gtsf2 (0, 1) != 0) fail ("gtsf2 0>1");
  if (gesf2 (2, 1) == 0) fail ("gesf2 2>=1");
  if (gesf2 (1, 1) == 0) fail ("gesf2 1>=1");
  if (gesf2 (0, 1) != 0) fail ("gesf2 0>=1");
  if (ltsf2 (1, 2) == 0) fail ("ltsf2 1<2");
  if (ltsf2 (1, 1) != 0) fail ("ltsf2 1<1");
  if (ltsf2 (1, 0) != 0) fail ("ltsf2 1<0");
  if (lesf2 (1, 2) == 0) fail ("lesf2 1<=2");
  if (lesf2 (1, 1) == 0) fail ("lesf2 1<=1");
  if (lesf2 (1, 0) != 0) fail ("lesf2 1<=0");

  if (fail_count != 0)
    abort ();
  exit (0);
}
Example #9
0
/* Computes log(x) and stores the result in out.
 * The current method of computation is as follows:
 * 1) Separate x into the form y * 10^n, where 1 <= y < 10.
 * 2) Approximate log(y) using a 5/5 Pade approximant.
 * 3) Compute log(10^n) via n * log(10).
 * 4) Since log(ab) = log(a) + log(b), compute final result by:
 *    log(y) + n log(10).
 */
fp_t fp_log(fp_t x) {
  fp_t num, div, tmp, p10;
  
  /* Extract the fractional part of x. */
  tmp      = x;
  tmp.expt = 0x7f;
  
  /* Compute the quotient approximation of frac(x) */
  num = fp_poly(QUOT_NUMERATOR, sizeof QUOT_NUMERATOR / sizeof QUOT_NUMERATOR[0], tmp);
  div = fp_poly(QUOT_DENOMINATOR, sizeof QUOT_DENOMINATOR / sizeof QUOT_DENOMINATOR[0], tmp);
  
  /* Compute the quotient. */
  tmp = fp_div(num, div);
  
  p10 = fp_fromint(x.expt - 0x7f);
  p10 = fp_mul(p10, FP_LOG10);
  
  tmp = fp_add(tmp, p10);
  return tmp;
}
Example #10
0
int main() {
  int count = numbers_cnt * numbers_cnt;
  int errors = 0;
  for (int i = 0; i < numbers_cnt; i++) {
    for (int j = 0; j < numbers_cnt; j++) {
      uint32_t v = v_div(numbers[i], numbers[j]);
      uint32_t fp = fp_div(numbers[i], numbers[j]);
      if (v != fp && errors < 10) {
        printf("div: %08x %08x => v %08x | fp %08x\n", numbers[i], numbers[j], v, fp);
      }
      errors += fp != v;
    }
    if ((i % 500) == 0) {
      int p = count * 100 / numbers_cnt / numbers_cnt;
      printf("div: %d%% (%d errors)\n", p, errors);
    }
  }
  printf("div: errors: %d tests: %d\n", errors, count);
  return errors != 0;
}
Example #11
0
{
    long sin, cos;
    int32_t b0, b1, a0, a1; /* s3.28 */
    const long g = get_replaygain_int(A*5) << 4; /* 10^(db/40), s3.28 */

    sin = fp_sincos(cutoff/2, &cos);
    if (low) {
        const int32_t sin_div_g = fp_div(sin, g, 25);
        const int32_t sin_g = FRACMUL(sin, g);
        cos >>= 3;
        b0 = sin_g + cos;             /* 0.25 .. 4.10 */
        b1 = sin_g - cos;             /* -1 .. 3.98 */
        a0 = sin_div_g + cos;         /* 0.25 .. 4.10 */
        a1 = sin_div_g - cos;         /* -1 .. 3.98 */
    } else {
        const int32_t cos_div_g = fp_div(cos, g, 25);
        const int32_t cos_g = FRACMUL(cos, g);
        sin >>= 3;
        b0 = sin + cos_g;             /* 0.25 .. 4.10 */
        b1 = sin - cos_g;             /* -3.98 .. 1 */
        a0 = sin + cos_div_g;         /* 0.25 .. 4.10 */
        a1 = sin - cos_div_g;         /* -3.98 .. 1 */
    }

    const int32_t rcp_a0 = fp_div(1, a0, 57); /* 0.24 .. 3.98, s2.29 */
    *c++ = FRACMUL_SHL(b0, rcp_a0, 1);       /* 0.063 .. 15.85 */
    *c++ = FRACMUL_SHL(b1, rcp_a0, 1);       /* -15.85 .. 15.85 */
    *c++ = -FRACMUL_SHL(a1, rcp_a0, 1);      /* -1 .. 1 */
}

Example #12
0
/**
 * 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;
}
Example #13
0
/* div */
static int divide(void *a, void *b, void *c, void *d)
{
   LTC_ARGCHK(a != NULL);
   LTC_ARGCHK(b != NULL);
   return tfm_to_ltc_error(fp_div(a, b, c, d));
}