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