Diff: main.cpp

Revision:

3:6f7e05dd8d15

Parent:

2:0705bf3a3e1e

--- a/main.cpp	Mon Nov 14 10:08:40 2016 +0000
+++ b/main.cpp	Fri Mar 06 06:52:14 2020 +0000
@@ -9,38 +9,134 @@
 
 #include "mbed.h"
 #include "SDFileSystem.h"   // SDFileSystem クラスを使うため
+#include <stdio.h>
+#include <math.h>
 
-DigitalOut led_green(LED1);
+#define N 1024  // FFTデータの大きさ 1024,2048,4096
+#define M 10    //DFT演算ステージ数    10,11,12
+#define PI 3.14159265359//円周率
+
+Timer t;
+Ticker t2;
+
+AnalogIn x(A5);
+AnalogIn y(A3);
+AnalogIn z(A4);
+
+volatile int done = false;
+volatile int sampleIndex = 0;
+
+float meas_x[N];
+float meas_y[N];
+float meas_z[N];
+
+float meas_X[N];
+float meas_Y[N];
+float meas_Z[N];
+
+float meas_XI[N];
+float meas_YI[N];
+float meas_ZI[N];
+
+void FFT_x(float *Freq);
+void adc_xyz(void);
+void FFT_makeTABLE( int n , float *tS , float *tC);
+void bit_reverse(float *X, float *RX);
+void butterfly(float *XR,float *XI,float *tS,float *tC);
+
+float rev_meas_X[N] ,rev_meas_XI[N];
+float rev_meas_Y[N] ,rev_meas_YI[N];
+float rev_meas_Z[N] ,rev_meas_ZI[N];
+
+
+
 SDFileSystem sd("sd");      // SDFileSystem: SD 用のクラス, 引数の文字列は任意
 
 int main()
 {
-    printf("\r\nSD Text file write/read test: started\r\n\n");
-
+    Serial pc (SERIAL_TX, SERIAL_RX);
+    pc.baud(115200);
+  
+    float tSIN[ N/2 ] ,tCOS[ N/2 ];
+    float Xydb[N],Yydb[N],Zydb[N];
+    float freq[N];
+    float off_v = 1.65;
+    int i;
+    char Tm[30];
+    char Name[30];
+    
+    FFT_x(freq);
+    
+    printf("FFT : started\r\n");
+    
+    time_t seconds = time(NULL) + (60 * 60 * 9); // JST
+    struct tm *t = localtime(&seconds);
+ 
+    
+      
+    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);
+    sprintf(Name,"/sd/%s.csv",Tm);
+    printf("%s\n",Tm);
+    
+    FFT_makeTABLE(N , tSIN , tCOS);
+    
+    t2.attach_us(&adc_xyz,200);
+    while (!done) {
+        ;
+    }
+    t2.detach();
+    
+    for(i=0;i<N;i++){
+          meas_X[i] = meas_x[i] * 3.3f; // Converts value in the 0V-3.3V range
+          meas_Y[i] = meas_y[i] * 3.3f;
+           meas_Z[i] = meas_z[i] * 3.3f;
+    }
+    /* 
+    for(i=0;i<N;i++){
+          meas_X[i] = meas_X[i] - off_v;
+         meas_Y[i] = meas_Y[i] - off_v;
+          meas_Z[i] = meas_Z[i] - off_v;
+    }
+    */
+    butterfly(meas_X,meas_XI,tSIN,tCOS);
+    bit_reverse(meas_X,rev_meas_X);
+    bit_reverse(meas_XI,rev_meas_XI);
+    
+    butterfly(meas_Y,meas_YI,tSIN,tCOS);
+    bit_reverse(meas_Y,rev_meas_Y);
+    bit_reverse(meas_YI,rev_meas_YI);
+    
+    butterfly(meas_Z,meas_ZI,tSIN,tCOS);
+    bit_reverse(meas_Z,rev_meas_Z);
+    bit_reverse(meas_ZI,rev_meas_ZI);
+    
     sd.mount(); // SD 用
 
     // SD へ書き込み
-    FILE *fp = fopen("/sd/WriteReadTest.txt", "w");
+    //sprintf(Name,"/sd/FFT_xyz.csv");
+    FILE *fp = fopen(Name, "w");
     if (fp == NULL)
     {
         fprintf(stderr, "Open error for writing!!\r\n");
         while (true) {}
     }
-
-    fprintf(fp, "Hello!\n");
-    fprintf(fp, "Example of writing and reading of text file.\n");
+    fprintf(fp,"Freq,Xziku,Yziku,Zziku,XdB,YdB,ZdB\n");
+    
+    for(i=0;i<N;i++){
+         Xydb[i] = sqrt((rev_meas_X[i]*rev_meas_X[i]) + (rev_meas_XI[i]*rev_meas_XI[i])) / (N/2);
+         Yydb[i] = sqrt((rev_meas_Y[i]*rev_meas_Y[i]) + (rev_meas_YI[i]*rev_meas_YI[i])) / (N/2);
+         Zydb[i] = sqrt((rev_meas_Z[i]*rev_meas_Z[i]) + (rev_meas_ZI[i]*rev_meas_ZI[i])) / (N/2);
+         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]);
+    }
     fclose(fp);
-
+    
     // SD から読み出し
-    fp = fopen("/sd/WriteReadTest.txt", "r");
+    fp = fopen(Name, "r");
     if (fp == NULL)
     {
         fprintf(stderr, "Open error for reading!!\r\n");
         while (true) {}
     }
-
-    printf("String in read file is as follows:\r\n");
-
     while (true)
     {
         int chr = fgetc(fp);
@@ -48,15 +144,110 @@
         printf("%c", chr);
         if (chr == '\n') printf("\r");
     }
-
+ 
     fclose(fp);
+ 
     sd.unmount();    // SD 用
 
-    printf("\r\nSD Text file write/read test: completed\r\n");
+    printf("FFT : completed\n");
+
+}
 
-    while(true)
-    {
-        led_green = !led_green;
-        wait(0.2f);
+void FFT_x(float *Freq)
+{
+    int a;
+    float b;
+    Freq[0] = 0;
+    b = (float)1024/5000;
+    for(a=1;a<N;a++){
+        Freq[a] = (float)a/b;
     }
 }
+
+
+void adc_xyz()
+{
+    meas_x[sampleIndex] = x.read();
+    meas_y[sampleIndex] = y.read();
+    meas_z[sampleIndex] = z.read();
+
+    if (sampleIndex >= N) {
+        done = true;
+    }
+    sampleIndex++;
+}
+
+
+void FFT_makeTABLE( int n , float *tS , float *tC)
+{
+    int i;
+    float a;
+    float b;
+
+    a=0.0;
+    b= (PI * 2.0) / (float)N;
+
+    for( i=0 ; i<n/2 ; i++ ){
+        tS[i] = sin(a);
+        tC[i] = cos(a);
+        a=a+b;
+    }
+
+}
+
+
+//bit逆順
+void bit_reverse(float *X, float *RX)
+{
+    unsigned long m, k, j;
+    j = 0;
+    for (k = 0; k < N; k++) {
+        RX[k] = X[j];
+        m = N >> 1;
+        while (m >= 1 && j >= m) {
+            j -= m;
+            m >>= 1;
+        }
+        j += m;
+    }
+
+}
+
+//バタフライ演算
+void butterfly(float *XR,float *XI,float *tS,float *tC)
+{
+    float a,b;
+
+    //バタフライ演算
+    int G,L,H,K,P,I,J,Q;
+
+    L = N;
+    H = 1;
+    a = 0.0;
+    b = 0.0;
+
+    for( G=1 ; G<=M ; G++ ){
+        L = L/2;
+        K = 0;
+        for( Q=1 ; Q<=H; Q++ ){
+            P = 0;
+            for( I=K ; I<=(L+K-1) ; I++){
+                J = I+L;
+                a = XR[I] - XR[J];
+                b = XI[I] - XI[J];
+                XR[I] = XR[I] + XR[J];
+                XI[I] = XI[I] + XI[J];
+                if( P==0){
+                    XR[J] = a;
+                    XI[J] = b;
+                }else{
+                    XR[J] = a * tC[P] + b * tS[P];
+                    XI[J] = b * tC[P] - a * tS[P];
+                }
+                P = P+H;
+            }
+            K = K +L +L;
+        }
+        H = H+H;
+    }
+}