void pre_emphasis(fract32 data[], int arr_length)
{
fract32 xBuffer[BUFFER_SIZE] = {0};
// input buffer
fract32 yBuffer[BUFFER_SIZE] = {0};
// output buffer
int current = 0;
int index;
for (index = 0; index < arr_length; index++) {
fract32 temp_b0 = mult_fr1x32x32(shelving_coef.b0, data[index]);
fract32 temp_b1 = mult_fr1x32x32(shelving_coef.b1, xBuffer[INDEX(current-1)]);
fract32 temp_b2 = mult_fr1x32x32(shelving_coef.b2, xBuffer[INDEX(current-2)]);
fract32 temp_a1 = mult_fr1x32x32(shelving_coef.a1, yBuffer[INDEX(current-1)]);
fract32 temp_a2 = mult_fr1x32x32(shelving_coef.a2, yBuffer[INDEX(current-2)]);
fract32 temp_b = add_fr1x32(add_fr1x32(temp_b0, temp_b1), temp_b2);
fract32 temp_a = add_fr1x32(temp_a1, temp_a2);
fract32 temp = sub_fr1x32(temp_b, temp_a);
yBuffer[current] = shl_fr1x32(temp, 2);
xBuffer[current] = data[index]; //input
data[index] = yBuffer[current]; //output
current++;
current %= BUFFER_SIZE;
}
}
// set reciprocal of Q
extern void filter_svf_set_rq( filter_svf* f, fract32 rq) {
/// FIXME: want additional kind of scale/tune here?
// ok, this is kind of weird
// rq range is [0, 2],
// fract32 positive range is [0, .9999...]
// so: move the radix to interpret rq as 2.30
// and store lshift value
if(rq > 0x3fffffff) {
f->rqShift = 1;
// clear the highest non-sign bit before shifting? (probably doesn't matter)
f->rq = shl_fr1x32(rq & 0xbfffffff, 1);
} else {
f->rqShift = 0;
f->rq = shl_fr1x32(rq, 1);
}
}
float calc_energy(fract32 data[], int arr_length)
{
int shift = 9;
// shift = log_2(arr_length)
fract32 energy_fr = float_to_fr32(0.0);
int index;
for (index = 0; index < arr_length; index++) {
fract32 temp = mult_fr1x32x32(data[index], data[index]);
temp = shl_fr1x32(temp, -shift);
// right shift in case of overflow
energy_fr = add_fr1x32(energy_fr, temp);
}
float energy = fr32_to_float(energy_fr) * (1<<shift);
return energy;
}
static fract32 filter_svf_calc_frame( filter_svf* f, fract32 in) {
// fract32 out;
f->low = add_fr1x32(f->low,
mult_fr1x32x32(f->freq, f->band));
f->high = sub_fr1x32(
sub_fr1x32(
in,
shl_fr1x32(mult_fr1x32x32(f->rq, f->band), f->rqShift)
),
f->low
);
f->band = add_fr1x32(f->band,
mult_fr1x32x32(f->freq, f->high) );
// f->notch = add_fr1x32(f->low, f->high);
// return out;
return *(f->mode);
}
