n5ac/mmsstv
1
0
Fork 0
mirror of https://github.com/n5ac/mmsstv.git synced 2025-12-06 04:12:03 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity
mmsstv/fir.cpp

1172 lines
26 KiB
C++
Raw Normal View History

Added LGPL files received from AA6YQ
2013-07-05 22:15:14 +02:00
//Copyright+LGPL
//-----------------------------------------------------------------------------------------------------------------------------------------------
// Copyright 2000-2013 Makoto Mori, Nobuyuki Oba
//-----------------------------------------------------------------------------------------------------------------------------------------------
// This file is part of MMSSTV.
// MMSSTV is free software: you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License
// as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
// MMSSTV is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public License along with MMTTY. If not, see
// <http://www.gnu.org/licenses/>.
//-----------------------------------------------------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------
#include <vcl.h> //ja7ude 0521
#pragma hdrstop
#include <math.h>
#include "fir.h"
//-------------------------------------------------
// <20>e<EFBFBD>h<EFBFBD>q<EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>̂<EFBFBD><CC82><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>݉<EFBFBD><DD89>Z
double __fastcall DoFIR(double *hp, double *zp, double d, int tap)
{
memcpy(zp, &zp[1], sizeof(double)*tap);
zp[tap] = d;
d = 0.0;
for( int i = 0; i <= tap; i++, hp++, zp++ ){
d += (*zp) * (*hp);
}
return d;
}
//---------------------------------------------------------------------------
CIIRTANK::CIIRTANK()
{
b1 = b2 = a0 = z1 = z2 = 0;
SetFreq(2000.0, SampFreq, 50.0);
}
//---------------------------------------------------------------------------
void CIIRTANK::SetFreq(double f, double smp, double bw)
{
double lb1, lb2, la0;
lb1 = 2 * exp(-PI * bw/smp) * cos(2 * PI * f / smp);
lb2 = -exp(-2*PI*bw/smp);
if( bw ){
#if 0
const double _gt[]={18.0, 26.0, 20.0, 20.0};
la0 = sin(2 * PI * f/smp) / (_gt[SampType] * 50 / bw);
#else
la0 = sin(2 * PI * f/smp) / ((smp/6.0) / bw);
#endif
}
else {
la0 = sin(2 * PI * f/smp);
}
b1 = lb1; b2 = lb2; a0 = la0;
}
//---------------------------------------------------------------------------
double CIIRTANK::Do(double d)
{
d *= a0;
d += (z1 * b1);
d += (z2 * b2);
z2 = z1;
if( fabs(d) < 1e-37 ) d = 0.0;
z1 = d;
return d;
}
//---------------------------------------------------------------------------
CLMS::CLMS()
{
Z = new double[LMSTAP+1];
H = new double[LMSTAP+1];
D = new double[LMSTAP+1];
memset(Z, 0, sizeof(double[LMSTAP+1]));
memset(H, 0, sizeof(double[LMSTAP+1]));
memset(D, 0, sizeof(double[LMSTAP+1]));
m_D = 0;
m_lmsADJSC = 1.0 / double(32768 * 32768); // <20>X<EFBFBD>P<EFBFBD>[<5B><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>l
m_lmsErr = m_lmsMErr = 0;
m_Tap = int((4 * SampBase/11025) + 0.5);
// m_Tap = int((8 * SampBase/11025) + 0.5);
if( m_Tap > LMSTAP ) m_Tap = LMSTAP;
m_lmsMU2 = 0.003; // LMS 2<><32>
m_lmsGM = 0.9999; // LMS <20><>
m_Tap_N = int((48 * SampBase/11025) + 0.5);
if( m_Tap_N > LMSTAP ) m_Tap_N = LMSTAP;
// m_lmsMU2_N = 0.00018; // LMS 2<><32>
// m_lmsGM_N = 0.999999; // LMS <20><>
m_lmsMU2_N = 0.00018; // LMS 2<><32>
m_lmsGM_N = 0.999999; // LMS <20><>
m_lmsDelay_N = int((12 * SampBase/11025) + 0.5);
if( m_lmsDelay_N > LMSTAP ) m_lmsDelay_N = LMSTAP;
}
CLMS::~CLMS()
{
delete D;
delete H;
delete Z;
}
//-------------------------------------------------
// <20>K<EFBFBD><4B><EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>̉<EFBFBD><CC89>Z
double CLMS::Do(double d)
{
double a = 0.0;
int i;
double *zp = Z;
double *hp = H;
// <20>g<EFBFBD><67><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD>o<EFBFBD>[<5B>T<EFBFBD><54><EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^
memcpy(Z, &Z[1], sizeof(double)*m_Tap);
#if 0
Z[m_Tap] = D[0];
#else
Z[m_Tap] = m_D;
#endif
for( i = 0; i <= m_Tap; i++, zp++, hp++ ){
a += (*zp) * (*hp);
}
// <20>덷<EFBFBD>v<EFBFBD>Z
m_lmsErr = d - a;
m_lmsMErr = m_lmsErr * m_lmsMU2 * m_lmsADJSC; // lmsADJSC = 1/(32768 * 32768) <20>X<EFBFBD>P<EFBFBD>[<5B><><EFBFBD><EFBFBD><EFBFBD>O<EFBFBD><4F><EFBFBD><EFBFBD><EFBFBD>l
// <20>x<EFBFBD><78><EFBFBD><EFBFBD><EFBFBD>̈ړ<CC88>
#if 0
if( m_lmsDelay ) memcpy(D, &D[1], sizeof(double)*m_lmsDelay);
D[m_lmsDelay] = d;
#else
m_D = d;
#endif
// <20>W<EFBFBD><57><EFBFBD>X<EFBFBD>V
zp = Z;
hp = H;
double sum = 0;
for( i = 0; i <= m_Tap; i++, zp++, hp++ ){
*hp = (m_lmsMErr * (*zp)) + (*hp * m_lmsGM);
if( *hp >= 0 ){
sum += *hp;
}
else {
sum -= *hp;
}
}
if( sum > 0 ) a /= sum;
return a;
}
//-------------------------------------------------
// <20>K<EFBFBD><4B><EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>̉<EFBFBD><CC89>Z
double CLMS::DoN(double d)
{
double a = 0.0;
int i;
double *zp = Z;
double *hp = H;
// <20>g<EFBFBD><67><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD>o<EFBFBD>[<5B>T<EFBFBD><54><EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^
memcpy(Z, &Z[1], sizeof(double)*m_Tap_N);
Z[m_Tap_N] = D[0];
for( i = 0; i <= m_Tap_N; i++, zp++, hp++ ){
a += (*zp) * (*hp);
}
// <20>덷<EFBFBD>v<EFBFBD>Z
m_lmsErr = d - a;
m_lmsMErr = m_lmsErr * m_lmsMU2_N * m_lmsADJSC; // lmsADJSC = 1/(32768 * 32768) <20>X<EFBFBD>P<EFBFBD>[<5B><><EFBFBD><EFBFBD><EFBFBD>O<EFBFBD><4F><EFBFBD><EFBFBD><EFBFBD>l
// <20>x<EFBFBD><78><EFBFBD><EFBFBD><EFBFBD>̈ړ<CC88>
memcpy(D, &D[1], sizeof(double)*m_lmsDelay_N);
D[m_lmsDelay_N] = d;
// <20>W<EFBFBD><57><EFBFBD>X<EFBFBD>V
zp = Z;
hp = H;
for( i = 0; i <= m_Tap_N; i++, zp++, hp++ ){
*hp = (m_lmsMErr * (*zp)) + (*hp * m_lmsGM_N);
}
return m_lmsErr;
}
//-------------------------------------------------
// <20>K<EFBFBD><4B><EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>̉<EFBFBD><CC89>Z
void CLMS::SetAN(int sw)
{
m_Tap_N = int((48 * SampBase/11025) + 0.5);
if( m_Tap_N > LMSTAP ) m_Tap_N = LMSTAP;
m_lmsDelay_N = int((12 * SampBase/11025) + 0.5);
if( m_lmsDelay_N > LMSTAP ) m_lmsDelay_N = LMSTAP;
memset(Z, 0, sizeof(double[LMSTAP+1]));
memset(H, 0, sizeof(double[LMSTAP+1]));
memset(D, 0, sizeof(double[LMSTAP+1]));
switch(sw){
case 1:
m_lmsMU2_N = 0.00018; // LMS 2<><32>
m_lmsGM_N = 0.999998; // LMS <20><>
break;
default:
m_lmsMU2_N = 0.00005; // LMS 2<><32>
m_lmsGM_N = 0.9999985; // LMS <20><>
break;
}
}
//-------------------------------------------------
// <20><><EFBFBD>֌v<D68C>Z<EFBFBD>̉<EFBFBD><CC89>Z
int CLMS::Sig(double d)
{
double a = 0.0;
int i;
double *zp = Z;
double *hp = H;
// <20>g<EFBFBD><67><EFBFBD><EFBFBD><EFBFBD>X<EFBFBD>o<EFBFBD>[<5B>T<EFBFBD><54><EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^
memcpy(Z, &Z[1], sizeof(double)*m_Tap);
#if 0
Z[m_Tap] = D[0];
#else
Z[m_Tap] = m_D;
#endif
for( i = 0; i <= m_Tap; i++, zp++, hp++ ){
a += (*zp) * (*hp);
}
// <20>덷<EFBFBD>v<EFBFBD>Z
m_lmsErr = d - a;
m_lmsMErr = m_lmsErr * m_lmsMU2 * m_lmsADJSC; // lmsADJSC = 1/(32768 * 32768) <20>X<EFBFBD>P<EFBFBD>[<5B><><EFBFBD><EFBFBD><EFBFBD>O<EFBFBD><4F><EFBFBD><EFBFBD><EFBFBD>l
// <20>x<EFBFBD><78><EFBFBD><EFBFBD><EFBFBD>̈ړ<CC88>
#if 0
if( m_lmsDelay ) memcpy(D, &D[1], sizeof(double)*m_lmsDelay);
D[m_lmsDelay] = d;
#else
m_D = d;
#endif
// <20>W<EFBFBD><57><EFBFBD>X<EFBFBD>V
zp = Z;
hp = H;
double sum = 0;
for( i = 0; i <= m_Tap; i++, zp++, hp++ ){
*hp = (m_lmsMErr * (*zp)) + (*hp * m_lmsGM);
if( *hp >= 0 ){
sum += *hp;
}
else {
sum -= *hp;
}
}
return sum * 32768.0;
}
//---------------------------------------------------------------------------
// <20>m<EFBFBD>b<EFBFBD>`<60>t<EFBFBD>B<EFBFBD><42><EFBFBD>^
__fastcall CNotch::CNotch()
{
// m_pZ = new double[NOTCHTAPMAX+1];
// m_pH = new double[NOTCHTAPMAX+1];
// memset(m_pZ, 0, sizeof(double)*NOTCHTAPMAX+1);
m_tap = 96 * SampBase / 11025.0;
if( m_tap & 1 ) m_tap++;
if( m_tap >= NOTCHTAPMAX ) m_tap = NOTCHTAPMAX;
SetNotchFreq(2400);
}
__fastcall CNotch::~CNotch()
{
// delete m_pZ;
// delete m_pH;
}
void __fastcall CNotch::SetNotchFreq(double fq)
{
m_freq = fq;
double fl, fh;
if( (fq < 1050.0) || (fq > 2350) ){
fl = fq - 30;
fh = fq + 30;
}
else {
fl = fq - 15;
fh = fq + 15;
}
m_Notch.Create(m_tap, ffBEF, SampFreq, fl, fh, 10, 1.0);
// MakeFilter(m_pH, m_tap, ffBEF, SampFreq, fl, fh, 10, 1.0);
}
double __fastcall CNotch::Do(double d)
{
return m_Notch.Do(d);
// return DoFIR( m_pH, m_pZ, d, m_tap);
}
/**********************************************************************
<EFBFBD>e<EFBFBD>h<EFBFBD>q<EFBFBD>t<EFBFBD>B<EFBFBD><EFBFBD><EFBFBD>^<EFBFBD>݌v<EFBFBD>֐<EFBFBD>
**********************************************************************/
/*
====================================================
<EFBFBD>x<EFBFBD>b<EFBFBD>Z<EFBFBD><EFBFBD><EFBFBD>֐<EFBFBD>
====================================================
*/
static double I0(double x)
{
double sum, xj;
int j;
sum = 1.0;
xj = 1.0;
j = 1;
while(1){
xj *= ((0.5 * x) / (double)j);
sum += (xj*xj);
j++;
if( ((0.00000001 * sum) - (xj*xj)) > 0 ) break;
}
return(sum);
}
/*
====================================================
<EFBFBD>e<EFBFBD>h<EFBFBD>q<EFBFBD>t<EFBFBD>B<EFBFBD><EFBFBD><EFBFBD>^<EFBFBD>̐݌v
====================================================
*/
void MakeFilter(double *HP, int tap, int type, double fs, double fcl, double fch, double att, double gain)
{
FIR fir;
fir.typ = type;
fir.n = tap;
fir.fs = fs;
fir.fcl = fcl;
fir.fch = fch;
fir.att = att;
fir.gain = gain;
MakeFilter(HP, &fir);
}
void MakeFilter(double *HP, FIR *fp)
{
int j, m;
double alpha, win, fm, w0, sum;
double *hp;
if( fp->typ == ffHPF ){
fp->fc = 0.5*fp->fs - fp->fcl;
}
else if( fp->typ != ffLPF ){
fp->fc = (fp->fch - fp->fcl)/2.0;
}
else {
fp->fc = fp->fcl;
}
if( fp->att >= 50.0 ){
alpha = 0.1102 * (fp->att - 8.7);
}
else if( fp->att >= 21 ){
alpha = (0.5842 * pow(fp->att - 21.0, 0.4)) + (0.07886 * (fp->att - 21.0));
}
else {
alpha = 0.0;
}
hp = fp->hp;
sum = PI*2.0*fp->fc/fp->fs;
if( fp->att >= 21 ){ // <20>C<EFBFBD><43><EFBFBD>p<EFBFBD><70><EFBFBD>X<EFBFBD><58><EFBFBD><EFBFBD><EFBFBD>Ƒ<EFBFBD><C691>֐<EFBFBD><D690><EFBFBD><EFBFBD>v<EFBFBD>Z
for( j = 0; j <= (fp->n/2); j++, hp++ ){
fm = (double)(2 * j)/(double)fp->n;
win = I0(alpha * sqrt(1.0-(fm*fm)))/I0(alpha);
if( !j ){
*hp = fp->fc * 2.0/fp->fs;
}
else {
*hp = (1.0/(PI*(double)j))*sin((double)j*sum)*win;
}
}
}
else { // <20>C<EFBFBD><43><EFBFBD>p<EFBFBD><70><EFBFBD>X<EFBFBD><58><EFBFBD><EFBFBD><EFBFBD>̂݌v<DD8C>Z
for( j = 0; j <= (fp->n/2); j++, hp++ ){
if( !j ){
*hp = fp->fc * 2.0/fp->fs;
}
else {
*hp = (1.0/(PI*(double)j))*sin((double)j*sum);
}
}
}
hp = fp->hp;
sum = *hp++;
for( j = 1; j <= (fp->n/2); j++, hp++ ) sum += 2.0 * (*hp);
hp = fp->hp;
if( sum > 0.0 ){
for( j = 0; j <= (fp->n/2); j++, hp++ ) (*hp) /= sum;
}
// <20><><EFBFBD>g<EFBFBD><67><EFBFBD>ϊ<EFBFBD>
if( fp->typ == ffHPF ){
hp = fp->hp;
for( j = 0; j <= (fp->n/2); j++, hp++ ) (*hp) *= cos((double)j*PI);
}
else if( fp->typ != ffLPF ){
w0 = PI * (fp->fcl + fp->fch) / fp->fs;
if( fp->typ == ffBPF ){
hp = fp->hp;
for( j = 0; j <= (fp->n/2); j++, hp++ ) (*hp) *= 2.0*cos((double)j*w0);
}
else {
hp = fp->hp;
*hp = 1.0 - (2.0 * (*hp));
for( hp++, j = 1; j <= (fp->n/2); j++, hp++ ) (*hp) *= -2.0*cos((double)j*w0);
}
}
for( m = fp->n/2, hp = &fp->hp[m], j = m; j >= 0; j--, hp-- ){
*HP++ = (*hp) * fp->gain;
}
for( hp = &fp->hp[1], j = 1; j <= (fp->n/2); j++, hp++ ){
*HP++ = (*hp) * fp->gain;
}
}
//---------------------------------------------------------------------------
//<2F>e<EFBFBD>h<EFBFBD>q<EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>i<EFBFBD>q<EFBFBD><71><EFBFBD>x<EFBFBD><78><EFBFBD>g<EFBFBD>ϊ<EFBFBD><CF8A>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>j<EFBFBD>̐݌v
//
void MakeHilbert(double *H, int N, double fs, double fc1, double fc2)
{
int L = N / 2;
double T = 1.0 / fs;
double W1 = 2 * PI * fc1;
double W2 = 2 * PI * fc2;
// 2*fc2*T*SA((n-L)*W2*T) - 2*fc1*T*SA((n-L)*W1*T)
double w;
int n;
double x1, x2;
for( n = 0; n <= N; n++ ){
if( n == L ){
x1 = x2 = 0.0;
}
else if( (n - L) ){
x1 = ((n - L) * W1 * T);
x1 = cos(x1) / x1;
x2 = ((n - L) * W2 * T);
x2 = cos(x2) / x2;
}
else {
x1 = x2 = 1.0;
}
w = 0.54 - 0.46 * cos(2*PI*n/(N));
H[n] = -(2 * fc2 * T * x2 - 2 * fc1 * T * x1) * w;
}
if( N < 8 ){
w = 0;
for( n = 0; n <= N; n++ ){
w += fabs(H[n]);
}
if( w ){
w = 1.0 / w;
for( n = 0; n <= N; n++ ){
H[n] *= w;
}
}
}
}
//---------------------------------------------------------------------
// <20><><EFBFBD>g<EFBFBD><67><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>O<EFBFBD><4F><EFBFBD>t<EFBFBD>i<EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>X<EFBFBD><58><EFBFBD>b<EFBFBD>h<EFBFBD><68><EFBFBD>ŃR<C583>[<5B><><EFBFBD><EFBFBD><EFBFBD>Ă͂<C482><CD82><EFBFBD><EFBFBD>Ȃ<EFBFBD><C882>j
//
// H(ej<65><6A>T) = [<5B><>0]Hm cos(m<><6D>T) - j[<5B><>1]Hm sin(m<><6D>T)
//
void DrawGraph(Graphics::TBitmap *pBitmap, const double *H, int Tap, int Over, int &nmax, int init, TColor col)
{
int k, x, y;
double f, gdb, g, pi2t, fs;
double max;
char bf[80];
TCanvas *tp = pBitmap->Canvas;
TRect rc;
rc.Left = 0;
rc.Right = pBitmap->Width;
rc.Top = 0;
rc.Bottom = pBitmap->Height;
if( init ){
tp->Brush->Color = clWhite;
tp->FillRect(rc);
}
int LM; // <20><><EFBFBD>g<EFBFBD><67><EFBFBD>\<5C><><EFBFBD>̂<EFBFBD><CC82>郉<EFBFBD>C<EFBFBD><43><EFBFBD><EFBFBD>
int DM; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̐<EFBFBD>
int MM; // <20><><EFBFBD><EFBFBD><EFBFBD>̊Ԋu
switch(Over){
case 2:
max = 3000;
fs = SampFreq/2.0;
break;
case 3:
max = 2000;
fs = SampFreq/3.0;
break;
default:
max = 4000;
fs = SampFreq;
break;
}
if( nmax ){
max = nmax;
}
else {
nmax = max;
}
switch(nmax){
case 3000:
LM = 3;
DM = 14;
MM = 5;
break;
case 100:
case 200:
case 2000:
LM = 4;
DM = 19;
MM = 5;
break;
case 400:
case 800:
case 4000:
LM = 4;
DM = 19;
MM = 5;
break;
default: // 6000
LM = 3;
DM = 11;
MM = 4;
break;
}
int XL = 32;
int XR = pBitmap->Width - 16;
int YT = 16;
int YB = pBitmap->Height - 24;
int i;
if( init ){
tp->Pen->Color = clBlack;
tp->Font->Size = 8;
tp->MoveTo(XL, YT); tp->LineTo(XR, YT); tp->LineTo(XR, YB); tp->LineTo(XL, YB); tp->LineTo(XL, YT);
tp->Pen->Color = clGray;
for( i = 0; i < 7; i++ ){
tp->Pen->Style = (i & 1) ? psSolid : psDot;
y = (int)(double(i + 1) * double(YB - YT)/8.0 + YT);
tp->MoveTo(XL, y); tp->LineTo(XR, y);
}
for( i = 1; i < 5; i++ ){
y = (int)(double(i) * double(YB - YT)/4.0 + YT);
sprintf( bf, "-%2u", (80 / 4)*i );
::SetBkMode(tp->Handle, TRANSPARENT);
tp->TextOut(XL - 6 - tp->TextWidth(bf), y - (tp->TextHeight(bf)/2), bf);
}
strcpy(bf, "dB");
tp->TextOut(XL - 6 - tp->TextWidth(bf), YT-(tp->TextHeight(bf)/2), bf);
for( i = 1; i <= DM; i++ ){
tp->Pen->Style = (i % MM) ? psDot : psSolid;
x = (int)(double(i) * double(XR - XL)/double(DM+1) + XL);
tp->MoveTo(x, YT); tp->LineTo(x, YB);
}
for( i = 0; i <= LM; i++ ){
x = (int)(double(i) * double(XR - XL)/double(LM) + XL);
sprintf(bf, "%4.0lf", (max*i)/LM);
::SetBkMode(tp->Handle, TRANSPARENT);
tp->TextOut(x - (tp->TextWidth(bf)/2), YB + 6, bf);
}
tp->Pen->Color = clRed;
tp->Pen->Style = psDot;
x = (int)(XL + (fs/2) * (double(XR-XL)/max));
tp->MoveTo(x, YT); tp->LineTo(x, YB);
tp->Pen->Color = clBlue;
tp->Pen->Style = psSolid;
}
int ay = 0;
double ra, im;
pi2t = 2.0 * PI / fs;
tp->Pen->Color = col;
for( x = XL, f = 0.0; x < XR; x++, f += (max/double(XR-XL)) ){
if( Tap ){
ra = im = 0.0;
for( k = 0; k <= Tap; k++ ){
ra += H[k] * cos(pi2t*f*k);
if( k ) im -= H[k] * sin(pi2t*f*k);
}
if( ra * im ){
g = sqrt(ra * ra + im * im);
}
else {
g = 0.0;
}
}
else {
g = 1.0;
}
if( g == 0 ) g = 1.0e-38;
gdb = 20*0.4342944*log(fabs(g)) + 80.0;
if( gdb < 0.0 ) gdb = 0.0;
gdb = (gdb * double(YB-YT))/80.0;
y = YB - (int)gdb;
if( x == XL ){
tp->MoveTo(x, y);
tp->LineTo(x, y);
}
else {
tp->MoveTo(x-1, ay);
tp->LineTo(x, y);
}
ay = y;
}
}
//---------------------------------------------------------------------
// <20><><EFBFBD>g<EFBFBD><67><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>O<EFBFBD><4F><EFBFBD>t<EFBFBD>i<EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>X<EFBFBD><58><EFBFBD>b<EFBFBD>h<EFBFBD><68><EFBFBD>ŃR<C583>[<5B><><EFBFBD><EFBFBD><EFBFBD>Ă͂<C482><CD82><EFBFBD><EFBFBD>Ȃ<EFBFBD><C882>j
//
//
void DrawGraphIIR(Graphics::TBitmap *pBitmap, double a0, double a1, double a2, double b1, double b2, int Over, int &nmax, int init, TColor col)
{
int x, y;
double f, gdb, g, pi2t, pi4t, fs;
double max;
char bf[80];
TCanvas *tp = pBitmap->Canvas;
TRect rc;
rc.Left = 0;
rc.Right = pBitmap->Width;
rc.Top = 0;
rc.Bottom = pBitmap->Height;
if( init ){
tp->Brush->Color = clWhite;
tp->FillRect(rc);
}
int LM; // <20><><EFBFBD>g<EFBFBD><67><EFBFBD>\<5C><><EFBFBD>̂<EFBFBD><CC82>郉<EFBFBD>C<EFBFBD><43><EFBFBD><EFBFBD>
int DM; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̐<EFBFBD>
int MM; // <20><><EFBFBD><EFBFBD><EFBFBD>̊Ԋu
switch(Over){
case 2:
max = 3000;
fs = SampFreq/2.0;
break;
case 3:
max = 2000;
fs = SampFreq/3.0;
break;
default:
max = 4000;
fs = SampFreq;
break;
}
if( nmax ){
max = nmax;
}
else {
nmax = max;
}
switch(nmax){
case 3000:
LM = 3;
DM = 14;
MM = 5;
break;
case 100:
case 200:
case 2000:
LM = 4;
DM = 19;
MM = 5;
break;
case 400:
case 800:
case 4000:
LM = 4;
DM = 19;
MM = 5;
break;
default: // 6000
LM = 3;
DM = 11;
MM = 4;
break;
}
int XL = 32;
int XR = pBitmap->Width - 16;
int YT = 16;
int YB = pBitmap->Height - 24;
int i;
if( init ){
tp->Pen->Color = clBlack;
tp->Font->Size = 8;
tp->MoveTo(XL, YT); tp->LineTo(XR, YT); tp->LineTo(XR, YB); tp->LineTo(XL, YB); tp->LineTo(XL, YT);
tp->Pen->Color = clGray;
for( i = 0; i < 5; i++ ){
tp->Pen->Style = (i & 1) ? psSolid : psDot;
y = (int)(double(i + 1) * double(YB - YT)/6.0 + YT);
tp->MoveTo(XL, y); tp->LineTo(XR, y);
}
for( i = 1; i < 4; i++ ){
y = (int)(double(i) * double(YB - YT)/3.0 + YT);
sprintf( bf, "-%2u", (60 / 3)*i );
::SetBkMode(tp->Handle, TRANSPARENT);
tp->TextOut(XL - 6 - tp->TextWidth(bf), y - (tp->TextHeight(bf)/2), bf);
}
strcpy(bf, "dB");
tp->TextOut(XL - 6 - tp->TextWidth(bf), YT-(tp->TextHeight(bf)/2), bf);
for( i = 1; i <= DM; i++ ){
tp->Pen->Style = (i % MM) ? psDot : psSolid;
x = (int)(double(i) * double(XR - XL)/double(DM+1) + XL);
tp->MoveTo(x, YT); tp->LineTo(x, YB);
}
for( i = 0; i <= LM; i++ ){
x = (int)(double(i) * double(XR - XL)/double(LM) + XL);
sprintf(bf, "%4.0lf", (max*i)/LM);
::SetBkMode(tp->Handle, TRANSPARENT);
tp->TextOut(x - (tp->TextWidth(bf)/2), YB + 6, bf);
}
tp->Pen->Color = clRed;
tp->Pen->Style = psDot;
x = (int)(XL + (fs/2) * (double(XR-XL)/max));
tp->MoveTo(x, YT); tp->LineTo(x, YB);
tp->Pen->Color = clBlue;
tp->Pen->Style = psSolid;
}
int ay = 0;
pi2t = 2.0 * PI / fs;
pi4t = 2.0 * pi2t;
tp->Pen->Color = col;
double A, B, C, D, P, R;
double cosw, sinw, cos2w, sin2w;
for( x = XL, f = 0.0; x < XR; x++, f += (max/double(XR-XL)) ){
cosw = cos(pi2t*f);
sinw = sin(pi2t*f);
cos2w = cos(pi4t*f);
sin2w = sin(pi4t*f);
A = a0 + a1 * cosw + a2 * cos2w;
B = 1 + b1 * cosw + b2 * cos2w;
C = a1 * sinw + a2 * sin2w;
D = b1 * sinw + b2 * sin2w;
P = A*A + C*C;
R = B*B + D*D;
g = sqrt(P/R);
if( g == 0 ) g = 1.0e-38;
gdb = 20*0.4342944*log(fabs(g)) + 60.0;
if( gdb < 0.0 ) gdb = 0.0;
gdb = (gdb * double(YB-YT))/60.0;
y = YB - (int)gdb;
if( x == XL ){
tp->MoveTo(x, y);
tp->LineTo(x, y);
}
else {
tp->MoveTo(x-1, ay);
tp->LineTo(x, y);
}
ay = y;
}
}
//---------------------------------------------------------------------
// <20><><EFBFBD>g<EFBFBD><67><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>O<EFBFBD><4F><EFBFBD>t<EFBFBD>i<EFBFBD>t<EFBFBD>B<EFBFBD><42><EFBFBD>^<5E>X<EFBFBD><58><EFBFBD>b<EFBFBD>h<EFBFBD><68><EFBFBD>ŃR<C583>[<5B><><EFBFBD><EFBFBD><EFBFBD>Ă͂<C482><CD82><EFBFBD><EFBFBD>Ȃ<EFBFBD><C882>j
//
//
void DrawGraphIIR(Graphics::TBitmap *pBitmap, CIIR *ip, int Over, int &nmax, int init, TColor col)
{
int x, y;
double f, gdb, g, pi2t, pi4t, fs;
double max;
char bf[80];
TCanvas *tp = pBitmap->Canvas;
TRect rc;
rc.Left = 0;
rc.Right = pBitmap->Width;
rc.Top = 0;
rc.Bottom = pBitmap->Height;
if( init ){
tp->Brush->Color = clWhite;
tp->FillRect(rc);
}
int LM; // <20><><EFBFBD>g<EFBFBD><67><EFBFBD>\<5C><><EFBFBD>̂<EFBFBD><CC82>郉<EFBFBD>C<EFBFBD><43><EFBFBD><EFBFBD>
int DM; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̐<EFBFBD>
int MM; // <20><><EFBFBD><EFBFBD><EFBFBD>̊Ԋu
switch(Over){
case 2:
max = 3000;
fs = SampFreq/2.0;
break;
case 3:
max = 2000;
fs = SampFreq/3.0;
break;
default:
max = 4000;
fs = SampFreq;
break;
}
if( nmax ){
max = nmax;
}
else {
nmax = max;
}
switch(nmax){
case 3000:
LM = 3;
DM = 14;
MM = 5;
break;
case 100:
case 200:
case 2000:
LM = 4;
DM = 19;
MM = 5;
break;
case 400:
case 800:
case 4000:
LM = 4;
DM = 19;
MM = 5;
break;
default: // 6000
LM = 3;
DM = 11;
MM = 4;
break;
}
int XL = 32;
int XR = pBitmap->Width - 16;
int YT = 16;
int YB = pBitmap->Height - 24;
int i;
if( init ){
tp->Pen->Color = clBlack;
tp->Font->Size = 8;
tp->MoveTo(XL, YT); tp->LineTo(XR, YT); tp->LineTo(XR, YB); tp->LineTo(XL, YB); tp->LineTo(XL, YT);
tp->Pen->Color = clGray;
for( i = 0; i < 5; i++ ){
tp->Pen->Style = (i & 1) ? psSolid : psDot;
y = (int)(double(i + 1) * double(YB - YT)/6.0 + YT);
tp->MoveTo(XL, y); tp->LineTo(XR, y);
}
for( i = 1; i < 4; i++ ){
y = (int)(double(i) * double(YB - YT)/3.0 + YT);
sprintf( bf, "-%2u", (60 / 3)*i );
::SetBkMode(tp->Handle, TRANSPARENT);
tp->TextOut(XL - 6 - tp->TextWidth(bf), y - (tp->TextHeight(bf)/2), bf);
}
strcpy(bf, "dB");
tp->TextOut(XL - 6 - tp->TextWidth(bf), YT-(tp->TextHeight(bf)/2), bf);
for( i = 1; i <= DM; i++ ){
tp->Pen->Style = (i % MM) ? psDot : psSolid;
x = (int)(double(i) * double(XR - XL)/double(DM+1) + XL);
tp->MoveTo(x, YT); tp->LineTo(x, YB);
}
for( i = 0; i <= LM; i++ ){
x = (int)(double(i) * double(XR - XL)/double(LM) + XL);
sprintf(bf, "%4.0lf", (max*i)/LM);
::SetBkMode(tp->Handle, TRANSPARENT);
tp->TextOut(x - (tp->TextWidth(bf)/2), YB + 6, bf);
}
tp->Pen->Color = clRed;
tp->Pen->Style = psDot;
x = (int)(XL + (fs/2) * (double(XR-XL)/max));
tp->MoveTo(x, YT); tp->LineTo(x, YB);
tp->Pen->Color = clBlue;
tp->Pen->Style = psSolid;
}
int ay = 0;
pi2t = 2.0 * PI / fs;
pi4t = 2.0 * pi2t;
tp->Pen->Color = col;
double A, B, C, D, P, R;
double cosw, sinw, cos2w, sin2w;
for( x = XL, f = 0.0; x < XR; x++, f += (max/double(XR-XL)) ){
cosw = cos(pi2t*f);
sinw = sin(pi2t*f);
cos2w = cos(pi4t*f);
sin2w = sin(pi4t*f);
g = 1.0;
double *ap = ip->A;
double *bp = ip->B;
for( i = 0; i < ip->m_order/2; i++, ap += 3, bp += 2 ){
/*
A = a0 + a1 * cosw + a2 * cos2w;
B = 1 + b1 * cosw + b2 * cos2w;
C = a1 * sinw + a2 * sin2w;
D = b1 * sinw + b2 * sin2w;
*/
A = bp[0] + bp[1] * cosw + bp[0] * cos2w;
B = 1 + -ap[1] * cosw + -ap[2] * cos2w;
C = bp[1] * sinw + bp[0] * sin2w;
D = -ap[1] * sinw + -ap[2] * sin2w;
P = A*A + C*C;
R = B*B + D*D;
g *= sqrt(P/R);
}
if( ip->m_order & 1 ){
A = bp[0] + bp[1] * cosw;
B = 1 + -ap[1] * cosw;
C = bp[1] * sinw;
D = -ap[1] * sinw;
P = A*A + C*C;
R = B*B + D*D;
g *= sqrt(P/R);
}
if( g == 0 ) g = 1.0e-38;
gdb = 20*0.4342944*log(fabs(g)) + 60.0;
if( gdb < 0.0 ) gdb = 0.0;
gdb = (gdb * double(YB-YT))/60.0;
y = YB - (int)gdb;
if( x == XL ){
tp->MoveTo(x, y);
tp->LineTo(x, y);
}
else {
tp->MoveTo(x-1, ay);
tp->LineTo(x, y);
}
ay = y;
}
}
double asinh(double x)
{
return log(x + sqrt(x*x+1.0));
}
//------------------------------------------------------------------
// bc : 0-<2D>o<EFBFBD>^<5E>[<5B><><EFBFBD>[<5B>X, 1-<2D>`<60>F<EFBFBD>r<EFBFBD>V<EFBFBD>t
// rp : <20>ʉ߈<CA89><DF88>̃<EFBFBD><CC83>b<EFBFBD>v<EFBFBD><76>
void MakeIIR(double *A, double *B, double fc, double fs, int order, int bc, double rp)
{
double w0, wa, u, zt, x;
int j, n;
if( bc ){ // <20>`<60>F<EFBFBD>r<EFBFBD>V<EFBFBD>t
u = 1.0/double(order) * asinh(1.0/sqrt(pow(10.0,0.1*rp)-1.0));
}
wa = tan(PI*fc/fs);
w0 = 1.0;
n = (order & 1) + 1;
double *pA = A;
double *pB = B;
double d1, d2;
for( j = 1; j <= order/2; j++, pA+=3, pB+=2 ){
if( bc ){ // <20>`<60>F<EFBFBD>r<EFBFBD>V<EFBFBD>t
d1 = sinh(u)*cos(n*PI/(2*order));
d2 = cosh(u)*sin(n*PI/(2*order));
w0 = sqrt(d1 * d1 + d2 * d2);
zt = sinh(u)*cos(n*PI/(2*order))/w0;
}
else { // <20>o<EFBFBD>^<5E>[<5B><><EFBFBD>[<5B>X
w0 = 1.0;
zt = cos(n*PI/(2*order));
}
pA[0] = 1 + wa*w0*2*zt + wa*w0*wa*w0;
pA[1] = -2 * (wa*w0*wa*w0 - 1)/pA[0];
pA[2] = -(1.0 - wa*w0*2*zt + wa*w0*wa*w0)/pA[0];
pB[0] = wa*w0*wa*w0 / pA[0];
pB[1] = 2*pB[0];
n += 2;
}
if( bc && !(order & 1) ){
x = pow( 1.0/pow(10.0,rp/20.0), 1/double(order/2) );
pB = B;
for( j = 1; j <= order/2; j++, pB+=2 ){
pB[0] *= x;
pB[1] *= x;
}
}
if( order & 1 ){
if( bc ) w0 = sinh(u);
j = (order / 2);
pA = A + (j*3);
pB = B + (j*2);
pA[0] = 1 + wa*w0;
pA[1] = -(wa*w0 - 1)/pA[0];
pB[0] = wa*w0/pA[0];
pB[1] = pB[0];
}
}
//---------------------------------------------------------------------------
CIIR::CIIR()
{
m_order = 0;
A = new double[IIRMAX*3];
B = new double[IIRMAX*2];
Z = new double[IIRMAX*2];
memset(A, 0, sizeof(double[IIRMAX*3]));
memset(B, 0, sizeof(double[IIRMAX*2]));
memset(Z, 0, sizeof(double[IIRMAX*2]));
}
CIIR::~CIIR()
{
if( A != NULL ) delete A;
if( B != NULL ) delete B;
if( Z != NULL ) delete Z;
}
void CIIR::Clear(void)
{
memset(Z, 0, sizeof(double[IIRMAX*2]));
}
void CIIR::MakeIIR(double fc, double fs, int order, int bc, double rp)
{
m_order = order;
m_bc = bc;
m_rp = rp;
::MakeIIR(A, B, fc, fs, order, bc, rp);
}
double CIIR::Do(double d)
{
double *pA = A;
double *pB = B;
double *pZ = Z;
double o;
for( int i = 0; i < m_order/2; i++, pA+=3, pB+=2, pZ+=2 ){
d += pZ[0] * pA[1] + pZ[1] * pA[2];
o = d * pB[0] + pZ[0] * pB[1] + pZ[1] * pB[0];
pZ[1] = pZ[0];
if( fabs(d) < 1e-37 ) d = 0.0;
pZ[0] = d;
d = o;
}
if( m_order & 1 ){
d += pZ[0] * pA[1];
o = d * pB[0] + pZ[0] * pB[0];
if( fabs(d) < 1e-37 ) d = 0.0;
pZ[0] = d;
d = o;
}
return d;
}
//---------------------------------------------------------------------------
// CFIR2<52>N<EFBFBD><4E><EFBFBD>X
CFIR2::CFIR2()
{
m_pZ = NULL;
m_pH = NULL;
m_pZP = NULL;
m_W = 0;
m_Tap = 0;
}
//---------------------------------------------------------------------------
CFIR2::~CFIR2()
{
if( m_pZ ) delete m_pZ;
if( m_pH ) delete m_pH;
}
//---------------------------------------------------------------------------
void __fastcall CFIR2::Create(int tap)
{
if( !tap ){
if( m_pZ ) delete m_pZ;
m_pZ = NULL;
}
else if( (m_Tap != tap) || !m_pZ ){
if( m_pZ ) delete m_pZ;
m_pZ = new double[(tap+1)*2];
memset(m_pZ, 0, sizeof(double)*(tap+1)*2);
m_W = 0;
}
m_Tap = tap;
m_TapHalf = tap/2;
}
//---------------------------------------------------------------------------
void __fastcall CFIR2::Create(int tap, int type, double fs, double fcl, double fch, double att, double gain)
{
if( (m_Tap != tap) || !m_pZ || !m_pH ){
if( m_pZ ) delete m_pZ;
m_pZ = new double[(tap+1)*2];
memset(m_pZ, 0, sizeof(double)*(tap+1)*2);
if( m_pH ) delete m_pH;
m_pH = new double[tap+1];
m_W = 0;
}
m_Tap = tap;
m_TapHalf = tap/2;
::MakeFilter(m_pH, tap, type, fs, fcl, fch, att, gain);
}
//---------------------------------------------------------------------------
void __fastcall CFIR2::Clear(void)
{
if( m_pZ ) memset(m_pZ, 0, sizeof(double)*(m_Tap+1)*2);
}
//---------------------------------------------------------------------------
double __fastcall CFIR2::Do(double d)
{
double *dp1 = &m_pZ[m_W+m_Tap+1];
m_pZP = dp1;
*dp1 = d;
m_pZ[m_W] = d;
d = 0;
double *hp = m_pH;
for( int i = 0; i <= m_Tap; i++ ){
d += (*dp1--) * (*hp++);
}
m_W++;
if( m_W > m_Tap ) m_W = 0;
return d;
}
//---------------------------------------------------------------------------
double __fastcall CFIR2::Do(double d, double *hp)
{
double *dp1 = &m_pZ[m_W+m_Tap+1];
m_pZP = dp1;
*dp1 = d;
m_pZ[m_W] = d;
d = 0;
for( int i = 0; i <= m_Tap; i++ ){
d += (*dp1--) * (*hp++);
}
m_W++;
if( m_W > m_Tap ) m_W = 0;
return d;
}
//---------------------------------------------------------------------------
double __fastcall CFIR2::Do(double *hp)
{
double d = 0;
double *dp = m_pZP;
for( int i = 0; i <= m_Tap; i++ ){
d += (*dp--) * (*hp++);
}
return d;
}
//---------------------------------------------------------------------------
void __fastcall CFIR2::Do(double &d, double &j, double *hp)
{
double *dp1 = &m_pZ[m_W+m_Tap+1];
m_pZP = dp1;
*dp1 = d;
m_pZ[m_W] = d;
double dd = 0;
for( int i = 0; i <= m_Tap; i++ ){
dd += (*dp1--) * (*hp++);
}
j = dd;
d= m_pZ[m_W+m_TapHalf+1];
m_W++;
if( m_W > m_Tap ) m_W = 0;
}
Reference in a new issue Copy permalink