main.cpp

00001 //--------------------------------------------------------------
00002 //  SD カードのテキスト･ファイルの読み書きの例
00003 //
00004 //  mount(), unmount() を使うことを除けば通常のテキストファイルの
00005 //  読み書きと同じ．
00006 //
00007 //  2016/11/14, Copyright (c) 2016 MIKAMI, Naoki
00008 //--------------------------------------------------------------
00009 
00010 #include "mbed.h"
00011 #include "SDFileSystem.h"   // SDFileSystem クラスを使うため
00012 #include <stdio.h>
00013 #include <math.h>
00014 
00015 #define N 1024  // FFTデータの大きさ 1024,2048,4096
00016 #define M 10    //DFT演算ステージ数    10,11,12
00017 #define PI 3.14159265359//円周率
00018 
00019 Timer t;
00020 Ticker t2;
00021 
00022 AnalogIn x(A5);
00023 AnalogIn y(A3);
00024 AnalogIn z(A4);
00025 
00026 volatile int done = false;
00027 volatile int sampleIndex = 0;
00028 
00029 float meas_x[N];
00030 float meas_y[N];
00031 float meas_z[N];
00032 
00033 float meas_X[N];
00034 float meas_Y[N];
00035 float meas_Z[N];
00036 
00037 float meas_XI[N];
00038 float meas_YI[N];
00039 float meas_ZI[N];
00040 
00041 void FFT_x(float *Freq);
00042 void adc_xyz(void);
00043 void FFT_makeTABLE( int n , float *tS , float *tC);
00044 void bit_reverse(float *X, float *RX);
00045 void butterfly(float *XR,float *XI,float *tS,float *tC);
00046 
00047 float rev_meas_X[N] ,rev_meas_XI[N];
00048 float rev_meas_Y[N] ,rev_meas_YI[N];
00049 float rev_meas_Z[N] ,rev_meas_ZI[N];
00050 
00051 
00052 
00053 SDFileSystem sd("sd");      // SDFileSystem: SD 用のクラス, 引数の文字列は任意
00054 
00055 int main()
00056 {
00057     Serial pc (SERIAL_TX, SERIAL_RX);
00058     pc.baud(115200);
00059   
00060     float tSIN[ N/2 ] ,tCOS[ N/2 ];
00061     float Xydb[N],Yydb[N],Zydb[N];
00062     float freq[N];
00063     float off_v = 1.65;
00064     int i;
00065     char Tm[30];
00066     char Name[30];
00067     
00068     FFT_x(freq);
00069     
00070     printf("FFT : started\r\n");
00071     
00072     time_t seconds = time(NULL) + (60 * 60 * 9); // JST
00073     struct tm *t = localtime(&seconds);
00074  
00075     
00076       
00077     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);
00078     sprintf(Name,"/sd/%s.csv",Tm);
00079     printf("%s\n",Tm);
00080     
00081     FFT_makeTABLE(N , tSIN , tCOS);
00082     
00083     t2.attach_us(&adc_xyz,200);
00084     while (!done) {
00085         ;
00086     }
00087     t2.detach();
00088     
00089     for(i=0;i<N;i++){
00090           meas_X[i] = meas_x[i] * 3.3f; // Converts value in the 0V-3.3V range
00091           meas_Y[i] = meas_y[i] * 3.3f;
00092            meas_Z[i] = meas_z[i] * 3.3f;
00093     }
00094     /* 
00095     for(i=0;i<N;i++){
00096           meas_X[i] = meas_X[i] - off_v;
00097          meas_Y[i] = meas_Y[i] - off_v;
00098           meas_Z[i] = meas_Z[i] - off_v;
00099     }
00100     */
00101     butterfly(meas_X,meas_XI,tSIN,tCOS);
00102     bit_reverse(meas_X,rev_meas_X);
00103     bit_reverse(meas_XI,rev_meas_XI);
00104     
00105     butterfly(meas_Y,meas_YI,tSIN,tCOS);
00106     bit_reverse(meas_Y,rev_meas_Y);
00107     bit_reverse(meas_YI,rev_meas_YI);
00108     
00109     butterfly(meas_Z,meas_ZI,tSIN,tCOS);
00110     bit_reverse(meas_Z,rev_meas_Z);
00111     bit_reverse(meas_ZI,rev_meas_ZI);
00112     
00113     sd.mount(); // SD 用
00114 
00115     // SD へ書き込み
00116     //sprintf(Name,"/sd/FFT_xyz.csv");
00117     FILE *fp = fopen(Name, "w");
00118     if (fp == NULL)
00119     {
00120         fprintf(stderr, "Open error for writing!!\r\n");
00121         while (true) {}
00122     }
00123     fprintf(fp,"Freq,Xziku,Yziku,Zziku,XdB,YdB,ZdB\n");
00124     
00125     for(i=0;i<N;i++){
00126          Xydb[i] = sqrt((rev_meas_X[i]*rev_meas_X[i]) + (rev_meas_XI[i]*rev_meas_XI[i])) / (N/2);
00127          Yydb[i] = sqrt((rev_meas_Y[i]*rev_meas_Y[i]) + (rev_meas_YI[i]*rev_meas_YI[i])) / (N/2);
00128          Zydb[i] = sqrt((rev_meas_Z[i]*rev_meas_Z[i]) + (rev_meas_ZI[i]*rev_meas_ZI[i])) / (N/2);
00129          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]);
00130     }
00131     fclose(fp);
00132     
00133     // SD から読み出し
00134     fp = fopen(Name, "r");
00135     if (fp == NULL)
00136     {
00137         fprintf(stderr, "Open error for reading!!\r\n");
00138         while (true) {}
00139     }
00140     while (true)
00141     {
00142         int chr = fgetc(fp);
00143         if (chr == EOF) break;
00144         printf("%c", chr);
00145         if (chr == '\n') printf("\r");
00146     }
00147  
00148     fclose(fp);
00149  
00150     sd.unmount();    // SD 用
00151 
00152     printf("FFT : completed\n");
00153 
00154 }
00155 
00156 void FFT_x(float *Freq)
00157 {
00158     int a;
00159     float b;
00160     Freq[0] = 0;
00161     b = (float)1024/5000;
00162     for(a=1;a<N;a++){
00163         Freq[a] = (float)a/b;
00164     }
00165 }
00166 
00167 
00168 void adc_xyz()
00169 {
00170     meas_x[sampleIndex] = x.read();
00171     meas_y[sampleIndex] = y.read();
00172     meas_z[sampleIndex] = z.read();
00173 
00174     if (sampleIndex >= N) {
00175         done = true;
00176     }
00177     sampleIndex++;
00178 }
00179 
00180 
00181 void FFT_makeTABLE( int n , float *tS , float *tC)
00182 {
00183     int i;
00184     float a;
00185     float b;
00186 
00187     a=0.0;
00188     b= (PI * 2.0) / (float)N;
00189 
00190     for( i=0 ; i<n/2 ; i++ ){
00191         tS[i] = sin(a);
00192         tC[i] = cos(a);
00193         a=a+b;
00194     }
00195 
00196 }
00197 
00198 
00199 //bit逆順
00200 void bit_reverse(float *X, float *RX)
00201 {
00202     unsigned long m, k, j;
00203     j = 0;
00204     for (k = 0; k < N; k++) {
00205         RX[k] = X[j];
00206         m = N >> 1;
00207         while (m >= 1 && j >= m) {
00208             j -= m;
00209             m >>= 1;
00210         }
00211         j += m;
00212     }
00213 
00214 }
00215 
00216 //バタフライ演算
00217 void butterfly(float *XR,float *XI,float *tS,float *tC)
00218 {
00219     float a,b;
00220 
00221     //バタフライ演算
00222     int G,L,H,K,P,I,J,Q;
00223 
00224     L = N;
00225     H = 1;
00226     a = 0.0;
00227     b = 0.0;
00228 
00229     for( G=1 ; G<=M ; G++ ){
00230         L = L/2;
00231         K = 0;
00232         for( Q=1 ; Q<=H; Q++ ){
00233             P = 0;
00234             for( I=K ; I<=(L+K-1) ; I++){
00235                 J = I+L;
00236                 a = XR[I] - XR[J];
00237                 b = XI[I] - XI[J];
00238                 XR[I] = XR[I] + XR[J];
00239                 XI[I] = XI[I] + XI[J];
00240                 if( P==0){
00241                     XR[J] = a;
00242                     XI[J] = b;
00243                 }else{
00244                     XR[J] = a * tC[P] + b * tS[P];
00245                     XI[J] = b * tC[P] - a * tS[P];
00246                 }
00247                 P = P+H;
00248             }
00249             K = K +L +L;
00250         }
00251         H = H+H;
00252     }
00253 }