| User | Revision | Line number | New contents of line |
|---|
| ddobano |
0:e60134beb2ca
|
1
|
#include "mbed.h"
|
| ddobano |
0:e60134beb2ca
|
2
|
|
| ddobano |
0:e60134beb2ca
|
3
|
|
| ddobano |
0:e60134beb2ca
|
4
|
DigitalOut myled(LED1);
|
| ddobano |
0:e60134beb2ca
|
5
|
DigitalOut led(LED2);
|
| ddobano |
0:e60134beb2ca
|
6
|
DigitalOut led2(LED3);
|
| ddobano |
0:e60134beb2ca
|
7
|
DigitalOut led3(LED4);
|
| ddobano |
0:e60134beb2ca
|
8
|
const int N=1024; // Sample number
|
| ddobano |
0:e60134beb2ca
|
9
|
struct TWIDDLE { // Structure factor for calculate fft
|
| ddobano |
0:e60134beb2ca
|
10
|
|
| ddobano |
0:e60134beb2ca
|
11
|
float cosval [N/2];
|
| ddobano |
0:e60134beb2ca
|
12
|
float sinval [N/2];
|
| ddobano |
0:e60134beb2ca
|
13
|
|
| ddobano |
0:e60134beb2ca
|
14
|
};
|
| ddobano |
0:e60134beb2ca
|
15
|
void FFT(float datos[N], int n, TWIDDLE &w); //Declaration of functions
|
| ddobano |
0:e60134beb2ca
|
16
|
void enviodatos(float datos[N], int n);
|
| ddobano |
0:e60134beb2ca
|
17
|
Serial pc(USBTX, USBRX); //Declaration Serial port
|
| ddobano |
0:e60134beb2ca
|
18
|
|
| ddobano |
0:e60134beb2ca
|
19
|
int main() { //Main program
|
| ddobano |
0:e60134beb2ca
|
20
|
myled.write(0);
|
| ddobano |
0:e60134beb2ca
|
21
|
led.write(0);
|
| ddobano |
0:e60134beb2ca
|
22
|
led2.write(0);
|
| ddobano |
0:e60134beb2ca
|
23
|
led3.write(0);
|
| ddobano |
0:e60134beb2ca
|
24
|
int i; //Parameters to calculate the values of a sine function. DATOS[N]=Amp*sin(w0*t)
|
| ddobano |
0:e60134beb2ca
|
25
|
float arg;
|
| ddobano |
0:e60134beb2ca
|
26
|
arg=2*3.1416/N; //argument of the structure TWIDDLE
|
| ddobano |
0:e60134beb2ca
|
27
|
float tm,t,w0,T, Amp;
|
| ddobano |
0:e60134beb2ca
|
28
|
tm=0.1; // increments of time (0,1seg)
|
| ddobano |
0:e60134beb2ca
|
29
|
t=0; //initial time
|
| ddobano |
0:e60134beb2ca
|
30
|
T=2*3.1416; //Period of signal
|
| ddobano |
0:e60134beb2ca
|
31
|
w0=(2*3.1416/T); // angular frequency
|
| ddobano |
0:e60134beb2ca
|
32
|
Amp=3; //Amplitude
|
| ddobano |
0:e60134beb2ca
|
33
|
struct TWIDDLE W; //Declarate of a type structure
|
| ddobano |
0:e60134beb2ca
|
34
|
|
| ddobano |
0:e60134beb2ca
|
35
|
for (i=0; i<N/2; i++) { //Calculate all the TWIDDLE factors
|
| ddobano |
0:e60134beb2ca
|
36
|
W.cosval[i]= cos(i*arg);
|
| ddobano |
0:e60134beb2ca
|
37
|
W.sinval[i]= -sin(i*arg);
|
| ddobano |
0:e60134beb2ca
|
38
|
}
|
| ddobano |
0:e60134beb2ca
|
39
|
|
| ddobano |
0:e60134beb2ca
|
40
|
float DATOS[N]; //array to save the sinus values
|
| ddobano |
0:e60134beb2ca
|
41
|
for (i=0; i<N; i++) { //calculate the sinus values
|
| ddobano |
0:e60134beb2ca
|
42
|
DATOS[i]=Amp*sin(w0*t);
|
| ddobano |
0:e60134beb2ca
|
43
|
t+=tm;
|
| ddobano |
0:e60134beb2ca
|
44
|
}
|
| ddobano |
0:e60134beb2ca
|
45
|
led.write(1); //Execution of the program to here OK
|
| ddobano |
0:e60134beb2ca
|
46
|
FFT(DATOS,N,W); //Function to calculate FFT
|
| ddobano |
0:e60134beb2ca
|
47
|
enviodatos(DATOS,N); //Function to send values to serial port
|
| ddobano |
0:e60134beb2ca
|
48
|
led3.write(1); //Execution of the program to here OK
|
| ddobano |
0:e60134beb2ca
|
49
|
while (1); //
|
| ddobano |
0:e60134beb2ca
|
50
|
|
| ddobano |
0:e60134beb2ca
|
51
|
}
|
| ddobano |
0:e60134beb2ca
|
52
|
|
| ddobano |
0:e60134beb2ca
|
53
|
void FFT(float datos[N], int n,TWIDDLE &w) {
|
| ddobano |
0:e60134beb2ca
|
54
|
float temp1, temp2, temp3,temp4; //variables needed to calculate FFT
|
| ddobano |
0:e60134beb2ca
|
55
|
int i,j,k,p;
|
| ddobano |
0:e60134beb2ca
|
56
|
int upper_leg, lower_leg;
|
| ddobano |
0:e60134beb2ca
|
57
|
int leg_diff;
|
| ddobano |
0:e60134beb2ca
|
58
|
int num_stages=0;
|
| ddobano |
0:e60134beb2ca
|
59
|
int index, step;
|
| ddobano |
0:e60134beb2ca
|
60
|
float datos_real[n], datos_imag[n]; //real and imaginary values
|
| ddobano |
0:e60134beb2ca
|
61
|
for (p=0; p<n; p++) {
|
| ddobano |
0:e60134beb2ca
|
62
|
datos_real[p]=datos[p];
|
| ddobano |
0:e60134beb2ca
|
63
|
datos_imag[p]=0;
|
| ddobano |
0:e60134beb2ca
|
64
|
}
|
| ddobano |
0:e60134beb2ca
|
65
|
led2.write(1); // Execution of the program to here break it if N=>2048, else OK.
|
| ddobano |
0:e60134beb2ca
|
66
|
i=1;
|
| ddobano |
0:e60134beb2ca
|
67
|
do {
|
| ddobano |
0:e60134beb2ca
|
68
|
num_stages+=1;
|
| ddobano |
0:e60134beb2ca
|
69
|
i=i*2;
|
| ddobano |
0:e60134beb2ca
|
70
|
} while (i!=n);
|
| ddobano |
0:e60134beb2ca
|
71
|
|
| ddobano |
0:e60134beb2ca
|
72
|
leg_diff=n/2;
|
| ddobano |
0:e60134beb2ca
|
73
|
step=1;
|
| ddobano |
0:e60134beb2ca
|
74
|
|
| ddobano |
0:e60134beb2ca
|
75
|
for (i=0; i<num_stages; i++) {
|
| ddobano |
0:e60134beb2ca
|
76
|
index=0;
|
| ddobano |
0:e60134beb2ca
|
77
|
for (j=0; j<leg_diff; j++) {
|
| ddobano |
0:e60134beb2ca
|
78
|
for (upper_leg=j; upper_leg<n; upper_leg+=(2*leg_diff)) {
|
| ddobano |
0:e60134beb2ca
|
79
|
lower_leg=upper_leg+leg_diff;
|
| ddobano |
0:e60134beb2ca
|
80
|
|
| ddobano |
0:e60134beb2ca
|
81
|
temp1= (datos_real[upper_leg]+ datos_real[lower_leg]);
|
| ddobano |
0:e60134beb2ca
|
82
|
temp2= (datos_real[upper_leg]- datos_real[lower_leg]);
|
| ddobano |
0:e60134beb2ca
|
83
|
temp3= (datos_imag[upper_leg]+ datos_imag[lower_leg]);
|
| ddobano |
0:e60134beb2ca
|
84
|
temp4= (datos_imag[upper_leg]- datos_imag[lower_leg]);
|
| ddobano |
0:e60134beb2ca
|
85
|
datos_real[lower_leg]= (temp2*w.cosval[index])-(temp4*w.sinval[index]);
|
| ddobano |
0:e60134beb2ca
|
86
|
datos_imag[lower_leg]= (temp2*w.sinval[index]+temp4*w.cosval[index]);
|
| ddobano |
0:e60134beb2ca
|
87
|
datos_real[upper_leg]= temp1;
|
| ddobano |
0:e60134beb2ca
|
88
|
datos_imag[upper_leg]= temp3;
|
| ddobano |
0:e60134beb2ca
|
89
|
}
|
| ddobano |
0:e60134beb2ca
|
90
|
index+=step;
|
| ddobano |
0:e60134beb2ca
|
91
|
}
|
| ddobano |
0:e60134beb2ca
|
92
|
leg_diff=leg_diff/2;
|
| ddobano |
0:e60134beb2ca
|
93
|
step*=2;
|
| ddobano |
0:e60134beb2ca
|
94
|
}
|
| ddobano |
0:e60134beb2ca
|
95
|
//bit reversal
|
| ddobano |
0:e60134beb2ca
|
96
|
|
| ddobano |
0:e60134beb2ca
|
97
|
j=0;
|
| ddobano |
0:e60134beb2ca
|
98
|
for (i=1; i<(N-2); i++) {
|
| ddobano |
0:e60134beb2ca
|
99
|
k=n/2;
|
| ddobano |
0:e60134beb2ca
|
100
|
while (k<=j) {
|
| ddobano |
0:e60134beb2ca
|
101
|
j=j-k;
|
| ddobano |
0:e60134beb2ca
|
102
|
k=k/2;
|
| ddobano |
0:e60134beb2ca
|
103
|
}
|
| ddobano |
0:e60134beb2ca
|
104
|
j=j+k;
|
| ddobano |
0:e60134beb2ca
|
105
|
|
| ddobano |
0:e60134beb2ca
|
106
|
if (i<j) {
|
| ddobano |
0:e60134beb2ca
|
107
|
temp1= datos_real[j];
|
| ddobano |
0:e60134beb2ca
|
108
|
temp2= datos_imag[j];
|
| ddobano |
0:e60134beb2ca
|
109
|
datos_real[j]=datos_real[i];
|
| ddobano |
0:e60134beb2ca
|
110
|
datos_imag[j]=datos_imag[i];
|
| ddobano |
0:e60134beb2ca
|
111
|
datos_real[i]=temp1;
|
| ddobano |
0:e60134beb2ca
|
112
|
datos_imag[i]=temp2;
|
| ddobano |
0:e60134beb2ca
|
113
|
}
|
| ddobano |
0:e60134beb2ca
|
114
|
}
|
| ddobano |
0:e60134beb2ca
|
115
|
|
| ddobano |
0:e60134beb2ca
|
116
|
for (i=0; i<n; i++) {
|
| ddobano |
0:e60134beb2ca
|
117
|
|
| ddobano |
0:e60134beb2ca
|
118
|
datos[i]= pow(sqrt(pow(datos_real[i],2)+pow(datos_imag[i],2)),2); //Calculate the power fft
|
| ddobano |
0:e60134beb2ca
|
119
|
};
|
| ddobano |
0:e60134beb2ca
|
120
|
|
| ddobano |
0:e60134beb2ca
|
121
|
}
|
| ddobano |
0:e60134beb2ca
|
122
|
|
| ddobano |
0:e60134beb2ca
|
123
|
void enviodatos(float datos[N],int n) {
|
| ddobano |
0:e60134beb2ca
|
124
|
int i=0,j=0,k=0,aux=0; //send to serial port the values correctly.
|
| ddobano |
0:e60134beb2ca
|
125
|
unsigned char bytes[4];
|
| ddobano |
0:e60134beb2ca
|
126
|
float dato;
|
| ddobano |
0:e60134beb2ca
|
127
|
|
| ddobano |
0:e60134beb2ca
|
128
|
for (i=0; i<n; i++) {
|
| ddobano |
0:e60134beb2ca
|
129
|
dato=datos[i];
|
| ddobano |
0:e60134beb2ca
|
130
|
aux=dato;
|
| ddobano |
0:e60134beb2ca
|
131
|
for (j=0; j<4; j++) {
|
| ddobano |
0:e60134beb2ca
|
132
|
switch (j) {
|
| ddobano |
0:e60134beb2ca
|
133
|
case 0:
|
| ddobano |
0:e60134beb2ca
|
134
|
bytes[j]=aux;
|
| ddobano |
0:e60134beb2ca
|
135
|
aux=aux>>8;
|
| ddobano |
0:e60134beb2ca
|
136
|
break;
|
| ddobano |
0:e60134beb2ca
|
137
|
case 1:
|
| ddobano |
0:e60134beb2ca
|
138
|
bytes[j]=aux;
|
| ddobano |
0:e60134beb2ca
|
139
|
aux=aux>>8;
|
| ddobano |
0:e60134beb2ca
|
140
|
break;
|
| ddobano |
0:e60134beb2ca
|
141
|
case 2:
|
| ddobano |
0:e60134beb2ca
|
142
|
bytes[j]=aux;
|
| ddobano |
0:e60134beb2ca
|
143
|
aux=aux>>8;
|
| ddobano |
0:e60134beb2ca
|
144
|
break;
|
| ddobano |
0:e60134beb2ca
|
145
|
case 3:
|
| ddobano |
0:e60134beb2ca
|
146
|
bytes[j]=aux;
|
| ddobano |
0:e60134beb2ca
|
147
|
aux=aux>>8;
|
| ddobano |
0:e60134beb2ca
|
148
|
break;
|
| ddobano |
0:e60134beb2ca
|
149
|
}
|
| ddobano |
0:e60134beb2ca
|
150
|
}
|
| ddobano |
0:e60134beb2ca
|
151
|
for (k=3; k>=0; k--) {
|
| ddobano |
0:e60134beb2ca
|
152
|
|
| ddobano |
0:e60134beb2ca
|
153
|
while (pc.writeable()==0);
|
| ddobano |
0:e60134beb2ca
|
154
|
pc.putc(bytes[k]);
|
| ddobano |
0:e60134beb2ca
|
155
|
|
| ddobano |
0:e60134beb2ca
|
156
|
}
|
| ddobano |
0:e60134beb2ca
|
157
|
}
|
| ddobano |
0:e60134beb2ca
|
158
|
} |