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/Fft.cpp

872 lines
21 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 "fft.h"
#include "ComLib.h"
#define PI 3.1415926535897932384626433832795
#define PI2 (2*PI)
#define SCALEADJ_1 (-5.5)
#define SCALEADJ_2 (-6.5)
#define SCALEADJ_3 (-7.8)
#define SCALEADJ_4 (-9.0)
#define LOGADJ (2.81458e4)
#define SCALEPOW_1 (1.0/4.0)
#define SCALEPOW_ADJ_1 0.0018
#define SCALEPOW_2 (1.0/5.0)
#define SCALEPOW_ADJ_2 0.008
#define SCALEPOW_3 (1.0/6.0)
#define SCALEPOW_ADJ_3 0.022
#define SCALEPOW_4 (1.0/7.0)
#define SCALEPOW_ADJ_4 0.048
//-------------------------------------------------
// <20>e<EFBFBD>e<EFBFBD>s<EFBFBD><73><EFBFBD><EFBFBD><EFBFBD>N<EFBFBD><4E><EFBFBD>X
CFFT::CFFT()
{
m_FFTDIS = 0;
memset(m_fft, 0, sizeof(m_fft));
m_CollectFFT = 1;
m_CollectrPage = m_CollectwPage = 0;
m_CollectFFTCount = 0;
m_tWindow = new double[FFT_BUFSIZE];
m_tSinCos = new double[FFT_BUFSIZE/2];
m_Work = new int[SQRT_FFT_SIZE+2];
memset(m_Work, 0, sizeof(int[SQRT_FFT_SIZE+2]));
pStgBuf = new double[FFT_BUFSIZE];
m_Work[0] = 0;
makewt(FFT_SIZE/4, m_Work, m_tSinCos);
makect(FFT_SIZE/4, m_Work, m_tSinCos + m_Work[0]);
for(int i = 0; i < FFT_SIZE; i++){
pStgBuf[i] = 1.0;
m_tWindow[i] = (0.5 - 0.5*cos( (PI2*i)/(FFT_SIZE-1) )); //<2F>n<EFBFBD>j<EFBFBD><6A><EFBFBD>O<EFBFBD><4F>
// m_tWindow[i] = 1.0;
}
m_StgSize = 1;
m_StgScale = 1.0;
m_StgK = 0.0;
m_StgSW = FALSE;
m_CollectFlag = 0;
m_Handle = NULL;
m_CollectCount = 0;
m_CollectFFT = 0;
m_fqc.SetSampFreq(FFTSamp);
m_fqc.m_Limit = 0;
m_fqc.m_outOrder = 2;
m_fqc.m_outFC = 400;
m_fqc.CalcLPF();
}
//-------------------------------------------------
// <20>e<EFBFBD>e<EFBFBD>s<EFBFBD><73><EFBFBD><EFBFBD><EFBFBD>N<EFBFBD><4E><EFBFBD>X<EFBFBD>̍ď<CC8D><C48F><EFBFBD><EFBFBD><EFBFBD>
void CFFT::InitFFT(void)
{
m_CollectFFT = 0;
m_CollectFFTCount = 0;
m_CollectFFT = 0;
m_CollectFFTCount = 0;
memset(m_Work, 0, sizeof(int[SQRT_FFT_SIZE+2]));
makewt(FFT_SIZE/4, m_Work, m_tSinCos);
makect(FFT_SIZE/4, m_Work, m_tSinCos + m_Work[0]);
for(int i = 0; i < FFT_SIZE; i++){
pStgBuf[i] = 1.0;
// m_tWindow[i] = 1.0;
// m_tWindow[i] = (0.5 - 0.5*cos( (PI2*i)/(FFT_SIZE-1) )); //<2F>n<EFBFBD>j<EFBFBD><6A><EFBFBD>O<EFBFBD><4F>
m_tWindow[i] = (0.5 - 0.5*cos( (PI2*i)/FFT_SIZE )); //<2F>n<EFBFBD>j<EFBFBD><6A><EFBFBD>O<EFBFBD><4F>
// m_tWindow[i] = 0.46 - 0.5*cos( (PI2*i)/FFT_SIZE ) + 0.08*cos(2*PI2*i/FFT_SIZE); //<2F>n<EFBFBD>j<EFBFBD><6A><EFBFBD>O<EFBFBD><4F>
}
m_StgSize = 1;
m_StgScale = 1.0;
m_StgK = 0.0;
m_StgSW = FALSE;
m_CollectFlag = 0;
m_CollectCount = 0;
m_CollectFFT = 0;
}
//-------------------------------------------------
CFFT::~CFFT()
{
if(pStgBuf){
delete pStgBuf;
pStgBuf = NULL;
}
if(m_Work){
delete m_Work;
m_Work = NULL;
}
if(m_tSinCos){
delete m_tSinCos;
m_tSinCos = NULL;
}
if(m_tWindow){
delete m_tWindow;
m_tWindow = NULL;
}
}
//-------------------------------------------------
// <20>f<EFBFBD>[<5B>^<5E><><EFBFBD>W<EFBFBD>i<EFBFBD>X<EFBFBD><58><EFBFBD>b<EFBFBD>h<EFBFBD>O<EFBFBD>Ŏ<EFBFBD><C58E>s<EFBFBD><73><EFBFBD><EFBFBD><EFBFBD>j
void CFFT::TrigFFT(void)
{
m_CollectFFT = 1;
m_CollectFFTCount = 0;
m_CollectrPage = m_CollectwPage = 0;
m_CollectCount = 0;
m_fqc.m_fq = ZEROFQ;
m_CollectFFT = 0; // <20><><EFBFBD>̎<EFBFBD><CC8E>W<EFBFBD><57><EFBFBD>g<EFBFBD><67><EFBFBD>K
}
//-------------------------------------------------
// <20>f<EFBFBD>[<5B>^<5E><><EFBFBD>W<EFBFBD>i<EFBFBD>X<EFBFBD><58><EFBFBD>b<EFBFBD>h<EFBFBD><68><EFBFBD>Ŏ<EFBFBD><C58E>s<EFBFBD><73><EFBFBD><EFBFBD><EFBFBD>j
void CFFT::CollectFFT(double *lp, int dsize)
{
if( DisPaint || m_CollectFFT ) return;
if( sys.m_FFTPriority ){
while( (m_CollectCount < 3) && dsize ){
double *tp;
int i;
int size;
switch(FFTSampType){
case 1:
size = dsize / 2;
if( (size + m_CollectFFTCount) > FFT_SIZE ){
size = FFT_SIZE - m_CollectFFTCount;
}
dsize -= (size * 2);
for( tp = &m_CollectFFTBuf[m_CollectwPage][m_CollectFFTCount], i = 0; i < size; i++, tp++, lp+=2 ){
*tp = *lp;
}
break;
case 2:
size = dsize / 4;
if( (size + m_CollectFFTCount) > FFT_SIZE ){
size = FFT_SIZE - m_CollectFFTCount;
}
dsize -= (size * 4);
for( tp = &m_CollectFFTBuf[m_CollectwPage][m_CollectFFTCount], i = 0; i < size; i++, tp++, lp+=4 ){
*tp = *lp;
}
break;
default:
size = dsize;
if( (size + m_CollectFFTCount) > FFT_SIZE ){
size = FFT_SIZE - m_CollectFFTCount;
}
dsize -= size;
memcpy(&m_CollectFFTBuf[m_CollectwPage][m_CollectFFTCount], lp, sizeof(double)*size);
lp += size;
break;
}
m_CollectFFTCount += size;
if( m_CollectFFTCount >= FFT_SIZE ){
int page = m_CollectwPage;
m_CollectwPage++;
if( m_CollectwPage >= 3 ) m_CollectwPage = 0;
m_CollectCount++;
switch(sys.m_FFTPriority){
case 1:
m_CollectFFTCount = 0;
break;
case 2:
m_CollectFFTCount = 128;
memcpy(m_CollectFFTBuf[m_CollectwPage], &m_CollectFFTBuf[page][FFT_SIZE-128], sizeof(double)*128);
break;
case 3:
m_CollectFFTCount = 256;
memcpy(m_CollectFFTBuf[m_CollectwPage], &m_CollectFFTBuf[page][FFT_SIZE-256], sizeof(double)*256);
break;
default:
m_CollectFFTCount = 512;
memcpy(m_CollectFFTBuf[m_CollectwPage], &m_CollectFFTBuf[page][FFT_SIZE-512], sizeof(double)*512);
break;
}
if( m_Handle != NULL ){
::PostMessage(m_Handle, CM_FFT, 0, 0);
}
}
}
}
else if( !m_CollectCount ){
double *tp;
int i;
int size;
switch(FFTSampType){
case 1:
size = dsize / 2;
if( (size + m_CollectFFTCount) > FFT_SIZE ){
size = FFT_SIZE - m_CollectFFTCount;
}
for( tp = &m_CollectFFTBuf[m_CollectwPage][m_CollectFFTCount], i = 0; i < size; i++, tp++, lp+=2 ){
*tp = *lp;
}
break;
case 2:
size = dsize / 4;
if( (size + m_CollectFFTCount) > FFT_SIZE ){
size = FFT_SIZE - m_CollectFFTCount;
}
for( tp = &m_CollectFFTBuf[m_CollectwPage][m_CollectFFTCount], i = 0; i < size; i++, tp++, lp+=4 ){
*tp = *lp;
}
break;
default:
size = dsize;
if( (size + m_CollectFFTCount) > FFT_SIZE ){
size = FFT_SIZE - m_CollectFFTCount;
}
memcpy(&m_CollectFFTBuf[m_CollectwPage][m_CollectFFTCount], lp, sizeof(double)*size);
break;
}
m_CollectFFTCount += size;
if( m_CollectFFTCount >= FFT_SIZE ){
m_CollectwPage++;
if( m_CollectwPage >= 3 ) m_CollectwPage = 0;
m_CollectCount++;
m_CollectFFTCount = 0;
}
}
}
//-------------------------------------------------
// <20>v<EFBFBD>Z<EFBFBD>i<EFBFBD>X<EFBFBD><58><EFBFBD>b<EFBFBD>h<EFBFBD>O<EFBFBD>Ŏ<EFBFBD><C58E>s<EFBFBD><73><EFBFBD><EFBFBD><EFBFBD>j
void CFFT::CalcFFT(int size, double gain, int stg)
{
if( sys.m_FFTType == 2 ){
CalcFQC(m_CollectFFTBuf[m_CollectrPage], size, gain, stg, m_fft);
}
else {
Calc(m_CollectFFTBuf[m_CollectrPage], size, gain, stg, m_fft);
}
m_CollectrPage++;
if( m_CollectrPage >= 3 ) m_CollectrPage = 0;
if( m_CollectCount ) m_CollectCount--;
}
void CFFT::makewt(int nw, int *ip, double *w)
{
int nwh, j;
double delta, x, y;
ip[0] = nw;
ip[1] = 1;
if(nw > 2){
nwh = nw >> 1;
delta = atan(1.0) / nwh;
w[0] = 1;
w[1] = 0;
w[nwh] = cos(delta * nwh);
w[nwh + 1] = w[nwh];
for(j = 2; j < nwh; j += 2){
x = cos(delta * j);
y = sin(delta * j);
w[j] = x;
w[j + 1] = y;
w[nw - j] = y;
w[nw - j + 1] = x;
}
bitrv2(nw, ip + 2, w);
}
}
//-------------------------------------------------
// <20>f<EFBFBD>[<5B>^<5E>̏<EFBFBD><CC8F><EFBFBD><EFBFBD><EFBFBD>
void CFFT::makect(int nc, int *ip, double *c)
{
int nch, j;
double delta;
ip[1] = nc;
if(nc > 1){
nch = nc >> 1;
delta = atan(1.0) / nch;
c[0] = cos(delta * nch);
c[nch] = 0.5 * c[0];
for(j = 1; j < nch; j++){
c[j] = 0.5 * cos(delta * j);
c[nc - j] = 0.5 * sin(delta * j);
}
}
}
//-------------------------------------------------
// <20>f<EFBFBD>[<5B>^<5E>̏<EFBFBD><CC8F><EFBFBD><EFBFBD><EFBFBD>
void CFFT::bitrv2(int n, int *ip, double *a)
{
int j, j1, k, k1, l, m, m2;
double xr, xi;
ip[0] = 0;
l = n;
m = 1;
while((m << 2) < l){
l >>= 1;
for (j = 0; j < m; j++) {
ip[m + j] = ip[j] + l;
}
m <<= 1;
}
if((m << 2) > l){
for (k = 1; k < m; k++) {
for (j = 0; j < k; j++) {
j1 = (j << 1) + ip[k];
k1 = (k << 1) + ip[j];
xr = a[j1];
xi = a[j1 + 1];
a[j1] = a[k1];
a[j1 + 1] = a[k1 + 1];
a[k1] = xr;
a[k1 + 1] = xi;
}
}
} else {
m2 = m << 1;
for(k = 1; k < m; k++){
for(j = 0; j < k; j++){
j1 = (j << 1) + ip[k];
k1 = (k << 1) + ip[j];
xr = a[j1];
xi = a[j1 + 1];
a[j1] = a[k1];
a[j1 + 1] = a[k1 + 1];
a[k1] = xr;
a[k1 + 1] = xi;
j1 += m2;
k1 += m2;
xr = a[j1];
xi = a[j1 + 1];
a[j1] = a[k1];
a[j1 + 1] = a[k1 + 1];
a[k1] = xr;
a[k1 + 1] = xi;
}
}
}
}
//-------------------------------------------------
void CFFT::cftfsub(int n, double *a, double *w)
{
int j, j1, j2, j3, l;
double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
l = 2;
if(n > 8){
cft1st(n, a, w);
l = 8;
while((l << 2) < n){
cftmdl(n, l, a, w);
l <<= 2;
}
}
if((l << 2) == n){
for(j = 0; j < l; j += 2){
j1 = j + l;
j2 = j1 + l;
j3 = j2 + l;
x0r = a[j] + a[j1];
x0i = a[j + 1] + a[j1 + 1];
x1r = a[j] - a[j1];
x1i = a[j + 1] - a[j1 + 1];
x2r = a[j2] + a[j3];
x2i = a[j2 + 1] + a[j3 + 1];
x3r = a[j2] - a[j3];
x3i = a[j2 + 1] - a[j3 + 1];
a[j] = x0r + x2r;
a[j + 1] = x0i + x2i;
a[j2] = x0r - x2r;
a[j2 + 1] = x0i - x2i;
a[j1] = x1r - x3i;
a[j1 + 1] = x1i + x3r;
a[j3] = x1r + x3i;
a[j3 + 1] = x1i - x3r;
}
} else {
for(j = 0; j < l; j += 2){
j1 = j + l;
x0r = a[j] - a[j1];
x0i = a[j + 1] - a[j1 + 1];
a[j] += a[j1];
a[j + 1] += a[j1 + 1];
a[j1] = x0r;
a[j1 + 1] = x0i;
}
}
}
//-------------------------------------------------
void CFFT::cft1st(int n, double *a, double *w)
{
int j, k1, k2;
double wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
x0r = a[0] + a[2];
x0i = a[1] + a[3];
x1r = a[0] - a[2];
x1i = a[1] - a[3];
x2r = a[4] + a[6];
x2i = a[5] + a[7];
x3r = a[4] - a[6];
x3i = a[5] - a[7];
a[0] = x0r + x2r;
a[1] = x0i + x2i;
a[4] = x0r - x2r;
a[5] = x0i - x2i;
a[2] = x1r - x3i;
a[3] = x1i + x3r;
a[6] = x1r + x3i;
a[7] = x1i - x3r;
wk1r = w[2];
x0r = a[8] + a[10];
x0i = a[9] + a[11];
x1r = a[8] - a[10];
x1i = a[9] - a[11];
x2r = a[12] + a[14];
x2i = a[13] + a[15];
x3r = a[12] - a[14];
x3i = a[13] - a[15];
a[8] = x0r + x2r;
a[9] = x0i + x2i;
a[12] = x2i - x0i;
a[13] = x0r - x2r;
x0r = x1r - x3i;
x0i = x1i + x3r;
a[10] = wk1r * (x0r - x0i);
a[11] = wk1r * (x0r + x0i);
x0r = x3i + x1r;
x0i = x3r - x1i;
a[14] = wk1r * (x0i - x0r);
a[15] = wk1r * (x0i + x0r);
k1 = 0;
for(j = 16; j < n; j += 16){
k1 += 2;
k2 = k1 << 1;
wk2r = w[k1];
wk2i = w[k1 + 1];
wk1r = w[k2];
wk1i = w[k2 + 1];
wk3r = wk1r - 2 * wk2i * wk1i;
wk3i = 2 * wk2i * wk1r - wk1i;
x0r = a[j] + a[j + 2];
x0i = a[j + 1] + a[j + 3];
x1r = a[j] - a[j + 2];
x1i = a[j + 1] - a[j + 3];
x2r = a[j + 4] + a[j + 6];
x2i = a[j + 5] + a[j + 7];
x3r = a[j + 4] - a[j + 6];
x3i = a[j + 5] - a[j + 7];
a[j] = x0r + x2r;
a[j + 1] = x0i + x2i;
x0r -= x2r;
x0i -= x2i;
a[j + 4] = wk2r * x0r - wk2i * x0i;
a[j + 5] = wk2r * x0i + wk2i * x0r;
x0r = x1r - x3i;
x0i = x1i + x3r;
a[j + 2] = wk1r * x0r - wk1i * x0i;
a[j + 3] = wk1r * x0i + wk1i * x0r;
x0r = x1r + x3i;
x0i = x1i - x3r;
a[j + 6] = wk3r * x0r - wk3i * x0i;
a[j + 7] = wk3r * x0i + wk3i * x0r;
wk1r = w[k2 + 2];
wk1i = w[k2 + 3];
wk3r = wk1r - 2 * wk2r * wk1i;
wk3i = 2 * wk2r * wk1r - wk1i;
x0r = a[j + 8] + a[j + 10];
x0i = a[j + 9] + a[j + 11];
x1r = a[j + 8] - a[j + 10];
x1i = a[j + 9] - a[j + 11];
x2r = a[j + 12] + a[j + 14];
x2i = a[j + 13] + a[j + 15];
x3r = a[j + 12] - a[j + 14];
x3i = a[j + 13] - a[j + 15];
a[j + 8] = x0r + x2r;
a[j + 9] = x0i + x2i;
x0r -= x2r;
x0i -= x2i;
a[j + 12] = -wk2i * x0r - wk2r * x0i;
a[j + 13] = -wk2i * x0i + wk2r * x0r;
x0r = x1r - x3i;
x0i = x1i + x3r;
a[j + 10] = wk1r * x0r - wk1i * x0i;
a[j + 11] = wk1r * x0i + wk1i * x0r;
x0r = x1r + x3i;
x0i = x1i - x3r;
a[j + 14] = wk3r * x0r - wk3i * x0i;
a[j + 15] = wk3r * x0i + wk3i * x0r;
}
}
//-------------------------------------------------
void CFFT::cftmdl(int n, int l, double *a, double *w)
{
int j, j1, j2, j3, k, k1, k2, m, m2;
double wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
double x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
m = l << 2;
for(j = 0; j < l; j += 2){
j1 = j + l;
j2 = j1 + l;
j3 = j2 + l;
x0r = a[j] + a[j1];
x0i = a[j + 1] + a[j1 + 1];
x1r = a[j] - a[j1];
x1i = a[j + 1] - a[j1 + 1];
x2r = a[j2] + a[j3];
x2i = a[j2 + 1] + a[j3 + 1];
x3r = a[j2] - a[j3];
x3i = a[j2 + 1] - a[j3 + 1];
a[j] = x0r + x2r;
a[j + 1] = x0i + x2i;
a[j2] = x0r - x2r;
a[j2 + 1] = x0i - x2i;
a[j1] = x1r - x3i;
a[j1 + 1] = x1i + x3r;
a[j3] = x1r + x3i;
a[j3 + 1] = x1i - x3r;
}
wk1r = w[2];
for(j = m; j < l + m; j += 2){
j1 = j + l;
j2 = j1 + l;
j3 = j2 + l;
x0r = a[j] + a[j1];
x0i = a[j + 1] + a[j1 + 1];
x1r = a[j] - a[j1];
x1i = a[j + 1] - a[j1 + 1];
x2r = a[j2] + a[j3];
x2i = a[j2 + 1] + a[j3 + 1];
x3r = a[j2] - a[j3];
x3i = a[j2 + 1] - a[j3 + 1];
a[j] = x0r + x2r;
a[j + 1] = x0i + x2i;
a[j2] = x2i - x0i;
a[j2 + 1] = x0r - x2r;
x0r = x1r - x3i;
x0i = x1i + x3r;
a[j1] = wk1r * (x0r - x0i);
a[j1 + 1] = wk1r * (x0r + x0i);
x0r = x3i + x1r;
x0i = x3r - x1i;
a[j3] = wk1r * (x0i - x0r);
a[j3 + 1] = wk1r * (x0i + x0r);
}
k1 = 0;
m2 = m << 1;
for(k = m2; k < n; k += m2){
k1 += 2;
k2 = k1 << 1;
wk2r = w[k1];
wk2i = w[k1 + 1];
wk1r = w[k2];
wk1i = w[k2 + 1];
wk3r = wk1r - 2 * wk2i * wk1i;
wk3i = 2 * wk2i * wk1r - wk1i;
for(j = k; j < l + k; j += 2){
j1 = j + l;
j2 = j1 + l;
j3 = j2 + l;
x0r = a[j] + a[j1];
x0i = a[j + 1] + a[j1 + 1];
x1r = a[j] - a[j1];
x1i = a[j + 1] - a[j1 + 1];
x2r = a[j2] + a[j3];
x2i = a[j2 + 1] + a[j3 + 1];
x3r = a[j2] - a[j3];
x3i = a[j2 + 1] - a[j3 + 1];
a[j] = x0r + x2r;
a[j + 1] = x0i + x2i;
x0r -= x2r;
x0i -= x2i;
a[j2] = wk2r * x0r - wk2i * x0i;
a[j2 + 1] = wk2r * x0i + wk2i * x0r;
x0r = x1r - x3i;
x0i = x1i + x3r;
a[j1] = wk1r * x0r - wk1i * x0i;
a[j1 + 1] = wk1r * x0i + wk1i * x0r;
x0r = x1r + x3i;
x0i = x1i - x3r;
a[j3] = wk3r * x0r - wk3i * x0i;
a[j3 + 1] = wk3r * x0i + wk3i * x0r;
}
wk1r = w[k2 + 2];
wk1i = w[k2 + 3];
wk3r = wk1r - 2 * wk2r * wk1i;
wk3i = 2 * wk2r * wk1r - wk1i;
for(j = k + m; j < l + (k + m); j += 2){
j1 = j + l;
j2 = j1 + l;
j3 = j2 + l;
x0r = a[j] + a[j1];
x0i = a[j + 1] + a[j1 + 1];
x1r = a[j] - a[j1];
x1i = a[j + 1] - a[j1 + 1];
x2r = a[j2] + a[j3];
x2i = a[j2 + 1] + a[j3 + 1];
x3r = a[j2] - a[j3];
x3i = a[j2 + 1] - a[j3 + 1];
a[j] = x0r + x2r;
a[j + 1] = x0i + x2i;
x0r -= x2r;
x0i -= x2i;
a[j2] = -wk2i * x0r - wk2r * x0i;
a[j2 + 1] = -wk2i * x0i + wk2r * x0r;
x0r = x1r - x3i;
x0i = x1i + x3r;
a[j1] = wk1r * x0r - wk1i * x0i;
a[j1 + 1] = wk1r * x0i + wk1i * x0r;
x0r = x1r + x3i;
x0i = x1i - x3r;
a[j3] = wk3r * x0r - wk3i * x0i;
a[j3 + 1] = wk3r * x0i + wk3i * x0r;
}
}
}
//---------------------------------------------------------------------------
void CFFT::Calc(double * InBuf, int size, double gain, int stg, int* OutBuf)
{
if( m_FFTDIS ) return; // for math error
m_FFTDIS++;
int i;
if( stg > 1 ){
m_StgSW = TRUE;
}
else {
m_StgSW = FALSE;
}
m_StgSize = stg;
if( stg ){
m_StgScale = 1.0 / double(m_StgSize);
m_StgK = 1.0 - m_StgScale;
}
else {
m_StgScale = 1.0;
m_StgK = 0.0;
}
double *dp = InBuf;
for(i=0; i < FFT_SIZE; i++, dp++){
if( *dp > 32768.0 ){
*dp = 32768.0;
}
else if( *dp < -32768.0 ){
*dp = -32768.0;
}
(*dp) *= m_tWindow[i];
}
bitrv2(FFT_SIZE, m_Work + 2, InBuf);
cftfsub(FFT_SIZE, InBuf, m_tSinCos);
rftfsub(FFT_SIZE, InBuf, m_Work[1], m_tSinCos + m_Work[0]);
for( i = 0, dp = pStgBuf; i <= size; i++, dp+=2 ){
OutBuf[i] = int((gain * (*dp)));
}
m_FFTDIS--;
}
//---------------------------------------------------------------------------
void CFFT::CalcFQC(double * InBuf, int size, double gain, int stg, int* OutBuf)
{
double *dp = InBuf;
double d;
int i, r;
memset(OutBuf, 0, sizeof(int)*size);
int ad, ad2, max;
switch(sys.m_FFTGain&3){
case 0:
ad = 30;
max = ad * 32;
break;
case 1:
ad = 30;
max = ad * 24;
break;
case 2:
ad = 40;
max = ad * 16;
break;
default:
ad = 50;
max = ad * 12;
break;
}
ad2 = ad / 2;
int *op;
size--;
for( i = 0; i < FFT_SIZE; i++, dp++ ){
d = m_fqc.Do(*dp);
d = 1900.0 - (d * 400 / 16384.0);
r = d * FFT_SIZE / FFTSamp;
if( (r >= 1) && (r < size) ){
op = &OutBuf[r];
if( *op < max ){
*op += ad;
*(op-1) += ad2;
*(op+1) += ad2;
}
}
}
}
//---------------------------------------------------------------------------
void CFFT::rftfsub(int n, double *a, int nc, double *c)
{
int j, k, kk, ks, m;
double wkr, wki, xr, xi, yr, yi;
double d;
ks = (nc << 2) / n;
kk = 0;
m = n >> 1;
j = n - 2;
if(m_StgSW){
for (k = 2; k <= m; k += 2, j -= 2 ){
kk += ks;
wkr = 0.5 - c[nc - kk];
wki = c[kk];
xr = a[k] - a[j];
xi = a[k + 1] + a[j + 1];
yr = wkr * xr - wki * xi;
yi = wkr * xi + wki * xr;
a[k] -= yr;
xi = a[k]*a[k];
a[k+1] -= yi;
xi += ( a[k+1]*a[k+1]);
a[j] += yr;
xr = a[j]*a[j];
a[j+1] -= yi;
xr += (a[j+1]*a[j+1]);
if( xi <= 0 ) xi = 0.0001;
if( xi >= 1e38 ) xi = 1e38;
if( xr <= 0 ) xr = 0.0001;
if( xr >= 1e38 ) xr = 1e38;
if( FFT_SIZE == 1024 ){
xi *= 4;
xr *= 4;
}
switch(sys.m_FFTGain){
case 0:
pStgBuf[k] = m_StgK*pStgBuf[k] + m_StgScale*(log10(xi+LOGADJ) + SCALEADJ_1);
pStgBuf[j] = m_StgK*pStgBuf[j] + m_StgScale*(log10(xr+LOGADJ) + SCALEADJ_1);
break;
case 1:
pStgBuf[k] = m_StgK*pStgBuf[k] + m_StgScale*(log10(xi+LOGADJ) + SCALEADJ_2);
pStgBuf[j] = m_StgK*pStgBuf[j] + m_StgScale*(log10(xr+LOGADJ) + SCALEADJ_2);
break;
case 2:
pStgBuf[k] = m_StgK*pStgBuf[k] + m_StgScale*(log10(xi+LOGADJ) + SCALEADJ_3);
pStgBuf[j] = m_StgK*pStgBuf[j] + m_StgScale*(log10(xr+LOGADJ) + SCALEADJ_3);
break;
case 3:
pStgBuf[k] = m_StgK*pStgBuf[k] + m_StgScale*(log10(xi+LOGADJ) + SCALEADJ_4);
pStgBuf[j] = m_StgK*pStgBuf[j] + m_StgScale*(log10(xr+LOGADJ) + SCALEADJ_4);
break;
case 4:
d = xi * 32e-10;
pStgBuf[k] = m_StgK*pStgBuf[k] + m_StgScale*d;
d = xr * 32e-10;
pStgBuf[j] = m_StgK*pStgBuf[j] + d;
break;
case 5:
d = xi * 96e-10;
pStgBuf[k] = m_StgK*pStgBuf[k] + m_StgScale*d;
d = xr * 96e-10;
pStgBuf[j] = m_StgK*pStgBuf[j] + d;
break;
case 6:
d = xi * 256e-10;
pStgBuf[k] = m_StgK*pStgBuf[k] + m_StgScale*d;
d = xr * 256e-10;
pStgBuf[j] = m_StgK*pStgBuf[j] + d;
break;
default:
d = xi * 512e-10;
pStgBuf[k] = m_StgK*pStgBuf[k] + m_StgScale*d;
d = xr * 512e-10;
pStgBuf[j] = m_StgK*pStgBuf[j] + d;
break;
}
}
}
else {
for (k = 2; k <= m; k += 2, j -= 2 ){
kk += ks;
wkr = 0.5 - c[nc - kk];
wki = c[kk];
xr = a[k] - a[j];
xi = a[k + 1] + a[j + 1];
yr = wkr * xr - wki * xi;
yi = wkr * xi + wki * xr;
a[k] -= yr;
xi = a[k]*a[k];
a[k+1] -= yi;
xi += ( a[k+1]*a[k+1]);
a[j] += yr;
xr = a[j]*a[j];
a[j+1] -= yi;
xr += (a[j+1]*a[j+1]);
if( xi <= 0 ) xi = 0.0001;
if( xi >= 1e38 ) xi = 1e38;
if( xr <= 0 ) xr = 0.0001;
if( xr >= 1e38 ) xr = 1e38;
if( FFT_SIZE == 1024 ){
xi *= 4;
xr *= 4;
}
switch(sys.m_FFTGain){
case 0:
pStgBuf[k] = log10(xi+LOGADJ)+SCALEADJ_1;
pStgBuf[j] = log10(xr+LOGADJ)+SCALEADJ_1;
break;
case 1:
pStgBuf[k] = log10(xi+LOGADJ)+SCALEADJ_2;
pStgBuf[j] = log10(xr+LOGADJ)+SCALEADJ_2;
break;
case 2:
pStgBuf[k] = log10(xi+LOGADJ)+SCALEADJ_3;
pStgBuf[j] = log10(xr+LOGADJ)+SCALEADJ_3;
break;
case 3:
pStgBuf[k] = log10(xi+LOGADJ)+SCALEADJ_4;
pStgBuf[j] = log10(xr+LOGADJ)+SCALEADJ_4;
break;
case 4:
pStgBuf[k] = xi * 32e-10;
pStgBuf[j] = xr * 32e-10;
break;
case 5:
pStgBuf[k] = xi * 96e-10;
pStgBuf[j] = xr * 96e-10;
break;
case 6:
pStgBuf[k] = xi * 256e-10;
pStgBuf[j] = xr * 256e-10;
break;
default:
pStgBuf[k] = xi * 512e-10;
pStgBuf[j] = xr * 512e-10;
break;
}
}
}
pStgBuf[FFT_SIZE/2] = pStgBuf[(FFT_SIZE/2) - 2];
}
Reference in a new issue Copy permalink