60 #include "newparksmcclellan.h"
66 #define BIG 4096 // Used to define array sizes. Must be somewhat larger than 8 * MaxNumTaps
68 #define M_2PI 6.28318530717958647692
69 #define ITRMAX 50 // Max Number of Iterations. Some filters require as many as 45 iterations.
70 #define MIN_TEST_VAL 1.0E-6 // Min value used in LeGrangeInterp and GEE
72 int HalfTapCount, *ExchangeIndex;
73 double *LeGrangeD, *Alpha, *CosOfGrid, *DesPlus, *Coeff, *Edge, *BandMag, *InitWeight, *DesiredMag, *Grid, *Weight, *FirCoeff;
74 bool InitDone2 =
false;
78 void InitParksMcClellan2(
void)
82 ExchangeIndex =
new(std::nothrow)
int[SMALL];
83 if(ExchangeIndex == NULL)InitDone2 =
false;
85 LeGrangeD =
new(std::nothrow)
double[SMALL];
86 if(LeGrangeD == NULL)InitDone2 =
false;
88 Alpha =
new(std::nothrow)
double[SMALL];
89 if(Alpha == NULL)InitDone2 =
false;
91 CosOfGrid =
new(std::nothrow)
double[SMALL];
92 if(CosOfGrid == NULL)InitDone2 =
false;
94 DesPlus =
new(std::nothrow)
double[SMALL];
95 if(DesPlus == NULL)InitDone2 =
false;
97 Coeff =
new(std::nothrow)
double[SMALL];
98 if(Coeff == NULL)InitDone2 =
false;
100 Edge =
new(std::nothrow)
double[SMALL];
101 if(Edge == NULL)InitDone2 =
false;
103 BandMag =
new(std::nothrow)
double[SMALL];
104 if(BandMag == NULL)InitDone2 =
false;
106 InitWeight =
new(std::nothrow)
double[SMALL];
107 if(InitWeight == NULL)InitDone2 =
false;
109 DesiredMag =
new(std::nothrow)
double[BIG];
110 if(DesiredMag == NULL)InitDone2 =
false;
112 Grid =
new(std::nothrow)
double[BIG];
113 if(Grid == NULL)InitDone2 =
false;
115 Weight =
new(std::nothrow)
double[BIG];
116 if(Weight == NULL)InitDone2 =
false;
118 FirCoeff =
new(std::nothrow)
double[BIG];
119 if(FirCoeff == NULL)InitDone2 =
false;
148 void NewParksMcClellan(
int NumTaps,
double OmegaC,
double BW,
double ParksWidth, TPassType PassType)
154 InitParksMcClellan2();
155 if(!InitDone2)
return;
158 if(NumTaps > 128)NumTaps = 128;
159 if(NumTaps < 9)NumTaps = 9;
160 if( (PassType == HPF || PassType == NOTCH) && NumTaps % 2 == 0)NumTaps--;
169 if(Edge[2] < 0.01)Edge[2] = 0.01;
170 if(Edge[2] > 0.98)Edge[2] = 0.98;
171 Edge[3] = Edge[2] + ParksWidth;
172 if(Edge[3] > 0.99)Edge[3] = 0.99;
178 InitWeight[2] = 10.0;
186 if(Edge[3] > 0.99)Edge[3] = 0.99;
187 if(Edge[3] < 0.02)Edge[3] = 0.02;
188 Edge[2] = Edge[3] - ParksWidth;
189 if(Edge[2] < 0.01)Edge[2] = 0.01;
194 InitWeight[1] = 10.0;
202 Edge[3] = OmegaC - BW/2.0;
203 if(Edge[3] < 0.02)Edge[3] = 0.02;
204 Edge[2] = Edge[3] - ParksWidth;
205 if(Edge[2] < 0.01)Edge[2] = 0.01;
206 Edge[4] = OmegaC + BW/2.0;
207 if(Edge[4] > 0.98)Edge[4] = 0.98;
208 Edge[5] = Edge[4] + ParksWidth;
209 if(Edge[5] > 0.99)Edge[5] = 0.99;
215 InitWeight[1] = 10.0;
217 InitWeight[3] = 10.0;
220 if(PassType == NOTCH)
224 Edge[3] = OmegaC - BW/2.0;
225 if(Edge[3] < 0.02)Edge[3] = 0.02;
226 Edge[2] = Edge[3] - ParksWidth;
227 if(Edge[2] < 0.01)Edge[2] = 0.01;
228 Edge[4] = OmegaC + BW/2.0;
229 if(Edge[4] > 0.98)Edge[4] = 0.98;
230 Edge[5] = Edge[4] + ParksWidth;
231 if(Edge[5] > 0.99)Edge[5] = 0.99;
238 InitWeight[2] = 10.0;
243 for(j=1; j<=2*NumBands; j++)Edge[j] /= 2.0;
245 CalcParkCoeff2(NumBands, NumTaps);
251 void CalcParkCoeff2(
int NumBands,
int TapCount)
253 int j, k, GridCount, GridIndex, BandIndex, NumIterations;
254 double LowFreqEdge, UpperFreq, TempVar, Change;
258 if(TapCount % 2)OddNumTaps =
true;
259 else OddNumTaps =
false;
261 HalfTapCount = TapCount/2;
262 if(OddNumTaps) HalfTapCount++;
265 LowFreqEdge = GridCount * HalfTapCount;
266 LowFreqEdge = 0.5 / LowFreqEdge;
270 while(BandIndex <= NumBands)
272 UpperFreq = Edge[k+1];
273 while(Grid[j] <= UpperFreq)
276 DesiredMag[j] = BandMag[BandIndex];
277 Weight[j] = InitWeight[BandIndex];
279 Grid[j] = TempVar + LowFreqEdge;
282 Grid[j-1] = UpperFreq;
283 DesiredMag[j-1] = BandMag[BandIndex];
284 Weight[j-1] = InitWeight[BandIndex];
287 if(BandIndex <= NumBands)Grid[j] = Edge[k];
291 if(!OddNumTaps && Grid[GridIndex] > (0.5-LowFreqEdge)) GridIndex--;
295 for(j=1; j<=GridIndex; j++)
297 Change = cos(M_PI * Grid[j] );
298 DesiredMag[j] = DesiredMag[j] / Change;
299 Weight[j] = Weight[j] * Change;
303 TempVar = (double)(GridIndex-1)/(double)HalfTapCount;
304 for(j=1; j<=HalfTapCount; j++)
306 ExchangeIndex[j] = (double)(j-1) * TempVar + 1.0;
308 ExchangeIndex[HalfTapCount+1] = GridIndex;
310 NumIterations = Remez2(GridIndex);
316 for(j=1; j<=HalfTapCount-1; j++)
318 Coeff[j] = 0.5 * Alpha[HalfTapCount+1-j];
320 Coeff[HalfTapCount] = Alpha[1];
324 Coeff[1] = 0.25 * Alpha[HalfTapCount];
325 for(j=2; j<=HalfTapCount-1; j++)
327 Coeff[j] = 0.25 * (Alpha[HalfTapCount+1-j] + Alpha[HalfTapCount+2-j]);
329 Coeff[HalfTapCount] = 0.5 * Alpha[1] + 0.25 * Alpha[2];
334 for(j=1; j<=HalfTapCount; j++)FirCoeff[j-1] = Coeff[j];
336 for(j=1; j<HalfTapCount; j++)FirCoeff[HalfTapCount+j-1] = Coeff[HalfTapCount-j];
338 for(j=1; j<=HalfTapCount; j++)FirCoeff[HalfTapCount+j-1] = Coeff[HalfTapCount-j+1];
342 if(NumIterations <= 3)
356 int Remez2(
int GridIndex)
358 int j, JET, K, k, NU, JCHNGE, K1, KNZ, KLOW, NUT, KUP;
359 int NUT1, LUCK, KN, NITER;
360 double Deviation, DNUM, DDEN, TempVar;
361 double DEVL, COMP, YNZ, Y1, ERR;
368 ExchangeIndex[HalfTapCount+2] = GridIndex + 1;
370 for(j=1; j<=HalfTapCount+1; j++)
372 TempVar = Grid[ ExchangeIndex[j] ];
373 CosOfGrid[j] = cos(TempVar * M_2PI);
376 JET = (HalfTapCount-1)/15 + 1;
377 for(j=1; j<=HalfTapCount+1; j++)
379 LeGrangeD[j] = LeGrangeInterp2(j,HalfTapCount+1,JET);
385 for(j=1; j<=HalfTapCount+1; j++)
387 k = ExchangeIndex[j];
388 DNUM += LeGrangeD[j] * DesiredMag[k];
389 DDEN += (double)K * LeGrangeD[j]/Weight[k];
392 Deviation = DNUM / DDEN;
395 if(Deviation > 0.0) NU = -1;
396 Deviation = -(double)NU * Deviation;
398 for(j=1; j<=HalfTapCount+1; j++)
400 k = ExchangeIndex[j];
401 TempVar = (double)K * Deviation/Weight[k];
402 DesPlus[j] = DesiredMag[k] + TempVar;
406 if(Deviation <= DEVL)
return(NITER);
410 K1 = ExchangeIndex[1];
411 KNZ = ExchangeIndex[HalfTapCount+1];
419 while(j<HalfTapCount+2)
421 KUP = ExchangeIndex[j+1];
422 k = ExchangeIndex[j] + 1;
424 if(j == 2) Y1 = COMP;
427 if(k < KUP && !ErrTest(k, NUT, COMP, &ERR))
430 COMP = (double)NUT * ERR;
433 if( ErrTest(k, NUT, COMP, &ERR) )
break;
434 COMP = (double)NUT * ERR;
437 ExchangeIndex[j] = k-1;
449 k = ExchangeIndex[j] + 1;
452 ExchangeIndex[j] = k-1;
462 if( ErrTest(k, NUT, COMP, &ERR) )
continue;
466 KLOW = ExchangeIndex[j];
472 if( ErrTest(k, NUT, COMP, &ERR) && JCHNGE <= 0)
goto L225;
474 if( ErrTest(k, NUT, COMP, &ERR) )
476 KLOW = ExchangeIndex[j];
481 COMP = (double)NUT * ERR;
484 for(k--; k>KLOW; k--)
486 if( ErrTest(k, NUT, COMP, &ERR) )
break;
487 COMP = (double)NUT * ERR;
490 KLOW = ExchangeIndex[j];
491 ExchangeIndex[j] = k + 1;
496 if(j == HalfTapCount+2) YNZ = COMP;
498 while(j <= HalfTapCount+2)
500 if(K1 > ExchangeIndex[1]) K1 = ExchangeIndex[1];
501 if(KNZ < ExchangeIndex[HalfTapCount+1]) KNZ = ExchangeIndex[HalfTapCount+1];
506 COMP = YNZ * 1.00001;
511 if( ErrTest(k, NUT, COMP, &ERR) )
continue;
519 if(LUCK == 1 || LUCK == 2)
523 if(COMP > Y1) Y1 = COMP;
524 K1 = ExchangeIndex[HalfTapCount+2];
532 for(k--; k>KLOW; k--)
534 if( ErrTest(k, NUT, COMP, &ERR) )
continue;
536 COMP = (double)NUT * ERR;
543 if(JCHNGE > 0 && NITER++ < ITRMAX)
goto TOP_LINE;
547 for(j=1; j<=HalfTapCount; j++)
549 ExchangeIndex[HalfTapCount+2 - j] = ExchangeIndex[HalfTapCount+1 - j];
551 ExchangeIndex[1] = K1;
552 if(NITER++ < ITRMAX)
goto TOP_LINE;
557 KN = ExchangeIndex[HalfTapCount+2];
558 for(j=1; j<=HalfTapCount; j++)
560 ExchangeIndex[j] = ExchangeIndex[j+1];
562 ExchangeIndex[HalfTapCount+1] = KN;
563 if(NITER++ < ITRMAX)
goto TOP_LINE;
572 double LeGrangeInterp2(
int K,
int N,
int M)
581 if(j != K)Dee = 2.0 * Dee * (Q - CosOfGrid[j]);
583 if(fabs(Dee) < MIN_TEST_VAL )
585 if(Dee < 0.0)Dee = -MIN_TEST_VAL;
586 else Dee = MIN_TEST_VAL;
594 double GEE2(
int K,
int N)
600 XF = cos(M_2PI * XF);
604 C = XF - CosOfGrid[j];
605 if(fabs(C) < MIN_TEST_VAL )
607 if(C < 0.0)C = -MIN_TEST_VAL;
608 else C = MIN_TEST_VAL;
610 C = LeGrangeD[j] / C;
612 P = P + C*DesPlus[j];
614 if(fabs(Dee) < MIN_TEST_VAL )
616 if(Dee < 0.0)Dee = -MIN_TEST_VAL;
617 else Dee = MIN_TEST_VAL;
625 bool ErrTest(
int k,
int Nut,
double Comp,
double *Err)
627 *Err = GEE2(k, HalfTapCount+1);
628 *Err = (*Err - DesiredMag[k]) * Weight[k];
629 if((
double)Nut * *Err - Comp <= 0.0)
return(
true);
637 void CalcCoefficients(
void)
640 double GTempVar, OneOverNumTaps;
641 double Omega, TempVar, FreqN, TempX, GridCos;
642 double GeeArray[SMALL];
645 CosOfGrid[HalfTapCount+2] = -2.0;
646 OneOverNumTaps = 1.0 /(double)(2*HalfTapCount-1);
649 for(j=1; j<=HalfTapCount; j++)
651 FreqN = (double)(j-1) * OneOverNumTaps;
652 TempX = cos(M_2PI * FreqN);
654 GridCos = CosOfGrid[k];
657 while(TempX <= GridCos && (GridCos-TempX) >= MIN_TEST_VAL)
660 GridCos = CosOfGrid[k];
663 if(TempX <= GridCos || (TempX-GridCos) < MIN_TEST_VAL)
665 GeeArray[j] = DesPlus[k];
670 GeeArray[j] = GEE2(1, HalfTapCount+1);
676 for(j=1; j<=HalfTapCount; j++)
679 Omega = (double)(j-1) * M_2PI * OneOverNumTaps;
680 for(n=1; n<=HalfTapCount-1; n++)
682 TempVar += GeeArray[n+1] * cos(Omega * (
double)n);
684 TempVar = 2.0 * TempVar + GeeArray[1];
688 Alpha[1] = Alpha[1] * OneOverNumTaps;
689 for(j=2; j<=HalfTapCount; j++)
691 Alpha[j] = 2.0 * Alpha[j] * OneOverNumTaps;