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.
inthalfbandfilterst.h
Go to the documentation of this file.
1 // Copyright (C) 2016 F4EXB //
3 // written by Edouard Griffiths //
4 // //
5 // Integer half-band FIR based interpolator and decimator //
6 // This is the even/odd and I/Q stride with double buffering variant //
7 // //
8 // This program is free software; you can redistribute it and/or modify //
9 // it under the terms of the GNU General Public License as published by //
10 // the Free Software Foundation as version 3 of the License, or //
11 // (at your option) any later version. //
12 // //
13 // This program is distributed in the hope that it will be useful, //
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of //
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the //
16 // GNU General Public License V3 for more details. //
17 // //
18 // You should have received a copy of the GNU General Public License //
19 // along with this program. If not, see <http://www.gnu.org/licenses/>. //
21 
22 #ifndef INCLUDE_INTHALFBANDFILTER_ST_H
23 #define INCLUDE_INTHALFBANDFILTER_ST_H
24 
25 #include <stdint.h>
26 #include "dsp/dsptypes.h"
27 #include "dsp/hbfiltertraits.h"
29 #include "export.h"
30 
31 template<uint32_t HBFilterOrder>
32 class SDRANGEL_API IntHalfbandFilterST {
33 public:
35 
36  // downsample by 2, return center part of original spectrum
37  bool workDecimateCenter(Sample* sample)
38  {
39  // insert sample into ring-buffer
40  storeSample((FixReal) sample->real(), (FixReal) sample->imag());
41 
42  switch(m_state)
43  {
44  case 0:
45  // advance write-pointer
46  advancePointer();
47  // next state
48  m_state = 1;
49  // tell caller we don't have a new sample
50  return false;
51 
52  default:
53  // save result
54  doFIR(sample);
55  // advance write-pointer
56  advancePointer();
57  // next state
58  m_state = 0;
59 
60  // tell caller we have a new sample
61  return true;
62  }
63  }
64 
65  // upsample by 2, return center part of original spectrum - double buffer variant
66  bool workInterpolateCenter(Sample* sampleIn, Sample *SampleOut)
67  {
68  switch(m_state)
69  {
70  case 0:
71  // insert sample into ring-buffer
72  storeSample(0, 0);
73  // save result
74  doFIR(SampleOut);
75  // advance write-pointer
76  advancePointer();
77  // next state
78  m_state = 1;
79  // tell caller we didn't consume the sample
80  return false;
81 
82  default:
83  // insert sample into ring-buffer
84  storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag());
85  // save result
86  doFIR(SampleOut);
87  // advance write-pointer
88  advancePointer();
89  // next state
90  m_state = 0;
91  // tell caller we consumed the sample
92  return true;
93  }
94  }
95 
97  {
98  // insert sample into ring-buffer
99  storeSample(*x, *y);
100 
101  switch(m_state)
102  {
103  case 0:
104  // advance write-pointer
105  advancePointer();
106  // next state
107  m_state = 1;
108  // tell caller we don't have a new sample
109  return false;
110 
111  default:
112  // save result
113  doFIR(x, y);
114  // advance write-pointer
115  advancePointer();
116  // next state
117  m_state = 0;
118  // tell caller we have a new sample
119  return true;
120  }
121  }
122 
123  // downsample by 2, return lower half of original spectrum
125  {
126  switch(m_state)
127  {
128  case 0:
129  // insert sample into ring-buffer
130  storeSample((FixReal) -sample->imag(), (FixReal) sample->real());
131  // advance write-pointer
132  advancePointer();
133  // next state
134  m_state = 1;
135  // tell caller we don't have a new sample
136  return false;
137 
138  case 1:
139  // insert sample into ring-buffer
140  storeSample((FixReal) -sample->real(), (FixReal) -sample->imag());
141  // save result
142  doFIR(sample);
143  // advance write-pointer
144  advancePointer();
145  // next state
146  m_state = 2;
147  // tell caller we have a new sample
148  return true;
149 
150  case 2:
151  // insert sample into ring-buffer
152  storeSample((FixReal) sample->imag(), (FixReal) -sample->real());
153  // advance write-pointer
154  advancePointer();
155  // next state
156  m_state = 3;
157  // tell caller we don't have a new sample
158  return false;
159 
160  default:
161  // insert sample into ring-buffer
162  storeSample((FixReal) sample->real(), (FixReal) sample->imag());
163  // save result
164  doFIR(sample);
165  // advance write-pointer
166  advancePointer();
167  // next state
168  m_state = 0;
169  // tell caller we have a new sample
170  return true;
171  }
172  }
173 
174  // upsample by 2, from lower half of original spectrum - double buffer variant
175  bool workInterpolateLowerHalf(Sample* sampleIn, Sample *sampleOut)
176  {
177  Sample s;
178 
179  switch(m_state)
180  {
181  case 0:
182  // insert sample into ring-buffer
183  storeSample(0, 0);
184 
185  // save result
186  doFIR(&s);
187  sampleOut->setReal(s.imag());
188  sampleOut->setImag(-s.real());
189 
190  // advance write-pointer
191  advancePointer();
192 
193  // next state
194  m_state = 1;
195 
196  // tell caller we didn't consume the sample
197  return false;
198 
199  case 1:
200  // insert sample into ring-buffer
201  storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag());
202 
203  // save result
204  doFIR(&s);
205  sampleOut->setReal(-s.real());
206  sampleOut->setImag(-s.imag());
207 
208  // advance write-pointer
209  advancePointer();
210 
211  // next state
212  m_state = 2;
213 
214  // tell caller we consumed the sample
215  return true;
216 
217  case 2:
218  // insert sample into ring-buffer
219  storeSample(0, 0);
220 
221  // save result
222  doFIR(&s);
223  sampleOut->setReal(-s.imag());
224  sampleOut->setImag(s.real());
225 
226  // advance write-pointer
227  advancePointer();
228 
229  // next state
230  m_state = 3;
231 
232  // tell caller we didn't consume the sample
233  return false;
234 
235  default:
236  // insert sample into ring-buffer
237  storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag());
238 
239  // save result
240  doFIR(&s);
241  sampleOut->setReal(s.real());
242  sampleOut->setImag(s.imag());
243 
244  // advance write-pointer
245  advancePointer();
246 
247  // next state
248  m_state = 0;
249 
250  // tell caller we consumed the sample
251  return true;
252  }
253  }
254 
255  // downsample by 2, return upper half of original spectrum
257  {
258  switch(m_state)
259  {
260  case 0:
261  // insert sample into ring-buffer
262  storeSample((FixReal) sample->imag(), (FixReal) -sample->real());
263  // advance write-pointer
264  advancePointer();
265  // next state
266  m_state = 1;
267  // tell caller we don't have a new sample
268  return false;
269 
270  case 1:
271  // insert sample into ring-buffer
272  storeSample((FixReal) -sample->real(), (FixReal) -sample->imag());
273  // save result
274  doFIR(sample);
275  // advance write-pointer
276  advancePointer();
277  // next state
278  m_state = 2;
279  // tell caller we have a new sample
280  return true;
281 
282  case 2:
283  // insert sample into ring-buffer
284  storeSample((FixReal) -sample->imag(), (FixReal) sample->real());
285  // advance write-pointer
286  advancePointer();
287  // next state
288  m_state = 3;
289  // tell caller we don't have a new sample
290  return false;
291 
292  default:
293  // insert sample into ring-buffer
294  storeSample((FixReal) sample->real(), (FixReal) sample->imag());
295  // save result
296  doFIR(sample);
297  // advance write-pointer
298  advancePointer();
299  // next state
300  m_state = 0;
301  // tell caller we have a new sample
302  return true;
303  }
304  }
305 
306  // upsample by 2, move original spectrum to upper half - double buffer variant
307  bool workInterpolateUpperHalf(Sample* sampleIn, Sample *sampleOut)
308  {
309  Sample s;
310 
311  switch(m_state)
312  {
313  case 0:
314  // insert sample into ring-buffer
315  storeSample(0, 0);
316 
317  // save result
318  doFIR(&s);
319  sampleOut->setReal(-s.imag());
320  sampleOut->setImag(s.real());
321 
322  // advance write-pointer
323  advancePointer();
324 
325  // next state
326  m_state = 1;
327 
328  // tell caller we didn't consume the sample
329  return false;
330 
331  case 1:
332  // insert sample into ring-buffer
333  storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag());
334 
335  // save result
336  doFIR(&s);
337  sampleOut->setReal(-s.real());
338  sampleOut->setImag(-s.imag());
339 
340  // advance write-pointer
341  advancePointer();
342 
343  // next state
344  m_state = 2;
345 
346  // tell caller we consumed the sample
347  return true;
348 
349  case 2:
350  // insert sample into ring-buffer
351  storeSample(0, 0);
352 
353  // save result
354  doFIR(&s);
355  sampleOut->setReal(s.imag());
356  sampleOut->setImag(-s.real());
357 
358  // advance write-pointer
359  advancePointer();
360 
361  // next state
362  m_state = 3;
363 
364  // tell caller we didn't consume the sample
365  return false;
366 
367  default:
368  // insert sample into ring-buffer
369  storeSample((FixReal) sampleIn->real(), (FixReal) sampleIn->imag());
370 
371  // save result
372  doFIR(&s);
373  sampleOut->setReal(s.real());
374  sampleOut->setImag(s.imag());
375 
376  // advance write-pointer
377  advancePointer();
378 
379  // next state
380  m_state = 0;
381 
382  // tell caller we consumed the sample
383  return true;
384  }
385  }
386 
387  void myDecimate(const Sample* sample1, Sample* sample2)
388  {
389  storeSample((FixReal) sample1->real(), (FixReal) sample1->imag());
390  advancePointer();
391 
392  storeSample((FixReal) sample2->real(), (FixReal) sample2->imag());
393  doFIR(sample2);
394  advancePointer();
395  }
396 
397  void myDecimate(int32_t x1, int32_t y1, int32_t *x2, int32_t *y2)
398  {
399  storeSample(x1, y1);
400  advancePointer();
401 
402  storeSample(*x2, *y2);
403  doFIR(x2, y2);
404  advancePointer();
405  }
406 
407  void myInterpolate(Sample* sample1, Sample* sample2)
408  {
409  storeSample((FixReal) sample1->real(), (FixReal) sample1->imag());
410  doFIR(sample1);
411  advancePointer();
412 
413  storeSample(0, 0);
414  doFIR(sample2);
415  advancePointer();
416  }
417 
418  void myInterpolate(int32_t *x1, int32_t *y1, int32_t *x2, int32_t *y2)
419  {
420  storeSample(*x1, *y1);
421  doFIR(x1, y1);
422  advancePointer();
423 
424  storeSample(0, 0);
425  doFIR(x2, y2);
426  advancePointer();
427  }
428 
429 protected:
430  int32_t m_samplesDB[2*HBFilterOrder][2]; // double buffer technique with even/odd amnd I/Q stride
431  int32_t m_samplesAligned[HBFilterOrder][2] __attribute__ ((aligned (16)));
432  int m_ptr;
433  int m_size;
434  int m_state;
439 
440 
441  void storeSample(const FixReal& sampleI, const FixReal& sampleQ)
442  {
443  m_samplesDB[m_ptr][0] = sampleI;
444  m_samplesDB[m_ptr][1] = sampleQ;
445  m_samplesDB[m_ptr + m_size][0] = sampleI;
446  m_samplesDB[m_ptr + m_size][1] = sampleQ;
447  }
448 
450  {
451  m_samplesDB[m_ptr][0] = x;
452  m_samplesDB[m_ptr][1] = y;
453  m_samplesDB[m_ptr + m_size][0] = x;
454  m_samplesDB[m_ptr + m_size][1] = y;
455  }
456 
458  {
459  m_ptr = m_ptr + 1 < m_size ? m_ptr + 1: 0;
460  }
461 
462  void doFIR(Sample* sample)
463  {
464  // calculate on odd values
465 
466  if ((m_ptr % 2) == 1)
467  {
468  m_iEvenAcc = 0;
469  m_qEvenAcc = 0;
470  m_iOddAcc = 0;
471  m_qOddAcc = 0;
472 #ifdef USE_SSE4_1
473 // memcpy((void *) m_samplesAligned, (const void *) &(m_samplesDB[ m_ptr + 1][0]), HBFilterOrder*2*sizeof(int32_t));
475  m_ptr + 1,
476  m_samplesDB,
477  m_iEvenAcc,
478  m_qEvenAcc,
479  m_iOddAcc,
480  m_qOddAcc);
481 #else
482  int a = m_ptr + m_size; // tip pointer - odd
483  int b = m_ptr + 1; // tail pointer - aven
484 
485  for (int i = 0; i < HBFIRFilterTraits<HBFilterOrder>::hbOrder / 4; i++)
486  {
487  m_iEvenAcc += (m_samplesDB[a-1][0] + m_samplesDB[b][0]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
488  m_iOddAcc += (m_samplesDB[a][0] + m_samplesDB[b+1][0]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
489  m_qEvenAcc += (m_samplesDB[a-1][1] + m_samplesDB[b][1]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
490  m_qOddAcc += (m_samplesDB[a][1] + m_samplesDB[b+1][1]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
491  a -= 2;
492  b += 2;
493  }
494 #endif
495  m_iEvenAcc += ((int32_t)m_samplesDB[m_ptr + m_size/2][0]) << (HBFIRFilterTraits<HBFilterOrder>::hbShift - 1);
496  m_qEvenAcc += ((int32_t)m_samplesDB[m_ptr + m_size/2][1]) << (HBFIRFilterTraits<HBFilterOrder>::hbShift - 1);
497  m_iOddAcc += ((int32_t)m_samplesDB[m_ptr + m_size/2 + 1][0]) << (HBFIRFilterTraits<HBFilterOrder>::hbShift - 1);
498  m_qOddAcc += ((int32_t)m_samplesDB[m_ptr + m_size/2 + 1][1]) << (HBFIRFilterTraits<HBFilterOrder>::hbShift - 1);
499 
500  sample->setReal(m_iEvenAcc >> HBFIRFilterTraits<HBFilterOrder>::hbShift -1);
501  sample->setImag(m_qEvenAcc >> HBFIRFilterTraits<HBFilterOrder>::hbShift -1);
502  }
503  else
504  {
505  sample->setReal(m_iOddAcc >> HBFIRFilterTraits<HBFilterOrder>::hbShift -1);
506  sample->setImag(m_qOddAcc >> HBFIRFilterTraits<HBFilterOrder>::hbShift -1);
507  }
508  }
509 
510  void doFIR(int32_t *x, int32_t *y)
511  {
512  // calculate on odd values
513 
514  if ((m_ptr % 2) == 1)
515  {
516  m_iEvenAcc = 0;
517  m_qEvenAcc = 0;
518  m_iOddAcc = 0;
519  m_qOddAcc = 0;
520 
521 #ifdef USE_SSE4_1
522 // memcpy((void *) m_samplesAligned, (const void *) &(m_samplesDB[ m_ptr + 1][0]), HBFilterOrder*2*sizeof(int32_t));
524  m_ptr + 1,
525  m_samplesDB,
526  m_iEvenAcc,
527  m_qEvenAcc,
528  m_iOddAcc,
529  m_qOddAcc);
530 #else
531  int a = m_ptr + m_size; // tip pointer - odd
532  int b = m_ptr + 1; // tail pointer - aven
533 
534  for (int i = 0; i < HBFIRFilterTraits<HBFilterOrder>::hbOrder / 4; i++)
535  {
536  m_iEvenAcc += (m_samplesDB[a-1][0] + m_samplesDB[b][0]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
537  m_iOddAcc += (m_samplesDB[a][0] + m_samplesDB[b+1][0]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
538  m_qEvenAcc += (m_samplesDB[a-1][1] + m_samplesDB[b][1]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
539  m_qOddAcc += (m_samplesDB[a][1] + m_samplesDB[b+1][1]) * HBFIRFilterTraits<HBFilterOrder>::hbCoeffs[i];
540  a -= 2;
541  b += 2;
542  }
543 #endif
544  m_iEvenAcc += ((int32_t)m_samplesDB[m_ptr + m_size/2][0]) << (HBFIRFilterTraits<HBFilterOrder>::hbShift - 1);
545  m_qEvenAcc += ((int32_t)m_samplesDB[m_ptr + m_size/2][1]) << (HBFIRFilterTraits<HBFilterOrder>::hbShift - 1);
546  m_iOddAcc += ((int32_t)m_samplesDB[m_ptr + m_size/2 + 1][0]) << (HBFIRFilterTraits<HBFilterOrder>::hbShift - 1);
547  m_qOddAcc += ((int32_t)m_samplesDB[m_ptr + m_size/2 + 1][1]) << (HBFIRFilterTraits<HBFilterOrder>::hbShift - 1);
548 
549  *x = m_iEvenAcc >> HBFIRFilterTraits<HBFilterOrder>::hbShift -1;
550  *y = m_qEvenAcc >> HBFIRFilterTraits<HBFilterOrder>::hbShift -1;
551  }
552  else
553  {
554  *x = m_iOddAcc >> HBFIRFilterTraits<HBFilterOrder>::hbShift -1;
555  *y = m_qOddAcc >> HBFIRFilterTraits<HBFilterOrder>::hbShift -1;
556  }
557  }
558 };
559 
560 template<uint32_t HBFilterOrder>
562 {
564 
565  for (int i = 0; i < m_size; i++)
566  {
567  m_samplesDB[i][0] = 0;
568  m_samplesDB[i][1] = 0;
569  }
570 
571  m_ptr = 0;
572  m_state = 0;
573  m_iEvenAcc = 0;
574  m_qEvenAcc = 0;
575  m_iOddAcc = 0;
576  m_qOddAcc = 0;
577 }
578 
579 #endif // INCLUDE_INTHALFBANDFILTER_DB_H
void doFIR(Sample *sample)
bool workInterpolateUpperHalf(Sample *sampleIn, Sample *sampleOut)
void doFIR(int32_t *x, int32_t *y)
void storeSample(const FixReal &sampleI, const FixReal &sampleQ)
bool workInterpolateLowerHalf(Sample *sampleIn, Sample *sampleOut)
bool workDecimateCenter(Sample *sample)
int32_t i
Definition: decimators.h:244
void setImag(FixReal v)
Definition: dsptypes.h:59
int int32_t
Definition: rtptypes_win.h:45
bool workDecimateUpperHalf(Sample *sample)
typedef __attribute__
void myDecimate(int32_t x1, int32_t y1, int32_t *x2, int32_t *y2)
bool workDecimateLowerHalf(Sample *sample)
bool workDecimateCenter(int32_t *x, int32_t *y)
void setReal(FixReal v)
Definition: dsptypes.h:58
FixReal real() const
Definition: dsptypes.h:61
FixReal imag() const
Definition: dsptypes.h:62
bool workInterpolateCenter(Sample *sampleIn, Sample *SampleOut)
void myInterpolate(Sample *sample1, Sample *sample2)
static void workNA(int ptr, int32_t samples[HBFilterOrder *2][2], int32_t &iEvenAcc, int32_t &qEvenAcc, int32_t &iOddAcc, int32_t &qOddAcc)
void myDecimate(const Sample *sample1, Sample *sample2)
void myInterpolate(int32_t *x1, int32_t *y1, int32_t *x2, int32_t *y2)
void storeSample(int32_t x, int32_t y)
qint16 FixReal
Definition: dsptypes.h:35