void apf(short *in, short *coeff, short *out, long delayi, short alpha,
short beta, short u, short agc, short ltgain, short order, short length, short br)
{
static int FirstTime = 1;
static short FIRmem[ORDER]; /* FIR filter memory */
static short IIRmem[ORDER]; /* IIR filter memory */
static short last;
static short Residual[ACBMemSize + SubFrameSize]; /* local residual */
short wcoef1[ORDER];
short wcoef2[ORDER];
short scratch[SubFrameSize];
short temp[SubFrameSize];
short mem[ORDER];
long sum1, sum2;
long gamma, APFgain;
short i, j, n, best;
short Stemp, shift1, shift2;
long Ltemp;
/* initialization -- should be done in init routine for implementation */
if (FirstTime)
{
FirstTime = 0;
for (i = 0; i < ORDER; i++)
FIRmem[i] = 0;
for (i = 0; i < ORDER; i++)
IIRmem[i] = 0;
for (i = 0; i < ACBMemSize; i++)
Residual[i] = 0;
last = 0;
}
/* Compute weighted LPC coefficients */
weight(wcoef1, coeff, alpha, order);
weight(wcoef2, coeff, beta, order);
/* Tilt speech */
/*...no tilt in non-voiced regions...*/
for (i = 0, sum2 = 0; i < length - 1; i++)
sum2 = L_mac(sum2, in[i], in[i + 1]);
if (sum2 < 0)
u = 0; /*...no tilt...*/
for (i = 0; i < length; i++)
{
scratch[i] = msu_r(L_deposit_h(in[i]), u, last);
last = in[i];
}
/* Compute residual */
fir(scratch, scratch, wcoef1, FIRmem, order, length);
for (i = 0; i < SubFrameSize ; i++)
Residual[ACBMemSize+i] = scratch[i];
/* long term filtering */
/* Find best integer delay around delayi */
j = extract_h(L_add(delayi, 32768));
sum1 = 0;
shift1 = 1;
best = j;
for (i = Max(DMIN, j - 3); i <= Min(DMAX, j + 3); i++)
{
shift2 = 1;
for (n = ACBMemSize, sum2 = 0; n < ACBMemSize + length; n++)
{
Ltemp = L_mult(Residual[n], Residual[n - i]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if( ((shift1 >= shift2) && (L_shr(sum2,sub(shift1,shift2)) > sum1))
|| ((shift1 < shift2) && (sum2 > L_shr(sum1,sub(shift2,shift1)))))
{
sum1 = sum2;
shift1 = shift2;
best = i;
}
}
/* Get beta for delayi */
shift1 = 1;
for (i = ACBMemSize, sum1 = 0; i < ACBMemSize + length; i++)
{
Ltemp = L_mult(Residual[i - best], Residual[i - best]);
Ltemp = L_shr(Ltemp, shift1);
sum1 = L_add(sum1, Ltemp);
if (sum1 >= 0x40000000)
{
sum1 = L_shr(sum1, 1);
shift1++;
}
}
shift2 = 1;
for (i = ACBMemSize, sum2 = 0; i < ACBMemSize + length; i++)
{
Ltemp = L_mult(Residual[i], Residual[i - best]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if (shift1 > shift2)
{
shift1 = sub(shift1, shift2);
sum2 = L_shr(sum2, shift1);
}
else if (shift1 < shift2)
{
shift2 = sub(shift2, shift1);
sum1 = L_shr(sum1, shift2);
}
if ((sum2 == 0) || (sum1 == 0) || (br == 1))
for (i = 0; i < length; i++)
temp[i] = Residual[i + ACBMemSize];
else
{
if (sum2 >= sum1)
gamma = 0x7fffffff; /* Clip gamma at 1.0 */
else if (sum2 < 0)
gamma = 0;
else
{
shift1 = norm_l(sum1);
sum1 = L_shl(sum1, shift1);
sum2 = L_shl(sum2, shift1);
gamma = L_divide(sum2, sum1);
}
if (gamma < 0x40000000)
for (i = 0; i < length; i++)
temp[i] = Residual[i + ACBMemSize];
else
{
/* Do actual filtering */
for (i = 0; i < length; i++)
{
Ltemp = L_mpy_ls(gamma, ltgain);
Ltemp = L_mpy_ls(Ltemp, Residual[ACBMemSize + i - best]);
temp[i] = add(Residual[ACBMemSize + i], round(Ltemp));
}
}
}
/* iir short term filter - first run */
for (i = 0; i < length; i++)
scratch[i] = temp[i];
for (i = 0; i < order; i++)
mem[i] = IIRmem[i];
iir(scratch, scratch, wcoef2, mem, order, length);
/* Get filter gain */
shift1 = 1;
for (i = 0, sum1 = 0; i < length; i++)
{
Ltemp = L_mult(in[i], in[i]);
Ltemp = L_shr(Ltemp, shift1);
sum1 = L_add(sum1, Ltemp);
if (sum1 >= 0x40000000)
{
sum1 = L_shr(sum1, 1);
shift1++;
}
}
shift2 = 1;
for (i = 0, sum2 = 0; i < length; i++)
{
Ltemp = L_mult(scratch[i], scratch[i]);
Ltemp = L_shr(Ltemp, shift2);
sum2 = L_add(sum2, Ltemp);
if (sum2 >= 0x40000000)
{
sum2 = L_shr(sum2, 1);
shift2++;
}
}
if (shift1 > shift2)
{
shift1 = sub(shift1, shift2);
sum2 = L_shr(sum2, shift1);
}
else if (shift1 < shift2)
{
shift2 = sub(shift2, shift1);
sum1 = L_shr(sum1, shift2);
}
if (sum2 != 0)
{
shift1 = norm_l(sum2);
sum2 = L_shl(sum2, shift1);
shift1 = sub(shift1, 2); /* For (1. < APFgain < 2.) */
sum1 = L_shl(sum1, shift1);
Ltemp = L_divide(sum1, sum2);
shift1 = norm_l(Ltemp);
Ltemp = L_shl(Ltemp, shift1);
Stemp = sqroot(Ltemp);
if (shift1 & 1)
APFgain = L_mult(0x5a82, Stemp);
else
APFgain = L_deposit_h(Stemp);
shift1 = shr(shift1, 1);
APFgain = L_shr(APFgain, shift1);
/* Re-normalize the speech signal */
for (i = 0; i < length; i++)
{
Ltemp = L_mpy_ls(APFgain, temp[i]);
Ltemp = L_shl(Ltemp, 1); /* For (1. < APFgain < 2.) */
temp[i] = round(Ltemp);
}
}
else
APFgain = 0x40000000;
/* iir short term filter - second run */
iir(out, temp, wcoef2, IIRmem, order, length);
/* Update residual buffer */
for (i = 0; i < ACBMemSize; i++)
Residual[i] = Residual[i + length];
}
static float pll(pll_t *pll_m, float In)
{
double Io, Qo;
double Ip, Qp;
double DPhi;
double DF;
/* quadrature oscillator */
Io = cos(pll_m->PhaseOsc);
Qo = sin(pll_m->PhaseOsc);
/* phase detector */
Ip = iir(In * Io, &(pll_m->Ifilterbuff), &PhaseFilterCf);
Qp = iir(In * Qo, &(pll_m->Qfilterbuff), &PhaseFilterCf);
DPhi = -atan2(Qp, Ip) / M_PI;
/* loop filter */
DF = K1 * DPhi + pll_m->FreqOsc;
pll_m->FreqOsc += K2 * DPhi;
if (pll_m->FreqOsc > ((Fc + DFc) / Fe))
pll_m->FreqOsc = (Fc + DFc) / Fe;
if (pll_m->FreqOsc < ((Fc - DFc) / Fe))
pll_m->FreqOsc = (Fc - DFc) / Fe;
pll_m->PhaseOsc += 2.0 * M_PI * DF;
if (pll_m->PhaseOsc > M_PI)
pll_m->PhaseOsc -= 2.0 * M_PI;
if (pll_m->PhaseOsc <= -M_PI)
pll_m->PhaseOsc += 2.0 * M_PI;
pll_m->fr = 0.25 * pll_m->FreqOsc + 0.75 * pll_m->fr;
return (float) (In * Io);
}
float IQ_decoder(float I, float Q)
{
// return I;
I = iir(I, &data_I);
Q = iir(Q, &data_Q);
uint32_t idx = freq_idx%FREQ_SAMPLES;
float out = I*freq_cos[idx] + Q*freq_sin[idx];//SSB Upper Side Band
//float out = I*freq_cos[idx] - Q*freq_sin[idx];//SSB Lower Side Band
freq_idx++;
return out;
}
IMPLEMENT
bool Uart::startup(Address _port, int __irq)
{
port = _port;
_irq = __irq;
Proc::Status o = Proc::cli_save();
if (!valid())
{
Proc::sti_restore(o);
fail();
return false;
}
ier(Base_ier_bits);/* disable all rs-232 interrupts */
mcr(0x0b); /* out2, rts, and dtr enabled */
fcr(1); /* enable fifo */
fcr(0x07); /* clear rcv xmit fifo */
fcr(1); /* enable fifo */
lcr(0); /* clear line control register */
/* clearall interrupts */
/*read*/ msr(); /* IRQID 0*/
/*read*/ iir(); /* IRQID 1*/
/*read*/ trb(); /* IRQID 2*/
/*read*/ lsr(); /* IRQID 3*/
while(lsr() & 1/*DATA READY*/) /*read*/ trb();
Proc::sti_restore(o);
return true;
}
void process_buffer(size_t num_samples)
{
int tsamples = num_samples * num_channels;
if (!(WavpackGetMode (ctx) & MODE_FLOAT)) {
float scaler = (float) (1.0 / ((unsigned int32_t) 1 << (bytes_per_sample * 8 - 1)));
float *fptr = (float *) input;
int32_t *lptr = input;
int cnt = tsamples;
while (cnt--)
*fptr++ = *lptr++ * scaler;
}
if (play_gain != 1.0) {
float *fptr = (float *) input;
int cnt = tsamples;
while (cnt--)
*fptr++ *= play_gain;
}
if (tsamples) {
float *fptr = (float *) input;
short *sptr = (short *) output;
int cnt = num_samples, ch;
while (cnt--)
for (ch = 0; ch < num_channels; ++ch) {
int dst;
*fptr -= shaping_error [ch];
if (*fptr >= 1.0)
dst = 32767;
else if (*fptr <= -1.0)
dst = -32768;
else
dst = (int) floor (*fptr * 32768.0);
shaping_error [ch] = (float)(dst / 32768.0 - *fptr++);
*sptr++ = dst;
}
}
if (EQ_on)
iir ((char *) output, tsamples * sizeof(int16_t));
mod->add_vis_pcm(mod->output->written_time(), FMT_S16_NE, num_channels, tsamples * sizeof(int16_t), output);
mod->output->write_audio(output, tsamples * sizeof(int16_t));
}
static gint
xmms_eq_read (xmms_xform_t *xform, xmms_sample_t *buf, gint len,
xmms_error_t *error)
{
xmms_equalizer_data_t *priv;
gint read, chan;
g_return_val_if_fail (xform, -1);
priv = xmms_xform_private_data_get (xform);
g_return_val_if_fail (priv, -1);
read = xmms_xform_read (xform, buf, len, error);
chan = xmms_xform_indata_get_int (xform, XMMS_STREAM_TYPE_FMT_CHANNELS);
if (read > 0 && priv->enabled) {
iir (buf, read, chan, priv->extra_filtering);
}
return read;
}
main()
{
int i;
float *pcoef = coefficient[0][0];
float *pint = internal_state[0][0];
for(i=0;i<NPOINTS;i++)
{
pcoef[i]=1;
pint[i]=1;
}
//input_dsp(input,NPOINTS,0);
iir(input, output, coefficient, internal_state);
//output_dsp(input,NPOINTS,0);
//output_dsp(coefficient,NPOINTS,0);
//output_dsp(internal_state,NPOINTS,0);
//output_dsp(output,NPOINTS,0);
}
/* Other io help methods */
void filtertf(tf * f, defs * d)
{
if (f->current->data == NULL) {
f->current->dataf = NULL;
return;
}
f->current->dataf = io_freeData(f->current->dataf);
if (d->filter) {
yu_rtrend(f->current->data, f->current->head->npts);
f->current->dataf = iir(f->current->data, f->current->head->npts, f->current->head->delta,
(d->hp > 0.0) ? 2 : 0, d->hp, (d->lp > 0.0) ? 2 : 0,
d->lp);
if (f->current->dataf == NULL) {
sprintf(message, "Something wrong with the filter %f %f.", d->hp, d->lp);
alert(WARNING);
}
}
return;
}
/*
* The main stuff
*/
int do_equliazer(short * d, int length, int srate, int nch)
{
return iir(d, length, srate, nch);
}
