main.cpp

00001 #include "mbed.h"
00002 #include <ctype.h>
00003 #include "arm_math.h"
00004 #include "arm_const_structs.h"
00005 #include "stm32f4xx_hal.h"
00006 #include <stdio.h>
00007  
00008  
00009 /* SPI handler declaration */
00010 SPI_HandleTypeDef SpiHandle;//pemennaya dlia hraneniya 
00011  
00012 void arm_cfft_f32(
00013    const arm_cfft_instance_f32 * S,
00014    float32_t * p1,
00015    uint8_t ifftFlag,
00016    uint8_t bitReverseFlag);//fynktsiya
00017 
00018 Serial pc(USBTX, USBRX);//:D ПРИВЕТ!!!!
00019  
00020 AnalogIn   left(A2);//microfony
00021 AnalogIn   right(A3);
00022 AnalogIn   center(A3);
00023 
00024 int flag = 0;
00025  
00026 const int SAMPLE_RATE_HZ = 40000;             // Sample rate of the audio in hertz.
00027 const int FFT_SIZE = 16;                // Size of the FFT.
00028  
00029 const static arm_cfft_instance_f32 *S1;//ykazateli na prametru preobrazovaniya fyrie
00030 const static arm_cfft_instance_f32 *S2;
00031 const static arm_cfft_instance_f32 *S3;
00032 //static arm_cfft_radix2_instance_f32 *S;
00033 Ticker samplingTimer;
00034 float samples[FFT_SIZE*2];//dannue s analog vhodov
00035 float samples2[FFT_SIZE*2];
00036 float samples3[FFT_SIZE*2];
00037 
00038 float magnitudes[FFT_SIZE];
00039 float magnitudes2[FFT_SIZE];
00040 float magnitudes3[FFT_SIZE];
00041 int sampleCounter = 0;
00042 
00043 const int SAMPLE_QUANTITY = 1000000/(16*(1000000/SAMPLE_RATE_HZ));//dlitelnost sempla po vremeni v milisek
00044 
00045 float magnitudes_per_second[SAMPLE_QUANTITY];//sohran magnitydy
00046 int index_of_magnitudes[SAMPLE_QUANTITY];//mesto v masive magnitydu
00047 int index_of_samples;
00048 
00049 float magnitudes_per_second2[SAMPLE_QUANTITY];//sohran magnitydy
00050 int index_of_magnitudes2[SAMPLE_QUANTITY];//mesto v masive magnitydu
00051 int index_of_samples2;
00052 
00053 float magnitudes_per_second3[SAMPLE_QUANTITY];//sohran magnitydy
00054 int index_of_magnitudes3[SAMPLE_QUANTITY];//mesto v masive magnitydu
00055 int index_of_samples3;
00056             
00057 void samplingCallback()
00058 {   
00059     // Read from the ADC and store the sample data
00060     samples[sampleCounter] = 1000*left.read();
00061     samples2[sampleCounter] = 1000*left.read();
00062     samples3[sampleCounter] = 1000*left.read();
00063     // Complex FFT functions require a coefficient for the imaginary part of the input.
00064     // Since we only have real data, set this coefficient to zero.
00065     samples[sampleCounter+1] = 0.0;
00066     samples2[sampleCounter+1] = 0.0;
00067     samples3[sampleCounter+1] = 0.0;
00068     // Update sample buffer position and stop after the buffer is filled
00069        
00070     sampleCounter += 2;//pri razbitti dannuh ostavliaem mesto dila mnimoy 4sti
00071    
00072     if (sampleCounter >= FFT_SIZE*2) {
00073         samplingTimer.detach();
00074     }//obrabotano sobutie taimera
00075 }
00076  
00077 void samplingBegin()
00078 {
00079     // Reset sample buffer position and start callback at necessary rate.
00080     sampleCounter = 0;
00081     samplingTimer.attach_us(&samplingCallback, (float)(1000000/SAMPLE_RATE_HZ));//na4ala poly4eniya dannuh s microfona po taimery
00082 }
00083  
00084 bool samplingIsDone()
00085 {
00086     return sampleCounter >= FFT_SIZE*2;//koli4estvo poly4enuh semplov
00087 }
00088  
00089 int main()
00090 {
00091         
00092     static uint32_t cnt1=0;//s4et4ik vremeni
00093    
00094     // Init arm_ccft_32
00095     switch (FFT_SIZE)//vupolniaem operatsiy pobitovogo i
00096     {
00097     case 16:
00098         S1 = & arm_cfft_sR_f32_len16;
00099         S2 = & arm_cfft_sR_f32_len16;
00100         S3 = & arm_cfft_sR_f32_len16;
00101         break;
00102     case 32:
00103         S1 = & arm_cfft_sR_f32_len32;
00104         S2 = & arm_cfft_sR_f32_len32;
00105         S3 = & arm_cfft_sR_f32_len32;
00106         break;
00107     case 64:
00108         S1 = & arm_cfft_sR_f32_len64;
00109         S2 = & arm_cfft_sR_f32_len64;
00110         S3 = & arm_cfft_sR_f32_len64;
00111         break;
00112     case 128:
00113         S1 = & arm_cfft_sR_f32_len128;
00114         S2 = & arm_cfft_sR_f32_len128;
00115         S3 = & arm_cfft_sR_f32_len128;
00116         break;
00117     case 256:
00118         S1 = & arm_cfft_sR_f32_len256;
00119         S2 = & arm_cfft_sR_f32_len256;
00120         S3 = & arm_cfft_sR_f32_len256;
00121         break;
00122     case 512:
00123         S1 = & arm_cfft_sR_f32_len512;
00124         S2 = & arm_cfft_sR_f32_len512;
00125         S3 = & arm_cfft_sR_f32_len512;
00126         break;
00127     case 1024:
00128         S1 = & arm_cfft_sR_f32_len1024;
00129         S2 = & arm_cfft_sR_f32_len1024;
00130         S3 = & arm_cfft_sR_f32_len1024;
00131         break;
00132     case 2048:
00133         S1 = & arm_cfft_sR_f32_len2048;
00134         S2 = & arm_cfft_sR_f32_len2048;
00135         S3 = & arm_cfft_sR_f32_len2048;
00136         break;
00137     case 4096:
00138         S1 = & arm_cfft_sR_f32_len4096;
00139         S2 = & arm_cfft_sR_f32_len4096;
00140         S3 = & arm_cfft_sR_f32_len4096;
00141         break;
00142     }
00143     float maxValue = 0.0f;
00144     float maxValue2 = 0.0f;
00145     float maxValue3 = 0.0f;
00146    
00147     unsigned int testIndex = 0;
00148     unsigned int testIndex2 = 0;
00149     unsigned int testIndex3 = 0;
00150  
00151     // Begin sampling audio
00152     samplingBegin();
00153  
00154     while(1)
00155         // Calculate FFT if a full sample is available. vu4islenie bustrogo fyrie preobrazovaniya /rez v samples
00156         if (samplingIsDone())
00157         {           
00158             // Run FFT on sample data.
00159             arm_cfft_f32(S1, samples, 0, 1);
00160             arm_cfft_f32(S2, samples2, 0, 1);
00161             arm_cfft_f32(S3, samples3, 0, 1);
00162             samples[0]=0;
00163             samples2[0]=0;
00164             samples3[0]=0;
00165           
00166             arm_cmplx_mag_f32(samples, magnitudes, FFT_SIZE);
00167             arm_cmplx_mag_f32(samples2, magnitudes2, FFT_SIZE);
00168             arm_cmplx_mag_f32(samples3, magnitudes3, FFT_SIZE);
00169            
00170             arm_max_f32(magnitudes, FFT_SIZE, &maxValue, &testIndex);
00171             arm_max_f32(magnitudes2, FFT_SIZE, &maxValue2, &testIndex2);
00172             arm_max_f32(magnitudes3, FFT_SIZE, &maxValue3, &testIndex3);
00173            
00174 
00175            
00176                 {
00177        if (HAL_GetTick() > (cnt1 + 1000))
00178         {
00179             cnt1=HAL_GetTick();
00180             if (flag==0){
00181                 pc.printf("     MAX value at magnitudes 1[%d] : %+8.2f\r\n", testIndex, maxValue);
00182                 pc.printf("     MAX value at magnitudes 2[%d] : %+8.2f\r\n", testIndex2, maxValue2);
00183                 pc.printf("     MAX value at magnitudes 3[%d] : %+8.2f\r\n", testIndex3, maxValue3);
00184                 flag++;
00185                 }
00186             else if (flag==1){
00187 
00188                 flag++;
00189                 }
00190             else if (flag==2){
00191 
00192                 flag=0;
00193                 }
00194         }
00195            
00196            
00197             double delta_t = 0;
00198             if ((testIndex < testIndex2) and (testIndex < testIndex3) and (testIndex2 < testIndex3))
00199                 delta_t = (testIndex3 - testIndex)*0.000025;
00200             else if ((testIndex < testIndex2)and(testIndex < testIndex3)and(testIndex2 > testIndex3))
00201                 delta_t = (testIndex2 - testIndex)*0.000025;
00202             else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex < testIndex3))
00203                 delta_t = (testIndex3 - testIndex2)*0.000025;
00204             else if ((testIndex2 < testIndex3)and(testIndex2 < testIndex)and(testIndex > testIndex3))
00205                 delta_t = (testIndex - testIndex3)*0.000025;
00206             else if (testIndex2 < testIndex)
00207                 delta_t = (testIndex - testIndex3)*0.000025;
00208             else
00209                 delta_t = (testIndex - testIndex2)*0.000025;
00210             double fi = asin(330*delta_t/0.33);
00211 
00212 
00213 magnitudes_per_second[index_of_samples]=maxValue;
00214 index_of_magnitudes[testIndex]=testIndex;
00215 
00216 magnitudes_per_second2[index_of_samples2]=maxValue2;
00217 index_of_magnitudes2[testIndex2]=testIndex2;
00218 
00219 magnitudes_per_second3[index_of_samples3]=maxValue3;
00220 index_of_magnitudes3[testIndex3]=testIndex3;
00221            
00222        if (HAL_GetTick() > (cnt1 + 1000))
00223         {
00224             cnt1=HAL_GetTick();
00225             if (flag==0){
00226                 float max_magnitude_per_second = 0;
00227                
00228                 float max_magnitude_per_second2 = 0;
00229                
00230                 float max_magnitude_per_second3 = 0;
00231                
00232                
00233                 unsigned int index_max_of_mag;
00234                 unsigned int index_max_of_mag2;
00235                 unsigned int index_max_of_mag3;
00236                
00237                 arm_max_f32(magnitudes_per_second, FFT_SIZE, &max_magnitude_per_second, &index_max_of_mag);
00238                 arm_max_f32(magnitudes_per_second2, FFT_SIZE, &max_magnitude_per_second2, &index_max_of_mag2);
00239                 arm_max_f32(magnitudes, FFT_SIZE, &max_magnitude_per_second3, &index_max_of_mag3);
00240                
00241                 pc.printf("     MAX value at magnitudes 1[%d] : %+8.2f\r\n", index_max_of_mag, max_magnitude_per_second);//Я НЯШКА А АНЯ ВЛАСЮК КОЗА:DDDDDDDDDDDDD
00242                 pc.printf("     MAX value at magnitudes 2[%d] : %+8.2f\r\n", index_max_of_mag2, max_magnitude_per_second2);
00243                 pc.printf("     MAX value at magnitudes 3[%d] : %+8.2f\r\n", index_max_of_mag3, max_magnitude_per_second3);
00244                 pc.printf("     Angle is  : %+8.2f\r\n", fi);
00245                 index_of_samples = 0;
00246                 flag++;
00247                 }
00248             else if (flag==1){
00249 
00250                 flag++;
00251                 }
00252             else if (flag==2){
00253 
00254                 flag=0;
00255                 }
00256         }
00257         else
00258         index_of_samples++;
00259         index_of_samples2++;
00260         index_of_samples3++;
00261 
00262 
00263             samplingBegin();
00264         }
00265     }
00266 }