/*
** CZT計算用構造体に対し、標本数 n, 出力データ数 no 用の数表データを
** 作成する。
**
** cztp = CZT計算用構造体へのポインタ
** n = 標本点の数
** no = 出力するデータの数
** return = 0:正常終了 1:nが無効な数 2:メモリ不足
*/
int czt_init(czt_struct *cztp, int n, int no)
{
int i, nx;
if (n <= 1) return 1;
if (no <= 1 || n < no) no = n;
nx = n + no; /* (n + no)を2の整数乗まで拡張する(nx) */
for (i = 1; i < nx; i *= 2) ;
nx = i;
if (fft_init(&cztp->fft, nx) != 0) return 2;
cztp->samples = n;
cztp->samples_out = no;
cztp->samples_ex = nx;
cztp->wr = (REAL*)malloc(2 * n * sizeof(REAL));
cztp->vr = (REAL*)malloc(2 * nx * sizeof(REAL));
cztp->tr = (REAL*)malloc(2 * nx * sizeof(REAL));
if (cztp->wr == NULL || cztp->vr == NULL || cztp->tr == NULL) {
czt_end(cztp);
return 2;
}
cztp->wi = cztp->wr + n;
cztp->vi = cztp->vr + nx;
cztp->ti = cztp->tr + nx;
make_cztdata(n, no, nx, cztp->wr, cztp->wi, cztp->vr, cztp->vi);
fft(&cztp->fft, 0, cztp->vr, cztp->vi);
return 0;
}
int main(void)
{
static REAL r1[N], i1[N], r2[N], i2[N], r3[N], i3[N];
czt_struct cztd;
int i;
for (i = 0; i < N; i++) {
// r1[i] = r2[i] = 6 * sin(2*PI * i/N);
// r1[i] = r2[i] = 6 * cos(1*PI * i/N);
r1[i] = r2[i] = 9 * cos(2*2*PI * i/N) +
4 * sin(6*2*PI * i/N);
i1[i] = i2[i] = 0;
}
if (czt_init(&cztd, N, N) != 0) return EXIT_FAILURE;
/* 順変換 */
czt(&cztd, 0, r2, i2);
for (i = 0; i < N; i++) {
r3[i] = r2[i];
i3[i] = i2[i];
}
/* 逆変換 */
czt(&cztd, 1, r3, i3);
czt_end(&cztd);
printf("\n");
printf(" | サンプリングデータ | フーリエ変換 | フーリエ逆変換\n");
printf("-----+-----------------------+-----------------------+-----------------------\n");
for (i = 0; i < N; i++) {
printf("%4d | %10.5f %10.5f | %10.5f %10.5f | %10.5f %10.5f\n",
i+1, r1[i],i1[i], r2[i],i2[i], r3[i],i3[i]);
}
printf("-----+-----------------------+-----------------------+-----------------------\n");
printf("\n");
printf(" サンプリング波形\n");
plot_graph(N, r1);
printf("\n");
printf(" フーリエスペクトル(Re Axis)\n");
plot_graph(N, r2);
printf("\n");
printf(" フーリエスペクトル(Im Axis)\n");
plot_graph(N, i2);
printf("\n");
return EXIT_SUCCESS;
}
//基本周波数を求める
int estimatebasefreq(short *src, int length)
{
REAL *real, *imag, *autoc;
czt_struct cztd;
int i, index=1, pitch, error, half=length/2;
if (half > 530)
half = 530;
real = (REAL*)malloc(sizeof(REAL)*length);
imag = (REAL*)malloc(sizeof(REAL)*length);
autoc = (REAL*)malloc(sizeof(REAL)*length);
for (i = 0; i < length; i++) {
real[i] = src[i];
imag[i] = 0;
}
error = czt_init(&cztd, length, length);
if (error) {
printf("error:%d\n",error);
}
czt(&cztd, 0, real, imag);
//パワースペクトル化
for (i = 0; i < length; i++) {
real[i] = real[i]*real[i] + imag[i]*imag[i];
real[i] = pow(real[i], 1.0/3.0);
imag[i] = 0;
}
czt(&cztd, 1, real, imag); /* 逆変換して自己相関を求める */
czt_end(&cztd);
//負値をクリップさせる
for (i = 0; i < half; i++) {
if (real[i] < 0.0)
real[i] = 0.0;
imag[i] = real[i];
}
for (i = 0; i < half; i++)
if ((i % 2) == 0)
real[i] -= imag[i/2];
else
real[i] -= ((imag[i/2] + imag[i/2 + 1]) / 2);
//負値をクリップさせる
/*
for (i = 0; i < half; i++) {
if (real[i] < 0.0)
real[i] = 0.0;
}
*/
//ピッチ推定
for (i = 1; i < half; i++) {
if (real[i] > real[index]) {
index = i;
}
}
if ( index == 0) {
index = 1;
}
pitch = length/index;
free(real);
free(imag);
free(autoc);
return pitch;
}
