main.cpp

00001 #include "mbed.h"
00002 #include <ctype.h>
00003 #include "arm_math.h"
00004 #include "arm_const_structs.h"
00005  
00006 void arm_cfft_f32( 
00007    const arm_cfft_instance_f32 * S, 
00008    float32_t * p1,
00009    uint8_t ifftFlag,
00010    uint8_t bitReverseFlag);
00011 
00012 Serial pc(USBTX, USBRX);//:D ПРИВЕТ!!!!
00013 
00014 
00015  
00016 AnalogIn   left(A2);
00017 AnalogIn   right(A3);
00018 AnalogIn   center(A3);
00019  
00020 int SAMPLE_RATE_HZ = 40000;             // Sample rate of the audio in hertz.
00021 //const int FFT_SIZE = 16;                // Size of the FFT.
00022 const int FFT_SIZE = 1024;                // Size of the FFT.
00023  
00024 const static arm_cfft_instance_f32 *S;
00025 //static arm_cfft_radix2_instance_f32 *S;
00026 Ticker samplingTimer;
00027 float samples[FFT_SIZE*2];
00028 float samples2[FFT_SIZE*2];
00029 float samples3[FFT_SIZE*2];
00030 
00031 float magnitudes[FFT_SIZE];
00032 float magnitudes2[FFT_SIZE];
00033 float magnitudes3[FFT_SIZE];
00034 int sampleCounter = 0;
00035 
00036  
00037 void samplingCallback()
00038 {    
00039     // Read from the ADC and store the sample data
00040     samples[sampleCounter] = 1000*left.read();
00041     samples2[sampleCounter] = 1000*left.read();
00042     samples3[sampleCounter] = 1000*left.read();
00043     // Complex FFT functions require a coefficient for the imaginary part of the input.
00044     // Since we only have real data, set this coefficient to zero.
00045     samples[sampleCounter+1] = 0.0;
00046     samples2[sampleCounter+1] = 0.0;
00047     samples3[sampleCounter+1] = 0.0;
00048     // Update sample buffer position and stop after the buffer is filled
00049         
00050     sampleCounter += 2;
00051     
00052     if (sampleCounter >= FFT_SIZE*2) {
00053         samplingTimer.detach();
00054     }
00055 }
00056  
00057 void samplingBegin()
00058 {
00059     // Reset sample buffer position and start callback at necessary rate.
00060     sampleCounter = 0;
00061     samplingTimer.attach_us(&samplingCallback, (float)(1000000/SAMPLE_RATE_HZ));
00062 }
00063  
00064 bool samplingIsDone()
00065 {
00066     return sampleCounter >= FFT_SIZE*2;
00067 }
00068  
00069 int main()
00070 {
00071     // Set up serial port.
00072     //pc.baud (38400);
00073  
00074     // Init arm_ccft_32
00075     switch (FFT_SIZE)
00076     {
00077     case 16:
00078         S = & arm_cfft_sR_f32_len16;
00079         break;
00080     case 32:
00081         S = & arm_cfft_sR_f32_len32;
00082         break;
00083     case 64:
00084         S = & arm_cfft_sR_f32_len64;
00085         break;
00086     case 128:
00087         S = & arm_cfft_sR_f32_len128;
00088         break;
00089     case 256:
00090         S = & arm_cfft_sR_f32_len256;
00091         break;
00092     case 512:
00093         S = & arm_cfft_sR_f32_len512;
00094         break;
00095     case 1024:
00096         S = & arm_cfft_sR_f32_len1024;
00097         break;
00098     case 2048:
00099         S = & arm_cfft_sR_f32_len2048;
00100         break;
00101     case 4096:
00102         S = & arm_cfft_sR_f32_len4096;
00103         break;
00104     }
00105     float maxValue = 0.0f;
00106     float maxValue2 = 0.0f;
00107     float maxValue3 = 0.0f;
00108     
00109     unsigned int testIndex = 0;
00110     unsigned int testIndex2 = 0;
00111     unsigned int testIndex3 = 0;
00112  
00113     // Begin sampling audio
00114     samplingBegin();
00115  
00116     while(1)
00117     {
00118         // Calculate FFT if a full sample is available.
00119         if (samplingIsDone())
00120         {            
00121             // Run FFT on sample data.
00122             //arm_cfft_radix2_f32(arm_cfft_radix2_instance_f32*S, samples);
00123             arm_cfft_f32(S, samples, 0, 1);
00124             arm_cfft_f32(S, samples2, 0, 1);
00125             arm_cfft_f32(S, samples3, 0, 1);
00126            samples[0]=0;
00127            samples2[0]=0;
00128            samples3[0]=0;
00129             
00130             /* Initialize the CFFT/CIFFT module */
00131             //arm_cfft_radix2_init_f32(S, 128, 0, 1);
00132             //arm_cfft_radix2_f32(S, samples);
00133             //for(int i = 0;i < FFT_SIZE*2;++i)
00134             //    pc.printf("     Samples[%d]: %8.2f ",i,samples[i]);
00135             //pc.printf("\r\n");
00136             // Calculate magnitude of complex numbers output by the FFT.
00137             arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);
00138             arm_cmplx_mag_f32(samples2, magnitudes2, FFT_SIZE);
00139             arm_cmplx_mag_f32(samples3, magnitudes3, FFT_SIZE);
00140             
00141             //for(int i = 0;i < FFT_SIZE;++i)
00142             //    pc.printf("     Magnitude: %d = %8.2f ;", i, magnitudes[i]);
00143             //pc.printf("       \r\n");
00144             arm_max_f32(magnitudes, FFT_SIZE, &maxValue, &testIndex);
00145             arm_max_f32(magnitudes2, FFT_SIZE, &maxValue2, &testIndex2);
00146             arm_max_f32(magnitudes3, FFT_SIZE, &maxValue3, &testIndex3);
00147             pc.printf("     MAX value at magnitudes 1[%d] : %+8.2f\r\n", testIndex, maxValue);//Я НЯШКА А АНЯ ВЛАСЮК КОЗА:DDDDDDDDDDDDD
00148             pc.printf("     MAX value at magnitudes 2[%d] : %+8.2f\r\n", testIndex2, maxValue2);
00149             pc.printf("     MAX value at magnitudes 3[%d] : %+8.2f\r\n", testIndex3, maxValue3);
00150             
00151             double delta_t = 0;
00152             if ((testIndex < testIndex2) and (testIndex < testIndex3) and (testIndex2 < testIndex3))
00153                 delta_t = (testIndex3 - testIndex)*0.000025;
00154             else if ((testIndex < testIndex2)and(testIndex < testIndex3)and(testIndex2 > testIndex3))
00155                 delta_t = (testIndex2 - testIndex)*0.000025;
00156             else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex < testIndex3))
00157                 delta_t = (testIndex3 - testIndex2)*0.000025;
00158             else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex > testIndex3))
00159                 delta_t = (testIndex - testIndex3)*0.000025;
00160             else if (testIndex2 < testIndex)
00161                 delta_t = (testIndex - testIndex3)*0.000025;
00162             else
00163                 delta_t = (testIndex - testIndex2)*0.000025;
00164             double fi = asin(330*delta_t/0.33);
00165             pc.printf("     Angle is  : %+8.2f\r\n", fi);
00166             
00167             // Wait for user confirmation to restart audio sampling.
00168             //pc.getc();
00169             wait(1);
00170             samplingBegin();
00171         }
00172     }
00173 }
00174