void
AMDemod (AMD am)
{
int i;
REAL demout;
switch (am->mode)
{
case SAMdet:
for (i = 0; i < am->size; i++)
{
pll (am, CXBdata (am->ibuf, i));
demout = dem (am);
CXBdata (am->obuf, i) = Cmplx (demout, demout);
}
break;
case AMdet:
for (i = 0; i < am->size; i++)
{
am->lock.curr = Cmag (CXBdata (am->ibuf, i));
am->dc = 0.9999f * am->dc + 0.0001f * am->lock.curr;
am->smooth = 0.5f * am->smooth + 0.5f * (am->lock.curr /*- am->dc*/);
/* demout = am->smooth; */
CXBdata (am->obuf, i) = Cmplx (am->smooth, am->smooth);
}
break;
}
}
void CDCBlock::block()
{
unsigned int n = CXBhave(m_buf);
for (unsigned int i = 0; i < n; i++) {
float x = CXBreal(m_buf, i);
float y = x - m_xm1 + 0.995F * m_ym1;
m_xm1 = x;
m_ym1 = y;
CXBdata(m_buf, i) = Cmplx(y, 0.0F);
}
}
void
noiseblanker (NB nb)
{
int i;
for (i = 0; i < CXBsize (nb->sigbuf); i++)
{
REAL cmag = Cmag (CXBdata (nb->sigbuf, i));
nb->delay[nb->sigindex] = CXBdata (nb->sigbuf, i);
nb->average_mag = (REAL) (0.999 * (nb->average_mag) + 0.001 * cmag);
if ((nb->hangtime == 0) && (cmag > (nb->threshold * nb->average_mag)))
nb->hangtime = 7;
if (nb->hangtime > 0)
{
CXBdata (nb->sigbuf, i) = Cmplx (0.0, 0.0);
nb->hangtime--;
}
else
CXBdata (nb->sigbuf, i) = nb->delay[nb->delayindex];
nb->sigindex = (nb->sigindex + 7) & 7;
nb->delayindex = (nb->delayindex + 7) & 7;
}
}
/* ---------------------------------------------------------------------------- */
PRIVATE void
pll(FMD fm, COMPLEX sig) {
COMPLEX z = Cmplx((REAL) cos(fm->pll.phs), (IMAG) sin(fm->pll.phs));
REAL diff;
fm->pll.delay.re = z.re * sig.re + z.im * sig.im;
fm->pll.delay.im = -z.im * sig.re + z.re * sig.im;
diff = ATAN2(fm->pll.delay.im, fm->pll.delay.re);
fm->pll.freq.f += fm->pll.beta * diff;
if (fm->pll.freq.f < fm->pll.freq.l)
fm->pll.freq.f = fm->pll.freq.l;
if (fm->pll.freq.f > fm->pll.freq.h)
fm->pll.freq.f = fm->pll.freq.h;
fm->pll.phs += fm->pll.freq.f + fm->pll.alpha * diff;
while (fm->pll.phs >= TWOPI)
fm->pll.phs -= (REAL) TWOPI;
while (fm->pll.phs < 0)
fm->pll.phs += (REAL) TWOPI;
}
void CFMDemod::pll(COMPLEX sig)
{
COMPLEX z = Cmplx((float)cos(m_pllPhase), (float)sin (m_pllPhase));
m_pllDelay.re = z.re * sig.re + z.im * sig.im;
m_pllDelay.im = -z.im * sig.re + z.re * sig.im;
float diff = (float)::atan2(m_pllDelay.im, m_pllDelay.re);
m_pllFreqF += m_pllBeta * diff;
if (m_pllFreqF < m_pllFreqL)
m_pllFreqF = m_pllFreqL;
if (m_pllFreqF > m_pllFreqH)
m_pllFreqF = m_pllFreqH;
m_pllPhase += m_pllFreqF + m_pllAlpha * diff;
while (m_pllPhase >= 2.0 * M_PI)
m_pllPhase -= float(2.0 * M_PI);
while (m_pllPhase < 0.0F)
m_pllPhase += float(2.0 * M_PI);
}
static void
pll (AMD am, COMPLEX sig)
{
COMPLEX z = Cmplx ((REAL) cos (am->pll.phs), (REAL) sin (am->pll.phs));
REAL diff;
am->pll.delay.re = z.re * sig.re + z.im * sig.im;
am->pll.delay.im = -z.im * sig.re + z.re * sig.im;
diff = fast_atan2 (am->pll.delay.im, am->pll.delay.re);
am->pll.freq.f += am->pll.beta * diff;
if (am->pll.freq.f < am->pll.freq.l)
am->pll.freq.f = am->pll.freq.l;
if (am->pll.freq.f > am->pll.freq.h)
am->pll.freq.f = am->pll.freq.h;
am->pll.phs += am->pll.freq.f + am->pll.alpha * diff;
while (am->pll.phs >= TWOPI)
am->pll.phs -= (REAL) TWOPI;
while (am->pll.phs < 0)
am->pll.phs += (REAL) TWOPI;
}
/* ---------------------------------------------------------------------------- */
COMPLEX
Cr2p(COMPLEX z) {
return Cmplx((REAL) hypot(z.re, z.im),
(REAL) ATAN2(z.im, z.re));
}
/* ---------------------------------------------------------------------------- */
COMPLEX
Cp2r(COMPLEX z) {
return Cmplx((REAL) (z.re * cos(z.im)), (IMAG) (z.re * sin(z.im)));
}
/* ---------------------------------------------------------------------------- */
COMPLEX
Cexp(COMPLEX z) {
REAL r = (REAL) exp(z.re);
return Cmplx((REAL) (r * cos(z.im)), (IMAG) (r * sin(z.im)));
}
/* ---------------------------------------------------------------------------- */
COMPLEX
Conjg(COMPLEX z) {
return Cmplx(z.re, -z.im);
}
DttSP_EXP void
Audio_Callback2 (float **input, float **output, unsigned int nframes)
{
unsigned int thread;
BOOLEAN b = reset_em;
BOOLEAN return_empty=FALSE;
unsigned int i;
for(thread=0;thread<threadno;thread++)
{
if (top[thread].susp) return_empty = TRUE;
}
if (return_empty)
{
for(thread=0;thread<threadno;thread++)
{
memset (output[2*thread], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
if (b)
{
//fprintf(stderr, "reset_em!\n"); fflush(stderr);
//fprintf(stdout,"Audio_Callback2: reset_em = TRUE\n"), fflush(stdout);
reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++) {
memset (output[2*thread], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
#if 0
if (diversity.flag) {
// Deal with the transmitter first
if ((ringb_float_read_space (top[1].jack.ring.o.l) >= nframes)
&& (ringb_float_read_space (top[1].jack.ring.o.r) >= nframes))
{
ringb_float_read (top[1].jack.ring.o.l, output[2], nframes);
ringb_float_read (top[1].jack.ring.o.r, output[3], nframes);
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// input: copy from port to ring
if ((ringb_float_write_space (top[1].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[1].jack.ring.i.r) >= nframes))
{
ringb_float_write (top[1].jack.ring.i.l, input[2], nframes);
ringb_float_write (top[1].jack.ring.i.r, input[3], nframes);
}
else
{
// rb pathology
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// if enough accumulated in ring, fire dsp
if ((ringb_float_read_space (top[1].jack.ring.i.l) >= top[1].hold.size.frames) &&
(ringb_float_read_space (top[1].jack.ring.i.r) >= top[1].hold.size.frames))
sem_post (&top[1].sync.buf.sem);
//
// Deal with the diversity channel next
//
if ((ringb_float_read_space (top[0].jack.ring.o.l) >= nframes)
&& (ringb_float_read_space (top[0].jack.ring.o.r) >= nframes))
{
/*ringb_float_read (top[thread].jack.auxr.o.l, output[l], nframes);
ringb_float_read (top[thread].jack.auxr.o.r, output[r], nframes);*/
ringb_float_read (top[0].jack.ring.o.l, output[2], nframes);
ringb_float_read (top[0].jack.ring.o.r, output[3], nframes);
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// Deal with the diversity/phased array channel next
// input: copy from port to ring
if ((ringb_float_write_space (top[0].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[0].jack.ring.i.r) >= nframes) &&
(ringb_float_write_space (top[2].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[2].jack.ring.i.r) >= nframes))
{
REAL *l0 = input[0];
REAL *r0 = input[1];
REAL *l2 = input[4];
REAL *r2 = input[5];
for (i=0;i<nframes;i++) {
COMPLEX A = Cmplx(l0[i],r0[i]);
COMPLEX B = Cmplx(l2[i],r2[i]);
A = Cscl(Cadd(A,Cmul(B,diversity.scalar)),diversity.gain);
ringb_float_write (top[0].jack.ring.i.l, &A.re, 1);
ringb_float_write (top[0].jack.ring.i.r, &A.im, 1);
}
/*ringb_float_write (top[thread].jack.auxr.i.l, input[l], nframes);
ringb_float_write (top[thread].jack.auxr.i.r, input[r], nframes);*/
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// if enough accumulated in ring, fire dsp
if ((ringb_float_read_space (top[0].jack.ring.i.l) >= top[0].hold.size.frames) &&
(ringb_float_read_space (top[0].jack.ring.i.r) >= top[0].hold.size.frames))
sem_post (&top[0].sync.buf.sem);
//
// Deal with 2nd receiver channel now
//
if ((ringb_float_read_space (top[2].jack.ring.o.l) >= nframes)
&& (ringb_float_read_space (top[2].jack.ring.o.r) >= nframes))
{
/*ringb_float_read (top[thread].jack.auxr.o.l, output[l], nframes);
ringb_float_read (top[thread].jack.auxr.o.r, output[r], nframes);*/
ringb_float_read (top[2].jack.ring.o.l, output[4], nframes);
ringb_float_read (top[2].jack.ring.o.r, output[5], nframes);
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// input: copy from port to ring
if ((ringb_float_write_space (top[2].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[2].jack.ring.i.r) >= nframes))
{
ringb_float_write (top[2].jack.ring.i.l, input[4], nframes);
ringb_float_write (top[2].jack.ring.i.r, input[5], nframes);
}
else
{
// rb pathology
for(thread=0;thread<threadno;thread++)
{
memset (output[thread], 0, nframes * sizeof (float));
memset (output[thread], 0, nframes * sizeof (float));
}
return;
}
// if enough accumulated in ring, fire dsp
if ((ringb_float_read_space (top[2].jack.ring.i.l) >= top[2].hold.size.frames) &&
(ringb_float_read_space (top[2].jack.ring.i.r) >= top[2].hold.size.frames))
sem_post (&top[2].sync.buf.sem);
} else
#endif
for(thread=0; thread<threadno; thread++)
{
int l=2*thread, r = 2*thread+1;
if ((ringb_float_read_space (top[thread].jack.ring.o.l) >= nframes)
&& (ringb_float_read_space (top[thread].jack.ring.o.r) >= nframes))
{
/*ringb_float_read (top[thread].jack.auxr.o.l, output[l], nframes);
ringb_float_read (top[thread].jack.auxr.o.r, output[r], nframes);*/
ringb_float_read (top[thread].jack.ring.o.l, output[l], nframes);
ringb_float_read (top[thread].jack.ring.o.r, output[r], nframes);
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[2*thread ], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
// input: copy from port to ring
if ((ringb_float_write_space (top[thread].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[thread].jack.ring.i.r) >= nframes))
{
if (diversity.flag && (thread == 0)) {
if ((ringb_float_write_space (top[2].jack.ring.i.l) >= nframes)
&& (ringb_float_write_space (top[2].jack.ring.i.r) >= nframes))
{
REAL *l0 = input[0];
REAL *r0 = input[1];
REAL *l2 = input[4];
REAL *r2 = input[5];
for (i=0;i<nframes;i++) {
COMPLEX A = Cmplx(l0[i],r0[i]);
COMPLEX B = Cmplx(l2[i],r2[i]);
A = Cscl(Cadd(A,Cmul(B,diversity.scalar)),diversity.gain);
ringb_float_write (top[0].jack.ring.i.l, &A.re, 1);
ringb_float_write (top[0].jack.ring.i.r, &A.im, 1);
}
/*ringb_float_write (top[thread].jack.auxr.i.l, input[l], nframes);
ringb_float_write (top[thread].jack.auxr.i.r, input[r], nframes);*/
} else {
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[2*thread ], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
} else {
ringb_float_write (top[thread].jack.ring.i.l, input[l], nframes);
ringb_float_write (top[thread].jack.ring.i.r, input[r], nframes);
/*ringb_float_write (top[thread].jack.auxr.i.l, input[l], nframes);
ringb_float_write (top[thread].jack.auxr.i.r, input[r], nframes);*/
}
}
else
{
// rb pathology
//reset_system_audio(nframes);
for(thread=0;thread<threadno;thread++)
{
memset (output[2*thread ], 0, nframes * sizeof (float));
memset (output[2*thread+1], 0, nframes * sizeof (float));
}
return;
}
// if enough accumulated in ring, fire dsp
if ((ringb_float_read_space (top[thread].jack.ring.i.l) >= top[thread].hold.size.frames) &&
(ringb_float_read_space (top[thread].jack.ring.i.r) >= top[thread].hold.size.frames))
sem_post (&top[thread].sync.buf.sem);
}
}
