1 /*
2 * dsp.cc -- various DSP algorithms
3 *
4 * based on mt63 code by Pawel Jalocha
5 * Copyright (C) 1999-2004 Pawel Jalocha, SP9VRC
6 * Copyright (c) 2007-2011 Dave Freese, W1HKJ
7 *
8 * This file is part of fldigi.
9 *
10 * Fldigi is free software: you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation, either version 3 of the License, or
13 * (at your option) any later version.
14 *
15 * Fldigi is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU General Public License for more details.
19 *
20 * You should have received a copy of the GNU General Public License
21 * along with fldigi. If not, see <http://www.gnu.org/licenses/>.
22 *
23 */
24
25 // Please note, that you should not rely on the correctness
26 // of these routines. They generally work well, but you may find
27 // differences in respect to the mathematical formulas: signs flipped,
28 // orders swapped, etc.
29
30 #include <config.h>
31
32 #include <stdio.h> // only when we do some control printf's
33 #include <stdlib.h>
34 #include <math.h>
35 #include "dsp.h"
36
37 // ----------------------------------------------------------------------------
38
dspPower(double * X,int Len)39 double dspPower(double *X, int Len)
40 {
41 double Sum;
42 for(Sum = 0.0; Len; Len--,X++)
43 Sum += (*X)*(*X);
44 return Sum;
45 }
46
dspPower(double * I,double * Q,int Len)47 double dspPower(double *I, double *Q, int Len)
48 {
49 double Sum;
50 for(Sum = 0.0; Len; Len--,I++,Q++)
51 Sum += (*I)*(*I) + (*Q)*(*Q);
52 return Sum;
53 }
54
dspPower(dspCmpx * X,int Len)55 double dspPower(dspCmpx *X, int Len)
56 {
57 double Sum;
58 for(Sum = 0.0; Len; Len--,X++)
59 Sum += (X->re)*(X->re) + (X->im)*(X->im);
60 return Sum;
61 }
62
63 // ----------------------------------------------------------------------------
64 // dspAverage, extremes, fitting
65
dspAverage(double * Data,int Len)66 double dspAverage(double *Data, int Len)
67 {
68 double Sum; int i;
69 for(Sum = 0.0,i = 0; i < Len; i++) Sum += Data[i];
70 return Sum/Len;
71 }
72
dspCountInRange(double * Data,int Len,double Low,double Upp)73 int dspCountInRange(double *Data, int Len, double Low, double Upp)
74 {
75 int count, i;
76 double D;
77 for(count = i = 0; i<Len; i++) {
78 D = Data[i];
79 count += ((Low<=D)&&(D<=Upp));
80 }
81 return count;
82 }
83
dspFindMaxdspPower(dspCmpx * Data,int Len)84 double dspFindMaxdspPower(dspCmpx *Data, int Len)
85 {
86 double Max, Pwr;
87 int i;
88 Max = dspPower(Data[0]);
89 for(i = 1; i < Len; i++) {
90 Pwr = dspPower(Data[i]);
91 if (Pwr > Max) Max = Pwr;
92 }
93 return Max;
94 }
95
dspFindMaxdspPower(dspCmpx * Data,int Len,int & MaxPos)96 double dspFindMaxdspPower(dspCmpx *Data, int Len, int &MaxPos)
97 {
98 double Max, Pwr;
99 int i, pos;
100 Max = dspPower(Data[0]);
101 pos = 0;
102 for (i = 1; i < Len; i++) {
103 Pwr = dspPower(Data[i]);
104 if (Pwr > Max) {
105 Max = Pwr;
106 pos = i;
107 }
108 }
109 MaxPos = pos;
110 return Max;
111 }
112
dspFitPoly1(double * Data,int Len,double & A,double & B)113 double dspFitPoly1(double *Data, int Len, double &A, double &B)
114 {
115 double Sum;
116 int i;
117 A = (Data[Len-1] - Data[0])/(Len - 1);
118 for (Sum = 0.0,i = 0; i < Len; i++)
119 Sum += Data[i] - A*i;
120 B = Sum/Len;
121 for (Sum = 0.0, i = 0; i < Len; i++)
122 Sum += dspPower(Data[i] - (A*i + B));
123 return sqrt(Sum/Len);
124 }
125
dspFitPoly2(double * Data,int Len,double & A,double & B,double & C)126 double dspFitPoly2(double *Data, int Len, double &A, double &B, double &C)
127 {
128 double Sum;
129 int i;
130 A = ((Data[Len - 1] - Data[Len - 2]) - (Data[1] - Data[0]))/(Len - 2)/2;
131 B = (Data[Len - 1] - A*(Len - 1)*(Len - 1) - Data[0])/(Len - 1);
132 for (Sum = 0.0, i = 0; i < Len; i++)
133 Sum += Data[i] - (A*i*i + B*i);
134 C = Sum/Len;
135 for (Sum = 0.0, i = 0; i < Len; i++)
136 Sum += dspPower(Data[i] - (A*i*i + B*i + C));
137 return sqrt(Sum/Len);
138 }
139
dspFitPoly2(double Data[3],double & A,double & B,double & C)140 void dspFitPoly2(double Data[3], double &A, double &B, double &C)
141 {
142 C = Data[0];
143 A = (Data[0]- 2*Data[1] + Data[2])/2;
144 B = (Data[1] - Data[0]) - A;
145 }
146
147 // ----------------------------------------------------------------------------
148 // various window shapes (for the FFT and FIR filters)
149 // these functions are supposed to be called with the argument "dspPhase"
150 // between -PI and +PI. Most (or even all) will return zero for input
151 // euqal -PI or +PI.
152
WindowHamming(double dspPhase)153 double WindowHamming(double dspPhase)
154 {
155 return cos(dspPhase/2);
156 } // not exactly ...
157
dspWindowHanning(double dspPhase)158 double dspWindowHanning(double dspPhase)
159 {
160 return (1.0 + cos(dspPhase))/2;
161 }
162
WindowBlackman2(double dspPhase)163 double WindowBlackman2(double dspPhase) // from Freq 5.1 FFT analyzer
164 {
165 return 0.42 + 0.5*cos(dspPhase) + 0.08*cos(2*dspPhase);
166 }
167
dspWindowBlackman3(double dspPhase)168 double dspWindowBlackman3(double dspPhase) // from the Motorola BBS
169 {
170 return 0.35875 + 0.48829*cos(dspPhase) + 0.14128*cos(2*dspPhase) + 0.01168*cos(3*dspPhase);
171 }
172
173 // ----------------------------------------------------------------------------
174 // FIR shape calculation for a flat response from FreqLow to FreqUpp
175
dspWinFirI(double LowOmega,double UppOmega,double * Shape,int Len,double (* Window)(double),double shift)176 void dspWinFirI(
177 double LowOmega, double UppOmega,
178 double *Shape, int Len, double (*Window)(double), double shift)
179 {
180 int i;
181 double time, dspPhase, shape;
182 // printf("dspWinFirI: %5.3f %5.3f %d\n",LowOmega,UppOmega,Len);
183 for (i = 0; i < Len; i++) {
184 time = i + (1.0 - shift) - (double)Len/2;
185 dspPhase = 2*M_PI*time/Len;
186 if (time == 0)
187 shape = UppOmega - LowOmega;
188 else
189 shape = (sin(UppOmega*time) - sin(LowOmega*time))/time;
190 // printf("%2d %5.1f %5.2f %7.4f %7.4f\n",i,time,dspPhase,shape,(*Window)(dspPhase));
191 Shape[i] = shape*(*Window)(dspPhase)/M_PI; }
192 }
193
WinFirQ(double LowOmega,double UppOmega,double * Shape,int Len,double (* Window)(double),double shift)194 void WinFirQ(
195 double LowOmega, double UppOmega,
196 double *Shape, int Len, double (*Window)(double), double shift)
197 {
198 int i;
199 double time, dspPhase, shape;
200 // printf("WinFirQ: %5.3f %5.3f %d\n",LowOmega,UppOmega,Len);
201 for (i = 0; i < Len; i++) {
202 time = i + (1.0 - shift) - (double)Len/2;
203 dspPhase = 2*M_PI*time/Len;
204 if (time == 0)
205 shape=0.0;
206 else
207 shape = (-cos(UppOmega*time) + cos(LowOmega*time))/time;
208 // printf("%2d %5.1f %5.2f %7.4f %7.4f\n",i,time,dspPhase,shape,(*Window)(dspPhase));
209 Shape[i] = (-shape)*(*Window)(dspPhase)/M_PI;
210 }
211 } // we put minus for the Q-part because the FIR shapes must be placed
212 // in reverse order for simpler indexing
213
214 // ----------------------------------------------------------------------------
215 // convert 16-bit signed or 8-bit unsigned into doubles
216
dspConvS16todouble(dspS16 * dspS16,double * dble,int Len,double Gain)217 void dspConvS16todouble(dspS16 *dspS16, double *dble, int Len, double Gain)
218 {
219 for (; Len; Len--)
220 (*dble++) = (*dspS16++)*Gain;
221 }
222
dspConvS16todouble(short int * dspS16,double_buff * dble,int Len,double Gain)223 int dspConvS16todouble(short int *dspS16, double_buff *dble, int Len, double Gain)
224 {
225 int err = dble->EnsureSpace(Len);
226 if (err) return -1;
227 dspConvS16todouble(dspS16, dble->Data, Len, Gain);
228 dble->Len = Len;
229 return 0;
230 }
231
dspConvdoubleTodspS16(double * dble,dspS16 * dspS16,int Len,double Gain)232 void dspConvdoubleTodspS16(double *dble, dspS16 *dspS16, int Len, double Gain)
233 {
234 double out;
235 for (; Len; Len--) {
236 out = (*dble++)*Gain;
237 if (out > 32767.0)
238 out = 32767.0;
239 else if (out < (-32767.0))
240 out = (-32767.0);
241 (*dspS16++) = (short int)floor(out+0.5);
242 }
243 } // we could count the over/underflows ?
244
dspConvU8todouble(unsigned char * U8,double * dble,int Len,double Gain)245 void dspConvU8todouble(unsigned char *U8, double *dble, int Len, double Gain)
246 {
247 for (; Len; Len--)
248 (*dble++) = ((int)(*U8++) - 128)*Gain;
249 }
250
dspConvU8todouble(unsigned char * U8,double_buff * dble,int Len,double Gain)251 int dspConvU8todouble(unsigned char *U8, double_buff *dble, int Len, double Gain)
252 {
253 int err = dble->EnsureSpace(Len);
254 if (err) return -1;
255 dspConvU8todouble(U8, dble->Data, Len, Gain);
256 dble->Len = Len;
257 return 0;
258 }
259
260 // ----------------------------------------------------------------------------
261 // other converts
262
dspConvCmpxTodspPower(dspCmpx * Inp,int Len,double * Out)263 void dspConvCmpxTodspPower(dspCmpx *Inp, int Len, double *Out)
264 {
265 for (; Len; Len--)
266 (*Out++) = dspPower(*Inp++);
267 }
268
dspConvCmpxTodspPower(dspCmpx_buff * Input,double_buff * Output)269 int dspConvCmpxTodspPower(dspCmpx_buff *Input, double_buff *Output)
270 {
271 int err = Output->EnsureSpace(Input->Len);
272 if (err) return err;
273 dspConvCmpxTodspPower(Input->Data, Input->Len, Output->Data);
274 Output->Len = Input->Len;
275 return 0;
276 }
277
dspConvCmpxTodspAmpl(dspCmpx * Inp,int Len,double * Out)278 void dspConvCmpxTodspAmpl(dspCmpx *Inp, int Len, double *Out)
279 {
280 for (; Len; Len--)
281 (*Out++) = sqrt(dspPower(*Inp++));
282 }
283
dspConvCmpxTodspAmpl(dspCmpx_buff * Input,double_buff * Output)284 int dspConvCmpxTodspAmpl(dspCmpx_buff *Input, double_buff *Output)
285 {
286 int err = Output->EnsureSpace(Input->Len);
287 if(err) return err;
288 dspConvCmpxTodspAmpl(Input->Data, Input->Len, Output->Data);
289 Output->Len = Input->Len;
290 return 0;
291 }
292
dspConvCmpxTodspPhase(dspCmpx * Inp,int Len,double * Out)293 void dspConvCmpxTodspPhase(dspCmpx *Inp, int Len, double *Out)
294 {
295 for (; Len; Len--)
296 (*Out++) = dspPhase(*Inp++);
297 }
298
dspConvCmpxTodspPhase(dspCmpx_buff * Input,double_buff * Output)299 int dspConvCmpxTodspPhase(dspCmpx_buff *Input, double_buff *Output)
300 {
301 int err = Output->EnsureSpace(Input->Len);
302 if(err) return err;
303 dspConvCmpxTodspPhase(Input->Data, Input->Len, Output->Data);
304 Output->Len = Input->Len;
305 return 0;
306 }
307
308 // ----------------------------------------------------------------------------
309 // Pulse noise limiter
310
dspPulseLimiter()311 dspPulseLimiter::dspPulseLimiter()
312 {
313 Tap = NULL;
314 }
315
~dspPulseLimiter()316 dspPulseLimiter::~dspPulseLimiter()
317 {
318 free(Tap);
319 }
320
Free(void)321 void dspPulseLimiter::Free(void)
322 {
323 free(Tap);
324 Tap = NULL;
325 }
326
Preset(int TapLen,double LimitThres)327 int dspPulseLimiter::Preset(int TapLen, double LimitThres)
328 {
329 Len = TapLen;
330 Thres = LimitThres*LimitThres;
331 if (dspRedspAllocArray(&Tap, Len)) return -1;
332 dspClearArray(Tap, Len);
333 Ptr = 0;
334 PwrSum = 0.0;
335 return 0;
336 }
337
Process(double * Inp,int InpLen,double * Out)338 int dspPulseLimiter::Process(double *Inp, int InpLen, double *Out)
339 {
340 int i, o;
341 double Lim;
342 for (i = 0; i < InpLen; i++) {
343 PwrSum -= Tap[Ptr]*Tap[Ptr];
344 Tap[Ptr++] = Inp[i];
345 PwrSum += Inp[i]*Inp[i];
346 if (Ptr >= Len) Ptr -= Len;
347 o = Ptr + (Len/2);
348 if (o >= Len) o -= Len;
349 Lim = Thres*PwrSum/Len;
350 if (Tap[o]*Tap[o] <= Lim)
351 Out[i] = Tap[o];
352 else {
353 if (Tap[o] > 0.0)
354 Out[i] = sqrt(Lim);
355 else
356 Out[i] = (-sqrt(Lim));
357 }
358 }
359 for (PwrSum = 0.0, i = 0; i < Len; i++)
360 PwrSum += Tap[i]*Tap[i];
361 dspRMS = sqrt(PwrSum/Len);
362 return 0;
363 }
364
Process(double_buff * Input)365 int dspPulseLimiter::Process(double_buff *Input)
366 {
367 int err = Output.EnsureSpace(Input->Len);
368 if (err) return -1;
369 Process(Input->Data, Input->Len, Output.Data);
370 Output.Len = Input->Len;
371 return 0;
372 }
373
374 // ----------------------------------------------------------------------------
375 // Signal level monitor
376
dspLevelMonitor()377 dspLevelMonitor::dspLevelMonitor()
378 { }
379
~dspLevelMonitor()380 dspLevelMonitor::~dspLevelMonitor()
381 { }
382
Preset(double Integ,double Range)383 int dspLevelMonitor::Preset(double Integ, double Range)
384 {
385 dspLowPass2Coeff(Integ, W1, W2, W5);
386 MaxSqr = Range*Range;
387 PwrMid = 0.0;
388 PwrOut = 0.0;
389 dspRMS = 0.0;
390 OutOfRangeMid = 0.0;
391 OutOfRange = 0.0;
392 return 0;
393 }
394
Process(double * Inp,int Len)395 int dspLevelMonitor::Process(double *Inp, int Len)
396 {
397 int i, Out;
398 double Sqr, Sum;
399 if (Len <= 0) return 0;
400 for (Sum = 0.0, Out = 0,i = 0; i < Len; i++) {
401 Sum += Sqr = Inp[i]*Inp[i];
402 Out += (Sqr > MaxSqr);
403 }
404 dspLowPass2(Sum/Len, PwrMid, PwrOut, W1, W2, W5);
405 dspLowPass2((double)Out/Len, OutOfRangeMid, OutOfRange, W1, W2, W5);
406 if (OutOfRange < 0.0)
407 OutOfRange = 0.0;
408 if (PwrOut <= 0.0)
409 dspRMS = 0.0;
410 else
411 dspRMS = sqrt(PwrOut);
412 return 0;
413 }
414
Process(double_buff * Input)415 int dspLevelMonitor::Process(double_buff *Input)
416 { return Process(Input->Data,Input->Len); }
417
418 // ----------------------------------------------------------------------------
419 // Automatic Gain/Level Control for the Mixer
420
dspMixerAutoLevel()421 dspMixerAutoLevel::dspMixerAutoLevel()
422 {
423 MinMS = 0.01;
424 MaxMS = 0.05;
425 IntegLen = 8000;
426 PeakHold = 4000;
427 MinHold = 800;
428 MinLevel = 0;
429 MaxLevel = 100;
430 AdjStep = 1;
431 Level = 75;
432 Hold = (-IntegLen);
433 AvedspRMS = 0.0;
434 }
435
Process(double * Inp,int InpLen)436 int dspMixerAutoLevel::Process(double *Inp, int InpLen)
437 {
438 double MS = dspPower(Inp, InpLen) / IntegLen;
439 double W = 1.0 - ((double)InpLen) / IntegLen;
440
441 AvedspRMS = AvedspRMS*W + MS;
442 Hold += InpLen;
443 if (Hold < MinHold) return 0;
444
445 if(AvedspRMS>MaxMS) {
446 Level -= AdjStep;
447 if (Level < MinLevel)
448 Level = MinLevel;
449 Hold=0;
450 return 1;
451 }
452
453 if (Hold < PeakHold) return 0;
454
455 if (AvedspRMS < MinMS) {
456 Level += AdjStep;
457 if (Level > MaxLevel)
458 Level = MaxLevel;
459 Hold = 0;
460 return 1;
461 }
462
463 return 0;
464 }
465
466 // ----------------------------------------------------------------------------
467
dspLowPass2(dspCmpx * Inp,dspCmpx * Mid,dspCmpx * Out,double W1,double W2,double W5)468 void dspLowPass2(dspCmpx *Inp, dspCmpx *Mid, dspCmpx *Out, double W1, double W2, double W5)
469 {
470 double Sum, Diff;
471 // printf("\n[dspLowPass2] %6.3f %6.3f %6.3f",Inp->re,Mid->re,Out->re);
472 Sum = Mid->re + Out->re;
473 Diff = Mid->re - Out->re;
474 Mid->re += W2*Inp->re - W1*Sum;
475 Out->re += W5*Diff;
476 // printf(" => %6.3f %6.3f\n",Mid->re,Out->re);
477 Sum = Mid->im + Out->im;
478 Diff = Mid->im - Out->im;
479 Mid->im += W2*Inp->im - W1*Sum;
480 Out->im += W5*Diff;
481 }
482
483 // ----------------------------------------------------------------------------
484 // periodic low pass
485
dspPeriodLowPass2()486 dspPeriodLowPass2::dspPeriodLowPass2()
487 {
488 TapMid = NULL;
489 TapOut = NULL;
490 }
491
~dspPeriodLowPass2()492 dspPeriodLowPass2::~dspPeriodLowPass2()
493 {
494 free(TapMid);
495 free(TapOut);
496 Output.Free();
497 }
498
Free(void)499 void dspPeriodLowPass2::Free(void)
500 {
501 free(TapMid);
502 TapMid = NULL;
503 free(TapOut);
504 TapOut = NULL;
505 }
506
Preset(int Period,double IntegLen)507 int dspPeriodLowPass2::Preset(int Period, double IntegLen)
508 {
509 int i;
510 Len = Period;
511 if (dspRedspAllocArray(&TapMid, Len)) goto Error;
512 if (dspRedspAllocArray(&TapOut, Len)) goto Error;
513 for (i = 0; i < Len; i++) {
514 TapMid[i] = 0.0;
515 TapOut[i] = 0.0;
516 }
517 TapPtr = 0;
518 dspLowPass2Coeff(IntegLen, W1, W2, W5);
519 return 0;
520 Error:
521 Free();
522 return -1;
523 }
524
Process(double Inp,double & Out)525 int dspPeriodLowPass2::Process(double Inp, double &Out)
526 {
527 dspLowPass2(Inp, TapMid[TapPtr], TapOut[TapPtr], W1, W2, W5);
528 Out = TapOut[TapPtr++];
529 if(TapPtr >= Len)
530 TapPtr = 0;
531 return 0;
532 }
533
Process(double * Inp,int InpLen,double * Out)534 int dspPeriodLowPass2::Process(double *Inp, int InpLen, double *Out)
535 {
536 int i, batch;
537 for (i = 0; i < InpLen; ) {
538 for (batch = dspIntmin(InpLen-i, Len - TapPtr), i += batch; batch; batch--) {
539 dspLowPass2(*Inp++, TapMid[TapPtr], TapOut[TapPtr], W1, W2, W5);
540 (*Out++) = TapOut[TapPtr++];
541 }
542 if (TapPtr >= Len)
543 TapPtr = 0;
544 }
545 return 0;
546 }
547
Process(double_buff * Input)548 int dspPeriodLowPass2::Process(double_buff *Input)
549 {
550 int err = Output.EnsureSpace(Input->Len);
551 if (err) return -1;
552 Process(Input->Data, Input->Len, Output.Data);
553 Output.Len = Input->Len;
554 return 0;
555 }
556
557 // ----------------------------------------------------------------------------
558 // Low pass "moving box" FIR filter
559 // very unpure spectral response but complexity very low
560 // and independent on the integration time
561
dspBoxFilter()562 dspBoxFilter::dspBoxFilter()
563 {
564 Tap = NULL;
565 }
566
~dspBoxFilter()567 dspBoxFilter::~dspBoxFilter()
568 {
569 free(Tap);
570 }
571
Free(void)572 void dspBoxFilter::Free(void)
573 {
574 free(Tap);
575 Tap = NULL;
576 Output.Free();
577 }
578
Preset(int BoxLen)579 int dspBoxFilter::Preset(int BoxLen)
580 {
581 int i;
582 if (dspRedspAllocArray(&Tap, BoxLen)) return -1;
583 for (i = 0; i < BoxLen; i++)
584 Tap[i] = 0;
585 Len = BoxLen;
586 TapPtr = 0;
587 Sum = 0;
588 return 0;
589 }
590
Process(double * Inp,int InpLen,double * Out)591 int dspBoxFilter::Process(double *Inp, int InpLen, double *Out)
592 {
593 int i, batch;
594 for (i = 0; i < InpLen; ) {
595 for (batch = dspIntmin(InpLen-i, Len - TapPtr), i += batch; batch; batch--) {
596 Sum -= Tap[TapPtr];
597 Out[i] = (Sum += Tap[TapPtr++] = Inp[i]);
598 }
599 if (TapPtr >= Len)
600 TapPtr = 0;
601 }
602 for (Sum = 0, i = 0; i < Len; i++)
603 Sum += Tap[i];
604 return InpLen;
605 }
606
Recalibrate()607 void dspBoxFilter::Recalibrate()
608 {
609 int i;
610 for (Sum = 0, i = 0; i < Len; i++)
611 Sum += Tap[i];
612 }
613
Process(double_buff * Input)614 int dspBoxFilter::Process(double_buff *Input)
615 {
616 int err = Output.EnsureSpace(Input->Len);
617 if (err) return err;
618 Process(Input->Data, Input->Len, Output.Data);
619 Output.Len = Input->Len;
620 return 0;
621 }
622
dspCmpxBoxFilter()623 dspCmpxBoxFilter::dspCmpxBoxFilter()
624 {
625 Tap = NULL;
626 }
627
~dspCmpxBoxFilter()628 dspCmpxBoxFilter::~dspCmpxBoxFilter()
629 {
630 free(Tap);
631 }
632
Free(void)633 void dspCmpxBoxFilter::Free(void)
634 {
635 free(Tap);
636 Tap = NULL;
637 Output.Free();
638 }
639
Preset(int BoxLen)640 int dspCmpxBoxFilter::Preset(int BoxLen)
641 {
642 int i;
643 if (dspRedspAllocArray(&Tap, BoxLen)) return -1;
644 for (i = 0; i < BoxLen; i++)
645 Tap[i].re = Tap[i].im = 0.0;
646 Len = BoxLen;
647 TapPtr = 0;
648 Sum.re = 0.0;
649 Sum.im = 0.0;
650 return 0;
651 }
652
Process(dspCmpx * Inp,int InpLen,dspCmpx * Out)653 int dspCmpxBoxFilter::Process(dspCmpx *Inp, int InpLen, dspCmpx *Out)
654 {
655 int i, batch;
656 for (i = 0; i < InpLen; ) {
657 for (batch = dspIntmin(InpLen-i, Len - TapPtr), i += batch; batch; batch--) {
658 Sum.re -= Tap[TapPtr].re;
659 Sum.im -= Tap[TapPtr].im;
660 Tap[TapPtr] = Inp[i];
661 Sum.re += Inp[i].re;
662 Sum.im += Inp[i].im;
663 Out[i].re = Sum.re;
664 Out[i].im = Sum.im;
665 }
666 if (TapPtr >= Len)
667 TapPtr = 0;
668 }
669 for (Sum.re = Sum.im = 0.0, i = 0; i < Len; i++) {
670 Sum.re += Tap[i].re;
671 Sum.im += Tap[i].im;
672 }
673 return InpLen;
674 }
675
Recalibrate()676 void dspCmpxBoxFilter::Recalibrate()
677 {
678 int i;
679 for (Sum.re = Sum.im = 0.0, i = 0; i < Len; i++) {
680 Sum.re += Tap[i].re;
681 Sum.im += Tap[i].im;
682 }
683 }
684
Process(dspCmpx_buff * Input)685 int dspCmpxBoxFilter::Process(dspCmpx_buff *Input)
686 {
687 int err = Output.EnsureSpace(Input->Len);
688 if(err) return err;
689 Process(Input->Data, Input->Len, Output.Data);
690 Output.Len = Input->Len;
691 return 0;
692 }
693
694 // ----------------------------------------------------------------------------
695 // FIR filter with a given shape
696
dspFirFilter()697 dspFirFilter::dspFirFilter()
698 {
699 Tap = NULL;
700 ExternShape = 1;
701 }
702
~dspFirFilter()703 dspFirFilter::~dspFirFilter()
704 {
705 free(Tap);
706 if (!ExternShape)
707 free(Shape);
708 }
709
Free(void)710 void dspFirFilter::Free(void)
711 {
712 free(Tap);
713 Tap = NULL;
714 if (!ExternShape)
715 free(Shape);
716 Shape = NULL;
717 Output.Free();
718 }
719
Preset(int FilterLen,double * FilterShape)720 int dspFirFilter::Preset(int FilterLen, double *FilterShape)
721 {
722 int i;
723 if (dspRedspAllocArray(&Tap, FilterLen)) return -1;
724 for (i = 0; i < FilterLen; i++)
725 Tap[i] = 0;
726 Len = FilterLen;
727 TapPtr = 0;
728 if (!ExternShape)
729 free(Shape);
730 Shape = FilterShape;
731 return 0;
732 }
733
ComputeShape(double LowOmega,double UppOmega,double (* Window)(double))734 int dspFirFilter::ComputeShape(
735 double LowOmega, double UppOmega,
736 double (*Window)(double))
737 {
738 if (ExternShape) {
739 Shape = NULL;
740 ExternShape = 0;
741 }
742 if (dspRedspAllocArray(&Shape, Len)) return -1;
743 dspWinFirI(LowOmega, UppOmega, Shape, Len, Window);
744 return 0;
745 }
746
Process(double * Inp,int InpLen,double * Out)747 int dspFirFilter::Process(double *Inp, int InpLen, double *Out)
748 {
749 int i, s, t;
750 double Sum;
751 if(InpLen<Len) {
752 for (i = 0; i < InpLen; i++) {
753 for (Sum = 0.0, t = 0, s = i; s < Len; s++)
754 Sum += Tap[s]*Shape[t++];
755 for (s -= Len; t < Len; s++)
756 Sum += Inp[s]*Shape[t++];
757 Out[i] = Sum;
758 }
759 memmove(Tap, Tap + InpLen, (Len - InpLen)*sizeof(double));
760 memcpy(Tap + (Len - InpLen), Inp, InpLen*sizeof(double));
761 } else {
762 for (i = 0; i < Len; i++) {
763 for (Sum = 0.0, t = 0, s = i; s < Len; s++)
764 Sum += Tap[s]*Shape[t++];
765 for (s -= Len; t < Len; s++)
766 Sum += Inp[s]*Shape[t++];
767 Out[i] = Sum;
768 }
769 for (; i < InpLen; i++) {
770 for (Sum = 0.0, t = 0, s = i - Len; t < Len; s++)
771 Sum += Inp[s]*Shape[t++];
772 Out[i] = Sum;
773 }
774 memcpy(Tap, Inp + (InpLen - Len), Len*sizeof(double));
775 }
776 return InpLen;
777 }
778
Process(double_buff * Input)779 int dspFirFilter::Process(double_buff *Input)
780 {
781 int err = Output.EnsureSpace(Input->Len);
782 if(err) return err;
783 Process(Input->Data, Input->Len, Output.Data);
784 Output.Len = Input->Len;
785 return 0;
786 }
787
788 // ----------------------------------------------------------------------------
789 // a pair of FIR filters. for quadrature split & decimate
790 // the decimation rate must be an integer
791
dspQuadrSplit()792 dspQuadrSplit::dspQuadrSplit()
793 {
794 ExternShape = 1;
795 }
796
~dspQuadrSplit()797 dspQuadrSplit::~dspQuadrSplit()
798 {
799 if (!ExternShape) {
800 free(ShapeI);
801 free(ShapeQ);
802 }
803 }
804
Free(void)805 void dspQuadrSplit::Free(void)
806 {
807 Tap.Free();
808 if (!ExternShape) {
809 free(ShapeI);
810 free(ShapeQ);
811 }
812 ShapeI = NULL;
813 ShapeQ = NULL;
814 Output.Free();
815 }
816
Preset(int FilterLen,double * FilterShape_I,double * FilterShape_Q,int DecimateRate)817 int dspQuadrSplit::Preset(
818 int FilterLen,
819 double *FilterShape_I, double *FilterShape_Q,
820 int DecimateRate)
821 {
822 Len = FilterLen;
823 if (!ExternShape) {
824 free(ShapeI);
825 free(ShapeQ);
826 }
827 ShapeI = FilterShape_I;
828 ShapeQ = FilterShape_Q;
829 ExternShape = 1;
830 Tap.EnsureSpace(Len);
831 Tap.Len = Len;
832 dspClearArray(Tap.Data, Tap.Len);
833 Rate = DecimateRate;
834 return 0;
835 }
836
ComputeShape(double LowOmega,double UppOmega,double (* Window)(double))837 int dspQuadrSplit::ComputeShape(double LowOmega,double UppOmega,
838 double (*Window)(double))
839 {
840 if (ExternShape) {
841 ShapeI = NULL;
842 ShapeQ = NULL;
843 ExternShape = 0;
844 }
845 if (dspRedspAllocArray(&ShapeI, Len)) return -1;
846 if (dspRedspAllocArray(&ShapeQ, Len)) return -1;
847 dspWinFirI(LowOmega, UppOmega, ShapeI, Len, Window);
848 WinFirQ(LowOmega, UppOmega, ShapeQ, Len, Window);
849 return 0;
850 }
851
Process(double_buff * Input)852 int dspQuadrSplit::Process(double_buff *Input)
853 {
854 int err, i, s, t, o, l;
855 double SumI, SumQ;
856 double *Inp;
857 dspCmpx *Out;
858 int InpLen;
859
860 InpLen = Input->Len;
861 err = Tap.EnsureSpace(Tap.Len + InpLen);
862 if (err) return err;
863 dspCopyArray(Tap.Data+Tap.Len, Input->Data, InpLen);
864 // printf("dspQuadrSplit: InpLen=%d, Tap.Len=%d",InpLen,Tap.Len);
865 Tap.Len += InpLen;
866 Inp = Tap.Data;
867 // printf(" -> %d",Tap.Len);
868 err = Output.EnsureSpace( InpLen / Rate + 2);
869 if (err) return err;
870 Out = Output.Data;
871 for (l = Tap.Len-Len,o = 0, i = 0; i < l; i += Rate) {
872 for (SumI = SumQ = 0.0, s = i,t = 0; t < Len; t++,s++) {
873 SumI += Inp[s] * ShapeI[t];
874 SumQ += Inp[s] * ShapeQ[t];
875 }
876 Out[o].re=SumI;
877 Out[o++].im=SumQ;
878 }
879 Tap.Len -= i;
880 dspMoveArray(Tap.Data,Tap.Data+i,Tap.Len);
881 Output.Len = o;
882 // printf(" => Tap.Len=%d\n",Tap.Len);
883 return 0;
884 }
885
886 // ----------------------------------------------------------------------------
887 // reverse of dspQuadrSplit: interpolates and combines the I/Q
888 // back into 'real' signal.
889
dspQuadrComb()890 dspQuadrComb::dspQuadrComb()
891 {
892 Tap = NULL;
893 ExternShape = 1;
894 }
895
~dspQuadrComb()896 dspQuadrComb::~dspQuadrComb()
897 {
898 free(Tap);
899 if (!ExternShape) {
900 free(ShapeI);
901 free(ShapeQ);
902 }
903 }
904
Free(void)905 void dspQuadrComb::Free(void)
906 {
907 free(Tap);
908 Tap = NULL;
909 if (!ExternShape) {
910 free(ShapeI);
911 free(ShapeQ);
912 }
913 ShapeI = NULL;
914 ShapeQ = NULL;
915 Output.Free();
916 }
917
Preset(int FilterLen,double * FilterShape_I,double * FilterShape_Q,int DecimateRate)918 int dspQuadrComb::Preset(
919 int FilterLen,
920 double *FilterShape_I, double *FilterShape_Q,
921 int DecimateRate)
922 {
923 int i;
924 Len = FilterLen;
925 if (dspRedspAllocArray(&Tap, Len)) return -1;
926 if (!ExternShape) {
927 free(ShapeI);
928 free(ShapeQ);
929 }
930 ShapeI = FilterShape_I;
931 ShapeQ = FilterShape_Q;
932 ExternShape = 1;
933 for (i = 0; i < FilterLen; i++)
934 Tap[i] = 0.0;
935 TapPtr = 0;
936 Rate = DecimateRate;
937 return 0;
938 }
939
ComputeShape(double LowOmega,double UppOmega,double (* Window)(double))940 int dspQuadrComb::ComputeShape(
941 double LowOmega,double UppOmega,
942 double (*Window)(double))
943 {
944 if (ExternShape) {
945 ShapeI = NULL;
946 ShapeQ = NULL;
947 ExternShape = 0;
948 }
949 if (dspRedspAllocArray(&ShapeI, Len)) return -1;
950 if (dspRedspAllocArray(&ShapeQ, Len)) return -1;
951
952 dspWinFirI(LowOmega, UppOmega, ShapeI, Len, Window);
953 WinFirQ(LowOmega, UppOmega, ShapeQ, Len, Window);
954 return 0;
955 }
956
Process(dspCmpx_buff * Input)957 int dspQuadrComb::Process(dspCmpx_buff *Input)
958 {
959 int err, i, o, r, t, len;
960 dspCmpx *Inp;
961 double *Out;
962 int InpLen;
963 double I, Q;
964 InpLen = Input->Len;
965 err = Output.EnsureSpace(InpLen*Rate);
966 if (err) return err;
967 Inp = Input->Data;
968 Out = Output.Data;
969 Output.Len = InpLen*Rate;
970 for(o=0,i=0; i<InpLen; i++) {
971 I = Inp[i].re;
972 Q = Inp[i].im;
973 for (r = 0,t = TapPtr; t < Len; t++,r++)
974 Tap[t] += I*ShapeI[r] + Q*ShapeQ[r];
975 for (t = 0; t < TapPtr; t++, r++)
976 Tap[t] += I*ShapeI[r] + Q*ShapeQ[r];
977 len = Len - TapPtr;
978 if (len < Rate) {
979 for (r = 0; r < len; r++) {
980 Out[o++] = Tap[TapPtr];
981 Tap[TapPtr++] = 0.0;
982 }
983 TapPtr = 0;
984 for ( ; r<Rate; r++) {
985 Out[o++] = Tap[TapPtr];
986 Tap[TapPtr++] = 0.0;
987 }
988 } else {
989 for (r = 0; r < Rate; r++) {
990 Out[o++] = Tap[TapPtr];
991 Tap[TapPtr++] = 0.0;
992 }
993 }
994 }
995 return 0;
996 }
997
998 // ----------------------------------------------------------------------------
999 // complex mix with an oscilator (carrier)
1000
dspCmpxMixer()1001 dspCmpxMixer::dspCmpxMixer()
1002 {
1003 dspPhase = 0.0;
1004 Omega = 0.0;
1005 }
1006
Free(void)1007 void dspCmpxMixer::Free(void)
1008 {
1009 Output.Free();
1010 }
1011
Preset(double CarrierOmega)1012 int dspCmpxMixer::Preset(double CarrierOmega)
1013 {
1014 Omega = CarrierOmega;
1015 return 0;
1016 }
1017
Process(dspCmpx * Inp,int InpLen,dspCmpx * Out)1018 int dspCmpxMixer::Process(dspCmpx *Inp, int InpLen, dspCmpx *Out)
1019 {
1020 int i;
1021 double I, Q;
1022 for (i = 0; i < InpLen; i++) {
1023 I = cos(dspPhase);
1024 Q = sin(dspPhase);
1025 Out[i].re = I*Inp[i].re + Q*Inp[i].im;
1026 Out[i].im = I*Inp[i].im - Q*Inp[i].re;
1027 dspPhase += Omega;
1028 if (dspPhase >= 2*M_PI)
1029 dspPhase -= 2*M_PI;
1030 }
1031 return InpLen;
1032 }
1033
ProcessFast(dspCmpx * Inp,int InpLen,dspCmpx * Out)1034 int dspCmpxMixer::ProcessFast(dspCmpx *Inp, int InpLen, dspCmpx *Out)
1035 {
1036 int i;
1037 double dI, dQ, I, Q, nI, nQ, N;
1038 dI = cos(Omega);
1039 dQ = sin(Omega);
1040 I = cos(dspPhase);
1041 Q = sin(dspPhase);
1042 for (i = 0; i < InpLen; i++) {
1043 Out[i].re = I*Inp[i].re + Q*Inp[i].im;
1044 Out[i].im = I*Inp[i].im - Q*Inp[i].re;
1045 nI = I*dI - Q*dQ;
1046 nQ = Q*dI + I*dQ;
1047 I = nI;
1048 Q = nQ;
1049 }
1050 dspPhase += InpLen*Omega;
1051 N = floor(dspPhase/(2*M_PI));
1052 dspPhase -= N*2*M_PI;
1053 return InpLen;
1054 }
1055
Process(dspCmpx_buff * Input)1056 int dspCmpxMixer::Process(dspCmpx_buff *Input)
1057 {
1058 int err = Output.EnsureSpace(Input->Len);
1059 if (err) return err;
1060 Process(Input->Data, Input->Len, Output.Data);
1061 Output.Len = Input->Len;
1062 return 0;
1063 }
1064
ProcessFast(dspCmpx_buff * Input)1065 int dspCmpxMixer::ProcessFast(dspCmpx_buff *Input)
1066 {
1067 int err = Output.EnsureSpace(Input->Len);
1068 if(err) return err;
1069 ProcessFast(Input->Data, Input->Len, Output.Data);
1070 Output.Len = Input->Len;
1071 return 0;
1072 }
1073
1074 // ----------------------------------------------------------------------------
1075 // FM demodulator (dspPhase rotation speed meter)
1076
dspFMdemod()1077 dspFMdemod::dspFMdemod()
1078 {
1079 PrevdspPhase = 0.0;
1080 RefOmega = 0.0;
1081 }
1082
Preset(double CenterOmega)1083 int dspFMdemod::Preset(double CenterOmega)
1084 {
1085 RefOmega = CenterOmega;
1086 return 0;
1087 }
1088
Process(double * InpI,double * InpQ,int InpLen,double * Out)1089 int dspFMdemod::Process(double *InpI, double *InpQ, int InpLen, double *Out)
1090 {
1091 int i;
1092 double dspPhase, dspPhaseDiff;
1093 for (i = 0; i < InpLen; i++) {
1094 if ((InpI[i] == 0.0) && (InpQ[i] == 0.0))
1095 dspPhase = 0;
1096 else
1097 dspPhase = atan2(InpQ[i], InpI[i]);
1098 dspPhaseDiff = dspPhase - PrevdspPhase - RefOmega;
1099 if (dspPhaseDiff >= M_PI)
1100 dspPhaseDiff -= 2*M_PI;
1101 else if (dspPhaseDiff < (-M_PI))
1102 dspPhaseDiff += 2*M_PI;
1103 Out[i] = dspPhaseDiff;
1104 PrevdspPhase = dspPhase;
1105 }
1106 return InpLen;
1107 }
1108
Process(dspCmpx * Inp,int InpLen,double * Out)1109 int dspFMdemod::Process(dspCmpx *Inp, int InpLen, double *Out)
1110 {
1111 int i;
1112 double dspPhase, dspPhaseDiff;
1113 for (i = 0; i < InpLen; i++) {
1114 if ((Inp[i].re == 0.0) && (Inp[i].im == 0.0))
1115 dspPhase = PrevdspPhase;
1116 else
1117 dspPhase = atan2(Inp[i].im, Inp[i].re);
1118 dspPhaseDiff = dspPhase - PrevdspPhase - RefOmega;
1119 if (dspPhaseDiff >= M_PI)
1120 dspPhaseDiff -= 2*M_PI;
1121 else if (dspPhaseDiff < (-M_PI))
1122 dspPhaseDiff += 2*M_PI;
1123 Out[i] = dspPhaseDiff;
1124 PrevdspPhase = dspPhase;
1125 }
1126 return InpLen;
1127 }
1128
Process(dspCmpx_buff * Input)1129 int dspFMdemod::Process(dspCmpx_buff *Input)
1130 {
1131 int err = Output.EnsureSpace(Input->Len);
1132 if(err) return err;
1133 Process(Input->Data, Input->Len, Output.Data);
1134 Output.Len = Input->Len;
1135 return 0;
1136 }
1137
1138 // ----------------------------------------------------------------------------
1139 // Rate converter - real input/output, linear interpolation
1140 // expect large error when high frequency components are present
1141 // thus the best place to convert rates is after a low pass filter
1142 // of a demodulator.
1143
1144 // note: in fldigi these rate converters are not used.
1145 // libsamplerate is the preferred solution
1146
dspRateConvLin()1147 dspRateConvLin::dspRateConvLin()
1148 {
1149 PrevSample = 0;
1150 OutdspPhase = 0;
1151 OutStep = 1.0;
1152 }
1153
SetOutVsInp(double OutVsInp)1154 void dspRateConvLin::SetOutVsInp(double OutVsInp)
1155 {
1156 OutStep = 1.0 / OutVsInp;
1157 }
1158
SetInpVsOut(double InpVsOut)1159 void dspRateConvLin::SetInpVsOut(double InpVsOut)
1160 {
1161 OutStep = InpVsOut;
1162 }
1163
Process(double_buff * Input)1164 int dspRateConvLin::Process(double_buff *Input)
1165 {
1166 int err, i, o;
1167 double *Inp, *Out;
1168 int InpLen;
1169 Inp = Input->Data;
1170 InpLen = Input->Len;
1171 err = Output.EnsureSpace((int)ceil(InpLen/OutStep) + 2);
1172 if (err) return err;
1173 Out = Output.Data;
1174 for (o = 0; OutdspPhase < 0; ) {
1175 Out[o++] = Inp[0]*(1.0 + OutdspPhase) - OutdspPhase*PrevSample;
1176 OutdspPhase += OutStep;
1177 }
1178 for (i = 0; i < (InpLen-1); ) {
1179 if (OutdspPhase >= 1.0) {
1180 OutdspPhase -= 1.0;
1181 i++;
1182 } else {
1183 Out[o++] = Inp[i]*(1.0 - OutdspPhase) + Inp[i+1]*OutdspPhase;
1184 OutdspPhase += OutStep;
1185 }
1186 }
1187 Output.Len = o;
1188 PrevSample = Inp[i];
1189 OutdspPhase -= 1.0;
1190 return 0;
1191 }
1192
1193 // ----------------------------------------------------------------------------
1194 // Rate converter - real input/output, quadratic interpolation
1195 // similar limits like for RateConv1
1196
dspRateConvQuadr()1197 dspRateConvQuadr::dspRateConvQuadr()
1198 {
1199 int i;
1200 for (i = 0; i < 4; i++)
1201 Tap[i] = 0;
1202 OutStep = 1.0;
1203 OutdspPhase = 0;
1204 TapPtr = 0;
1205 }
1206
SetOutVsInp(double OutVsInp)1207 void dspRateConvQuadr::SetOutVsInp(double OutVsInp)
1208 {
1209 OutStep = 1.0 / OutVsInp;
1210 }
1211
SetInpVsOut(double InpVsOut)1212 void dspRateConvQuadr::SetInpVsOut(double InpVsOut)
1213 {
1214 OutStep = InpVsOut;
1215 }
1216
Process(double * Inp,int InpLen,double * Out,int MaxOutLen,int * OutLen)1217 int dspRateConvQuadr::Process(
1218 double *Inp, int InpLen,
1219 double *Out, int MaxOutLen, int *OutLen)
1220 {
1221 int i, o, t;
1222 double Ref0, Ref1, Diff0, Diff1;
1223 for (o = i = 0; (i < InpLen) && (o < MaxOutLen); ) {
1224 if (OutdspPhase >= 1.0) {
1225 Tap[TapPtr] = (*Inp++);
1226 i++;
1227 TapPtr = (TapPtr + 1) & 3;
1228 OutdspPhase -= 1.0;
1229 } else {
1230 t = TapPtr;
1231 Diff0 = (Tap[t^2] - Tap[t]) / 2;
1232 Ref1 = Tap[t^2];
1233 t = (t + 1) & 3;
1234 Diff1 = (Tap[t^2] - Tap[t]) / 2;
1235 Ref0 = Tap[t];
1236 (*Out++) = Ref0 * (1.0 - OutdspPhase) + Ref1*OutdspPhase // linear piece
1237 -(Diff1-Diff0)*OutdspPhase*(1.0-OutdspPhase)/2; // quadr. piece
1238 o++;
1239 OutdspPhase += OutStep;
1240 }
1241 }
1242 (*OutLen) = o;
1243 return i;
1244 }
1245
Process(double_buff * Input)1246 int dspRateConvQuadr::Process(double_buff *Input)
1247 {
1248 int err, i, o, t;
1249 double Ref0, Ref1, Diff0, Diff1;
1250 double *Inp,*Out;
1251 int InpLen;
1252 Inp = Input->Data;
1253 InpLen = Input->Len;
1254 err = Output.EnsureSpace((int)ceil(InpLen / OutStep) + 2);
1255 if (err) return err;
1256 Out = Output.Data;
1257 for (o = i = 0; i < InpLen; ) {
1258 if (OutdspPhase >= 1.0) {
1259 Tap[TapPtr] = (*Inp++);
1260 i++;
1261 TapPtr = (TapPtr + 1) & 3;
1262 OutdspPhase -= 1.0;
1263 } else {
1264 t = TapPtr;
1265 Diff0 = (Tap[t^2] - Tap[t]) / 2;
1266 Ref1 = Tap[t^2];
1267 t = (t + 1) & 3;
1268 Diff1 = (Tap[t^2] - Tap[t]) / 2;
1269 Ref0 = Tap[t];
1270 (*Out++) = Ref0 * (1.0 - OutdspPhase) + Ref1*OutdspPhase // linear piece
1271 -(Diff1 - Diff0)*OutdspPhase*(1.0 - OutdspPhase)/2; // quadr. piece
1272 o++;
1273 OutdspPhase += OutStep;
1274 }
1275 }
1276 Output.Len = o;
1277 return 0;
1278 }
1279
1280 // ----------------------------------------------------------------------------
1281 // Rate converter, real input/output,
1282 // bandwidth-limited interpolation, several shifted FIR filters
1283
dspRateConvBL()1284 dspRateConvBL::dspRateConvBL()
1285 {
1286 Tap = NULL;
1287 Shape = NULL;
1288 ExternShape = 1;
1289 }
1290
~dspRateConvBL()1291 dspRateConvBL::~dspRateConvBL()
1292 {
1293 Free();
1294 }
1295
Free(void)1296 void dspRateConvBL::Free(void)
1297 {
1298 int s;
1299 free(Tap);
1300 Tap = NULL;
1301 if (ExternShape) return;
1302 if (Shape) {
1303 for (s = 0; s < ShapeNum; s++)
1304 free(Shape[s]);
1305 free(Shape);
1306 Shape = NULL;
1307 }
1308 }
1309
Preset(int FilterLen,double ** FilterShape,int FilterShapeNum)1310 int dspRateConvBL::Preset(int FilterLen, double **FilterShape, int FilterShapeNum)
1311 {
1312 int i;
1313 Free();
1314 Len = FilterLen;
1315 if (dspRedspAllocArray(&Tap, Len)) return -1;
1316 TapSize = Len;
1317 for (i = 0; i < Len; i++)
1318 Tap[i] = 0.0;
1319 Shape = FilterShape;
1320 ShapeNum = FilterShapeNum;
1321 ExternShape = 1;
1322 OutStep = 1.0;
1323 OutdspPhase = 0.0;
1324 return 0;
1325 }
1326
ComputeShape(double LowOmega,double UppOmega,double (* Window)(double))1327 int dspRateConvBL::ComputeShape(double LowOmega, double UppOmega, double (*Window)(double))
1328 {
1329 int idx;
1330 if (ExternShape) {
1331 if (dspAllocArray(&Shape, ShapeNum)) return -1;
1332 for (idx = 0; idx < ShapeNum; idx++) {
1333 if (dspAllocArray(&Shape[idx], Len)) return -1;
1334 }
1335 ExternShape = 0;
1336 }
1337 for (idx = 0; idx < ShapeNum; idx++)
1338 dspWinFirI(LowOmega, UppOmega, Shape[idx], Len, Window, (double)idx/ShapeNum);
1339 return 0;
1340 }
1341
SetOutVsInp(double OutVsInp)1342 void dspRateConvBL::SetOutVsInp(double OutVsInp)
1343 {
1344 OutStep = 1.0 / OutVsInp;
1345 }
1346
SetInpVsOut(double InpVsOut)1347 void dspRateConvBL::SetInpVsOut(double InpVsOut)
1348 {
1349 OutStep = InpVsOut;
1350 }
1351
Process(double_buff * Input)1352 int dspRateConvBL::Process(double_buff *Input)
1353 {
1354 int i, o, idx, t, err;
1355 double *shape;
1356 double Sum;
1357 double *Inp, *Out;
1358 int InpLen;
1359 Inp = Input->Data;
1360 InpLen = Input->Len;
1361 err = Output.EnsureSpace((int)ceil(InpLen/OutStep)+2);
1362 if (err) return err;
1363 Out = Output.Data;
1364 if ((Len + InpLen) > TapSize) {
1365 Tap = (double*)realloc(Tap, (Len+InpLen)*sizeof(double));
1366 if (Tap == NULL) return -1;
1367 TapSize = Len + InpLen;
1368 }
1369 memcpy(Tap + Len, Inp, InpLen*sizeof(double));
1370 for(o=i=0; i<InpLen; ) {
1371 if (OutdspPhase >= 1.0) {
1372 OutdspPhase -= 1.0;
1373 i++;
1374 } else {
1375 idx = (int)floor(OutdspPhase*ShapeNum);
1376 shape = Shape[idx];
1377 for (Sum = 0.0, t = 0; t < Len; t++)
1378 Sum += Tap[i + t]*shape[t];
1379 Out[o++] = Sum;
1380 OutdspPhase += OutStep;
1381 }
1382 }
1383 Output.Len = o;
1384 memmove(Tap, Tap + InpLen, Len*sizeof(double));
1385 return 0;
1386 }
1387
ProcessLinI(double_buff * Input)1388 int dspRateConvBL::ProcessLinI(double_buff *Input)
1389 {
1390 int i, o, idx, t, err;
1391 double *Inp, *Out;
1392 int InpLen;
1393 double Sum0, Sum1;
1394 double *shape;
1395 double d;
1396 Inp = Input->Data;
1397 InpLen = Input->Len;
1398 err = Output.EnsureSpace((int)ceil(InpLen/OutStep)+2);
1399 if (err) return err;
1400 Out = Output.Data;
1401 if ((Len + InpLen) > TapSize) {
1402 Tap = (double*)realloc(Tap, (Len + InpLen)*sizeof(double));
1403 if (Tap == NULL) return -1;
1404 TapSize = Len + InpLen;
1405 }
1406 memcpy(Tap + Len, Inp, InpLen*sizeof(double));
1407 for (o = i = 0; i < InpLen; ) {
1408 if (OutdspPhase >= 1.0) {
1409 OutdspPhase -= 1.0;
1410 i++;
1411 } else {
1412 idx = (int)floor(OutdspPhase*ShapeNum);
1413 d = OutdspPhase*ShapeNum - idx;
1414 shape = Shape[idx];
1415 for (Sum0 = 0.0, t = 0; t < Len; t++)
1416 Sum0 += Tap[i+t]*shape[t];
1417 idx += 1;
1418 if (idx >= ShapeNum) {
1419 idx = 0;
1420 i++;
1421 }
1422 shape = Shape[idx];
1423 for (Sum1 = 0.0, t = 0; t < Len; t++)
1424 Sum1 += Tap[i + t]*shape[t];
1425 if (idx == 0) i--;
1426 Out[o++] = (1.0 - d)*Sum0 + d*Sum1;
1427 OutdspPhase += OutStep;
1428 }
1429 }
1430 Output.Len = o;
1431 memmove(Tap, Tap + InpLen, Len*sizeof(double));
1432 return 0;
1433 }
1434
1435 // ----------------------------------------------------------------------------
1436 // Sliding window (for FFT input)
1437
dspCmpxSlideWindow()1438 dspCmpxSlideWindow::dspCmpxSlideWindow()
1439 {
1440 Buff = NULL;
1441 Window = NULL;
1442 ExternWindow = 1;
1443 }
1444
~dspCmpxSlideWindow()1445 dspCmpxSlideWindow::~dspCmpxSlideWindow()
1446 {
1447 free(Buff);
1448 if (!ExternWindow)
1449 free(Window);
1450 }
1451
Free(void)1452 void dspCmpxSlideWindow::Free(void)
1453 {
1454 free(Buff);
1455 Buff = NULL;
1456 if (!ExternWindow)
1457 free(Window);
1458 Window = NULL;
1459 }
1460
Preset(int WindowLen,int SlideDist,double * WindowShape)1461 int dspCmpxSlideWindow::Preset(int WindowLen, int SlideDist, double *WindowShape)
1462 {
1463 int i;
1464 if (SlideDist > WindowLen) return -1;
1465 Len = WindowLen;
1466 Dist = SlideDist;
1467 if (dspRedspAllocArray(&Buff, Len)) return -1;
1468 for (i = 0; i < Len; i++)
1469 Buff[i].re = Buff[i].im = 0.0;
1470 Ptr = 0;
1471 if (!ExternWindow)
1472 free(Window);
1473 Window = WindowShape;
1474 ExternWindow = 1;
1475 return 0;
1476 }
1477
SetWindow(double (* NewWindow)(double dspPhase),double Scale)1478 int dspCmpxSlideWindow::SetWindow(double (*NewWindow)(double dspPhase), double Scale)
1479 {
1480 int idx;
1481 if (NewWindow == NULL) {
1482 if (!ExternWindow)
1483 free(Window);
1484 Window = NULL;
1485 ExternWindow = 1;
1486 return 0;
1487 }
1488 if (ExternWindow) {
1489 Window = NULL;
1490 ExternWindow = 0;
1491 }
1492 if (dspRedspAllocArray(&Window, Len)) return -1;
1493 for (idx = 0; idx < Len; idx++)
1494 Window[idx] = (*NewWindow)(2*M_PI*(idx - Len/2 + 0.5)/Len)*Scale;
1495 return 0;
1496 }
1497
Process(dspCmpx_buff * Input)1498 int dspCmpxSlideWindow::Process(dspCmpx_buff *Input)
1499 {
1500 dspCmpx *Inp = Input->Data;
1501 int InpLen = Input->Len;
1502 int i, len, err;
1503 Output.Len = 0;
1504 while (InpLen > 0) {
1505 len = dspIntmin(Len - Ptr, InpLen);
1506 memcpy(Buff + Ptr, Inp, len*sizeof(dspCmpx));
1507 Ptr += len;
1508 Inp += len;
1509 InpLen -= len;
1510 if (Ptr >= Len) {
1511 len = Output.Len;
1512 err = Output.EnsureSpace(len + Len); if(err) return err;
1513 if (Window == NULL)
1514 memcpy(Output.Data, Buff, Len*sizeof(dspCmpx));
1515 else for (i = 0; i < Len; i++) {
1516 Output.Data[len + i].re = Buff[i].re*Window[i];
1517 Output.Data[len + i].im = Buff[i].im*Window[i];
1518 }
1519 Output.Len += Len;
1520 memmove(Buff, Buff + Dist, (Len - Dist)*sizeof(dspCmpx));
1521 Ptr -= Dist;
1522 }
1523 }
1524 return 0;
1525 }
1526
1527 // ----------------------------------------------------------------------------
1528 // Overlaping window (for IFFT output)
1529
dspCmpxOverlapWindow()1530 dspCmpxOverlapWindow::dspCmpxOverlapWindow()
1531 {
1532 Buff = NULL;
1533 Window = NULL;
1534 ExternWindow = 1;
1535 }
1536
~dspCmpxOverlapWindow()1537 dspCmpxOverlapWindow::~dspCmpxOverlapWindow()
1538 {
1539 free(Buff);
1540 if (!ExternWindow)
1541 free(Window);
1542 }
1543
Free(void)1544 void dspCmpxOverlapWindow::Free(void)
1545 {
1546 free(Buff);
1547 Buff=NULL;
1548 if (!ExternWindow)
1549 free(Window);
1550 Window = NULL;
1551 }
1552
Preset(int WindowLen,int SlideDist,double * WindowShape)1553 int dspCmpxOverlapWindow::Preset(int WindowLen, int SlideDist, double *WindowShape)
1554 {
1555 int i;
1556 if (SlideDist > WindowLen) return -1;
1557 Len = WindowLen;
1558 Dist = SlideDist;
1559 if (dspRedspAllocArray(&Buff, Len)) return -1;
1560 for (i = 0; i < Len; i++)
1561 Buff[i].re = Buff[i].im = 0.0;
1562 if (!ExternWindow)
1563 free(Window);
1564 Window = WindowShape;
1565 ExternWindow = 1;
1566 return 0;
1567 }
1568
SetWindow(double (* NewWindow)(double dspPhase),double Scale)1569 int dspCmpxOverlapWindow::SetWindow(double (*NewWindow)(double dspPhase), double Scale)
1570 {
1571 int idx;
1572 if (NewWindow == NULL) {
1573 if (!ExternWindow)
1574 free(Window);
1575 Window = NULL;
1576 ExternWindow = 1;
1577 return 0;
1578 }
1579 if (ExternWindow) {
1580 Window = NULL;
1581 ExternWindow = 0;
1582 }
1583 if (dspRedspAllocArray(&Window, Len)) return -1;
1584 for (idx = 0; idx < Len; idx++)
1585 Window[idx] = (*NewWindow)(2*M_PI*(idx - Len/2 + 0.5)/Len)*Scale;
1586 return 0;
1587 }
1588
Process(dspCmpx_buff * Input)1589 int dspCmpxOverlapWindow::Process(dspCmpx_buff *Input)
1590 {
1591 int i, err;
1592 Output.Len = 0;
1593 for (i = 0; i < Input->Len; i += Len) {
1594 err = Output.EnsureSpace(Output.Len + Dist);
1595 if (err) return err;
1596 Process(Input->Data + i, Output.Data + Output.Len);
1597 Output.Len += Dist;
1598 }
1599 return 0;
1600 }
1601
Process(dspCmpx * Input)1602 int dspCmpxOverlapWindow::Process(dspCmpx *Input)
1603 {
1604 int err;
1605 err = Output.EnsureSpace(Dist);
1606 if (err) return err;
1607 Process(Input, Output.Data);
1608 Output.Len = Dist;
1609 return 0;
1610 }
1611
Process(dspCmpx * Inp,dspCmpx * Out)1612 void dspCmpxOverlapWindow::Process(dspCmpx *Inp, dspCmpx *Out)
1613 {
1614 int i;
1615 if(Window == NULL) {
1616 for (i = 0; i < Dist; i++) {
1617 Out[i].re = Buff[i].re + Inp[i].re;
1618 Out[i].im = Buff[i].im + Inp[i].im;
1619 }
1620 for ( ; i < Len - Dist; i++) {
1621 Buff[i - Dist].re = Buff[i].re + Inp[i].re;
1622 Buff[i - Dist].im = Buff[i].im + Inp[i].im;
1623 }
1624 for ( ; i < Len; i++) {
1625 Buff[i - Dist].re = Inp[i].re;
1626 Buff[i - Dist].im = Inp[i].im;
1627 }
1628 } else {
1629 for (i = 0; i < Dist; i++) {
1630 Out[i].re = Buff[i].re + Inp[i].re*Window[i];
1631 Out[i].im = Buff[i].im + Inp[i].im*Window[i];
1632 }
1633 for ( ; i < Len - Dist; i++) {
1634 Buff[i - Dist].re = Buff[i].re + Inp[i].re*Window[i];
1635 Buff[i - Dist].im = Buff[i].im + Inp[i].im*Window[i];
1636 }
1637 for ( ; i < Len; i++) {
1638 Buff[i - Dist].re = Inp[i].re*Window[i];
1639 Buff[i - Dist].im = Inp[i].im*Window[i]; }
1640 }
1641 }
1642
ProcessSilence(int Slides)1643 int dspCmpxOverlapWindow::ProcessSilence(int Slides)
1644 {
1645 int err, slide;
1646 err = Output.EnsureSpace(Slides*Dist);
1647 if (err) return err;
1648 Output.Len = 0;
1649 for (slide = 0; slide < Slides; slide++) {
1650 memcpy(Output.Data + Output.Len, Buff, Dist*sizeof(dspCmpx));
1651 memcpy(Buff, Buff + Dist, (Len - Dist)*sizeof(dspCmpx));
1652 Output.Len += Dist;
1653 }
1654 return 0;
1655 }
1656
1657 // ----------------------------------------------------------------------------
1658 // FFT dspPhase corrector
1659
dspFFT_TimeShift()1660 dspFFT_TimeShift::dspFFT_TimeShift()
1661 {
1662 FreqTable = NULL;
1663 }
1664
~dspFFT_TimeShift()1665 dspFFT_TimeShift::~dspFFT_TimeShift()
1666 {
1667 free(FreqTable);
1668 }
1669
Free(void)1670 void dspFFT_TimeShift::Free(void)
1671 {
1672 free(FreqTable);
1673 FreqTable = NULL;
1674 }
1675
Preset(int FFTlen,int Backwards)1676 int dspFFT_TimeShift::Preset(int FFTlen, int Backwards)
1677 {
1678 int i;
1679 double p;
1680 dspPhase = 0;
1681 Len = FFTlen;
1682 LenMask = FFTlen - 1;
1683 if ((LenMask^Len) != (2*Len - 1)) return -1;
1684 if (dspRedspAllocArray(&FreqTable, Len)) return -1;
1685 for (i = 0; i < Len; i++) {
1686 p = (2*M_PI*i)/Len;
1687 if (Backwards) p = (-p);
1688 FreqTable[i].re = cos(p);
1689 FreqTable[i].im = sin(p);
1690 }
1691 return 0;
1692 }
1693
Process(dspCmpx * Data,int Time)1694 int dspFFT_TimeShift::Process(dspCmpx *Data, int Time)
1695 {
1696 double nI, nQ;
1697 int i, p;
1698 dspPhase = (dspPhase + Time) & LenMask;
1699 for (p = i = 0; i < Len; i++) {
1700 nI = Data[i].re*FreqTable[i].re - Data[i].im*FreqTable[i].im;
1701 nQ = Data[i].re*FreqTable[i].im + Data[i].im*FreqTable[i].re;
1702 Data[i].re = nI;
1703 Data[i].im = nQ;
1704 p = (p + dspPhase) & LenMask;
1705 }
1706 return 0;
1707 }
1708
1709 // ----------------------------------------------------------------------------
1710 // bit synchronizer, the bit rate is the input rate divided by four
1711
dspDiffBitSync4(int IntegBits)1712 dspDiffBitSync4::dspDiffBitSync4(int IntegBits)
1713 {
1714 int i;
1715 IntegLen = IntegBits;
1716 InpTapLen = 4*IntegLen + 8;
1717 InpTap = (double*)malloc(InpTapLen*sizeof(double));
1718 for (i = 0; i < InpTapLen; i++)
1719 InpTap[i] = 0;
1720 InpTapPtr = 0;
1721 for (i = 0; i < 4; i++) {
1722 DiffInteg[i] = DiffInteg0[i] = 0.0;
1723 }
1724 DiffTapPtr = 0;
1725 BitPtr = 0;
1726 SyncdspPhase = 0.0;
1727 SyncDrift = SyncDrift0 = 0;
1728 SyncConfid = 0.0;
1729 dspLowPass2Coeff((double)IntegLen*2, W1, W2, W5);
1730 }
1731
~dspDiffBitSync4()1732 dspDiffBitSync4::~dspDiffBitSync4()
1733 {
1734 free(InpTap);
1735 }
1736
Free()1737 void dspDiffBitSync4::Free() { free(InpTap); InpTap=NULL; }
1738
Process(double * Inp,int InpLen,double * BitOut,double * IbitOut,int MaxOutLen,int * OutLen)1739 int dspDiffBitSync4::Process(
1740 double *Inp, int InpLen,
1741 double *BitOut, double *IbitOut,
1742 int MaxOutLen, int *OutLen)
1743 {
1744 int i, o, t, step;
1745 double diff;
1746 double Sum, SumI, SumQ, dspPhase;
1747 for (step = 0,o = i = 0; (i < InpLen) && (o < MaxOutLen); i++) {
1748 diff = (-InpTap[InpTapPtr++]);
1749 if (InpTapPtr >= InpTapLen)
1750 InpTapPtr = 0;
1751 diff += (InpTap[InpTapPtr] = (*Inp++));
1752 DiffTapPtr = (DiffTapPtr + 1) & 3;
1753 dspLowPass2(diff*diff, DiffInteg0[DiffTapPtr], DiffInteg[DiffTapPtr], W1, W2, W5);
1754 if (DiffTapPtr == BitPtr) {
1755 for (Sum = 0, t = 0; t < 4; t++)
1756 Sum += DiffInteg[t];
1757 t = DiffTapPtr;
1758 SumI = DiffInteg[t] - DiffInteg[t^2];
1759 t = (t + 1) & 3;
1760 SumQ = DiffInteg[t] - DiffInteg[t^2];
1761 if ((Sum == 0.0) || ((SyncConfid = (SumI*SumI + SumQ*SumQ)/(Sum*Sum)) == 0.0)) {
1762 (*BitOut++) = 0;
1763 (*IbitOut++) = 0;
1764 o++;
1765 continue;
1766 }
1767 dspPhase = atan2(-SumQ, -SumI)*(4/(2*M_PI));
1768 dspLowPass2(dspPhase - SyncdspPhase, SyncDrift0, SyncDrift, W1, W2, W5);
1769 SyncdspPhase = dspPhase;
1770 if (dspPhase > 0.52) {
1771 step = 1;
1772 SyncdspPhase -= 1.0;
1773 } else if (dspPhase < (-0.52)) {
1774 step = (-1);
1775 SyncdspPhase += 1.0;
1776 } else
1777 step = 0;
1778 double Samp[5], bit, ibit, dx;
1779 int p;
1780 p = InpTapPtr - 4*IntegLen - 2;
1781 if (p < 0)
1782 p += InpTapLen;
1783 for (t = 0; t < 5; t++) {
1784 Samp[t] = InpTap[p++];
1785 if (p >= InpTapLen)
1786 p = 0;
1787 }
1788 dx = dspPhase-0.5;
1789 // bit=Samp[2]+dx*(Samp[2]-Samp[1]); // linear interpolation
1790 bit = Samp[2]*(1.0 + dx) - Samp[1]*dx // or quadratic
1791 + ((Samp[3] - Samp[1]) - (Samp[2] - Samp[0]))/2*dx*(1.0 + dx)/2;
1792 ibit = Samp[4] + dx*(Samp[4] - Samp[3]); //linear interpolation is enough
1793 (*BitOut++) = bit;
1794 (*IbitOut++) = ibit;
1795 o++;
1796 } else if (DiffTapPtr == (BitPtr^2)) {
1797 BitPtr = (BitPtr + step) & 3;
1798 step = 0;
1799 }
1800 }
1801 (*OutLen) = o;
1802 return i;
1803 }
1804
GetSyncConfid()1805 double dspDiffBitSync4::GetSyncConfid()
1806 {
1807 return 4*SyncConfid;
1808 }
1809
GetSyncDriftRate()1810 double dspDiffBitSync4::GetSyncDriftRate()
1811 {
1812 return SyncDrift/4;
1813 }
1814
1815 // ----------------------------------------------------------------------------
1816 // bit slicer, SNR/Tune meter
1817
dspBitSlicer(int IntegBits)1818 dspBitSlicer::dspBitSlicer(int IntegBits)
1819 {
1820 int i;
1821 TapLen = IntegLen = IntegBits;
1822 Tap = (double *)malloc(TapLen*sizeof(double));
1823 for (i = 0; i < TapLen; i++)
1824 Tap[i] = 0;
1825 TapPtr = 0;
1826 for (i = 0; i < 2; i++) {
1827 Sum[i] = Sum0[i] = 0.0;
1828 SumSq[i] = SumSq0[i] = 0.0;
1829 TimeAsym = TimeAsym0 = 0.0;
1830 dspAmplAsym = dspAmplAsym0 = 0.0;
1831 Noise[i] = 0;
1832 }
1833 dspLowPass2Coeff((double)IntegLen*2, W1, W2, W5);
1834 PrevBit = PrevIBit = 0.0;
1835 OptimThres = 0.0;
1836 }
1837
~dspBitSlicer()1838 dspBitSlicer::~dspBitSlicer()
1839 {
1840 free(Tap);
1841 }
1842
Process(double * Bits,double * IBits,int InpLen,double * OutBits)1843 int dspBitSlicer::Process(double *Bits, double *IBits, int InpLen, double *OutBits)
1844 {
1845 int i, l;
1846 double Bit, soft;
1847 for (i = 0; i < InpLen; i++) {
1848 Bit = Bits[i];
1849 l = Bit > 0;
1850 dspLowPass2(Bit, Sum0[l], Sum[l], W1, W2, W5);
1851 dspLowPass2(Bit*Bit, SumSq0[l], SumSq[l], W1, W2, W5);
1852 Noise[l] = sqrt(SumSq[l] - Sum[l]*Sum[l]);
1853 if (Noise[0] + Noise[1] <= 0)
1854 OptimThres = 0;
1855 else
1856 OptimThres = (Sum[0]*Noise[1] + Sum[1]*Noise[0]) / (Noise[0] + Noise[1]);
1857 soft = Tap[TapPtr] - OptimThres; // we could do a better soft-decision
1858 if (Bit*PrevBit < 0) {
1859 dspLowPass2(PrevIBit, dspAmplAsym0, dspAmplAsym, W1, W2, W5);
1860 if (Bit > 0)
1861 PrevIBit = (-PrevIBit);
1862 dspLowPass2(PrevIBit, TimeAsym0, TimeAsym, W1, W2, W5);
1863 }
1864 (*OutBits++) = soft;
1865 PrevBit = Bit;
1866 PrevIBit = IBits[i];
1867 Tap[TapPtr] = Bit;
1868 TapPtr++;
1869 if (TapPtr >= TapLen)
1870 TapPtr = 0;
1871 }
1872 return InpLen;
1873 }
1874
GetSigToNoise()1875 double dspBitSlicer::GetSigToNoise()
1876 { return Noise[1]>0 ? (Sum[1]-OptimThres)/Noise[1] : 0.0; }
1877
GetdspAmplAsym()1878 double dspBitSlicer::GetdspAmplAsym()
1879 { double Sweep=Sum[1]-Sum[0]; return Sweep>0 ? 2*dspAmplAsym/Sweep : 0.0; }
1880
GetTimeAsym()1881 double dspBitSlicer::GetTimeAsym()
1882 { double Sweep=Sum[1]-Sum[0]; return Sweep>0 ? 2*TimeAsym/Sweep : 0.0; }
1883
1884 // ----------------------------------------------------------------------------
1885 // The decoder for the HDLC frames,
1886 // makes no AX.25 CRC check, only the length in bytes against MinLen and MaxLen
1887 // however it does not pass frames with non-complete bytes.
1888
dspHDLCdecoder(int minlen,int maxlen,int diff,int invert,int chan,int (* handler)(int,char *,int))1889 dspHDLCdecoder::dspHDLCdecoder(
1890 int minlen, int maxlen, int diff, int invert,
1891 int chan, int (*handler)(int, char *, int))
1892 {
1893 MinLen = minlen;
1894 MaxLen = maxlen;
1895 RxDiff = diff;
1896 RxInvert = invert;
1897 ChanId = chan;
1898 FrameHandler = handler;
1899 Buff = (char *)malloc(MaxLen);
1900 Len = (-1);
1901 PrevLev = 0;
1902 ShiftReg = 0;
1903 BitCount = 0;
1904 Count1s = 0;
1905 AllFrameCount = 0;
1906 BadFrameCount = 0;
1907 }
1908
~dspHDLCdecoder()1909 dspHDLCdecoder::~dspHDLCdecoder()
1910 {
1911 free(Buff);
1912 }
1913
Process(double * Inp,int InpLen)1914 int dspHDLCdecoder::Process(double *Inp, int InpLen)
1915 {
1916 int i, lev, bit, Flag;
1917
1918 for (i = 0; i < InpLen; i++) {
1919 lev = Inp[i] > 0;
1920 bit = (lev^(PrevLev & RxDiff))^RxInvert;
1921 PrevLev = lev;
1922 ShiftReg = (ShiftReg >> 1) | (bit << 7);
1923 BitCount += 1;
1924 Flag = 0;
1925 if (bit)
1926 Count1s += 1;
1927 else {
1928 if (Count1s >= 7)
1929 Len = (-1);
1930 else if (Count1s == 6)
1931 Flag = 1;
1932 else if (Count1s == 5) {
1933 ShiftReg <<= 1;
1934 BitCount -= 1;
1935 }
1936 Count1s = 0;
1937 }
1938 if (Flag) {
1939 if ((Len >= MinLen) && (BitCount == 8))
1940 (*FrameHandler)(ChanId, Buff, Len);
1941 Len = 0;
1942 BitCount = 0;
1943 } else if (Len >= 0) {
1944 if (BitCount == 8) {
1945 if (Len < MaxLen)
1946 Buff[Len++] = (char)ShiftReg;
1947 else
1948 Len = (-1);
1949 BitCount = 0;
1950 }
1951 }
1952 }
1953 return InpLen;
1954 }
1955
1956 // ----------------------------------------------------------------------------
1957 // AX.25 CRC, adress decoding, etc.
1958
1959 short unsigned int dspAX25CRCtable[256] = {
1960 0U, 4489U, 8978U, 12955U, 17956U, 22445U, 25910U, 29887U,
1961 35912U, 40385U, 44890U, 48851U, 51820U, 56293U, 59774U, 63735U,
1962 4225U, 264U, 13203U, 8730U, 22181U, 18220U, 30135U, 25662U,
1963 40137U, 36160U, 49115U, 44626U, 56045U, 52068U, 63999U, 59510U,
1964 8450U, 12427U, 528U, 5017U, 26406U, 30383U, 17460U, 21949U,
1965 44362U, 48323U, 36440U, 40913U, 60270U, 64231U, 51324U, 55797U,
1966 12675U, 8202U, 4753U, 792U, 30631U, 26158U, 21685U, 17724U,
1967 48587U, 44098U, 40665U, 36688U, 64495U, 60006U, 55549U, 51572U,
1968 16900U, 21389U, 24854U, 28831U, 1056U, 5545U, 10034U, 14011U,
1969 52812U, 57285U, 60766U, 64727U, 34920U, 39393U, 43898U, 47859U,
1970 21125U, 17164U, 29079U, 24606U, 5281U, 1320U, 14259U, 9786U,
1971 57037U, 53060U, 64991U, 60502U, 39145U, 35168U, 48123U, 43634U,
1972 25350U, 29327U, 16404U, 20893U, 9506U, 13483U, 1584U, 6073U,
1973 61262U, 65223U, 52316U, 56789U, 43370U, 47331U, 35448U, 39921U,
1974 29575U, 25102U, 20629U, 16668U, 13731U, 9258U, 5809U, 1848U,
1975 65487U, 60998U, 56541U, 52564U, 47595U, 43106U, 39673U, 35696U,
1976 33800U, 38273U, 42778U, 46739U, 49708U, 54181U, 57662U, 61623U,
1977 2112U, 6601U, 11090U, 15067U, 20068U, 24557U, 28022U, 31999U,
1978 38025U, 34048U, 47003U, 42514U, 53933U, 49956U, 61887U, 57398U,
1979 6337U, 2376U, 15315U, 10842U, 24293U, 20332U, 32247U, 27774U,
1980 42250U, 46211U, 34328U, 38801U, 58158U, 62119U, 49212U, 53685U,
1981 10562U, 14539U, 2640U, 7129U, 28518U, 32495U, 19572U, 24061U,
1982 46475U, 41986U, 38553U, 34576U, 62383U, 57894U, 53437U, 49460U,
1983 14787U, 10314U, 6865U, 2904U, 32743U, 28270U, 23797U, 19836U,
1984 50700U, 55173U, 58654U, 62615U, 32808U, 37281U, 41786U, 45747U,
1985 19012U, 23501U, 26966U, 30943U, 3168U, 7657U, 12146U, 16123U,
1986 54925U, 50948U, 62879U, 58390U, 37033U, 33056U, 46011U, 41522U,
1987 23237U, 19276U, 31191U, 26718U, 7393U, 3432U, 16371U, 11898U,
1988 59150U, 63111U, 50204U, 54677U, 41258U, 45219U, 33336U, 37809U,
1989 27462U, 31439U, 18516U, 23005U, 11618U, 15595U, 3696U, 8185U,
1990 63375U, 58886U, 54429U, 50452U, 45483U, 40994U, 37561U, 33584U,
1991 31687U, 27214U, 22741U, 18780U, 15843U, 11370U, 7921U, 3960U } ;
1992
dspAX25CRC(char * Data,int Len)1993 short unsigned int dspAX25CRC(char *Data, int Len)
1994 {
1995 int i, idx;
1996 short unsigned int CRC;
1997 for (CRC = 0xFFFF, i = 0; i < Len; i++) {
1998 idx = (unsigned char)CRC^(unsigned char)Data[i];
1999 CRC = (CRC>>8)^dspAX25CRCtable[idx];
2000 }
2001 CRC ^= 0xFFFF;
2002 return CRC;
2003 }
2004
2005 // ----------------------------------------------------------------------------
2006 // radix-2 FFT
2007
2008 // constructor
dsp_r2FFT()2009 dsp_r2FFT::dsp_r2FFT()
2010 {
2011 BitRevIdx = NULL;
2012 Twiddle = NULL; /* Window=NULL; */
2013 }
2014
2015 // destructor: free twiddles, bit-reverse lookup and window tables
~dsp_r2FFT()2016 dsp_r2FFT::~dsp_r2FFT()
2017 {
2018 free(BitRevIdx);
2019 free(Twiddle); /* free(Window); */
2020 }
2021
Free(void)2022 void dsp_r2FFT::Free(void)
2023 {
2024 free(BitRevIdx);
2025 BitRevIdx = NULL;
2026 free(Twiddle);
2027 Twiddle = NULL;
2028 }
2029
2030 // ..........................................................................
2031
2032 // a radix-2 FFT bufferfly
FFTbf(dspCmpx & x0,dspCmpx & x1,dspCmpx & W)2033 inline void dsp_r2FFT::FFTbf(dspCmpx &x0, dspCmpx &x1, dspCmpx &W)
2034 {
2035 dspCmpx x1W;
2036 x1W.re = x1.re*W.re + x1.im*W.im; // x1W.re=x1.re*W.re-x1.im*W.im;
2037 x1W.im = (-x1.re*W.im) + x1.im*W.re; // x1W.im=x1.re*W.im+x1.im*W.re;
2038 x1.re = x0.re - x1W.re;
2039 x1.im = x0.im - x1W.im;
2040 x0.re = x0.re + x1W.re;
2041 x0.im = x0.im + x1W.im;
2042 }
2043
2044 // 2-point FFT
FFT2(dspCmpx & x0,dspCmpx & x1)2045 inline void dsp_r2FFT::FFT2(dspCmpx &x0, dspCmpx &x1)
2046 {
2047 dspCmpx x1W;
2048 x1W.re = x1.re;
2049 x1W.im = x1.im;
2050 x1.re = x0.re - x1.re;
2051 x1.im = x0.im - x1.im;
2052 x0.re += x1W.re;
2053 x0.im += x1W.im;
2054 }
2055
2056 // 4-point FFT
2057 // beware: these depend on the convention for the twiddle factors !
FFT4(dspCmpx & x0,dspCmpx & x1,dspCmpx & x2,dspCmpx & x3)2058 inline void dsp_r2FFT::FFT4(dspCmpx &x0, dspCmpx &x1, dspCmpx &x2, dspCmpx &x3)
2059 {
2060 dspCmpx x1W;
2061 x1W.re = x2.re;
2062 x1W.im = x2.im;
2063 x2.re = x0.re - x1W.re;
2064 x2.im = x0.im - x1W.im;
2065 x0.re = x0.re + x1W.re;
2066 x0.im = x0.im + x1W.im;
2067 x1W.re = x3.im;
2068 x1W.im = (-x3.re);
2069 x3.re = x1.re - x1W.re;
2070 x3.im = x1.im - x1W.im;
2071 x1.re = x1.re + x1W.re;
2072 x1.im = x1.im + x1W.im;
2073 }
2074
2075 // ..........................................................................
2076
2077 // bit reverse (in place) the dspSequence (before the actuall FFT)
Scramble(dspCmpx x[])2078 void dsp_r2FFT::Scramble(dspCmpx x[])
2079 {
2080 int idx, ridx;
2081 dspCmpx tmp;
2082 for (idx = 0; idx < Size; idx++)
2083 if ((ridx = BitRevIdx[idx]) > idx) {
2084 tmp = x[idx];
2085 x[idx] = x[ridx];
2086 x[ridx] = tmp;
2087 /* printf("%d <=> %d\n",idx,ridx); */
2088 }
2089 }
2090
2091 // Preset for given processing size
Preset(int size)2092 int dsp_r2FFT::Preset(int size)
2093 {
2094 int err, idx, ridx, mask, rmask;
2095 double dspPhase;
2096
2097 if (!dspPowerOf2(size)) goto Error;
2098 Size = size;
2099 err = dspRedspAllocArray(&BitRevIdx, Size);
2100 if (err) goto Error;
2101 err = dspRedspAllocArray(&Twiddle, Size);
2102 if (err) goto Error;
2103 //printf("size, %d\n\n", size);
2104 //printf("idx,dspPhase,Twiddle.re,Twiddle.im\n");
2105 for (idx = 0; idx < Size; idx++) {
2106 dspPhase = (2*M_PI*idx)/Size;
2107 Twiddle[idx].re = cos(dspPhase);
2108 Twiddle[idx].im = sin(dspPhase);
2109 //printf("%2d,%6.4f,%6.4f,%6.4f\n", idx,dspPhase,Twiddle[idx].re,Twiddle[idx].im);
2110 }
2111 //printf("\n\nidx,BitRevIdx\n");
2112 for (ridx = 0, idx = 0; idx < Size; idx++) {
2113 for (ridx = 0, mask = Size/2, rmask = 1; mask; mask >>= 1, rmask <<= 1) {
2114 if (idx & mask)
2115 ridx |= rmask;
2116 }
2117 BitRevIdx[idx] = ridx;
2118 //printf("%d,%d\n",idx,ridx);
2119 }
2120 // free(Window); Window=NULL; WinInpScale=1.0/Size; WinOutScale=0.5;
2121 return 0;
2122
2123 Error:
2124 Free();
2125 return -1;
2126 }
2127
2128 // ..........................................................................
2129
2130 // radix-2 FFT: the first and the second pass are by hand
2131 // looks like there is no gain by separating the second pass
2132 // and even the first pass is in question ?
CoreProc(dspCmpx x[])2133 void dsp_r2FFT::CoreProc(dspCmpx x[])
2134 {
2135 int Groups, GroupHalfSize, Group, Bf, TwidIdx;
2136 int HalfSize = Size/2;
2137 for (Bf = 0; Bf < Size; Bf += 2)
2138 FFT2(x[Bf], x[Bf+1]); // first pass
2139 // for(Bf=0; Bf<Size; Bf+=4) FFT4(x[Bf],x[Bf+1],x[Bf+2],x[Bf+3]); // second
2140 for (Groups = HalfSize/2, GroupHalfSize = 2; Groups; Groups >>= 1, GroupHalfSize <<= 1)
2141 for (Group = 0, Bf = 0; Group < Groups; Group++, Bf += GroupHalfSize)
2142 for (TwidIdx = 0; TwidIdx < HalfSize; TwidIdx += Groups, Bf++) {
2143 FFTbf(x[Bf], x[Bf + GroupHalfSize], Twiddle[TwidIdx]);
2144 /* printf("%2d %2d %2d\n",Bf,Bf+GroupHalfSize,TwidIdx); */
2145 }
2146 }
2147
2148 // ..........................................................................
2149
2150 // separate the result of "two reals at one time" processing
SeparTwoReals(dspCmpx Buff[],dspCmpx Out0[],dspCmpx Out1[])2151 void dsp_r2FFT::SeparTwoReals(dspCmpx Buff[], dspCmpx Out0[], dspCmpx Out1[])
2152 {
2153 int idx, HalfSize = Size/2;
2154 // for(idx=0; idx<Size; idx++)
2155 // printf("%2d %9.5f %9.5f\n",idx,Buff[idx].re,Buff[idx].im);
2156 Out0[0].re = Buff[0].re;
2157 Out1[0].re = Buff[0].im;
2158 for (idx = 1; idx < HalfSize; idx++) {
2159 Out0[idx].re = Buff[idx].re + Buff[Size-idx].re;
2160 Out0[idx].im = Buff[idx].im - Buff[Size-idx].im;
2161 Out1[idx].re = Buff[idx].im + Buff[Size-idx].im;
2162 Out1[idx].im = (-Buff[idx].re) + Buff[Size-idx].re;
2163 }
2164 Out0[0].im = Buff[HalfSize].re;
2165 Out1[0].im = Buff[HalfSize].im;
2166 // for(idx=0; idx<HalfSize; idx++)
2167 // printf("%2d %9.5f %9.5f %9.5f %9.5f\n",
2168 // idx,Out0[idx].re,Out0[idx].im,Out1[idx].re,Out1[idx].im);
2169 }
2170
2171 // the oposite of SeparTwoReals()
2172 // but we NEGATE the .im part for Inverse FFT
2173 // as a "by-product" we multiply the transform by 2
JoinTwoReals(dspCmpx Inp0[],dspCmpx Inp1[],dspCmpx Buff[])2174 void dsp_r2FFT::JoinTwoReals(dspCmpx Inp0[], dspCmpx Inp1[], dspCmpx Buff[])
2175 {
2176 int idx, HalfSize = Size/2;
2177 // for(idx=0; idx<HalfSize; idx++)
2178 // printf("%2d %9.5f %9.5f %9.5f %9.5f\n",
2179 // idx,Inp0[idx].re,Inp0[idx].im,Inp1[idx].re,Inp1[idx].im);
2180 Buff[0].re = 2*Inp0[0].re;
2181 Buff[0].im = (-2*Inp1[0].re);
2182 for (idx = 1; idx < HalfSize; idx++) {
2183 Buff[idx].re = Inp0[idx].re - Inp1[idx].im;
2184 Buff[idx].im = (-Inp0[idx].im) - Inp1[idx].re;
2185 Buff[Size-idx].re = Inp0[idx].re + Inp1[idx].im;
2186 Buff[Size-idx].im = Inp0[idx].im - Inp1[idx].re;
2187 }
2188 Buff[HalfSize].re = 2*Inp0[0].im;
2189 Buff[HalfSize].im = (-2*Inp1[0].im);
2190 // for(idx=0; idx<Size; idx++)
2191 // printf("%2d %9.5f %9.5f\n",idx,Buff[idx].re,Buff[idx].im);
2192 }
2193
2194 // ----------------------------------------------------------------------------
2195 // Sliding window FFT for spectral analysis
2196 // input: real-valued time-domain signal,
2197 // output: complex-valued Fourier Transform
2198 //
2199 // We use a little trick here to process two real-valued FFT
2200 // in one go using the complex FFT engine.
2201 // This cuts the CPU but makes the input->output dspDelay longer.
2202
dspSlideWinFFT()2203 dspSlideWinFFT::dspSlideWinFFT()
2204 {
2205 SlideBuff = NULL;
2206 FFTbuff = NULL;
2207 Window = NULL;
2208 ExternWindow = 1;
2209 }
2210
~dspSlideWinFFT()2211 dspSlideWinFFT::~dspSlideWinFFT()
2212 {
2213 free(SlideBuff);
2214 free(FFTbuff);
2215 if (!ExternWindow)
2216 free(Window);
2217 }
2218
Free(void)2219 void dspSlideWinFFT::Free(void)
2220 {
2221 free(SlideBuff);
2222 SlideBuff = NULL;
2223 free(FFTbuff);
2224 FFTbuff = NULL;
2225 if (!ExternWindow)
2226 free(Window);
2227 Window = NULL;
2228 ExternWindow = 1;
2229 Output.Free();
2230 }
2231
Preset(int size,int step,double * window)2232 int dspSlideWinFFT::Preset(int size, int step, double *window)
2233 {
2234 int err,i;
2235
2236 Size = size;
2237 SizeMask = Size - 1;
2238 err = FFT.Preset(Size);
2239 if (err) goto Error;
2240
2241 if (!ExternWindow) {
2242 free(Window);
2243 ExternWindow = 1;
2244 }
2245 Window = window;
2246
2247 err = dspRedspAllocArray(&FFTbuff, Size);
2248 if (err) goto Error;
2249
2250 err = dspRedspAllocArray(&SlideBuff, Size);
2251 if (err) goto Error;
2252 for (i = 0; i < Size; i++)
2253 SlideBuff[i] = 0.0;
2254 SlidePtr = 0;
2255 Slide = 0;
2256 Dist = step;
2257 Left = Dist;
2258
2259 return 0;
2260
2261 Error:
2262 Free();
2263 return -1;
2264 }
2265
SetWindow(double (* NewWindow)(double dspPhase),double Scale)2266 int dspSlideWinFFT::SetWindow(double (*NewWindow)(double dspPhase), double Scale)
2267 {
2268 int idx, err;
2269 if (NewWindow == NULL) {
2270 if (!ExternWindow)
2271 free(Window);
2272 Window = NULL;
2273 ExternWindow = 1;
2274 return 0;
2275 }
2276 if (ExternWindow) {
2277 Window = NULL;
2278 ExternWindow = 0;
2279 }
2280 err = dspRedspAllocArray(&Window, Size);
2281 if (err) return -1;
2282 for (idx = 0; idx < Size; idx++)
2283 Window[idx] = Scale*(*NewWindow)(2*M_PI*(idx - Size/2 + 0.5)/Size);
2284 return 0;
2285 }
2286
Preset(int size,int step,double (* NewWindow)(double dspPhase),double Scale)2287 int dspSlideWinFFT::Preset(
2288 int size, int step,
2289 double (*NewWindow)(double dspPhase), double Scale)
2290 {
2291 int err;
2292 err = Preset(size, step, (double *)NULL);
2293 if (err) return -1;
2294 err = SetWindow(NewWindow, Scale);
2295 if (err) {
2296 Free();
2297 return -1;
2298 }
2299 return 0;
2300 }
2301
SetWindow(double * window)2302 int dspSlideWinFFT::SetWindow(double *window)
2303 {
2304 if (!ExternWindow) {
2305 free(Window);
2306 ExternWindow = 1;
2307 }
2308 Window = window;
2309 return 0;
2310 }
2311
Process(double_buff * Input)2312 int dspSlideWinFFT::Process(double_buff *Input)
2313 {
2314 int err, len, i, t;
2315 int InpLen;
2316 double *Inp;
2317 Inp = Input->Data;
2318 InpLen = Input->Len;
2319 Output.Len = 0;
2320 while (InpLen) {
2321 for (i = len = dspIntmin(InpLen, Left); i; i--) {
2322 SlideBuff[SlidePtr++] = (*Inp++);
2323 SlidePtr &= SizeMask;
2324 }
2325 InpLen -= len;
2326 Left -= len;
2327 if(Left==0) {
2328 Slide ^= 1;
2329 Left = Dist;
2330 if (Slide) {
2331 for (t = 0, i = SlidePtr; i < Size; t++,i++)
2332 FFTbuff[t].re = Window[t]*SlideBuff[i];
2333 for (i = 0; t < Size; t++, i++)
2334 FFTbuff[t].re = Window[t]*SlideBuff[i];
2335 } else {
2336 for (t = 0, i = SlidePtr; i < Size; t++,i++)
2337 FFTbuff[t].im = Window[t]*SlideBuff[i];
2338 for (i = 0; t < Size; t++,i++)
2339 FFTbuff[t].im = Window[t]*SlideBuff[i];
2340 FFT.Scramble(FFTbuff);
2341 FFT.CoreProc(FFTbuff);
2342 len = Output.Len;
2343 err = Output.EnsureSpace(len + Size);
2344 if (err) return -1;
2345 FFT.SeparTwoReals(FFTbuff, Output.Data + len, Output.Data + len + Size/2);
2346 Output.Len += Size;
2347 }
2348 }
2349 }
2350 return 0;
2351 }
2352
2353 // ----------------------------------------------------------------------------
2354 // Overlapping IFFT to convert sliced spectra into time-domain output
2355
2356
2357
2358 // ----------------------------------------------------------------------------
2359 // Sliding window FFT for spectral processing
2360 // input: real-valued signal
2361 // in the middle you are given a chance to process
2362 // the complex-valued Fourier Transform (SpectraProc() routine).
2363 // output: real-valued signal
2364 // If you don't touch the spectra in SpectralProc()
2365 // the output will be an exact copy (only dspDelayed) of the input.
2366
dspSlideWinFFTproc()2367 dspSlideWinFFTproc::dspSlideWinFFTproc()
2368 {
2369 SlideBuff = NULL;
2370 OvlapBuff = NULL;
2371 FFTbuff = NULL;
2372 Spectr[0] = NULL;
2373 Spectr[1] = NULL;
2374 Window = NULL;
2375 ExternWindow = 1;
2376 }
2377
~dspSlideWinFFTproc()2378 dspSlideWinFFTproc::~dspSlideWinFFTproc()
2379 {
2380 free(SlideBuff);
2381 free(OvlapBuff);
2382 free(FFTbuff);
2383 free(Spectr[0]);
2384 free(Spectr[1]);
2385 if (!ExternWindow)
2386 free(Window);
2387 }
2388
Free(void)2389 void dspSlideWinFFTproc::Free(void)
2390 {
2391 int i;
2392 free(SlideBuff);
2393 SlideBuff=NULL;
2394 free(OvlapBuff);
2395 OvlapBuff=NULL;
2396 free(FFTbuff);
2397 FFTbuff = NULL;
2398 for (i = 0; i < 2; i++) {
2399 free(Spectr[0]);
2400 Spectr[0] = NULL;
2401 }
2402 if (!ExternWindow)
2403 free(Window);
2404 Window = NULL;
2405 ExternWindow = 1;
2406 Output.Free();
2407 }
2408
Preset(int size,int step,void (* proc)(dspCmpx * Spectra,int Len),double * window)2409 int dspSlideWinFFTproc::Preset(
2410 int size, int step,
2411 void (*proc)(dspCmpx *Spectra, int Len), double *window)
2412 {
2413 int err, i;
2414
2415 Size = size;
2416 SizeMask = Size - 1;
2417 err = FFT.Preset(Size);
2418 if (err) goto Error;
2419
2420 if (!ExternWindow) {
2421 free(Window);
2422 ExternWindow = 1;
2423 }
2424 Window = window;
2425
2426 dspRedspAllocArray(&FFTbuff, Size);
2427 if (err) goto Error;
2428
2429 for (i = 0; i < 2; i++) {
2430 err = dspRedspAllocArray(&Spectr[i], Size/2);
2431 if (err) goto Error;
2432 }
2433
2434 err = dspRedspAllocArray(&SlideBuff, Size);
2435 if (err) goto Error;
2436 for (i = 0; i < Size; i++)
2437 SlideBuff[i] = 0.0;
2438 SlidePtr = 0;
2439 Slide = 0;
2440 Dist = step;
2441 Left = Dist;
2442
2443 err = dspRedspAllocArray(&OvlapBuff, Size);
2444 if (err) goto Error;
2445 for (i = 0; i < Size; i++)
2446 OvlapBuff[i] = 0.0;
2447 OvlapPtr = 0;
2448
2449 SpectraProc = proc;
2450
2451 return 0;
2452
2453 Error:
2454 Free();
2455 return -1;
2456 }
2457
Preset(int size,int step,void (* proc)(dspCmpx * Spectra,int Len),double (* NewWindow)(double dspPhase),double Scale)2458 int dspSlideWinFFTproc::Preset(int size, int step,
2459 void (*proc)(dspCmpx *Spectra, int Len),
2460 double (*NewWindow)(double dspPhase), double Scale)
2461 {
2462 int err;
2463 err = Preset(size, step, proc, (double *)NULL);
2464 if (err) return -1;
2465
2466 err = SetWindow(NewWindow, Scale);
2467 if (err) {
2468 Free();
2469 return -1;
2470 }
2471 return 0;
2472 }
2473
SetWindow(double * window)2474 int dspSlideWinFFTproc::SetWindow(double *window)
2475 {
2476 if (!ExternWindow) {
2477 free(Window);
2478 ExternWindow = 1;
2479 }
2480 Window = window;
2481 return 0;
2482 }
2483
SetWindow(double (* NewWindow)(double dspPhase),double Scale)2484 int dspSlideWinFFTproc::SetWindow(double (*NewWindow)(double dspPhase), double Scale)
2485 {
2486 int idx, err;
2487 if (NewWindow == NULL) {
2488 if (!ExternWindow)
2489 free(Window);
2490 Window = NULL;
2491 ExternWindow = 1;
2492 return 0;
2493 }
2494 if (ExternWindow) {
2495 Window = NULL;
2496 ExternWindow = 0;
2497 }
2498
2499 err = dspRedspAllocArray(&Window, Size);
2500 if (err) return -1;
2501
2502 if (Scale == 0.0)
2503 Scale = sqrt(0.5/Size);
2504 for (idx = 0; idx < Size; idx++)
2505 Window[idx] = Scale*(*NewWindow)(2*M_PI*(idx - Size/2 + 0.5)/Size);
2506 return 0;
2507 }
2508
Process(double_buff * Input)2509 int dspSlideWinFFTproc::Process(double_buff *Input)
2510 {
2511 int err, len, i, t;
2512 int InpLen;
2513 double *Inp, *Out;
2514 Inp = Input->Data;
2515 InpLen = Input->Len;
2516 Output.Len = 0;
2517 while (InpLen) {
2518 for (i = len = dspIntmin(InpLen, Left); i; i--) {
2519 SlideBuff[SlidePtr++] = (*Inp++);
2520 SlidePtr &= SizeMask;
2521 }
2522 InpLen -= len;
2523 Left -= len;
2524 if (Left == 0) {
2525 Slide ^= 1;
2526 Left = Dist;
2527 if (Slide) {
2528 for (t = 0, i = SlidePtr; i < Size; t++,i++)
2529 FFTbuff[t].re = Window[t]*SlideBuff[i];
2530 for (i = 0; t < Size; t++,i++)
2531 FFTbuff[t].re = Window[t]*SlideBuff[i];
2532 } else {
2533 for (t = 0, i = SlidePtr; i < Size; t++,i++)
2534 FFTbuff[t].im = Window[t]*SlideBuff[i];
2535 for (i = 0; t < Size; t++,i++)
2536 FFTbuff[t].im = Window[t]*SlideBuff[i];
2537
2538 FFT.Scramble(FFTbuff);
2539 FFT.CoreProc(FFTbuff);
2540 FFT.SeparTwoReals(FFTbuff, Spectr[0], Spectr[1]);
2541
2542 for (i = 0; i < 2; i++)
2543 (*SpectraProc)(Spectr[i], Size);
2544
2545 FFT.JoinTwoReals(Spectr[0], Spectr[1], FFTbuff);
2546 FFT.Scramble(FFTbuff);
2547 FFT.CoreProc(FFTbuff);
2548
2549 err = Output.EnsureSpace(Output.Len + 2*Dist);
2550 if (err) return -1;
2551 Out = Output.Data + Output.Len;
2552
2553 for (t = 0, i = OvlapPtr; i < Size; t++,i++)
2554 OvlapBuff[i] += Window[t]*FFTbuff[t].re;
2555
2556 for (i = 0; t < Size; t++, i++)
2557 OvlapBuff[i] += Window[t]*FFTbuff[t].re;
2558
2559 for (i = 0; i < Dist; i++) {
2560 (*Out++) = OvlapBuff[OvlapPtr];
2561 OvlapBuff[OvlapPtr++] = 0.0;
2562 OvlapPtr &= SizeMask;
2563 }
2564
2565 for (t = 0, i = OvlapPtr; i < Size; t++,i++)
2566 OvlapBuff[i] -= Window[t]*FFTbuff[t].im;
2567
2568 for (i = 0; t < Size; t++,i++)
2569 OvlapBuff[i] -= Window[t]*FFTbuff[t].im;
2570
2571 for (i = 0; i < Dist; i++) {
2572 (*Out++) = OvlapBuff[OvlapPtr];
2573 OvlapBuff[OvlapPtr++] = 0.0;
2574 OvlapPtr &= SizeMask;
2575 }
2576
2577 Output.Len += 2*Dist;
2578 }
2579 }
2580 }
2581 return 0;
2582 }
2583
2584
2585
2586 // ----------------------------------------------------------------------------
2587 // Walsh (Hadamard ?) transform.
2588
dspWalshTrans(double * Data,int Len)2589 void dspWalshTrans(double *Data, int Len) // Len must be 2^N
2590 {
2591 int step, ptr, ptr2;
2592 double bit1, bit2;
2593 for (step = 1; step < Len; step *= 2) {
2594 for (ptr = 0; ptr < Len; ptr += 2*step) {
2595 for (ptr2 = ptr; (ptr2 - ptr) < step; ptr2 += 1) {
2596 bit1 = Data[ptr2];
2597 bit2 = Data[ptr2 + step];
2598 // Data[ptr2]=(bit1+bit2); Data[ptr2+step]=(bit1-bit2);
2599 Data[ptr2] = (bit1 + bit2);
2600 Data[ptr2 + step] = (bit2 - bit1);
2601 }
2602 }
2603 }
2604 }
2605
dspWalshInvTrans(double * Data,int Len)2606 void dspWalshInvTrans(double *Data, int Len) // Len must be 2^N
2607 {
2608 int step, ptr, ptr2;
2609 double bit1, bit2;
2610 for (step = Len/2; step; step /= 2) {
2611 for (ptr = 0; ptr < Len; ptr += 2*step) {
2612 for (ptr2 = ptr; (ptr2 - ptr) < step; ptr2 += 1) {
2613 bit1 = Data[ptr2];
2614 bit2 = Data[ptr2 + step];
2615 // Data[ptr2]=(bit1+bit2); Data[ptr2+step]=(bit1-bit2);
2616 Data[ptr2] = (bit1 - bit2);
2617 Data[ptr2 + step] = (bit1 + bit2);
2618 }
2619 }
2620 }
2621 }
2622
2623 // ----------------------------------------------------------------------------
2624
2625
2626