mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-10-05 11:16:40 -04:00
1396 lines
41 KiB
C
1396 lines
41 KiB
C
|
#ifndef LEANSDR_SDR_H
|
||
|
#define LEANSDR_SDR_H
|
||
|
|
||
|
#include "leansdr/math.h"
|
||
|
#include "leansdr/dsp.h"
|
||
|
|
||
|
namespace leansdr {
|
||
|
|
||
|
// Abbreviations for floating-point types
|
||
|
|
||
|
typedef float f32;
|
||
|
|
||
|
typedef complex<u8> cu8;
|
||
|
typedef complex<s8> cs8;
|
||
|
typedef complex<u16> cu16;
|
||
|
typedef complex<s16> cs16;
|
||
|
typedef complex<f32> cf32;
|
||
|
|
||
|
|
||
|
//////////////////////////////////////////////////////////////////////
|
||
|
// SDR blocks
|
||
|
//////////////////////////////////////////////////////////////////////
|
||
|
|
||
|
// AUTO-NOTCH FILTER
|
||
|
|
||
|
// Periodically detects the [n__slots] strongest peaks with a FFT,
|
||
|
// removes them with a first-order filter.
|
||
|
|
||
|
|
||
|
|
||
|
template<typename T>
|
||
|
struct auto_notch : runnable {
|
||
|
int decimation;
|
||
|
float k;
|
||
|
auto_notch(scheduler *sch, pipebuf< complex<T> > &_in,
|
||
|
pipebuf< complex<T> > &_out, int _n__slots,
|
||
|
T _agc_rms_setpoint)
|
||
|
: runnable(sch, "auto_notch"),
|
||
|
decimation(1024*4096), k(0.002), // k(0.01)
|
||
|
fft(4096),
|
||
|
in(_in), out(_out,fft.n),
|
||
|
n__slots(_n__slots), __slots(new slot[n__slots]),
|
||
|
phase(0), gain(1), agc_rms_setpoint(_agc_rms_setpoint) {
|
||
|
for ( int s=0; s<n__slots; ++s ) {
|
||
|
__slots[s].i = -1;
|
||
|
__slots[s].expj = new complex<float>[fft.n];
|
||
|
}
|
||
|
}
|
||
|
void run() {
|
||
|
while ( in.readable()>=fft.n && out.writable()>=fft.n ) {
|
||
|
phase += fft.n;
|
||
|
if ( phase >= decimation ) {
|
||
|
phase -= decimation;
|
||
|
detect();
|
||
|
}
|
||
|
process();
|
||
|
in.read(fft.n);
|
||
|
out.written(fft.n);
|
||
|
}
|
||
|
}
|
||
|
void detect() {
|
||
|
complex<T> *pin = in.rd();
|
||
|
complex<float> data[fft.n];
|
||
|
float m0=0, m2=0;
|
||
|
for ( int i=0; i<fft.n; ++i ) {
|
||
|
data[i].re = pin[i].re;
|
||
|
data[i].im = pin[i].im;
|
||
|
m2 += (float)pin[i].re*pin[i].re + (float)pin[i].im*pin[i].im;
|
||
|
if ( gen_abs(pin[i].re) > m0 ) m0 = gen_abs(pin[i].re);
|
||
|
if ( gen_abs(pin[i].im) > m0 ) m0 = gen_abs(pin[i].im);
|
||
|
}
|
||
|
if ( agc_rms_setpoint && m2 ) {
|
||
|
float rms = gen_sqrt(m2/fft.n);
|
||
|
if ( sch->debug ) fprintf(stderr, "(pow %f max %f)", rms, m0);
|
||
|
float new_gain = agc_rms_setpoint / rms;
|
||
|
gain = gain*0.9 + new_gain*0.1;
|
||
|
}
|
||
|
fft.inplace(data, true);
|
||
|
float amp[fft.n];
|
||
|
for ( int i=0; i<fft.n; ++i ) amp[i] = hypotf(data[i].re, data[i].im);
|
||
|
for ( slot *s=__slots; s<__slots+n__slots; ++s ) {
|
||
|
int iamax = 0;
|
||
|
for ( int i=0; i<fft.n; ++i )
|
||
|
if ( amp[i] > amp[iamax] ) iamax=i;
|
||
|
if ( iamax != s->i ) {
|
||
|
if ( sch->debug )
|
||
|
fprintf(stderr, "%s: slot %d new peak %d -> %d\n",
|
||
|
name, (int)(s-__slots), s->i, iamax);
|
||
|
s->i = iamax;
|
||
|
s->estim.re = 0;
|
||
|
s->estim.im = 0;
|
||
|
s->estt = 0;
|
||
|
for ( int i=0; i<fft.n; ++i ) {
|
||
|
float a = 2 * M_PI * s->i * i / fft.n;
|
||
|
s->expj[i].re = cosf(a);
|
||
|
s->expj[i].im = sinf(a);
|
||
|
}
|
||
|
}
|
||
|
amp[iamax] = 0;
|
||
|
if ( iamax-1 >= 0 ) amp[iamax-1] = 0;
|
||
|
if ( iamax+1 < fft.n ) amp[iamax+1] = 0;
|
||
|
}
|
||
|
}
|
||
|
void process() {
|
||
|
complex<T> *pin=in.rd(), *pend=pin+fft.n, *pout=out.wr();
|
||
|
for ( slot *s=__slots; s<__slots+n__slots; ++s ) s->ej = s->expj;
|
||
|
for ( ; pin<pend; ++pin,++pout ) {
|
||
|
complex<float> out = *pin;
|
||
|
// TODO Optimize for n__slots==1 ?
|
||
|
for ( slot *s=__slots; s<__slots+n__slots; ++s->ej,++s ) {
|
||
|
complex<float> bb(pin->re*s->ej->re + pin->im*s->ej->im,
|
||
|
-pin->re*s->ej->im + pin->im*s->ej->re);
|
||
|
s->estim.re = bb.re*k + s->estim.re*(1-k);
|
||
|
s->estim.im = bb.im*k + s->estim.im*(1-k);
|
||
|
complex<float> sub(s->estim.re*s->ej->re - s->estim.im*s->ej->im,
|
||
|
s->estim.re*s->ej->im + s->estim.im*s->ej->re);
|
||
|
out.re -= sub.re;
|
||
|
out.im -= sub.im;
|
||
|
}
|
||
|
pout->re = gain * out.re;
|
||
|
pout->im = gain * out.im;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
private:
|
||
|
cfft_engine<float> fft;
|
||
|
pipereader< complex<T> > in;
|
||
|
pipewriter< complex<T> > out;
|
||
|
int n__slots;
|
||
|
|
||
|
struct slot {
|
||
|
int i;
|
||
|
complex<float> estim;
|
||
|
complex<float> *expj, *ej;
|
||
|
int estt;
|
||
|
} *__slots;
|
||
|
int phase;
|
||
|
float gain;
|
||
|
T agc_rms_setpoint;
|
||
|
};
|
||
|
|
||
|
|
||
|
// SIGNAL STRENGTH ESTIMATOR
|
||
|
|
||
|
// Outputs RMS values.
|
||
|
|
||
|
template<typename T>
|
||
|
struct ss_estimator : runnable {
|
||
|
unsigned long window_size; // Samples per estimation
|
||
|
unsigned long decimation; // Output rate
|
||
|
ss_estimator(scheduler *sch, pipebuf< complex<T> > &_in, pipebuf<T> &_out)
|
||
|
: runnable(sch, "SS estimator"),
|
||
|
window_size(1024), decimation(1024),
|
||
|
in(_in), out(_out),
|
||
|
phase(0) {
|
||
|
}
|
||
|
void run() {
|
||
|
while ( in.readable()>=window_size && out.writable()>=1 ) {
|
||
|
phase += window_size;
|
||
|
if ( phase >= decimation ) {
|
||
|
phase -= decimation;
|
||
|
complex<T> *p=in.rd(), *pend=p+window_size;
|
||
|
float s = 0;
|
||
|
for ( ; p<pend; ++p )
|
||
|
s += (float)p->re*p->re + (float)p->im*p->im;
|
||
|
out.write(sqrtf(s/window_size));
|
||
|
}
|
||
|
in.read(window_size);
|
||
|
}
|
||
|
}
|
||
|
private:
|
||
|
pipereader< complex<T> > in;
|
||
|
pipewriter<T> out;
|
||
|
unsigned long phase;
|
||
|
};
|
||
|
|
||
|
template<typename T>
|
||
|
struct ss_amp_estimator : runnable {
|
||
|
unsigned long window_size; // Samples per estimation
|
||
|
unsigned long decimation; // Output rate
|
||
|
ss_amp_estimator(scheduler *sch, pipebuf< complex<T> > &_in,
|
||
|
pipebuf<T> &_out_ss,
|
||
|
pipebuf<T> &_out_ampmin, pipebuf<T> &_out_ampmax)
|
||
|
: runnable(sch, "SS estimator"),
|
||
|
window_size(1024), decimation(1024),
|
||
|
in(_in), out_ss(_out_ss),
|
||
|
out_ampmin(_out_ampmin), out_ampmax(_out_ampmax),
|
||
|
phase(0) {
|
||
|
}
|
||
|
void run() {
|
||
|
while ( in.readable() >= window_size &&
|
||
|
out_ss.writable() >= 1 &&
|
||
|
out_ampmin.writable() >= 1 &&
|
||
|
out_ampmax.writable() >= 1 ) {
|
||
|
phase += window_size;
|
||
|
if ( phase >= decimation ) {
|
||
|
phase -= decimation;
|
||
|
complex<T> *p=in.rd(), *pend=p+window_size;
|
||
|
float s2 = 0;
|
||
|
float amin=1e38, amax=0;
|
||
|
for ( ; p<pend; ++p ) {
|
||
|
float mag2 = (float)p->re*p->re + (float)p->im*p->im;
|
||
|
s2 += mag2;
|
||
|
float mag = sqrtf(mag2);
|
||
|
if ( mag < amin ) amin = mag;
|
||
|
if ( mag > amax ) amax = mag;
|
||
|
}
|
||
|
out_ss.write(sqrtf(s2/window_size));
|
||
|
out_ampmin.write(amin);
|
||
|
out_ampmax.write(amax);
|
||
|
}
|
||
|
in.read(window_size);
|
||
|
}
|
||
|
}
|
||
|
private:
|
||
|
pipereader< complex<T> > in;
|
||
|
pipewriter<T> out_ss, out_ampmin, out_ampmax;
|
||
|
unsigned long phase;
|
||
|
};
|
||
|
|
||
|
// AGC
|
||
|
|
||
|
template<typename T>
|
||
|
struct simple_agc : runnable {
|
||
|
float out_rms; // Desired RMS output power
|
||
|
float bw; // Bandwidth
|
||
|
float estimated; // Input power
|
||
|
simple_agc(scheduler *sch,
|
||
|
pipebuf< complex<T> > &_in,
|
||
|
pipebuf< complex<T> > &_out)
|
||
|
: runnable(sch, "AGC"),
|
||
|
out_rms(1), bw(0.001), estimated(0),
|
||
|
in(_in), out(_out) {
|
||
|
}
|
||
|
private:
|
||
|
pipereader< complex<T> > in;
|
||
|
pipewriter< complex<T> > out;
|
||
|
static const int chunk_size = 128;
|
||
|
void run() {
|
||
|
while ( in.readable() >= chunk_size &&
|
||
|
out.writable() >= chunk_size ) {
|
||
|
complex<T> *pin=in.rd(), *pend=pin+chunk_size;
|
||
|
float amp2 = 0;
|
||
|
for ( ; pin<pend; ++pin ) amp2 += pin->re*pin->re + pin->im*pin->im;
|
||
|
amp2 /= chunk_size;
|
||
|
if ( ! estimated ) estimated = amp2;
|
||
|
estimated = estimated*(1-bw) + amp2*bw;
|
||
|
float gain = estimated ? out_rms / sqrtf(estimated) : 0;
|
||
|
pin = in.rd();
|
||
|
complex<T> *pout = out.wr();
|
||
|
float bwcomp = 1 - bw;
|
||
|
for ( ; pin<pend; ++pin,++pout ) {
|
||
|
pout->re = pin->re * gain;
|
||
|
pout->im = pin->im * gain;
|
||
|
}
|
||
|
in.read(chunk_size);
|
||
|
out.written(chunk_size);
|
||
|
}
|
||
|
}
|
||
|
}; // simple_agc
|
||
|
|
||
|
|
||
|
typedef uint16_t u_angle; // [0,2PI[ in 65536 steps
|
||
|
typedef int16_t s_angle; // [-PI,PI[ in 65536 steps
|
||
|
|
||
|
|
||
|
// GENERIC CONSTELLATION DECODING BY LOOK-UP TABLE.
|
||
|
|
||
|
// Metrics and phase errors are pre-computed on a RxR grid.
|
||
|
// R must be a power of 2.
|
||
|
// Up to 256 symbols.
|
||
|
|
||
|
struct softsymbol {
|
||
|
int16_t cost; // For Viterbi with TBM=int16_t
|
||
|
uint8_t symbol;
|
||
|
};
|
||
|
|
||
|
// Target RMS amplitude for AGC
|
||
|
//const float cstln_amp = 73; // Best for 32APSK 9/10
|
||
|
//const float cstln_amp = 90; // Best for QPSK
|
||
|
//const float cstln_amp = 64; // Best for BPSK
|
||
|
//const float cstln_amp = 75; // Best for BPSK at 45°
|
||
|
const float cstln_amp = 75; // Trade-off
|
||
|
|
||
|
template<int R>
|
||
|
struct cstln_lut {
|
||
|
complex<signed char> *symbols;
|
||
|
int nsymbols;
|
||
|
int nrotations;
|
||
|
|
||
|
enum predef {
|
||
|
BPSK, // DVB-S2 (and DVB-S variant)
|
||
|
QPSK, // DVB-S
|
||
|
PSK8, APSK16, APSK32, // DVB-S2
|
||
|
APSK64E, // DVB-S2X
|
||
|
QAM16, QAM64, QAM256 // For experimentation only
|
||
|
};
|
||
|
|
||
|
cstln_lut(predef type, float gamma1=1, float gamma2=1, float gamma3=1) {
|
||
|
switch ( type ) {
|
||
|
case BPSK:
|
||
|
nrotations = 2;
|
||
|
nsymbols = 2;
|
||
|
symbols = new complex<signed char>[nsymbols];
|
||
|
#if 0 // BPSK at 0°
|
||
|
symbols[0] = polar(1, 2, 0);
|
||
|
symbols[1] = polar(1, 2, 1);
|
||
|
#else // BPSK at 45°
|
||
|
symbols[0] = polar(1, 8, 1);
|
||
|
symbols[1] = polar(1, 8, 5);
|
||
|
#endif
|
||
|
make_lut_from_symbols();
|
||
|
break;
|
||
|
case QPSK:
|
||
|
// EN 300 421, section 4.5 Baseband shaping and modulation
|
||
|
// EN 302 307, section 5.4.1
|
||
|
nrotations = 4;
|
||
|
nsymbols = 4;
|
||
|
symbols = new complex<signed char>[nsymbols];
|
||
|
symbols[0] = polar(1, 4, 0.5);
|
||
|
symbols[1] = polar(1, 4, 3.5);
|
||
|
symbols[2] = polar(1, 4, 1.5);
|
||
|
symbols[3] = polar(1, 4, 2.5);
|
||
|
make_lut_from_symbols();
|
||
|
break;
|
||
|
case PSK8:
|
||
|
// EN 302 307, section 5.4.2
|
||
|
nrotations = 8;
|
||
|
nsymbols = 8;
|
||
|
symbols = new complex<signed char>[nsymbols];
|
||
|
symbols[0] = polar(1, 8, 1);
|
||
|
symbols[1] = polar(1, 8, 0);
|
||
|
symbols[2] = polar(1, 8, 4);
|
||
|
symbols[3] = polar(1, 8, 5);
|
||
|
symbols[4] = polar(1, 8, 2);
|
||
|
symbols[5] = polar(1, 8, 7);
|
||
|
symbols[6] = polar(1, 8, 3);
|
||
|
symbols[7] = polar(1, 8, 6);
|
||
|
make_lut_from_symbols();
|
||
|
break;
|
||
|
case APSK16: {
|
||
|
// EN 302 307, section 5.4.3
|
||
|
float r1 = sqrtf(4 / (1+3*gamma1*gamma1));
|
||
|
float r2 = gamma1 * r1;
|
||
|
nrotations = 4;
|
||
|
nsymbols = 16;
|
||
|
symbols = new complex<signed char>[nsymbols];
|
||
|
symbols[0] = polar(r2, 12, 1.5);
|
||
|
symbols[1] = polar(r2, 12, 10.5);
|
||
|
symbols[2] = polar(r2, 12, 4.5);
|
||
|
symbols[3] = polar(r2, 12, 7.5);
|
||
|
symbols[4] = polar(r2, 12, 0.5);
|
||
|
symbols[5] = polar(r2, 12, 11.5);
|
||
|
symbols[6] = polar(r2, 12, 5.5);
|
||
|
symbols[7] = polar(r2, 12, 6.5);
|
||
|
symbols[8] = polar(r2, 12, 2.5);
|
||
|
symbols[9] = polar(r2, 12, 9.5);
|
||
|
symbols[10] = polar(r2, 12, 3.5);
|
||
|
symbols[11] = polar(r2, 12, 8.5);
|
||
|
symbols[12] = polar(r1, 4, 0.5);
|
||
|
symbols[13] = polar(r1, 4, 3.5);
|
||
|
symbols[14] = polar(r1, 4, 1.5);
|
||
|
symbols[15] = polar(r1, 4, 2.5);
|
||
|
make_lut_from_symbols();
|
||
|
break;
|
||
|
}
|
||
|
case APSK32: {
|
||
|
// EN 302 307, section 5.4.3
|
||
|
float r1 = sqrtf(8 / (1+3*gamma1*gamma1+4*gamma2*gamma2));
|
||
|
float r2 = gamma1 * r1;
|
||
|
float r3 = gamma2 * r1;
|
||
|
nrotations = 4;
|
||
|
nsymbols = 32;
|
||
|
symbols = new complex<signed char>[nsymbols];
|
||
|
symbols[0] = polar(r2, 12, 1.5);
|
||
|
symbols[1] = polar(r2, 12, 2.5);
|
||
|
symbols[2] = polar(r2, 12, 10.5);
|
||
|
symbols[3] = polar(r2, 12, 9.5);
|
||
|
symbols[4] = polar(r2, 12, 4.5);
|
||
|
symbols[5] = polar(r2, 12, 3.5);
|
||
|
symbols[6] = polar(r2, 12, 7.5);
|
||
|
symbols[7] = polar(r2, 12, 8.5);
|
||
|
symbols[8] = polar(r3, 16, 1 );
|
||
|
symbols[9] = polar(r3, 16, 3 );
|
||
|
symbols[10] = polar(r3, 16, 14 );
|
||
|
symbols[11] = polar(r3, 16, 12 );
|
||
|
symbols[12] = polar(r3, 16, 6 );
|
||
|
symbols[13] = polar(r3, 16, 4 );
|
||
|
symbols[14] = polar(r3, 16, 9 );
|
||
|
symbols[15] = polar(r3, 16, 11 );
|
||
|
symbols[16] = polar(r2, 12, 0.5);
|
||
|
symbols[17] = polar(r1, 4, 0.5);
|
||
|
symbols[18] = polar(r2, 12, 11.5);
|
||
|
symbols[19] = polar(r1, 4, 3.5);
|
||
|
symbols[20] = polar(r2, 12, 5.5);
|
||
|
symbols[21] = polar(r1, 4, 1.5);
|
||
|
symbols[22] = polar(r2, 12, 6.5);
|
||
|
symbols[23] = polar(r1, 4, 2.5);
|
||
|
symbols[24] = polar(r3, 16, 0 );
|
||
|
symbols[25] = polar(r3, 16, 2 );
|
||
|
symbols[26] = polar(r3, 16, 15 );
|
||
|
symbols[27] = polar(r3, 16, 13 );
|
||
|
symbols[28] = polar(r3, 16, 7 );
|
||
|
symbols[29] = polar(r3, 16, 5 );
|
||
|
symbols[30] = polar(r3, 16, 8 );
|
||
|
symbols[31] = polar(r3, 16, 10 );
|
||
|
make_lut_from_symbols();
|
||
|
break;
|
||
|
}
|
||
|
case APSK64E: {
|
||
|
// EN 302 307-2, section 5.4.5, Table 13e
|
||
|
float r1 =
|
||
|
sqrtf(64 / (4+12*gamma1*gamma1+20*gamma2*gamma2+28*gamma3*gamma3));
|
||
|
float r2 = gamma1 * r1;
|
||
|
float r3 = gamma2 * r1;
|
||
|
float r4 = gamma3 * r1;
|
||
|
nrotations = 4;
|
||
|
nsymbols = 64;
|
||
|
symbols = new complex<signed char>[nsymbols];
|
||
|
polar2( 0, r4, 1.0/ 4, 7.0/4, 3.0/ 4, 5.0/ 4);
|
||
|
polar2( 4, r4, 13.0/28, 43.0/28, 15.0/28, 41.0/28);
|
||
|
polar2( 8, r4, 1.0/28, 55.0/28, 27.0/28, 29.0/28);
|
||
|
polar2(12, r1, 1.0/ 4, 7.0/ 4, 3.0/ 4, 5.0/ 4);
|
||
|
polar2(16, r4, 9.0/28, 47.0/28, 19.0/28, 37.0/28);
|
||
|
polar2(20, r4, 11.0/28, 45.0/28, 17.0/28, 39.0/28);
|
||
|
polar2(24, r3, 1.0/20, 39.0/20, 19.0/20, 21.0/20);
|
||
|
polar2(28, r2, 1.0/12, 23.0/12, 11.0/12, 13.0/12);
|
||
|
polar2(32, r4, 5.0/28, 51.0/28, 23.0/28, 33.0/28);
|
||
|
polar2(36, r3, 9.0/20, 31.0/20, 11.0/20, 29.0/20);
|
||
|
polar2(40, r4, 3.0/28, 53.0/28, 25.0/28, 31.0/28);
|
||
|
polar2(44, r2, 5.0/12, 19.0/12, 7.0/12, 17.0/12);
|
||
|
polar2(48, r3, 1.0/ 4, 7.0/ 4, 3.0/ 4, 5.0/ 4);
|
||
|
polar2(52, r3, 7.0/20, 33.0/20, 13.0/20, 27.0/20);
|
||
|
polar2(56, r3, 3.0/20, 37.0/20, 17.0/20, 23.0/20);
|
||
|
polar2(60, r2, 1.0/ 4, 7.0/ 4, 3.0/ 4, 5.0/ 4);
|
||
|
make_lut_from_symbols();
|
||
|
break;
|
||
|
}
|
||
|
case QAM16:
|
||
|
make_qam(16);
|
||
|
break;
|
||
|
case QAM64:
|
||
|
make_qam(64);
|
||
|
break;
|
||
|
case QAM256:
|
||
|
make_qam(256);
|
||
|
break;
|
||
|
default:
|
||
|
fail("Constellation not implemented");
|
||
|
}
|
||
|
}
|
||
|
struct result {
|
||
|
struct softsymbol ss;
|
||
|
s_angle phase_error;
|
||
|
};
|
||
|
inline result *lookup(float I, float Q) {
|
||
|
// Handling of overflows beyond the lookup table:
|
||
|
// - For BPSK/QPSK/8PSK we only care about the phase,
|
||
|
// so the following is harmless and improves locking at low SNR.
|
||
|
// - For amplitude modulations this is not appropriate.
|
||
|
// However, if there is enough noise to cause overflow,
|
||
|
// demodulation would probably fail anyway.
|
||
|
//
|
||
|
// Comment-out for better throughput at high SNR.
|
||
|
#if 1
|
||
|
while ( I<-128 || I>127 || Q<-128 || Q>127 ) {
|
||
|
I *= 0.5;
|
||
|
Q *= 0.5;
|
||
|
}
|
||
|
#endif
|
||
|
return &lut[(u8)(s8)I][(u8)(s8)Q];
|
||
|
}
|
||
|
inline result *lookup(int I, int Q) {
|
||
|
// Ignore wrapping modulo 256
|
||
|
return &lut[(u8)I][(u8)Q];
|
||
|
}
|
||
|
private:
|
||
|
complex<signed char> polar(float r, int n, float i) {
|
||
|
float a = i * 2*M_PI / n;
|
||
|
return complex<signed char>(r*cosf(a)*cstln_amp, r*sinf(a)*cstln_amp);
|
||
|
}
|
||
|
// Helper function for some constellation tables
|
||
|
void polar2(int i, float r, float a0, float a1, float a2, float a3) {
|
||
|
float a[] = { a0, a1, a2, a3 };
|
||
|
for ( int j=0; j<4; ++j ) {
|
||
|
float phi = a[j] * M_PI;
|
||
|
symbols[i+j] = complex<signed char>(r*cosf(phi)*cstln_amp,
|
||
|
r*sinf(phi)*cstln_amp);
|
||
|
}
|
||
|
}
|
||
|
void make_qam(int n) {
|
||
|
nrotations = 4;
|
||
|
nsymbols = n;
|
||
|
symbols = new complex<signed char>[nsymbols];
|
||
|
int m = sqrtl(n);
|
||
|
float scale;
|
||
|
{ // Average power in first quadrant with unit grid
|
||
|
int q = m / 2;
|
||
|
float avgpower = 2*(q*0.25+(q-1)*q/2+(q-1)*q*(2*q-1)/6) / q;
|
||
|
scale = 1.0 / sqrtf(avgpower);
|
||
|
}
|
||
|
// Arbitrary mapping
|
||
|
int s = 0;
|
||
|
for ( int x=0; x<m; ++x )
|
||
|
for ( int y=0; y<m; ++y ) {
|
||
|
float I = x - (float)(m-1)/2;
|
||
|
float Q = y - (float)(m-1)/2;
|
||
|
symbols[s].re = I * scale * cstln_amp;
|
||
|
symbols[s].im = Q * scale * cstln_amp;
|
||
|
++s;
|
||
|
}
|
||
|
make_lut_from_symbols();
|
||
|
}
|
||
|
result lut[R][R];
|
||
|
void make_lut_from_symbols() {
|
||
|
for ( int I=-R/2; I<R/2; ++I )
|
||
|
for ( int Q=-R/2; Q<R/2; ++Q ) {
|
||
|
result *pr = &lut[I&(R-1)][Q&(R-1)];
|
||
|
// Simplified metric:
|
||
|
// Distance to nearest minus distance to second-nearest.
|
||
|
// Null at edge of decision regions
|
||
|
// => Suitable for Viterbi with partial metrics.
|
||
|
uint8_t nearest = 0;
|
||
|
int32_t cost=R*R*2, cost2=R*R*2;
|
||
|
for ( int s=0; s<nsymbols; ++s ) {
|
||
|
int32_t d2 =
|
||
|
(I-symbols[s].re)*(I-symbols[s].re) +
|
||
|
(Q-symbols[s].im)*(Q-symbols[s].im);
|
||
|
if ( d2 < cost ) {
|
||
|
cost2 = cost;
|
||
|
cost = d2;
|
||
|
nearest = s;
|
||
|
} else if ( d2 < cost2 ) {
|
||
|
cost2 = d2;
|
||
|
}
|
||
|
}
|
||
|
if ( cost > 32767 ) cost = 32767;
|
||
|
if ( cost2 > 32767 ) cost2 = 32767;
|
||
|
pr->ss.cost = cost - cost2;
|
||
|
pr->ss.symbol = nearest;
|
||
|
float ph_symbol = atan2f(symbols[pr->ss.symbol].im,
|
||
|
symbols[pr->ss.symbol].re);
|
||
|
float ph_err = atan2f(Q,I) - ph_symbol;
|
||
|
pr->phase_error = (s32)(ph_err * 65536 / (2*M_PI)); // Mod 65536
|
||
|
}
|
||
|
}
|
||
|
|
||
|
public:
|
||
|
// Convert soft metric to Hamming distance
|
||
|
void harden() {
|
||
|
for ( int i=0; i<R; ++i )
|
||
|
for ( int q=0; q<R; ++q ) {
|
||
|
softsymbol *ss = &lut[i][q].ss;
|
||
|
if ( ss->cost < 0 ) ss->cost = -1;
|
||
|
if ( ss->cost > 0 ) ss->cost = 1;
|
||
|
} // for I,Q
|
||
|
}
|
||
|
|
||
|
}; // cstln_lut
|
||
|
|
||
|
static const char *cstln_names[] = {
|
||
|
[cstln_lut<256>::BPSK] = "BPSK",
|
||
|
[cstln_lut<256>::QPSK] = "QPSK",
|
||
|
[cstln_lut<256>::PSK8] = "8PSK",
|
||
|
[cstln_lut<256>::APSK16] = "16APSK",
|
||
|
[cstln_lut<256>::APSK32] = "32APSK",
|
||
|
[cstln_lut<256>::APSK64E] = "64APSKe",
|
||
|
[cstln_lut<256>::QAM16] = "16QAM",
|
||
|
[cstln_lut<256>::QAM64] = "64QAM",
|
||
|
[cstln_lut<256>::QAM256] = "256QAM"
|
||
|
};
|
||
|
|
||
|
// SAMPLER INTERFACE FOR CSTLN_RECEIVER
|
||
|
|
||
|
template<typename T>
|
||
|
struct sampler_interface {
|
||
|
virtual complex<T> interp(const complex<T> *pin, float mu, float phase) = 0;
|
||
|
virtual void update_freq(float freqw) { } // 65536 = 1 Hz
|
||
|
virtual int readahead() { return 0; }
|
||
|
};
|
||
|
|
||
|
|
||
|
// NEAREST-SAMPLE SAMPLER FOR CSTLN_RECEIVER
|
||
|
// Suitable for bandpass-filtered, oversampled signals only
|
||
|
|
||
|
template<typename T>
|
||
|
struct nearest_sampler : sampler_interface<T> {
|
||
|
int readahead() { return 0; }
|
||
|
complex<T> interp(const complex<T> *pin, float mu, float phase) {
|
||
|
return pin[0]*trig.expi(-phase);
|
||
|
}
|
||
|
private:
|
||
|
trig16 trig;
|
||
|
}; // nearest_sampler
|
||
|
|
||
|
|
||
|
// LINEAR SAMPLER FOR CSTLN_RECEIVER
|
||
|
|
||
|
template<typename T>
|
||
|
struct linear_sampler : sampler_interface<T> {
|
||
|
int readahead() { return 1; }
|
||
|
|
||
|
complex<T> interp(const complex<T> *pin, float mu, float phase) {
|
||
|
// Derotate pin[0] and pin[1]
|
||
|
complex<T> s0 = pin[0]*trig.expi(-phase);
|
||
|
complex<T> s1 = pin[1]*trig.expi(-(phase+freqw));
|
||
|
// Interpolate linearly
|
||
|
return s0*(1-mu) + s1*mu;
|
||
|
}
|
||
|
|
||
|
void update_freq(float _freqw) { freqw = _freqw; }
|
||
|
|
||
|
private:
|
||
|
trig16 trig;
|
||
|
float freqw;
|
||
|
}; // linear_sampler
|
||
|
|
||
|
|
||
|
// FIR SAMPLER FOR CSTLN_RECEIVER
|
||
|
|
||
|
template<typename T, typename Tc>
|
||
|
struct fir_sampler : sampler_interface<T> {
|
||
|
fir_sampler(int _ncoeffs, Tc *_coeffs, int _subsampling=1)
|
||
|
: ncoeffs(_ncoeffs), coeffs(_coeffs), subsampling(_subsampling),
|
||
|
shifted_coeffs(new complex<T>[ncoeffs]),
|
||
|
update_freq_phase(0)
|
||
|
{
|
||
|
}
|
||
|
|
||
|
int readahead() { return ncoeffs-1; }
|
||
|
|
||
|
complex<T> interp(const complex<T> *pin, float mu, float phase) {
|
||
|
// Apply FIR filter with subsampling
|
||
|
complex<T> acc(0, 0);
|
||
|
complex<T> *pc = shifted_coeffs + (int)((1-mu)*subsampling);
|
||
|
complex<T> *pcend = shifted_coeffs + ncoeffs;
|
||
|
if ( subsampling == 1 ) {
|
||
|
// Special case for heavily oversampled signals,
|
||
|
// where filtering is expensive.
|
||
|
// gcc-4.9.2 can vectorize this form with NEON on ARM.
|
||
|
while ( pc < pcend )
|
||
|
acc += (*pc++)*(*pin++);
|
||
|
} else {
|
||
|
// Not vectorized because the coefficients are not
|
||
|
// guaranteed to be contiguous in memory.
|
||
|
for ( ; pc<pcend; pc+=subsampling,++pin )
|
||
|
acc += (*pc)*(*pin);
|
||
|
}
|
||
|
// Derotate
|
||
|
return trig.expi(-phase) * acc;
|
||
|
}
|
||
|
|
||
|
void update_freq(float freqw) {
|
||
|
// Throttling: Update one coeff per 16 processed samples,
|
||
|
// to keep the overhead of freq tracking below about 10%.
|
||
|
update_freq_phase -= 128; // chunk_size of cstln_receiver
|
||
|
if ( update_freq_phase <= 0 ) {
|
||
|
update_freq_phase = ncoeffs*16;
|
||
|
do_update_freq(freqw);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
private:
|
||
|
void do_update_freq(float freqw) {
|
||
|
float f = freqw / subsampling;
|
||
|
for ( int i=0; i<ncoeffs; ++i )
|
||
|
shifted_coeffs[i] = trig.expi(-f*(i-ncoeffs/2)) * coeffs[i];
|
||
|
}
|
||
|
trig16 trig;
|
||
|
int ncoeffs;
|
||
|
Tc *coeffs;
|
||
|
int subsampling;
|
||
|
cf32 *shifted_coeffs;
|
||
|
int update_freq_phase;
|
||
|
}; // fir_sampler
|
||
|
|
||
|
|
||
|
// CONSTELLATION RECEIVER
|
||
|
|
||
|
// Linear interpolation: good enough for 1.2 samples/symbol,
|
||
|
// but higher oversampling is recommended.
|
||
|
|
||
|
template<typename T>
|
||
|
struct cstln_receiver : runnable {
|
||
|
sampler_interface<T> *sampler;
|
||
|
cstln_lut<256> *cstln;
|
||
|
unsigned long meas_decimation; // Measurement rate
|
||
|
float omega, min_omega, max_omega; // Samples per symbol
|
||
|
float freqw, min_freqw, max_freqw; // Freq offs (65536 = 1 Hz)
|
||
|
float pll_adjustment;
|
||
|
bool allow_drift; // Follow carrier beyond safe limits
|
||
|
static const unsigned int chunk_size = 128;
|
||
|
float kest;
|
||
|
|
||
|
cstln_receiver(scheduler *sch,
|
||
|
sampler_interface<T> *_sampler,
|
||
|
pipebuf< complex<T> > &_in,
|
||
|
pipebuf<softsymbol> &_out,
|
||
|
pipebuf<float> *_freq_out=NULL,
|
||
|
pipebuf<float> *_ss_out=NULL,
|
||
|
pipebuf<float> *_mer_out=NULL,
|
||
|
pipebuf<cf32> *_cstln_out=NULL)
|
||
|
: runnable(sch, "Constellation receiver"),
|
||
|
sampler(_sampler),
|
||
|
cstln(NULL),
|
||
|
meas_decimation(1048576),
|
||
|
pll_adjustment(1.0),
|
||
|
allow_drift(false),
|
||
|
kest(0.01),
|
||
|
in(_in), out(_out, chunk_size),
|
||
|
est_insp(cstln_amp*cstln_amp), agc_gain(1),
|
||
|
mu(0), phase(0),
|
||
|
est_sp(0), est_ep(0),
|
||
|
meas_count(0) {
|
||
|
set_omega(1);
|
||
|
set_freq(0);
|
||
|
freq_out = _freq_out ? new pipewriter<float>(*_freq_out) : NULL;
|
||
|
ss_out = _ss_out ? new pipewriter<float>(*_ss_out) : NULL;
|
||
|
mer_out = _mer_out ? new pipewriter<float>(*_mer_out) : NULL;
|
||
|
cstln_out = _cstln_out ? new pipewriter<cf32>(*_cstln_out) : NULL;
|
||
|
memset(hist, 0, sizeof(hist));
|
||
|
}
|
||
|
|
||
|
void set_omega(float _omega, float tol=10e-6) {
|
||
|
omega = _omega;
|
||
|
min_omega = omega * (1-tol);
|
||
|
max_omega = omega * (1+tol);
|
||
|
update_freq_limits();
|
||
|
}
|
||
|
|
||
|
void set_freq(float freq) {
|
||
|
freqw = freq * 65536;
|
||
|
update_freq_limits();
|
||
|
refresh_freq_tap();
|
||
|
}
|
||
|
|
||
|
void set_allow_drift(bool d) {
|
||
|
allow_drift = d;
|
||
|
}
|
||
|
|
||
|
void update_freq_limits() {
|
||
|
// Prevent PLL from crossing +-SR/n/2 and locking at +-SR/n.
|
||
|
int n = 4;
|
||
|
if ( cstln ) {
|
||
|
switch ( cstln->nsymbols ) {
|
||
|
case 2: n = 2; break; // BPSK
|
||
|
case 4: n = 4; break; // QPSK
|
||
|
case 8: n = 8; break; // 8PSK
|
||
|
case 16: n = 12; break; // 16APSK
|
||
|
case 32: n = 16; break; // 32APSK
|
||
|
default: n = 4; break;
|
||
|
}
|
||
|
}
|
||
|
min_freqw = freqw - 65536/max_omega/n/2;
|
||
|
max_freqw = freqw + 65536/max_omega/n/2;
|
||
|
}
|
||
|
|
||
|
void run() {
|
||
|
if ( ! cstln ) fail("constellation not set");
|
||
|
|
||
|
// Magic constants that work with the qa recordings.
|
||
|
float freq_alpha = 0.04;
|
||
|
float freq_beta = 0.0012 / omega * pll_adjustment;
|
||
|
float gain_mu = 0.02 / (cstln_amp*cstln_amp) * 2;
|
||
|
|
||
|
int max_meas = chunk_size/meas_decimation + 1;
|
||
|
// Large margin on output_size because mu adjustments
|
||
|
// can lead to more than chunk_size/min_omega symbols.
|
||
|
while ( in.readable() >= chunk_size+sampler->readahead() &&
|
||
|
out.writable() >= chunk_size &&
|
||
|
( !freq_out || freq_out ->writable()>=max_meas ) &&
|
||
|
( !ss_out || ss_out ->writable()>=max_meas ) &&
|
||
|
( !mer_out || mer_out ->writable()>=max_meas ) &&
|
||
|
( !cstln_out || cstln_out->writable()>=max_meas ) ) {
|
||
|
|
||
|
sampler->update_freq(freqw);
|
||
|
|
||
|
complex<T> *pin=in.rd(), *pin0=pin, *pend=pin+chunk_size;
|
||
|
softsymbol *pout=out.wr(), *pout0=pout;
|
||
|
|
||
|
// These are scoped outside the loop for SS and MER estimation.
|
||
|
complex<float> sg; // Symbol before AGC;
|
||
|
complex<float> s; // For MER estimation and constellation viewer
|
||
|
complex<signed char> *cstln_point = NULL;
|
||
|
|
||
|
while ( pin < pend ) {
|
||
|
// Here mu is the time of the next symbol counted from 0 at pin.
|
||
|
if ( mu < 1 ) {
|
||
|
// Here 0<=mu<1 is the fractional time of the next symbol
|
||
|
// between pin and pin+1.
|
||
|
sg = sampler->interp(pin, mu, phase);
|
||
|
s = sg * agc_gain;
|
||
|
|
||
|
// Constellation look-up
|
||
|
cstln_lut<256>::result *cr = cstln->lookup(s.re, s.im);
|
||
|
*pout = cr->ss;
|
||
|
++pout;
|
||
|
|
||
|
// PLL
|
||
|
phase += cr->phase_error * freq_alpha;
|
||
|
freqw += cr->phase_error * freq_beta;
|
||
|
|
||
|
// Modified Mueller and Müller
|
||
|
// mu[k]=real((c[k]-c[k-2])*conj(p[k-1])-(p[k]-p[k-2])*conj(c[k-1]))
|
||
|
// =dot(c[k]-c[k-2],p[k-1]) - dot(p[k]-p[k-2],c[k-1])
|
||
|
// p = received signals
|
||
|
// c = decisions (constellation points)
|
||
|
hist[2] = hist[1];
|
||
|
hist[1] = hist[0];
|
||
|
hist[0].p.re = s.re;
|
||
|
hist[0].p.im = s.im;
|
||
|
cstln_point = &cstln->symbols[cr->ss.symbol];
|
||
|
hist[0].c.re = cstln_point->re;
|
||
|
hist[0].c.im = cstln_point->im;
|
||
|
float muerr =
|
||
|
( (hist[0].p.re-hist[2].p.re)*hist[1].c.re +
|
||
|
(hist[0].p.im-hist[2].p.im)*hist[1].c.im ) -
|
||
|
( (hist[0].c.re-hist[2].c.re)*hist[1].p.re +
|
||
|
(hist[0].c.im-hist[2].c.im)*hist[1].p.im );
|
||
|
float mucorr = muerr * gain_mu;
|
||
|
const float max_mucorr = 0.1;
|
||
|
// TBD Optimize out statically
|
||
|
if ( mucorr < -max_mucorr ) mucorr = -max_mucorr;
|
||
|
if ( mucorr > max_mucorr ) mucorr = max_mucorr;
|
||
|
mu += mucorr;
|
||
|
mu += omega; // Next symbol time;
|
||
|
} // mu<1
|
||
|
|
||
|
// Next sample
|
||
|
++pin;
|
||
|
--mu;
|
||
|
phase += freqw;
|
||
|
} // chunk_size
|
||
|
|
||
|
in.read(pin-pin0);
|
||
|
out.written(pout-pout0);
|
||
|
|
||
|
// Normalize phase so that it never exceeds 32 bits.
|
||
|
// Max freqw is 2^31/65536/chunk_size = 256 Hz
|
||
|
// (this may happen with leandvb --drift --decim).
|
||
|
phase = fmodf(phase, 65536);
|
||
|
|
||
|
if ( cstln_point ) {
|
||
|
|
||
|
// Output the last interpolated PSK symbol, max once per chunk_size
|
||
|
if ( cstln_out )
|
||
|
cstln_out->write(s);
|
||
|
|
||
|
// AGC
|
||
|
// For APSK we must do AGC on the symbols, not the whole signal.
|
||
|
// TODO Use a better estimator at low SNR.
|
||
|
float insp = sg.re*sg.re + sg.im*sg.im;
|
||
|
est_insp = insp*kest + est_insp*(1-kest);
|
||
|
if ( est_insp )
|
||
|
agc_gain = cstln_amp / gen_sqrt(est_insp);
|
||
|
|
||
|
// SS and MER
|
||
|
complex<float> ev(s.re-cstln_point->re, s.im-cstln_point->im);
|
||
|
float sig_power, ev_power;
|
||
|
if ( cstln->nsymbols == 2 ) {
|
||
|
// Special case for BPSK: Ignore quadrature component of noise.
|
||
|
// TBD Projection on I axis assumes BPSK at 45°
|
||
|
float sig_real = (cstln_point->re+cstln_point->im) * 0.707;
|
||
|
float ev_real = (ev.re+ev.im) * 0.707;
|
||
|
sig_power = sig_real * sig_real;
|
||
|
ev_power = ev_real * ev_real;
|
||
|
} else {
|
||
|
sig_power =
|
||
|
(int)cstln_point->re*cstln_point->re +
|
||
|
(int)cstln_point->im*cstln_point->im;
|
||
|
ev_power = ev.re*ev.re + ev.im*ev.im;
|
||
|
}
|
||
|
est_sp = sig_power*kest + est_sp*(1-kest);
|
||
|
est_ep = ev_power*kest + est_ep*(1-kest);
|
||
|
|
||
|
}
|
||
|
|
||
|
// This is best done periodically ouside the inner loop,
|
||
|
// but will cause non-deterministic output.
|
||
|
|
||
|
if ( ! allow_drift ) {
|
||
|
if ( freqw < min_freqw || freqw > max_freqw )
|
||
|
freqw = (max_freqw+min_freqw) / 2;
|
||
|
}
|
||
|
|
||
|
// Output measurements
|
||
|
|
||
|
refresh_freq_tap();
|
||
|
|
||
|
meas_count += pin-pin0;
|
||
|
while ( meas_count >= meas_decimation ) {
|
||
|
meas_count -= meas_decimation;
|
||
|
if ( freq_out )
|
||
|
freq_out->write(freq_tap);
|
||
|
if ( ss_out )
|
||
|
ss_out->write(sqrtf(est_insp));
|
||
|
if ( mer_out )
|
||
|
mer_out->write(est_ep ? 10*logf(est_sp/est_ep)/logf(10) : 0);
|
||
|
}
|
||
|
|
||
|
} // Work to do
|
||
|
}
|
||
|
|
||
|
float freq_tap;
|
||
|
void refresh_freq_tap() {
|
||
|
freq_tap = freqw / 65536;
|
||
|
}
|
||
|
private:
|
||
|
struct {
|
||
|
complex<float> p; // Received symbol
|
||
|
complex<float> c; // Matched constellation point
|
||
|
} hist[3];
|
||
|
pipereader< complex<T> > in;
|
||
|
pipewriter<softsymbol> out;
|
||
|
float est_insp, agc_gain;
|
||
|
float mu; // PSK time expressed in clock ticks
|
||
|
float phase; // 65536=2pi
|
||
|
// Signal estimation
|
||
|
float est_sp; // Estimated RMS signal power
|
||
|
float est_ep; // Estimated RMS error vector power
|
||
|
unsigned long meas_count;
|
||
|
pipewriter<float> *freq_out, *ss_out, *mer_out;
|
||
|
pipewriter<cf32> *cstln_out;
|
||
|
};
|
||
|
|
||
|
|
||
|
// FAST QPSK RECEIVER
|
||
|
|
||
|
// Optimized for u8 input, no AGC, uses phase information only.
|
||
|
// Outputs hard symbols.
|
||
|
|
||
|
template<typename T>
|
||
|
struct fast_qpsk_receiver : runnable {
|
||
|
typedef u8 hardsymbol;
|
||
|
unsigned long meas_decimation; // Measurement rate
|
||
|
float omega, min_omega, max_omega; // Samples per symbol
|
||
|
signed long freqw, min_freqw, max_freqw; // Freq offs (angle per sample)
|
||
|
float pll_adjustment;
|
||
|
bool allow_drift; // Follow carrier beyond safe limits
|
||
|
static const unsigned int chunk_size = 128;
|
||
|
|
||
|
fast_qpsk_receiver(scheduler *sch,
|
||
|
pipebuf< complex<T> > &_in,
|
||
|
pipebuf<hardsymbol> &_out,
|
||
|
pipebuf<float> *_freq_out=NULL,
|
||
|
pipebuf< complex<T> > *_cstln_out=NULL)
|
||
|
: runnable(sch, "Fast QPSK receiver"),
|
||
|
meas_decimation(1048576),
|
||
|
pll_adjustment(1.0),
|
||
|
allow_drift(false),
|
||
|
in(_in), out(_out, chunk_size),
|
||
|
mu(0), phase(0),
|
||
|
meas_count(0)
|
||
|
{
|
||
|
set_omega(1);
|
||
|
set_freq(0);
|
||
|
freq_out = _freq_out ? new pipewriter<float>(*_freq_out) : NULL;
|
||
|
cstln_out = _cstln_out ? new pipewriter< complex<T> >(*_cstln_out) : NULL;
|
||
|
memset(hist, 0, sizeof(hist));
|
||
|
init_lookup_tables();
|
||
|
}
|
||
|
|
||
|
void set_omega(float _omega, float tol=10e-6) {
|
||
|
omega = _omega;
|
||
|
min_omega = omega * (1-tol);
|
||
|
max_omega = omega * (1+tol);
|
||
|
update_freq_limits();
|
||
|
}
|
||
|
|
||
|
void set_freq(float freq) {
|
||
|
freqw = freq * 65536;
|
||
|
update_freq_limits();
|
||
|
}
|
||
|
|
||
|
void update_freq_limits() {
|
||
|
// Prevent PLL from locking at +-symbolrate/4.
|
||
|
// TODO The +-SR/8 limit is suitable for QPSK only.
|
||
|
min_freqw = freqw - 65536/max_omega/8;
|
||
|
max_freqw = freqw + 65536/max_omega/8;
|
||
|
}
|
||
|
|
||
|
static const int RLUT_BITS = 8;
|
||
|
static const int RLUT_ANGLES = 1 << RLUT_BITS;
|
||
|
|
||
|
void run() {
|
||
|
// Magic constants that work with the qa recordings.
|
||
|
signed long freq_alpha = 0.04 * 65536;
|
||
|
signed long freq_beta = 0.0012 * 256 * 65536 / omega * pll_adjustment;
|
||
|
if ( ! freq_beta ) fail("Excessive oversampling");
|
||
|
|
||
|
float gain_mu = 0.02 / (cstln_amp*cstln_amp) * 2;
|
||
|
|
||
|
int max_meas = chunk_size/meas_decimation + 1;
|
||
|
// Largin margin on output_size because mu adjustments
|
||
|
// can lead to more than chunk_size/min_omega symbols.
|
||
|
while ( in.readable() >= chunk_size+1 && // +1 for interpolation
|
||
|
out.writable() >= chunk_size &&
|
||
|
( !freq_out || freq_out ->writable()>=max_meas ) &&
|
||
|
( !cstln_out || cstln_out->writable()>=max_meas ) ) {
|
||
|
|
||
|
complex<T> *pin=in.rd(), *pin0=pin, *pend=pin+chunk_size;
|
||
|
hardsymbol *pout=out.wr(), *pout0=pout;
|
||
|
|
||
|
cu8 s;
|
||
|
u_angle symbol_arg = 0; // Exported for constellation viewer
|
||
|
|
||
|
while ( pin < pend ) {
|
||
|
// Here mu is the time of the next symbol counted from 0 at pin.
|
||
|
if ( mu < 1 ) {
|
||
|
// Here 0<=mu<1 is the fractional time of the next symbol
|
||
|
// between pin and pin+1.
|
||
|
|
||
|
// Derotate and interpolate
|
||
|
#if 0 // Phase only (does not work)
|
||
|
// Careful with the float/signed/unsigned casts
|
||
|
u_angle a0 = fast_arg(pin[0]) - phase;
|
||
|
u_angle a1 = fast_arg(pin[1]) - (phase+freqw);
|
||
|
s_angle da = a1 - a0;
|
||
|
symbol_arg = a0 + (s_angle)(da*mu);
|
||
|
s = arg_to_symbol(symbol_arg);
|
||
|
#elif 1 // Linear by lookup-table. 1.2M on bench3bishs
|
||
|
polar *p0 = &lut_polar[pin[0].re][pin[0].im];
|
||
|
u_angle a0 = (u_angle)(p0->a-phase) >> (16-RLUT_BITS);
|
||
|
cu8 *p0r = &lut_rect[a0][p0->r>>1];
|
||
|
polar *p1 = &lut_polar[pin[1].re][pin[1].im];
|
||
|
u_angle a1 = (u_angle)(p1->a-(phase+freqw)) >> (16-RLUT_BITS);
|
||
|
cu8 *p1r = &lut_rect[a1][p1->r>>1];
|
||
|
s.re = (int)(p0r->re + (p1r->re-p0r->re)*mu);
|
||
|
s.im = (int)(p0r->im + (p1r->im-p0r->im)*mu);
|
||
|
symbol_arg = fast_arg(s);
|
||
|
#else // Linear floating-point, for reference
|
||
|
float a0 = -(int)phase*M_PI/32768;
|
||
|
float cosa0=cosf(a0), sina0=sinf(a0);
|
||
|
complex<float>
|
||
|
p0r(((float)pin[0].re-128)*cosa0 - ((float)pin[0].im-128)*sina0,
|
||
|
((float)pin[0].re-128)*sina0 + ((float)pin[0].im-128)*cosa0);
|
||
|
float a1 = -(int)(phase+freqw)*M_PI/32768;
|
||
|
float cosa1=cosf(a1), sina1=sinf(a1);
|
||
|
complex<float>
|
||
|
p1r(((float)pin[1].re-128)*cosa1 - ((float)pin[1].im-128)*sina1,
|
||
|
((float)pin[1].re-128)*sina1 + ((float)pin[1].im-128)*cosa1);
|
||
|
s.re = (int)(128 + p0r.re + (p1r.re-p0r.re)*mu);
|
||
|
s.im = (int)(128 + p0r.im + (p1r.im-p0r.im)*mu);
|
||
|
symbol_arg = fast_arg(s);
|
||
|
#endif
|
||
|
|
||
|
int quadrant = symbol_arg >> 14;
|
||
|
static unsigned char quadrant_to_symbol[4] = { 0, 2, 3, 1 };
|
||
|
*pout = quadrant_to_symbol[quadrant];
|
||
|
++pout;
|
||
|
|
||
|
// PLL
|
||
|
s_angle phase_error = (s_angle)(symbol_arg&16383) - 8192;
|
||
|
phase += (phase_error * freq_alpha + 32768) >> 16;
|
||
|
freqw += (phase_error * freq_beta + 32768*256) >> 24;
|
||
|
|
||
|
// Modified Mueller and Müller
|
||
|
// mu[k]=real((c[k]-c[k-2])*conj(p[k-1])-(p[k]-p[k-2])*conj(c[k-1]))
|
||
|
// =dot(c[k]-c[k-2],p[k-1]) - dot(p[k]-p[k-2],c[k-1])
|
||
|
// p = received signals
|
||
|
// c = decisions (constellation points)
|
||
|
hist[2] = hist[1];
|
||
|
hist[1] = hist[0];
|
||
|
#define HIST_FLOAT 0
|
||
|
#if HIST_FLOAT
|
||
|
hist[0].p.re = (float)s.re - 128;
|
||
|
hist[0].p.im = (float)s.im - 128;
|
||
|
|
||
|
cu8 cp = arg_to_symbol((symbol_arg&49152)+8192);
|
||
|
hist[0].c.re = (float)cp.re - 128;
|
||
|
hist[0].c.im = (float)cp.im - 128;
|
||
|
|
||
|
float muerr =
|
||
|
( (hist[0].p.re-hist[2].p.re)*hist[1].c.re +
|
||
|
(hist[0].p.im-hist[2].p.im)*hist[1].c.im ) -
|
||
|
( (hist[0].c.re-hist[2].c.re)*hist[1].p.re +
|
||
|
(hist[0].c.im-hist[2].c.im)*hist[1].p.im );
|
||
|
#else
|
||
|
hist[0].p = s;
|
||
|
hist[0].c = arg_to_symbol((symbol_arg&49152)+8192);
|
||
|
|
||
|
int muerr =
|
||
|
( (signed char)(hist[0].p.re-hist[2].p.re)*((int)hist[1].c.re-128) +
|
||
|
(signed char)(hist[0].p.im-hist[2].p.im)*((int)hist[1].c.im-128) ) -
|
||
|
( (signed char)(hist[0].c.re-hist[2].c.re)*((int)hist[1].p.re-128) +
|
||
|
(signed char)(hist[0].c.im-hist[2].c.im)*((int)hist[1].p.im-128) );
|
||
|
#endif
|
||
|
float mucorr = muerr * gain_mu;
|
||
|
const float max_mucorr = 0.1;
|
||
|
// TBD Optimize out statically
|
||
|
if ( mucorr < -max_mucorr ) mucorr = -max_mucorr;
|
||
|
if ( mucorr > max_mucorr ) mucorr = max_mucorr;
|
||
|
mu += mucorr;
|
||
|
mu += omega; // Next symbol time;
|
||
|
} // mu<1
|
||
|
|
||
|
// Next sample
|
||
|
++pin;
|
||
|
--mu;
|
||
|
phase += freqw;
|
||
|
} // chunk_size
|
||
|
|
||
|
in.read(pin-pin0);
|
||
|
out.written(pout-pout0);
|
||
|
|
||
|
if ( symbol_arg && cstln_out )
|
||
|
// Output the last interpolated PSK symbol, max once per chunk_size
|
||
|
cstln_out->write(s);
|
||
|
|
||
|
// This is best done periodically ouside the inner loop,
|
||
|
// but will cause non-deterministic output.
|
||
|
|
||
|
if ( ! allow_drift ) {
|
||
|
if ( freqw < min_freqw || freqw > max_freqw )
|
||
|
freqw = (max_freqw+min_freqw) / 2;
|
||
|
}
|
||
|
|
||
|
// Output measurements
|
||
|
|
||
|
meas_count += pin-pin0;
|
||
|
while ( meas_count >= meas_decimation ) {
|
||
|
meas_count -= meas_decimation;
|
||
|
if ( freq_out )
|
||
|
freq_out->write((float)freqw / 65536);
|
||
|
}
|
||
|
|
||
|
} // Work to do
|
||
|
}
|
||
|
|
||
|
private:
|
||
|
|
||
|
struct polar { u_angle a; unsigned char r; } lut_polar[256][256];
|
||
|
u_angle fast_arg(const cu8 &c) {
|
||
|
// TBD read cu8 as u16 index, same endianness as in init()
|
||
|
return lut_polar[c.re][c.im].a;
|
||
|
}
|
||
|
cu8 lut_rect[RLUT_ANGLES][256];
|
||
|
cu8 lut_sincos[65536];
|
||
|
cu8 arg_to_symbol(u_angle a) { return lut_sincos[a]; }
|
||
|
void init_lookup_tables() {
|
||
|
for ( int i=0; i<256; ++i )
|
||
|
for ( int q=0; q<256; ++q ) {
|
||
|
// Don't cast float to unsigned directly
|
||
|
lut_polar[i][q].a = (s_angle)(atan2f(q-128,i-128)*65536/(2*M_PI));
|
||
|
lut_polar[i][q].r = (int)hypotf(i-128,q-128);
|
||
|
}
|
||
|
for ( unsigned long a=0; a<65536; ++a ) {
|
||
|
float f = 2*M_PI * a / 65536;
|
||
|
lut_sincos[a].re = 128 + cstln_amp*cosf(f);
|
||
|
lut_sincos[a].im = 128 + cstln_amp*sinf(f);
|
||
|
}
|
||
|
for ( int a=0; a<RLUT_ANGLES; ++a )
|
||
|
for ( int r=0; r<256; ++r ) {
|
||
|
lut_rect[a][r].re = (int)(128 + r*cos(2*M_PI*a/RLUT_ANGLES));
|
||
|
lut_rect[a][r].im = (int)(128 + r*sin(2*M_PI*a/RLUT_ANGLES));
|
||
|
}
|
||
|
}
|
||
|
|
||
|
struct {
|
||
|
#if HIST_FLOAT
|
||
|
complex<float> p; // Received symbol
|
||
|
complex<float> c; // Matched constellation point
|
||
|
#else
|
||
|
cu8 p; // Received symbol
|
||
|
cu8 c; // Matched constellation point
|
||
|
#endif
|
||
|
} hist[3];
|
||
|
pipereader<cu8> in;
|
||
|
pipewriter<hardsymbol> out;
|
||
|
float mu; // PSK time expressed in clock ticks. TBD fixed point.
|
||
|
u_angle phase;
|
||
|
unsigned long meas_count;
|
||
|
pipewriter<float> *freq_out, *mer_out;
|
||
|
pipewriter<cu8> *cstln_out;
|
||
|
}; // fast_qpsk_receiver
|
||
|
|
||
|
|
||
|
// CONSTELLATION TRANSMITTER
|
||
|
|
||
|
// Maps symbols to I/Q points.
|
||
|
|
||
|
template<typename Tout, int Zout>
|
||
|
struct cstln_transmitter : runnable {
|
||
|
cstln_lut<256> *cstln;
|
||
|
cstln_transmitter(scheduler *sch,
|
||
|
pipebuf<u8> &_in, pipebuf< complex<Tout> > &_out)
|
||
|
: runnable(sch, "cstln_transmitter"),
|
||
|
in(_in), out(_out)
|
||
|
{
|
||
|
}
|
||
|
void run() {
|
||
|
if ( ! cstln ) fail("constellation not set");
|
||
|
int count = min(in.readable(), out.writable());
|
||
|
u8 *pin=in.rd(), *pend=pin+count;
|
||
|
complex<Tout> *pout = out.wr();
|
||
|
for ( ; pin<pend; ++pin,++pout ) {
|
||
|
complex<signed char> *cp = &cstln->symbols[*pin];
|
||
|
pout->re = Zout + cp->re;
|
||
|
pout->im = Zout + cp->im;
|
||
|
}
|
||
|
in.read(count);
|
||
|
out.written(count);
|
||
|
}
|
||
|
private:
|
||
|
pipereader<u8> in;
|
||
|
pipewriter< complex<Tout> > out;
|
||
|
}; // cstln_transmitter
|
||
|
|
||
|
|
||
|
// FREQUENCY SHIFTER
|
||
|
|
||
|
// Resolution is sample_freq/65536.
|
||
|
|
||
|
template<typename T>
|
||
|
struct rotator : runnable {
|
||
|
rotator(scheduler *sch, pipebuf< complex<T> > &_in,
|
||
|
pipebuf< complex<T> > &_out, float freq)
|
||
|
: runnable(sch, "rotator"),
|
||
|
in(_in), out(_out), index(0) {
|
||
|
int ifreq = freq * 65536;
|
||
|
if ( sch->debug )
|
||
|
fprintf(stderr, "Rotate: req=%f real=%f\n", freq, ifreq/65536.0);
|
||
|
for ( int i=0; i<65536; ++i ) {
|
||
|
lut_cos[i] = cosf(2*M_PI * i * ifreq / 65536);
|
||
|
lut_sin[i] = sinf(2*M_PI * i * ifreq / 65536);
|
||
|
}
|
||
|
}
|
||
|
void run() {
|
||
|
unsigned long count = min(in.readable(), out.writable());
|
||
|
complex<T> *pin = in.rd(), *pend = pin+count;
|
||
|
complex<T> *pout = out.wr();
|
||
|
for ( ; pin<pend; ++pin,++pout,++index ) {
|
||
|
float c = lut_cos[index];
|
||
|
float s = lut_sin[index];
|
||
|
pout->re = pin->re*c - pin->im*s;
|
||
|
pout->im = pin->re*s + pin->im*c;
|
||
|
}
|
||
|
in.read(count);
|
||
|
out.written(count);
|
||
|
}
|
||
|
private:
|
||
|
pipereader< complex<T> > in;
|
||
|
pipewriter< complex<T> > out;
|
||
|
float lut_cos[65536];
|
||
|
float lut_sin[65536];
|
||
|
unsigned short index; // Current phase
|
||
|
}; // rotator
|
||
|
|
||
|
|
||
|
// SPECTRUM-BASED CNR ESTIMATOR
|
||
|
|
||
|
// Assumes that the spectrum is as follows:
|
||
|
//
|
||
|
// ---|--noise---|-roll-off-|---carrier+noise----|-roll-off-|---noise--|---
|
||
|
// | (bw/2) | (bw) | (bw/2) | (bw) | (bw/2) |
|
||
|
//
|
||
|
// Maximum roll-off 0.5
|
||
|
|
||
|
template<typename T>
|
||
|
struct cnr_fft : runnable {
|
||
|
cnr_fft(scheduler *sch, pipebuf< complex<T> > &_in, pipebuf<float> &_out,
|
||
|
float _bandwidth, int nfft=4096)
|
||
|
: runnable(sch, "cnr_fft"),
|
||
|
bandwidth(_bandwidth), freq_tap(NULL), tap_multiplier(1),
|
||
|
decimation(1048576), kavg(0.1),
|
||
|
in(_in), out(_out),
|
||
|
fft(nfft), avgpower(NULL), phase(0) {
|
||
|
if ( bandwidth > 0.25 )
|
||
|
fail("CNR estimator requires Fsampling > 4x Fsignal");
|
||
|
}
|
||
|
|
||
|
float bandwidth;
|
||
|
float *freq_tap, tap_multiplier;
|
||
|
int decimation;
|
||
|
float kavg;
|
||
|
|
||
|
void run() {
|
||
|
while ( in.readable()>=fft.n && out.writable()>=1 ) {
|
||
|
phase += fft.n;
|
||
|
if ( phase >= decimation ) {
|
||
|
phase -= decimation;
|
||
|
do_cnr();
|
||
|
}
|
||
|
in.read(fft.n);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
private:
|
||
|
|
||
|
void do_cnr() {
|
||
|
float center_freq = freq_tap ? *freq_tap * tap_multiplier : 0;
|
||
|
int icf = floor(center_freq*fft.n+0.5);
|
||
|
complex<T> data[fft.n];
|
||
|
memcpy(data, in.rd(), fft.n*sizeof(data[0]));
|
||
|
fft.inplace(data, true);
|
||
|
T power[fft.n];
|
||
|
for ( int i=0; i<fft.n; ++i )
|
||
|
power[i] = data[i].re*data[i].re + data[i].im*data[i].im;
|
||
|
if ( ! avgpower ) {
|
||
|
// Initialize with first spectrum
|
||
|
avgpower = new T[fft.n];
|
||
|
memcpy(avgpower, power, fft.n*sizeof(avgpower[0]));
|
||
|
}
|
||
|
// Accumulate and low-pass filter
|
||
|
for ( int i=0; i<fft.n; ++i )
|
||
|
avgpower[i] = avgpower[i]*(1-kavg) + power[i]*kavg;
|
||
|
|
||
|
int bw__slots = (bandwidth/4) * fft.n;
|
||
|
if ( ! bw__slots ) return;
|
||
|
// Measure carrier+noise in center band
|
||
|
float c2plusn2 = avg__slots(icf-bw__slots, icf+bw__slots);
|
||
|
// Measure noise left and right of roll-off zones
|
||
|
float n2 = ( avg__slots(icf-bw__slots*4, icf-bw__slots*3) +
|
||
|
avg__slots(icf+bw__slots*3, icf+bw__slots*4) ) / 2;
|
||
|
float c2 = c2plusn2 - n2;
|
||
|
float cnr = (c2>0 && n2>0) ? 10 * logf(c2/n2)/logf(10) : -50;
|
||
|
out.write(cnr);
|
||
|
}
|
||
|
|
||
|
float avg__slots(int i0, int i1) { // i0 <= i1
|
||
|
T s = 0;
|
||
|
for ( int i=i0; i<=i1; ++i ) s += avgpower[i&(fft.n-1)];
|
||
|
return s / (i1-i0+1);
|
||
|
}
|
||
|
|
||
|
pipereader< complex<T> > in;
|
||
|
pipewriter< float > out;
|
||
|
cfft_engine<T> fft;
|
||
|
T *avgpower;
|
||
|
int phase;
|
||
|
}; // cnr_fft
|
||
|
|
||
|
template<typename T>
|
||
|
struct spectrum : runnable {
|
||
|
static const int nfft = 1024;
|
||
|
spectrum(scheduler *sch, pipebuf< complex<T> > &_in,
|
||
|
pipebuf<float[nfft]> &_out)
|
||
|
: runnable(sch, "spectrum"),
|
||
|
decimation(1048576), kavg(0.1),
|
||
|
in(_in), out(_out),
|
||
|
fft(nfft), avgpower(NULL), phase(0) {
|
||
|
}
|
||
|
|
||
|
int decimation;
|
||
|
float kavg;
|
||
|
|
||
|
void run() {
|
||
|
while ( in.readable()>=fft.n && out.writable()>=1 ) {
|
||
|
phase += fft.n;
|
||
|
if ( phase >= decimation ) {
|
||
|
phase -= decimation;
|
||
|
do_spectrum();
|
||
|
}
|
||
|
in.read(fft.n);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
private:
|
||
|
|
||
|
void do_spectrum() {
|
||
|
complex<T> data[fft.n];
|
||
|
memcpy(data, in.rd(), fft.n*sizeof(data[0]));
|
||
|
fft.inplace(data, true);
|
||
|
float power[nfft];
|
||
|
for ( int i=0; i<fft.n; ++i )
|
||
|
power[i] = (float)data[i].re*data[i].re + (float)data[i].im*data[i].im;
|
||
|
if ( ! avgpower ) {
|
||
|
// Initialize with first spectrum
|
||
|
avgpower = new float[fft.n];
|
||
|
memcpy(avgpower, power, fft.n*sizeof(avgpower[0]));
|
||
|
}
|
||
|
// Accumulate and low-pass filter
|
||
|
for ( int i=0; i<fft.n; ++i )
|
||
|
avgpower[i] = avgpower[i]*(1-kavg) + power[i]*kavg;
|
||
|
|
||
|
// Reuse power[]
|
||
|
for ( int i=0; i<fft.n/2; ++i ) {
|
||
|
power[i] = 10 * log10f(avgpower[nfft/2+i]);
|
||
|
power[nfft/2+i] = 10 * log10f(avgpower[i]);
|
||
|
}
|
||
|
memcpy(out.wr(), power, sizeof(power[0])*nfft);
|
||
|
out.written(1);
|
||
|
}
|
||
|
|
||
|
pipereader< complex<T> > in;
|
||
|
pipewriter< float[nfft] > out;
|
||
|
cfft_engine<T> fft;
|
||
|
T *avgpower;
|
||
|
int phase;
|
||
|
}; // spectrum
|
||
|
|
||
|
|
||
|
} // namespace
|
||
|
|
||
|
#endif // LEANSDR_SDR_H
|