VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-11-27 10:19:15 -05:00
Code Issues Projects Releases Wiki Activity
deae4e6271
sdrangel/plugins/channelrx/demoddatv/leansdr/sdr.h

1699 lines
53 KiB
C++
Raw Blame History

#ifndef LEANSDR_SDR_H
#define LEANSDR_SDR_H
#include "leansdr/math.h"
#include "leansdr/dsp.h"
#include "leansdr/incrementalarray.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];
}
m_data.allocate(fft.n);
m_amp.allocate(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];
complex<float> *data = m_data.m_array;
float m0 = 0, m2 = 0;
for (unsigned 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];
float *amp = m_amp.m_array;
for (unsigned 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 (unsigned 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 (unsigned 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 < (int) 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;
IncrementalArray<complex<float> > m_data;
IncrementalArray<float> m_amp;
};
// 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();
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("cstln_lut::cstln_lut", "Constellation not implemented");
return;
}
}
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 __attribute__((unused)))
{
} // 65536 = 1 Hz
virtual int readahead()
{
return 0;
}
virtual ~sampler_interface()
{
}
};
// 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 __attribute__((unused)), 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("cstln_lut::run", "constellation not set");
return;
}
// 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;
unsigned 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(0.0, 0.0); // Symbol before AGC;
complex<float> s(0.0, 0.0); // 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("fast_qpsk_receiver::run", "Excessive oversampling");
return;
}
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"),
cstln(0),
in(_in),
out(_out)
{
}
void run()
{
if (!cstln)
{
fail("cstln_transmitter::run", "constellation not set");
return;
}
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_fft::cnr_fft", "CNR estimator requires Fsampling > 4x Fsignal");
}
m_data.allocate(fft.n);
m_power.allocate(fft.n);
}
float bandwidth;
float *freq_tap, tap_multiplier;
int decimation;
float kavg;
IncrementalArray<complex<T> > m_data;
IncrementalArray<T> m_power;
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];
complex<T> *data = m_data.m_array;
memcpy(data, in.rd(), fft.n * sizeof(data[0]));
fft.inplace(data, true);
//T power[fft.n];
T *power = m_power.m_array;
for (unsigned 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 (unsigned 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
Reference in New Issue View Git Blame Copy Permalink