Nikolay Sergeev / Mbed 2 deprecated fir_f32

Ura

Dependencies:   CMSIS_DSP_401 mbed

main.cpp@6:0de27f5ec409, 2014-11-18 (annotated)

Committer:
Sergeev
Date:
Tue Nov 18 17:21:07 2014 +0000
Revision:
6:0de27f5ec409
Parent:
5:173ae8477ae1
Child:
7:a4cff60f035b
wester

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sergeev 0:b712470aea1d 1 #include "mbed.h"
Sergeev 1:9b1df0b2507d 2 #include <ctype.h>
Sergeev 0:b712470aea1d 3 #include "arm_math.h"
Sergeev 1:9b1df0b2507d 4 #include "arm_const_structs.h"
Sergeev 1:9b1df0b2507d 5
Sergeev 1:9b1df0b2507d 6 void arm_cfft_f32(
Sergeev 1:9b1df0b2507d 7 const arm_cfft_instance_f32 * S,
Sergeev 1:9b1df0b2507d 8 float32_t * p1,
Sergeev 1:9b1df0b2507d 9 uint8_t ifftFlag,
Sergeev 1:9b1df0b2507d 10 uint8_t bitReverseFlag);
Sergeev 0:b712470aea1d 11
Sergeev 5:173ae8477ae1 12 Serial pc(USBTX, USBRX);//:D ПРИВЕТ!!!!
Sergeev 5:173ae8477ae1 13
Sergeev 5:173ae8477ae1 14
Sergeev 1:9b1df0b2507d 15
Sergeev 1:9b1df0b2507d 16 AnalogIn left(A2);
Sergeev 1:9b1df0b2507d 17 AnalogIn right(A3);
Sergeev 5:173ae8477ae1 18 AnalogIn center(A3);
Sergeev 1:9b1df0b2507d 19
Sergeev 1:9b1df0b2507d 20 int SAMPLE_RATE_HZ = 40000; // Sample rate of the audio in hertz.
Sergeev 6:0de27f5ec409 21 const int FFT_SIZE = 16; // Size of the FFT.
Sergeev 1:9b1df0b2507d 22
Sergeev 6:0de27f5ec409 23 const static arm_cfft_instance_f32 *S1;
Sergeev 6:0de27f5ec409 24 const static arm_cfft_instance_f32 *S2;
Sergeev 6:0de27f5ec409 25 const static arm_cfft_instance_f32 *S3;
Sergeev 3:1f56b8f439a1 26 //static arm_cfft_radix2_instance_f32 *S;
Sergeev 1:9b1df0b2507d 27 Ticker samplingTimer;
Sergeev 1:9b1df0b2507d 28 float samples[FFT_SIZE*2];
Sergeev 5:173ae8477ae1 29 float samples2[FFT_SIZE*2];
Sergeev 5:173ae8477ae1 30 float samples3[FFT_SIZE*2];
Sergeev 5:173ae8477ae1 31
Sergeev 1:9b1df0b2507d 32 float magnitudes[FFT_SIZE];
Sergeev 5:173ae8477ae1 33 float magnitudes2[FFT_SIZE];
Sergeev 5:173ae8477ae1 34 float magnitudes3[FFT_SIZE];
Sergeev 1:9b1df0b2507d 35 int sampleCounter = 0;
Sergeev 5:173ae8477ae1 36
Sergeev 1:9b1df0b2507d 37
Sergeev 1:9b1df0b2507d 38 void samplingCallback()
Sergeev 3:1f56b8f439a1 39 {
Sergeev 1:9b1df0b2507d 40 // Read from the ADC and store the sample data
Sergeev 3:1f56b8f439a1 41 samples[sampleCounter] = 1000*left.read();
Sergeev 5:173ae8477ae1 42 samples2[sampleCounter] = 1000*left.read();
Sergeev 5:173ae8477ae1 43 samples3[sampleCounter] = 1000*left.read();
Sergeev 1:9b1df0b2507d 44 // Complex FFT functions require a coefficient for the imaginary part of the input.
Sergeev 1:9b1df0b2507d 45 // Since we only have real data, set this coefficient to zero.
Sergeev 1:9b1df0b2507d 46 samples[sampleCounter+1] = 0.0;
Sergeev 5:173ae8477ae1 47 samples2[sampleCounter+1] = 0.0;
Sergeev 5:173ae8477ae1 48 samples3[sampleCounter+1] = 0.0;
Sergeev 1:9b1df0b2507d 49 // Update sample buffer position and stop after the buffer is filled
Sergeev 3:1f56b8f439a1 50
Sergeev 1:9b1df0b2507d 51 sampleCounter += 2;
Sergeev 2:707eb5bc5dfc 52
Sergeev 1:9b1df0b2507d 53 if (sampleCounter >= FFT_SIZE*2) {
Sergeev 1:9b1df0b2507d 54 samplingTimer.detach();
Sergeev 1:9b1df0b2507d 55 }
Sergeev 1:9b1df0b2507d 56 }
Sergeev 1:9b1df0b2507d 57
Sergeev 1:9b1df0b2507d 58 void samplingBegin()
Sergeev 1:9b1df0b2507d 59 {
Sergeev 1:9b1df0b2507d 60 // Reset sample buffer position and start callback at necessary rate.
Sergeev 1:9b1df0b2507d 61 sampleCounter = 0;
Sergeev 3:1f56b8f439a1 62 samplingTimer.attach_us(&samplingCallback, (float)(1000000/SAMPLE_RATE_HZ));
Sergeev 1:9b1df0b2507d 63 }
Sergeev 1:9b1df0b2507d 64
Sergeev 1:9b1df0b2507d 65 bool samplingIsDone()
Sergeev 1:9b1df0b2507d 66 {
Sergeev 1:9b1df0b2507d 67 return sampleCounter >= FFT_SIZE*2;
Sergeev 0:b712470aea1d 68 }
Sergeev 1:9b1df0b2507d 69
Sergeev 1:9b1df0b2507d 70 int main()
Sergeev 0:b712470aea1d 71 {
Sergeev 1:9b1df0b2507d 72 // Set up serial port.
Sergeev 3:1f56b8f439a1 73 //pc.baud (38400);
Sergeev 1:9b1df0b2507d 74
Sergeev 1:9b1df0b2507d 75 // Init arm_ccft_32
Sergeev 1:9b1df0b2507d 76 switch (FFT_SIZE)
Sergeev 1:9b1df0b2507d 77 {
Sergeev 1:9b1df0b2507d 78 case 16:
Sergeev 6:0de27f5ec409 79 S1 = & arm_cfft_sR_f32_len16;
Sergeev 6:0de27f5ec409 80 S2 = & arm_cfft_sR_f32_len16;
Sergeev 6:0de27f5ec409 81 S3 = & arm_cfft_sR_f32_len16;
Sergeev 1:9b1df0b2507d 82 break;
Sergeev 1:9b1df0b2507d 83 case 32:
Sergeev 6:0de27f5ec409 84 S1 = & arm_cfft_sR_f32_len32;
Sergeev 6:0de27f5ec409 85 S2 = & arm_cfft_sR_f32_len32;
Sergeev 6:0de27f5ec409 86 S3 = & arm_cfft_sR_f32_len32;
Sergeev 1:9b1df0b2507d 87 break;
Sergeev 1:9b1df0b2507d 88 case 64:
Sergeev 6:0de27f5ec409 89 S1 = & arm_cfft_sR_f32_len64;
Sergeev 6:0de27f5ec409 90 S2 = & arm_cfft_sR_f32_len64;
Sergeev 6:0de27f5ec409 91 S3 = & arm_cfft_sR_f32_len64;
Sergeev 1:9b1df0b2507d 92 break;
Sergeev 1:9b1df0b2507d 93 case 128:
Sergeev 6:0de27f5ec409 94 S1 = & arm_cfft_sR_f32_len128;
Sergeev 6:0de27f5ec409 95 S2 = & arm_cfft_sR_f32_len128;
Sergeev 6:0de27f5ec409 96 S3 = & arm_cfft_sR_f32_len128;
Sergeev 1:9b1df0b2507d 97 break;
Sergeev 1:9b1df0b2507d 98 case 256:
Sergeev 6:0de27f5ec409 99 S1 = & arm_cfft_sR_f32_len256;
Sergeev 6:0de27f5ec409 100 S2 = & arm_cfft_sR_f32_len256;
Sergeev 6:0de27f5ec409 101 S3 = & arm_cfft_sR_f32_len256;
Sergeev 1:9b1df0b2507d 102 break;
Sergeev 1:9b1df0b2507d 103 case 512:
Sergeev 6:0de27f5ec409 104 S1 = & arm_cfft_sR_f32_len512;
Sergeev 6:0de27f5ec409 105 S2 = & arm_cfft_sR_f32_len512;
Sergeev 6:0de27f5ec409 106 S3 = & arm_cfft_sR_f32_len512;
Sergeev 1:9b1df0b2507d 107 break;
Sergeev 1:9b1df0b2507d 108 case 1024:
Sergeev 6:0de27f5ec409 109 S1 = & arm_cfft_sR_f32_len1024;
Sergeev 6:0de27f5ec409 110 S2 = & arm_cfft_sR_f32_len1024;
Sergeev 6:0de27f5ec409 111 S3 = & arm_cfft_sR_f32_len1024;
Sergeev 1:9b1df0b2507d 112 break;
Sergeev 1:9b1df0b2507d 113 case 2048:
Sergeev 6:0de27f5ec409 114 S1 = & arm_cfft_sR_f32_len2048;
Sergeev 6:0de27f5ec409 115 S2 = & arm_cfft_sR_f32_len2048;
Sergeev 6:0de27f5ec409 116 S3 = & arm_cfft_sR_f32_len2048;
Sergeev 1:9b1df0b2507d 117 break;
Sergeev 1:9b1df0b2507d 118 case 4096:
Sergeev 6:0de27f5ec409 119 S1 = & arm_cfft_sR_f32_len4096;
Sergeev 6:0de27f5ec409 120 S2 = & arm_cfft_sR_f32_len4096;
Sergeev 6:0de27f5ec409 121 S3 = & arm_cfft_sR_f32_len4096;
Sergeev 1:9b1df0b2507d 122 break;
Sergeev 1:9b1df0b2507d 123 }
Sergeev 1:9b1df0b2507d 124 float maxValue = 0.0f;
Sergeev 5:173ae8477ae1 125 float maxValue2 = 0.0f;
Sergeev 5:173ae8477ae1 126 float maxValue3 = 0.0f;
Sergeev 5:173ae8477ae1 127
Sergeev 1:9b1df0b2507d 128 unsigned int testIndex = 0;
Sergeev 5:173ae8477ae1 129 unsigned int testIndex2 = 0;
Sergeev 5:173ae8477ae1 130 unsigned int testIndex3 = 0;
Sergeev 1:9b1df0b2507d 131
Sergeev 1:9b1df0b2507d 132 // Begin sampling audio
Sergeev 1:9b1df0b2507d 133 samplingBegin();
Sergeev 1:9b1df0b2507d 134
Sergeev 1:9b1df0b2507d 135 while(1)
Sergeev 1:9b1df0b2507d 136 {
Sergeev 1:9b1df0b2507d 137 // Calculate FFT if a full sample is available.
Sergeev 1:9b1df0b2507d 138 if (samplingIsDone())
Sergeev 3:1f56b8f439a1 139 {
Sergeev 1:9b1df0b2507d 140 // Run FFT on sample data.
Sergeev 1:9b1df0b2507d 141 //arm_cfft_radix2_f32(arm_cfft_radix2_instance_f32*S, samples);
Sergeev 6:0de27f5ec409 142 arm_cfft_f32(S1, samples, 0, 1);
Sergeev 6:0de27f5ec409 143 arm_cfft_f32(S2, samples2, 0, 1);
Sergeev 6:0de27f5ec409 144 arm_cfft_f32(S3, samples3, 0, 1);
Sergeev 5:173ae8477ae1 145 samples[0]=0;
Sergeev 5:173ae8477ae1 146 samples2[0]=0;
Sergeev 5:173ae8477ae1 147 samples3[0]=0;
Sergeev 5:173ae8477ae1 148
Sergeev 3:1f56b8f439a1 149 /* Initialize the CFFT/CIFFT module */
Sergeev 3:1f56b8f439a1 150 //arm_cfft_radix2_init_f32(S, 128, 0, 1);
Sergeev 3:1f56b8f439a1 151 //arm_cfft_radix2_f32(S, samples);
Sergeev 5:173ae8477ae1 152 //for(int i = 0;i < FFT_SIZE*2;++i)
Sergeev 5:173ae8477ae1 153 // pc.printf(" Samples[%d]: %8.2f ",i,samples[i]);
Sergeev 5:173ae8477ae1 154 //pc.printf("\r\n");
Sergeev 1:9b1df0b2507d 155 // Calculate magnitude of complex numbers output by the FFT.
Sergeev 1:9b1df0b2507d 156 arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);
Sergeev 5:173ae8477ae1 157 arm_cmplx_mag_f32(samples2, magnitudes2, FFT_SIZE);
Sergeev 5:173ae8477ae1 158 arm_cmplx_mag_f32(samples3, magnitudes3, FFT_SIZE);
Sergeev 5:173ae8477ae1 159
Sergeev 5:173ae8477ae1 160 //for(int i = 0;i < FFT_SIZE;++i)
Sergeev 5:173ae8477ae1 161 // pc.printf(" Magnitude: %d = %8.2f ;", i, magnitudes[i]);
Sergeev 5:173ae8477ae1 162 //pc.printf(" \r\n");
Sergeev 1:9b1df0b2507d 163 arm_max_f32(magnitudes, FFT_SIZE, &maxValue, &testIndex);
Sergeev 5:173ae8477ae1 164 arm_max_f32(magnitudes2, FFT_SIZE, &maxValue2, &testIndex2);
Sergeev 5:173ae8477ae1 165 arm_max_f32(magnitudes3, FFT_SIZE, &maxValue3, &testIndex3);
Sergeev 5:173ae8477ae1 166 pc.printf(" MAX value at magnitudes 1[%d] : %+8.2f\r\n", testIndex, maxValue);//Я НЯШКА А АНЯ ВЛАСЮК КОЗА:DDDDDDDDDDDDD
Sergeev 5:173ae8477ae1 167 pc.printf(" MAX value at magnitudes 2[%d] : %+8.2f\r\n", testIndex2, maxValue2);
Sergeev 6:0de27f5ec409 168 pc.printf(" MAX value at magnitudes 3[%d] : %+8.2f\r\n", testIndex2, maxValue3);
Sergeev 6:0de27f5ec409 169 // Wait for user confirmation to restart audio sampling.
Sergeev 6:0de27f5ec409 170 //pc.getc();
Sergeev 5:173ae8477ae1 171
Sergeev 5:173ae8477ae1 172 double delta_t = 0;
Sergeev 5:173ae8477ae1 173 if ((testIndex < testIndex2) and (testIndex < testIndex3) and (testIndex2 < testIndex3))
Sergeev 5:173ae8477ae1 174 delta_t = (testIndex3 - testIndex)*0.000025;
Sergeev 5:173ae8477ae1 175 else if ((testIndex < testIndex2)and(testIndex < testIndex3)and(testIndex2 > testIndex3))
Sergeev 5:173ae8477ae1 176 delta_t = (testIndex2 - testIndex)*0.000025;
Sergeev 5:173ae8477ae1 177 else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex < testIndex3))
Sergeev 5:173ae8477ae1 178 delta_t = (testIndex3 - testIndex2)*0.000025;
Sergeev 5:173ae8477ae1 179 else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex > testIndex3))
Sergeev 5:173ae8477ae1 180 delta_t = (testIndex - testIndex3)*0.000025;
Sergeev 5:173ae8477ae1 181 else if (testIndex2 < testIndex)
Sergeev 5:173ae8477ae1 182 delta_t = (testIndex - testIndex3)*0.000025;
Sergeev 5:173ae8477ae1 183 else
Sergeev 5:173ae8477ae1 184 delta_t = (testIndex - testIndex2)*0.000025;
Sergeev 5:173ae8477ae1 185 double fi = asin(330*delta_t/0.33);
Sergeev 5:173ae8477ae1 186 pc.printf(" Angle is : %+8.2f\r\n", fi);
Sergeev 5:173ae8477ae1 187
Sergeev 1:9b1df0b2507d 188 wait(1);
Sergeev 1:9b1df0b2507d 189 samplingBegin();
Sergeev 1:9b1df0b2507d 190 }
Sergeev 1:9b1df0b2507d 191 }
Sergeev 6:0de27f5ec409 192 }