n5ac/mmtty
1
0
Fork 0
mirror of https://github.com/n5ac/mmtty.git synced 2025-12-06 04:12:03 +01:00
Code Issues Actions Packages Projects Releases Wiki Activity
mmtty/fir.cpp
Stephen Hicks 979c196eb6 Added source file to repo as received from AA6YQ
2013-07-05 15:00:12 -05:00

2182 lines
47 KiB
C++
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

//Copyright+LGPL
//-----------------------------------------------------------------------------------------------------------------------------------------------
// Copyright 2000-2013 Makoto Mori, Nobuyuki Oba
//-----------------------------------------------------------------------------------------------------------------------------------------------
// This file is part of MMTTY.
// MMTTY 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.
// MMTTY 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>
#pragma hdrstop
#include <math.h>
#include "fir.h"
//-------------------------------------------------
// ＦＩＲフィルタのたたき込み演算
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[TAPMAX+1];
H = new double[TAPMAX+1];
D = new double[DELAYMAX+1];
memset(Z, 0, sizeof(double[TAPMAX+1]));
memset(H, 0, sizeof(double[TAPMAX+1]));
memset(D, 0, sizeof(double[DELAYMAX+1]));
m_lmsADJSC = 1.0 / double(32768 * 32768); // スケール調整値
m_lmsErr = m_lmsMErr = 0;
m_Type = 1; // 0-LMS, 1-NOTICH
m_lmsNotch = 0;
m_lmsNotch2 = 0;
m_twoNotch = 0;
m_Tap = 56;
m_NotchTap = 72;
m_lmsInv = 0;
m_lmsDelay = 0; // LMS Delay
m_lmsAGC = 0; // LMS AGC
m_lmsMU2 = 0.003; // LMS 2μ
m_lmsGM = 0.9999; // LMS γ
m_bpf = 1;
SetWindow(2125, 2125+170);
}
CLMS::~CLMS()
{
delete[] D;
delete[] H;
delete[] Z;
}
void CLMS::Copy(CLMS &other)
{
m_Type = other.m_Type;
m_Tap = other.m_Tap;
m_NotchTap = other.m_NotchTap;
m_lmsInv = other.m_lmsInv; // LMS InvOutput
m_lmsDelay = other.m_lmsDelay; // LMS Delay
m_lmsAGC = other.m_lmsAGC; // LMS AGC
m_lmsMU2 = other.m_lmsMU2; // LMS 2μ
m_lmsGM = other.m_lmsGM; // LMS γ
m_lmsNotch = other.m_lmsNotch;
m_lmsNotch2 = other.m_lmsNotch2;
m_twoNotch = other.m_twoNotch;
m_bpf = other.m_bpf;
SetWindow(m_MarkFreq, m_SpaceFreq);
}
void CLMS::GetFW(double &fl, double &fh, double fq)
{
double fw;
if( fq < m_MarkFreq ){
fw = m_MarkFreq - fq;
if( fw < 80.0 ){
fw = 15.0;
}
else {
fw *= 0.5;
}
}
else if( fq > m_SpaceFreq ){
fw = fq - m_SpaceFreq;
if( fw < 80.0 ){
fw = 15.0;
}
else {
fw *= 0.5;
}
}
else {
fq = (m_MarkFreq + m_SpaceFreq)/2.0;
fw = 15.0;
}
fh = fq + fw;
fl = fq - fw;
}
void CLMS::SetWindow(double mfq, double sfq)
{
m_MarkFreq = mfq;
m_SpaceFreq = sfq;
if( m_Type ){
if( m_lmsNotch && (m_lmsNotch2 == m_lmsNotch) ) m_lmsNotch2 = 0;
double fl, fh;
double c = (m_MarkFreq + m_SpaceFreq)/2;
if( !m_lmsNotch || ((m_lmsNotch >= m_MarkFreq) && (m_lmsNotch <= m_SpaceFreq)) ){
m_lmsNotch = c;
GetFW(fl, fh, c);
MakeFilter(H, m_NotchTap, ffBEF, SampFreq, fl, fh, 10, 1.0);
if( (m_lmsNotch2 >= m_MarkFreq) && (m_lmsNotch2 <= m_SpaceFreq) ){
m_lmsNotch2 = 0;
}
if( m_lmsNotch2 ){
GetFW(fl, fh, m_lmsNotch2);
MakeFilter(HBPF, m_NotchTap, ffBEF, SampFreq, fl, fh, 10, 1.0);
}
}
else {
GetFW(fl, fh, m_lmsNotch);
MakeFilter(H, m_NotchTap, ffBEF, SampFreq, fl, fh, 10, 1.0);
if( (m_lmsNotch2 >= m_MarkFreq) && (m_lmsNotch2 <= m_SpaceFreq) ){
m_lmsNotch2 = c;
}
if( m_lmsNotch2 ){
GetFW(fl, fh, m_lmsNotch2);
MakeFilter(HBPF, m_NotchTap, ffBEF, SampFreq, fl, fh, 10, 1.0);
}
}
}
else {
MakeFilter(HBPF, m_Tap, ffBPF, SampFreq, m_MarkFreq - 150, m_SpaceFreq + 150, 60, 0.00002);
}
}
//-------------------------------------------------
// 適応フィルタの演算
double CLMS::Do(double d)
{
double a = 0.0;
int i;
double *zp = Z;
double *hp = H;
if( m_Type ){
if( !m_NotchTap ) return d; // スルーの時
// ノッチフィルタ
memcpy(Z, &Z[1], sizeof(double)*m_NotchTap);
Z[m_NotchTap] = d;
for( i = 0; i <= m_NotchTap; i++, zp++, hp++ ){
a += (*zp) * (*hp);
}
if( m_lmsNotch2 && m_twoNotch ){
memcpy(D, &D[1], sizeof(double)*m_NotchTap);
D[m_NotchTap] = a;
zp = D;
hp = HBPF;
a = 0;
for( i = 0; i <= m_NotchTap; i++, zp++, hp++ ){
a += (*zp) * (*hp);
}
}
return a;
}
else {
if( !m_Tap ) return d; // スルーの時
// トランスバーサルフィルタ
memcpy(Z, &Z[1], sizeof(double)*m_Tap);
Z[m_Tap] = D[0];
for( i = 0; i <= m_Tap; i++, zp++, hp++ ){
a += (*zp) * (*hp);
}
}
// 誤差計算
m_lmsErr = d - a;
m_lmsMErr = m_lmsErr * m_lmsMU2 * m_lmsADJSC; // lmsADJSC = 1/(32768 * 32768) スケーリング調整値
// 遅延器の移動
if( m_lmsDelay ) memcpy(D, &D[1], sizeof(double)*m_lmsDelay);
D[m_lmsDelay] = d;
// 係数更新
zp = Z;
hp = H;
if( m_lmsAGC ){
double sum = 0.0;
for( i = 0; i <= m_Tap; i++, zp++, hp++ ){
*hp = (m_lmsMErr * (*zp)) + (*hp * m_lmsGM);
if( m_bpf ) *hp += HBPF[i];
sum += fabs(*hp);
}
if( sum >= 1e-3 ) a /= sum;
}
else {
for( i = 0; i <= m_Tap; i++, zp++, hp++ ){
*hp = (m_lmsMErr * (*zp)) + (*hp * m_lmsGM);
if( m_bpf ) *hp += HBPF[i];
}
}
return m_lmsInv ? m_lmsErr : a;
}
//*********************************************************************
// CDECM2 1/2デシメータ処理クラス
//
CDECM2::CDECM2()
{
memset(Z1, 0, sizeof(Z1));
memset(Z2, 0, sizeof(Z2));
::MakeFilter(H, 36, ffLPF, SampFreq, 2900, 3000, 70, 1.0);
}
double CDECM2::Do(double d1, double d2)
{
memcpy(Z1, &Z1[1], sizeof(double)*18);
memcpy(Z2, &Z2[1], sizeof(double)*17);
Z1[18] = d1;
Z2[17] = d2;
double a;
a = Z1[0] * H[36];
a += Z2[0] * H[35];
a += Z1[1] * H[34];
a += Z2[1] * H[33];
a += Z1[2] * H[32];
a += Z2[2] * H[31];
a += Z1[3] * H[30];
a += Z2[3] * H[29];
a += Z1[4] * H[28];
a += Z2[4] * H[27];
a += Z1[5] * H[26];
a += Z2[5] * H[25];
a += Z1[6] * H[24];
a += Z2[6] * H[23];
a += Z1[7] * H[22];
a += Z2[7] * H[21];
a += Z1[8] * H[20];
a += Z2[8] * H[19];
a += Z1[9] * H[18];
a += Z2[9] * H[17];
a += Z1[10] * H[16];
a += Z2[10] * H[15];
a += Z1[11] * H[14];
a += Z2[11] * H[13];
a += Z1[12] * H[12];
a += Z2[12] * H[11];
a += Z1[13] * H[10];
a += Z2[13] * H[9];
a += Z1[14] * H[8];
a += Z2[14] * H[7];
a += Z1[15] * H[6];
a += Z2[15] * H[5];
a += Z1[16] * H[4];
a += Z2[16] * H[3];
a += Z1[17] * H[2];
a += Z2[17] * H[1];
a += Z1[18] * H[0];
return a;
}
#if 0
//*********************************************************************
// CDECM2H 1/2デシメータ処理クラス 64tap
//
CDECM2H::CDECM2H()
{
memset(Z1, 0, sizeof(Z1));
memset(Z2, 0, sizeof(Z2));
::MakeFilter(H, 64, ffLPF, SampFreq, 2900, 3000, 70, 1.0);
}
double CDECM2H::Do(double d1, double d2)
{
memcpy(Z1, &Z1[1], sizeof(double)*32);
memcpy(Z2, &Z2[1], sizeof(double)*31);
Z1[32] = d1;
Z2[31] = d2;
double a;
a = Z1[0] * H[64];
a += Z2[0] * H[63];
a += Z1[1] * H[62];
a += Z2[1] * H[61];
a += Z1[2] * H[60];
a += Z2[2] * H[59];
a += Z1[3] * H[58];
a += Z2[3] * H[57];
a += Z1[4] * H[56];
a += Z2[4] * H[55];
a += Z1[5] * H[54];
a += Z2[5] * H[53];
a += Z1[6] * H[52];
a += Z2[6] * H[51];
a += Z1[7] * H[50];
a += Z2[7] * H[49];
a += Z1[8] * H[48];
a += Z2[8] * H[47];
a += Z1[9] * H[46];
a += Z2[9] * H[45];
a += Z1[10] * H[44];
a += Z2[10] * H[43];
a += Z1[11] * H[42];
a += Z2[11] * H[41];
a += Z1[12] * H[40];
a += Z2[12] * H[39];
a += Z1[13] * H[38];
a += Z2[13] * H[37];
a += Z1[14] * H[36];
a += Z2[14] * H[35];
a += Z1[15] * H[34];
a += Z2[15] * H[33];
a += Z1[16] * H[32];
a += Z2[16] * H[31];
a += Z1[17] * H[30];
a += Z2[17] * H[29];
a += Z1[18] * H[28];
a += Z2[18] * H[27];
a += Z1[19] * H[26];
a += Z2[19] * H[25];
a += Z1[20] * H[24];
a += Z2[20] * H[23];
a += Z1[21] * H[22];
a += Z2[21] * H[21];
a += Z1[22] * H[20];
a += Z2[22] * H[19];
a += Z1[23] * H[18];
a += Z2[23] * H[17];
a += Z1[24] * H[16];
a += Z2[24] * H[15];
a += Z1[25] * H[14];
a += Z2[25] * H[13];
a += Z1[26] * H[12];
a += Z2[26] * H[11];
a += Z1[27] * H[10];
a += Z2[27] * H[9];
a += Z1[28] * H[8];
a += Z2[28] * H[7];
a += Z1[29] * H[6];
a += Z2[29] * H[5];
a += Z1[30] * H[4];
a += Z2[30] * H[3];
a += Z1[31] * H[2];
a += Z2[31] * H[1];
a += Z1[32] * H[0];
return a;
}
#endif
#if 0
//*********************************************************************
// CINTP2 ×2インタポーレータ処理クラス
//
CINTP2::CINTP2()
{
memset(Z, 0, sizeof(Z));
::MakeFilter(H, 36, ffLPF, SampFreq, 2900, 3000, 70, 2.0);
}
void CINTP2::Do(double &d1, double &d2, double d)
{
memcpy(Z, &Z[1], sizeof(double)*18);
Z[18] = d;
d1 = Z[0] * H[36];
d2 = Z[1] * H[35];
d1 += Z[1] * H[34];
d2 += Z[2] * H[33];
d1 += Z[2] * H[32];
d2 += Z[3] * H[31];
d1 += Z[3] * H[30];
d2 += Z[4] * H[29];
d1 += Z[4] * H[28];
d2 += Z[5] * H[27];
d1 += Z[5] * H[26];
d2 += Z[6] * H[25];
d1 += Z[6] * H[24];
d2 += Z[7] * H[23];
d1 += Z[7] * H[22];
d2 += Z[8] * H[21];
d1 += Z[8] * H[20];
d2 += Z[9] * H[19];
d1 += Z[9] * H[18];
d2 += Z[10] * H[17];
d1 += Z[10] * H[16];
d2 += Z[11] * H[15];
d1 += Z[11] * H[14];
d2 += Z[12] * H[13];
d1 += Z[12] * H[12];
d2 += Z[13] * H[11];
d1 += Z[13] * H[10];
d2 += Z[14] * H[9];
d1 += Z[14] * H[8];
d2 += Z[15] * H[7];
d1 += Z[15] * H[6];
d2 += Z[16] * H[5];
d1 += Z[16] * H[4];
d2 += Z[17] * H[3];
d1 += Z[17] * H[2];
d2 += Z[18] * H[1];
d1 += Z[18] * H[0];
}
//*********************************************************************
// CDECM3 1/3デシメータ処理クラス
//
CDECM3::CDECM3()
{
memset(Z1, 0, sizeof(Z1));
memset(Z2, 0, sizeof(Z2));
memset(Z3, 0, sizeof(Z3));
::MakeFilter(H, 48, ffLPF, SampFreq, 1300, 3000, 70, 1.0);
}
double CDECM3::Do(double d1, double d2, double d3)
{
memcpy(Z1, &Z1[1], sizeof(double)*16);
memcpy(Z2, &Z2[1], sizeof(double)*15);
memcpy(Z3, &Z3[1], sizeof(double)*15);
Z1[16] = d1;
Z2[15] = d2;
Z3[15] = d3;
double a;
a = Z1[0] * H[48];
a += Z3[0] * H[47];
a += Z2[0] * H[46];
a += Z1[1] * H[45];
a += Z3[1] * H[44];
a += Z2[1] * H[43];
a += Z1[2] * H[42];
a += Z3[2] * H[41];
a += Z2[2] * H[40];
a += Z1[3] * H[39];
a += Z3[3] * H[38];
a += Z2[3] * H[37];
a += Z1[4] * H[36];
a += Z3[4] * H[35];
a += Z2[4] * H[34];
a += Z1[5] * H[33];
a += Z3[5] * H[32];
a += Z2[5] * H[31];
a += Z1[6] * H[30];
a += Z3[6] * H[29];
a += Z2[6] * H[28];
a += Z1[7] * H[27];
a += Z3[7] * H[26];
a += Z2[7] * H[25];
a += Z1[8] * H[24];
a += Z3[8] * H[23];
a += Z2[8] * H[22];
a += Z1[9] * H[21];
a += Z3[9] * H[20];
a += Z2[9] * H[19];
a += Z1[10] * H[18];
a += Z3[10] * H[17];
a += Z2[10] * H[16];
a += Z1[11] * H[15];
a += Z3[11] * H[14];
a += Z2[11] * H[13];
a += Z1[12] * H[12];
a += Z3[12] * H[11];
a += Z2[12] * H[10];
a += Z1[13] * H[9];
a += Z3[13] * H[8];
a += Z2[13] * H[7];
a += Z1[14] * H[6];
a += Z3[14] * H[5];
a += Z2[14] * H[4];
a += Z1[15] * H[3];
a += Z3[15] * H[2];
a += Z2[15] * H[1];
a += Z1[16] * H[0];
return a;
}
//*********************************************************************
// CINTP3 ×3インタポーレータ処理クラス
//
CINTP3::CINTP3()
{
memset(Z, 0, sizeof(Z));
::MakeFilter(H, 48, ffLPF, SampFreq, 1300, 3000, 70, 3.0);
}
void CINTP3::Do(double &d1, double &d2, double &d3, double d)
{
memcpy(Z, &Z[1], sizeof(double)*16);
Z[16] = d;
d1 = Z[0] * H[48];
d3 = Z[1] * H[47];
d2 = Z[1] * H[46];
d1 += Z[1] * H[45];
d3 += Z[2] * H[44];
d2 += Z[2] * H[43];
d1 += Z[2] * H[42];
d3 += Z[3] * H[41];
d2 += Z[3] * H[40];
d1 += Z[3] * H[39];
d3 += Z[4] * H[38];
d2 += Z[4] * H[37];
d1 += Z[4] * H[36];
d3 += Z[5] * H[35];
d2 += Z[5] * H[34];
d1 += Z[5] * H[33];
d3 += Z[6] * H[32];
d2 += Z[6] * H[31];
d1 += Z[6] * H[30];
d3 += Z[7] * H[29];
d2 += Z[7] * H[28];
d1 += Z[7] * H[27];
d3 += Z[8] * H[26];
d2 += Z[8] * H[25];
d1 += Z[8] * H[24];
d3 += Z[9] * H[23];
d2 += Z[9] * H[22];
d1 += Z[9] * H[21];
d3 += Z[10] * H[20];
d2 += Z[10] * H[19];
d1 += Z[10] * H[18];
d3 += Z[11] * H[17];
d2 += Z[11] * H[16];
d1 += Z[11] * H[15];
d3 += Z[12] * H[14];
d2 += Z[12] * H[13];
d1 += Z[12] * H[12];
d3 += Z[13] * H[11];
d2 += Z[13] * H[10];
d1 += Z[13] * H[9];
d3 += Z[14] * H[8];
d2 += Z[14] * H[7];
d1 += Z[14] * H[6];
d3 += Z[15] * H[5];
d2 += Z[15] * H[4];
d1 += Z[15] * H[3];
d3 += Z[16] * H[2];
d2 += Z[16] * H[1];
d1 += Z[16] * H[0];
}
#endif
/**********************************************************************
ＦＩＲフィルタ設計関数
**********************************************************************/
/*
====================================================
ベッセル関数
====================================================
*/
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);
}
/*
====================================================
ＦＩＲフィルタの設計
====================================================
*/
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 ){ // インパルス応答と窓関数を計算
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 { // インパルス応答のみ計算
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;
}
// 周波数変換
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;
}
}
//---------------------------------------------------------------------------
//ＦＩＲフィルタ（ヒルベルト変換フィルタ）の設計
// Added by JE3HHT on Aug.2010
void __fastcall 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;
}
}
}
}
//---------------------------------------------------------------------
// 周波数特性グラフ（フィルタスレッド内でコールしてはいけない）
//
// H(ejωT) = [Σ0]Hm cos(mωT) - j[Σ1]Hm sin(mω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; // 周波数表示のあるライン数
int DM; // 内部線の数
int MM; // 実線の間隔
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;
}
}
void DrawGraph2(Graphics::TBitmap *pBitmap, const double *H1, int Tap1, const double *H2, int Tap2, 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; // 周波数表示のあるライン数
int DM; // 内部線の数
int MM; // 実線の間隔
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( Tap1 ){
ra = im = 0.0;
for( k = 0; k <= Tap1; k++ ){
ra += H1[k] * cos(pi2t*f*k);
if( k ) im -= H1[k] * sin(pi2t*f*k);
}
if( ra * im ){
g = sqrt(ra * ra + im * im);
}
else {
g = 0.0;
}
}
else {
g = 1.0;
}
if( Tap2 ){
ra = im = 0.0;
for( k = 0; k <= Tap2; k++ ){
ra += H2[k] * cos(pi2t*f*k);
if( k ) im -= H2[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;
}
}
//---------------------------------------------------------------------
// 周波数特性グラフ（フィルタスレッド内でコールしてはいけない）
//
//
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; // 周波数表示のあるライン数
int DM; // 内部線の数
int MM; // 実線の間隔
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;
}
}
//---------------------------------------------------------------------
// 周波数特性グラフ（フィルタスレッド内でコールしてはいけない）
//
//
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; // 周波数表示のあるライン数
int DM; // 内部線の数
int MM; // 実線の間隔
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-バターワース, 1-チェビシフ
// rp : 通過域のリップル
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 ){ // チェビシフ
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 ){ // チェビシフ
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 { // バターワース
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;
}
//*********************************************************************
// CINTPXY XYScope 用 ×2インタポーレータ処理クラス
//
CINTPXY2::CINTPXY2()
{
iir.MakeIIR(2800, DemSamp * 2.0, 14, 0, 0);
}
void __fastcall CINTPXY2::Do(double *p, double d)
{
*p++ = iir.Do(d);
*p = iir.Do(d);
}
#if 0
//*********************************************************************
// CINTPXY XYScope 用 ×4インタポーレータ処理クラス
//
CINTPXY4::CINTPXY4()
{
iir.MakeIIR(2800, DemSamp * 4.0, 24, 0, 0);
}
void __fastcall CINTPXY4::Do(double *p, double d)
{
*p++ = iir.Do(d);
*p++ = iir.Do(d);
*p++ = iir.Do(d);
*p = iir.Do(d);
}
//*********************************************************************
// CINTPXY XYScope 用 ×8インタポーレータ処理クラス
//
CINTPXY8::CINTPXY8()
{
iir.MakeIIR(2800, DemSamp * 8.0, 30, 0, 0);
}
void __fastcall CINTPXY8::Do(double *p, double d)
{
*p++ = iir.Do(d);
*p++ = iir.Do(d);
*p++ = iir.Do(d);
*p++ = iir.Do(d);
*p++ = iir.Do(d);
*p++ = iir.Do(d);
*p++ = iir.Do(d);
*p = iir.Do(d);
}
//*********************************************************************
// CINTPXYFIR XYScope 用 ×2インタポーレータ処理クラス
//
CINTPXY2FIR::CINTPXY2FIR()
{
memset(Z, 0, sizeof(Z));
::MakeFilter(H, 32, ffLPF, DemSamp * 2.0, 2800, 3000, 40, 1.7);
}
void CINTPXY2FIR::Do(double *dp, double d)
{
memcpy(Z, &Z[1], sizeof(double)*16);
Z[16] = d;
dp[0] = Z[0] * H[32];
dp[1] = Z[1] * H[31];
dp[0] += Z[1] * H[30];
dp[1] += Z[2] * H[29];
dp[0] += Z[2] * H[28];
dp[1] += Z[3] * H[27];
dp[0] += Z[3] * H[26];
dp[1] += Z[4] * H[25];
dp[0] += Z[4] * H[24];
dp[1] += Z[5] * H[23];
dp[0] += Z[5] * H[22];
dp[1] += Z[6] * H[21];
dp[0] += Z[6] * H[20];
dp[1] += Z[7] * H[19];
dp[0] += Z[7] * H[18];
dp[1] += Z[8] * H[17];
dp[0] += Z[8] * H[16];
dp[1] += Z[9] * H[15];
dp[0] += Z[9] * H[14];
dp[1] += Z[10] * H[13];
dp[0] += Z[10] * H[12];
dp[1] += Z[11] * H[11];
dp[0] += Z[11] * H[10];
dp[1] += Z[12] * H[9];
dp[0] += Z[12] * H[8];
dp[1] += Z[13] * H[7];
dp[0] += Z[13] * H[6];
dp[1] += Z[14] * H[5];
dp[0] += Z[14] * H[4];
dp[1] += Z[15] * H[3];
dp[0] += Z[15] * H[2];
dp[1] += Z[16] * H[1];
dp[0] += Z[16] * H[0];
}
#endif
//*********************************************************************
// CINTPXYFIR XYScope 用 ×4インタポーレータ処理クラス
//
CINTPXY4FIR::CINTPXY4FIR()
{
memset(Z, 0, sizeof(Z));
::MakeFilter(H, 64, ffLPF, DemSamp * 4.0, 2800, 3000, 60, 4.0);
}
void __fastcall CINTPXY4FIR::Do(double *dp, double d)
{
memcpy(Z, &Z[1], sizeof(double)*16);
Z[16] = d;
dp[0] = Z[0] * H[64];
dp[1] = Z[1] * H[63];
dp[2] = Z[1] * H[62];
dp[3] = Z[1] * H[61];
dp[0] += Z[1] * H[60];
dp[1] += Z[2] * H[59];
dp[2] += Z[2] * H[58];
dp[3] += Z[2] * H[57];
dp[0] += Z[2] * H[56];
dp[1] += Z[3] * H[55];
dp[2] += Z[3] * H[54];
dp[3] += Z[3] * H[53];
dp[0] += Z[3] * H[52];
dp[1] += Z[4] * H[51];
dp[2] += Z[4] * H[50];
dp[3] += Z[4] * H[49];
dp[0] += Z[4] * H[48];
dp[1] += Z[5] * H[47];
dp[2] += Z[5] * H[46];
dp[3] += Z[5] * H[45];
dp[0] += Z[5] * H[44];
dp[1] += Z[6] * H[43];
dp[2] += Z[6] * H[42];
dp[3] += Z[6] * H[41];
dp[0] += Z[6] * H[40];
dp[1] += Z[7] * H[39];
dp[2] += Z[7] * H[38];
dp[3] += Z[7] * H[37];
dp[0] += Z[7] * H[36];
dp[1] += Z[8] * H[35];
dp[2] += Z[8] * H[34];
dp[3] += Z[8] * H[33];
dp[0] += Z[8] * H[32];
dp[1] += Z[9] * H[31];
dp[2] += Z[9] * H[30];
dp[3] += Z[9] * H[29];
dp[0] += Z[9] * H[28];
dp[1] += Z[10] * H[27];
dp[2] += Z[10] * H[26];
dp[3] += Z[10] * H[25];
dp[0] += Z[10] * H[24];
dp[1] += Z[11] * H[23];
dp[2] += Z[11] * H[22];
dp[3] += Z[11] * H[21];
dp[0] += Z[11] * H[20];
dp[1] += Z[12] * H[19];
dp[2] += Z[12] * H[18];
dp[3] += Z[12] * H[17];
dp[0] += Z[12] * H[16];
dp[1] += Z[12] * H[15];
dp[2] += Z[12] * H[14];
dp[3] += Z[12] * H[13];
dp[0] += Z[12] * H[12];
dp[1] += Z[12] * H[11];
dp[2] += Z[12] * H[10];
dp[3] += Z[12] * H[9];
dp[0] += Z[12] * H[8];
dp[1] += Z[12] * H[7];
dp[2] += Z[12] * H[6];
dp[3] += Z[12] * H[5];
dp[0] += Z[12] * H[4];
dp[1] += Z[12] * H[3];
dp[2] += Z[12] * H[2];
dp[3] += Z[12] * H[1];
dp[0] += Z[12] * H[0];
}
//*********************************************************************
// CINTPXYFIR XYScope 用 ×8インタポーレータ処理クラス
//
CINTPXY8FIR::CINTPXY8FIR()
{
memset(Z, 0, sizeof(Z));
::MakeFilter(H, 96, ffLPF, DemSamp * 8.0, 2800, 3000, 60, 8.0);
}
void __fastcall CINTPXY8FIR::Do(double *dp, double d)
{
memcpy(Z, &Z[1], sizeof(double)*12);
Z[12] = d;
dp[0] = Z[0] * H[96];
dp[1] = Z[1] * H[95];
dp[2] = Z[1] * H[94];
dp[3] = Z[1] * H[93];
dp[4] = Z[1] * H[92];
dp[5] = Z[1] * H[91];
dp[6] = Z[1] * H[90];
dp[7] = Z[1] * H[89];
dp[0] += Z[1] * H[88];
dp[1] += Z[2] * H[87];
dp[2] += Z[2] * H[86];
dp[3] += Z[2] * H[85];
dp[4] += Z[2] * H[84];
dp[5] += Z[2] * H[83];
dp[6] += Z[2] * H[82];
dp[7] += Z[2] * H[81];
dp[0] += Z[2] * H[80];
dp[1] += Z[3] * H[79];
dp[2] += Z[3] * H[78];
dp[3] += Z[3] * H[77];
dp[4] += Z[3] * H[76];
dp[5] += Z[3] * H[75];
dp[6] += Z[3] * H[74];
dp[7] += Z[3] * H[73];
dp[0] += Z[3] * H[72];
dp[1] += Z[4] * H[71];
dp[2] += Z[4] * H[70];
dp[3] += Z[4] * H[69];
dp[4] += Z[4] * H[68];
dp[5] += Z[4] * H[67];
dp[6] += Z[4] * H[66];
dp[7] += Z[4] * H[65];
dp[0] += Z[4] * H[64];
dp[1] += Z[5] * H[63];
dp[2] += Z[5] * H[62];
dp[3] += Z[5] * H[61];
dp[4] += Z[5] * H[60];
dp[5] += Z[5] * H[59];
dp[6] += Z[5] * H[58];
dp[7] += Z[5] * H[57];
dp[0] += Z[5] * H[56];
dp[1] += Z[6] * H[55];
dp[2] += Z[6] * H[54];
dp[3] += Z[6] * H[53];
dp[4] += Z[6] * H[52];
dp[5] += Z[6] * H[51];
dp[6] += Z[6] * H[50];
dp[7] += Z[6] * H[49];
dp[0] += Z[6] * H[48];
dp[1] += Z[7] * H[47];
dp[2] += Z[7] * H[46];
dp[3] += Z[7] * H[45];
dp[4] += Z[7] * H[44];
dp[5] += Z[7] * H[43];
dp[6] += Z[7] * H[42];
dp[7] += Z[7] * H[41];
dp[0] += Z[7] * H[40];
dp[1] += Z[8] * H[39];
dp[2] += Z[8] * H[38];
dp[3] += Z[8] * H[37];
dp[4] += Z[8] * H[36];
dp[5] += Z[8] * H[35];
dp[6] += Z[8] * H[34];
dp[7] += Z[8] * H[33];
dp[0] += Z[8] * H[32];
dp[1] += Z[9] * H[31];
dp[2] += Z[9] * H[30];
dp[3] += Z[9] * H[29];
dp[4] += Z[9] * H[28];
dp[5] += Z[9] * H[27];
dp[6] += Z[9] * H[26];
dp[7] += Z[9] * H[25];
dp[0] += Z[9] * H[24];
dp[1] += Z[10] * H[23];
dp[2] += Z[10] * H[22];
dp[3] += Z[10] * H[21];
dp[4] += Z[10] * H[20];
dp[5] += Z[10] * H[19];
dp[6] += Z[10] * H[18];
dp[7] += Z[10] * H[17];
dp[0] += Z[10] * H[16];
dp[1] += Z[11] * H[15];
dp[2] += Z[11] * H[14];
dp[3] += Z[11] * H[13];
dp[4] += Z[11] * H[12];
dp[5] += Z[11] * H[11];
dp[6] += Z[11] * H[10];
dp[7] += Z[11] * H[9];
dp[0] += Z[11] * H[8];
dp[1] += Z[12] * H[7];
dp[2] += Z[12] * H[6];
dp[3] += Z[12] * H[5];
dp[4] += Z[12] * H[4];
dp[5] += Z[12] * H[3];
dp[6] += Z[12] * H[2];
dp[7] += Z[12] * H[1];
dp[0] += Z[12] * H[0];
}
#if OVERFIR
//*********************************************************************
// CDECM4 1/4デシメータ処理クラス
//
CDECM4::CDECM4()
{
const double _mt[]={1.11, 1.25, 1.40, 1.11};
memset(Z1, 0, sizeof(Z1));
memset(Z2, 0, sizeof(Z2));
memset(Z3, 0, sizeof(Z3));
memset(Z4, 0, sizeof(Z4));
::MakeFilter(H, 80, ffLPF, SampFreq*4, 2800, 3000, 50, _mt[SampType]);
}
double __fastcall CDECM4::Do(double *dp)
{
memcpy(Z1, &Z1[1], sizeof(double)*20);
memcpy(Z2, &Z2[1], sizeof(double)*19);
memcpy(Z3, &Z3[1], sizeof(double)*19);
memcpy(Z4, &Z4[1], sizeof(double)*19);
#if 1
Z4[19] = *dp++;
Z3[19] = *dp++;
Z2[19] = *dp++;
Z1[20] = *dp;
#else
Z1[20] = *dp++;
Z2[19] = *dp++;
Z3[19] = *dp++;
Z4[19] = *dp;
#endif
double a;
a = Z1[0] * H[80];
a += Z4[0] * H[79];
a += Z3[0] * H[78];
a += Z2[0] * H[77];
a += Z1[1] * H[76];
a += Z4[1] * H[75];
a += Z3[1] * H[74];
a += Z2[1] * H[73];
a += Z1[2] * H[72];
a += Z4[2] * H[71];
a += Z3[2] * H[70];
a += Z2[2] * H[69];
a += Z1[3] * H[68];
a += Z4[3] * H[67];
a += Z3[3] * H[66];
a += Z2[3] * H[65];
a += Z1[4] * H[64];
a += Z4[4] * H[63];
a += Z3[4] * H[62];
a += Z2[4] * H[61];
a += Z1[5] * H[60];
a += Z4[5] * H[59];
a += Z3[5] * H[58];
a += Z2[5] * H[57];
a += Z1[6] * H[56];
a += Z4[6] * H[55];
a += Z3[6] * H[54];
a += Z2[6] * H[53];
a += Z1[7] * H[52];
a += Z4[7] * H[51];
a += Z3[7] * H[50];
a += Z2[7] * H[49];
a += Z1[8] * H[48];
a += Z4[8] * H[47];
a += Z3[8] * H[46];
a += Z2[8] * H[45];
a += Z1[9] * H[44];
a += Z4[9] * H[43];
a += Z3[9] * H[42];
a += Z2[9] * H[41];
a += Z1[10] * H[40];
a += Z4[10] * H[39];
a += Z3[10] * H[38];
a += Z2[10] * H[37];
a += Z1[11] * H[36];
a += Z4[11] * H[35];
a += Z3[11] * H[34];
a += Z2[11] * H[33];
a += Z1[12] * H[32];
a += Z4[12] * H[31];
a += Z3[12] * H[30];
a += Z2[12] * H[29];
a += Z1[13] * H[28];
a += Z4[13] * H[27];
a += Z3[13] * H[26];
a += Z2[13] * H[25];
a += Z1[14] * H[24];
a += Z4[14] * H[23];
a += Z3[14] * H[22];
a += Z2[14] * H[21];
a += Z1[15] * H[20];
a += Z4[15] * H[19];
a += Z3[15] * H[18];
a += Z2[15] * H[17];
a += Z1[16] * H[16];
a += Z4[16] * H[15];
a += Z3[16] * H[14];
a += Z2[16] * H[13];
a += Z1[17] * H[12];
a += Z4[17] * H[11];
a += Z3[17] * H[10];
a += Z2[17] * H[9];
a += Z1[18] * H[8];
a += Z4[18] * H[7];
a += Z3[18] * H[6];
a += Z2[18] * H[5];
a += Z1[19] * H[4];
a += Z4[19] * H[3];
a += Z3[19] * H[2];
a += Z2[19] * H[1];
a += Z1[20] * H[0];
return a;
}
//*********************************************************************
// CINTP4 x4インタポーレータ処理クラス
//
CINTP4::CINTP4()
{
memset(Z, 0, sizeof(Z));
::MakeFilter(H, 80, ffLPF, SampFreq*4, 2800, 3000, 50, 4.0);
}
void __fastcall CINTP4::Do(double *dp, double d)
{
memcpy(Z, &Z[1], sizeof(double)*20);
Z[20] = d;
dp[0] = Z[0] * H[80];
dp[1] = Z[1] * H[79];
dp[2] = Z[1] * H[78];
dp[3] = Z[1] * H[77];
dp[0] += Z[1] * H[76];
dp[1] += Z[2] * H[75];
dp[2] += Z[2] * H[74];
dp[3] += Z[2] * H[73];
dp[0] += Z[2] * H[72];
dp[1] += Z[3] * H[71];
dp[2] += Z[3] * H[70];
dp[3] += Z[3] * H[69];
dp[0] += Z[3] * H[68];
dp[1] += Z[4] * H[67];
dp[2] += Z[4] * H[66];
dp[3] += Z[4] * H[65];
dp[0] += Z[4] * H[64];
dp[1] += Z[5] * H[63];
dp[2] += Z[5] * H[62];
dp[3] += Z[5] * H[61];
dp[0] += Z[5] * H[60];
dp[1] += Z[6] * H[59];
dp[2] += Z[6] * H[58];
dp[3] += Z[6] * H[57];
dp[0] += Z[6] * H[56];
dp[1] += Z[7] * H[55];
dp[2] += Z[7] * H[54];
dp[3] += Z[7] * H[53];
dp[0] += Z[7] * H[52];
dp[1] += Z[8] * H[51];
dp[2] += Z[8] * H[50];
dp[3] += Z[8] * H[49];
dp[0] += Z[8] * H[48];
dp[1] += Z[9] * H[47];
dp[2] += Z[9] * H[46];
dp[3] += Z[9] * H[45];
dp[0] += Z[9] * H[44];
dp[1] += Z[10] * H[43];
dp[2] += Z[10] * H[42];
dp[3] += Z[10] * H[41];
dp[0] += Z[10] * H[40];
dp[1] += Z[11] * H[39];
dp[2] += Z[11] * H[38];
dp[3] += Z[11] * H[37];
dp[0] += Z[11] * H[36];
dp[1] += Z[12] * H[35];
dp[2] += Z[12] * H[34];
dp[3] += Z[12] * H[33];
dp[0] += Z[12] * H[32];
dp[1] += Z[13] * H[31];
dp[2] += Z[13] * H[30];
dp[3] += Z[13] * H[29];
dp[0] += Z[13] * H[28];
dp[1] += Z[14] * H[27];
dp[2] += Z[14] * H[26];
dp[3] += Z[14] * H[25];
dp[0] += Z[14] * H[24];
dp[1] += Z[15] * H[23];
dp[2] += Z[15] * H[22];
dp[3] += Z[15] * H[21];
dp[0] += Z[15] * H[20];
dp[1] += Z[16] * H[19];
dp[2] += Z[16] * H[18];
dp[3] += Z[16] * H[17];
dp[0] += Z[16] * H[16];
dp[1] += Z[17] * H[15];
dp[2] += Z[17] * H[14];
dp[3] += Z[17] * H[13];
dp[0] += Z[17] * H[12];
dp[1] += Z[18] * H[11];
dp[2] += Z[18] * H[10];
dp[3] += Z[18] * H[9];
dp[0] += Z[18] * H[8];
dp[1] += Z[19] * H[7];
dp[2] += Z[19] * H[6];
dp[3] += Z[19] * H[5];
dp[0] += Z[19] * H[4];
dp[1] += Z[20] * H[3];
dp[2] += Z[20] * H[2];
dp[3] += Z[20] * H[1];
dp[0] += Z[20] * H[0];
}
#endif
//*********************************************************************
// COVERLIMIT 高品質リミッタ
//
COVERLIMIT::COVERLIMIT()
{
#if !OVERFIR
iira.MakeIIR(2800, SampFreq * 4.0, 24, 0, 0);
iirb.MakeIIR(2800, SampFreq * 4.0, 24, 0, 0);
#endif
}
double COVERLIMIT::Do(double d, double lmt)
{
#if OVERFIR
double dt[4];
intp.Do(dt, d);
dt[0] *= lmt;
if( dt[0] > 16384.0 ) dt[0] = 16384.0;
if( dt[0] < -16384.0 ) dt[0] = -16384.0;
dt[1] *= lmt;
if( dt[1] > 16384.0 ) dt[1] = 16384.0;
if( dt[1] < -16384.0 ) dt[1] = -16384.0;
dt[2] *= lmt;
if( dt[2] > 16384.0 ) dt[2] = 16384.0;
if( dt[2] < -16384.0 ) dt[2] = -16384.0;
dt[3] *= lmt;
if( dt[3] > 16384.0 ) dt[3] = 16384.0;
if( dt[3] < -16384.0 ) dt[3] = -16384.0;
return decm.Do(dt);
#else
double d1, d2, d3, d4;
// X4 インターポーレータ
d1 = iira.Do(d);
d2 = iira.Do(d);
d3 = iira.Do(d);
d4 = iira.Do(d);
d1 *= lmt;
if( d1 > 16384.0 ) d1 = 16384.0;
if( d1 < -16384.0 ) d1 = -16384.0;
d2 *= lmt;
if( d2 > 16384.0 ) d2 = 16384.0;
if( d2 < -16384.0 ) d2 = -16384.0;
d3 *= lmt;
if( d3 > 16384.0 ) d3 = 16384.0;
if( d3 < -16384.0 ) d3 = -16384.0;
d4 *= lmt;
if( d4 > 16384.0 ) d4 = 16384.0;
if( d4 < -16384.0 ) d4 = -16384.0;
// 1/4 デシメータ
iirb.Do(d1);
iirb.Do(d2);
iirb.Do(d3);
return iirb.Do(d4);
#endif
}
//*********************************************************************
// CDECM2IIR 1/2デシメータ処理クラス
//
CDECM2IIR::CDECM2IIR()
{
iir.MakeIIR(2900, SampFreq, 12, 0, 0);
}
double CDECM2IIR::Do(double d1, double d2)
{
iir.Do(d2);
return iir.Do(d1);
}
#if 0
//*********************************************************************
// CHILL ヒルベルト変換クラス
//
CHILL::CHILL()
{
memset(Z, 0, sizeof(Z));
m_tap = 8;
m_htap = m_tap / 2;
m_CenterFreq = 2125 + 170*0.5;
SetSampFreq(SampFreq);
SetFreq(2125, 2125+170);
}
void CHILL::SetSampFreq(double samp)
{
m_SampFreq = samp;
m_OUT = 32768.0 * m_SampFreq / (2 * PI * 800);
m_OFF = (2 * PI * m_CenterFreq) / m_SampFreq;
}
void CHILL::SetFreq(double mark, double space)
{
m_MarkFreq = mark;
m_SpaceFreq = space;
m_CenterFreq = (mark + space) * 0.5;
m_OFF = (2 * PI * m_CenterFreq) / m_SampFreq;
MakeFilter(H, m_tap, ffLPF, m_SampFreq, m_CenterFreq, m_CenterFreq, 6, 1.0);
H[m_htap] = 0;
for( int i = 0; i < m_htap; i++ ){
H[i] = -H[i];
}
m_A[0] = m_A[1] = m_A[2] = m_A[3] = 0;
m_ph = &Z[m_htap];
}
void CHILL::SetMarkFreq(double mark)
{
SetFreq(mark, m_SpaceFreq);
}
void CHILL::SetSpaceFreq(double space)
{
SetFreq(m_MarkFreq, space);
}
double CHILL::Do(double d)
{
d = DoFIR(H, Z, d, m_tap);
double a = *m_ph;
if( a ) a = atan2(d, a);
d = a - m_A[0];
m_A[0] = a;
if( d >= PI ){
d = d - PI*2;
}
else if( d <= -PI ){
d = d + PI*2;
}
d += m_OFF;
return d * m_OUT;
}
#endif
Reference in a new issue View git blame Copy permalink