Tamura Mizuki / STM32F746_FFT

fft

Dependencies:   BSP_DISCO_F746NG SDFileSystem_Warning_Fixed

main.cpp@3:6f7e05dd8d15, 2020-03-06 (annotated)

Committer:
aria19970520
Date:
Fri Mar 06 06:52:14 2020 +0000
Revision:
3:6f7e05dd8d15
Parent:
2:0705bf3a3e1e 
fft

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MikamiUitOpen 0:3e46577dc273 1 //--------------------------------------------------------------
MikamiUitOpen 0:3e46577dc273 2 // SD カードのテキスト･ファイルの読み書きの例
MikamiUitOpen 0:3e46577dc273 3 //
MikamiUitOpen 0:3e46577dc273 4 // mount(), unmount() を使うことを除けば通常のテキストファイルの
MikamiUitOpen 0:3e46577dc273 5 // 読み書きと同じ．
MikamiUitOpen 0:3e46577dc273 6 //
MikamiUitOpen 2:0705bf3a3e1e 7 // 2016/11/14, Copyright (c) 2016 MIKAMI, Naoki
MikamiUitOpen 0:3e46577dc273 8 //--------------------------------------------------------------
MikamiUitOpen 0:3e46577dc273 9
MikamiUitOpen 0:3e46577dc273 10 #include "mbed.h"
MikamiUitOpen 0:3e46577dc273 11 #include "SDFileSystem.h" // SDFileSystem クラスを使うため
aria19970520 3:6f7e05dd8d15 12 #include <stdio.h>
aria19970520 3:6f7e05dd8d15 13 #include <math.h>
MikamiUitOpen 0:3e46577dc273 14
aria19970520 3:6f7e05dd8d15 15 #define N 1024 // FFTデータの大きさ 1024,2048,4096
aria19970520 3:6f7e05dd8d15 16 #define M 10 //DFT演算ステージ数 10,11,12
aria19970520 3:6f7e05dd8d15 17 #define PI 3.14159265359//円周率
aria19970520 3:6f7e05dd8d15 18
aria19970520 3:6f7e05dd8d15 19 Timer t;
aria19970520 3:6f7e05dd8d15 20 Ticker t2;
aria19970520 3:6f7e05dd8d15 21
aria19970520 3:6f7e05dd8d15 22 AnalogIn x(A5);
aria19970520 3:6f7e05dd8d15 23 AnalogIn y(A3);
aria19970520 3:6f7e05dd8d15 24 AnalogIn z(A4);
aria19970520 3:6f7e05dd8d15 25
aria19970520 3:6f7e05dd8d15 26 volatile int done = false;
aria19970520 3:6f7e05dd8d15 27 volatile int sampleIndex = 0;
aria19970520 3:6f7e05dd8d15 28
aria19970520 3:6f7e05dd8d15 29 float meas_x[N];
aria19970520 3:6f7e05dd8d15 30 float meas_y[N];
aria19970520 3:6f7e05dd8d15 31 float meas_z[N];
aria19970520 3:6f7e05dd8d15 32
aria19970520 3:6f7e05dd8d15 33 float meas_X[N];
aria19970520 3:6f7e05dd8d15 34 float meas_Y[N];
aria19970520 3:6f7e05dd8d15 35 float meas_Z[N];
aria19970520 3:6f7e05dd8d15 36
aria19970520 3:6f7e05dd8d15 37 float meas_XI[N];
aria19970520 3:6f7e05dd8d15 38 float meas_YI[N];
aria19970520 3:6f7e05dd8d15 39 float meas_ZI[N];
aria19970520 3:6f7e05dd8d15 40
aria19970520 3:6f7e05dd8d15 41 void FFT_x(float *Freq);
aria19970520 3:6f7e05dd8d15 42 void adc_xyz(void);
aria19970520 3:6f7e05dd8d15 43 void FFT_makeTABLE( int n , float *tS , float *tC);
aria19970520 3:6f7e05dd8d15 44 void bit_reverse(float *X, float *RX);
aria19970520 3:6f7e05dd8d15 45 void butterfly(float *XR,float *XI,float *tS,float *tC);
aria19970520 3:6f7e05dd8d15 46
aria19970520 3:6f7e05dd8d15 47 float rev_meas_X[N] ,rev_meas_XI[N];
aria19970520 3:6f7e05dd8d15 48 float rev_meas_Y[N] ,rev_meas_YI[N];
aria19970520 3:6f7e05dd8d15 49 float rev_meas_Z[N] ,rev_meas_ZI[N];
aria19970520 3:6f7e05dd8d15 50
aria19970520 3:6f7e05dd8d15 51
aria19970520 3:6f7e05dd8d15 52
MikamiUitOpen 0:3e46577dc273 53 SDFileSystem sd("sd"); // SDFileSystem: SD 用のクラス, 引数の文字列は任意
MikamiUitOpen 0:3e46577dc273 54
MikamiUitOpen 0:3e46577dc273 55 int main()
MikamiUitOpen 0:3e46577dc273 56 {
aria19970520 3:6f7e05dd8d15 57 Serial pc (SERIAL_TX, SERIAL_RX);
aria19970520 3:6f7e05dd8d15 58 pc.baud(115200);
aria19970520 3:6f7e05dd8d15 59
aria19970520 3:6f7e05dd8d15 60 float tSIN[ N/2 ] ,tCOS[ N/2 ];
aria19970520 3:6f7e05dd8d15 61 float Xydb[N],Yydb[N],Zydb[N];
aria19970520 3:6f7e05dd8d15 62 float freq[N];
aria19970520 3:6f7e05dd8d15 63 float off_v = 1.65;
aria19970520 3:6f7e05dd8d15 64 int i;
aria19970520 3:6f7e05dd8d15 65 char Tm[30];
aria19970520 3:6f7e05dd8d15 66 char Name[30];
aria19970520 3:6f7e05dd8d15 67
aria19970520 3:6f7e05dd8d15 68 FFT_x(freq);
aria19970520 3:6f7e05dd8d15 69
aria19970520 3:6f7e05dd8d15 70 printf("FFT : started\r\n");
aria19970520 3:6f7e05dd8d15 71
aria19970520 3:6f7e05dd8d15 72 time_t seconds = time(NULL) + (60 * 60 * 9); // JST
aria19970520 3:6f7e05dd8d15 73 struct tm *t = localtime(&seconds);
aria19970520 3:6f7e05dd8d15 74
aria19970520 3:6f7e05dd8d15 75
aria19970520 3:6f7e05dd8d15 76
aria19970520 3:6f7e05dd8d15 77 sprintf(Tm,"FFT_%04d%02d%02d_%02d%02d%02d",t->tm_year + 1900, t->tm_mon + 1, t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec);
aria19970520 3:6f7e05dd8d15 78 sprintf(Name,"/sd/%s.csv",Tm);
aria19970520 3:6f7e05dd8d15 79 printf("%s\n",Tm);
aria19970520 3:6f7e05dd8d15 80
aria19970520 3:6f7e05dd8d15 81 FFT_makeTABLE(N , tSIN , tCOS);
aria19970520 3:6f7e05dd8d15 82
aria19970520 3:6f7e05dd8d15 83 t2.attach_us(&adc_xyz,200);
aria19970520 3:6f7e05dd8d15 84 while (!done) {
aria19970520 3:6f7e05dd8d15 85 ;
aria19970520 3:6f7e05dd8d15 86 }
aria19970520 3:6f7e05dd8d15 87 t2.detach();
aria19970520 3:6f7e05dd8d15 88
aria19970520 3:6f7e05dd8d15 89 for(i=0;i<N;i++){
aria19970520 3:6f7e05dd8d15 90 meas_X[i] = meas_x[i] * 3.3f; // Converts value in the 0V-3.3V range
aria19970520 3:6f7e05dd8d15 91 meas_Y[i] = meas_y[i] * 3.3f;
aria19970520 3:6f7e05dd8d15 92 meas_Z[i] = meas_z[i] * 3.3f;
aria19970520 3:6f7e05dd8d15 93 }
aria19970520 3:6f7e05dd8d15 94 /*
aria19970520 3:6f7e05dd8d15 95 for(i=0;i<N;i++){
aria19970520 3:6f7e05dd8d15 96 meas_X[i] = meas_X[i] - off_v;
aria19970520 3:6f7e05dd8d15 97 meas_Y[i] = meas_Y[i] - off_v;
aria19970520 3:6f7e05dd8d15 98 meas_Z[i] = meas_Z[i] - off_v;
aria19970520 3:6f7e05dd8d15 99 }
aria19970520 3:6f7e05dd8d15 100 */
aria19970520 3:6f7e05dd8d15 101 butterfly(meas_X,meas_XI,tSIN,tCOS);
aria19970520 3:6f7e05dd8d15 102 bit_reverse(meas_X,rev_meas_X);
aria19970520 3:6f7e05dd8d15 103 bit_reverse(meas_XI,rev_meas_XI);
aria19970520 3:6f7e05dd8d15 104
aria19970520 3:6f7e05dd8d15 105 butterfly(meas_Y,meas_YI,tSIN,tCOS);
aria19970520 3:6f7e05dd8d15 106 bit_reverse(meas_Y,rev_meas_Y);
aria19970520 3:6f7e05dd8d15 107 bit_reverse(meas_YI,rev_meas_YI);
aria19970520 3:6f7e05dd8d15 108
aria19970520 3:6f7e05dd8d15 109 butterfly(meas_Z,meas_ZI,tSIN,tCOS);
aria19970520 3:6f7e05dd8d15 110 bit_reverse(meas_Z,rev_meas_Z);
aria19970520 3:6f7e05dd8d15 111 bit_reverse(meas_ZI,rev_meas_ZI);
aria19970520 3:6f7e05dd8d15 112
MikamiUitOpen 0:3e46577dc273 113 sd.mount(); // SD 用
MikamiUitOpen 0:3e46577dc273 114
MikamiUitOpen 1:7aa80a497ed2 115 // SD へ書き込み
aria19970520 3:6f7e05dd8d15 116 //sprintf(Name,"/sd/FFT_xyz.csv");
aria19970520 3:6f7e05dd8d15 117 FILE *fp = fopen(Name, "w");
MikamiUitOpen 0:3e46577dc273 118 if (fp == NULL)
MikamiUitOpen 0:3e46577dc273 119 {
MikamiUitOpen 0:3e46577dc273 120 fprintf(stderr, "Open error for writing!!\r\n");
MikamiUitOpen 0:3e46577dc273 121 while (true) {}
MikamiUitOpen 0:3e46577dc273 122 }
aria19970520 3:6f7e05dd8d15 123 fprintf(fp,"Freq,Xziku,Yziku,Zziku,XdB,YdB,ZdB\n");
aria19970520 3:6f7e05dd8d15 124
aria19970520 3:6f7e05dd8d15 125 for(i=0;i<N;i++){
aria19970520 3:6f7e05dd8d15 126 Xydb[i] = sqrt((rev_meas_X[i]*rev_meas_X[i]) + (rev_meas_XI[i]*rev_meas_XI[i])) / (N/2);
aria19970520 3:6f7e05dd8d15 127 Yydb[i] = sqrt((rev_meas_Y[i]*rev_meas_Y[i]) + (rev_meas_YI[i]*rev_meas_YI[i])) / (N/2);
aria19970520 3:6f7e05dd8d15 128 Zydb[i] = sqrt((rev_meas_Z[i]*rev_meas_Z[i]) + (rev_meas_ZI[i]*rev_meas_ZI[i])) / (N/2);
aria19970520 3:6f7e05dd8d15 129 fprintf(fp,"%f,%f,%f,%f,%f,%f,%f\n",freq[i],meas_X[i],meas_Y[i],meas_Z[i],Xydb[i],Yydb[i],Zydb[i]);
aria19970520 3:6f7e05dd8d15 130 }
MikamiUitOpen 0:3e46577dc273 131 fclose(fp);
aria19970520 3:6f7e05dd8d15 132
MikamiUitOpen 1:7aa80a497ed2 133 // SD から読み出し
aria19970520 3:6f7e05dd8d15 134 fp = fopen(Name, "r");
MikamiUitOpen 0:3e46577dc273 135 if (fp == NULL)
MikamiUitOpen 0:3e46577dc273 136 {
MikamiUitOpen 0:3e46577dc273 137 fprintf(stderr, "Open error for reading!!\r\n");
MikamiUitOpen 0:3e46577dc273 138 while (true) {}
MikamiUitOpen 0:3e46577dc273 139 }
MikamiUitOpen 0:3e46577dc273 140 while (true)
MikamiUitOpen 0:3e46577dc273 141 {
MikamiUitOpen 0:3e46577dc273 142 int chr = fgetc(fp);
MikamiUitOpen 0:3e46577dc273 143 if (chr == EOF) break;
MikamiUitOpen 0:3e46577dc273 144 printf("%c", chr);
MikamiUitOpen 0:3e46577dc273 145 if (chr == '\n') printf("\r");
MikamiUitOpen 0:3e46577dc273 146 }
aria19970520 3:6f7e05dd8d15 147
MikamiUitOpen 0:3e46577dc273 148 fclose(fp);
aria19970520 3:6f7e05dd8d15 149
MikamiUitOpen 0:3e46577dc273 150 sd.unmount(); // SD 用
MikamiUitOpen 2:0705bf3a3e1e 151
aria19970520 3:6f7e05dd8d15 152 printf("FFT : completed\n");
aria19970520 3:6f7e05dd8d15 153
aria19970520 3:6f7e05dd8d15 154 }
MikamiUitOpen 2:0705bf3a3e1e 155
aria19970520 3:6f7e05dd8d15 156 void FFT_x(float *Freq)
aria19970520 3:6f7e05dd8d15 157 {
aria19970520 3:6f7e05dd8d15 158 int a;
aria19970520 3:6f7e05dd8d15 159 float b;
aria19970520 3:6f7e05dd8d15 160 Freq[0] = 0;
aria19970520 3:6f7e05dd8d15 161 b = (float)1024/5000;
aria19970520 3:6f7e05dd8d15 162 for(a=1;a<N;a++){
aria19970520 3:6f7e05dd8d15 163 Freq[a] = (float)a/b;
MikamiUitOpen 0:3e46577dc273 164 }
MikamiUitOpen 0:3e46577dc273 165 }
aria19970520 3:6f7e05dd8d15 166
aria19970520 3:6f7e05dd8d15 167
aria19970520 3:6f7e05dd8d15 168 void adc_xyz()
aria19970520 3:6f7e05dd8d15 169 {
aria19970520 3:6f7e05dd8d15 170 meas_x[sampleIndex] = x.read();
aria19970520 3:6f7e05dd8d15 171 meas_y[sampleIndex] = y.read();
aria19970520 3:6f7e05dd8d15 172 meas_z[sampleIndex] = z.read();
aria19970520 3:6f7e05dd8d15 173
aria19970520 3:6f7e05dd8d15 174 if (sampleIndex >= N) {
aria19970520 3:6f7e05dd8d15 175 done = true;
aria19970520 3:6f7e05dd8d15 176 }
aria19970520 3:6f7e05dd8d15 177 sampleIndex++;
aria19970520 3:6f7e05dd8d15 178 }
aria19970520 3:6f7e05dd8d15 179
aria19970520 3:6f7e05dd8d15 180
aria19970520 3:6f7e05dd8d15 181 void FFT_makeTABLE( int n , float *tS , float *tC)
aria19970520 3:6f7e05dd8d15 182 {
aria19970520 3:6f7e05dd8d15 183 int i;
aria19970520 3:6f7e05dd8d15 184 float a;
aria19970520 3:6f7e05dd8d15 185 float b;
aria19970520 3:6f7e05dd8d15 186
aria19970520 3:6f7e05dd8d15 187 a=0.0;
aria19970520 3:6f7e05dd8d15 188 b= (PI * 2.0) / (float)N;
aria19970520 3:6f7e05dd8d15 189
aria19970520 3:6f7e05dd8d15 190 for( i=0 ; i<n/2 ; i++ ){
aria19970520 3:6f7e05dd8d15 191 tS[i] = sin(a);
aria19970520 3:6f7e05dd8d15 192 tC[i] = cos(a);
aria19970520 3:6f7e05dd8d15 193 a=a+b;
aria19970520 3:6f7e05dd8d15 194 }
aria19970520 3:6f7e05dd8d15 195
aria19970520 3:6f7e05dd8d15 196 }
aria19970520 3:6f7e05dd8d15 197
aria19970520 3:6f7e05dd8d15 198
aria19970520 3:6f7e05dd8d15 199 //bit逆順
aria19970520 3:6f7e05dd8d15 200 void bit_reverse(float *X, float *RX)
aria19970520 3:6f7e05dd8d15 201 {
aria19970520 3:6f7e05dd8d15 202 unsigned long m, k, j;
aria19970520 3:6f7e05dd8d15 203 j = 0;
aria19970520 3:6f7e05dd8d15 204 for (k = 0; k < N; k++) {
aria19970520 3:6f7e05dd8d15 205 RX[k] = X[j];
aria19970520 3:6f7e05dd8d15 206 m = N >> 1;
aria19970520 3:6f7e05dd8d15 207 while (m >= 1 && j >= m) {
aria19970520 3:6f7e05dd8d15 208 j -= m;
aria19970520 3:6f7e05dd8d15 209 m >>= 1;
aria19970520 3:6f7e05dd8d15 210 }
aria19970520 3:6f7e05dd8d15 211 j += m;
aria19970520 3:6f7e05dd8d15 212 }
aria19970520 3:6f7e05dd8d15 213
aria19970520 3:6f7e05dd8d15 214 }
aria19970520 3:6f7e05dd8d15 215
aria19970520 3:6f7e05dd8d15 216 //バタフライ演算
aria19970520 3:6f7e05dd8d15 217 void butterfly(float *XR,float *XI,float *tS,float *tC)
aria19970520 3:6f7e05dd8d15 218 {
aria19970520 3:6f7e05dd8d15 219 float a,b;
aria19970520 3:6f7e05dd8d15 220
aria19970520 3:6f7e05dd8d15 221 //バタフライ演算
aria19970520 3:6f7e05dd8d15 222 int G,L,H,K,P,I,J,Q;
aria19970520 3:6f7e05dd8d15 223
aria19970520 3:6f7e05dd8d15 224 L = N;
aria19970520 3:6f7e05dd8d15 225 H = 1;
aria19970520 3:6f7e05dd8d15 226 a = 0.0;
aria19970520 3:6f7e05dd8d15 227 b = 0.0;
aria19970520 3:6f7e05dd8d15 228
aria19970520 3:6f7e05dd8d15 229 for( G=1 ; G<=M ; G++ ){
aria19970520 3:6f7e05dd8d15 230 L = L/2;
aria19970520 3:6f7e05dd8d15 231 K = 0;
aria19970520 3:6f7e05dd8d15 232 for( Q=1 ; Q<=H; Q++ ){
aria19970520 3:6f7e05dd8d15 233 P = 0;
aria19970520 3:6f7e05dd8d15 234 for( I=K ; I<=(L+K-1) ; I++){
aria19970520 3:6f7e05dd8d15 235 J = I+L;
aria19970520 3:6f7e05dd8d15 236 a = XR[I] - XR[J];
aria19970520 3:6f7e05dd8d15 237 b = XI[I] - XI[J];
aria19970520 3:6f7e05dd8d15 238 XR[I] = XR[I] + XR[J];
aria19970520 3:6f7e05dd8d15 239 XI[I] = XI[I] + XI[J];
aria19970520 3:6f7e05dd8d15 240 if( P==0){
aria19970520 3:6f7e05dd8d15 241 XR[J] = a;
aria19970520 3:6f7e05dd8d15 242 XI[J] = b;
aria19970520 3:6f7e05dd8d15 243 }else{
aria19970520 3:6f7e05dd8d15 244 XR[J] = a * tC[P] + b * tS[P];
aria19970520 3:6f7e05dd8d15 245 XI[J] = b * tC[P] - a * tS[P];
aria19970520 3:6f7e05dd8d15 246 }
aria19970520 3:6f7e05dd8d15 247 P = P+H;
aria19970520 3:6f7e05dd8d15 248 }
aria19970520 3:6f7e05dd8d15 249 K = K +L +L;
aria19970520 3:6f7e05dd8d15 250 }
aria19970520 3:6f7e05dd8d15 251 H = H+H;
aria19970520 3:6f7e05dd8d15 252 }
aria19970520 3:6f7e05dd8d15 253 }