SDRAngel  4.11.5
Developer docs for <a href="https://github.com/f4exb/sdrangel">SDRangel<\a>, an Open Source Qt5 / OpenGL 3.0+ SDR and signal analyzer frontend to various hardware.
dsp.h
Go to the documentation of this file.
1 // This file is part of LeanSDR Copyright (C) 2016-2018 <pabr@pabr.org>.
2 // See the toplevel README for more information.
3 //
4 // This program is free software: you can redistribute it and/or modify
5 // it under the terms of the GNU General Public License as published by
6 // the Free Software Foundation, either version 3 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU General Public License for more details.
13 //
14 // You should have received a copy of the GNU General Public License
15 // along with this program. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef LEANSDR_DSP_H
18 #define LEANSDR_DSP_H
19 
20 #include "leansdr/framework.h"
21 #include "leansdr/math.h"
22 #include <math.h>
23 
24 namespace leansdr
25 {
26 
28 // DSP blocks
30 
31 // [cconverter] converts complex streams between numric types,
32 // with optional ofsetting and rational scaling.
33 template <typename Tin, int Zin, typename Tout, int Zout, int Gn, int Gd>
35 {
37  pipebuf<complex<Tout>> &_out)
38  : runnable(sch, "cconverter"),
39  in(_in), out(_out)
40  {
41  }
42  void run()
43  {
44  unsigned long count = min(in.readable(), out.writable());
45  complex<Tin> *pin = in.rd(), *pend = pin + count;
46  complex<Tout> *pout = out.wr();
47  for (; pin < pend; ++pin, ++pout)
48  {
49  pout->re = Zout + (pin->re - (Tin)Zin) * Gn / Gd;
50  pout->im = Zout + (pin->im - (Tin)Zin) * Gn / Gd;
51  }
52  in.read(count);
53  out.written(count);
54  }
55 
56  private:
59 };
60 
61 template <typename T>
63 {
64  const int n;
65  cfft_engine(int _n) : n(_n), invsqrtn(1.0 / sqrt(n))
66  {
67  // Compute log2(n)
68  logn = 0;
69  for (int t = n; t > 1; t >>= 1)
70  ++logn;
71  // Bit reversal
72  bitrev = new int[n];
73  for (int i = 0; i < n; ++i)
74  {
75  bitrev[i] = 0;
76  for (int b = 0; b < logn; ++b)
77  bitrev[i] = (bitrev[i] << 1) | ((i >> b) & 1);
78  }
79  // Float constants
80  omega = new complex<T>[n];
81  omega_rev = new complex<T>[n];
82  for (int i = 0; i < n; ++i)
83  {
84  float a = 2.0 * M_PI * i / n;
85  omega_rev[i].re = (omega[i].re = cosf(a));
86  omega_rev[i].im = -(omega[i].im = sinf(a));
87  }
88  }
89  void inplace(complex<T> *data, bool reverse = false)
90  {
91  // Bit-reversal permutation
92  for (int i = 0; i < n; ++i)
93  {
94  int r = bitrev[i];
95  if (r < i)
96  {
97  complex<T> tmp = data[i];
98  data[i] = data[r];
99  data[r] = tmp;
100  }
101  }
102  complex<T> *om = reverse ? omega_rev : omega;
103  // Danielson-Lanczos
104  for (int i = 0; i < logn; ++i)
105  {
106  int hbs = 1 << i;
107  int dom = 1 << (logn - 1 - i);
108  for (int j = 0; j < dom; ++j)
109  {
110  int p = j * hbs * 2, q = p + hbs;
111  for (int k = 0; k < hbs; ++k)
112  {
113  complex<T> &w = om[k * dom];
114  complex<T> &dqk = data[q + k];
115  complex<T> x(w.re * dqk.re - w.im * dqk.im,
116  w.re * dqk.im + w.im * dqk.re);
117  data[q + k].re = data[p + k].re - x.re;
118  data[q + k].im = data[p + k].im - x.im;
119  data[p + k].re = data[p + k].re + x.re;
120  data[p + k].im = data[p + k].im + x.im;
121  }
122  }
123  }
124  if (reverse)
125  {
126  float invn = 1.0 / n;
127  for (int i = 0; i < n; ++i)
128  {
129  data[i].re *= invn;
130  data[i].im *= invn;
131  }
132  }
133  }
134 
135  private:
136  int logn;
137  int *bitrev;
139  float invsqrtn;
140 };
141 
142 template <typename T>
143 struct adder : runnable
144 {
146  pipebuf<T> &_in1, pipebuf<T> &_in2, pipebuf<T> &_out)
147  : runnable(sch, "adder"),
148  in1(_in1), in2(_in2), out(_out)
149  {
150  }
151  void run()
152  {
153  int n = out.writable();
154  if (in1.readable() < n)
155  n = in1.readable();
156  if (in2.readable() < n)
157  n = in2.readable();
158  T *pin1 = in1.rd(), *pin2 = in2.rd(), *pout = out.wr(), *pend = pout + n;
159  while (pout < pend)
160  *pout++ = *pin1++ + *pin2++;
161  in1.read(n);
162  in2.read(n);
163  out.written(n);
164  }
165 
166  private:
169 };
170 
171 template <typename Tscale, typename Tin, typename Tout>
172 struct scaler : runnable
173 {
174  Tscale scale;
175  scaler(scheduler *sch, Tscale _scale,
176  pipebuf<Tin> &_in, pipebuf<Tout> &_out)
177  : runnable(sch, "scaler"),
178  scale(_scale),
179  in(_in), out(_out)
180  {
181  }
182  void run()
183  {
184  unsigned long count = min(in.readable(), out.writable());
185  Tin *pin = in.rd(), *pend = pin + count;
186  Tout *pout = out.wr();
187  for (; pin < pend; ++pin, ++pout)
188  *pout = *pin * scale;
189  in.read(count);
190  out.written(count);
191  }
192 
193  private:
196 };
197 
198 // [awgb_c] generates complex white gaussian noise.
199 
200 template <typename T>
201 struct wgn_c : runnable
202 {
204  : runnable(sch, "awgn"), stddev(1.0), out(_out)
205  {
206  }
207  void run()
208  {
209  int n = out.writable();
210  complex<T> *pout = out.wr(), *pend = pout + n;
211  while (pout < pend)
212  {
213  // TAOCP
214  float x, y, r2;
215  do
216  {
217  x = 2 * drand48() - 1;
218  y = 2 * drand48() - 1;
219  r2 = x * x + y * y;
220  } while (r2 == 0 || r2 >= 1);
221  float k = sqrtf(-logf(r2) / r2) * stddev;
222  pout->re = k * x;
223  pout->im = k * y;
224  ++pout;
225  }
226  out.written(n);
227  }
228  float stddev;
229 
230  private:
232 };
233 
234 template <typename T>
236 {
238  : runnable(sch, "lowpass"), in(_in), out(_out), w(_w)
239  {
240  }
241 
242  void run()
243  {
244  if (in.readable() < w)
245  return;
246  unsigned long count = min(in.readable() - w, out.writable());
247  T *pin = in.rd(), *pend = pin + count;
248  T *pout = out.wr();
249  float k = 1.0 / w;
250  for (; pin < pend; ++pin, ++pout)
251  {
252  T x = 0.0;
253  for (int i = 0; i < w; ++i)
254  x = x + pin[i];
255  *pout = x * k;
256  }
257  in.read(count);
258  out.written(count);
259  }
260 
261  private:
264  int w;
265 };
266 
267 template <typename T, typename Tc>
269 {
270  fir_filter(scheduler *sch, int _ncoeffs, Tc *_coeffs,
271  pipebuf<T> &_in, pipebuf<T> &_out,
272  unsigned int _decim = 1)
273  : runnable(sch, "fir_filter"),
274  ncoeffs(_ncoeffs), coeffs(_coeffs),
275  in(_in), out(_out),
276  decim(_decim),
277  freq_tap(NULL), tap_multiplier(1), freq_tol(0.1)
278  {
279  shifted_coeffs = new T[ncoeffs];
280  set_freq(0);
281  }
282 
283  void run()
284  {
285  if (in.readable() < ncoeffs)
286  return;
287 
288  if (freq_tap)
289  {
290  float new_freq = *freq_tap * tap_multiplier;
291  if (fabs(current_freq - new_freq) > freq_tol)
292  {
293  if (sch->verbose)
294  fprintf(stderr, "Shifting filter %f -> %f\n",
295  current_freq, new_freq);
296  set_freq(new_freq);
297  }
298  }
299 
300  long count = min((in.readable() - ncoeffs) / decim,
301  out.writable());
302  T *pin = in.rd() + ncoeffs, *pend = pin + count * decim, *pout = out.wr();
303  // TBD use coeffs when current_freq=0 (fewer mults if float)
304  for (; pin < pend; pin += decim, ++pout)
305  {
306  T *pc = shifted_coeffs;
307  T *pi = pin;
308  T x = 0;
309  for (int i = ncoeffs; i--; ++pc, --pi)
310  x = x + (*pc) * (*pi);
311  *pout = x;
312  }
313  in.read(count * decim);
314  out.written(count);
315  }
316 
317  private:
318  int ncoeffs;
319  Tc *coeffs;
322  int decim;
323 
326  void set_freq(float f)
327  {
328  for (int i = 0; i < ncoeffs; ++i)
329  {
330  float a = 2 * M_PI * f * (i - ncoeffs / 2.0);
331  float c = cosf(a), s = sinf(a);
332  // TBD Support T=complex
333  shifted_coeffs[i].re = coeffs[i] * c;
334  shifted_coeffs[i].im = coeffs[i] * s;
335  }
336  current_freq = f;
337  }
338 
339  public:
340  float *freq_tap;
342  float freq_tol;
343 }; // fir_filter
344 
345 // FIR FILTER WITH INTERPOLATION AND DECIMATION
346 
347 template <typename T, typename Tc>
349 {
350  fir_resampler(scheduler *sch, int _ncoeffs, Tc *_coeffs,
351  pipebuf<T> &_in, pipebuf<T> &_out,
352  int _interp = 1, int _decim = 1)
353  : runnable(sch, "fir_resampler"),
354  ncoeffs(_ncoeffs), coeffs(_coeffs),
355  interp(_interp), decim(_decim),
356  in(_in), out(_out, interp),
357  freq_tap(NULL), tap_multiplier(1), freq_tol(0.1)
358  {
359  if (decim != 1)
360  fail("fir_resampler: decim not implemented"); // TBD
361  shifted_coeffs = new T[ncoeffs];
362  set_freq(0);
363  }
364 
365  void run()
366  {
367  if (in.readable() < ncoeffs)
368  return;
369 
370  if (freq_tap)
371  {
372  float new_freq = *freq_tap * tap_multiplier;
373  if (fabs(current_freq - new_freq) > freq_tol)
374  {
375  if (sch->verbose)
376  fprintf(stderr, "Shifting filter %f -> %f\n",
377  current_freq, new_freq);
378  set_freq(new_freq);
379  }
380  }
381 
382  if (in.readable() * interp < ncoeffs)
383  return;
384  unsigned long count = min((in.readable() * interp - ncoeffs) / interp,
385  out.writable() / interp);
386  int latency = (ncoeffs + interp) / interp;
387  T *pin = in.rd() + latency, *pend = pin + count, *pout = out.wr();
388  // TBD use coeffs when current_freq=0 (fewer mults if float)
389  for (; pin < pend; ++pin)
390  {
391  for (int i = 0; i < interp; ++i, ++pout)
392  {
393  T *pi = pin;
394  T *pc = shifted_coeffs + i, *pcend = shifted_coeffs + ncoeffs;
395  T x = 0;
396  for (; pc < pcend; pc += interp, --pi)
397  x = x + (*pc) * (*pi);
398  *pout = x;
399  }
400  }
401  in.read(count);
402  out.written(count * interp);
403  }
404 
405  private:
406  unsigned int ncoeffs;
407  Tc *coeffs;
408  int interp, decim;
411 
412  public:
413  float *freq_tap;
415  float freq_tol;
416 
417  private:
420  void set_freq(float f)
421  {
422  for (int i = 0; i < ncoeffs; ++i)
423  {
424  float a = 2 * M_PI * f * i;
425  float c = cosf(a), s = sinf(a);
426  // TBD Support T=complex
427  shifted_coeffs[i].re = coeffs[i] * c;
428  shifted_coeffs[i].im = coeffs[i] * s;
429  }
430  current_freq = f;
431  }
432 }; // fir_resampler
433 
434 } // namespace leansdr
435 
436 #endif // LEANSDR_DSP_H
float * freq_tap
Definition: dsp.h:340
pipewriter< complex< Tout > > out
Definition: dsp.h:58
pipewriter< complex< T > > out
Definition: dsp.h:231
pipereader< T > in2
Definition: dsp.h:167
cfft_engine(int _n)
Definition: dsp.h:65
float invsqrtn
Definition: dsp.h:139
pipewriter< T > out
Definition: dsp.h:410
pipereader< T > in
Definition: dsp.h:409
pipereader< Tin > in
Definition: dsp.h:194
float stddev
Definition: dsp.h:228
T * shifted_coeffs
Definition: dsp.h:324
pipereader< T > in
Definition: dsp.h:262
void run()
Definition: dsp.h:151
#define M_PI
Definition: rdsdemod.cpp:27
wgn_c(scheduler *sch, pipebuf< complex< T >> &_out)
Definition: dsp.h:203
float tap_multiplier
Definition: dsp.h:341
void set_freq(float f)
Definition: dsp.h:326
complex< T > * omega_rev
Definition: dsp.h:138
float current_freq
Definition: dsp.h:325
scaler(scheduler *sch, Tscale _scale, pipebuf< Tin > &_in, pipebuf< Tout > &_out)
Definition: dsp.h:175
void fail(const char *s)
Definition: framework.cpp:11
int32_t i
Definition: decimators.h:244
pipewriter< Tout > out
Definition: dsp.h:195
naive_lowpass(scheduler *sch, pipebuf< T > &_in, pipebuf< T > &_out, int _w)
Definition: dsp.h:237
Fixed< IntType, IntBits > sqrt(Fixed< IntType, IntBits > const &x)
Definition: fixed.h:2283
pipewriter< T > out
Definition: dsp.h:321
pipewriter< T > out
Definition: dsp.h:168
void set_freq(float f)
Definition: dsp.h:420
void run()
Definition: dsp.h:283
float freq_tol
Definition: dsp.h:342
adder(scheduler *sch, pipebuf< T > &_in1, pipebuf< T > &_in2, pipebuf< T > &_out)
Definition: dsp.h:145
scheduler * sch
Definition: framework.h:199
void run()
Definition: dsp.h:207
void run()
Definition: dsp.h:182
fir_resampler(scheduler *sch, int _ncoeffs, Tc *_coeffs, pipebuf< T > &_in, pipebuf< T > &_out, int _interp=1, int _decim=1)
Definition: dsp.h:350
const int n
Definition: dsp.h:64
Tscale scale
Definition: dsp.h:174
fir_filter(scheduler *sch, int _ncoeffs, Tc *_coeffs, pipebuf< T > &_in, pipebuf< T > &_out, unsigned int _decim=1)
Definition: dsp.h:270
float * freq_tap
Definition: dsp.h:413
pipereader< complex< Tin > > in
Definition: dsp.h:57
float current_freq
Definition: dsp.h:419
pipereader< T > in
Definition: dsp.h:320
float tap_multiplier
Definition: dsp.h:414
cconverter(scheduler *sch, pipebuf< complex< Tin >> &_in, pipebuf< complex< Tout >> &_out)
Definition: dsp.h:36
unsigned int ncoeffs
Definition: dsp.h:406
pipewriter< T > out
Definition: dsp.h:263
void run()
Definition: dsp.h:42
void inplace(complex< T > *data, bool reverse=false)
Definition: dsp.h:89
T min(const T &x, const T &y)
Definition: framework.h:440